I am trying to use ROS2 Humble and PX4 usiing the microDDS communications to offboard control the standard VTOL airframe to different lat/long waypoints using SITL. I have tried using MAVLINK command 82 which did not work, so then I conerted the long / lat points into local NED points. Below is my timer_callback function in my offboard control node, it is just a modified version of the offboard control code in the PX4 documentation:
def timer_callback(self) -> None:
"""Callback function for the timer."""
self.publish_offboard_control_heartbeat_signal()
if self.offboard_setpoint_counter == 10:
self.engage_offboard_mode()
self.arm()
if self.vehicle_local_position.z > self.takeoff_height and self.vehicle_status.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD:
self.publish_vehicle_command_vtol_transition(2)
self.publish_position_setpoint(0.0, 0.0, self.takeoff_height)
if self.vehicle_local_position.z < self.takeoff_height + 0.2:
self.publish_velocity_setpoint(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)
self.publish_vehicle_command_vtol_transition(1)
self.publish_position_setpoint(117.5 -240.0, self.takeoff_height)
if self.offboard_setpoint_counter < 11:
self.offboard_setpoint_counter += 1
The VTOL will takeoff fine, and then not go to the waypoint I want it to go to, and just hover forever instead. When I run this code with the PX4 SITL simulation (make px4_sitl gz_standard_vtol) this is what I get from PX4:
px4 starting.
INFO [px4] startup script: /bin/sh etc/init.d-posix/rcS 0
INFO [init] found model autostart file as SYS_AUTOSTART=4004
INFO [param] selected parameter default file parameters.bson
INFO [param] importing from 'parameters.bson'
INFO [parameters] BSON document size 316 bytes, decoded 316 bytes (INT32:13, FLOAT:3)
INFO [param] selected parameter backup file parameters_backup.bson
INFO [dataman] data manager file './dataman' size is 7868392 bytes
INFO [init] Gazebo simulator
SIM_GZ_HOME_LAT: curr: 47.3977 -> new: 48.5102
SIM_GZ_HOME_LON: curr: 8.5456 -> new: -71.6473
SIM_GZ_HOME_ALT: curr: 488.0000 -> new: 0.0000
INFO [init] starting gazebo with world: /home/gabe/PX4-Autopilot/Tools/simulation/gz/worlds/default.sdf
WARN [init] PX4_GZ_MODEL_POSE not set, spawning at origin.
INFO [gz_bridge] world: default, model name: standard_vtol_0, simulation model: standard_vtol
INFO [gz_bridge] Requested Model Position: 0,0,0,0,0,0
INFO [gz_bridge] Model position z is less or equal 0.0, moving upwards
Error [Utils.cc:174] Missing element description for [persistent]
INFO [lockstep_scheduler] setting initial absolute time to 8000 us
WARN [vehicle_angular_velocity] no gyro selected, using sensor_gyro:0 1310988
INFO [commander] LED: open /dev/led0 failed (22)
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
INFO [tone_alarm] home set
INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030
INFO [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280
INFO [logger] logger started (mode=all)
INFO [logger] Start file log (type: full)
INFO [logger] [logger] ./log/2024-01-30/01_28_42.ulg
INFO [logger] Opened full log file: ./log/2024-01-30/01_28_42.ulg
INFO [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO [px4] Startup script returned successfully
pxh> libEGL warning: egl: failed to create dri2 screen
libEGL warning: egl: failed to create dri2 screen
INFO [airspeed_selector] No airspeed sensor detected. Switch to non-airspeed mode.
WARN [health_and_arming_checks] Preflight Fail: Airspeed invalid
INFO [airspeed_selector] Airspeed sensor healthy, start using again (-1, 1)
INFO [commander] Ready for takeoff!
INFO [mavlink] partner IP: 127.0.0.1
INFO [uxrce_dds_client] synchronized with time offset 1706578122541600us
INFO [uxrce_dds_client] successfully created rt/fmu/out/battery_status data writer, topic id: 19
INFO [uxrce_dds_client] successfully created rt/fmu/out/estimator_status_flags data writer, topic id: 86
INFO [uxrce_dds_client] successfully created rt/fmu/out/failsafe_flags data writer, topic id: 92
INFO [uxrce_dds_client] successfully created rt/fmu/out/manual_control_setpoint data writer, topic id: 138
INFO [uxrce_dds_client] successfully created rt/fmu/out/position_setpoint_triplet data writer, topic id: 169INFO [uxrce_dds_client] successfully created rt/fmu/out/sensor_combined data writer, topic id: 191
INFO [uxrce_dds_client] successfully created rt/fmu/out/timesync_status data writer, topic id: 214
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_attitude data writer, topic id: 230
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_control_mode data writer, topic id: 237
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_command_ack data writer, topic id: 234
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_global_position data writer, topic id: 238
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_gps_position data writer, topic id: 240
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_local_position data writer, topic id: 244
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_odometry data writer, topic id: 249
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_status data writer, topic id: 254
INFO [commander] Armed by external command
INFO [tone_alarm] arming warning
INFO [commander] Takeoff detected
WARN [timesync] time jump detected. Resetting time synchroniser.
WARN [timesync] time jump detected. Resetting time synchroniser.
WARN [timesync] time jump detected. Resetting time synchroniser.
WARN [timesync] time jump detected. Resetting time synchroniser.
WARN [timesync] time jump detected. Resetting time synchroniser.
WARN [failsafe] Failsafe activated
INFO [tone_alarm] battery warning (fast)
ERROR [flight_mode_manager] Matching flight task was not able to run, Nav state: 2, Task: 1
PX4 Exiting...
pxh> Exiting NOW.
ninja: build stopped: interrupted by user.
make: *** [Makefile:232: px4_sitl] Interrupt
I have looked into the error but have not been able to find a solution that works.
In addition to this, my ROS2 node now outputs the following error (this is new and worrying) after I deleted and recloned PX4 autopilot on my laptop due to errors with the GPS and Airspeed sensor in SITL (GPS horizontal Pos shift too high, no airspeed sensor detected):
2024-01-29 21:12:16.160 [RTPS_READER_HISTORY Error] Change payload size of '204' bytes is larger than the history payload size of '199' bytes and cannot be resized. -> Function can_change_be_added_nts
^CTraceback (most recent call last):
File "/home/gabe/QADT_REPOS/local_ros2_ws/src/vtol_visualizer/vtol_visualizer/./controls.py", line 169, in <module>
main()
File "/home/gabe/QADT_REPOS/local_ros2_ws/src/vtol_visualizer/vtol_visualizer/./controls.py", line 163, in main
rclpy.spin(offboard_control)
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/__init__.py", line 222, in spin
executor.spin_once()
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 705, in spin_once
handler, entity, node = self.wait_for_ready_callbacks(timeout_sec=timeout_sec)
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 691, in wait_for_ready_callbacks
return next(self._cb_iter)
File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 588, in _wait_for_ready_callbacks
wait_set.wait(timeout_nsec)
KeyboardInterrupt
2024-01-29 21:12:16.168 [RTPS_READER_HISTORY Error] Change payload size of '204' bytes is larger than the history payload size of '199' bytes and cannot be resized. -> Function can_change_be_added_nts
2024-01-29 21:12:16.180 [RTPS_READER_HISTORY Error] Change payload size of '204' bytes is larger than the history payload size of '199' bytes and cannot be resized. -> Function can_change_be_added_nts
Much help would be greatly appreciated with this error as well.
Thank you for reading and let me know if more information is required!