Hello, teachers. I am currently working on writing a ROS Python code where, when I input the coordinates (4 points) of a target location in the simulation, the vehicle flies to these locations sequentially. My version is ROS2 Humble, and I am using uORB. Based on advice and recommendations from many people, I am using the geodetic2enu module to write and use convert_global_to_local_waypoint. It seems like the conversion is working fine, but there is no response from /fmu/in/trajectory_setpoint. I would appreciate it if you could provide me with the code or method used when moving to the local position based on the initial position.
Here is the conversion I wrote :
def convert_global_to_local_waypoint(self, home_position_gps):
# Check if home_position_gps contains valid lat, lon, alt
if not isinstance(home_position_gps, (list, tuple)) or len(home_position_gps[0]) != 3:
return
# Use only the first waypoint as the home position
home_position = home_position_gps[0]
# Set home position based on local position
self.home_position = self.vehicle_local_position
# Convert global GPS waypoints to local ENU coordinates
self.setpoint_position = []
for i in range(self.point_flag + 1):
waypoint = self.way_point_gps[i]
mission_position = geodetic2enu(waypoint[0], waypoint[1], waypoint[2],
home_position[0], home_position[1], home_position[2])
mission_position = np.array(mission_position)
self.setpoint_position.append(mission_position)
# Add final setpoint for landing at the takeoff height
self.setpoint_position.append(np.array([0.0, 0.0, -self.takeoff_height]))
self.get_logger().info(f"Converted ENU Waypoints: {self.setpoint_position}")