Сгенерируйте узел ROS для последователя UAV Waypoint
В этом примере показано, как использовать генерацию кода MATLAB®, чтобы создать узел ROS, чтобы переместить беспилотное воздушное транспортное средство (UAV) вдоль предопределенного пути. Этот пример демонстрирует развертывание waypoint последователя для фиксированного кругового пути и набора заданного пользовательского waypoints.
В этом примере вы развертываете функцию MATLAB как автономный узел ROS, чтобы управлять симулированным PX4® UAV с Gazebo. Для получения дополнительной информации о генерации узла ROS из кода MATLAB смотрите, что MATLAB Программирует для Генерации кода.
Запустите PX4 SITL средство моделирования Gazebo
Загрузите виртуальную машину с помощью инструкций в Начало работы с Gazebo и Симулированным TurtleBot.
Запустите рабочий стол виртуальной машины Ubuntu®.
В рабочем столе Ubuntu нажмите значок Gazebo PX4 SITL, чтобы запустить симулированный мир PX4 Gazebo.
Задайте IP-адрес и номер порта ведущего устройства ROS в Gazebo так, чтобы MATLAB® мог связаться со средством моделирования PX4. В данном примере ведущим устройством ROS в Gazebo является
http://192.168.192.136:11311192.168.31.1.Запустите сеть ROS с помощью
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/
Просмотрите темы ROS, доступные в сети. Темы с пространством имен /mavros доступны.
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
Сгенерируйте пользовательские сообщения
Этот пример использует MAVROS, чтобы управлять беспилотником PX4, использующим mavros_msgs пакет.
Загрузите mavros_msgs пакет. Затем выполните шаги в ROS Пользовательская документация Поддержки сообщения, чтобы создать mavros_msgs пакет.
Проверьте, что сообщения отображаются в 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' }
⋮
Управляйте PX4 в круговом пути
Откройте функцию MATLAB px4sitlCircularLoop и исследуйте код.
Функция имеет три раздела: последовательность инициализации, цикл контроллера, который перемещает PX4 вдоль кругового пути и приземляющуюся последовательность.
Инициализация и круговой путь Setup Waypoint
Функция объявляет, что все необходимые издатели ROS, подписчики и сервисные клиенты связываются по 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 a 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);
Создайте круговой путь вдоль источника.
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
numSteps управление переменными гранулярность waypoints.
Уровнем, с которым публикуется тема, управляет rosrate функция. В данном примере установите уровень на 20 Гц.
% Publish the messages at 20 Hz
r = rosrate(20);Круговой цикл управления последователя пути
В px4sitlCircularLoop функция, цикл управления инициализирует последовательность взлета путем изменения режима и вооружения PX4. Если PX4 готов принять команды, эта функция сразу возвращается.
exampleHelperUAVTakeOff(stateSub,setModeCli,armingCli,5.0);
PX4 OFFBOARD mode set
Цикл управления устанавливает поля X and Y сообщения положения типа geometry_msgs/PoseStamped и отправляет сообщение по /mavros/setpoint_position/local тема. Высота обеспечена на уровне 2 метров.
В MATLAB выходы цикла управления после того, как PX4 следует за круговым путем три раза.
На C++ узел ROS, развернутый от px4sitlCircularLoop функция, цикл управления подписывается на /land_message тема типа std_msgs/Bool и выходы, когда значение сообщения установлено к true.
После выхода из цикла управления функция запускает приземляющуюся последовательность.
Приземление последовательности
Во время приземления функция MATLAB вычисляет waypoints от текущего положения на круговом пути назад до начала координат. PX4 следует за waypoints, чтобы достигнуть положения источника при поддержании высоты. После того, как это достигает источника, exampleHelperUAVLand функционируйте сажает PX4.
exampleHelperUAVLand(posePub,poseMsg,r);
Landed successfully
Подготовьте рабочую область на Ubuntu VM
Прежде, чем развернуть узел на VM, создайте папку рабочей области Сережки путем выполнения следующих команд в MATLAB.
vmdev = rosdevice(ipaddress,'user','password'); vmdev.CatkinWorkspace = '/tmp/px4stil_catkinws'; system(vmdev,['mkdir -p ', vmdev.CatkinWorkspace, '/src; source /opt/ros/melodic/setup.bash; cd ', vmdev.CatkinWorkspace, '/src; catkin_init_workspace']);
Сгенерируйте и разверните узел ROS
Создайте объект настройки MATLAB Coder™.
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
Установите свойства устройства развертывания VM Ubuntu.
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'
Разверните функцию MATLAB как автономный узел ROS.
codegen("px4sitlCircularLoop","-config",cfg);
Сгенерированный узел начинает работать на устройстве ROS. Можно проверить это при помощи rosdevice объект.
isNodeRunning(vmdev,"px4sitlCircularLoop")
Симулированный PX4 взлетает и начинает лететь в круговом шаблоне в Gazebo.
Остановите узел ROS путем отправки приземляющегося сообщения от 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);
Разверните последователя UAV Waypoint узел ROS
Откройте px4sitlWaypointFollower функционируйте и исследуйте код.
Подобно px4sitlCircularLoop функция, эта функция также имеет три раздела: последовательность инициализации, контроллер последователя траектории цикл, который перемещает PX4 вдоль желаемого waypoints и приземляющуюся последовательность.
Создайте Reference Trajectory и Waypoints
Функция помощника exampleHelperCreateReferencePath генерирует траекторию вдоль обеспеченного waypoints в широте, долготе и высотном формате в системе координат ENU.
Это использует minsnappolytraj (Robotics System Toolbox), чтобы возвратить массив с положением, скоростью и ускорением в различных точках вдоль траектории.
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);
Отметьте точность значений широты и долготы. Если вы изменяете функцию, чтобы использовать различный набор waypoints, необходимо предоставить waypoints подобную точность.
Цикл управления траектории
Цикл управления запускается с последовательности взлета с помощью exampleHelperUAVTakeOff функция помощника.
Цикл управления вызывает exampleHelperTrajectoryController функция, которая принимает текущее положение и скоростные значения, ссылочное положение, ссылочную скорость и ссылочные ускорения, сгенерированные exampleHelperCreateReferencePath функция и PD (пропорционально-производные) значения усиления и возвращают желаемые ускоряющие значения вдоль этих X, Y и осей Z.
accCMD = exampleHelperTrajectoryController([0 0 0]',[0 0 0]',q,qd,qdd,-4,-2)
accCMD = 3×1
0.0195
-0.1541
0.1840
Цикл управления преобразует эти ускоряющие значения в ускоряющее сообщение типа geometry_msgs/Vector3Stamped, и отправляет сообщение по /mavros/setpoint_accel/accel тема.
Цикл управления отслеживает индекс сегментов траектории и близости к финалу waypoint. Выходы цикла управления однажды PX4 достигают в 1-метровом радиусе финала waypoint.
Разверните последователя Waypoint узел ROS
Разверните функцию MATLAB как автономный узел ROS с помощью codegen команда:
codegen("px4sitlWaypointFollower","-config",cfg);
Наблюдайте PX4 SITL в Gazebo. Если узел начинает запускаться, PX4 UAV взлетает и запускается после waypoints.
Развернутый узел ROS останавливается, если он достигает финала waypoint.