From 555f9a8e137461acfee158dcebd491b5ceec70d9 Mon Sep 17 00:00:00 2001
From: Marcus Grieger <marcus@grieger.xyz>
Date: Mon, 5 Feb 2024 21:38:41 +0100
Subject: [PATCH] multi-leg test
---
hexapod_python/jtc_test.py | 47 ++++++++++++++++++++++++--------------
1 file changed, 30 insertions(+), 17 deletions(-)
diff --git a/hexapod_python/jtc_test.py b/hexapod_python/jtc_test.py
index 37b8685..73d1a35 100644
--- a/hexapod_python/jtc_test.py
+++ b/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)