Hey, im using sitl simulation and gazebo. im trying to send the position i get from the gazebo to the mavlink, im trying to send the position to LOCAL_POSITION_NED (32) with publisher i create on ros.
when i send the message from ros to mavlink the position doesnt change, does anyone do something similar and can help?
this is the code to send the message :
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from pymavlink import mavutil
import time
class PoseToMavlink(Node):
def __init__(self):
super().__init__('pose_to_mavlink')
self.start_time = time.time()
self.connection = mavutil.mavlink_connection("udp:127.0.0.1:14555")
self.get_logger().info("Waiting for heartbeat from SITL...")
self.connection.wait_heartbeat()
self.get_logger().info(f"✅ Heartbeat received from system {self.connection.target_system}")
self.subscription = self.create_subscription(Odometry, "/ariel/pose_gt", self.pose_callback, 10)
self.timer = self.create_timer(0.1, self.send_mavlink_local_position)
self.current_pose = None
self.current_velocity = None
def pose_callback(self, msg):
self.current_pose = msg.pose.pose.position
self.current_velocity = msg.twist.twist.linear
def send_mavlink_local_position(self):
if self.current_pose is None:
return # No data received yet
time_boot_ms = int((time.time() - self.start_time) * 1000)
x_ned, y_ned, z_ned = self.enu_to_ned((self.current_pose.x, self.current_pose.y, self.current_pose.z))
vx_ned, vy_ned, vz_ned = self.enu_to_ned((self.current_velocity.x, self.current_velocity.y, self.current_velocity.z))
# ✅ Send `LOCAL_POSITION_NED` message
self.connection.mav.local_position_ned_send(
time_boot_ms, # Timestamp (ms)
x_ned, y_ned, z_ned, # Position (NED)
vx_ned, vy_ned, vz_ned # Velocity (NED)
)
self.get_logger().info(f"📡 Sent LOCAL_POSITION_NED: x={x_ned}, y={y_ned}, z={z_ned}, vx={vx_ned}, vy={vy_ned}, vz={vz_ned}")
def enu_to_ned(enu):
x_enu, y_enu, z_enu = enu
return y_enu, x_enu, -z_enu
def main():
rclpy.init()
node = PoseToMavlink()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()