Generate ROS Node for UAV Waypoint Follower

This example uses:

This example shows how to use MATLAB® code generation to create a ROS node to move an unmanned aerial vehicle (UAV) along a predefined circular path and a set of specified custom waypoints.

In this example, you deploy a MATLAB function as a standalone ROS node to control a simulated PX4® UAV with Gazebo. For more information on ROS node generation from MATLAB code, see MATLAB Programming for Code Generation.

Start PX4 SITL Gazebo Simulator

Download a virtual machine using instructions in Get Started with Gazebo and Simulated TurtleBot.

Start the Ubuntu® virtual machine desktop.
In the Ubuntu desktop, click the Gazebo PX4 SITL icon to start the simulated PX4 Gazebo world.

Specify the IP address and port number of the ROS master in Gazebo so that MATLAB® can communicate with the PX4 simulator. For this example, the ROS master in Gazebo is http://192.168.192.136:11311 and your host computer address is 192.168.31.1.
Start the ROS network using rosinit.

ipaddress = '192.168.93.136';
%Shut down global node if one is already running
rosshutdown();

Shutting down global node /matlab_global_node_29172 with NodeURI http://192.168.93.1:62022/

rosinit(ipaddress,11311);

Initializing global node /matlab_global_node_22217 with NodeURI http://192.168.93.1:62084/

View the ROS topics available on the network. Verify that the topics with the namespace /mavros are available.

rostopic("list");

/clock                                     
/diagnostics                               
/gazebo/link_states                        
/gazebo/model_states                       
/gazebo/parameter_descriptions             
/gazebo/parameter_updates                  
/gazebo/set_link_state                     
/gazebo/set_model_state                    
/mavlink/from                              
/mavlink/gcs_ip                            
/mavlink/to                                
/mavros/actuator_control                   
/mavros/adsb/send                          
/mavros/adsb/vehicle                       
/mavros/altitude                           
/mavros/battery                            
/mavros/cam_imu_sync/cam_imu_stamp         
/mavros/companion_process/status           
/mavros/debug_value/debug                  
/mavros/debug_value/debug_vector           
/mavros/debug_value/named_value_float      
/mavros/debug_value/named_value_int        
/mavros/debug_value/send                   
/mavros/esc_info                           
/mavros/esc_status                         
/mavros/estimator_status                   
/mavros/extended_state                     
/mavros/fake_gps/mocap/tf                  
/mavros/geofence/waypoints                 
/mavros/global_position/compass_hdg        
/mavros/global_position/global             
/mavros/global_position/gp_lp_offset       
/mavros/global_position/gp_origin          
/mavros/global_position/home               
/mavros/global_position/local              
/mavros/global_position/raw/fix            
/mavros/global_position/raw/gps_vel        
/mavros/global_position/raw/satellites     
/mavros/global_position/rel_alt            
/mavros/global_position/set_gp_origin      
/mavros/gps_rtk/rtk_baseline               
/mavros/gps_rtk/send_rtcm                  
/mavros/gpsstatus/gps1/raw                 
/mavros/gpsstatus/gps1/rtk                 
/mavros/gpsstatus/gps2/raw                 
/mavros/gpsstatus/gps2/rtk                 
/mavros/hil/actuator_controls              
/mavros/hil/controls                       
/mavros/hil/gps                            
/mavros/hil/imu_ned                        
/mavros/hil/optical_flow                   
/mavros/hil/rc_inputs                      
/mavros/hil/state                          
/mavros/home_position/home                 
/mavros/home_position/set                  
/mavros/imu/data                           
/mavros/imu/data_raw                       
/mavros/imu/diff_pressure                  
/mavros/imu/mag                            
/mavros/imu/static_pressure                
/mavros/imu/temperature_baro               
/mavros/imu/temperature_imu                
/mavros/landing_target/lt_marker           
/mavros/landing_target/pose                
/mavros/landing_target/pose_in             
/mavros/local_position/accel               
/mavros/local_position/odom                
/mavros/local_position/pose                
/mavros/local_position/pose_cov            
/mavros/local_position/velocity_body       
/mavros/local_position/velocity_body_cov   
/mavros/local_position/velocity_local      
/mavros/log_transfer/raw/log_data          
/mavros/log_transfer/raw/log_entry         
/mavros/manual_control/control             
/mavros/manual_control/send                
/mavros/mission/reached                    
/mavros/mission/waypoints                  
/mavros/mocap/pose                         
/mavros/mount_control/command              
/mavros/mount_control/orientation          
/mavros/obstacle/send                      
/mavros/odometry/in                        
/mavros/odometry/out                       
/mavros/onboard_computer/status            
/mavros/param/param_value                  
/mavros/play_tune                          
/mavros/px4flow/ground_distance            
/mavros/px4flow/raw/optical_flow_rad       
/mavros/px4flow/raw/send                   
/mavros/px4flow/temperature                
/mavros/radio_status                       
/mavros/rallypoint/waypoints               
/mavros/rc/in                              
/mavros/rc/out                             
/mavros/rc/override                        
/mavros/setpoint_accel/accel               
/mavros/setpoint_attitude/cmd_vel          
/mavros/setpoint_attitude/thrust           
/mavros/setpoint_position/global           
/mavros/setpoint_position/global_to_local  
/mavros/setpoint_position/local            
/mavros/setpoint_raw/attitude              
/mavros/setpoint_raw/global                
/mavros/setpoint_raw/local                 
/mavros/setpoint_raw/target_attitude       
/mavros/setpoint_raw/target_global         
/mavros/setpoint_raw/target_local          
/mavros/setpoint_trajectory/desired        
/mavros/setpoint_trajectory/local          
/mavros/setpoint_velocity/cmd_vel          
/mavros/setpoint_velocity/cmd_vel_unstamped
/mavros/state                              
/mavros/statustext/recv                    
/mavros/statustext/send                    
/mavros/target_actuator_control            
/mavros/time_reference                     
/mavros/timesync_status                    
/mavros/trajectory/desired                 
/mavros/trajectory/generated               
/mavros/trajectory/path                    
/mavros/vfr_hud                            
/mavros/vision_pose/pose                   
/mavros/vision_pose/pose_cov               
/mavros/vision_speed/speed_twist_cov       
/mavros/wind_estimation                    
/rosout                                    
/rosout_agg                                
/tf                                        
/tf_static

