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
