Control a Simulated UAV Using ROS 2 and PX4 Bridge
This example demonstrates how to receive sensor readings and autopilot status from a simulated UAV with PX4 autopilot, and send control commands to navigate the simulated UAV.
Set Up Simulation Environment
In MATLAB®, generate custom messages for the
px4_msgs package. You can add this to an existing custom message folder and regenerate it, or generate them in this example folder. This can take some time, as there are a large number of messages to generate.
Download and connect to the virtual machine (VM) using instructions in Get Started with Gazebo and Simulated TurtleBot (ROS Toolbox). On the VM, start the simulator and PX4 Bridge by clicking the "Gazebo PX4 SITL RTPS" desktop shortcut. This brings up a simple simulator of a quadrotor in Gazebo.
This also brings up the Agent half of the
microRTPS bridge. The Client half of the communication is included in the PX4 Autopilot packages, and launches with the simulator.
You can enter PX4 commands on the terminal corresponding to the Gazebo simulation, but that is not required for this example.
Set Up ROS 2 Network
View the available topics that are broadcast by the bridge Agent. Notice the convention it uses to indicate the topics it is publishes to (
out) and those it is subscribing to (
ros2 topic list
/LaserScan/out /clock /fmu/collision_constraints/out /fmu/debug_array/in /fmu/debug_key_value/in /fmu/debug_value/in /fmu/debug_vect/in /fmu/offboard_control_mode/in /fmu/onboard_computer_status/in /fmu/optical_flow/in /fmu/position_setpoint/in /fmu/position_setpoint_triplet/in /fmu/sensor_combined/out /fmu/telemetry_status/in /fmu/timesync/in /fmu/timesync/out /fmu/trajectory_bezier/in /fmu/trajectory_setpoint/in /fmu/trajectory_waypoint/out /fmu/vehicle_command/in /fmu/vehicle_control_mode/out /fmu/vehicle_local_position_setpoint/in /fmu/vehicle_mocap_odometry/in /fmu/vehicle_odometry/out /fmu/vehicle_status/out /fmu/vehicle_trajectory_bezier/in /fmu/vehicle_trajectory_waypoint/in /fmu/vehicle_trajectory_waypoint_desired/out /fmu/vehicle_visual_odometry/in /parameter_events /rosout /timesync_status
Create a node to handle the sensor input and control feedback for the UAV. Create subscribers for sensors and information of interest, and publishers to direct the UAV in offboard control mode.
node = ros2node("/control_node"); % General information about the UAV system controlModePub = ros2publisher(node,"fmu/offboard_control_mode/in","px4_msgs/OffboardControlMode"); statusSub = ros2subscriber(node,"/fmu/vehicle_status/out","px4_msgs/VehicleStatus"); timeSub = ros2subscriber(node,"fmu/timesync/out","px4_msgs/Timesync"); % Sensor and control communication odomSub = ros2subscriber(node,"/fmu/vehicle_odometry/out","px4_msgs/VehicleOdometry"); setpointPub = ros2publisher(node,"fmu/trajectory_setpoint/in","px4_msgs/TrajectorySetpoint"); cmdPub = ros2publisher(node,"/fmu/vehicle_command/in","px4_msgs/VehicleCommand");
Get the system and component IDs from the UAV status. These help direct the commands to the UAV. This also ensures the UAV is up and running before moving into the control phase.
timeLimit = 5; statusMsg = receive(statusSub,timeLimit); receive(odomSub,timeLimit); receive(timeSub,timeLimit);
Test MATLAB Communication with Gazebo instance
Use the previously created publisher and subscriber to instruct the UAV to take off, fly to a new point, and then land. To perform offboard control, the UAV requires control messages to be flowing regularly (at least 2 Hz). Starting the control messages before engaging offboard mode is the easiest way to achieve this.
% Start flow of control messages tStart = tic; xyz = [0 0 -1]; % NED coordinates - negative Z is higher altitude while toc(tStart) < 1 publishOffboardControlMode(timeSub,controlModePub,"position") publishTrajectorySetpoint(timeSub,setpointPub,xyz) pause(0.1) end % Instruct the UAV to accept offboard commands % Arm the UAV so it allows flying engageOffboardMode(timeSub,cmdPub) arm(timeSub,cmdPub) % Navigate to a nearby location and hover there while toc(tStart) < 21 publishOffboardControlMode(timeSub,controlModePub,"position") publishTrajectorySetpoint(timeSub,setpointPub,xyz) pause(0.1) end % Land once complete land(timeSub,cmdPub)
Control UAV from Simulink
You can also control the UAV using the Simulink® model
ControlUAVUsingROS2. The Simulink model includes four main subsystems: Arm, Offboard, Takeoff, and Waypoint following with obstacle avoidance.
A Clock block triggers different UAV activity according to the simulation time. The model arms and enables the offboard control mode on the PX4 autopilot for the first 2 seconds. It sends out a takeoff command to bring the UAV to 1 meter above the ground for the first 8 seconds, then it engages in waypoint-following behavior.
You can use the control panel to enable or disable the obstacle avoidance behavior.
If obstacle avoidance is turned off, the UAV flies towards the goal point behind the obstacle box on a straight path, resulting in a collision. If obstacle avoidance is turned on, the UAV tries to fly above the obstacle based on the lidar sensor readings.
Once you start a fresh PX4-SITL and Gazebo instance in the VM, you can run the model and observe the flight behavior of the UAV.
function arm(timeSub,cmdPub) % Allow UAV flight cmdMsg = ros2message(cmdPub); cmdMsg.command = uint32(cmdMsg.VEHICLE_CMD_COMPONENT_ARM_DISARM); cmdMsg.param1 = single(1); publishVehicleCommand(timeSub,cmdPub,cmdMsg) end function land(timeSub,cmdPub) % Land the UAV cmdMsg = ros2message(cmdPub); cmdMsg.command = uint32(cmdMsg.VEHICLE_CMD_NAV_LAND); publishVehicleCommand(timeSub,cmdPub,cmdMsg) end function engageOffboardMode(timeSub,cmdPub) % Allow offboard control messages to be utilized cmdMsg = ros2message(cmdPub); cmdMsg.command = uint32(cmdMsg.VEHICLE_CMD_DO_SET_MODE); cmdMsg.param1 = single(1); cmdMsg.param2 = single(6); publishVehicleCommand(timeSub,cmdPub,cmdMsg) end function publishOffboardControlMode(timeSub,controlModePub,controlType) % Set the type of offboard control to be used % controlType must match the field name in the OffboardControlMode message modeMsg = ros2message(controlModePub); modeMsg.timestamp = timeSub.LatestMessage.timestamp; % Initially set all to false modeMsg.position = false; modeMsg.velocity = false; modeMsg.acceleration = false; modeMsg.attitude = false; modeMsg.body_rate = false; % Override desired control mode to true modeMsg.(controlType) = true; send(controlModePub,modeMsg) end function publishTrajectorySetpoint(timeSub,setpointPub, xyz) setpointMsg = ros2message(setpointPub); setpointMsg.timestamp = timeSub.LatestMessage.timestamp; setpointMsg.x = single(xyz(1)); setpointMsg.y = single(xyz(2)); setpointMsg.z = single(xyz(3)); send(setpointPub,setpointMsg) end function publishVehicleCommand(timeSub,cmdPub,cmdMsg) cmdMsg.timestamp = timeSub.LatestMessage.timestamp; cmdMsg.target_system = uint8(1); cmdMsg.target_component = uint8(1); cmdMsg.source_system = uint8(1); cmdMsg.source_component = uint8(1); cmdMsg.from_external = true; send(cmdPub,cmdMsg) end