def __init__(self):
self._tf_broadcaster = tf2_ros.TransformBroadcaster()
self._tf_buffer = tf2_ros.Buffer()
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
self._localized_point_tf_from_localized_origin = init_tf_stamped(
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_ORIGIN_FRAME_ID),
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_FRAME_ID))
self._mobile_base_tf_for_calc_from_localized_origin = init_tf_stamped(
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_ORIGIN_FRAME_ID_FOR_CALC),
rospy.get_param(PARAM_NAME_MOBILE_BASE_FRAME_ID_FOR_CALC))
self._localized_point_tf_for_calc_from_mobile_base = init_tf_stamped(
rospy.get_param(PARAM_NAME_MOBILE_BASE_FRAME_ID_FOR_CALC),
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_FRAME_ID_FOR_CALC))
self._input = TwistStamped()
self.timer = LoopTimeManager()
def __init__(self, registered_tf_buffer=None):
self._namespace = rospy.get_param(PARAM_NAME_MICO_EEPOSE_INPUT_ACTIONTOPIC)
super(MicoCtrl_EEPose, self).__init__(PoseStamped, self._namespace + '/goal',
registered_tf_buffer)
self.cancel_publisher = rospy.Publisher(self._namespace + '/cancel',
GoalID,
queue_size=1)
self.ref_pose_from_origin_frame = Pose(orientation=Quaternion(w=1.0))
self.arm_base_tf = init_tf_stamped(rospy.get_param(PARAM_NAME_SYS_ORIGIN_FRAME_ID),
rospy.get_param(PARAM_NAME_ARM_CTRL_ORIGIN_FRAME_ID))