Generate Custom Messages

This example uses MAVROS to control the PX4 drone using the mavros_msgs package.

Verify that the messages are visible in MATLAB:

msgs = rosmsg("list");
mavrosMsgs = msgs(startsWith(msgs,"mavros_msgs"))

mavrosMsgs = 127×1 cell
    {'mavros_msgs/ADSBVehicle'                   }
    {'mavros_msgs/ActuatorControl'               }
    {'mavros_msgs/Altitude'                      }
    {'mavros_msgs/AttitudeTarget'                }
    {'mavros_msgs/BatteryStatus'                 }
    {'mavros_msgs/CamIMUStamp'                   }
    {'mavros_msgs/CommandBoolRequest'            }
    {'mavros_msgs/CommandBoolResponse'           }
    {'mavros_msgs/CommandCode'                   }
    {'mavros_msgs/CommandHomeRequest'            }
    {'mavros_msgs/CommandHomeResponse'           }
    {'mavros_msgs/CommandIntRequest'             }
    {'mavros_msgs/CommandIntResponse'            }
    {'mavros_msgs/CommandLongRequest'            }
    {'mavros_msgs/CommandLongResponse'           }
    {'mavros_msgs/CommandTOLRequest'             }
    {'mavros_msgs/CommandTOLResponse'            }
    {'mavros_msgs/CommandTriggerControlRequest'  }
    {'mavros_msgs/CommandTriggerControlResponse' }
    {'mavros_msgs/CommandTriggerIntervalRequest' }
    {'mavros_msgs/CommandTriggerIntervalResponse'}
    {'mavros_msgs/CommandVtolTransitionRequest'  }
    {'mavros_msgs/CommandVtolTransitionResponse' }
    {'mavros_msgs/CompanionProcessStatus'        }
    {'mavros_msgs/DebugValue'                    }
    {'mavros_msgs/ESCInfo'                       }
    {'mavros_msgs/ESCInfoItem'                   }
    {'mavros_msgs/ESCStatus'                     }
    {'mavros_msgs/ESCStatusItem'                 }
    {'mavros_msgs/EstimatorStatus'               }
      ⋮

Fly the PX4 in a Circular Path

