def __init__(self): self._joy_data = JoyData() self._mico_input_data = TwistStamped() self._is_linear_control = True self._is_able_to_change_mode = False self._scale_twist = Twist(linear=Vector3(x=1.0, y=1.0, z=1.0), angular=Vector3(x=1.0, y=1.0, z=1.0)) super(JoyStickInputGenerator, self).__init__('joy', Joy, self._mico_input_data)
class JoyStickInputGenerator(MicoInputGeneratorAbstract): def __init__(self): self._joy_data = JoyData() self._mico_input_data = TwistStamped() self._is_linear_control = True self._is_able_to_change_mode = False self._scale_twist = Twist(linear=Vector3(x=1.0, y=1.0, z=1.0), angular=Vector3(x=1.0, y=1.0, z=1.0)) super(JoyStickInputGenerator, self).__init__('joy', Joy, self._mico_input_data) def temp(self): pos = self._current_ee_pose.pose.position print(str(pow(pow(pos.x, 2.0) + pow(pos.y, 2.0), 0.5)) + ' ' + str(pow(pow(pos.x, 2.0) + pow(pos.y, 2.0) + pow(pos.z-0.25, 2.0), 0.5))) def _device_data_callback(self, joy_data): self.temp() self._joy_data.set_data(joy_data.axes, joy_data.buttons) self._mico_input_data.header.stamp = rospy.Time.now() if self._joy_data.is_pressed('R3', is_only=True): self._is_able_to_change_mode = True elif self._is_able_to_change_mode: self._is_able_to_change_mode = False self._is_linear_control = not self._is_linear_control mode_str = 'position' if self._is_linear_control else 'orientation' rospy.loginfo(mode_str + ' mode now') if self._is_linear_control: self._mico_input_data.twist.linear.x = self._scale_twist.linear.x * self._joy_data.rstick.x self._mico_input_data.twist.linear.y = self._scale_twist.linear.y * self._joy_data.rstick.y self._mico_input_data.twist.linear.z = self._scale_twist.linear.z * self._joy_data.dpad.x self._mico_input_data.twist.angular = Vector3(x=0.0, y=0.0, z=0.0) else: self._mico_input_data.twist.linear = Vector3(x=0.0, y=0.0, z=0.0) self._mico_input_data.twist.angular.x = self._scale_twist.angular.x * self._joy_data.rstick.y self._mico_input_data.twist.angular.y = self._scale_twist.angular.y * self._joy_data.dpad.x self._mico_input_data.twist.angular.z = self._scale_twist.angular.z * self._joy_data.rstick.x