First try at /cmd_vel control

hexapod_python/ik_calibration.py

@@ -1,4 +1,4 @@
-from geometry_msgs.msg import Point, Pose
+from geometry_msgs.msg import Point, Pose, Twist
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from builtin_interfaces.msg import Duration
 import rclpy
@@ -8,18 +8,36 @@ from hexapod_python.ik_publisher_node import IKPublisherNode
 from hexapod_python.ik import ik
 from hexapod_python.ik_poses import calibration_pose
 
+import copy
+
 class IKCalibrationNode(IKPublisherNode):
     def __init__(self):
             super().__init__('ik_calibration')
-            ik_values = [ ik(calibration_pose)]
+            self.modified_calibration_pose = copy.deepcopy(calibration_pose)
+            ik_values = [ ik(self.modified_calibration_pose)]
             self.publish_ik_values(ik_values)
-            raise SystemExit
+
+            self.cmd_vel_subscription = self.create_subscription(
+                 Twist,
+                 "/cmd_vel",
+                 self.twist_callback,
+                 10                
+            )
+    
+    def twist_callback(self, msg):
+        print(msg)
+        self.modified_calibration_pose['legs']['e1'].x -= msg.linear.x
+        self.modified_calibration_pose['legs']['e1'].y -= msg.linear.y
+        self.modified_calibration_pose['legs']['e1'].z -= msg.linear.z
+        ik_values = [ ik(self.modified_calibration_pose)]
+        self.publish_ik_values(ik_values)
+
 
 def main(args = None):
     rclpy.init(args=args)
 
     ik_publisher = IKCalibrationNode()
-    rclpy.spin_once(ik_publisher)
+    rclpy.spin(ik_publisher)
     ik_publisher.destroy_node()
     rclpy.shutdown()