add gimbal yaw and pitch command ros2 message

castacks · Jan 16, 2025 · 00c82a9 · 00c82a9
1 parent 5dc5121
commit 00c82a9
Showing 1 changed file with 18 additions and 5 deletions.
diff --git a/...s_ws/src/autonomy/1_sensors/gimbal_stabilizer/gimbal_stabilizer/gimbal_stabilizer_node.py b/...s_ws/src/autonomy/1_sensors/gimbal_stabilizer/gimbal_stabilizer/gimbal_stabilizer_node.py
@@ -5,6 +5,7 @@
 from nav_msgs.msg import Odometry
 from sensor_msgs.msg import JointState
 from transforms3d.euler import quat2euler
+from std_msgs.msg import Float64  # Assuming the desired yaw is published as a Float64
 
 class GimbalStabilizerNode(Node):
     def __init__(self):
@@ -16,13 +17,25 @@ def __init__(self):
         # Subscriber to receive drone odometry
         self.create_subscription(Odometry, '/robot_1/odometry_conversion/odometry', self.odometry_callback, 10)
         self.create_subscription(JointState, '/joint_states', self.joint_callback, 10)
+        self.create_subscription(Float64, '/desired_gimbal_yaw', self.yaw_callback, 10)
+        self.create_subscription(Float64, '/desired_gimbal_pitch', self.pitch_callback, 10)
 
         # Initialize joint state message
         self.joint_command = JointState()
         self.joint_command.name = ["yaw_joint","roll_joint", "pitch_joint"]
         self.joint_command.position = [0.0, 0.0, 0.0]
+        self.desired_yaw = 0.0
+        self.desired_pitch = 0.0
+
+    def yaw_callback(self, msg):
+        self.desired_yaw = msg.data
+        # self.get_logger().info(f"Received desired yaw angle: {self.desired_yaw}")
+
+    def pitch_callback(self, msg):
+        self.desired_pitch = msg.data
 
     def joint_callback(self, msg):
+        self.got_joint_states = True
         # Inverse the drone angles to stabilize the gimbal
         # self.joint_command.position[0] = -roll  # roll joint
         # self.joint_command.position[1] = -pitch  # pitch joint
@@ -33,7 +46,7 @@ def joint_callback(self, msg):
         # self.joint_command.position[0] = -20.0/180*3.14  # yaw joint
         # self.joint_command.position[1] = 10.0/180*3.14  # roll joint
         # self.joint_command.position[2] = 20.0/180*3.14  # pitch joint
-        self.joint_command.velocity = [float('nan'), float('nan'), float('nan')]
+        # self.joint_command.velocity = [float('nan'), float('nan'), float('nan')]
         # self.joint_command.velocity = [-1.0, -1.0, -1.0]
 
         # Publish the joint command
@@ -57,11 +70,11 @@ def odometry_callback(self, msg):
         # self.joint_command.position[1] = -pitch  # pitch joint
         # self.joint_command.position[2] = -yaw  # yaw joint
 
-        self.joint_command.position[0] = -0.0/180*3.14  # yaw joint
-        self.joint_command.position[1] = -roll  # roll joint
-        self.joint_command.position[2] = 0.0/180*3.14  # pitch joint
+        self.joint_command.position[0] = -self.desired_yaw/180*3.14  # yaw joint
+        self.joint_command.position[1] = -roll/180*3.14  # roll joint
+        self.joint_command.position[2] = self.desired_pitch/180*3.14  # pitch joint
         self.joint_command.velocity = [float('nan'), float('nan'), float('nan')]
-        self.joint_command.velocity = [-1.0, -1.0, -1.0]
+        # self.joint_command.velocity = [-1.0, -1.0, -1.0]
 
         # Publish the joint command
         self.joint_pub.publish(self.joint_command)