为无人机航路点跟踪器生成ROS节点

这个例子使用了:

打开实时脚本

本示例展示如何使用MATLAB®代码生成创建ROS节点，以沿着预定义的路径移动无人机(UAV)。此示例演示了固定循环路径和一组指定自定义路径点的路径点跟踪器的部署。

在本例中，将MATLAB函数部署为独立ROS节点，以控制控件模拟PX4®无人机与凉亭．有关从MATLAB代码生成ROS节点的更多信息，请参见代码生成的MATLAB编程．

启动PX4 SITL凉亭模拟器

启动Ubuntu®虚拟机桌面。
在Ubuntu桌面上，单击凉亭PX4 SITL图标开始模拟PX4凉亭世界。

在Gazebo中指定ROS主机的IP地址和端口号，以便MATLAB®可以与PX4模拟器通信。在本例中，Gazebo中的ROS主机为http://192.168.192.136:11311你的主机地址是192.168.31.1．
使用启动ROS网络rosinit．

ipaddress =“192.168.93.136”；如果一个全局节点正在运行，则关闭全局节点rosshutdown ();

使用NodeURI http://192.168.93.1:62022/关闭全局节点/matlab_global_node_29172

rosinit (ipaddress, 11311);

使用NodeURI http://192.168.93.1:62084/初始化全局节点/matlab_global_node_22217

查看网络上可用的ROS主题。具有名称空间的主题/ mavros是可用的。

