def activate(self): rospy.Subscriber(rospy.get_param(PARAM_NAME_BS_INPUT_TOPIC), TwistStamped, self._bs_input_callback) origin_to_localized_origin = wait_for_tf(self._tf_buffer, rospy.get_param(PARAM_NAME_SYS_ORIGIN_FRAME_ID), rospy.get_param(PARAM_NAME_LOCALIZED_POINT_ORIGIN_FRAME_ID)) origin_to_localized_origin.child_frame_id = rospy.get_param(PARAM_NAME_LOCALIZED_POINT_ORIGIN_FRAME_ID_FOR_CALC) rospy.Publisher('/tf_static', TFMessage, queue_size=100, latch=True).publish([origin_to_localized_origin]) self._localized_point_tf_for_calc_from_mobile_base = wait_for_tf(self._tf_buffer, self._localized_point_tf_for_calc_from_mobile_base.header.frame_id, self._localized_point_tf_for_calc_from_mobile_base.child_frame_id, interval_sec=5.0) rospy.loginfo('slam_mock activated') return self
def activate_controller(self): self.arm_base_tf = wait_for_tf(self._tf_buffer, self.arm_base_tf.header.frame_id, self.arm_base_tf.child_frame_id) self._feedback_subscriber = rospy.Subscriber(self._namespace + '/feedback', PoseStamped, self._feedback_callback) self._mani_ref_subscriber = rospy.Subscriber(rospy.get_param(PARAM_NAME_MANI_REF_TOPIC), ManipulationReference, self._mani_ref_callback) return True