Refactorization

hexapod_python/ik.py

@@ -30,8 +30,6 @@ standing_pose_ = HexapodPose(
     ),
     legs = [
         HexapodLeg(label="e1", position=Point(x = leg_offsets['e1'].x + -100.0, y = leg_offsets['e1'].y + 0.0, z =leg_offsets['e1'].z + -130.0)),
-
-        
     ]
 
 )
@@ -100,7 +98,6 @@ def interpolate_pose(start, end, steps):
 
     poses = []
     for i in range(0, steps):
-        print(i)
         pose = {
             'body': Pose(
                 position = Point(

hexapod_python/ik_calibration.py

@@ -0,0 +1,61 @@
+from geometry_msgs.msg import Point, Pose
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from builtin_interfaces.msg import Duration
+import rclpy
+from rclpy.node import Node
+
+from hexapod_python.ik import ik, interpolate_pose, calibration_pose, shutdown_pose
+
+import time
+
+
+class IKPublisherNode(Node):
+    def __init__(self, node_name = "ik_publisher"):
+        super().__init__(node_name)
+
+        self.leg_groups_ = [ "e1", "e2", "e3", "e4", "e5", "e6"]
+        self.create_leg_publishers()
+        ik_values = [ ik(calibration_pose)]
+
+    def create_leg_publishers(self):
+        self.publisher_ = {}
+        for leg in self.leg_groups_:            
+            controller_topic = f"/{leg}_group_controller/joint_trajectory"
+            self.publisher_[leg] = self.create_publisher(JointTrajectory, controller_topic , 10)
+
+        # wait for the controller to subscribe
+        print("Waiting for subscribers")
+        for leg in self.leg_groups_:
+            while self.count_subscribers(f"/{leg}_group_controller/joint_trajectory") == 0:                    
+                time.sleep(0.1)
+
+
+    def publish_ik_values(self, ik_values):
+        for leg in self.leg_groups_:
+            t = 0
+            joint_trajectory_points = []
+            for i in ik_values:
+                joint_trajectory_points.append(
+                    JointTrajectoryPoint(
+                        positions = i[leg],
+                        time_from_start = Duration(nanosec = t%50 * 20000000 , sec = int(t/50))
+                    )
+                )
+                t += 1
+
+
+            group_num = leg[1:]
+            trajectory = JointTrajectory(
+                joint_names = [ f"c{group_num}_joint", f"f{group_num}_joint", f"t{group_num}_joint" ],
+                points = joint_trajectory_points
+            )
+            self.publisher_[leg].publish(trajectory)
+
+def main(args = None):
+    rclpy.init(args=args)
+
+    ik_publisher = IKPublisher()
+    rclpy.spin(ik_publisher)
+    ik_publisher.destroy_node()
+    rclpy.shutdown()
+    

hexapod_python/ik_publisher_node.py

@@ -0,0 +1,49 @@
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from builtin_interfaces.msg import Duration
+from rclpy.node import Node
+
+from hexapod_python.ik import ik
+
+import time
+
+
+class IKPublisherNode(Node):
+    def __init__(self, node_name = "ik_publisher"):
+        super().__init__(node_name)
+
+        self.leg_groups_ = [ "e1", "e2", "e3", "e4", "e5", "e6"]
+        self.create_leg_publishers()
+
+    def create_leg_publishers(self):
+        self.publisher_ = {}
+        for leg in self.leg_groups_:            
+            controller_topic = f"/{leg}_group_controller/joint_trajectory"
+            self.publisher_[leg] = self.create_publisher(JointTrajectory, controller_topic , 10)
+
+        # wait for the controller to subscribe
+        print("Waiting for subscribers")
+        for leg in self.leg_groups_:
+            while self.count_subscribers(f"/{leg}_group_controller/joint_trajectory") == 0:                    
+                time.sleep(0.1)
+
+
+    def publish_ik_values(self, ik_values):
+        for leg in self.leg_groups_:
+            t = 0
+            joint_trajectory_points = []
+            for i in ik_values:
+                joint_trajectory_points.append(
+                    JointTrajectoryPoint(
+                        positions = i[leg],
+                        time_from_start = Duration(nanosec = t%50 * 20000000 , sec = int(t/50))
+                    )
+                )
+                t += 1
+
+
+            group_num = leg[1:]
+            trajectory = JointTrajectory(
+                joint_names = [ f"c{group_num}_joint", f"f{group_num}_joint", f"t{group_num}_joint" ],
+                points = joint_trajectory_points
+            )
+            self.publisher_[leg].publish(trajectory)

hexapod_python/ik_test.py

@@ -4,57 +4,24 @@ from builtin_interfaces.msg import Duration
 import rclpy
 from rclpy.node import Node
 
+from hexapod_python.ik_publisher_node import IKPublisherNode
 from hexapod_python.ik import ik, interpolate_pose, standing_pose, shutdown_pose
 
 import time
 
 
-class JTCPublisher(Node):
+class IKTestNode(IKPublisherNode):
     def __init__(self):
-            super().__init__('jtc_publisher')
-
-            self.leg_groups_ = [ "e1", "e2", "e3", "e4", "e5", "e6"]
-            self.publisher_ = {}
-
-            for leg in self.leg_groups_:            
-                controller_topic = f"/{leg}_group_controller/joint_trajectory"
-                self.publisher_[leg] = self.create_publisher(JointTrajectory, controller_topic , 10)
-
-
-            # wait for the controller to subscribe
-            print("Waiting for subscribers")
-            for tag in self.leg_groups_:
-                while self.count_subscribers(f"/{leg}_group_controller/joint_trajectory") == 0:                    
-                    time.sleep(0.1)
-
+            super().__init__('ik_test')
             ik_values = [ ik(pose) for pose in interpolate_pose(shutdown_pose, standing_pose, 150) + interpolate_pose(standing_pose, shutdown_pose, 150)]
-            print(ik_values)
-            for leg in self.leg_groups_:
-                t = 0
-                joint_trajectory_points = []
-                for i in ik_values:
-                    joint_trajectory_points.append(
-                        JointTrajectoryPoint(
-                            positions = i[leg],
-                            time_from_start = Duration(nanosec = t%50 * 20000000 , sec = int(t/50))
-                        )
-                    )
-                    t += 1
-
-
-                print(joint_trajectory_points)
-                group_num = leg[1:]
-                trajectory = JointTrajectory(
-                    joint_names = [ f"c{group_num}_joint", f"f{group_num}_joint", f"t{group_num}_joint" ],
-                    points = joint_trajectory_points
-                )
-                self.publisher_[leg].publish(trajectory)
+            self.publish_ik_values(ik_values)
+            raise SystemExit
 
 def main(args = None):
     rclpy.init(args=args)
 
-    jtc_publisher = JTCPublisher()
-    rclpy.spin(jtc_publisher)
-    jtc_publisher.destroy_node()
+    ik_publisher = IKTestNode()
+    rclpy.spin_once(ik_publisher)
+    ik_publisher.destroy_node()
     rclpy.shutdown()
     

setup.py

@@ -21,7 +21,8 @@ setup(
     entry_points={
         'console_scripts': [
             'jtc_test = hexapod_python.jtc_test:main',
-            'ik_test = hexapod_python.ik_test:main'
+            'ik_test = hexapod_python.ik_test:main',
+            'ik_calibration = hexapod_python.ik_calibration:main'
         ],
     },
 )