rostopic (“列表”）;

/clock /diagnostics /凉亭/link_states /凉亭/model_states /凉亭/ parameter_描述/凉亭/parameter_updates /凉亭/set_link_state /凉亭/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/ rail_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/ 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/satellite /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/ gps2/raw /mavros/gpsstatus/gps2/rtk /mavros/hil/actuator_controls /mavros/hil/gps /mavros/hil/imu_ned /mavros/hil/optical_flow /mavros/hil/rc_inputs /mavros/hil/ 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/着陆目标/lt_marker /mavros/着陆目标/pose /mavros/着陆目标/pose_in /mavros/local_position/accel /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 /生/ log_data / mavros / log_transfer /生/ log_entry / mavros / manual_control /控制/ mavros / manual_control /发送/ mavros /任务/达到/ mavros /任务/路径点/ mavros /灭克磷/姿势/ mavros / mount_control /命令/ mavros / mount_control /定位/ mavros /障碍/发送/ mavros /测程法/ / mavros /测程法/ / mavros / onboard_computer /地位/ mavros /参数/ param_value / mavros / play_tune / mavros / px4flow / ground_distance / mavros / px4flow /生/ optical_flow_rad / mavros / px4flow /生/发送/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_raw/attitude /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/path /mavros/vfr_hud /mavros/vision_pose/pose /mavros/vision_pose/pose_cov/mavros/vision_speed/speed_twist_cov /mavros/ wind_estimate /rosout /rosout_agg /tf /tf_static

生成自定义消息

这个例子使用了MAVROS来控制PX4无人机mavros_msgs包中。

下载mavros_msgs包中。然后，按照步骤ROS自定义消息支持金宝app用于构建mavros_msgs包中。

验证这些消息在MATLAB中是可见的:

MSGS = rosmsg(“列表”）;mavrosMsgs = msgs(startsWith(msgs，“mavros_msgs”）)

mavrosMsgs =127×1细胞{' mavros_msgs / ADSBVehicle '} {' mavros_msgs / ActuatorControl '} {mavros_msgs /高度的}{' 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'} `

以圆形路径飞行PX4

打开MATLAB函数px4sitlCircularLoop并检查代码。

该函数有三个部分:初始化序列、沿圆形路径移动PX4的控制器循环和着陆序列。

初始化和循环路径路点设置

该函数声明了所有必需的ROS发布者、订阅者和服务客户端，以便通过MAVROS进行通信。

rossubscriber(“mavros /状态”，“mavros_msgs /状态”，“DataFormat”，“结构”）;创建SetMode服务客户端setModeCli = rossvcclient(“/ mavros / set_mode”，“mavros_msgs / SetMode”，“DataFormat”，“结构”）;创建arm命令服务客户端armingCli = rossvcclient(“/ mavros / cmd /武装”，“mavros_msgs / CommandBool”，“DataFormat”，“结构”）;订阅着陆消息landNowSub = rossubscriber(“/ land_message”，“std_msgs / Bool”，“DataFormat”，“结构”）;%订阅PX4当前位置currPosSub = rossubscriber(“/ mavros / local_position /姿势”，“geometry_msgs / PoseStamped”，“DataFormat”，“结构”）;为PX4 position命令创建发布者posePub = rospublisher(“/ mavros / setpoint_position /地方”，“geometry_msgs / PoseStamped”，“DataFormat”，“结构”）;poseMsg = rosmessage(pospub);

沿着原点创建一个圆形路径。

半径= 5.0;%半径(米)设置划分循环路径的步数numSteps = 100.0;numPoints =半径* numSteps;沿着指定半径的圆创建[x,y]点xpoints =半径* sin(linspace(-pi,pi,numPoints));ypoints =半径* cos(linspace(-pi,pi,numPoints));海拔= 2.0;%米如果isempty (coder.target)图(“名称”，“圆形轨迹”）;plot3 (xpoints ypoints,高度*的(numPoints));网格在；结束

的numSteps变量控制路径点的粒度。

对象控制主题发布的速率rosrate函数。本例设置为20hz。

以20hz的频率发布消息R = rosrate(20);

环形路径跟随控制回路

在px4sitlCircularLoop函数，控制回路通过改变模式和武装PX4来初始化起飞序列。如果PX4准备好接受命令，该函数立即返回。

exampleHelperUAVTakeOff (stateSub setModeCli armingCli, 5.0);

PX4 OFFBOARD模式设置

控制循环设置类型的姿态消息的X和Y字段geometry_msgs / PoseStamped并将消息发送到/ mavros setpoint_position /当地的话题。海拔保持在2米。

在MATLAB中，控制回路在PX4沿圆形路径走三次后退出。

在c++中，ROS节点部署于px4sitlCircularLoop函数时，控制循环订阅/ land_message类型主题std_msgs / Bool并在消息值设置为时退出真正的．

退出控制循环后，函数运行着陆序列。

着陆顺序

在着陆过程中，MATLAB函数计算从圆形路径上的当前位置返回原点的路径点。PX4沿着路径点到达原点位置，同时保持高度。当它到达原点后exampleHelperUAVLand函数登陆PX4。

exampleHelperUAVLand (posePub poseMsg, r);

登陆成功

在Ubuntu VM上准备工作区

在将节点部署到虚拟机上之前，请在MATLAB中运行以下命令创建一个Catkin工作区文件夹。

Vmdev = rosdevice(ipaddress，“用户”，“密码”）;vmdev。CatkinWorkspace =“/ tmp / px4stil_catkinws”；系统(vmdev, ('mkdir -p ', vmdev。CatkinWorkspace,' / src;源/ opt / ros /旋律/ setup.bash;光盘的, vmdev。CatkinWorkspace,' / src;catkin_init_workspace”]);

生成和部署ROS节点

创建一个MATLAB Coder™配置对象。

CFG = code .config(“exe”）;cfg。硬件= code . Hardware (机器人操作系统(ROS)）;cfg.Hardware.DeployTo =“远程设备”；cfg.Hardware.BuildAction =“构建并运行”；cfg.HardwareImplementation.ProdHWDeviceType ="Intel->x86-64 (Linux 64)"；cfg.HardwareImplementation.ProdLongLongMode = true;对于int64或uint64类型的数据，使用'long long'

设置Ubuntu虚拟机的部署设备属性。

cfg.Hardware.RemoteDeviceAddress = ipaddress;cfg.Hardware.CatkinWorkspace = vmdev.CatkinWorkspace;cfg.Hardware.RemoteDevicePassword =“密码”；cfg.Hardware.RemoteDeviceUsername =“用户”；cfg。硬件

ans =硬件与属性:名称:“机器人操作系统(ROS)”CPUClockRate: 1000 PackageMaintainerName:“ROS用户”PackageMaintainerEmail:“rosuser@test.com”RemoteDeviceAddress:“192.168.93.136”PackageLicense:“BSD”RemoteDeviceUsername:“User”CatkinWorkspace:“/tmp/px4stil_catkinws”RemoteDevicePassword:“password”PackageVersion:“1.0.0”BuildAction:“构建和运行”ROSFolder:“/opt/ ROS /melodic”部署到:“远程设备”

将MATLAB函数部署为独立的ROS节点。

codegen (“px4sitlCircularLoop”，“配置”cfg);

生成的节点开始在ROS设备上运行。方法可以验证这一点rosdevice对象。

isNodeRunning (vmdev“px4sitlCircularLoop”）

模拟PX4起飞并开始在凉亭中以圆形模式飞行。

通过从MATLAB发送着陆消息来停止ROS节点。

pubLandMsg =俄罗斯出版商(“/ land_message”，“DataFormat”，“结构”）

pubLandMsg = Publisher with properties: TopicName: '/land_message' numsubscriber: 1 IsLatching: 1 MessageType: 'std_msgs/Bool' DataFormat: 'struct'

msg = rosmessage(pubLandMsg);味精。数据= true;发送(pubLandMsg,味精);

部署一个无人机航路点跟踪器ROS节点

打开px4sitlWaypointFollower函数和检查代码。

类似于px4sitlCircularLoop函数，该函数也有三个部分:初始化序列，轨迹跟随控制器循环，移动PX4沿着所需的路径点，和着陆序列。

创建参考轨迹和路径点

辅助函数exampleHelperCreateReferencePath沿着ENU框架中提供的经度、纬度和高度格式的路径点生成一个轨迹。

它使用minsnappolytraj(机器人系统工具箱)返回一个数组，其中包含沿轨迹各点的位置、速度和加速度。

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;%秒sampleSize = 100;每段样品百分比[q,qd,qdd] = exampleHelperCreateReferencePath(latlonhome,latlonpoints,timeOfFlight,sampleSize);

注意纬度和经度值的精度。如果修改函数以使用不同的路径点集，则必须提供具有类似精度的路径点。

轨迹控制回路

控制循环从使用的起飞序列开始exampleHelperUAVTakeOffhelper函数。

控件循环调用exampleHelperTrajectoryController方法生成的当前位置和速度值、参考位置、参考速度和参考加速度exampleHelperCreateReferencePath函数，PD(比例导数)增益值，并返回沿X, Y和Z轴所需的加速度值。

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

accCMD =3×10.0195 -0.1541 0.1840

控制循环将这些加速度值转换为类型的加速度消息geometry_msgs / Vector3Stamped，并将消息发送到/ mavros setpoint_accel / accel的话题。

控制回路跟踪轨迹段的索引和到最终航路点的接近度。一旦PX4到达最终航路点半径1米范围内，控制回路就会退出。

部署路径点跟随ROS节点

将MATLAB函数部署为独立的ROS节点codegen命令:

codegen (“px4sitlWaypointFollower”，“配置”cfg);

在凉亭观察PX4 SITL。一旦节点开始运行，PX4无人机起飞并开始跟随路径点。

部署的ROS节点在到达最终路径点后停止。