実際のところ
rclpyでやる
せっかくVelもあるので
xarm_ros2/xarm_msgs/srv/MoveVelocity.srv at humble · xArm-Developer/xarm_ros2 · GitHub
import rclpy from rclpy.node import Node from xarm_msgs.srv import SetInt16ById,SetInt16, MoveVelocity from sensor_msgs.msg import Joy class XArmSetupClient(Node): def __init__(self): super().__init__('xarm_setup_client') # Enable all joints self.motion_enable_client = self.create_client(SetInt16ById, '/xarm/motion_enable') self.motion_enable_request = SetInt16ById.Request() self.motion_enable_request.id = 8 self.motion_enable_request.data = 1 # Set proper mode and state self.set_mode_client = self.create_client(SetInt16, '/xarm/set_mode') self.set_mode_request = SetInt16.Request() self.set_mode_request.data = 5 ## 5 is for velocity control self.set_state_client = self.create_client(SetInt16, '/xarm/set_state') self.set_state_request = SetInt16.Request() self.set_state_request.data = 0 self.call_services() def call_services(self): # Enable all joints if not self.motion_enable_client.wait_for_service(timeout_sec=3.0): self.get_logger().error('Service /xarm/motion_enable is not available!') return self.motion_enable_client.call_async(self.motion_enable_request) # Set mode and state if not self.set_mode_client.wait_for_service(timeout_sec=3.0): self.get_logger().error('Service /xarm/set_mode is not available!') return self.set_mode_client.call_async(self.set_mode_request) if not self.set_state_client.wait_for_service(timeout_sec=3.0): self.get_logger().error('Service /xarm/set_state is not available!') return self.set_state_client.call_async(self.set_state_request) class JoyTranslate(Node): def __init__(self): super().__init__('joy_translate_node') self.set_position_client = self.create_client(MoveVelocity, '/xarm/vc_set_cartesian_velocity') self.subscription = self.create_subscription(Joy,'joy', self.listener_callback, 10) def listener_callback(self, Joy): vel_speed = 40.0 vel_linear_x = vel_speed * Joy.axes[1] vel_linear_y = vel_speed * Joy.axes[0] request = MoveVelocity.Request() request.speeds = [vel_linear_x, vel_linear_y, 0.0, 0.0, 0.0, 0.0] # 非同期でサービスをコール self.future = self.set_position_client.call_async(request) def main(args=None): rclpy.init() xarm_setup_client = XArmSetupClient() joy_translate = JoyTranslate() rclpy.spin(joy_translate) rclpy.shutdown() if __name__ == '__main__': main()
