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))