Multi-Robot Setup

rbs_skill_servers/scripts/test_cartesian_controller.py

@@ -0,0 +1,47 @@
+import rclpy
+from rclpy.node import Node
+import argparse
+
+from geometry_msgs.msg import PoseStamped
+
+class CartesianControllerPublisher(Node):
+
+    def __init__(self, robot_name: str):
+        super().__init__("cartesian_controller_pose_publisher")
+        self.publisher_ = self.create_publisher(
+            PoseStamped, 
+            "/" + robot_name + "/cartesian_motion_controller/target_frame", 10)
+        timer_period = 0.5  # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+    def timer_callback(self):
+        msg = PoseStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = "base_link"
+        msg.pose.position.x = 0.7
+        msg.pose.position.y = 0.0
+        msg.pose.position.z = 0.45
+        msg.pose.orientation.x = 0.0
+        msg.pose.orientation.y = 0.707
+        msg.pose.orientation.z = 0.0
+        msg.pose.orientation.w = 0.707
+
+        self.publisher_.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    parser = argparse.ArgumentParser(description='ROS2 Minimal Publisher')
+    parser.add_argument('--robot-name', type=str, default='arm0', help='Specify the robot name')
+    args = parser.parse_args()
+    minimal_publisher = CartesianControllerPublisher(args.robot_name)
+
+    rclpy.spin(minimal_publisher)
+
+    minimal_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

rbs_skill_servers/scripts/test_move_to_pose.py

@@ -0,0 +1,69 @@
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+
+from rbs_skill_interfaces.action import MoveitSendPose
+
+
+class TestMoveToPose(Node):
+
+    def __init__(self, robot_name: str):
+        super().__init__('test_move_to_pose')
+        self._action_client = ActionClient(self, MoveitSendPose, "/"+ robot_name + "/move_topose")
+
+    def send_goal(self, msg):
+        goal_msg = MoveitSendPose.Goal()
+        # goal_msg.target_pose
+        goal_msg.target_pose.position.x = msg["target_pose"]["position"]["x"]
+        goal_msg.target_pose.position.y = msg["target_pose"]["position"]["y"]
+        goal_msg.target_pose.position.z = msg["target_pose"]["position"]["z"]
+
+        goal_msg.target_pose.orientation.x = msg["target_pose"]["orientation"]["x"]
+        goal_msg.target_pose.orientation.y = msg["target_pose"]["orientation"]["y"]
+        goal_msg.target_pose.orientation.z = msg["target_pose"]["orientation"]["z"]
+        goal_msg.target_pose.orientation.w = msg["target_pose"]["orientation"]["w"]
+
+        goal_msg.robot_name = msg["robot_name"]
+        goal_msg.end_effector_velocity = msg["end_effector_velocity"]
+        goal_msg.end_effector_acceleration = msg["end_effector_acceleration"]
+
+        self._action_client.wait_for_server()
+
+        return self._action_client.send_goal_async(goal_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    robot_name = "arm0"
+
+    action_client = TestMoveToPose(robot_name)
+
+    msg = {
+        "constraint_mode": 0,
+        "target_pose": {
+            "position": {
+                "x": 0.2,
+                "y": 0.2,
+                "z": 0.35,
+            },
+            "orientation": {
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0,
+                "w": 1.0,
+            },
+        },
+        "robot_name": robot_name,
+        "end_effector_velocity": 1.0,
+        "end_effector_acceleration": 1.0,
+        "timeout_seconds": 0.0
+    }
+
+    future = action_client.send_goal(msg)
+
+    rclpy.spin_until_future_complete(action_client, future)
+
+
+if __name__ == '__main__':
+    main()