Open the MATLAB function px4sitlCircularLoop and examine the code.

The function has three sections:

The initialization sequence
The controller loop that moves the PX4 along a circular path
The landing sequence.

Initialization and Circular Path Waypoint Setup

Declare all the required ROS publishers, subscribers, and service clients to communicate over MAVROS.

stateSub = rossubscriber("/mavros/state","mavros_msgs/State","DataFormat","struct");
% Create a SetMode service client
setModeCli = rossvcclient("/mavros/set_mode","mavros_msgs/SetMode","DataFormat","struct");
% Create an arm command service client
armingCli = rossvcclient("/mavros/cmd/arming","mavros_msgs/CommandBool","DataFormat","struct");
% Subscribe to landing message
landNowSub = rossubscriber("/land_message","std_msgs/Bool","DataFormat","struct");
% Subscribe to current position of PX4
currPosSub = rossubscriber("/mavros/local_position/pose","geometry_msgs/PoseStamped","DataFormat","struct");
% Create a publisher for PX4 position command
posePub = rospublisher("/mavros/setpoint_position/local","geometry_msgs/PoseStamped","DataFormat","struct");
poseMsg = rosmessage(posePub);

Create a circular path along the origin.

radius = 5.0; % radius in meters
% Set the number of steps to divide the circular path
numSteps = 100.0;
numPoints = radius * numSteps;
% Create [x,y] points along a circle with specified radius
xpoints = radius * sin(linspace(-pi,pi,numPoints));
ypoints = radius * cos(linspace(-pi,pi,numPoints));
altitude = 2.0; % meters

if isempty(coder.target)
    figure("Name","Circular trajectory");
    plot3(xpoints,ypoints,altitude*ones(1,numPoints));
    grid on;
end

The numSteps variable controls the granularity of the waypoints.

The rate with which the topic is published is controlled by the rosrate function. For this example, set the rate to 20 Hz.

% Publish the messages at 20 Hz
r = rosrate(20);

Circular Path Follower Control Loop

In px4sitlCircularLoop function, the control loop initializes the takeoff sequence by changing the mode and arming PX4. If the PX4 is ready to accept commands, this function returns immediately.

exampleHelperUAVTakeOff(stateSub,setModeCli,armingCli,5.0);

PX4 OFFBOARD mode set

The control loop sets the X and Y fields of the pose message of type geometry_msgs/PoseStamped and sends the message over the /mavros/setpoint_position/local topic. The altitude is maintained at 2 meters.

In MATLAB, the control loop exits after PX4 follows the circular path three times.

In C++ ROS node deployed from px4sitlCircularLoop function, the control loop subscribes to the /land_message topic of type std_msgs/Bool and exits when the message value is set to true.

After exiting the control loop, the function runs the landing sequence.

Landing Sequence

During landing, the MATLAB function calculates the waypoints from current position on the circular path back to the origin. The PX4 follows the waypoints to reach the origin position while maintaining the altitude. After it reaches the origin, the exampleHelperUAVLand function lands the PX4.

exampleHelperUAVLand(posePub,poseMsg,r);

Landed successfully

Prepare Workspace on Ubuntu VM

Before deploying the node on the VM, create a Catkin workspace folder by running the following commands in MATLAB.

vmdev = rosdevice(ipaddress,'user','password');
vmdev.CatkinWorkspace = '/tmp/px4stil_catkinws';
system(vmdev,['mkdir -p ', vmdev.CatkinWorkspace, '/src; source /opt/ros/noetic/setup.bash; cd ', vmdev.CatkinWorkspace, '/src; catkin_init_workspace']);

Generate and Deploy ROS Node

Create a MATLAB Coder™ configuration object.

cfg = coder.config("exe");
cfg.Hardware = coder.hardware("Robot Operating System (ROS)");
cfg.Hardware.DeployTo = "Remote Device"; 
cfg.Hardware.BuildAction = "Build and run";
cfg.HardwareImplementation.ProdHWDeviceType = "Intel->x86-64 (Linux 64)";
cfg.HardwareImplementation.ProdLongLongMode = true; % Use 'long long' for int64 or uint64 data types

Set the deployment device properties of the Ubuntu VM.

