multi-leg test

hexapod_python/jtc_test.py

@@ -6,25 +6,38 @@ from rclpy.node import Node
 class JTCPublisher(Node):
     def __init__(self):
             super().__init__('jtc_publisher')
-            self.publisher_ = self.create_publisher(JointTrajectory, "/e1_group_controller/joint_trajectory", 10)
 
+            self.group_tags_ = [ "e1", "e2", "e3", "e4", "e5", "e6"]
+            
+            self.publisher_ = {}
 
-            trajectory = JointTrajectory(
-                joint_names = [ "c1_joint", "f1_joint", "t1_joint" ],
-                  points = [ 
-                        JointTrajectoryPoint (time_from_start = Duration(sec = 1), positions = [0,    -1.4,   0]),
-                        JointTrajectoryPoint (time_from_start = Duration(sec = 2), positions = [-0.3, -1.3,   0]),
-                        JointTrajectoryPoint (time_from_start = Duration(sec = 3), positions = [0,    -1.4, 0.5]),
-                        JointTrajectoryPoint (time_from_start = Duration(sec = 4), positions = [-0.3, -1.3,   0]),
-                        JointTrajectoryPoint (time_from_start = Duration(sec = 5), positions = [-0.5, -1.3,   0.6]),
-                        JointTrajectoryPoint (time_from_start = Duration(sec = 6), positions = [-0.3, -1.3,   0.6]),
-                        JointTrajectoryPoint (time_from_start = Duration(sec = 7), positions = [-0.5, -1.3,   0.6]),
-                        JointTrajectoryPoint (time_from_start = Duration(sec = 8), positions = [-0.5, -1.3,   0]),
-                        JointTrajectoryPoint (time_from_start = Duration(sec = 9), positions = [-0.3, -1.3,   0]),
-                        JointTrajectoryPoint (time_from_start = Duration(sec = 10), positions =[   0, -1.4,   0]),
-                  ]
-            )
-            self.publisher_.publish(trajectory)
+            for tag in self.group_tags_:
+            
+                group_num = tag[1:]
+                controller_topic = f"/{tag}_group_controller/joint_trajectory"
+                print(controller_topic, tag, group_num)
+
+                direction = 1
+                if int(group_num)>3:
+                    direction = -1
+
+                self.publisher_[tag] = self.create_publisher(JointTrajectory, controller_topic , 10)
+                trajectory = JointTrajectory(
+                    joint_names = [ f"c{group_num}_joint", f"f{group_num}_joint", f"t{group_num}_joint" ],
+                    points = [ 
+                            JointTrajectoryPoint (time_from_start = Duration(sec = 1), positions = [direction * 0,    direction * -1.4, direction *   0]),
+                            JointTrajectoryPoint (time_from_start = Duration(sec = 2), positions = [direction * -0.3, direction * -1.3, direction *   0]),
+                            JointTrajectoryPoint (time_from_start = Duration(sec = 3), positions = [direction * 0,    direction * -1.4, direction * 0.5]),
+                            JointTrajectoryPoint (time_from_start = Duration(sec = 4), positions = [direction * -0.3, direction * -1.3, direction *   0]),
+                            JointTrajectoryPoint (time_from_start = Duration(sec = 5), positions = [direction * -0.5, direction * -1.3, direction *   0.6]),
+                            JointTrajectoryPoint (time_from_start = Duration(sec = 6), positions = [direction * -0.3, direction * -1.3, direction *   0.6]),
+                            JointTrajectoryPoint (time_from_start = Duration(sec = 7), positions = [direction * -0.5, direction * -1.3, direction *   0.6]),
+                            JointTrajectoryPoint (time_from_start = Duration(sec = 8), positions = [direction * -0.5, direction * -1.3, direction *   0]),
+                            JointTrajectoryPoint (time_from_start = Duration(sec = 9), positions = [direction * -0.3, direction * -1.3, direction *   0]),
+                            JointTrajectoryPoint (time_from_start = Duration(sec = 10), positions =[direction *    0, direction * -1.4, direction *   0]),
+                    ]
+                )
+                self.publisher_[tag].publish(trajectory)
 
 def main(args = None):
     rclpy.init(args=args)