cfg.Hardware.RemoteDeviceAddress = ipaddress;
cfg.Hardware.CatkinWorkspace = vmdev.CatkinWorkspace;
cfg.Hardware.RemoteDevicePassword = 'password';
cfg.Hardware.RemoteDeviceUsername = 'user';
cfg.Hardware

ans = 
  Hardware with properties:

                      Name: 'Robot Operating System (ROS)'
              CPUClockRate: 1000
     PackageMaintainerName: 'ROS User'
    PackageMaintainerEmail: 'rosuser@test.com'
       RemoteDeviceAddress: '192.168.93.136'
            PackageLicense: 'BSD'
      RemoteDeviceUsername: 'user'
           CatkinWorkspace: '/tmp/px4stil_catkinws'
      RemoteDevicePassword: 'password'
            PackageVersion: '1.0.0'
               BuildAction: 'Build and run'
                 ROSFolder: '/opt/ros/melodic'
                  DeployTo: 'Remote Device'

Deploy the MATLAB function as a standalone ROS node.

codegen("px4sitlCircularLoop","-config",cfg);

The generated node starts running on the ROS device. You can verify this by using the rosdevice object.

isNodeRunning(vmdev,"px4sitlCircularLoop")

Simulated PX4 takes off and starts flying in a circular pattern in Gazebo.

Stop the ROS node by sending the landing message from MATLAB.

pubLandMsg = rospublisher("/land_message","DataFormat","struct")

pubLandMsg = 
  Publisher with properties:

         TopicName: '/land_message'
    NumSubscribers: 1
        IsLatching: 1
       MessageType: 'std_msgs/Bool'
        DataFormat: 'struct'

msg = rosmessage(pubLandMsg);
msg.Data = true;
send(pubLandMsg,msg);

Deploy a UAV Waypoint Follower ROS Node

Open the px4sitlWaypointFollower function and examine the code.

Similar to the px4sitlCircularLoop function, this function also has three sections: the initialization sequence, the trajectory follower controller loop that moves the PX4 along the desired waypoints, and the landing sequence.

Create Reference Trajectory and Waypoints

The helper function exampleHelperCreateReferencePath generates a trajectory along the provided waypoints in latitude, longitude, and altitude format in the ENU frame.

It uses minsnappolytraj (Robotics System Toolbox) to return an array with the position, velocity, and acceleration at various points along the trajectory.

latlonhome = [42.301389286674166 -71.37609390148994 0];

latlonpoints = [latlonhome; 
    42.301389286674166 -71.37609390148994 10;
    42.302289545648726 -71.376093901489938 10; 
    42.302289539236234 -71.374881155270842 10;
    42.301389280261866 -71.374881172546949 10;
    42.301389286674166 -71.37609390148994 10];
timeOfFlight = 100; % seconds
sampleSize = 100; % samples per segment
[q,qd,qdd] = exampleHelperCreateReferencePath(latlonhome,latlonpoints,timeOfFlight,sampleSize);

Note the precision of the latitude and longitude values. If you modify the function to use a different set of waypoints, you must provide waypoints with similar precision.

Trajectory Control Loop

The control loop starts with the takeoff sequence using exampleHelperUAVTakeOff helper function.

The control loop calls the exampleHelperTrajectoryController function, which accepts the current position and velocity values, the reference position, reference velocity, and reference accelerations generated by the exampleHelperCreateReferencePath function, and the PD (proportional-derivative) gain values and returns the desired acceleration values along the X, Y and Z axes.

accCMD = exampleHelperTrajectoryController([0 0 0]',[0 0 0]',q,qd,qdd,-4,-2)

accCMD = 3×1

    0.0195
   -0.1541
    0.1840

The control loop converts these acceleration values into the acceleration message of type geometry_msgs/Vector3Stamped, and sends the message over the /mavros/setpoint_accel/accel topic.

The control loop tracks the index of the trajectory segments and the proximity to the final waypoint. The control loop exits once the PX4 reaches within 1 meter radius of the final waypoint.

Deploy Waypoint Follower ROS Node

Deploy the MATLAB function as a standalone ROS node using the codegen command:

codegen("px4sitlWaypointFollower","-config",cfg);

Observe the PX4 SITL in Gazebo. Once the node starts running, the PX4 UAV takes off and starts following the waypoints.

The deployed ROS node stops once it reaches the final waypoint.