From f6633ea68df50eb7a3c17fe9889953d985874392 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 5 May 2023 16:42:33 +0000 Subject: [PATCH] Imported upstream version '2.5.0' of 'upstream' --- .editorconfig | 3 + .github/workflows/main.yml | 10 +- .ycm_extra_conf.py | 263 -- CONTRIBUTING.md | 92 +- LICENSE-BSD.txt | 45 +- README.md | 5 +- cog-requirements.txt | 4 + dependencies.rosinstall | 2 +- libmavconn/CHANGELOG.rst | 281 +- libmavconn/CMakeLists.txt | 144 +- libmavconn/README.md | 4 +- .../Modules/CheckGeographicLibDatasets.cmake | 47 +- libmavconn/cmake/Modules/EnableCXX11.cmake | 20 - libmavconn/cmake/Modules/FindASIO.cmake | 25 + libmavconn/cmake/Modules/MavrosMavlink.cmake | 9 - libmavconn/cmake/Modules/em_expand.cmake | 71 + .../libmavconn-extras.cmake.develspace.in | 2 - .../libmavconn-extras.cmake.installspace.in | 2 - .../include/mavconn/console_bridge_compat.h | 57 - .../include/mavconn/console_bridge_compat.hpp | 71 + libmavconn/include/mavconn/interface.h | 321 --- libmavconn/include/mavconn/interface.hpp | 347 +++ .../include/mavconn/mavlink_dialect.h.em | 30 - .../include/mavconn/mavlink_dialect.hpp.em | 52 + libmavconn/include/mavconn/msgbuffer.h | 94 - libmavconn/include/mavconn/msgbuffer.hpp | 111 + libmavconn/include/mavconn/serial.h | 71 - libmavconn/include/mavconn/serial.hpp | 84 + libmavconn/include/mavconn/tcp.h | 144 - libmavconn/include/mavconn/tcp.hpp | 170 ++ libmavconn/include/mavconn/thread_utils.h | 83 - libmavconn/include/mavconn/thread_utils.hpp | 93 + libmavconn/include/mavconn/udp.h | 89 - libmavconn/include/mavconn/udp.hpp | 111 + libmavconn/libmavconn-extras.cmake | 10 + libmavconn/package.xml | 20 +- libmavconn/src/interface.cpp | 656 +++-- libmavconn/src/mavlink_helpers.cpp.em | 79 +- libmavconn/src/serial.cpp | 416 +-- libmavconn/src/tcp.cpp | 759 ++--- libmavconn/src/udp.cpp | 534 ++-- libmavconn/test/test_hang.cpp | 87 - libmavconn/test/test_mavconn.cpp | 416 +-- mavros/CHANGELOG.rst | 670 ++++- mavros/CMakeLists.txt | 339 ++- mavros/README.md | 81 +- .../mavros/{frame_tf.h => frame_tf.hpp} | 350 ++- mavros/include/mavros/mavlink_diag.h | 44 - mavros/include/mavros/mavros.h | 82 - mavros/include/mavros/mavros_plugin.h | 150 - .../mavros/mavros_plugin_register_macro.hpp | 35 + mavros/include/mavros/mavros_router.hpp | 305 ++ mavros/include/mavros/mavros_uas.h | 476 --- mavros/include/mavros/mavros_uas.hpp | 641 +++++ mavros/include/mavros/mission_protocol_base.h | 603 ---- .../include/mavros/mission_protocol_base.hpp | 654 +++++ mavros/include/mavros/plugin.hpp | 309 ++ mavros/include/mavros/plugin_filter.hpp | 83 + ...{px4_custom_mode.h => px4_custom_mode.hpp} | 101 +- mavros/include/mavros/setpoint_mixin.h | 249 -- mavros/include/mavros/setpoint_mixin.hpp | 293 ++ mavros/include/mavros/uas_executor.hpp | 60 + mavros/include/mavros/{utils.h => utils.hpp} | 78 +- mavros/launch/apm.launch | 23 +- mavros/launch/apm2.launch | 1 - mavros/launch/apm_config.yaml | 378 +-- mavros/launch/apm_pluginlists.yaml | 33 +- mavros/launch/event_launcher.yaml | 69 - mavros/launch/mavlink_bridge.launch | 11 - mavros/launch/multi_uas.launch | 21 + mavros/launch/node.launch | 18 +- mavros/launch/px4.launch | 26 +- mavros/launch/px4_config.yaml | 405 +-- mavros/launch/px4_pluginlists.yaml | 23 +- mavros/mavros/__init__.py | 84 + mavros/mavros/base.py | 210 ++ mavros/mavros/cmd/__init__.py | 132 + .../event_launcher => mavros/cmd/__main__.py} | 10 +- mavros/mavros/cmd/checkid.py | 130 + mavros/mavros/cmd/cmd.py | 350 +++ mavros/mavros/cmd/ftp.py | 299 ++ mavros/mavros/cmd/mission.py | 454 +++ mavros/mavros/cmd/param.py | 144 + mavros/mavros/cmd/safety.py | 62 + mavros/mavros/cmd/system.py | 156 + mavros/mavros/cmd/utils.py | 80 + mavros/mavros/command.py | 58 + mavros/mavros/ftp.py | 233 ++ mavros/mavros/global_position.py | 69 + mavros/mavros/local_position.py | 82 + mavros/{src => }/mavros/mavlink.py | 86 +- mavros/mavros/mission.py | 245 ++ mavros/mavros/nuttx_crc32.py | 277 ++ mavros/mavros/param.py | 347 +++ mavros/mavros/setpoint.py | 136 + mavros/mavros/system.py | 94 + mavros/mavros/utils.py | 113 + mavros/mavros_plugins.xml | 225 +- mavros/package.xml | 38 +- mavros/pytest.ini | 2 + mavros/scripts/checkid | 109 - .../scripts/install_geographiclib_datasets.sh | 2 +- mavros/scripts/mav | 14 + mavros/scripts/mavcmd | 287 -- mavros/scripts/mavftp | 296 -- mavros/scripts/mavparam | 110 - mavros/scripts/mavsafety | 108 - mavros/scripts/mavsetp | 123 - mavros/scripts/mavsys | 143 - mavros/scripts/mavwp | 223 -- mavros/setup.cfg | 30 + mavros/setup.py | 45 +- mavros/src/gcs_bridge.cpp | 89 - mavros/src/lib/enum_sensor_orientation.cpp | 223 +- mavros/src/lib/enum_to_string.cpp | 1040 +++---- mavros/src/lib/ftf_frame_conversions.cpp | 430 +-- mavros/src/lib/ftf_quaternion_utils.cpp | 65 +- mavros/src/lib/mavlink_diag.cpp | 56 - mavros/src/lib/mavros.cpp | 403 --- mavros/src/lib/mavros_router.cpp | 552 ++++ mavros/src/lib/mavros_uas.cpp | 442 +++ mavros/src/lib/plugin.cpp | 66 + mavros/src/lib/rosconsole_bridge.cpp | 16 - mavros/src/lib/uas_ap.cpp | 102 + mavros/src/lib/uas_data.cpp | 305 +- mavros/src/lib/uas_executor.cpp | 45 + mavros/src/lib/uas_stringify.cpp | 395 +-- mavros/src/lib/uas_tf.cpp | 49 + mavros/src/lib/uas_timesync.cpp | 60 +- mavros/src/mavros/__init__.py | 53 - mavros/src/mavros/command.py | 52 - mavros/src/mavros/event_launcher.py | 311 -- mavros/src/mavros/ftp.py | 234 -- mavros/src/mavros/mission.py | 147 - mavros/src/mavros/nuttx_crc32.py | 57 - mavros/src/mavros/param.py | 216 -- mavros/src/mavros/setpoint.py | 66 - mavros/src/mavros/utils.py | 54 - mavros/src/mavros_node.cpp | 85 +- mavros/src/plugins/3dr_radio.cpp | 160 -- mavros/src/plugins/actuator_control.cpp | 146 +- mavros/src/plugins/altitude.cpp | 112 +- mavros/src/plugins/command.cpp | 905 +++--- mavros/src/plugins/dummy.cpp | 167 +- mavros/src/plugins/ftp.cpp | 2177 +++++++------- mavros/src/plugins/geofence.cpp | 428 ++- mavros/src/plugins/global_position.cpp | 1047 +++---- mavros/src/plugins/hil.cpp | 339 --- mavros/src/plugins/home_position.cpp | 351 ++- mavros/src/plugins/imu.cpp | 1155 ++++---- mavros/src/plugins/local_position.cpp | 501 ++-- mavros/src/plugins/manual_control.cpp | 148 +- mavros/src/plugins/mission_protocol_base.cpp | 839 ++++-- mavros/src/plugins/nav_controller_output.cpp | 96 +- mavros/src/plugins/param.cpp | 1992 +++++++------ mavros/src/plugins/rallypoint.cpp | 427 ++- mavros/src/plugins/rc_io.cpp | 524 ++-- mavros/src/plugins/safety_area.cpp | 178 -- mavros/src/plugins/setpoint_accel.cpp | 173 +- mavros/src/plugins/setpoint_attitude.cpp | 392 +-- mavros/src/plugins/setpoint_position.cpp | 542 ++-- mavros/src/plugins/setpoint_raw.cpp | 558 ++-- mavros/src/plugins/setpoint_trajectory.cpp | 466 +-- mavros/src/plugins/setpoint_velocity.cpp | 258 +- mavros/src/plugins/sys_status.cpp | 2560 +++++++++-------- mavros/src/plugins/sys_time.cpp | 980 ++++--- mavros/src/plugins/vfr_hud.cpp | 73 - mavros/src/plugins/waypoint.cpp | 622 ++-- mavros/src/plugins/wind_estimation.cpp | 179 +- mavros/test/mavros_py/__init__.py | 0 mavros/test/mavros_py/test_mission.py | 41 + mavros/test/mavros_py/test_param.py | 175 ++ .../test/mavros_py/testdata/CMAC-circuit.txt | 9 + mavros/test/mavros_py/testdata/mavproxy.parm | 1296 +++++++++ .../mavros_py/testdata/missionplanner.parm | 354 +++ .../mavros_py/testdata/qgroundcontrol.params | 1304 +++++++++ mavros/test/mavros_py/testdata/simple.plan | 271 ++ mavros/test/test_frame_conversions.cpp | 255 +- mavros/test/test_quaternion_utils.cpp | 190 +- mavros/test/test_router.cpp | 474 +++ mavros/test/test_sensor_orientation.cpp | 86 +- mavros/test/test_uas.cpp | 263 ++ mavros/tools/cogall.sh | 2 +- mavros/tools/uncrustify-cpp.cfg | 125 - mavros_cog.py | 168 +- mavros_extras/CHANGELOG.rst | 414 ++- mavros_extras/CMakeLists.txt | 267 +- .../mavros_extras/servo_state_publisher.hpp | 145 + mavros_extras/launch/f710_joy.yaml | 29 - mavros_extras/launch/px4_image.launch | 21 - mavros_extras/launch/teleop.launch | 16 - mavros_extras/mavros_plugins.xml | 323 ++- mavros_extras/package.xml | 72 +- mavros_extras/scripts/mavftpfuse | 200 -- mavros_extras/scripts/mavteleop | 311 -- .../src/lib/servo_state_publisher.cpp | 148 + mavros_extras/src/plugins/3dr_radio.cpp | 192 ++ mavros_extras/src/plugins/adsb.cpp | 276 +- mavros_extras/src/plugins/cam_imu_sync.cpp | 87 +- mavros_extras/src/plugins/camera.cpp | 119 +- mavros_extras/src/plugins/cellular_status.cpp | 115 +- .../src/plugins/companion_process_status.cpp | 108 +- mavros_extras/src/plugins/debug_value.cpp | 675 ++--- mavros_extras/src/plugins/distance_sensor.cpp | 848 +++--- mavros_extras/src/plugins/esc_status.cpp | 264 +- mavros_extras/src/plugins/esc_telemetry.cpp | 195 +- mavros_extras/src/plugins/fake_gps.cpp | 751 ++--- mavros_extras/src/plugins/gps_input.cpp | 181 +- mavros_extras/src/plugins/gps_rtk.cpp | 250 +- mavros_extras/src/plugins/gps_status.cpp | 341 ++- mavros_extras/src/plugins/guided_target.cpp | 339 ++- mavros_extras/src/plugins/hil.cpp | 373 +++ mavros_extras/src/plugins/landing_target.cpp | 824 +++--- mavros_extras/src/plugins/log_transfer.cpp | 309 +- .../src/plugins/mag_calibration_status.cpp | 186 +- .../src/plugins/mocap_pose_estimate.cpp | 217 +- mavros_extras/src/plugins/mount_control.cpp | 676 +++-- .../src/plugins/obstacle_distance.cpp | 215 +- mavros_extras/src/plugins/odom.cpp | 559 ++-- .../src/plugins/onboard_computer_status.cpp | 188 +- mavros_extras/src/plugins/play_tune.cpp | 86 +- mavros_extras/src/plugins/px4flow.cpp | 364 +-- mavros_extras/src/plugins/rangefinder.cpp | 99 +- mavros_extras/src/plugins/terrain.cpp | 103 +- mavros_extras/src/plugins/trajectory.cpp | 766 ++--- mavros_extras/src/plugins/tunnel.cpp | 174 +- mavros_extras/src/plugins/vfr_hud.cpp | 75 + mavros_extras/src/plugins/vibration.cpp | 115 +- .../src/plugins/vision_pose_estimate.cpp | 311 +- .../src/plugins/vision_speed_estimate.cpp | 278 +- mavros_extras/src/plugins/wheel_odometry.cpp | 1219 ++++---- mavros_extras/src/servo_state_publisher.cpp | 213 -- mavros_extras/src/visualization.cpp | 318 -- mavros_msgs/CHANGELOG.rst | 328 ++- mavros_msgs/CMakeLists.txt | 305 +- .../include/mavros_msgs/mavlink_convert.h | 129 - .../include/mavros_msgs/mavlink_convert.hpp | 145 + mavros_msgs/mavros_msgs_mapping_rule.yaml | 8 + mavros_msgs/msg/ADSBVehicle.msg | 4 +- mavros_msgs/msg/BatteryStatus.msg | 9 - mavros_msgs/msg/CamIMUStamp.msg | 2 +- mavros_msgs/msg/CommandCode.msg | 54 +- mavros_msgs/msg/FileEntry.msg | 2 +- mavros_msgs/msg/LogEntry.msg | 2 +- mavros_msgs/msg/Mavlink.msg | 6 +- mavros_msgs/msg/Param.msg | 2 + mavros_msgs/msg/ParamEvent.msg | 14 + mavros_msgs/msg/ParamValue.msg | 2 + mavros_msgs/msg/RTCM.msg | 2 +- mavros_msgs/msg/{VFR_HUD.msg => VfrHud.msg} | 0 mavros_msgs/msg/WaypointReached.msg | 2 +- mavros_msgs/package.xml | 30 +- mavros_msgs/srv/EndpointAdd.srv | 14 + mavros_msgs/srv/EndpointDel.srv | 17 + mavros_msgs/srv/ParamGet.srv | 2 + mavros_msgs/srv/ParamPush.srv | 2 + mavros_msgs/srv/ParamSet.srv | 2 + mavros_msgs/srv/ParamSetV2.srv | 14 + mavros_msgs/srv/SetMavFrame.srv | 26 +- test_mavros/AMENT_IGNORE | 0 test_mavros/CHANGELOG.rst | 3 - test_mavros/COLCON_IGNORE | 0 test_mavros/package.xml | 2 +- test_mavros/scripts/setpoint_position_demo | 29 +- .../scripts/test_mavlink_convert_to_rosmsg.py | 99 + 265 files changed, 36688 insertions(+), 26195 deletions(-) create mode 100644 .editorconfig delete mode 100644 .ycm_extra_conf.py create mode 100644 cog-requirements.txt delete mode 100644 libmavconn/cmake/Modules/EnableCXX11.cmake create mode 100644 libmavconn/cmake/Modules/FindASIO.cmake delete mode 100644 libmavconn/cmake/Modules/MavrosMavlink.cmake create mode 100644 libmavconn/cmake/Modules/em_expand.cmake delete mode 100644 libmavconn/cmake/libmavconn-extras.cmake.develspace.in delete mode 100644 libmavconn/cmake/libmavconn-extras.cmake.installspace.in delete mode 100644 libmavconn/include/mavconn/console_bridge_compat.h create mode 100644 libmavconn/include/mavconn/console_bridge_compat.hpp delete mode 100644 libmavconn/include/mavconn/interface.h create mode 100644 libmavconn/include/mavconn/interface.hpp delete mode 100644 libmavconn/include/mavconn/mavlink_dialect.h.em create mode 100644 libmavconn/include/mavconn/mavlink_dialect.hpp.em delete mode 100644 libmavconn/include/mavconn/msgbuffer.h create mode 100644 libmavconn/include/mavconn/msgbuffer.hpp delete mode 100644 libmavconn/include/mavconn/serial.h create mode 100644 libmavconn/include/mavconn/serial.hpp delete mode 100644 libmavconn/include/mavconn/tcp.h create mode 100644 libmavconn/include/mavconn/tcp.hpp delete mode 100644 libmavconn/include/mavconn/thread_utils.h create mode 100644 libmavconn/include/mavconn/thread_utils.hpp delete mode 100644 libmavconn/include/mavconn/udp.h create mode 100644 libmavconn/include/mavconn/udp.hpp create mode 100644 libmavconn/libmavconn-extras.cmake delete mode 100644 libmavconn/test/test_hang.cpp rename mavros/include/mavros/{frame_tf.h => frame_tf.hpp} (50%) delete mode 100644 mavros/include/mavros/mavlink_diag.h delete mode 100644 mavros/include/mavros/mavros.h delete mode 100644 mavros/include/mavros/mavros_plugin.h create mode 100644 mavros/include/mavros/mavros_plugin_register_macro.hpp create mode 100644 mavros/include/mavros/mavros_router.hpp delete mode 100644 mavros/include/mavros/mavros_uas.h create mode 100644 mavros/include/mavros/mavros_uas.hpp delete mode 100644 mavros/include/mavros/mission_protocol_base.h create mode 100644 mavros/include/mavros/mission_protocol_base.hpp create mode 100644 mavros/include/mavros/plugin.hpp create mode 100644 mavros/include/mavros/plugin_filter.hpp rename mavros/include/mavros/{px4_custom_mode.h => px4_custom_mode.hpp} (70%) delete mode 100644 mavros/include/mavros/setpoint_mixin.h create mode 100644 mavros/include/mavros/setpoint_mixin.hpp create mode 100644 mavros/include/mavros/uas_executor.hpp rename mavros/include/mavros/{utils.h => utils.hpp} (67%) delete mode 120000 mavros/launch/apm2.launch delete mode 100644 mavros/launch/event_launcher.yaml delete mode 100644 mavros/launch/mavlink_bridge.launch create mode 100644 mavros/launch/multi_uas.launch create mode 100644 mavros/mavros/__init__.py create mode 100644 mavros/mavros/base.py create mode 100644 mavros/mavros/cmd/__init__.py rename mavros/{scripts/event_launcher => mavros/cmd/__main__.py} (64%) mode change 100755 => 100644 create mode 100644 mavros/mavros/cmd/checkid.py create mode 100644 mavros/mavros/cmd/cmd.py create mode 100644 mavros/mavros/cmd/ftp.py create mode 100644 mavros/mavros/cmd/mission.py create mode 100644 mavros/mavros/cmd/param.py create mode 100644 mavros/mavros/cmd/safety.py create mode 100644 mavros/mavros/cmd/system.py create mode 100644 mavros/mavros/cmd/utils.py create mode 100644 mavros/mavros/command.py create mode 100644 mavros/mavros/ftp.py create mode 100644 mavros/mavros/global_position.py create mode 100644 mavros/mavros/local_position.py rename mavros/{src => }/mavros/mavlink.py (57%) create mode 100644 mavros/mavros/mission.py create mode 100644 mavros/mavros/nuttx_crc32.py create mode 100644 mavros/mavros/param.py create mode 100644 mavros/mavros/setpoint.py create mode 100644 mavros/mavros/system.py create mode 100644 mavros/mavros/utils.py create mode 100644 mavros/pytest.ini delete mode 100755 mavros/scripts/checkid create mode 100755 mavros/scripts/mav delete mode 100755 mavros/scripts/mavcmd delete mode 100755 mavros/scripts/mavftp delete mode 100755 mavros/scripts/mavparam delete mode 100755 mavros/scripts/mavsafety delete mode 100755 mavros/scripts/mavsetp delete mode 100755 mavros/scripts/mavsys delete mode 100755 mavros/scripts/mavwp create mode 100644 mavros/setup.cfg delete mode 100644 mavros/src/gcs_bridge.cpp delete mode 100644 mavros/src/lib/mavlink_diag.cpp delete mode 100644 mavros/src/lib/mavros.cpp create mode 100644 mavros/src/lib/mavros_router.cpp create mode 100644 mavros/src/lib/mavros_uas.cpp create mode 100644 mavros/src/lib/plugin.cpp delete mode 100644 mavros/src/lib/rosconsole_bridge.cpp create mode 100644 mavros/src/lib/uas_ap.cpp create mode 100644 mavros/src/lib/uas_executor.cpp create mode 100644 mavros/src/lib/uas_tf.cpp delete mode 100644 mavros/src/mavros/__init__.py delete mode 100644 mavros/src/mavros/command.py delete mode 100644 mavros/src/mavros/event_launcher.py delete mode 100644 mavros/src/mavros/ftp.py delete mode 100644 mavros/src/mavros/mission.py delete mode 100644 mavros/src/mavros/nuttx_crc32.py delete mode 100644 mavros/src/mavros/param.py delete mode 100644 mavros/src/mavros/setpoint.py delete mode 100644 mavros/src/mavros/utils.py delete mode 100644 mavros/src/plugins/3dr_radio.cpp delete mode 100644 mavros/src/plugins/hil.cpp delete mode 100644 mavros/src/plugins/safety_area.cpp delete mode 100644 mavros/src/plugins/vfr_hud.cpp create mode 100644 mavros/test/mavros_py/__init__.py create mode 100644 mavros/test/mavros_py/test_mission.py create mode 100644 mavros/test/mavros_py/test_param.py create mode 100644 mavros/test/mavros_py/testdata/CMAC-circuit.txt create mode 100644 mavros/test/mavros_py/testdata/mavproxy.parm create mode 100644 mavros/test/mavros_py/testdata/missionplanner.parm create mode 100644 mavros/test/mavros_py/testdata/qgroundcontrol.params create mode 100644 mavros/test/mavros_py/testdata/simple.plan create mode 100644 mavros/test/test_router.cpp create mode 100644 mavros/test/test_uas.cpp delete mode 100644 mavros/tools/uncrustify-cpp.cfg create mode 100644 mavros_extras/include/mavros_extras/servo_state_publisher.hpp delete mode 100644 mavros_extras/launch/f710_joy.yaml delete mode 100644 mavros_extras/launch/px4_image.launch delete mode 100644 mavros_extras/launch/teleop.launch delete mode 100755 mavros_extras/scripts/mavftpfuse delete mode 100755 mavros_extras/scripts/mavteleop create mode 100644 mavros_extras/src/lib/servo_state_publisher.cpp create mode 100644 mavros_extras/src/plugins/3dr_radio.cpp create mode 100644 mavros_extras/src/plugins/hil.cpp create mode 100644 mavros_extras/src/plugins/vfr_hud.cpp delete mode 100644 mavros_extras/src/servo_state_publisher.cpp delete mode 100644 mavros_extras/src/visualization.cpp delete mode 100644 mavros_msgs/include/mavros_msgs/mavlink_convert.h create mode 100644 mavros_msgs/include/mavros_msgs/mavlink_convert.hpp create mode 100644 mavros_msgs/mavros_msgs_mapping_rule.yaml delete mode 100644 mavros_msgs/msg/BatteryStatus.msg create mode 100644 mavros_msgs/msg/ParamEvent.msg rename mavros_msgs/msg/{VFR_HUD.msg => VfrHud.msg} (100%) create mode 100644 mavros_msgs/srv/EndpointAdd.srv create mode 100644 mavros_msgs/srv/EndpointDel.srv create mode 100644 mavros_msgs/srv/ParamSetV2.srv create mode 100644 test_mavros/AMENT_IGNORE create mode 100644 test_mavros/COLCON_IGNORE create mode 100644 test_mavros/scripts/test_mavlink_convert_to_rosmsg.py diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 000000000..ff9e29cd5 --- /dev/null +++ b/.editorconfig @@ -0,0 +1,3 @@ +[*.{cpp,hpp}] +indent_style = space +indent_size = 4 diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index a41e7300a..191fb811a 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -7,11 +7,15 @@ on: [push, pull_request] jobs: industrial_ci: strategy: + fail-fast: false matrix: env: - # - {ROS_DISTRO: melodic, ROS_REPO: testing} - - {ROS_DISTRO: melodic, ROS_REPO: main} - - {ROS_DISTRO: noetic, ROS_REPO: main} + # - {ROS_DISTRO: foxy, ROS_REPO: testing} + - {ROS_DISTRO: foxy, ROS_REPO: main} + # - {ROS_DISTRO: galactic, ROS_REPO: testing} + - {ROS_DISTRO: galactic, ROS_REPO: main} + - {ROS_DISTRO: humble, ROS_REPO: main} + - {ROS_DISTRO: rolling, ROS_REPO: testing} env: CCACHE_DIR: /github/home/.ccache AFTER_INSTALL_TARGET_DEPENDENCIES: ./mavros/scripts/install_geographiclib_datasets.sh diff --git a/.ycm_extra_conf.py b/.ycm_extra_conf.py deleted file mode 100644 index 33183c399..000000000 --- a/.ycm_extra_conf.py +++ /dev/null @@ -1,263 +0,0 @@ -# -*- coding: utf-8 -*- - -########################################################################## -# YouCompleteMe configuration for ROS # -# Author: Gaël Ecorchard (2015) # -# # -# The file requires the definition of the $ROS_WORKSPACE variable in # -# your shell. # -# Name this file .ycm_extra_conf.py and place it in $ROS_WORKSPACE to # -# use it. # -# # -# Tested with Ubuntu 14.04 and Indigo. # -# # -# License: CC0 # -########################################################################## - -import os -import ycm_core - - -def GetRosIncludePaths(): - """Return a list of potential include directories - - The directories are looked for in $ROS_WORKSPACE. - """ - try: - from rospkg import RosPack - except ImportError: - return [] - rospack = RosPack() - includes = [] - includes.append(os.path.expandvars('$ROS_WORKSPACE') + '/devel/include') - for p in rospack.list(): - if os.path.exists(rospack.get_path(p) + '/include'): - includes.append(rospack.get_path(p) + '/include') - - for distribution in os.listdir('/opt/ros'): - includes.append('/opt/ros/' + distribution + '/include') - - return includes - - -def GetRosIncludeFlags(): - includes = GetRosIncludePaths() - flags = [] - for include in includes: - flags.append('-isystem') - flags.append(include) - - return flags - -# These are the compilation flags that will be used in case there's no -# compilation database set (by default, one is not set). -# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR. -# You can get CMake to generate the compilation_commands.json file for you by -# adding: -# set(CMAKE_EXPORT_COMPILE_COMMANDS 1) -# to your CMakeLists.txt file or by once entering -# catkin config --cmake-args '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON' -# in your shell. - -default_flags = [ - '-Wall', - '-Wextra', - '-Werror', - # '-Wc++98-compat', - # '-Wno-long-long', - # '-Wno-variadic-macros', - '-fexceptions', - '-DNDEBUG', - # THIS IS IMPORTANT! Without a "-std=" flag, clang won't know - # which language to use when compiling headers. So it will guess. Badly. So - # C++ headers will be compiled as C headers. You don't want that so ALWAYS - # specify a "-std=". - # For a C project, you would set this to something like 'c99' instead of - # 'c++11'. - # '-std=c++03', - '-std=c++11', - # ...and the same thing goes for the magic -x option which specifies the - # language that the files to be compiled are written in. This is mostly - # relevant for c++ headers. - # For a C project, you would set this to 'c' instead of 'c++'. - '-x', - 'c++', - '-I', - '.', - - # include third party libraries - # '-isystem', - # '/some/path/include', - '-isystem', - '/usr/include/eigen3', - - # mavros flags - #'-DMAVLINK_DIALECT=ardupilotmega', -] - -flags = default_flags + GetRosIncludeFlags() - - -def GetCompilationDatabaseFolder(filename): - """Return the directory potentially containing compilation_commands.json - - Return the absolute path to the folder (NOT the file!) containing the - compile_commands.json file to use that instead of 'flags'. See here for - more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html. - The compilation_commands.json for the given file is returned by getting - the package the file belongs to. - """ - try: - import rospkg - except ImportError: - return '' - pkg_name = rospkg.get_package_name(filename) - if not pkg_name: - return '' - dir = (os.path.expandvars('$ROS_WORKSPACE') + - os.path.sep + - 'build' + - os.path.sep + - pkg_name) - - return dir - - -def GetDatabase(compilation_database_folder): - if os.path.exists(compilation_database_folder): - return ycm_core.CompilationDatabase(compilation_database_folder) - return None - -SOURCE_EXTENSIONS = ['.cpp', '.cxx', '.cc', '.c', '.m', '.mm'] - - -def DirectoryOfThisScript(): - return os.path.dirname(os.path.abspath(__file__)) - - -def MakeRelativePathsInFlagsAbsolute(flags, working_directory): - if not working_directory: - return list(flags) - new_flags = [] - make_next_absolute = False - path_flags = ['-isystem', '-I', '-iquote', '--sysroot='] - for flag in flags: - new_flag = flag - - if make_next_absolute: - make_next_absolute = False - if not flag.startswith('/'): - new_flag = os.path.join(working_directory, flag) - - for path_flag in path_flags: - if flag == path_flag: - make_next_absolute = True - break - - if flag.startswith(path_flag): - path = flag[len(path_flag):] - new_flag = path_flag + os.path.join(working_directory, path) - break - - if new_flag: - new_flags.append(new_flag) - - return new_flags - - -def IsHeaderFile(filename): - extension = os.path.splitext(filename)[1] - return extension in ['.h', '.hxx', '.hpp', '.hh'] - - -def GetCompilationInfoForHeaderSameDir(headerfile, database): - """Return compile flags for src file with same base in the same directory - """ - filename_no_ext = os.path.splitext(headerfile)[0] - for extension in SOURCE_EXTENSIONS: - replacement_file = filename_no_ext + extension - if os.path.exists(replacement_file): - compilation_info = database.GetCompilationInfoForFile( - replacement_file) - if compilation_info.compiler_flags_: - return compilation_info - return None - - -def GetCompilationInfoForHeaderRos(headerfile, database): - """Return the compile flags for the corresponding src file in ROS - - Return the compile flags for the source file corresponding to the header - file in the ROS where the header file is. - """ - try: - import rospkg - except ImportError: - return None - pkg_name = rospkg.get_package_name(headerfile) - if not pkg_name: - return None - try: - pkg_path = rospkg.RosPack().get_path(pkg_name) - except rospkg.ResourceNotFound: - return None - filename_no_ext = os.path.splitext(headerfile)[0] - hdr_basename_no_ext = os.path.basename(filename_no_ext) - for path, dirs, files in os.walk(pkg_path): - for src_filename in files: - src_basename_no_ext = os.path.splitext(src_filename)[0] - if hdr_basename_no_ext != src_basename_no_ext: - continue - for extension in SOURCE_EXTENSIONS: - if src_filename.endswith(extension): - compilation_info = database.GetCompilationInfoForFile( - path + os.path.sep + src_filename) - if compilation_info.compiler_flags_: - return compilation_info - return None - - -def GetCompilationInfoForFile(filename, database): - # The compilation_commands.json file generated by CMake does not have - # entries for header files. So we do our best by asking the db for flags - # for a corresponding source file, if any. If one exists, the flags for - # that file should be good enough. - # Corresponding source file are looked for in the same package. - if IsHeaderFile(filename): - # Look in the same directory. - compilation_info = GetCompilationInfoForHeaderSameDir( - filename, database) - if compilation_info: - return compilation_info - # Look in the package. - compilation_info = GetCompilationInfoForHeaderRos(filename, database) - if compilation_info: - return compilation_info - return database.GetCompilationInfoForFile(filename) - - -def FlagsForFile(filename): - database = GetDatabase(GetCompilationDatabaseFolder(filename)) - if database: - # Bear in mind that compilation_info.compiler_flags_ does NOT return a - # python list, but a "list-like" StringVec object - compilation_info = GetCompilationInfoForFile(filename, database) - if not compilation_info: - # Return the default flags defined above. - return { - 'flags': flags, - 'do_cache': True, - } - - final_flags = MakeRelativePathsInFlagsAbsolute( - compilation_info.compiler_flags_, - compilation_info.compiler_working_dir_) - final_flags += default_flags - else: - relative_to = DirectoryOfThisScript() - final_flags = MakeRelativePathsInFlagsAbsolute(flags, relative_to) - - return { - 'flags': final_flags, - 'do_cache': True - } diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index eb9018d20..5cddcfae6 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,46 +1,76 @@ -Contributing -============ - - -1. Fork the repo: - ![fork](http://s24.postimg.org/pfvt9sdv9/Fork_mavros.png) -2. Clone the repo (`git clone https://github.com/mavlink/mavros.git`); -3. Create a remote connection to your repo (`git remote add git@github.com:/mavros.git`); -4. Create a feature/dev branch (`git checkout -b `); -5. Add the changes; -6. Apply the changes by committing (`git commit -m ""` or `git commit -a` and then write message; if adding new files: `git add `); -7. Check code style `uncrustify -c ${ROS_WORKSPACE}/src/mavros/mavros/tools/uncrustify-cpp.cfg --replace --no-backup `; -8. Fix small code style errors and typos; -9. Commit with description like "uncrustify" or "code style fix". Please avoid changes in program logic (separate commit are better than mix of style and bug fix); +# Contributing + +1. Fork the repo. [](https://github.com/mavlink/mavros/fork) +2. Clone the repo into your workspace: + + ```bash + git clone https://github.com/mavlink/mavros.git + ``` + +3. Create a remote connection to your repo: + + ```bash + git remote add origin git@github.com:/mavros.git + ``` + +4. Create a feature/dev branch: + + ```bash + git checkout -b + ``` + +5. Make your changes. +6. Commit the changes. The `-a` option automatically adds and removes files for you. + + ```bash + git commit -a -m "" + ``` + +7. Check your code style: + + ```bash + uncrustify -c ${ROS_WORKSPACE}/src/mavros/mavros/tools/uncrustify-cpp.cfg --replace --no-backup + ``` + +8. Fix small code style errors and typos. +9. Commit with a description like "uncrustify" or "code style fix". Please avoid changes in program logic (separate commits are better than a mix of style and bug fixes). 10. Run tests: - - with `catkin_make`, issue `catkin_make tests` and then `catkin_make run_tests`; - - with `catkin tools`, issue `catkin run_tests`; -11. If everything goes as planned, push the changes (`git push -u `) and issue a pull request. +- with `catkin_make`, issue `catkin_make tests` and then `catkin_make run_tests`; +- with `catkin tools`, issue `catkin run_tests`; + +11. If everything goes as planned, push the changes and issue a pull request. -cog.py generators ------------------ +```bash +git push -u origin +``` -In many places we need to copy some data from MAVLink, and in many places we have regular patterns of code (e.g. copy message fields). -To avoid manual copy-paste work (and errors!) we use [cog.py][cog] generator/preprocessor. -Generator program written in comment blocks on Python (that allow import pymavlink), output will be inserted between markers. -As an example you may look at `utils::to_string()` implementation for some enums: [lib/enum_to_string.cpp][ets]. +## cog.py generators -To install it : +In many places we need to copy some data from MAVLink, and in many places we have regular patterns of code (e.g. copied message fields). +To avoid manual copy-paste work (and errors!) we use the the [cog.py][cog] code generator/preprocessor. - pip install --user cogapp pymavlink +Cog generates C++ code from code blocks written in Python that you add into your C++ code file as specially formatted comments. Since you are now using Python, you can import and make use of the [pymavlink][pml] module, which already comes with MAVlink message definitions that you can reuse. +An example you may look at is the `utils::to_string()` implementation for some enums in [lib/enum_to_string.cpp][ets]. -Then fill the behaviour you when between the `[[[cog:]]]` `[[[end]]]` balise -and invoke cog like this: +Install cog and pymavlink: - cog.py -cr your_file.h/cpp +```bash +pip install --user cogapp pymavlink +``` -Your file will be updated by cog. +Add your generator code to your file as comments enclosed in the `[[[cog:]]]` and `[[[end]]]` tags. Then invoke cog so that it updates your file: - ./mavros/tools/cogall.sh +```bash +cog.py -cr your_file.h/cpp +``` -This script will regenerate all files with generators. +In addition, this script will re-generate all files with cog code: +```bash +./mavros/tools/cogall.sh +``` [cog]: https://nedbatchelder.com/code/cog/ [ets]: https://github.com/mavlink/mavros/blob/master/mavros/src/lib/enum_to_string.cpp +[pml]: https://mavlink.io/en/mavgen_python/ diff --git a/LICENSE-BSD.txt b/LICENSE-BSD.txt index 65225f5bd..22064a6d6 100644 --- a/LICENSE-BSD.txt +++ b/LICENSE-BSD.txt @@ -1,28 +1,27 @@ - Copyright (c) 2013-2015 Vladimir Ermakov. All rights reserved. + Copyright (c) 2013-2021 Vladimir Ermakov. All rights reserved. Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions -are met: +modification, are permitted provided that the following conditions are met: -1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. -2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in - the documentation and/or other materials provided with the - distribution. -3. Neither the name PX4 nor the names of its contributors may be - used to endorse or promote products derived from this software - without specific prior written permission. + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md index d5b3e403d..09ecb782b 100644 --- a/README.md +++ b/README.md @@ -69,8 +69,9 @@ We'd like to keep the project bug tracker as free as possible, so please contact CI Statuses ----------- - - ROS Melodic: [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__mavros__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__mavros__ubuntu_bionic_amd64/) - - ROS Noetic: [![Build Status](http://build.ros.org/buildStatus/icon?job=Ndev__mavros__ubuntu_focal_amd64)](http://build.ros.org/job/Ndev__mavros__ubuntu_focal_amd64/) + - ROS2 Foxy: [![Build Status](http://build.ros2.org/buildStatus/icon?job=Fdev__mavros__ubuntu_focal_amd64)](http://build.ros2.org/job/Fdev__mavros__ubuntu_focal_amd64/) + - ROS2 Galactic: [![Build Status](http://build.ros2.org/buildStatus/icon?job=Gdev__mavros__ubuntu_focal_amd64)](http://build.ros2.org/job/Gdev__mavros__ubuntu_focal_amd64/) + - ROS2 Rolling: [![Build Status](http://build.ros2.org/buildStatus/icon?job=Rdev__mavros__ubuntu_focal_amd64)](http://build.ros2.org/job/Rdev__mavros__ubuntu_focal_amd64/) [mrrm]: https://github.com/mavlink/mavros/blob/master/mavros/README.md diff --git a/cog-requirements.txt b/cog-requirements.txt new file mode 100644 index 000000000..e25b46914 --- /dev/null +++ b/cog-requirements.txt @@ -0,0 +1,4 @@ +attrs +comment_parser +cogapp +# pymavlink diff --git a/dependencies.rosinstall b/dependencies.rosinstall index b1b701537..b2b38f775 100644 --- a/dependencies.rosinstall +++ b/dependencies.rosinstall @@ -1,4 +1,4 @@ - git: local-name: mavlink uri: https://github.com/mavlink/mavlink-gbp-release.git - version: release/kinetic/mavlink + version: release/foxy/mavlink diff --git a/libmavconn/CHANGELOG.rst b/libmavconn/CHANGELOG.rst index 36a87fccc..fc3e3993b 100644 --- a/libmavconn/CHANGELOG.rst +++ b/libmavconn/CHANGELOG.rst @@ -2,8 +2,285 @@ Changelog for package libmavconn ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.16.0 (2023-05-05) -------------------- +2.5.0 (2023-05-05) +------------------ + +2.4.0 (2022-12-30) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.15.0 + update changelog + ci: update actions + Implement debug float array handler + mavros_extras: Fix a sequence point warning + mavros_extras: Fix a comparison that shouldn't be bitwise + mavros: Fix some warnings + mavros_extras: Fix buggy check for lat/lon ignored + libmavconn: fix MAVLink v1.0 output selection +* 1.15.0 +* update changelog +* Merge pull request `#1794 `_ from rossizero/master + libmavconn: fix MAVLink v1.0 output selection +* libmavconn: fix MAVLink v1.0 output selection + Fix `#1787 `_ +* Contributors: Vladimir Ermakov, rosrunne + +2.3.0 (2022-09-24) +------------------ +* mavros: remove custom find script, re-generate +* Merge branch 'master' into ros2 + * master: + 1.14.0 + update changelog + scripts: waypoint and param files are text, not binary + libmavconn: fix MAVLink v1.0 output selection + plugins: add guided_target to accept offboard position targets + add cmake module path for geographiclib on debian based systems + use already installed FindGeographicLib.cmake +* 1.14.0 +* update changelog +* libmavconn: fix MAVLink v1.0 output selection + Fix `#1787 `_ +* Merge pull request `#1775 `_ from acxz/find-geographiclib + use already installed FindGeographicLib.cmake +* use already installed FindGeographicLib.cmake +* Contributors: Vladimir Ermakov, acxz + +2.2.0 (2022-06-27) +------------------ +* Merge pull request `#1720 `_ from SylvainPastor/fix/libmavconn/udp/deadlocks + libmavconn: fix UDP deadlocks +* libmavconn: fix UDP deadlock + Same problems as for the TCP: + - `#1682 `_: fix std::system_error when tcp interface loses connection + - `#1679 `_: fix deadlock when call close() +* Contributors: Sylvain Pastor, Vladimir Ermakov + +2.1.1 (2022-03-02) +------------------ + +2.1.0 (2022-02-02) +------------------ +* lib: fix reorder +* Merge branch 'master' into ros2 + * master: + 1.13.0 + update changelog + py-lib: fix compatibility with py3 for Noetic + re-generate all coglets + test: add checks for ROTATION_CUSTOM + lib: Fix rotation search for CUSTOM + Removed CamelCase for class members. Publish to "report" + More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages + Fixed callback name to match `handle\_{MESSAGE_NAME.lower()}` convention + Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html + Fixed topic names to match more closely what other plugins use. Fixed a typo. + Add plugin for reporting terrain height estimate from FCU + 1.12.2 + update changelog + Set time/publish_sim_time to false by default + plugin: setpoint_raw: move getParam to initializer + extras: trajectory: backport `#1667 `_ +* 1.13.0 +* update changelog +* Merge pull request `#1682 `_ from SylvainPastor/fix/libmavconn/tcp/resource_deadlock + fix std::system_error when tcp interface loses connection +* fix code style divergences +* fix std::system_error when tcp interface loses connection + When the tcp connection is lost (remote TCP server stopped), an 'End of file' error is caught + by the io_thread in the do_recv() that calls the close() function. + In the close() function, we stop the io_service and wait for the end of the io_thread which + causes an std::system_error exception (cause: Resource deadlock avoided). + Error: mavconn: tcp0: receive: End of file at line 250 in libmavconn/src/tcp.cpp + terminate called after throwing an instance of 'std::system_error' + what(): Resource deadlock avoided + Aborted (core dumped) + fix: + - close() function: stop io_service if current thread id != io_thread id + - ~MAVConnTCPClient(): stop io_service and io_thread if thread is running +* Merge pull request `#1679 `_ from SylvainPastor/libmavconn/fix-tcp-deadlock-when-close + libmavconn: fix deadlock when call close() +* fix deadlock when call close() + When calling the close() function (by a different thread), a lock (mutex) is taken at + the start of this function which closes the socket and waits the end of io_service thread. + Closing the socket causes the 'Operation aborted' error in do_recv() function called by + io_service thread which in turn calls the close() function: sthis->close(). + This causes a 'deadlock'. + fix: Reduce the scope of the lock in the close() function so that it is released before + waiting for the thread to end. +* 1.12.2 +* update changelog +* lib: fix linter errors +* Merge branch 'master' into ros2 + * master: + 1.12.1 + update changelog + mavconn: fix connection issue introduced by `#1658 `_ + mavros_extras: Fix some warnings + mavros: Fix some warnings +* 1.12.1 +* update changelog +* mavconn: fix connection issue introduced by `#1658 `_ +* Contributors: Sylvain Pastor, Vladimir Ermakov + +2.0.5 (2021-11-28) +------------------ +* extras: fix some linter errors. + Do you know how to make me mad? Just let ament_uncrustify and + ament_cpplint require opposite requirements! +* lib: fix linter errors +* fix some build warnings; drop old copter vis +* lib: fix merge artifact +* Merge branch 'master' into ros2 + * master: + 1.12.0 + update changelog + Fix multiple bugs + lib: fix mission frame debug print + extras: distance_sensor: revert back to zero quaternion +* 1.12.0 +* update changelog +* Merge pull request `#1658 `_ from asherikov/as_bugfixes + Fix multiple bugs +* Fix multiple bugs + - fix bad_weak_ptr on connect and disconnect + - introduce new API to avoid thread race when assigning callbacks + - fix uninitialized variable in TCP client constructor which would + randomly block TCP server + This is an API breaking change: if client code creates connections using + make_shared<>() instead of open_url(), it is now necessary to call new + connect() method explicitly. +* cmake: require C++20 to build all modules +* lib: ignore MAVPACKED-related warnings from mavlink +* Merge branch 'master' into ros2 + * master: + 1.11.1 + update changelog + lib: fix build +* 1.11.1 +* update changelog +* Merge branch 'master' into ros2 + * master: + 1.11.0 + update changelog + lib: fix ftf warnings + msgs: use pragmas to ignore unaligned pointer warnings + extras: landing_target: fix misprint + msgs: fix convert const + plugin: setpoint_raw: fix misprint + msgs: try to hide 'unaligned pointer' warning + plugin: sys: fix compillation error + plugin: initialize quaternions with identity + plugin: sys: Use wall timers for connection management + Use meters for relative altitude + distance_sensor: Initialize sensor orientation quaternion to zero + Address review comments + Add camera plugin for interfacing with mavlink camera protocol +* 1.11.0 +* update changelog +* Contributors: Alexander Sherikov, Vladimir Ermakov + +2.0.4 (2021-11-04) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.10.0 + prepare release +* 1.10.0 +* prepare release +* mavconn: update to use std::error_code +* Merge branch 'master' into ros2 + * master: (25 commits) + Remove reference + Catch std::length_error in send_message + Show ENOTCONN error instead of crash + Tunnel: Check for invalid payload length + Tunnel.msg: Generate enum with cog + mavros_extras: Create tunnel plugin + mavros_msgs: Add Tunnel message + MountControl.msg: fix copy-paste + sys_time.cpp: typo + sys_time: publish /clock for simulation times + 1.9.0 + update changelog + Spelling corrections + Changed OverrideRCIn to 18 channels + This adds functionality to erase all logs on the SD card via mavlink + publish BATTERY2 message as /mavros/battery2 topic + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 + Added NAV_CONTROLLER_OUTPUT Plugin + Added GPS_INPUT plugin + Update esc_status plugin with datatype change on MAVLink. + ... +* Merge pull request `#1626 `_ from valbok/crash_on_shutdown + Show ENOTCONN error instead of crash on socket's shutdown +* Show ENOTCONN error instead of crash + When a client suddenly drops the connection, + socket.shutdown() will throw an exception: + boost::exception_detail::clone_impl > + what(): shutdown: Transport endpoint is not connected + Showing an error in this common case looks more reasonable than crashing. +* 1.9.0 +* update changelog +* Contributors: Val Doroshchuk, Vladimir Ermakov + +2.0.3 (2021-06-20) +------------------ + +2.0.2 (2021-06-20) +------------------ +* lib: yet another fix of cmake module +* lib: fix lint error +* lib: fix cmake lint error +* Contributors: Vladimir Ermakov + +2.0.1 (2021-06-06) +------------------ +* Merge branch 'master' into ros2 + * master: + readme: update + 1.8.0 + update changelog + Create semgrep-analysis.yml + Create codeql-analysis.yml +* 1.8.0 +* update changelog +* Contributors: Vladimir Ermakov + +2.0.0 (2021-05-28) +------------------ +* pylib: fix flake8 +* libmavconn: fix uncrustify test error +* Merge branch 'master' into ros2 + * master: + 1.7.1 + update changelog + re-generate all pymavlink enums + 1.7.0 + update changelog +* router: rename mavlink to/from to source/sink, i think that terms more descriptive +* mavros: fix cmake to build libmavros +* lib: make ament_lint_cmake happy +* msgs: add linter +* lib: fix all ament_cpplint errors +* lib: make cpplint happy +* lib: make ament_uncrustify happy, update BSD license text to one known by ament_copyright +* lib: try to fix ament_copyright lint +* lib: port cpp, update license headers for ament_copyright +* lib: port to standalone asio +* lib: remove boost usage from headers +* lib: update code style +* lib: rename cpp headers +* lib: provide copy of em_expand() +* lib: update readme +* libmavconn: start porintg, will use plain asio, without boost +* Merge pull request `#1186 `_ from PickNikRobotics/ros2 + mavros_msgs Ros2 +* Merge branch 'ros2' into ros2 +* msgs: start porting to ROS2 +* disable all packages but messages +* Contributors: Mikael Arguedas, Vladimir Ermakov 1.15.0 (2022-12-30) ------------------- diff --git a/libmavconn/CMakeLists.txt b/libmavconn/CMakeLists.txt index 1bcd328bc..5ee8cee83 100644 --- a/libmavconn/CMakeLists.txt +++ b/libmavconn/CMakeLists.txt @@ -1,113 +1,117 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(libmavconn) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) +# Default to C11 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 11) +endif() -## System dependencies are found with CMake's conventions -find_package(console_bridge REQUIRED) -find_package(Boost REQUIRED COMPONENTS system) +# Default to C++20 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++2a") +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # we dont use add_compile_options with pedantic in message packages + # because the Python C extensions dont comply with it + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wcomment") + +# Allow GNU extensions (-std=gnu++20) +set(CMAKE_C_EXTENSIONS ON) +set(CMAKE_CXX_EXTENSIONS ON) + +find_package(ament_cmake REQUIRED) # add package modules path, not needed in dependend packages list(INSERT CMAKE_MODULE_PATH 0 "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules") -include(EnableCXX11) -include(MavrosMavlink) - -# Fixed in mavlink 2016.7.7 -#-> enum values out of int range -#list(APPEND IGNORE_DIALECTS "autoquad") - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS include - LIBRARIES mavconn - DEPENDS Boost console_bridge mavlink - CFG_EXTRAS libmavconn-extras.cmake -) -########### -## Build ## -########### +## System dependencies are found with CMake's conventions +find_package(console_bridge REQUIRED) +find_package(ASIO REQUIRED) +find_package(mavlink REQUIRED) + +include(em_expand) include_directories( include - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/include - ${Boost_INCLUDE_DIRS} + ${CMAKE_CURRENT_BINARY_DIR}/generated/include + ${ASIO_INCLUDE_DIRS} ${mavlink_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS} ) ## Declare a cpp library -add_library(mavconn - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/src/mavlink_helpers.cpp +add_library(mavconn SHARED + ${CMAKE_CURRENT_BINARY_DIR}/generated/src/mavlink_helpers.cpp src/interface.cpp src/serial.cpp src/tcp.cpp src/udp.cpp ) -target_link_libraries(mavconn - ${Boost_LIBRARIES} - ${console_bridge_LIBRARIES} +ament_target_dependencies(mavconn + "console_bridge" ) -# Use catkin-supplied em_expand macros to generate source files +# Use em_expand macros to generate source files em_expand(${CMAKE_CURRENT_SOURCE_DIR}/mavlink.context.py.in - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/mavlink.context.py - ${CMAKE_CURRENT_SOURCE_DIR}/include/mavconn/mavlink_dialect.h.em - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/include/mavconn/mavlink_dialect.h) + ${CMAKE_CURRENT_BINARY_DIR}/generated/mavlink.context.py + ${CMAKE_CURRENT_SOURCE_DIR}/include/mavconn/mavlink_dialect.hpp.em + ${CMAKE_CURRENT_BINARY_DIR}/generated/include/mavconn/mavlink_dialect.hpp) em_expand(${CMAKE_CURRENT_SOURCE_DIR}/mavlink.context.py.in - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/mavlink.context.py + ${CMAKE_CURRENT_BINARY_DIR}/generated/mavlink.context.py ${CMAKE_CURRENT_SOURCE_DIR}/src/mavlink_helpers.cpp.em - ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/src/mavlink_helpers.cpp) + ${CMAKE_CURRENT_BINARY_DIR}/generated/src/mavlink_helpers.cpp) -message(STATUS "Copy mavlink_dialect.h to source tree") +message(STATUS "Copy mavlink_dialect.hpp to source tree") file( - COPY ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/include/mavconn/mavlink_dialect.h + COPY ${CMAKE_CURRENT_BINARY_DIR}/generated/include/mavconn/mavlink_dialect.hpp DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/include/mavconn/ - ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html +) -## Mark executables and/or libraries for installation install(TARGETS mavconn - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -## Mark cpp header files for installation -install(DIRECTORY include/mavconn/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/mavconn - FILES_MATCHING PATTERN "*.h" +install(DIRECTORY include/ + DESTINATION include ) -install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/catkin_generated/include/mavconn/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/mavconn - FILES_MATCHING PATTERN "*.h" +install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/generated/include/mavconn/ + DESTINATION include/mavconn + FILES_MATCHING PATTERN "*.hpp" ) -## Install cmake files (thanks to cmake_modules package) -install(DIRECTORY cmake/Modules - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cmake +install(DIRECTORY cmake + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) -if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(mavconn-test test/test_mavconn.cpp) - target_link_libraries(mavconn-test mavconn pthread) + # NOTE(vooon): Does not support our custom triple-license, tiered to make it to work. + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_mavconn test/test_mavconn.cpp) + target_link_libraries(test_mavconn mavconn) + ament_target_dependencies(test_mavconn + "console_bridge" + ) endif() +ament_export_dependencies(console_bridge) +ament_export_include_directories(include) +ament_export_libraries(mavconn) +#ament_export_targets(mavconn) +ament_package( + CONFIG_EXTRAS "libmavconn-extras.cmake" +) + # vim: ts=2 sw=2 et: diff --git a/libmavconn/README.md b/libmavconn/README.md index d3357646f..325d1fed8 100644 --- a/libmavconn/README.md +++ b/libmavconn/README.md @@ -32,9 +32,9 @@ Dependencies Same as for mavros: - Linux host - - Boost >= 1.46 (used Boost.ASIO) + - Asio library ( https://think-async.com/Asio/ ) - console-bridge library - - compiller with C++11 support + - compiller with C++14 support License diff --git a/libmavconn/cmake/Modules/CheckGeographicLibDatasets.cmake b/libmavconn/cmake/Modules/CheckGeographicLibDatasets.cmake index 561ff5952..aef85fbb1 100644 --- a/libmavconn/cmake/Modules/CheckGeographicLibDatasets.cmake +++ b/libmavconn/cmake/Modules/CheckGeographicLibDatasets.cmake @@ -1,27 +1,56 @@ +# +# libmavconn +# Copyright 2013-2016,2018,2021 Vladimir Ermakov, All rights reserved. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +# + ## # This module verifies the installation of the GeographicLib datasets and warns # if it doesn't detect them. ## -find_path(GEOGRAPHICLIB_GEOID_PATH NAMES geoids PATH_SUFFIXES share/GeographicLib share/geographiclib) -find_path(GEOGRAPHICLIB_GRAVITY_PATH_ NAMES gravity PATH_SUFFIXES share/GeographicLib) -find_path(GEOGRAPHICLIB_MAGNETIC_PATH_ NAMES magnetic PATH_SUFFIXES share/GeographicLib) +find_path(GEOGRAPHICLIB_GEOID_PATH + NAMES geoids + PATH_SUFFIXES share/GeographicLib share/geographiclib +) +find_path(GEOGRAPHICLIB_GRAVITY_PATH_ + NAMES gravity + PATH_SUFFIXES share/GeographicLib +) +find_path(GEOGRAPHICLIB_MAGNETIC_PATH_ + NAMES magnetic + PATH_SUFFIXES share/GeographicLib +) if(NOT GEOGRAPHICLIB_GEOID_PATH) - message(STATUS "No geoid model datasets found. This will result on a SIGINT! Please execute the script install_geographiclib_dataset.sh in /mavros/scripts") + message(STATUS "\ +No geoid model datasets found. This will result on a SIGINT! \ +Please execute the script install_geographiclib_dataset.sh") else() - message(STATUS "Geoid model datasets found in: " ${GEOGRAPHICLIB_GEOID_PATH}/geoid) + message(STATUS "\ +Geoid model datasets found in: ${GEOGRAPHICLIB_GEOID_PATH}/geoid") set(GEOGRAPHICLIB_GEOID_PATH ${GEOGRAPHICLIB_GEOID_PATH}/geoid) endif() if(NOT GEOGRAPHICLIB_GRAVITY_PATH_) - message(STATUS "No gravity field model datasets found. Please execute the script install_geographiclib_dataset.sh in /mavros/scripts") + message(STATUS "\ +No gravity field model datasets found. +Please execute the script install_geographiclib_dataset.sh") else() - message(STATUS "Gravity Field model datasets found in: " ${GEOGRAPHICLIB_GRAVITY_PATH_}/gravity) + message(STATUS "\ +Gravity Field model datasets found in: \ +${GEOGRAPHICLIB_GRAVITY_PATH_}/gravity") set(GEOGRAPHICLIB_GRAVITY_PATH ${GEOGRAPHICLIB_GRAVITY_PATH_}/gravity) endif() if(NOT GEOGRAPHICLIB_MAGNETIC_PATH_) - message(STATUS "No magnetic field model datasets found. Please execute the script install_geographiclib_dataset.sh in /mavros/scripts") + message(STATUS "\ +No magnetic field model datasets found. \ +Please execute the script install_geographiclib_dataset.sh") else() - message(STATUS "Magnetic Field model datasets found in: " ${GEOGRAPHICLIB_MAGNETIC_PATH_}/magnetic) + message(STATUS "\ +Magnetic Field model datasets found in: \ +${GEOGRAPHICLIB_MAGNETIC_PATH_}/magnetic") set(GEOGRAPHICLIB_MAGNETIC_PATH ${GEOGRAPHICLIB_MAGNETIC_PATH_}/magnetic) endif() diff --git a/libmavconn/cmake/Modules/EnableCXX11.cmake b/libmavconn/cmake/Modules/EnableCXX11.cmake deleted file mode 100644 index 73d819e2a..000000000 --- a/libmavconn/cmake/Modules/EnableCXX11.cmake +++ /dev/null @@ -1,20 +0,0 @@ -# This module enables C++11 or C++14 support -# -# thanks for: http://www.guyrutenberg.com/2014/01/05/enabling-c11-c0x-in-cmake/ - -include(CheckCXXCompilerFlag) - -check_cxx_compiler_flag("-std=c++14" COMPILER_SUPPORTS_STD_CXX14) -if (NOT COMPILER_SUPPORTS_STD_CXX14) - check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_STD_CXX11) -endif () - -if (COMPILER_SUPPORTS_STD_CXX14) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") -elseif (COMPILER_SUPPORTS_STD_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -else () - message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") -endif () - -# vim: set ts=2 sw=2 et: diff --git a/libmavconn/cmake/Modules/FindASIO.cmake b/libmavconn/cmake/Modules/FindASIO.cmake new file mode 100644 index 000000000..f6901c6f7 --- /dev/null +++ b/libmavconn/cmake/Modules/FindASIO.cmake @@ -0,0 +1,25 @@ +# +# libmavconn +# Copyright 2013-2016,2018,2021 Vladimir Ermakov, All rights reserved. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +# + +# Look for Asio +# +# Set +# Asio_FOUND = TRUE +# Asio_INCLUDE_DIRS = /usr/local/include + +find_path(ASIO_INCLUDE_DIRS + NAMES asio.hpp + PATHS /usr/include /usr/local/include +) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(ASIO DEFAULT_MSG + ASIO_INCLUDE_DIRS +) +mark_as_advanced(ASIO_INCLUDE_DIRS) diff --git a/libmavconn/cmake/Modules/MavrosMavlink.cmake b/libmavconn/cmake/Modules/MavrosMavlink.cmake deleted file mode 100644 index 7e1bb418f..000000000 --- a/libmavconn/cmake/Modules/MavrosMavlink.cmake +++ /dev/null @@ -1,9 +0,0 @@ -# This module includes MAVLink package and - -find_package(mavlink REQUIRED) - -# -# Nothing to do since MAVLink v2.0 -# - -# vim: set ts=2 sw=2 et: diff --git a/libmavconn/cmake/Modules/em_expand.cmake b/libmavconn/cmake/Modules/em_expand.cmake new file mode 100644 index 000000000..a83011cf2 --- /dev/null +++ b/libmavconn/cmake/Modules/em_expand.cmake @@ -0,0 +1,71 @@ +# +# libmavconn +# Copyright 2021 Vladimir Ermakov, All rights reserved. +# +# That module ported from catkin project. +# Here original copyright notice: +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +find_package(Python3 COMPONENTS Interpreter) + +function(em_expand context_in context_out em_file_in file_out) + assert_file_exists("${context_in}" "input file for context missing") + assert_file_exists("${em_file_in}" "template file missing") + message(DEBUG "configure_file(${context_in}, ${context_out})") + configure_file(${context_in} ${context_out} @ONLY) + assert_file_exists("${context_out}" "context file was not generated correctly") + + stamp(${em_file_in}) + + # create directory if necessary + get_filename_component(_folder_out ${file_out} PATH) + if(NOT IS_DIRECTORY ${_folder_out}) + file(MAKE_DIRECTORY ${_folder_out}) + endif() + + message(DEBUG "Evaluate template '${em_file_in}' to '${file_out}' (with context from '${context_out}')") + execute_process(COMMAND + ${Python3_EXECUTABLE} + -m em + --raw-errors + -F ${context_out} + -o ${file_out} + ${em_file_in} + RESULT_VARIABLE _res) + + if(NOT _res EQUAL 0) + message(FATAL_ERROR "em.py return error code ${_res}") + endif() +endfunction() + diff --git a/libmavconn/cmake/libmavconn-extras.cmake.develspace.in b/libmavconn/cmake/libmavconn-extras.cmake.develspace.in deleted file mode 100644 index 0c73a9406..000000000 --- a/libmavconn/cmake/libmavconn-extras.cmake.develspace.in +++ /dev/null @@ -1,2 +0,0 @@ -# Prepend cmake modules from source directory to the cmake module path -list(INSERT CMAKE_MODULE_PATH 0 "@CMAKE_CURRENT_SOURCE_DIR@/cmake/Modules") diff --git a/libmavconn/cmake/libmavconn-extras.cmake.installspace.in b/libmavconn/cmake/libmavconn-extras.cmake.installspace.in deleted file mode 100644 index b29f87120..000000000 --- a/libmavconn/cmake/libmavconn-extras.cmake.installspace.in +++ /dev/null @@ -1,2 +0,0 @@ -# Prepend the installed cmake modules to the cmake module path -list(INSERT CMAKE_MODULE_PATH 0 "${libmavconn_DIR}/../../../@CATKIN_PACKAGE_SHARE_DESTINATION@/cmake/Modules") diff --git a/libmavconn/include/mavconn/console_bridge_compat.h b/libmavconn/include/mavconn/console_bridge_compat.h deleted file mode 100644 index a0052da95..000000000 --- a/libmavconn/include/mavconn/console_bridge_compat.h +++ /dev/null @@ -1,57 +0,0 @@ -/** - * @brief MAVConn console-bridge compatibility header - * @file console_bridge_compat.h - * @author Vladimir Ermakov - * - * @addtogroup mavconn - * @{ - */ -/* - * libmavconn - * Copyright 2018 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include - -// [[[cog: -// for idx, func in enumerate(('debug', 'inform', 'warn', 'error')): -// fn = f'CONSOLE_BRIDGE_log{func.title()}' -// fu = func.upper() -// -// if func == 'inform': # NOTE: special case -// fu = 'INFO' -// -// if idx != 0: -// cog.outl() -// -// cog.outl(f'#ifndef {fn}') -// cog.outl(f'#define {fn}(fmt, ...) \\') -// cog.outl(f'\tconsole_bridge::log(__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_{fu}, fmt, ##__VA_ARGS__)') -// cog.outl(f'#endif // {fn}') -// ]]] -#ifndef CONSOLE_BRIDGE_logDebug -#define CONSOLE_BRIDGE_logDebug(fmt, ...) \ - console_bridge::log(__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_DEBUG, fmt, ##__VA_ARGS__) -#endif // CONSOLE_BRIDGE_logDebug - -#ifndef CONSOLE_BRIDGE_logInform -#define CONSOLE_BRIDGE_logInform(fmt, ...) \ - console_bridge::log(__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_INFO, fmt, ##__VA_ARGS__) -#endif // CONSOLE_BRIDGE_logInform - -#ifndef CONSOLE_BRIDGE_logWarn -#define CONSOLE_BRIDGE_logWarn(fmt, ...) \ - console_bridge::log(__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_WARN, fmt, ##__VA_ARGS__) -#endif // CONSOLE_BRIDGE_logWarn - -#ifndef CONSOLE_BRIDGE_logError -#define CONSOLE_BRIDGE_logError(fmt, ...) \ - console_bridge::log(__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, fmt, ##__VA_ARGS__) -#endif // CONSOLE_BRIDGE_logError -// [[[end]]] (checksum: c8dda3189b05a621b7244bf375275345) diff --git a/libmavconn/include/mavconn/console_bridge_compat.hpp b/libmavconn/include/mavconn/console_bridge_compat.hpp new file mode 100644 index 000000000..fbf3199ec --- /dev/null +++ b/libmavconn/include/mavconn/console_bridge_compat.hpp @@ -0,0 +1,71 @@ +// +// libmavconn +// Copyright 2018 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// +/** + * @brief MAVConn console-bridge compatibility header + * @file console_bridge_compat.h + * @author Vladimir Ermakov + * + * @addtogroup mavconn + * @{ + */ + +#pragma once +#ifndef MAVCONN__CONSOLE_BRIDGE_COMPAT_HPP_ +#define MAVCONN__CONSOLE_BRIDGE_COMPAT_HPP_ + +#include + +// [[[cog: +// for idx, func in enumerate(('debug', 'inform', 'warn', 'error')): +// fn = f'CONSOLE_BRIDGE_log{func.title()}' +// fu = func.upper() +// +// if func == 'inform': # NOTE: special case +// fu = 'INFO' +// +// if idx != 0: +// cog.outl() +// +// cog.outl(f'#ifndef {fn}') +// cog.outl(f'#define {fn}(fmt, ...) \\') +// cog.outl(f' console_bridge::log( \\') +// cog.outl(f' __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_{fu}, fmt, \\') +// cog.outl(f' ## __VA_ARGS__)') +// cog.outl(f'#endif // {fn}') +// ]]] +#ifndef CONSOLE_BRIDGE_logDebug +#define CONSOLE_BRIDGE_logDebug(fmt, ...) \ + console_bridge::log( \ + __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_DEBUG, fmt, \ + ## __VA_ARGS__) +#endif // CONSOLE_BRIDGE_logDebug + +#ifndef CONSOLE_BRIDGE_logInform +#define CONSOLE_BRIDGE_logInform(fmt, ...) \ + console_bridge::log( \ + __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_INFO, fmt, \ + ## __VA_ARGS__) +#endif // CONSOLE_BRIDGE_logInform + +#ifndef CONSOLE_BRIDGE_logWarn +#define CONSOLE_BRIDGE_logWarn(fmt, ...) \ + console_bridge::log( \ + __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_WARN, fmt, \ + ## __VA_ARGS__) +#endif // CONSOLE_BRIDGE_logWarn + +#ifndef CONSOLE_BRIDGE_logError +#define CONSOLE_BRIDGE_logError(fmt, ...) \ + console_bridge::log( \ + __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, fmt, \ + ## __VA_ARGS__) +#endif // CONSOLE_BRIDGE_logError +// [[[end]]] (checksum: ac76c84da411728c99a9b3995d1ffe37) + +#endif // MAVCONN__CONSOLE_BRIDGE_COMPAT_HPP_ diff --git a/libmavconn/include/mavconn/interface.h b/libmavconn/include/mavconn/interface.h deleted file mode 100644 index c93d013c7..000000000 --- a/libmavconn/include/mavconn/interface.h +++ /dev/null @@ -1,321 +0,0 @@ -/** - * @brief MAVConn class interface - * @file interface.h - * @author Vladimir Ermakov - * - * @addtogroup mavconn - * @{ - * @brief MAVConn connection handling library - * - * This lib provide simple interface to MAVLink enabled devices - * such as autopilots. - */ -/* - * libmavconn - * Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace mavconn { -using steady_clock = std::chrono::steady_clock; -using lock_guard = std::lock_guard; - -//! Same as @p mavlink::common::MAV_COMPONENT::COMP_ID_UDP_BRIDGE -static constexpr auto MAV_COMP_ID_UDP_BRIDGE = 240; - -//! Rx packer framing status. (same as @p mavlink::mavlink_framing_t) -enum class Framing : uint8_t { - incomplete = mavlink::MAVLINK_FRAMING_INCOMPLETE, - ok = mavlink::MAVLINK_FRAMING_OK, - bad_crc = mavlink::MAVLINK_FRAMING_BAD_CRC, - bad_signature = mavlink::MAVLINK_FRAMING_BAD_SIGNATURE, -}; - -//! MAVLink protocol version -enum class Protocol : uint8_t { - V10 = 1, //!< MAVLink v1.0 - V20 = 2 //!< MAVLink v2.0 -}; - -/** - * @brief Common exception for communication error - */ -class DeviceError : public std::runtime_error { -public: - /** - * @breif Construct error. - */ - template - DeviceError(const char *module, T msg) : - std::runtime_error(make_message(module, msg)) - { } - - template - static std::string make_message(const char *module, T msg) { - std::ostringstream ss; - ss << "DeviceError:" << module << ":" << msg_to_string(msg); - return ss.str(); - } - - static std::string msg_to_string(const char *description) { - return description; - } - - static std::string msg_to_string(int errnum) { - return ::strerror(errnum); - } - - static std::string msg_to_string(boost::system::system_error &err) { - return err.what(); - } -}; - -/** - * @brief Generic mavlink interface - */ -class MAVConnInterface { -private: - MAVConnInterface(const MAVConnInterface&) = delete; - -public: - using ReceivedCb = std::function; - using ClosedCb = std::function; - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - using WeakPtr = std::weak_ptr; - - struct IOStat { - size_t tx_total_bytes; //!< total bytes transferred - size_t rx_total_bytes; //!< total bytes received - float tx_speed; //!< current transfer speed [B/s] - float rx_speed; //!< current receive speed [B/s] - }; - - /** - * @param[in] system_id sysid for send_message - * @param[in] component_id compid for send_message - */ - MAVConnInterface(uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE); - - /** - * @brief Establish connection, automatically called by open_url() - */ - virtual void connect( - const ReceivedCb &cb_handle_message, - const ClosedCb &cb_handle_closed_port = ClosedCb()) = 0; - - /** - * @brief Close connection. - */ - virtual void close() = 0; - - /** - * @brief Send message (mavlink_message_t) - * - * Can be used to forward messages from other connection channel. - * - * @note Does not do finalization! - * - * @throws std::length_error On exceeding Tx queue limit (MAX_TXQ_SIZE) - * @param[in] *message not changed - */ - virtual void send_message(const mavlink::mavlink_message_t *message) = 0; - - /** - * @brief Send message (child of mavlink::Message) - * - * Does serialization inside. - * System and Component ID = from this object. - * - * @throws std::length_error On exceeding Tx queue limit (MAX_TXQ_SIZE) - * @param[in] &message not changed - */ - virtual void send_message(const mavlink::Message &message) { - send_message(message, this->comp_id); - } - - /** - * @brief Send message (child of mavlink::Message) - * - * Does serialization inside. - * System ID = from this object. - * Component ID passed by argument. - * - * @throws std::length_error On exceeding Tx queue limit (MAX_TXQ_SIZE) - * @param[in] &message not changed - * @param[in] src_compid sets the component ID of the message source - */ - virtual void send_message(const mavlink::Message &message, const uint8_t src_compid) = 0; - - /** - * @brief Send raw bytes (for some quirks) - * @throws std::length_error On exceeding Tx queue limit (MAX_TXQ_SIZE) - */ - virtual void send_bytes(const uint8_t *bytes, size_t length) = 0; - - /** - * @brief Send message and ignore possible drop due to Tx queue limit - */ - void send_message_ignore_drop(const mavlink::mavlink_message_t *message); - - /** - * @brief Send message and ignore possible drop due to Tx queue limit - * - * System and Component ID = from this object. - */ - void send_message_ignore_drop(const mavlink::Message &message) { - send_message_ignore_drop(message, this->comp_id); - } - - /** - * @brief Send message and ignore possible drop due to Tx queue limit - * - * System ID = from this object. - * Component ID passed by argument. - */ - void send_message_ignore_drop(const mavlink::Message &message, const uint8_t src_compid); - - //! Message receive callback - ReceivedCb message_received_cb; - //! Port closed notification callback - ClosedCb port_closed_cb; - - virtual mavlink::mavlink_status_t get_status(); - virtual IOStat get_iostat(); - virtual bool is_open() = 0; - - inline uint8_t get_system_id() { - return sys_id; - } - inline void set_system_id(uint8_t sysid) { - sys_id = sysid; - } - inline uint8_t get_component_id() { - return comp_id; - } - inline void set_component_id(uint8_t compid) { - comp_id = compid; - } - - /** - * Set protocol used for encoding mavlink::Mavlink messages. - */ - void set_protocol_version(Protocol pver); - Protocol get_protocol_version(); - - /** - * @brief Construct connection from URL - * - * Supported URL schemas: - * - serial:// - * - udp:// - * - tcp:// - * - tcp-l:// - * - * Please see user's documentation for details. - * - * @param[in] url resource locator - * @param[in] system_id optional system id - * @param[in] component_id optional component id - * @return @a Ptr to constructed interface class, - * or throw @a DeviceError if error occured. - */ - static Ptr open_url( - std::string url, - uint8_t system_id = 1, - uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, - const ReceivedCb &cb_handle_message = ReceivedCb(), - const ClosedCb &cb_handle_closed_port = ClosedCb()); - - /** - * @brief version of open_url() which do not perform connect() - */ - static Ptr open_url_no_connect( - std::string url, - uint8_t system_id = 1, - uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE); - - static std::vector get_known_dialects(); - -protected: - uint8_t sys_id; //!< Connection System Id - uint8_t comp_id; //!< Connection Component Id - - //! Maximum mavlink packet size + some extra bytes for padding. - static constexpr size_t MAX_PACKET_SIZE = MAVLINK_MAX_PACKET_LEN + 16; - //! Maximum count of transmission buffers. - static constexpr size_t MAX_TXQ_SIZE = 1000; - - //! This map merge all dialect mavlink_msg_entry_t structs. Needed for packet parser. - static std::unordered_map message_entries; - - //! Channel number used for logging. - size_t conn_id; - - inline mavlink::mavlink_status_t *get_status_p() { - return &m_parse_status; - } - - inline mavlink::mavlink_message_t *get_buffer_p() { - return &m_buffer; - } - - /** - * Parse buffer and emit massage_received. - */ - void parse_buffer(const char *pfx, uint8_t *buf, const size_t bufsize, size_t bytes_received); - - void iostat_tx_add(size_t bytes); - void iostat_rx_add(size_t bytes); - - void log_recv(const char *pfx, mavlink::mavlink_message_t &msg, Framing framing); - void log_send(const char *pfx, const mavlink::mavlink_message_t *msg); - void log_send_obj(const char *pfx, const mavlink::Message &msg); - -private: - friend const mavlink::mavlink_msg_entry_t* mavlink::mavlink_get_msg_entry(uint32_t msgid); - - mavlink::mavlink_status_t m_parse_status; - mavlink::mavlink_message_t m_buffer; - mavlink::mavlink_status_t m_mavlink_status; - - std::atomic tx_total_bytes, rx_total_bytes; - std::recursive_mutex iostat_mutex; - size_t last_tx_total_bytes, last_rx_total_bytes; - std::chrono::time_point last_iostat; - - //! monotonic counter (increment only) - static std::atomic conn_id_counter; - - //! init_msg_entry() once flag - static std::once_flag init_flag; - - /** - * Initialize message_entries map - * - * autogenerated. placed in mavlink_helpers.cpp - */ - static void init_msg_entry(); -}; -} // namespace mavconn diff --git a/libmavconn/include/mavconn/interface.hpp b/libmavconn/include/mavconn/interface.hpp new file mode 100644 index 000000000..8b71120fc --- /dev/null +++ b/libmavconn/include/mavconn/interface.hpp @@ -0,0 +1,347 @@ +// +// libmavconn +// Copyright 2013,2014,2015,2016,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// +/** + * @brief MAVConn class interface + * @file interface.h + * @author Vladimir Ermakov + * + * @addtogroup mavconn + * @{ + * @brief MAVConn connection handling library + * + * This lib provide simple interface to MAVLink enabled devices + * such as autopilots. + */ + +#pragma once +#ifndef MAVCONN__INTERFACE_HPP_ +#define MAVCONN__INTERFACE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mavconn +{ +using steady_clock = std::chrono::steady_clock; +using lock_guard = std::lock_guard; + +//! Same as @p mavlink::common::MAV_COMPONENT::COMP_ID_UDP_BRIDGE +static constexpr auto MAV_COMP_ID_UDP_BRIDGE = 240; + +//! Rx packer framing status. (same as @p mavlink::mavlink_framing_t) +enum class Framing : uint8_t +{ + incomplete = mavlink::MAVLINK_FRAMING_INCOMPLETE, + ok = mavlink::MAVLINK_FRAMING_OK, + bad_crc = mavlink::MAVLINK_FRAMING_BAD_CRC, + bad_signature = mavlink::MAVLINK_FRAMING_BAD_SIGNATURE, +}; + +//! MAVLink protocol version +enum class Protocol : uint8_t +{ + V10 = 1, //!< MAVLink v1.0 + V20 = 2 //!< MAVLink v2.0 +}; + +/** + * @brief Common exception for communication error + */ +class DeviceError : public std::runtime_error +{ +public: + /** + * @breif Construct error. + */ + template + DeviceError(const char * module, T msg) + : std::runtime_error(make_message(module, msg)) + { + } + + template + static std::string make_message(const char * module, T msg) + { + std::ostringstream ss; + ss << "DeviceError:" << module << ":" << msg_to_string(msg); + return ss.str(); + } + + static std::string msg_to_string(const char * description) + { + return description; + } + + static std::string msg_to_string(int errnum) + { + return ::strerror(errnum); + } + + static std::string msg_to_string(std::system_error & err) + { + return err.what(); + } +}; + +/** + * @brief Generic mavlink interface + */ +class MAVConnInterface +{ +private: + MAVConnInterface(const MAVConnInterface &) = delete; + +public: + using ReceivedCb = std::function; + using ClosedCb = std::function; + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using WeakPtr = std::weak_ptr; + + struct IOStat + { + size_t tx_total_bytes; //!< total bytes transferred + size_t rx_total_bytes; //!< total bytes received + float tx_speed; //!< current transfer speed [B/s] + float rx_speed; //!< current receive speed [B/s] + }; + + /** + * @param[in] system_id sysid for send_message + * @param[in] component_id compid for send_message + */ + explicit MAVConnInterface(uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE); + + /** + * @brief Establish connection, automatically called by open_url() + */ + virtual void connect( + const ReceivedCb & cb_handle_message, + const ClosedCb & cb_handle_closed_port = ClosedCb()) = 0; + + /** + * @brief Close connection. + */ + virtual void close() = 0; + + /** + * @brief Send message (mavlink_message_t) + * + * Can be used to forward messages from other connection channel. + * + * @note Does not do finalization! + * + * @throws std::length_error On exceeding Tx queue limit (MAX_TXQ_SIZE) + * @param[in] *message not changed + */ + virtual void send_message(const mavlink::mavlink_message_t * message) = 0; + + /** + * @brief Send message (child of mavlink::Message) + * + * Does serialization inside. + * System and Component ID = from this object. + * + * @throws std::length_error On exceeding Tx queue limit (MAX_TXQ_SIZE) + * @param[in] &message not changed + */ + virtual void send_message(const mavlink::Message & message) + { + send_message(message, this->comp_id); + } + + /** + * @brief Send message (child of mavlink::Message) + * + * Does serialization inside. + * System ID = from this object. + * Component ID passed by argument. + * + * @throws std::length_error On exceeding Tx queue limit (MAX_TXQ_SIZE) + * @param[in] &message not changed + * @param[in] src_compid sets the component ID of the message source + */ + virtual void send_message(const mavlink::Message & message, const uint8_t src_compid) = 0; + + /** + * @brief Send raw bytes (for some quirks) + * @throws std::length_error On exceeding Tx queue limit (MAX_TXQ_SIZE) + */ + virtual void send_bytes(const uint8_t * bytes, size_t length) = 0; + + /** + * @brief Send message and ignore possible drop due to Tx queue limit + */ + void send_message_ignore_drop(const mavlink::mavlink_message_t * message); + + /** + * @brief Send message and ignore possible drop due to Tx queue limit + * + * System and Component ID = from this object. + */ + void send_message_ignore_drop(const mavlink::Message & message) + { + send_message_ignore_drop(message, this->comp_id); + } + + /** + * @brief Send message and ignore possible drop due to Tx queue limit + * + * System ID = from this object. + * Component ID passed by argument. + */ + void send_message_ignore_drop(const mavlink::Message & message, const uint8_t src_compid); + + //! Message receive callback + ReceivedCb message_received_cb; + //! Port closed notification callback + ClosedCb port_closed_cb; + + virtual mavlink::mavlink_status_t get_status(); + virtual IOStat get_iostat(); + virtual bool is_open() = 0; + + inline uint8_t get_system_id() + { + return sys_id; + } + inline void set_system_id(uint8_t sysid) + { + sys_id = sysid; + } + inline uint8_t get_component_id() + { + return comp_id; + } + inline void set_component_id(uint8_t compid) + { + comp_id = compid; + } + + /** + * Set protocol used for encoding mavlink::Mavlink messages. + */ + void set_protocol_version(Protocol pver); + Protocol get_protocol_version(); + + /** + * @brief Construct connection from URL + * + * Supported URL schemas: + * - serial:// + * - udp:// + * - tcp:// + * - tcp-l:// + * + * Please see user's documentation for details. + * + * @param[in] url resource locator + * @param[in] system_id optional system id + * @param[in] component_id optional component id + * @return @a Ptr to constructed interface class, + * or throw @a DeviceError if error occured. + */ + static Ptr open_url( + std::string url, + uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, + const ReceivedCb & cb_handle_message = ReceivedCb(), + const ClosedCb & cb_handle_closed_port = ClosedCb() + ); + + /** + * @brief version of open_url() which do not perform connect() + */ + static Ptr open_url_no_connect( + std::string url, + uint8_t system_id = 1, + uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE); + + static std::vector get_known_dialects(); + +protected: + uint8_t sys_id; //!< Connection System Id + uint8_t comp_id; //!< Connection Component Id + + //! Maximum mavlink packet size + some extra bytes for padding. + static constexpr size_t MAX_PACKET_SIZE = MAVLINK_MAX_PACKET_LEN + 16; + //! Maximum count of transmission buffers. + static constexpr size_t MAX_TXQ_SIZE = 1000; + + //! This map merge all dialect mavlink_msg_entry_t structs. Needed for packet parser. + static std::unordered_map message_entries; + + //! Channel number used for logging. + size_t conn_id; + + inline mavlink::mavlink_status_t * get_status_p() + { + return &m_parse_status; + } + + inline mavlink::mavlink_message_t * get_buffer_p() + { + return &m_buffer; + } + + /** + * Parse buffer and emit massage_received. + */ + void parse_buffer(const char * pfx, uint8_t * buf, const size_t bufsize, size_t bytes_received); + + void iostat_tx_add(size_t bytes); + void iostat_rx_add(size_t bytes); + + void log_recv(const char * pfx, mavlink::mavlink_message_t & msg, Framing framing); + void log_send(const char * pfx, const mavlink::mavlink_message_t * msg); + void log_send_obj(const char * pfx, const mavlink::Message & msg); + +private: + friend const mavlink::mavlink_msg_entry_t * mavlink::mavlink_get_msg_entry(uint32_t msgid); + + mavlink::mavlink_status_t m_parse_status; + mavlink::mavlink_message_t m_buffer; + mavlink::mavlink_status_t m_mavlink_status; + + std::atomic tx_total_bytes, rx_total_bytes; + std::recursive_mutex iostat_mutex; + size_t last_tx_total_bytes, last_rx_total_bytes; + std::chrono::time_point last_iostat; + + //! monotonic counter (increment only) + static std::atomic conn_id_counter; + + //! init_msg_entry() once flag + static std::once_flag init_flag; + + /** + * Initialize message_entries map + * + * autogenerated. placed in mavlink_helpers.cpp + */ + static void init_msg_entry(); +}; + +} // namespace mavconn + +#endif // MAVCONN__INTERFACE_HPP_ diff --git a/libmavconn/include/mavconn/mavlink_dialect.h.em b/libmavconn/include/mavconn/mavlink_dialect.h.em deleted file mode 100644 index 4ba9ebb57..000000000 --- a/libmavconn/include/mavconn/mavlink_dialect.h.em +++ /dev/null @@ -1,30 +0,0 @@ -/** - * @@brief MAVConn mavlink.h selector - * @@file mavlink_dialect.h - * @@author Vladimir Ermakov - * - * @@addtogroup mavconn - * @@{ - */ -/* - * libmavconn - * Copyright 2014,2015,2016 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -@# -@# EmPy template of dialect include file -@# - -#pragma once - -// AUTOMATIC GENERATED FILE! -// from include/mavconn/mavlink_dialect.h.em - -#define MAVLINK_START_SIGN_STREAM(link_id) -#define MAVLINK_END_SIGN_STREAM(link_id) - -@[for dialect in MAVLINK_V20_DIALECTS]#include -@[end for] diff --git a/libmavconn/include/mavconn/mavlink_dialect.hpp.em b/libmavconn/include/mavconn/mavlink_dialect.hpp.em new file mode 100644 index 000000000..6520f0fd3 --- /dev/null +++ b/libmavconn/include/mavconn/mavlink_dialect.hpp.em @@ -0,0 +1,52 @@ +// +// libmavconn +// Copyright 2014,2015,2016,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// +/** + * @@brief MAVConn mavlink.h selector + * @@file mavlink_dialect.hpp + * @@author Vladimir Ermakov + * + * @@addtogroup mavconn + * @@{ + */ +@# +@# EmPy template of dialect include file +@# + +#pragma once +#ifndef MAVCONN__MAVLINK_DIALECT_HPP_ +#define MAVCONN__MAVLINK_DIALECT_HPP_ + +// AUTOMATIC GENERATED FILE! +// from include/mavconn/mavlink_dialect.hpp.em + +namespace mavlink +{ +#ifndef MAVLINK_VERSION +#include +constexpr auto version = MAVLINK_VERSION; +#undef MAVLINK_VERSION +#else +constexpr auto version = "unknown"; +#endif +} // namespace mavlink + +#define MAVLINK_START_SIGN_STREAM(link_id) +#define MAVLINK_END_SIGN_STREAM(link_id) + +// NOTE(vooon): ignore warning +// warning: ISO C++ prohibits anonymous structs +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" + +@[for dialect in MAVLINK_V20_DIALECTS]#include +@[end for] + +#pragma GCC diagnostic pop + +#endif // MAVCONN__MAVLINK_DIALECT_HPP_ diff --git a/libmavconn/include/mavconn/msgbuffer.h b/libmavconn/include/mavconn/msgbuffer.h deleted file mode 100644 index 539f20341..000000000 --- a/libmavconn/include/mavconn/msgbuffer.h +++ /dev/null @@ -1,94 +0,0 @@ -/** - * @brief MAVConn message buffer class (internal) - * @file msgbuffer.h - * @author Vladimir Ermakov - * - * @addtogroup mavconn - * @{ - */ -/* - * libmavconn - * Copyright 2014,2015,2016 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include - -namespace mavconn { -/** - * @brief Message buffer for internal use in libmavconn - */ -struct MsgBuffer { - //! Maximum buffer size with padding for CRC bytes (280 + padding) - static constexpr ssize_t MAX_SIZE = MAVLINK_MAX_PACKET_LEN + 16; - uint8_t data[MAX_SIZE]; - ssize_t len; - ssize_t pos; - - MsgBuffer() : - len(0), - pos(0) - { } - - /** - * @brief Buffer constructor from mavlink_message_t - */ - explicit MsgBuffer(const mavlink::mavlink_message_t *msg) : - pos(0) - { - len = mavlink::mavlink_msg_to_send_buffer(data, msg); - // paranoic check, it must be less than MAVLINK_MAX_PACKET_LEN - assert(len < MAX_SIZE); - } - - /** - * @brief Buffer constructor for mavlink::Message derived object. - */ - MsgBuffer(const mavlink::Message &obj, mavlink::mavlink_status_t *status, uint8_t sysid, uint8_t compid) : - pos(0) - { - mavlink::mavlink_message_t msg; - mavlink::MsgMap map(msg); - - auto mi = obj.get_message_info(); - - obj.serialize(map); - mavlink::mavlink_finalize_message_buffer(&msg, sysid, compid, status, mi.min_length, mi.length, mi.crc_extra); - - len = mavlink::mavlink_msg_to_send_buffer(data, &msg); - // paranoic check, it must be less than MAVLINK_MAX_PACKET_LEN - assert(len < MAX_SIZE); - } - - /** - * @brief Buffer constructor for send_bytes() - * @param[in] nbytes should be less than MAX_SIZE - */ - MsgBuffer(const uint8_t *bytes, ssize_t nbytes) : - len(nbytes), - pos(0) - { - assert(0 < nbytes && nbytes < MAX_SIZE); - memcpy(data, bytes, nbytes); - } - - virtual ~MsgBuffer() { - pos = 0; - len = 0; - } - - uint8_t *dpos() { - return data + pos; - } - - ssize_t nbytes() { - return len - pos; - } -}; -} // namespace mavconn diff --git a/libmavconn/include/mavconn/msgbuffer.hpp b/libmavconn/include/mavconn/msgbuffer.hpp new file mode 100644 index 000000000..4f5143bd0 --- /dev/null +++ b/libmavconn/include/mavconn/msgbuffer.hpp @@ -0,0 +1,111 @@ +// +// libmavconn +// Copyright 2014,2015,2016,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// +/** + * @brief MAVConn message buffer class (internal) + * @file msgbuffer.h + * @author Vladimir Ermakov + * + * @addtogroup mavconn + * @{ + */ + +#pragma once +#ifndef MAVCONN__MSGBUFFER_HPP_ +#define MAVCONN__MSGBUFFER_HPP_ + +#include +#include + +namespace mavconn +{ + +/** + * @brief Message buffer for internal use in libmavconn + */ +struct MsgBuffer +{ + //! Maximum buffer size with padding for CRC bytes (280 + padding) + static constexpr ssize_t MAX_SIZE = MAVLINK_MAX_PACKET_LEN + 16; + uint8_t data[MAX_SIZE]; + ssize_t len; + ssize_t pos; + + MsgBuffer() + : data{}, + len(0), + pos(0) + { + } + + /** + * @brief Buffer constructor from mavlink_message_t + */ + explicit MsgBuffer(const mavlink::mavlink_message_t * msg) + : pos(0) + { + len = mavlink::mavlink_msg_to_send_buffer(data, msg); + // paranoic check, it must be less than MAVLINK_MAX_PACKET_LEN + assert(len < MAX_SIZE); + } + + /** + * @brief Buffer constructor for mavlink::Message derived object. + */ + MsgBuffer( + const mavlink::Message & obj, mavlink::mavlink_status_t * status, uint8_t sysid, + uint8_t compid) + : pos(0) + { + mavlink::mavlink_message_t msg; + mavlink::MsgMap map(msg); + + auto mi = obj.get_message_info(); + + obj.serialize(map); + mavlink::mavlink_finalize_message_buffer( + &msg, sysid, compid, status, mi.min_length, mi.length, + mi.crc_extra); + + len = mavlink::mavlink_msg_to_send_buffer(data, &msg); + // paranoic check, it must be less than MAVLINK_MAX_PACKET_LEN + assert(len < MAX_SIZE); + } + + /** + * @brief Buffer constructor for send_bytes() + * @param[in] nbytes should be less than MAX_SIZE + */ + MsgBuffer(const uint8_t * bytes, ssize_t nbytes) + : len(nbytes), + pos(0) + { + assert(0 < nbytes && nbytes < MAX_SIZE); + std::memcpy(data, bytes, nbytes); + } + + virtual ~MsgBuffer() + { + pos = 0; + len = 0; + } + + uint8_t * dpos() + { + return data + pos; + } + + ssize_t nbytes() + { + return len - pos; + } +}; + +} // namespace mavconn + +#endif // MAVCONN__MSGBUFFER_HPP_ diff --git a/libmavconn/include/mavconn/serial.h b/libmavconn/include/mavconn/serial.h deleted file mode 100644 index 5e848a82b..000000000 --- a/libmavconn/include/mavconn/serial.h +++ /dev/null @@ -1,71 +0,0 @@ -/** - * @brief MAVConn Serial link class - * @file serial.h - * @author Vladimir Ermakov - * - * @addtogroup mavconn - * @{ - */ -/* - * libmavconn - * Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include -#include -#include - -namespace mavconn { -/** - * @brief Serial interface - */ -class MAVConnSerial : public MAVConnInterface, - public std::enable_shared_from_this { -public: - static constexpr auto DEFAULT_DEVICE = "/dev/ttyACM0"; - static constexpr auto DEFAULT_BAUDRATE = 57600; - - /** - * Open and run serial link. - * - * @param[in] device TTY device path - * @param[in] baudrate serial baudrate - */ - MAVConnSerial(uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, - std::string device = DEFAULT_DEVICE, unsigned baudrate = DEFAULT_BAUDRATE, bool hwflow = false); - virtual ~MAVConnSerial(); - - void connect( - const ReceivedCb &cb_handle_message, - const ClosedCb &cb_handle_closed_port = ClosedCb()) override; - void close() override; - - void send_message(const mavlink::mavlink_message_t *message) override; - void send_message(const mavlink::Message &message, const uint8_t source_compid) override; - void send_bytes(const uint8_t *bytes, size_t length) override; - - inline bool is_open() override { - return serial_dev.is_open(); - } - -private: - boost::asio::io_service io_service; - std::thread io_thread; - boost::asio::serial_port serial_dev; - - std::atomic tx_in_progress; - std::deque tx_q; - std::array rx_buf; - std::recursive_mutex mutex; - - void do_read(); - void do_write(bool check_tx_state); -}; -} // namespace mavconn diff --git a/libmavconn/include/mavconn/serial.hpp b/libmavconn/include/mavconn/serial.hpp new file mode 100644 index 000000000..cf2ac1669 --- /dev/null +++ b/libmavconn/include/mavconn/serial.hpp @@ -0,0 +1,84 @@ +// +// libmavconn +// Copyright 2013,2014,2015,2016,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// +/** + * @brief MAVConn Serial link class + * @file serial.h + * @author Vladimir Ermakov + * + * @addtogroup mavconn + * @{ + */ + +#pragma once +#ifndef MAVCONN__SERIAL_HPP_ +#define MAVCONN__SERIAL_HPP_ + +#include +#include + +#include +#include +#include +#include + +namespace mavconn +{ + +/** + * @brief Serial interface + */ +class MAVConnSerial : public MAVConnInterface, + public std::enable_shared_from_this +{ +public: + static constexpr auto DEFAULT_DEVICE = "/dev/ttyACM0"; + static constexpr auto DEFAULT_BAUDRATE = 57600; + + /** + * Open and run serial link. + * + * @param[in] device TTY device path + * @param[in] baudrate serial baudrate + */ + MAVConnSerial( + uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, + std::string device = DEFAULT_DEVICE, unsigned baudrate = DEFAULT_BAUDRATE, bool hwflow = false); + virtual ~MAVConnSerial(); + + void connect( + const ReceivedCb & cb_handle_message, + const ClosedCb & cb_handle_closed_port = ClosedCb()) override; + void close() override; + + void send_message(const mavlink::mavlink_message_t * message) override; + void send_message(const mavlink::Message & message, const uint8_t source_compid) override; + void send_bytes(const uint8_t * bytes, size_t length) override; + + inline bool is_open() override + { + return serial_dev.is_open(); + } + +private: + asio::io_service io_service; + std::thread io_thread; + asio::serial_port serial_dev; + + std::atomic tx_in_progress; + std::deque tx_q; + std::array rx_buf; + std::recursive_mutex mutex; + + void do_read(); + void do_write(bool check_tx_state); +}; + +} // namespace mavconn + +#endif // MAVCONN__SERIAL_HPP_ diff --git a/libmavconn/include/mavconn/tcp.h b/libmavconn/include/mavconn/tcp.h deleted file mode 100644 index 5653cce75..000000000 --- a/libmavconn/include/mavconn/tcp.h +++ /dev/null @@ -1,144 +0,0 @@ -/** - * @brief MAVConn TCP link classes - * @file mavconn_tcp.h - * @author Vladimir Ermakov - * - * @addtogroup mavconn - * @{ - */ -/* - * libmavconn - * Copyright 2014,2015,2016 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - - -namespace mavconn { -/** - * @brief TCP client interface - * - * @note IPv4 only - */ -class MAVConnTCPClient : public MAVConnInterface, - public std::enable_shared_from_this { -public: - static constexpr auto DEFAULT_SERVER_HOST = "localhost"; - static constexpr auto DEFAULT_SERVER_PORT = 5760; - - /** - * Create generic TCP client (connect to the server) - * @param[id] server_addr remote host - * @param[id] server_port remote port - */ - MAVConnTCPClient(uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, - std::string server_host = DEFAULT_SERVER_HOST, unsigned short server_port = DEFAULT_SERVER_PORT); - /** - * Special client variation for use in MAVConnTCPServer - */ - explicit MAVConnTCPClient(uint8_t system_id, uint8_t component_id, - boost::asio::io_service &server_io); - virtual ~MAVConnTCPClient(); - - void connect( - const ReceivedCb &cb_handle_message, - const ClosedCb &cb_handle_closed_port = ClosedCb()) override; - void close() override; - - void send_message(const mavlink::mavlink_message_t *message) override; - void send_message(const mavlink::Message &message, const uint8_t source_compid) override; - void send_bytes(const uint8_t *bytes, size_t length) override; - - inline bool is_open() override { - return socket.is_open(); - } - -private: - friend class MAVConnTCPServer; - boost::asio::io_service io_service; - std::unique_ptr io_work; - std::thread io_thread; - - boost::asio::ip::tcp::socket socket; - boost::asio::ip::tcp::endpoint server_ep; - - std::atomic is_destroying; - - std::atomic tx_in_progress; - std::deque tx_q; - std::array rx_buf; - std::recursive_mutex mutex; - - /** - * This special function called by TCP server when connection accepted. - */ - void client_connected(size_t server_channel); - - void do_recv(); - void do_send(bool check_tx_state); -}; - -/** - * @brief TCP server interface - * - * @note IPv4 only - */ -class MAVConnTCPServer : public MAVConnInterface, - public std::enable_shared_from_this { -public: - static constexpr auto DEFAULT_BIND_HOST = "localhost"; - static constexpr auto DEFAULT_BIND_PORT = 5760; - - /** - * @param[id] server_addr bind host - * @param[id] server_port bind port - */ - MAVConnTCPServer(uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, - std::string bind_host = DEFAULT_BIND_HOST, unsigned short bind_port = DEFAULT_BIND_PORT); - virtual ~MAVConnTCPServer(); - - void connect( - const ReceivedCb &cb_handle_message, - const ClosedCb &cb_handle_closed_port = ClosedCb()) override; - void close() override; - - void send_message(const mavlink::mavlink_message_t *message) override; - void send_message(const mavlink::Message &message, const uint8_t source_compid) override; - void send_bytes(const uint8_t *bytes, size_t length) override; - - mavlink::mavlink_status_t get_status() override; - IOStat get_iostat() override; - inline bool is_open() override { - return acceptor.is_open(); - } - -private: - boost::asio::io_service io_service; - std::unique_ptr io_work; - std::thread io_thread; - - boost::asio::ip::tcp::acceptor acceptor; - boost::asio::ip::tcp::endpoint bind_ep; - - std::atomic is_destroying; - - std::list > client_list; - std::recursive_mutex mutex; - - void do_accept(); - - // client slots - void client_closed(std::weak_ptr weak_instp); -}; -} // namespace mavconn diff --git a/libmavconn/include/mavconn/tcp.hpp b/libmavconn/include/mavconn/tcp.hpp new file mode 100644 index 000000000..698874fbe --- /dev/null +++ b/libmavconn/include/mavconn/tcp.hpp @@ -0,0 +1,170 @@ +// +// libmavconn +// Copyright 2014,2015,2016,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// +/** + * @brief MAVConn TCP link classes + * @file mavconn_tcp.h + * @author Vladimir Ermakov + * + * @addtogroup mavconn + * @{ + */ + +#pragma once +#ifndef MAVCONN__TCP_HPP_ +#define MAVCONN__TCP_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace mavconn +{ + +/** + * @brief TCP client interface + * + * @note IPv4 only + */ +class MAVConnTCPClient : public MAVConnInterface, + public std::enable_shared_from_this +{ +public: + static constexpr auto DEFAULT_SERVER_HOST = "localhost"; + static constexpr auto DEFAULT_SERVER_PORT = 5760; + + /** + * Create generic TCP client (connect to the server) + * @param[id] server_addr remote host + * @param[id] server_port remote port + */ + MAVConnTCPClient( + uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, + std::string server_host = DEFAULT_SERVER_HOST, + uint16_t server_port = DEFAULT_SERVER_PORT); + + /** + * Special client variation for use in MAVConnTCPServer + */ + explicit MAVConnTCPClient( + uint8_t system_id, uint8_t component_id, + asio::io_service & server_io); + + virtual ~MAVConnTCPClient(); + + void connect( + const ReceivedCb & cb_handle_message, + const ClosedCb & cb_handle_closed_port = ClosedCb()) override; + void close() override; + + void send_message(const mavlink::mavlink_message_t * message) override; + void send_message(const mavlink::Message & message, const uint8_t source_compid) override; + void send_bytes(const uint8_t * bytes, size_t length) override; + + inline bool is_open() override + { + return socket.is_open(); + } + +private: + friend class MAVConnTCPServer; + asio::io_service io_service; + std::unique_ptr io_work; + std::thread io_thread; + std::atomic is_running; //!< io_thread running + + asio::ip::tcp::socket socket; + asio::ip::tcp::endpoint server_ep; + + std::atomic is_destroying; + + std::atomic tx_in_progress; + std::deque tx_q; + std::array rx_buf; + std::recursive_mutex mutex; + + /** + * This special function called by TCP server when connection accepted. + */ + void client_connected(size_t server_channel); + + void do_recv(); + void do_send(bool check_tx_state); + + /** + * Stop io_service. + */ + void stop(); +}; + +/** + * @brief TCP server interface + * + * @note IPv4 only + */ +class MAVConnTCPServer : public MAVConnInterface, + public std::enable_shared_from_this +{ +public: + static constexpr auto DEFAULT_BIND_HOST = "localhost"; + static constexpr auto DEFAULT_BIND_PORT = 5760; + + /** + * @param[id] server_addr bind host + * @param[id] server_port bind port + */ + MAVConnTCPServer( + uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, + std::string bind_host = DEFAULT_BIND_HOST, uint16_t bind_port = DEFAULT_BIND_PORT); + virtual ~MAVConnTCPServer(); + + void connect( + const ReceivedCb & cb_handle_message, + const ClosedCb & cb_handle_closed_port = ClosedCb()) override; + void close() override; + + void send_message(const mavlink::mavlink_message_t * message) override; + void send_message(const mavlink::Message & message, const uint8_t source_compid) override; + void send_bytes(const uint8_t * bytes, size_t length) override; + + mavlink::mavlink_status_t get_status() override; + IOStat get_iostat() override; + inline bool is_open() override + { + return acceptor.is_open(); + } + +private: + asio::io_service io_service; + std::unique_ptr io_work; + std::thread io_thread; + + asio::ip::tcp::acceptor acceptor; + asio::ip::tcp::endpoint bind_ep; + + std::atomic is_destroying; + + std::list> client_list; + std::recursive_mutex mutex; + + void do_accept(); + + // client slots + void client_closed(std::weak_ptr weak_instp); +}; + +} // namespace mavconn + +#endif // MAVCONN__TCP_HPP_ diff --git a/libmavconn/include/mavconn/thread_utils.h b/libmavconn/include/mavconn/thread_utils.h deleted file mode 100644 index d388a1dcc..000000000 --- a/libmavconn/include/mavconn/thread_utils.h +++ /dev/null @@ -1,83 +0,0 @@ -/** - * @brief some useful utils - * @file thread_utils.h - * @author Vladimir Ermakov - * - * @addtogroup mavutils - * @{ - * @brief Some useful utils - */ -/* - * libmavconn - * Copyright 2014,2015,2016 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace mavconn { -namespace utils { - -/** - * @brief Make printf-formatted std::string - * - */ -template -std::string format(const std::string &fmt, Args ... args) -{ - // C++11 specify that string store elements continously - std::string ret; - - auto sz = std::snprintf(nullptr, 0, fmt.c_str(), args...); - ret.reserve(sz + 1); ret.resize(sz); // to be sure there have room for \0 - std::snprintf(&ret.front(), ret.capacity() + 1, fmt.c_str(), args...); - return ret; -} - -/** - * @brief Set name to current thread, printf-like - * @param[in] name name for thread - * @return true if success - * - * @note Only for Linux target - * @todo add for other posix system - */ -template -bool set_this_thread_name(const std::string &name, Args&& ... args) -{ - auto new_name = format(name, std::forward(args)...); - -#ifdef __APPLE__ - return pthread_setname_np(new_name.c_str()) == 0; -#else - pthread_t pth = pthread_self(); - return pthread_setname_np(pth, new_name.c_str()) == 0; -#endif -} - -/** - * @brief Convert to string objects with operator << - */ -template -inline const std::string to_string_ss(T &obj) -{ - std::ostringstream ss; - ss << obj; - return ss.str(); -} - -constexpr size_t operator"" _KiB (unsigned long long sz) -{ - return sz * 1024; -} -} // namespace utils -} // namespace mavconn diff --git a/libmavconn/include/mavconn/thread_utils.hpp b/libmavconn/include/mavconn/thread_utils.hpp new file mode 100644 index 000000000..584a15d74 --- /dev/null +++ b/libmavconn/include/mavconn/thread_utils.hpp @@ -0,0 +1,93 @@ +// +// libmavconn +// Copyright 2014,2015,2016,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// +/** + * @brief some useful utils + * @file thread_utils.h + * @author Vladimir Ermakov + * + * @addtogroup mavutils + * @{ + * @brief Some useful utils + */ + +#pragma once +#ifndef MAVCONN__THREAD_UTILS_HPP_ +#define MAVCONN__THREAD_UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include + +namespace mavconn +{ +namespace utils +{ + +/** + * @brief Make printf-formatted std::string + * + */ +template +std::string format(const std::string & fmt, Args... args) +{ + // C++11 specify that string store elements continously + std::string ret; + + auto sz = std::snprintf(nullptr, 0, fmt.c_str(), args ...); + ret.reserve(sz + 1); + ret.resize(sz); // to be sure there have room for \0 + std::snprintf(&ret.front(), ret.capacity() + 1, fmt.c_str(), args ...); + return ret; +} + +/** + * @brief Set name to current thread, printf-like + * @param[in] name name for thread + * @return true if success + * + * @note Only for Linux target + * @todo add for other posix system + */ +template +bool set_this_thread_name(const std::string & name, Args && ... args) +{ + auto new_name = format(name, std::forward(args)...); + +#ifdef __APPLE__ + return pthread_setname_np(new_name.c_str()) == 0; +#else + pthread_t pth = pthread_self(); + return pthread_setname_np(pth, new_name.c_str()) == 0; +#endif +} + +/** + * @brief Convert to string objects with operator << + */ +template +inline const std::string to_string_ss(T & obj) +{ + std::ostringstream ss; + ss << obj; + return ss.str(); +} + +constexpr size_t operator"" _KiB(unsigned long long sz) // NOLINT +{ + return sz * 1024; +} + +} // namespace utils +} // namespace mavconn + +#endif // MAVCONN__THREAD_UTILS_HPP_ diff --git a/libmavconn/include/mavconn/udp.h b/libmavconn/include/mavconn/udp.h deleted file mode 100644 index 9ca50339e..000000000 --- a/libmavconn/include/mavconn/udp.h +++ /dev/null @@ -1,89 +0,0 @@ -/** - * @brief MAVConn UDP link class - * @file mavconn_udp.h - * @author Vladimir Ermakov - * - * @addtogroup mavconn - * @{ - */ -/* - * libmavconn - * Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include -#include -#include - -namespace mavconn { -/** - * @brief UDP interface - * - * @note IPv4 only - */ -class MAVConnUDP : public MAVConnInterface, - public std::enable_shared_from_this { -public: - static constexpr auto DEFAULT_BIND_HOST = "localhost"; - static constexpr auto DEFAULT_BIND_PORT = 14555; - static constexpr auto DEFAULT_REMOTE_HOST = ""; - static constexpr auto DEFAULT_REMOTE_PORT = 14550; - //! Markers for broadcast modes. Not valid domain names. - static constexpr auto BROADCAST_REMOTE_HOST = "***i want broadcast***"; - static constexpr auto PERMANENT_BROADCAST_REMOTE_HOST = "***permanent broadcast***"; - - /** - * @param[id] bind_host bind host - * @param[id] bind_port bind port - * @param[id] remote_host remote host (optional) - * @param[id] remote_port remote port (optional) - */ - MAVConnUDP(uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, - std::string bind_host = DEFAULT_BIND_HOST, unsigned short bind_port = DEFAULT_BIND_PORT, - std::string remote_host = DEFAULT_REMOTE_HOST, unsigned short remote_port = DEFAULT_REMOTE_PORT); - virtual ~MAVConnUDP(); - - void connect( - const ReceivedCb &cb_handle_message, - const ClosedCb &cb_handle_closed_port = ClosedCb()) override; - void close() override; - - void send_message(const mavlink::mavlink_message_t *message) override; - void send_message(const mavlink::Message &message, const uint8_t source_compid) override; - void send_bytes(const uint8_t *bytes, size_t length) override; - - inline bool is_open() override { - return socket.is_open(); - } - - std::string get_remote_endpoint() const; - -private: - boost::asio::io_service io_service; - std::unique_ptr io_work; - std::thread io_thread; - bool permanent_broadcast; - - std::atomic remote_exists; - boost::asio::ip::udp::socket socket; - boost::asio::ip::udp::endpoint remote_ep; - boost::asio::ip::udp::endpoint recv_ep; - boost::asio::ip::udp::endpoint last_remote_ep; - boost::asio::ip::udp::endpoint bind_ep; - - std::atomic tx_in_progress; - std::deque tx_q; - std::array rx_buf; - std::recursive_mutex mutex; - - void do_recvfrom(); - void do_sendto(bool check_tx_state); -}; -} // namespace mavconn diff --git a/libmavconn/include/mavconn/udp.hpp b/libmavconn/include/mavconn/udp.hpp new file mode 100644 index 000000000..e0e23ce53 --- /dev/null +++ b/libmavconn/include/mavconn/udp.hpp @@ -0,0 +1,111 @@ +// +// libmavconn +// Copyright 2013,2014,2015,2016,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// +/** + * @brief MAVConn UDP link class + * @file mavconn_udp.h + * @author Vladimir Ermakov + * + * @addtogroup mavconn + * @{ + */ + +#pragma once +#ifndef MAVCONN__UDP_HPP_ +#define MAVCONN__UDP_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +namespace mavconn +{ + +/** + * @brief UDP interface + * + * @note IPv4 only + */ +class MAVConnUDP : public MAVConnInterface, + public std::enable_shared_from_this +{ +public: + static constexpr auto DEFAULT_BIND_HOST = "localhost"; + static constexpr auto DEFAULT_BIND_PORT = 14555; + static constexpr auto DEFAULT_REMOTE_HOST = ""; + static constexpr auto DEFAULT_REMOTE_PORT = 14550; + //! Markers for broadcast modes. Not valid domain names. + static constexpr auto BROADCAST_REMOTE_HOST = "***i want broadcast***"; + static constexpr auto PERMANENT_BROADCAST_REMOTE_HOST = "***permanent broadcast***"; + + /** + * @param[id] bind_host bind host + * @param[id] bind_port bind port + * @param[id] remote_host remote host (optional) + * @param[id] remote_port remote port (optional) + */ + MAVConnUDP( + uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE, + std::string bind_host = DEFAULT_BIND_HOST, uint16_t bind_port = DEFAULT_BIND_PORT, + std::string remote_host = DEFAULT_REMOTE_HOST, + uint16_t remote_port = DEFAULT_REMOTE_PORT); + + virtual ~MAVConnUDP(); + + void connect( + const ReceivedCb & cb_handle_message, + const ClosedCb & cb_handle_closed_port = ClosedCb()) override; + void close() override; + + void send_message(const mavlink::mavlink_message_t * message) override; + void send_message(const mavlink::Message & message, const uint8_t source_compid) override; + void send_bytes(const uint8_t * bytes, size_t length) override; + + inline bool is_open() override + { + return socket.is_open(); + } + + std::string get_remote_endpoint() const; + +private: + asio::io_service io_service; + std::unique_ptr io_work; + std::thread io_thread; + std::atomic is_running; //!< io_thread running + bool permanent_broadcast; + + std::atomic remote_exists; + asio::ip::udp::socket socket; + asio::ip::udp::endpoint remote_ep; + asio::ip::udp::endpoint recv_ep; + asio::ip::udp::endpoint last_remote_ep; + asio::ip::udp::endpoint bind_ep; + + std::atomic tx_in_progress; + std::deque tx_q; + std::array rx_buf; + std::recursive_mutex mutex; + + void do_recvfrom(); + void do_sendto(bool check_tx_state); + + /** + * Stop io_service. + */ + void stop(); +}; + +} // namespace mavconn + +#endif // MAVCONN__UDP_HPP_ diff --git a/libmavconn/libmavconn-extras.cmake b/libmavconn/libmavconn-extras.cmake new file mode 100644 index 000000000..c1a1dd69e --- /dev/null +++ b/libmavconn/libmavconn-extras.cmake @@ -0,0 +1,10 @@ +# +# libmavconn +# Copyright 2013-2016,2018,2021 Vladimir Ermakov, All rights reserved. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +# + +list(INSERT CMAKE_MODULE_PATH 0 "${libmavconn_DIR}/Modules") diff --git a/libmavconn/package.xml b/libmavconn/package.xml index fdc15cdf1..4d9c8d4b7 100644 --- a/libmavconn/package.xml +++ b/libmavconn/package.xml @@ -1,7 +1,8 @@ - + + libmavconn - 1.16.0 + 2.5.0 MAVLink communication library. This library provide unified connection handling classes @@ -11,7 +12,7 @@ Vladimir Ermakov - Vladimir Ermakov + GPLv3 LGPLv3 BSD @@ -20,16 +21,21 @@ https://github.com/mavlink/mavros.git https://github.com/mavlink/mavros/issues - catkin + Vladimir Ermakov + + ament_cmake - boost + asio mavlink libconsole-bridge-dev + python3-empy - rosunit - gtest + ament_cmake_gtest + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/libmavconn/src/interface.cpp b/libmavconn/src/interface.cpp index 78751e166..6d784303d 100644 --- a/libmavconn/src/interface.cpp +++ b/libmavconn/src/interface.cpp @@ -1,3 +1,11 @@ +// +// libmavconn +// Copyright 2013,2014,2015,2016,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// /** * @brief MAVConn class interface * @file interface.cpp @@ -6,425 +14,445 @@ * @addtogroup mavconn * @{ */ -/* - * libmavconn - * Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include +#include +#include +#include +#include +#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -namespace mavconn { -#define PFX "mavconn: " +namespace mavconn +{ +#define PFX "mavconn: " using mavlink::mavlink_message_t; using mavlink::mavlink_status_t; // static members std::once_flag MAVConnInterface::init_flag; -std::unordered_map MAVConnInterface::message_entries {}; +std::unordered_map +MAVConnInterface::message_entries {}; std::atomic MAVConnInterface::conn_id_counter {0}; - -MAVConnInterface::MAVConnInterface(uint8_t system_id, uint8_t component_id) : - sys_id(system_id), - comp_id(component_id), - m_parse_status {}, - m_buffer {}, - m_mavlink_status {}, - tx_total_bytes(0), - rx_total_bytes(0), - last_tx_total_bytes(0), - last_rx_total_bytes(0), - last_iostat(steady_clock::now()) +MAVConnInterface::MAVConnInterface(uint8_t system_id, uint8_t component_id) +: sys_id(system_id), + comp_id(component_id), + m_parse_status{}, + m_buffer{}, + m_mavlink_status{}, + tx_total_bytes(0), + rx_total_bytes(0), + last_tx_total_bytes(0), + last_rx_total_bytes(0), + last_iostat(steady_clock::now()) { - conn_id = conn_id_counter.fetch_add(1); - std::call_once(init_flag, init_msg_entry); + conn_id = conn_id_counter.fetch_add(1); + std::call_once(init_flag, init_msg_entry); } mavlink_status_t MAVConnInterface::get_status() { - return m_mavlink_status; + return m_mavlink_status; } MAVConnInterface::IOStat MAVConnInterface::get_iostat() { - std::lock_guard lock(iostat_mutex); - IOStat stat; + std::lock_guard lock(iostat_mutex); + IOStat stat; - stat.tx_total_bytes = tx_total_bytes; - stat.rx_total_bytes = rx_total_bytes; + stat.tx_total_bytes = tx_total_bytes; + stat.rx_total_bytes = rx_total_bytes; - auto d_tx = stat.tx_total_bytes - last_tx_total_bytes; - auto d_rx = stat.rx_total_bytes - last_rx_total_bytes; - last_tx_total_bytes = stat.tx_total_bytes; - last_rx_total_bytes = stat.rx_total_bytes; + auto d_tx = stat.tx_total_bytes - last_tx_total_bytes; + auto d_rx = stat.rx_total_bytes - last_rx_total_bytes; + last_tx_total_bytes = stat.tx_total_bytes; + last_rx_total_bytes = stat.rx_total_bytes; - auto now = steady_clock::now(); - auto dt = now - last_iostat; - last_iostat = now; + auto now = steady_clock::now(); + auto dt = now - last_iostat; + last_iostat = now; - float dt_s = std::chrono::duration_cast(dt).count(); + float dt_s = std::chrono::duration_cast(dt).count(); - stat.tx_speed = d_tx / dt_s; - stat.rx_speed = d_rx / dt_s; + stat.tx_speed = d_tx / dt_s; + stat.rx_speed = d_rx / dt_s; - return stat; + return stat; } void MAVConnInterface::iostat_tx_add(size_t bytes) { - tx_total_bytes += bytes; + tx_total_bytes += bytes; } void MAVConnInterface::iostat_rx_add(size_t bytes) { - rx_total_bytes += bytes; + rx_total_bytes += bytes; } -void MAVConnInterface::parse_buffer(const char *pfx, uint8_t *buf, const size_t bufsize, size_t bytes_received) +void MAVConnInterface::parse_buffer( + const char * pfx, uint8_t * buf, const size_t bufsize, + size_t bytes_received) { - mavlink::mavlink_message_t message; + mavlink::mavlink_message_t message; - assert(bufsize >= bytes_received); + assert(bufsize >= bytes_received); - iostat_rx_add(bytes_received); - for (; bytes_received > 0; bytes_received--) { - auto c = *buf++; + iostat_rx_add(bytes_received); + for (; bytes_received > 0; bytes_received--) { + auto c = *buf++; - auto msg_received = static_cast(mavlink::mavlink_frame_char_buffer(&m_buffer, &m_parse_status, c, &message, &m_mavlink_status)); + auto msg_received = static_cast(mavlink::mavlink_frame_char_buffer( + &m_buffer, &m_parse_status, c, + &message, &m_mavlink_status)); - if (msg_received != Framing::incomplete) { - log_recv(pfx, message, msg_received); + if (msg_received != Framing::incomplete) { + log_recv(pfx, message, msg_received); - if (message_received_cb) - message_received_cb(&message, msg_received); - } - } + if (message_received_cb) { + message_received_cb(&message, msg_received); + } + } + } } -void MAVConnInterface::log_recv(const char *pfx, mavlink_message_t &msg, Framing framing) +void MAVConnInterface::log_recv(const char * pfx, mavlink_message_t & msg, Framing framing) { - const char *framing_str = (framing == Framing::ok) ? "OK" : - (framing == Framing::bad_crc) ? "!CRC" : - (framing == Framing::bad_signature) ? "!SIG" : "ERR"; - - const char *proto_version_str = (msg.magic == MAVLINK_STX) ? "v2.0" : "v1.0"; - - CONSOLE_BRIDGE_logDebug("%s%zu: recv: %s %4s Message-Id: %u [%u bytes] IDs: %u.%u Seq: %u", - pfx, conn_id, - proto_version_str, - framing_str, - msg.msgid, msg.len, msg.sysid, msg.compid, msg.seq); + const char * framing_str = + (framing == Framing::ok) ? "OK" : (framing == Framing::bad_crc) ? "!CRC" : + (framing == Framing::bad_signature) ? "!SIG" : + "ERR"; + + const char * proto_version_str = (msg.magic == MAVLINK_STX) ? "v2.0" : "v1.0"; + + CONSOLE_BRIDGE_logDebug( + "%s%zu: recv: %s %4s Message-Id: %u [%u bytes] IDs: %u.%u Seq: %u", + pfx, conn_id, + proto_version_str, + framing_str, + msg.msgid, msg.len, msg.sysid, msg.compid, msg.seq); } -void MAVConnInterface::log_send(const char *pfx, const mavlink_message_t *msg) +void MAVConnInterface::log_send(const char * pfx, const mavlink_message_t * msg) { - const char *proto_version_str = (msg->magic == MAVLINK_STX) ? "v2.0" : "v1.0"; + const char * proto_version_str = (msg->magic == MAVLINK_STX) ? "v2.0" : "v1.0"; - CONSOLE_BRIDGE_logDebug("%s%zu: send: %s Message-Id: %u [%u bytes] IDs: %u.%u Seq: %u", - pfx, conn_id, - proto_version_str, - msg->msgid, msg->len, msg->sysid, msg->compid, msg->seq); + CONSOLE_BRIDGE_logDebug( + "%s%zu: send: %s Message-Id: %u [%u bytes] IDs: %u.%u Seq: %u", + pfx, conn_id, + proto_version_str, + msg->msgid, msg->len, msg->sysid, msg->compid, msg->seq); } -void MAVConnInterface::log_send_obj(const char *pfx, const mavlink::Message &msg) +void MAVConnInterface::log_send_obj(const char * pfx, const mavlink::Message & msg) { - CONSOLE_BRIDGE_logDebug("%s%zu: send: %s", pfx, conn_id, msg.to_yaml().c_str()); + CONSOLE_BRIDGE_logDebug("%s%zu: send: %s", pfx, conn_id, msg.to_yaml().c_str()); } -void MAVConnInterface::send_message_ignore_drop(const mavlink::mavlink_message_t *msg) +void MAVConnInterface::send_message_ignore_drop(const mavlink::mavlink_message_t * msg) { - try { - send_message(msg); - } - catch (std::length_error &e) { - CONSOLE_BRIDGE_logError(PFX "%zu: DROPPED Message-Id %u [%u bytes] IDs: %u.%u Seq: %u: %s", - conn_id, - msg->msgid, msg->len, msg->sysid, msg->compid, msg->seq, - e.what()); - } + try { + send_message(msg); + } catch (std::length_error & e) { + CONSOLE_BRIDGE_logError( + PFX "%zu: DROPPED Message-Id %u [%u bytes] IDs: %u.%u Seq: %u: %s", + conn_id, + msg->msgid, msg->len, msg->sysid, msg->compid, msg->seq, + e.what()); + } } -void MAVConnInterface::send_message_ignore_drop(const mavlink::Message &msg, uint8_t source_compid) +void MAVConnInterface::send_message_ignore_drop(const mavlink::Message & msg, uint8_t source_compid) { - try { - send_message(msg, source_compid); - } - catch (std::length_error &e) { - CONSOLE_BRIDGE_logError(PFX "%zu: DROPPED Message %s: %s", - conn_id, - msg.get_name().c_str(), - e.what()); - } + try { + send_message(msg, source_compid); + } catch (std::length_error & e) { + CONSOLE_BRIDGE_logError( + PFX "%zu: DROPPED Message %s: %s", + conn_id, + msg.get_name().c_str(), + e.what()); + } } void MAVConnInterface::set_protocol_version(Protocol pver) { - if (pver == Protocol::V10) { - m_mavlink_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - m_parse_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - - } - else { - m_mavlink_status.flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); - m_parse_status.flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); - } + if (pver == Protocol::V10) { + m_mavlink_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + m_parse_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } else { + m_mavlink_status.flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); + m_parse_status.flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); + } } Protocol MAVConnInterface::get_protocol_version() { - if (m_mavlink_status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) - return Protocol::V10; - else - return Protocol::V20; + if (m_mavlink_status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + return Protocol::V10; + } else { + return Protocol::V20; + } } /** * Parse host:port pairs */ -static void url_parse_host(std::string host, - std::string &host_out, int &port_out, - const std::string def_host, const int def_port) +static void url_parse_host( + const std::string & host, + std::string & host_out, int & port_out, + const std::string & def_host, const int def_port) { - std::string port; - - auto sep_it = std::find(host.begin(), host.end(), ':'); - if (sep_it == host.end()) { - // host - if (!host.empty()) { - host_out = host; - port_out = def_port; - } - else { - host_out = def_host; - port_out = def_port; - } - return; - } - - if (sep_it == host.begin()) { - // :port - host_out = def_host; - } - else { - // host:port - host_out.assign(host.begin(), sep_it); - } - - port.assign(sep_it + 1, host.end()); - port_out = std::stoi(port); + std::string port; + + auto sep_it = std::find(host.begin(), host.end(), ':'); + if (sep_it == host.end()) { + // host + if (!host.empty()) { + host_out = host; + port_out = def_port; + } else { + host_out = def_host; + port_out = def_port; + } + return; + } + + if (sep_it == host.begin()) { + // :port + host_out = def_host; + } else { + // host:port + host_out.assign(host.begin(), sep_it); + } + + port.assign(sep_it + 1, host.end()); + port_out = std::stoi(port); } /** * Parse ?ids=sid,cid */ -static void url_parse_query(std::string query, uint8_t &sysid, uint8_t &compid) +static void url_parse_query(const std::string & query, uint8_t & sysid, uint8_t & compid) { - const std::string ids_end("ids="); - std::string sys, comp; - - if (query.empty()) - return; - - auto ids_it = std::search(query.begin(), query.end(), - ids_end.begin(), ids_end.end()); - if (ids_it == query.end()) { - CONSOLE_BRIDGE_logWarn(PFX "URL: unknown query arguments"); - return; - } - - std::advance(ids_it, ids_end.length()); - auto comma_it = std::find(ids_it, query.end(), ','); - if (comma_it == query.end()) { - CONSOLE_BRIDGE_logError(PFX "URL: no comma in ids= query"); - return; - } - - sys.assign(ids_it, comma_it); - comp.assign(comma_it + 1, query.end()); - - sysid = std::stoi(sys); - compid = std::stoi(comp); - - CONSOLE_BRIDGE_logDebug(PFX "URL: found system/component id = [%u, %u]", sysid, compid); + const std::string ids_end("ids="); + std::string sys, comp; + + if (query.empty()) { + return; + } + + auto ids_it = std::search( + query.begin(), query.end(), + ids_end.begin(), ids_end.end()); + if (ids_it == query.end()) { + CONSOLE_BRIDGE_logWarn(PFX "URL: unknown query arguments"); + return; + } + + std::advance(ids_it, ids_end.length()); + auto comma_it = std::find(ids_it, query.end(), ','); + if (comma_it == query.end()) { + CONSOLE_BRIDGE_logError(PFX "URL: no comma in ids= query"); + return; + } + + sys.assign(ids_it, comma_it); + comp.assign(comma_it + 1, query.end()); + + sysid = std::stoi(sys); + compid = std::stoi(comp); + + CONSOLE_BRIDGE_logDebug(PFX "URL: found system/component id = [%u, %u]", sysid, compid); } static MAVConnInterface::Ptr url_parse_serial( - std::string path, std::string query, - uint8_t system_id, uint8_t component_id, bool hwflow) + const std::string & path, const std::string & query, + uint8_t system_id, uint8_t component_id, bool hwflow) { - std::string file_path; - int baudrate; - - // /dev/ttyACM0:57600 - url_parse_host(path, file_path, baudrate, MAVConnSerial::DEFAULT_DEVICE, MAVConnSerial::DEFAULT_BAUDRATE); - url_parse_query(query, system_id, component_id); - - return std::make_shared(system_id, component_id, - file_path, baudrate, hwflow); + std::string file_path; + int baudrate; + + // /dev/ttyACM0:57600 + url_parse_host( + path, file_path, baudrate, MAVConnSerial::DEFAULT_DEVICE, + MAVConnSerial::DEFAULT_BAUDRATE); + url_parse_query(query, system_id, component_id); + + return std::make_shared( + system_id, component_id, + file_path, baudrate, hwflow); } static MAVConnInterface::Ptr url_parse_udp( - std::string hosts, std::string query, - uint8_t system_id, uint8_t component_id, bool is_udpb, bool permanent_broadcast) + const std::string & hosts, const std::string & query, + uint8_t system_id, uint8_t component_id, bool is_udpb, bool permanent_broadcast) { - std::string bind_pair, remote_pair; - std::string bind_host, remote_host; - int bind_port, remote_port; - - auto sep_it = std::find(hosts.begin(), hosts.end(), '@'); - if (sep_it == hosts.end()) { - CONSOLE_BRIDGE_logError(PFX "UDP URL should contain @!"); - throw DeviceError("url", "UDP separator not found"); - } - - bind_pair.assign(hosts.begin(), sep_it); - remote_pair.assign(sep_it + 1, hosts.end()); - - // udp://0.0.0.0:14555@:14550 - url_parse_host(bind_pair, bind_host, bind_port, "0.0.0.0", MAVConnUDP::DEFAULT_BIND_PORT); - url_parse_host(remote_pair, remote_host, remote_port, MAVConnUDP::DEFAULT_REMOTE_HOST, MAVConnUDP::DEFAULT_REMOTE_PORT); - url_parse_query(query, system_id, component_id); - - if (is_udpb) - remote_host = permanent_broadcast ? MAVConnUDP::PERMANENT_BROADCAST_REMOTE_HOST : MAVConnUDP::BROADCAST_REMOTE_HOST; - - return std::make_shared(system_id, component_id, - bind_host, bind_port, - remote_host, remote_port); + std::string bind_pair, remote_pair; + std::string bind_host, remote_host; + int bind_port, remote_port; + + auto sep_it = std::find(hosts.begin(), hosts.end(), '@'); + if (sep_it == hosts.end()) { + CONSOLE_BRIDGE_logError(PFX "UDP URL should contain @!"); + throw DeviceError("url", "UDP separator not found"); + } + + bind_pair.assign(hosts.begin(), sep_it); + remote_pair.assign(sep_it + 1, hosts.end()); + + // udp://0.0.0.0:14555@:14550 + url_parse_host(bind_pair, bind_host, bind_port, "0.0.0.0", MAVConnUDP::DEFAULT_BIND_PORT); + url_parse_host( + remote_pair, remote_host, remote_port, MAVConnUDP::DEFAULT_REMOTE_HOST, + MAVConnUDP::DEFAULT_REMOTE_PORT); + url_parse_query(query, system_id, component_id); + + if (is_udpb) { + remote_host = + permanent_broadcast ? MAVConnUDP::PERMANENT_BROADCAST_REMOTE_HOST : MAVConnUDP:: + BROADCAST_REMOTE_HOST; + } + + return std::make_shared( + system_id, component_id, + bind_host, bind_port, + remote_host, remote_port); } static MAVConnInterface::Ptr url_parse_tcp_client( - std::string host, std::string query, - uint8_t system_id, uint8_t component_id) + const std::string & host, const std::string & query, + uint8_t system_id, uint8_t component_id) { - std::string server_host; - int server_port; + std::string server_host; + int server_port; - // tcp://localhost:5760 - url_parse_host(host, server_host, server_port, "localhost", 5760); - url_parse_query(query, system_id, component_id); + // tcp://localhost:5760 + url_parse_host(host, server_host, server_port, "localhost", 5760); + url_parse_query(query, system_id, component_id); - return std::make_shared(system_id, component_id, - server_host, server_port); + return std::make_shared( + system_id, component_id, + server_host, server_port); } static MAVConnInterface::Ptr url_parse_tcp_server( - std::string host, std::string query, - uint8_t system_id, uint8_t component_id) + const std::string & host, const std::string & query, + uint8_t system_id, uint8_t component_id) { - std::string bind_host; - int bind_port; + std::string bind_host; + int bind_port; - // tcp-l://0.0.0.0:5760 - url_parse_host(host, bind_host, bind_port, "0.0.0.0", 5760); - url_parse_query(query, system_id, component_id); + // tcp-l://0.0.0.0:5760 + url_parse_host(host, bind_host, bind_port, "0.0.0.0", 5760); + url_parse_query(query, system_id, component_id); - return std::make_shared(system_id, component_id, - bind_host, bind_port); + return std::make_shared( + system_id, component_id, + bind_host, bind_port); } MAVConnInterface::Ptr MAVConnInterface::open_url_no_connect( - std::string url, - uint8_t system_id, - uint8_t component_id) + std::string url, + uint8_t system_id, + uint8_t component_id) { - /* Based on code found here: - * http://stackoverflow.com/questions/2616011/easy-way-to-parse-a-url-in-c-cross-platform - */ - - const std::string proto_end("://"); - std::string proto; - std::string host; - std::string path; - std::string query; - - auto proto_it = std::search( - url.begin(), url.end(), - proto_end.begin(), proto_end.end()); - if (proto_it == url.end()) { - // looks like file path - CONSOLE_BRIDGE_logDebug(PFX "URL: %s: looks like file path", url.c_str()); - return url_parse_serial(url, "", system_id, component_id, false); - } - - // copy protocol - proto.reserve(std::distance(url.begin(), proto_it)); - std::transform(url.begin(), proto_it, - std::back_inserter(proto), - std::ref(tolower)); - - // copy host - std::advance(proto_it, proto_end.length()); - auto path_it = std::find(proto_it, url.end(), '/'); - std::transform(proto_it, path_it, - std::back_inserter(host), - std::ref(tolower)); - - // copy path, and query if exists - auto query_it = std::find(path_it, url.end(), '?'); - path.assign(path_it, query_it); - if (query_it != url.end()) - ++query_it; - query.assign(query_it, url.end()); - - CONSOLE_BRIDGE_logDebug(PFX "URL: %s: proto: %s, host: %s, path: %s, query: %s", - url.c_str(), proto.c_str(), host.c_str(), - path.c_str(), query.c_str()); - - MAVConnInterface::Ptr interface_ptr; - - if (proto == "udp") - interface_ptr = url_parse_udp(host, query, system_id, component_id, false, false); - else if (proto == "udp-b") - interface_ptr = url_parse_udp(host, query, system_id, component_id, true, false); - else if (proto == "udp-pb") - interface_ptr = url_parse_udp(host, query, system_id, component_id, true, true); - else if (proto == "tcp") - interface_ptr = url_parse_tcp_client(host, query, system_id, component_id); - else if (proto == "tcp-l") - interface_ptr = url_parse_tcp_server(host, query, system_id, component_id); - else if (proto == "serial") - interface_ptr = url_parse_serial(path, query, system_id, component_id, false); - else if (proto == "serial-hwfc") - interface_ptr = url_parse_serial(path, query, system_id, component_id, true); - else - throw DeviceError("url", "Unknown URL type"); - - return interface_ptr; + /* Based on code found here: + * http://stackoverflow.com/questions/2616011/easy-way-to-parse-a-url-in-c-cross-platform + */ + + const std::string proto_end("://"); + std::string proto; + std::string host; + std::string path; + std::string query; + + auto proto_it = std::search( + url.begin(), url.end(), + proto_end.begin(), proto_end.end()); + if (proto_it == url.end()) { + // looks like file path + CONSOLE_BRIDGE_logDebug(PFX "URL: %s: looks like file path", url.c_str()); + return url_parse_serial(url, "", system_id, component_id, false); + } + + // copy protocol + proto.reserve(std::distance(url.begin(), proto_it)); + std::transform( + url.begin(), proto_it, + std::back_inserter(proto), + std::ref(tolower)); + + // copy host + std::advance(proto_it, proto_end.length()); + auto path_it = std::find(proto_it, url.end(), '/'); + std::transform( + proto_it, path_it, + std::back_inserter(host), + std::ref(tolower)); + + // copy path, and query if exists + auto query_it = std::find(path_it, url.end(), '?'); + path.assign(path_it, query_it); + if (query_it != url.end()) { + ++query_it; + } + query.assign(query_it, url.end()); + + CONSOLE_BRIDGE_logDebug( + PFX "URL: %s: proto: %s, host: %s, path: %s, query: %s", + url.c_str(), proto.c_str(), host.c_str(), + path.c_str(), query.c_str()); + + MAVConnInterface::Ptr interface_ptr; + + if (proto == "udp") { + interface_ptr = url_parse_udp(host, query, system_id, component_id, false, false); + } else if (proto == "udp-b") { + interface_ptr = url_parse_udp(host, query, system_id, component_id, true, false); + } else if (proto == "udp-pb") { + interface_ptr = url_parse_udp(host, query, system_id, component_id, true, true); + } else if (proto == "tcp") { + interface_ptr = url_parse_tcp_client(host, query, system_id, component_id); + } else if (proto == "tcp-l") { + interface_ptr = url_parse_tcp_server(host, query, system_id, component_id); + } else if (proto == "serial") { + interface_ptr = url_parse_serial(path, query, system_id, component_id, false); + } else if (proto == "serial-hwfc") { + interface_ptr = url_parse_serial(path, query, system_id, component_id, true); + } else { + throw DeviceError("url", "Unknown URL type"); + } + + return interface_ptr; } MAVConnInterface::Ptr MAVConnInterface::open_url( - std::string url, - uint8_t system_id, - uint8_t component_id, - const ReceivedCb &cb_handle_message, - const ClosedCb &cb_handle_closed_port) + std::string url, + uint8_t system_id, + uint8_t component_id, + const ReceivedCb & cb_handle_message, + const ClosedCb & cb_handle_closed_port) { - auto interface_ptr = open_url_no_connect(url, system_id, component_id); - if (interface_ptr) - { - if (!cb_handle_message) - { - CONSOLE_BRIDGE_logWarn(PFX "You did not provide message handling callback to open_url(), it is unsafe to set it later."); - } - interface_ptr->connect(cb_handle_message, cb_handle_closed_port); - } - - return interface_ptr; - + auto interface_ptr = open_url_no_connect(url, system_id, component_id); + if (interface_ptr) { + if (!cb_handle_message) { + CONSOLE_BRIDGE_logWarn( + PFX "You did not provide message handling callback to open_url(), " + "It is unsafe to set it later."); + } + interface_ptr->connect(cb_handle_message, cb_handle_closed_port); + } + + return interface_ptr; } -} // namespace mavconn +} // namespace mavconn diff --git a/libmavconn/src/mavlink_helpers.cpp.em b/libmavconn/src/mavlink_helpers.cpp.em index 1d66b4b6a..215f0969c 100644 --- a/libmavconn/src/mavlink_helpers.cpp.em +++ b/libmavconn/src/mavlink_helpers.cpp.em @@ -1,3 +1,11 @@ +// +// libmavconn +// Copyright 2015,2016,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// /** * @@brief MAVLink helpers * @@file mavlink_helpers.cpp @@ -8,20 +16,15 @@ * @@addtogroup mavconn * @@{ */ -/* - * libmavconn - * Copyright 2015,2016 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ @# @# EmPy template of dialect helpers source file @# -#include -#include +#include +#include + +#include "mavconn/console_bridge_compat.hpp" +#include "mavconn/interface.hpp" // AUTOMATIC GENERATED FILE! // from src/mavlink_helpers.cpp.em @@ -30,34 +33,36 @@ using mavconn::MAVConnInterface; void MAVConnInterface::init_msg_entry() { - CONSOLE_BRIDGE_logDebug("mavconn: Initialize message_entries map"); + CONSOLE_BRIDGE_logDebug("mavconn: Initialize message_entries map"); - auto load = [&](const char *dialect, const mavlink::mavlink_msg_entry_t & e) { - auto it = message_entries.find(e.msgid); - if (it != message_entries.end()) { - if (memcmp(&e, it->second, sizeof(e)) != 0) { - CONSOLE_BRIDGE_logDebug("mavconn: init: message from %s, MSG-ID %d ignored! Table has different entry.", dialect, e.msgid); - } - else { - CONSOLE_BRIDGE_logDebug("mavconn: init: message from %s, MSG-ID %d in table.", dialect, e.msgid); - } - } - else { - CONSOLE_BRIDGE_logDebug("mavconn: init: add message entry for %s, MSG-ID %d", dialect, e.msgid); - message_entries[e.msgid] = &e; - } - }; + auto load = [&](const char* dialect, const mavlink::mavlink_msg_entry_t& e) { + auto it = message_entries.find(e.msgid); + if (it != message_entries.end()) { + if (memcmp(&e, it->second, sizeof(e)) != 0) { + CONSOLE_BRIDGE_logDebug("mavconn: init: message from %s, " + "MSG-ID %d ignored! Table has different entry.", dialect, e.msgid); + } else { + CONSOLE_BRIDGE_logDebug("mavconn: init: message from %s, " + "MSG-ID %d in table.", dialect, e.msgid); + } + } else { + CONSOLE_BRIDGE_logDebug("mavconn: init: add message entry for %s, " + "MSG-ID %d", dialect, e.msgid); + message_entries[e.msgid] = &e; + } + }; - @[for dialect in MAVLINK_V20_DIALECTS]for (auto &e : mavlink::@dialect::MESSAGE_ENTRIES) @(' ' * (20 - len(dialect))) load("@dialect", e); - @[end for] + @[for dialect in MAVLINK_V20_DIALECTS]for (auto &e : mavlink::@dialect::MESSAGE_ENTRIES) @(' ' * (20 - len(dialect))) load("@dialect", e); + @[end for] } std::vector MAVConnInterface::get_known_dialects() { - return { - @[for dialect in MAVLINK_V20_DIALECTS]"@dialect", - @[end for] - }; + return + { + @[for dialect in MAVLINK_V20_DIALECTS]"@dialect", + @[end for] + }; } /** @@ -65,9 +70,9 @@ std::vector MAVConnInterface::get_known_dialects() */ const mavlink::mavlink_msg_entry_t* mavlink::mavlink_get_msg_entry(uint32_t msgid) { - auto it = MAVConnInterface::message_entries.find(msgid); - if (it != MAVConnInterface::message_entries.end()) - return it->second; - else - return nullptr; + auto it = MAVConnInterface::message_entries.find(msgid); + if (it != MAVConnInterface::message_entries.end()) + return it->second; + else + return nullptr; } diff --git a/libmavconn/src/serial.cpp b/libmavconn/src/serial.cpp index 8ca57c588..351f0c7d7 100644 --- a/libmavconn/src/serial.cpp +++ b/libmavconn/src/serial.cpp @@ -1,3 +1,11 @@ +// +// libmavconn +// Copyright 2013,2014,2015,2016,2018,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// /** * @brief MAVConn Serial link class * @file serial.cpp @@ -6,267 +14,273 @@ * @addtogroup mavconn * @{ */ -/* - * libmavconn - * Copyright 2013,2014,2015,2016,2018 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include - -#include -#include -#include #if defined(__linux__) #include #endif -namespace mavconn { - -using boost::system::error_code; -using boost::asio::io_service; -using boost::asio::buffer; -using mavlink::mavlink_message_t; - +#include +#include +#include -#define PFX "mavconn: serial" -#define PFXd PFX "%zu: " +#include +#include +namespace mavconn +{ -MAVConnSerial::MAVConnSerial(uint8_t system_id, uint8_t component_id, - std::string device, unsigned baudrate, bool hwflow) : - MAVConnInterface(system_id, component_id), - io_service(), - serial_dev(io_service), - tx_in_progress(false), - tx_q {}, - rx_buf {} +using asio::buffer; +using asio::io_service; +using mavlink::mavlink_message_t; +using std::error_code; + +#define PFX "mavconn: serial" +#define PFXd PFX "%zu: " + +MAVConnSerial::MAVConnSerial( + uint8_t system_id, uint8_t component_id, + std::string device, unsigned baudrate, bool hwflow) +: MAVConnInterface(system_id, component_id), + io_service(), + serial_dev(io_service), + tx_in_progress(false), + tx_q{}, + rx_buf{} { - using SPB = boost::asio::serial_port_base; - - CONSOLE_BRIDGE_logInform(PFXd "device: %s @ %d bps", conn_id, device.c_str(), baudrate); - - try { - serial_dev.open(device); - - // Set baudrate and 8N1 mode - serial_dev.set_option(SPB::baud_rate(baudrate)); - serial_dev.set_option(SPB::character_size(8)); - serial_dev.set_option(SPB::parity(SPB::parity::none)); - serial_dev.set_option(SPB::stop_bits(SPB::stop_bits::one)); - -#if BOOST_ASIO_VERSION >= 101200 || !defined(__linux__) - // Flow control setting in older versions of Boost.ASIO is broken, use workaround (below) for now. - serial_dev.set_option(SPB::flow_control( (hwflow) ? SPB::flow_control::hardware : SPB::flow_control::none)); -#elif BOOST_ASIO_VERSION < 101200 && defined(__linux__) - // Workaround to set some options for the port manually. This is done in - // Boost.ASIO, but until v1.12.0 (Boost 1.66) there was a bug which doesn't enable relevant - // code. Fixed by commit: https://github.com/boostorg/asio/commit/619cea4356 - { - int fd = serial_dev.native_handle(); - - termios tio; - tcgetattr(fd, &tio); - - // Set hardware flow control settings - if (hwflow) { - tio.c_iflag &= ~(IXOFF | IXON); - tio.c_cflag |= CRTSCTS; - } else { - tio.c_iflag &= ~(IXOFF | IXON); - tio.c_cflag &= ~CRTSCTS; - } - - // Set serial port to "raw" mode to prevent EOF exit. - cfmakeraw(&tio); - - // Commit settings - tcsetattr(fd, TCSANOW, &tio); - } + using SPB = asio::serial_port_base; + + CONSOLE_BRIDGE_logInform(PFXd "device: %s @ %d bps", conn_id, device.c_str(), baudrate); + + try { + serial_dev.open(device); + + // Set baudrate and 8N1 mode + serial_dev.set_option(SPB::baud_rate(baudrate)); + serial_dev.set_option(SPB::character_size(8)); + serial_dev.set_option(SPB::parity(SPB::parity::none)); + serial_dev.set_option(SPB::stop_bits(SPB::stop_bits::one)); + +#if ASIO_VERSION >= 101200 || !defined(__linux__) + // Flow control setting in older versions of ASIO is broken, use workaround (below) for now. + serial_dev.set_option( + SPB::flow_control( + (hwflow) ? SPB::flow_control::hardware : SPB::flow_control::none)); +#elif ASIO_VERSION < 101200 && defined(__linux__) + // Workaround to set some options for the port manually. This is done in + // ASIO, but until v1.12.0 there was a bug which doesn't enable relevant + // code. Fixed by commit: https://github.com/boostorg/asio/commit/619cea4356 + { + int fd = serial_dev.native_handle(); + + termios tio; + tcgetattr(fd, &tio); + + // Set hardware flow control settings + if (hwflow) { + tio.c_iflag &= ~(IXOFF | IXON); + tio.c_cflag |= CRTSCTS; + } else { + tio.c_iflag &= ~(IXOFF | IXON); + tio.c_cflag &= ~CRTSCTS; + } + + // Set serial port to "raw" mode to prevent EOF exit. + cfmakeraw(&tio); + + // Commit settings + tcsetattr(fd, TCSANOW, &tio); + } #endif #if defined(__linux__) - // Enable low latency mode on Linux - { - int fd = serial_dev.native_handle(); + // Enable low latency mode on Linux + { + int fd = serial_dev.native_handle(); - struct serial_struct ser_info; - ioctl(fd, TIOCGSERIAL, &ser_info); + struct serial_struct ser_info; + ioctl(fd, TIOCGSERIAL, &ser_info); - ser_info.flags |= ASYNC_LOW_LATENCY; + ser_info.flags |= ASYNC_LOW_LATENCY; - ioctl(fd, TIOCSSERIAL, &ser_info); - } + ioctl(fd, TIOCSSERIAL, &ser_info); + } #endif - } - catch (boost::system::system_error &err) { - throw DeviceError("serial", err); - } + } catch (asio::system_error & err) { + throw DeviceError("serial", err); + } } MAVConnSerial::~MAVConnSerial() { - close(); + close(); } void MAVConnSerial::connect( - const ReceivedCb &cb_handle_message, - const ClosedCb &cb_handle_closed_port) + const ReceivedCb & cb_handle_message, + const ClosedCb & cb_handle_closed_port) { - message_received_cb = cb_handle_message; - port_closed_cb = cb_handle_closed_port; - - // give some work to io_service before start - io_service.post(std::bind(&MAVConnSerial::do_read, this)); - - // run io_service for async io - io_thread = std::thread([this] () { - utils::set_this_thread_name("mserial%zu", conn_id); - io_service.run(); - }); + message_received_cb = cb_handle_message; + port_closed_cb = cb_handle_closed_port; + + // give some work to io_service before start + io_service.post(std::bind(&MAVConnSerial::do_read, this)); + + // run io_service for async io + io_thread = std::thread( + [this]() { + utils::set_this_thread_name("mserial%zu", conn_id); + io_service.run(); + }); } + void MAVConnSerial::close() { - lock_guard lock(mutex); - if (!is_open()) - return; + lock_guard lock(mutex); + if (!is_open()) { + return; + } - serial_dev.cancel(); - serial_dev.close(); + serial_dev.cancel(); + serial_dev.close(); - io_service.stop(); + io_service.stop(); - if (io_thread.joinable()) - io_thread.join(); + if (io_thread.joinable()) { + io_thread.join(); + } - io_service.reset(); + io_service.reset(); - if (port_closed_cb) - port_closed_cb(); + if (port_closed_cb) { + port_closed_cb(); + } } -void MAVConnSerial::send_bytes(const uint8_t *bytes, size_t length) +void MAVConnSerial::send_bytes(const uint8_t * bytes, size_t length) { - if (!is_open()) { - CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); - return; - } + if (!is_open()) { + CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); + return; + } - { - lock_guard lock(mutex); + { + lock_guard lock(mutex); - if (tx_q.size() >= MAX_TXQ_SIZE) - throw std::length_error("MAVConnSerial::send_bytes: TX queue overflow"); + if (tx_q.size() >= MAX_TXQ_SIZE) { + throw std::length_error("MAVConnSerial::send_bytes: TX queue overflow"); + } - tx_q.emplace_back(bytes, length); - } - io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true)); + tx_q.emplace_back(bytes, length); + } + io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true)); } -void MAVConnSerial::send_message(const mavlink_message_t *message) +void MAVConnSerial::send_message(const mavlink_message_t * message) { - assert(message != nullptr); + assert(message != nullptr); - if (!is_open()) { - CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); - return; - } + if (!is_open()) { + CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); + return; + } - log_send(PFX, message); + log_send(PFX, message); - { - lock_guard lock(mutex); + { + lock_guard lock(mutex); - if (tx_q.size() >= MAX_TXQ_SIZE) - throw std::length_error("MAVConnSerial::send_message: TX queue overflow"); + if (tx_q.size() >= MAX_TXQ_SIZE) { + throw std::length_error("MAVConnSerial::send_message: TX queue overflow"); + } - tx_q.emplace_back(message); - } - io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true)); + tx_q.emplace_back(message); + } + io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true)); } -void MAVConnSerial::send_message(const mavlink::Message &message, const uint8_t source_compid) +void MAVConnSerial::send_message(const mavlink::Message & message, const uint8_t source_compid) { - if (!is_open()) { - CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); - return; - } + if (!is_open()) { + CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); + return; + } - log_send_obj(PFX, message); + log_send_obj(PFX, message); - { - lock_guard lock(mutex); + { + lock_guard lock(mutex); - if (tx_q.size() >= MAX_TXQ_SIZE) - throw std::length_error("MAVConnSerial::send_message: TX queue overflow"); + if (tx_q.size() >= MAX_TXQ_SIZE) { + throw std::length_error("MAVConnSerial::send_message: TX queue overflow"); + } - tx_q.emplace_back(message, get_status_p(), sys_id, source_compid); - } - io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true)); + tx_q.emplace_back(message, get_status_p(), sys_id, source_compid); + } + io_service.post(std::bind(&MAVConnSerial::do_write, shared_from_this(), true)); } void MAVConnSerial::do_read(void) { - auto sthis = shared_from_this(); - serial_dev.async_read_some( - buffer(rx_buf), - [sthis] (error_code error, size_t bytes_transferred) { - if (error) { - CONSOLE_BRIDGE_logError(PFXd "receive: %s", sthis->conn_id, error.message().c_str()); - sthis->close(); - return; - } - - sthis->parse_buffer(PFX, sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred); - sthis->do_read(); - }); + auto sthis = shared_from_this(); + serial_dev.async_read_some( + buffer(rx_buf), + [sthis](error_code error, size_t bytes_transferred) { + if (error) { + CONSOLE_BRIDGE_logError(PFXd "receive: %s", sthis->conn_id, error.message().c_str()); + sthis->close(); + return; + } + + sthis->parse_buffer(PFX, sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred); + sthis->do_read(); + }); } void MAVConnSerial::do_write(bool check_tx_state) { - if (check_tx_state && tx_in_progress) - return; - - lock_guard lock(mutex); - if (tx_q.empty()) - return; - - tx_in_progress = true; - auto sthis = shared_from_this(); - auto &buf_ref = tx_q.front(); - serial_dev.async_write_some( - buffer(buf_ref.dpos(), buf_ref.nbytes()), - [sthis, &buf_ref] (error_code error, size_t bytes_transferred) { - assert(bytes_transferred <= buf_ref.len); - - if (error) { - CONSOLE_BRIDGE_logError(PFXd "write: %s", sthis->conn_id, error.message().c_str()); - sthis->close(); - return; - } - - sthis->iostat_tx_add(bytes_transferred); - lock_guard lock(sthis->mutex); - - if (sthis->tx_q.empty()) { - sthis->tx_in_progress = false; - return; - } - - buf_ref.pos += bytes_transferred; - if (buf_ref.nbytes() == 0) { - sthis->tx_q.pop_front(); - } - - if (!sthis->tx_q.empty()) - sthis->do_write(false); - else - sthis->tx_in_progress = false; - }); + if (check_tx_state && tx_in_progress) { + return; + } + + lock_guard lock(mutex); + if (tx_q.empty()) { + return; + } + + tx_in_progress = true; + auto sthis = shared_from_this(); + auto & buf_ref = tx_q.front(); + serial_dev.async_write_some( + buffer(buf_ref.dpos(), buf_ref.nbytes()), + [sthis, &buf_ref](error_code error, size_t bytes_transferred) { + assert(ssize_t(bytes_transferred) <= buf_ref.len); + + if (error) { + CONSOLE_BRIDGE_logError(PFXd "write: %s", sthis->conn_id, error.message().c_str()); + sthis->close(); + return; + } + + sthis->iostat_tx_add(bytes_transferred); + lock_guard lock(sthis->mutex); + + if (sthis->tx_q.empty()) { + sthis->tx_in_progress = false; + return; + } + + buf_ref.pos += bytes_transferred; + if (buf_ref.nbytes() == 0) { + sthis->tx_q.pop_front(); + } + + if (!sthis->tx_q.empty()) { + sthis->do_write(false); + } else { + sthis->tx_in_progress = false; + } + }); } -} // namespace mavconn + +} // namespace mavconn diff --git a/libmavconn/src/tcp.cpp b/libmavconn/src/tcp.cpp index f7b4d0b20..60d633456 100644 --- a/libmavconn/src/tcp.cpp +++ b/libmavconn/src/tcp.cpp @@ -1,3 +1,11 @@ +// +// libmavconn +// Copyright 2014,2015,2016,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// /** * @brief MAVConn TCP link classes * @file tcp.cpp @@ -6,471 +14,518 @@ * @addtogroup mavconn * @{ */ -/* - * libmavconn - * Copyright 2014,2015,2016 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include +#include +#include -#include -#include -#include +#include +#include +#include +#include -// Ensure the correct io_service() is called based on boost version -#if BOOST_VERSION >= 107000 -#define GET_IO_SERVICE(s) ((boost::asio::io_context&)(s).get_executor().context()) +// Ensure the correct io_service() is called based on asio version +#if ASIO_VERSION >= 101400 +#define GET_IO_SERVICE(s) ((asio::io_context &)(s).get_executor().context()) #else #define GET_IO_SERVICE(s) ((s).get_io_service()) #endif -namespace mavconn { +namespace mavconn +{ -using boost::system::error_code; -using boost::asio::io_service; -using boost::asio::ip::tcp; -using boost::asio::buffer; -using utils::to_string_ss; +using asio::buffer; +using asio::error_code; +using asio::io_service; +using asio::ip::tcp; using mavlink::mavlink_message_t; using mavlink::mavlink_status_t; +using utils::to_string_ss; -#define PFX "mavconn: tcp" -#define PFXd PFX "%zu: " - +#define PFX "mavconn: tcp" +#define PFXd PFX "%zu: " -static bool resolve_address_tcp(io_service &io, size_t chan, std::string host, unsigned short port, tcp::endpoint &ep) +static bool resolve_address_tcp( + io_service & io, size_t chan, std::string host, uint16_t port, + tcp::endpoint & ep) { - bool result = false; - tcp::resolver resolver(io); - error_code ec; - - tcp::resolver::query query(host, ""); - - auto fn = [&](const tcp::endpoint & q_ep) { - ep = q_ep; - ep.port(port); - result = true; - CONSOLE_BRIDGE_logDebug(PFXd "host %s resolved as %s", chan, host.c_str(), to_string_ss(ep).c_str()); - }; - -#if BOOST_ASIO_VERSION >= 101200 - for (auto q_ep : resolver.resolve(query, ec)) fn(q_ep); + bool result = false; + tcp::resolver resolver(io); + error_code ec; + + tcp::resolver::query query(host, ""); + + auto fn = [&](const tcp::endpoint & q_ep) { + ep = q_ep; + ep.port(port); + result = true; + CONSOLE_BRIDGE_logDebug( + PFXd "host %s resolved as %s", chan, host.c_str(), to_string_ss(ep).c_str()); + }; + +#if ASIO_VERSION >= 101200 + for (auto q_ep : resolver.resolve(query, ec)) { + fn(q_ep); + } #else - std::for_each(resolver.resolve(query, ec), tcp::resolver::iterator(), fn); + std::for_each(resolver.resolve(query, ec), tcp::resolver::iterator(), fn); #endif - if (ec) { - CONSOLE_BRIDGE_logWarn(PFXd "resolve error: %s", chan, ec.message().c_str()); - result = false; - } + if (ec) { + CONSOLE_BRIDGE_logWarn(PFXd "resolve error: %s", chan, ec.message().c_str()); + result = false; + } - return result; + return result; } - /* -*- TCP client variant -*- */ -MAVConnTCPClient::MAVConnTCPClient(uint8_t system_id, uint8_t component_id, - std::string server_host, unsigned short server_port) : - MAVConnInterface(system_id, component_id), - io_service(), - io_work(new io_service::work(io_service)), - socket(io_service), - is_destroying(false), - tx_in_progress(false), - tx_q {}, - rx_buf {} +MAVConnTCPClient::MAVConnTCPClient( + uint8_t system_id, uint8_t component_id, + std::string server_host, uint16_t server_port) +: MAVConnInterface(system_id, component_id), + io_service(), + io_work(new io_service::work(io_service)), + is_running(false), + socket(io_service), + is_destroying(false), + tx_in_progress(false), + tx_q{}, + rx_buf{} { - if (!resolve_address_tcp(io_service, conn_id, server_host, server_port, server_ep)) - throw DeviceError("tcp: resolve", "Bind address resolve failed"); - - CONSOLE_BRIDGE_logInform(PFXd "Server address: %s", conn_id, to_string_ss(server_ep).c_str()); - - try { - socket.open(tcp::v4()); - socket.connect(server_ep); - } - catch (boost::system::system_error &err) { - throw DeviceError("tcp", err); - } + if (!resolve_address_tcp(io_service, conn_id, server_host, server_port, server_ep)) { + throw DeviceError("tcp: resolve", "Bind address resolve failed"); + } + + CONSOLE_BRIDGE_logInform(PFXd "Server address: %s", conn_id, to_string_ss(server_ep).c_str()); + + try { + socket.open(tcp::v4()); + socket.connect(server_ep); + } catch (asio::system_error & err) { + throw DeviceError("tcp", err); + } } -MAVConnTCPClient::MAVConnTCPClient(uint8_t system_id, uint8_t component_id, - boost::asio::io_service &server_io) : - MAVConnInterface(system_id, component_id), - socket(server_io), - is_destroying(false), - tx_in_progress(false), - tx_q {}, - rx_buf {} +MAVConnTCPClient::MAVConnTCPClient( + uint8_t system_id, uint8_t component_id, + asio::io_service & server_io) +: MAVConnInterface(system_id, component_id), + is_running(false), + socket(server_io), + is_destroying(false), + tx_in_progress(false), + tx_q{}, + rx_buf{} { - // waiting when server call client_connected() + // waiting when server call client_connected() } void MAVConnTCPClient::client_connected(size_t server_channel) { - CONSOLE_BRIDGE_logInform(PFXd "Got client, id: %zu, address: %s", - server_channel, conn_id, to_string_ss(server_ep).c_str()); + CONSOLE_BRIDGE_logInform( + PFXd "Got client, id: %zu, address: %s", + server_channel, conn_id, to_string_ss(server_ep).c_str()); - // start recv - GET_IO_SERVICE(socket).post(std::bind(&MAVConnTCPClient::do_recv, shared_from_this())); + // start recv + GET_IO_SERVICE(socket).post(std::bind(&MAVConnTCPClient::do_recv, shared_from_this())); } MAVConnTCPClient::~MAVConnTCPClient() { - is_destroying = true; - close(); + is_destroying = true; + close(); + + // If the client is already disconnected on error (By the io_service thread) + // and io_service running + if (is_running) { + stop(); + } } void MAVConnTCPClient::connect( - const ReceivedCb &cb_handle_message, - const ClosedCb &cb_handle_closed_port) + const ReceivedCb & cb_handle_message, + const ClosedCb & cb_handle_closed_port) { - message_received_cb = cb_handle_message; - port_closed_cb = cb_handle_closed_port; - - // give some work to io_service before start - io_service.post(std::bind(&MAVConnTCPClient::do_recv, this)); - - // run io_service for async io - io_thread = std::thread([this] () { - utils::set_this_thread_name("mtcp%zu", conn_id); - io_service.run(); - }); + message_received_cb = cb_handle_message; + port_closed_cb = cb_handle_closed_port; + + // give some work to io_service before start + io_service.post(std::bind(&MAVConnTCPClient::do_recv, this)); + + // run io_service for async io + io_thread = std::thread( + [this]() { + is_running = true; + utils::set_this_thread_name("mtcp%zu", conn_id); + try { + io_service.run(); + } catch (std::exception & ex) { + CONSOLE_BRIDGE_logError(PFXd "io_service execption: %s", conn_id, ex.what()); + } + is_running = false; + }); } -void MAVConnTCPClient::close() +void MAVConnTCPClient::stop() { - lock_guard lock(mutex); - if (!is_open()) - return; - - boost::system::error_code ec; - socket.shutdown(boost::asio::ip::tcp::socket::shutdown_send, ec); - if (ec) - CONSOLE_BRIDGE_logError(PFXd "shutdown: %s", conn_id, ec.message().c_str()); - socket.cancel(); - socket.close(); + io_work.reset(); + io_service.stop(); - io_work.reset(); - io_service.stop(); + if (io_thread.joinable()) { + io_thread.join(); + } - if (io_thread.joinable()) - io_thread.join(); - - io_service.reset(); + io_service.reset(); +} - if (port_closed_cb) - port_closed_cb(); +void MAVConnTCPClient::close() +{ + { + lock_guard lock(mutex); + if (!is_open()) { + return; + } + + std::error_code ec; + socket.shutdown(asio::ip::tcp::socket::shutdown_send, ec); + if (ec) { + CONSOLE_BRIDGE_logError(PFXd "shutdown: %s", conn_id, ec.message().c_str()); + } + socket.cancel(); + socket.close(); + } + + // Stop io_service if the thread is not the io_thread (else exception "resource deadlock avoided") + if (std::this_thread::get_id() != io_thread.get_id()) { + stop(); + } + + if (port_closed_cb) { + port_closed_cb(); + } } -void MAVConnTCPClient::send_bytes(const uint8_t *bytes, size_t length) +void MAVConnTCPClient::send_bytes(const uint8_t * bytes, size_t length) { - if (!is_open()) { - CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); - return; - } + if (!is_open()) { + CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); + return; + } - { - lock_guard lock(mutex); + { + lock_guard lock(mutex); - if (tx_q.size() >= MAX_TXQ_SIZE) - throw std::length_error("MAVConnTCPClient::send_bytes: TX queue overflow"); + if (tx_q.size() >= MAX_TXQ_SIZE) { + throw std::length_error("MAVConnTCPClient::send_bytes: TX queue overflow"); + } - tx_q.emplace_back(bytes, length); - } - GET_IO_SERVICE(socket).post(std::bind(&MAVConnTCPClient::do_send, shared_from_this(), true)); + tx_q.emplace_back(bytes, length); + } + GET_IO_SERVICE(socket).post(std::bind(&MAVConnTCPClient::do_send, shared_from_this(), true)); } -void MAVConnTCPClient::send_message(const mavlink_message_t *message) +void MAVConnTCPClient::send_message(const mavlink_message_t * message) { - assert(message != nullptr); + assert(message != nullptr); - if (!is_open()) { - CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); - return; - } + if (!is_open()) { + CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); + return; + } - log_send(PFX, message); + log_send(PFX, message); - { - lock_guard lock(mutex); + { + lock_guard lock(mutex); - if (tx_q.size() >= MAX_TXQ_SIZE) - throw std::length_error("MAVConnTCPClient::send_message: TX queue overflow"); + if (tx_q.size() >= MAX_TXQ_SIZE) { + throw std::length_error("MAVConnTCPClient::send_message: TX queue overflow"); + } - tx_q.emplace_back(message); - } - GET_IO_SERVICE(socket).post(std::bind(&MAVConnTCPClient::do_send, shared_from_this(), true)); + tx_q.emplace_back(message); + } + GET_IO_SERVICE(socket).post(std::bind(&MAVConnTCPClient::do_send, shared_from_this(), true)); } -void MAVConnTCPClient::send_message(const mavlink::Message &message, const uint8_t source_compid) +void MAVConnTCPClient::send_message(const mavlink::Message & message, const uint8_t source_compid) { - if (!is_open()) { - CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); - return; - } + if (!is_open()) { + CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); + return; + } - log_send_obj(PFX, message); + log_send_obj(PFX, message); - { - lock_guard lock(mutex); + { + lock_guard lock(mutex); - if (tx_q.size() >= MAX_TXQ_SIZE) - throw std::length_error("MAVConnTCPClient::send_message: TX queue overflow"); + if (tx_q.size() >= MAX_TXQ_SIZE) { + throw std::length_error("MAVConnTCPClient::send_message: TX queue overflow"); + } - tx_q.emplace_back(message, get_status_p(), sys_id, source_compid); - } - GET_IO_SERVICE(socket).post(std::bind(&MAVConnTCPClient::do_send, shared_from_this(), true)); + tx_q.emplace_back(message, get_status_p(), sys_id, source_compid); + } + GET_IO_SERVICE(socket).post(std::bind(&MAVConnTCPClient::do_send, shared_from_this(), true)); } void MAVConnTCPClient::do_recv() { - if (is_destroying) { - return; - } - auto sthis = shared_from_this(); - socket.async_receive( - buffer(rx_buf), - [sthis] (error_code error, size_t bytes_transferred) { - if (error) { - CONSOLE_BRIDGE_logError(PFXd "receive: %s", sthis->conn_id, error.message().c_str()); - sthis->close(); - return; - } - - sthis->parse_buffer(PFX, sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred); - sthis->do_recv(); - }); + if (is_destroying) { + return; + } + auto sthis = shared_from_this(); + socket.async_receive( + buffer(rx_buf), + [sthis](error_code error, size_t bytes_transferred) { + if (error) { + CONSOLE_BRIDGE_logError(PFXd "receive: %s", sthis->conn_id, error.message().c_str()); + sthis->close(); + return; + } + + sthis->parse_buffer(PFX, sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred); + sthis->do_recv(); + }); } void MAVConnTCPClient::do_send(bool check_tx_state) { - if (check_tx_state && tx_in_progress) - return; - - lock_guard lock(mutex); - if (tx_q.empty()) - return; - - tx_in_progress = true; - auto sthis = shared_from_this(); - auto &buf_ref = tx_q.front(); - socket.async_send( - buffer(buf_ref.dpos(), buf_ref.nbytes()), - [sthis, &buf_ref] (error_code error, size_t bytes_transferred) { - assert(bytes_transferred <= buf_ref.len); - - if (error) { - CONSOLE_BRIDGE_logError(PFXd "send: %s", sthis->conn_id, error.message().c_str()); - sthis->close(); - return; - } - - sthis->iostat_tx_add(bytes_transferred); - lock_guard lock(sthis->mutex); - - if (sthis->tx_q.empty()) { - sthis->tx_in_progress = false; - return; - } - - buf_ref.pos += bytes_transferred; - if (buf_ref.nbytes() == 0) { - sthis->tx_q.pop_front(); - } - - if (!sthis->tx_q.empty()) - sthis->do_send(false); - else - sthis->tx_in_progress = false; - }); + if (check_tx_state && tx_in_progress) { + return; + } + + lock_guard lock(mutex); + if (tx_q.empty()) { + return; + } + + tx_in_progress = true; + auto sthis = shared_from_this(); + auto & buf_ref = tx_q.front(); + socket.async_send( + buffer(buf_ref.dpos(), buf_ref.nbytes()), + [sthis, &buf_ref](error_code error, size_t bytes_transferred) { + assert(ssize_t(bytes_transferred) <= buf_ref.len); + + if (error) { + CONSOLE_BRIDGE_logError(PFXd "send: %s", sthis->conn_id, error.message().c_str()); + sthis->close(); + return; + } + + sthis->iostat_tx_add(bytes_transferred); + lock_guard lock(sthis->mutex); + + if (sthis->tx_q.empty()) { + sthis->tx_in_progress = false; + return; + } + + buf_ref.pos += bytes_transferred; + if (buf_ref.nbytes() == 0) { + sthis->tx_q.pop_front(); + } + + if (!sthis->tx_q.empty()) { + sthis->do_send(false); + } else { + sthis->tx_in_progress = false; + } + }); } - /* -*- TCP server variant -*- */ -MAVConnTCPServer::MAVConnTCPServer(uint8_t system_id, uint8_t component_id, - std::string server_host, unsigned short server_port) : - MAVConnInterface(system_id, component_id), - io_service(), - acceptor(io_service), - is_destroying(false) +MAVConnTCPServer::MAVConnTCPServer( + uint8_t system_id, uint8_t component_id, + std::string server_host, uint16_t server_port) +: MAVConnInterface(system_id, component_id), + io_service(), + acceptor(io_service), + is_destroying(false) { - if (!resolve_address_tcp(io_service, conn_id, server_host, server_port, bind_ep)) - throw DeviceError("tcp-l: resolve", "Bind address resolve failed"); - - CONSOLE_BRIDGE_logInform(PFXd "Bind address: %s", conn_id, to_string_ss(bind_ep).c_str()); - - try { - acceptor.open(tcp::v4()); - acceptor.set_option(tcp::acceptor::reuse_address(true)); - acceptor.bind(bind_ep); - acceptor.listen(); - } - catch (boost::system::system_error &err) { - throw DeviceError("tcp-l", err); - } + if (!resolve_address_tcp(io_service, conn_id, server_host, server_port, bind_ep)) { + throw DeviceError("tcp-l: resolve", "Bind address resolve failed"); + } + + CONSOLE_BRIDGE_logInform(PFXd "Bind address: %s", conn_id, to_string_ss(bind_ep).c_str()); + + try { + acceptor.open(tcp::v4()); + acceptor.set_option(tcp::acceptor::reuse_address(true)); + acceptor.bind(bind_ep); + acceptor.listen(); + } catch (asio::system_error & err) { + throw DeviceError("tcp-l", err); + } } MAVConnTCPServer::~MAVConnTCPServer() { - is_destroying = true; - close(); + is_destroying = true; + close(); } void MAVConnTCPServer::connect( - const ReceivedCb &cb_handle_message, - const ClosedCb &cb_handle_closed_port) + const ReceivedCb & cb_handle_message, + const ClosedCb & cb_handle_closed_port) { - message_received_cb = cb_handle_message; - port_closed_cb = cb_handle_closed_port; - - // give some work to io_service before start - io_service.post(std::bind(&MAVConnTCPServer::do_accept, this)); - - // run io_service for async io - io_thread = std::thread([this] () { - utils::set_this_thread_name("mtcps%zu", conn_id); - io_service.run(); - }); + message_received_cb = cb_handle_message; + port_closed_cb = cb_handle_closed_port; + + // give some work to io_service before start + io_service.post(std::bind(&MAVConnTCPServer::do_accept, this)); + + // run io_service for async io + io_thread = std::thread( + [this]() { + utils::set_this_thread_name("mtcps%zu", conn_id); + io_service.run(); + }); } void MAVConnTCPServer::close() { - lock_guard lock(mutex); - if (!is_open()) - return; - - CONSOLE_BRIDGE_logInform(PFXd "Terminating server. " - "All connections will be closed.", conn_id); - - io_service.stop(); - acceptor.close(); - - if (io_thread.joinable()) - io_thread.join(); - - if (port_closed_cb) - port_closed_cb(); + lock_guard lock(mutex); + if (!is_open()) { + return; + } + + CONSOLE_BRIDGE_logInform( + PFXd "Terminating server. " + "All connections will be closed.", + conn_id); + + io_service.stop(); + acceptor.close(); + + if (io_thread.joinable()) { + io_thread.join(); + } + + if (port_closed_cb) { + port_closed_cb(); + } } mavlink_status_t MAVConnTCPServer::get_status() { - mavlink_status_t status {}; - - lock_guard lock(mutex); - for (auto &instp : client_list) { - auto inst_status = instp->get_status(); - - // [[[cog: - // for f in ('packet_rx_success_count', 'packet_rx_drop_count', 'buffer_overrun', 'parse_error'): - // cog.outl("status.{f:23s} += inst_status.{f};".format(**locals())) - // ]]] - status.packet_rx_success_count += inst_status.packet_rx_success_count; - status.packet_rx_drop_count += inst_status.packet_rx_drop_count; - status.buffer_overrun += inst_status.buffer_overrun; - status.parse_error += inst_status.parse_error; - // [[[end]]] (checksum: a6186246ed026f1cf2b4ffc7407e893b) - - /* seq counters always 0 for this connection type */ - } - - return status; + mavlink_status_t status {}; + + lock_guard lock(mutex); + for (auto & instp : client_list) { + auto inst_status = instp->get_status(); + + // [[[cog: + // for f in ('packet_rx_success_count', 'packet_rx_drop_count', + // 'buffer_overrun', 'parse_error'): + // cog.outl("status.{f} += inst_status.{f};".format(**locals())) + // ]]] + status.packet_rx_success_count += inst_status.packet_rx_success_count; + status.packet_rx_drop_count += inst_status.packet_rx_drop_count; + status.buffer_overrun += inst_status.buffer_overrun; + status.parse_error += inst_status.parse_error; + // [[[end]]] (checksum: cd582e46d3a563caabfefe819243b62c) + + /* seq counters always 0 for this connection type */ + } + + return status; } MAVConnInterface::IOStat MAVConnTCPServer::get_iostat() { - MAVConnInterface::IOStat iostat {}; - - lock_guard lock(mutex); - for (auto &instp : client_list) { - auto inst_iostat = instp->get_iostat(); - - // [[[cog: - // for p in ('tx', 'rx'): - // for f in ('total_bytes', 'speed'): - // cog.outl("iostat.{p}_{f:11s} += inst_iostat.{p}_{f};".format(**locals())) - // ]]] - iostat.tx_total_bytes += inst_iostat.tx_total_bytes; - iostat.tx_speed += inst_iostat.tx_speed; - iostat.rx_total_bytes += inst_iostat.rx_total_bytes; - iostat.rx_speed += inst_iostat.rx_speed; - // [[[end]]] (checksum: fb4fe06794471d9b068ce0c129ee7673) - } - - return iostat; + MAVConnInterface::IOStat iostat {}; + + lock_guard lock(mutex); + for (auto & instp : client_list) { + auto inst_iostat = instp->get_iostat(); + + // [[[cog: + // for p in ('tx', 'rx'): + // for f in ('total_bytes', 'speed'): + // cog.outl("iostat.{p}_{f} += inst_iostat.{p}_{f};".format(**locals())) + // ]]] + iostat.tx_total_bytes += inst_iostat.tx_total_bytes; + iostat.tx_speed += inst_iostat.tx_speed; + iostat.rx_total_bytes += inst_iostat.rx_total_bytes; + iostat.rx_speed += inst_iostat.rx_speed; + // [[[end]]] (checksum: b86c7c86ee2b15eb702c2e1da3ca82d8) + } + + return iostat; } -void MAVConnTCPServer::send_bytes(const uint8_t *bytes, size_t length) +void MAVConnTCPServer::send_bytes(const uint8_t * bytes, size_t length) { - lock_guard lock(mutex); - for (auto &instp : client_list) { - instp->send_bytes(bytes, length); - } + lock_guard lock(mutex); + for (auto & instp : client_list) { + instp->send_bytes(bytes, length); + } } -void MAVConnTCPServer::send_message(const mavlink_message_t *message) +void MAVConnTCPServer::send_message(const mavlink_message_t * message) { - lock_guard lock(mutex); - for (auto &instp : client_list) { - instp->send_message(message); - } + lock_guard lock(mutex); + for (auto & instp : client_list) { + instp->send_message(message); + } } -void MAVConnTCPServer::send_message(const mavlink::Message &message, const uint8_t source_compid) +void MAVConnTCPServer::send_message(const mavlink::Message & message, const uint8_t source_compid) { - lock_guard lock(mutex); - for (auto &instp : client_list) { - instp->send_message(message, source_compid); - } + lock_guard lock(mutex); + for (auto & instp : client_list) { + instp->send_message(message, source_compid); + } } void MAVConnTCPServer::do_accept() { - if (is_destroying) { - return; - } - auto sthis = shared_from_this(); - auto acceptor_client = std::make_shared(sys_id, comp_id, io_service); - acceptor.async_accept( - acceptor_client->socket, - acceptor_client->server_ep, - [sthis, acceptor_client] (error_code error) { - if (error) { - CONSOLE_BRIDGE_logError(PFXd "accept: %s", sthis->conn_id, error.message().c_str()); - sthis->close(); - return; - } - - { - lock_guard lock(sthis->mutex); - - std::weak_ptr weak_client{acceptor_client}; - acceptor_client->message_received_cb = sthis->message_received_cb; - acceptor_client->port_closed_cb = [weak_client, sthis] () { sthis->client_closed(weak_client); }; - acceptor_client->client_connected(sthis->conn_id); - - sthis->client_list.push_back(acceptor_client); - sthis->do_accept(); - } - }); + if (is_destroying) { + return; + } + auto sthis = shared_from_this(); + auto acceptor_client = std::make_shared(sys_id, comp_id, io_service); + acceptor.async_accept( + acceptor_client->socket, + acceptor_client->server_ep, + [sthis, acceptor_client](error_code error) { + if (error) { + CONSOLE_BRIDGE_logError(PFXd "accept: %s", sthis->conn_id, error.message().c_str()); + sthis->close(); + return; + } + + { + lock_guard lock(sthis->mutex); + + std::weak_ptr weak_client{acceptor_client}; + acceptor_client->message_received_cb = sthis->message_received_cb; + acceptor_client->port_closed_cb = [weak_client, sthis]() { + sthis->client_closed(weak_client); + }; + acceptor_client->client_connected(sthis->conn_id); + + sthis->client_list.push_back(acceptor_client); + sthis->do_accept(); + } + }); } void MAVConnTCPServer::client_closed(std::weak_ptr weak_instp) { - if (auto instp = weak_instp.lock()) { - CONSOLE_BRIDGE_logInform(PFXd "Client connection closed, id: %p, address: %s", - conn_id, instp.get(), to_string_ss(instp->server_ep).c_str()); - - { - lock_guard lock(mutex); - client_list.remove(instp); - } - } + if (auto instp = weak_instp.lock()) { + CONSOLE_BRIDGE_logInform( + PFXd "Client connection closed, id: %p, address: %s", + conn_id, instp.get(), to_string_ss(instp->server_ep).c_str()); + + { + lock_guard lock(mutex); + client_list.remove(instp); + } + } } -} // namespace mavconn +} // namespace mavconn diff --git a/libmavconn/src/udp.cpp b/libmavconn/src/udp.cpp index 16afe5b14..165678991 100644 --- a/libmavconn/src/udp.cpp +++ b/libmavconn/src/udp.cpp @@ -1,3 +1,11 @@ +// +// libmavconn +// Copyright 2013,2014,2015,2016,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// /** * @brief MAVConn UDP link class * @file udp.cpp @@ -6,314 +14,348 @@ * @addtogroup mavconn * @{ */ -/* - * libmavconn - * Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include +#include +#include -#include -#include -#include +#include +#include -namespace mavconn { +namespace mavconn +{ -using boost::system::error_code; -using boost::asio::io_service; -using boost::asio::ip::udp; -using boost::asio::buffer; +using asio::buffer; +using asio::error_code; +using asio::io_service; +using asio::ip::udp; using utils::to_string_ss; using utils::operator"" _KiB; using mavlink::mavlink_message_t; -#define PFX "mavconn: udp" -#define PFXd PFX "%zu: " - +#define PFX "mavconn: udp" +#define PFXd PFX "%zu: " -static bool resolve_address_udp(io_service &io, size_t chan, std::string host, unsigned short port, udp::endpoint &ep) +static bool resolve_address_udp( + io_service & io, size_t chan, std::string host, unsigned short port, + udp::endpoint & ep) { - bool result = false; - udp::resolver resolver(io); - error_code ec; - - udp::resolver::query query(host, ""); - - auto fn = [&](const udp::endpoint & q_ep) { - ep = q_ep; - ep.port(port); - result = true; - CONSOLE_BRIDGE_logDebug(PFXd "host %s resolved as %s", chan, host.c_str(), to_string_ss(ep).c_str()); - }; - -#if BOOST_ASIO_VERSION >= 101200 - for (auto q_ep : resolver.resolve(query, ec)) fn(q_ep); + bool result = false; + udp::resolver resolver(io); + error_code ec; + + udp::resolver::query query(host, ""); + + auto fn = [&](const udp::endpoint & q_ep) { + ep = q_ep; + ep.port(port); + result = true; + CONSOLE_BRIDGE_logDebug( + PFXd "host %s resolved as %s", chan, host.c_str(), to_string_ss(ep).c_str()); + }; + +#if ASIO_VERSION >= 101200 + for (auto q_ep : resolver.resolve(query, ec)) { + fn(q_ep); + } #else - std::for_each(resolver.resolve(query, ec), udp::resolver::iterator(), fn); + std::for_each(resolver.resolve(query, ec), udp::resolver::iterator(), fn); #endif - if (ec) { - CONSOLE_BRIDGE_logWarn(PFXd "resolve error: %s", chan, ec.message().c_str()); - result = false; - } + if (ec) { + CONSOLE_BRIDGE_logWarn(PFXd "resolve error: %s", chan, ec.message().c_str()); + result = false; + } - return result; + return result; } - -MAVConnUDP::MAVConnUDP(uint8_t system_id, uint8_t component_id, - std::string bind_host, unsigned short bind_port, - std::string remote_host, unsigned short remote_port) : - MAVConnInterface(system_id, component_id), - io_service(), - io_work(new io_service::work(io_service)), - permanent_broadcast(false), - remote_exists(false), - socket(io_service), - tx_in_progress(false), - tx_q {}, - rx_buf {} +MAVConnUDP::MAVConnUDP( + uint8_t system_id, uint8_t component_id, + std::string bind_host, uint16_t bind_port, + std::string remote_host, uint16_t remote_port) +: MAVConnInterface(system_id, component_id), + io_service(), + io_work(new io_service::work(io_service)), + is_running(false), + permanent_broadcast(false), + remote_exists(false), + socket(io_service), + tx_in_progress(false), + tx_q{}, + rx_buf{} { - using udps = boost::asio::ip::udp::socket; - - if (!resolve_address_udp(io_service, conn_id, bind_host, bind_port, bind_ep)) - throw DeviceError("udp: resolve", "Bind address resolve failed"); - - CONSOLE_BRIDGE_logInform(PFXd "Bind address: %s", conn_id, to_string_ss(bind_ep).c_str()); - - if (remote_host != "") { - if (remote_host != BROADCAST_REMOTE_HOST && remote_host != PERMANENT_BROADCAST_REMOTE_HOST) - remote_exists = resolve_address_udp(io_service, conn_id, remote_host, remote_port, remote_ep); - else { - remote_exists = true; - remote_ep = udp::endpoint(boost::asio::ip::address_v4::broadcast(), remote_port); - } - - if (remote_exists) - CONSOLE_BRIDGE_logInform(PFXd "Remote address: %s", conn_id, to_string_ss(remote_ep).c_str()); - else - CONSOLE_BRIDGE_logWarn(PFXd "Remote address resolve failed.", conn_id); - } - - try { - socket.open(udp::v4()); - - // set buffer opt. size from QGC - socket.set_option(udps::reuse_address(true)); - socket.set_option(udps::send_buffer_size(256_KiB)); - socket.set_option(udps::receive_buffer_size(512_KiB)); - - socket.bind(bind_ep); - - if (remote_host == BROADCAST_REMOTE_HOST) { - socket.set_option(udps::broadcast(true)); - } else if (remote_host == PERMANENT_BROADCAST_REMOTE_HOST) { - socket.set_option(udps::broadcast(true)); - permanent_broadcast = true; - } - } - catch (boost::system::system_error &err) { - throw DeviceError("udp", err); - } + using udps = asio::ip::udp::socket; + + if (!resolve_address_udp(io_service, conn_id, bind_host, bind_port, bind_ep)) { + throw DeviceError("udp: resolve", "Bind address resolve failed"); + } + + CONSOLE_BRIDGE_logInform(PFXd "Bind address: %s", conn_id, to_string_ss(bind_ep).c_str()); + + if (remote_host != "") { + if (remote_host != BROADCAST_REMOTE_HOST && remote_host != PERMANENT_BROADCAST_REMOTE_HOST) { + remote_exists = resolve_address_udp(io_service, conn_id, remote_host, remote_port, remote_ep); + } else { + remote_exists = true; + remote_ep = udp::endpoint(asio::ip::address_v4::broadcast(), remote_port); + } + + if (remote_exists) { + CONSOLE_BRIDGE_logInform(PFXd "Remote address: %s", conn_id, to_string_ss(remote_ep).c_str()); + } else { + CONSOLE_BRIDGE_logWarn(PFXd "Remote address resolve failed.", conn_id); + } + } + + try { + socket.open(udp::v4()); + + // set buffer opt. size from QGC + socket.set_option(udps::reuse_address(true)); + socket.set_option(udps::send_buffer_size(256_KiB)); + socket.set_option(udps::receive_buffer_size(512_KiB)); + + socket.bind(bind_ep); + + if (remote_host == BROADCAST_REMOTE_HOST) { + socket.set_option(udps::broadcast(true)); + } else if (remote_host == PERMANENT_BROADCAST_REMOTE_HOST) { + socket.set_option(udps::broadcast(true)); + permanent_broadcast = true; + } + } catch (asio::system_error & err) { + throw DeviceError("udp", err); + } } MAVConnUDP::~MAVConnUDP() { - close(); + close(); + + // If the socket already closed and the io_service running + if (is_running) { + stop(); + } } void MAVConnUDP::connect( - const ReceivedCb &cb_handle_message, - const ClosedCb &cb_handle_closed_port) + const ReceivedCb & cb_handle_message, + const ClosedCb & cb_handle_closed_port) { - message_received_cb = cb_handle_message; - port_closed_cb = cb_handle_closed_port; - - // give some work to io_service before start - io_service.post(std::bind(&MAVConnUDP::do_recvfrom, this)); - - // run io_service for async io - io_thread = std::thread([this] () { - utils::set_this_thread_name("mudp%zu", conn_id); - io_service.run(); - }); + message_received_cb = cb_handle_message; + port_closed_cb = cb_handle_closed_port; + + // give some work to io_service before start + io_service.post(std::bind(&MAVConnUDP::do_recvfrom, this)); + + // run io_service for async io + io_thread = std::thread( + [this]() { + is_running = true; + utils::set_this_thread_name("mudp%zu", conn_id); + try { + io_service.run(); + } catch (std::exception & ex) { + CONSOLE_BRIDGE_logError(PFXd "io_service execption: %s", conn_id, ex.what()); + } + is_running = false; + }); } -void MAVConnUDP::close() +void MAVConnUDP::stop() { - lock_guard lock(mutex); - if (!is_open()) - return; - - socket.cancel(); - socket.close(); + io_work.reset(); + io_service.stop(); - io_work.reset(); - io_service.stop(); + if (io_thread.joinable()) { + io_thread.join(); + } - if (io_thread.joinable()) - io_thread.join(); - - io_service.reset(); - - if (port_closed_cb) - port_closed_cb(); + io_service.reset(); } -void MAVConnUDP::send_bytes(const uint8_t *bytes, size_t length) +void MAVConnUDP::close() { - if (!is_open()) { - CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); - return; - } - - if (!remote_exists) { - CONSOLE_BRIDGE_logDebug(PFXd "send: Remote not known, message dropped.", conn_id); - return; - } - - { - lock_guard lock(mutex); - - if (tx_q.size() >= MAX_TXQ_SIZE) - throw std::length_error("MAVConnUDP::send_bytes: TX queue overflow"); + { + lock_guard lock(mutex); + if (!is_open()) { + return; + } + + socket.cancel(); + socket.close(); + } + + // Stop io_service if the thread is not the io_thread (else exception "resource deadlock avoided") + if (std::this_thread::get_id() != io_thread.get_id()) { + stop(); + } + + if (port_closed_cb) { + port_closed_cb(); + } +} - tx_q.emplace_back(bytes, length); - } - io_service.post(std::bind(&MAVConnUDP::do_sendto, shared_from_this(), true)); +void MAVConnUDP::send_bytes(const uint8_t * bytes, size_t length) +{ + if (!is_open()) { + CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); + return; + } + + if (!remote_exists) { + CONSOLE_BRIDGE_logDebug(PFXd "send: Remote not known, message dropped.", conn_id); + return; + } + + { + lock_guard lock(mutex); + + if (tx_q.size() >= MAX_TXQ_SIZE) { + throw std::length_error("MAVConnUDP::send_bytes: TX queue overflow"); + } + + tx_q.emplace_back(bytes, length); + } + io_service.post(std::bind(&MAVConnUDP::do_sendto, shared_from_this(), true)); } -void MAVConnUDP::send_message(const mavlink_message_t *message) +void MAVConnUDP::send_message(const mavlink_message_t * message) { - assert(message != nullptr); + assert(message != nullptr); - if (!is_open()) { - CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); - return; - } + if (!is_open()) { + CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); + return; + } - if (!remote_exists) { - CONSOLE_BRIDGE_logDebug(PFXd "send: Remote not known, message dropped.", conn_id); - return; - } + if (!remote_exists) { + CONSOLE_BRIDGE_logDebug(PFXd "send: Remote not known, message dropped.", conn_id); + return; + } - log_send(PFX, message); + log_send(PFX, message); - { - lock_guard lock(mutex); + { + lock_guard lock(mutex); - if (tx_q.size() >= MAX_TXQ_SIZE) - throw std::length_error("MAVConnUDP::send_message: TX queue overflow"); + if (tx_q.size() >= MAX_TXQ_SIZE) { + throw std::length_error("MAVConnUDP::send_message: TX queue overflow"); + } - tx_q.emplace_back(message); - } - io_service.post(std::bind(&MAVConnUDP::do_sendto, shared_from_this(), true)); + tx_q.emplace_back(message); + } + io_service.post(std::bind(&MAVConnUDP::do_sendto, shared_from_this(), true)); } -void MAVConnUDP::send_message(const mavlink::Message &message, const uint8_t source_compid) +void MAVConnUDP::send_message(const mavlink::Message & message, const uint8_t source_compid) { - if (!is_open()) { - CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); - return; - } + if (!is_open()) { + CONSOLE_BRIDGE_logError(PFXd "send: channel closed!", conn_id); + return; + } - if (!remote_exists) { - CONSOLE_BRIDGE_logDebug(PFXd "send: Remote not known, message dropped.", conn_id); - return; - } + if (!remote_exists) { + CONSOLE_BRIDGE_logDebug(PFXd "send: Remote not known, message dropped.", conn_id); + return; + } - log_send_obj(PFX, message); + log_send_obj(PFX, message); - { - lock_guard lock(mutex); + { + lock_guard lock(mutex); - if (tx_q.size() >= MAX_TXQ_SIZE) - throw std::length_error("MAVConnUDP::send_message: TX queue overflow"); + if (tx_q.size() >= MAX_TXQ_SIZE) { + throw std::length_error("MAVConnUDP::send_message: TX queue overflow"); + } - tx_q.emplace_back(message, get_status_p(), sys_id, source_compid); - } - io_service.post(std::bind(&MAVConnUDP::do_sendto, shared_from_this(), true)); + tx_q.emplace_back(message, get_status_p(), sys_id, source_compid); + } + io_service.post(std::bind(&MAVConnUDP::do_sendto, shared_from_this(), true)); } void MAVConnUDP::do_recvfrom() { - auto sthis = shared_from_this(); - socket.async_receive_from( - buffer(rx_buf), - permanent_broadcast ? recv_ep : remote_ep, - [sthis] (error_code error, size_t bytes_transferred) { - if (error) { - CONSOLE_BRIDGE_logError(PFXd "receive: %s", sthis->conn_id, error.message().c_str()); - sthis->close(); - return; - } - - if (!sthis->permanent_broadcast && sthis->remote_ep != sthis->last_remote_ep) { - CONSOLE_BRIDGE_logInform(PFXd "Remote address: %s", sthis->conn_id, to_string_ss(sthis->remote_ep).c_str()); - sthis->remote_exists = true; - sthis->last_remote_ep = sthis->remote_ep; - } - - sthis->parse_buffer(PFX, sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred); - sthis->do_recvfrom(); - }); + auto sthis = shared_from_this(); + socket.async_receive_from( + buffer(rx_buf), + permanent_broadcast ? recv_ep : remote_ep, + [sthis](error_code error, size_t bytes_transferred) { + if (error) { + CONSOLE_BRIDGE_logError(PFXd "receive: %s", sthis->conn_id, error.message().c_str()); + sthis->close(); + return; + } + + if (!sthis->permanent_broadcast && sthis->remote_ep != sthis->last_remote_ep) { + CONSOLE_BRIDGE_logInform( + PFXd "Remote address: %s", sthis->conn_id, + to_string_ss(sthis->remote_ep).c_str()); + sthis->remote_exists = true; + sthis->last_remote_ep = sthis->remote_ep; + } + + sthis->parse_buffer(PFX, sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred); + sthis->do_recvfrom(); + }); } void MAVConnUDP::do_sendto(bool check_tx_state) { - if (check_tx_state && tx_in_progress) - return; - - lock_guard lock(mutex); - if (tx_q.empty()) - return; - - tx_in_progress = true; - auto sthis = shared_from_this(); - auto &buf_ref = tx_q.front(); - socket.async_send_to( - buffer(buf_ref.dpos(), buf_ref.nbytes()), - remote_ep, - [sthis, &buf_ref] (error_code error, size_t bytes_transferred) { - assert(bytes_transferred <= buf_ref.len); - - if (error == boost::asio::error::network_unreachable) { - CONSOLE_BRIDGE_logWarn(PFXd "sendto: %s, retrying", sthis->conn_id, error.message().c_str()); - // do not return, try to resend - } - else if (error) { - CONSOLE_BRIDGE_logError(PFXd "sendto: %s", sthis->conn_id, error.message().c_str()); - sthis->close(); - return; - } - - sthis->iostat_tx_add(bytes_transferred); - lock_guard lock(sthis->mutex); - - if (sthis->tx_q.empty()) { - sthis->tx_in_progress = false; - return; - } - - buf_ref.pos += bytes_transferred; - if (buf_ref.nbytes() == 0) { - sthis->tx_q.pop_front(); - } - - if (!sthis->tx_q.empty()) - sthis->do_sendto(false); - else - sthis->tx_in_progress = false; - }); + if (check_tx_state && tx_in_progress) { + return; + } + + lock_guard lock(mutex); + if (tx_q.empty()) { + return; + } + + tx_in_progress = true; + auto sthis = shared_from_this(); + auto & buf_ref = tx_q.front(); + socket.async_send_to( + buffer(buf_ref.dpos(), buf_ref.nbytes()), + remote_ep, + [sthis, &buf_ref](error_code error, size_t bytes_transferred) { + assert(ssize_t(bytes_transferred) <= buf_ref.len); + + if (error == asio::error::network_unreachable) { + CONSOLE_BRIDGE_logWarn( + PFXd "sendto: %s, retrying", sthis->conn_id, + error.message().c_str()); + // do not return, try to resend + } else if (error) { + CONSOLE_BRIDGE_logError(PFXd "sendto: %s", sthis->conn_id, error.message().c_str()); + sthis->close(); + return; + } + + sthis->iostat_tx_add(bytes_transferred); + lock_guard lock(sthis->mutex); + + if (sthis->tx_q.empty()) { + sthis->tx_in_progress = false; + return; + } + + buf_ref.pos += bytes_transferred; + if (buf_ref.nbytes() == 0) { + sthis->tx_q.pop_front(); + } + + if (!sthis->tx_q.empty()) { + sthis->do_sendto(false); + } else { + sthis->tx_in_progress = false; + } + }); } std::string MAVConnUDP::get_remote_endpoint() const { - return to_string_ss(remote_ep); + return to_string_ss(remote_ep); } -} // namespace mavconn +} // namespace mavconn diff --git a/libmavconn/test/test_hang.cpp b/libmavconn/test/test_hang.cpp deleted file mode 100644 index 661ee4e73..000000000 --- a/libmavconn/test/test_hang.cpp +++ /dev/null @@ -1,87 +0,0 @@ -/** - * Test mavconn library - * - * This test created for issue #72. - * It is hand test, no build rules. - * - * Compile command: - * g++ -I/opt/ros/kinetic/include -I$HOME/ros/install/include -Iinclude \ - * -std=c++11 -ggdb test/test_hang.cpp -o /tmp/hang \ - * -L/opt/ros/kinetic/lib -L$HOME/ros/devel/lib -lroscpp -lrosconsole -lboost_system -lmavconn -lrt \ - * -DMAVLINK_DIALECT=ardupilotmega - */ - -#include - -#include - -using namespace mavconn; -using namespace mavlink; - -static void send_heartbeat(MAVConnInterface *ip) { -#if 0 - mavlink_message_t msg; - mavlink_msg_heartbeat_pack_chan(ip->get_system_id(), ip->get_component_id(), ip->get_channel(), &msg, - MAV_TYPE_ONBOARD_CONTROLLER, - MAV_AUTOPILOT_INVALID, - MAV_MODE_MANUAL_ARMED, - 0, - MAV_STATE_ACTIVE); - ip->send_message(&msg); -#endif - - using mavlink::common::MAV_TYPE; - using mavlink::common::MAV_AUTOPILOT; - using mavlink::common::MAV_MODE; - using mavlink::common::MAV_STATE; - - mavlink::common::msg::HEARTBEAT hb {}; - hb.type = int(MAV_TYPE::ONBOARD_CONTROLLER); - hb.autopilot = int(MAV_AUTOPILOT::INVALID); - hb.base_mode = int(MAV_MODE::MANUAL_ARMED); - hb.custom_mode = 0; - hb.system_status = int(MAV_STATE::ACTIVE); - - ip->send_message_ignore_drop(hb); -} - -static void send_sys_status(MAVConnInterface *ip) { - mavlink_message_t msg {}; - mavlink::MsgMap map(msg); - - mavlink::common::msg::SYS_STATUS st {}; - st.load = 100; - - st.serialize(map); - mavlink::mavlink_finalize_message(&msg, ip->get_system_id(), ip->get_component_id(), st.MIN_LENGTH, st.LENGTH, st.CRC_EXTRA); - - //const mavlink::mavlink_msg_entry_t *e = mavlink::mavlink_get_msg_entry(msg.msgid); - - ip->send_message_ignore_drop(&msg); -} - -int main(int argc, char **argv){ - ros::init(argc, argv, "mavconn_test", ros::init_options::AnonymousName); - - MAVConnInterface::Ptr server; - MAVConnInterface::Ptr client; - - // create echo server - server = MAVConnInterface::open_url("udp://:45000@", 42, 200, - [&](const mavlink_message_t * msg, const Framing framing) { - server->send_message_ignore_drop(msg); - send_sys_status(server.get()); - }); - - // create client - client = MAVConnInterface::open_url("udp://:45001@localhost:45000", 44, 200, - [&](const mavlink_message_t * msg, const Framing framing) { - //std::cout << "C:RECV: " << msg->msgid << std::endl; - ROS_INFO("RECV: MsgID: %4u St: %d", msg->msgid, int(framing)); - }); - - while (ros::ok()) { - send_heartbeat(client.get()); - } - return 0; -} diff --git a/libmavconn/test/test_mavconn.cpp b/libmavconn/test/test_mavconn.cpp index 5565b1acc..98728e9f8 100644 --- a/libmavconn/test/test_mavconn.cpp +++ b/libmavconn/test/test_mavconn.cpp @@ -1,254 +1,302 @@ +// +// libmavconn +// Copyright 2013-2016,2018,2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// + /** * Test mavconn library */ +#include +#include +#include +#include + // Gmock broken on Ubuntu (thrusty), // its gmock 1.6 require gtest 1.7, while repository only provides 1.6 // this error exist one year without any updates. // https://bugs.launchpad.net/ubuntu/+source/google-mock/+bug/1201279 // // I think on Debian that was fixed while ago. But i can't use it in ros buildfarm. -//#include +// #include #include -//#include - #include #include +#include +#include -#include -#include -#include -#include - -using namespace mavconn; -using mavlink::mavlink_message_t; -using mavlink::msgid_t; - -namespace mavlink { - namespace common { - using namespace mavlink::minimal; - namespace msg { - using namespace mavlink::minimal::msg; - } - } -} - -static void send_heartbeat(MAVConnInterface *ip) { - using mavlink::common::MAV_TYPE; - using mavlink::common::MAV_AUTOPILOT; - using mavlink::common::MAV_MODE; - using mavlink::common::MAV_STATE; +using namespace mavconn; // NOLINT +using mavlink_message_t = mavlink::mavlink_message_t; +using msgid_t = mavlink::msgid_t; - mavlink::common::msg::HEARTBEAT hb = {}; - hb.type = int(MAV_TYPE::ONBOARD_CONTROLLER); - hb.autopilot = int(MAV_AUTOPILOT::INVALID); - hb.base_mode = int(MAV_MODE::MANUAL_ARMED); - hb.custom_mode = 0; - hb.system_status = int(MAV_STATE::ACTIVE); +namespace mavlink +{ +namespace common +{ +using namespace mavlink::minimal; // NOLINT +namespace msg +{ +using namespace mavlink::minimal::msg; // NOLINT +} +} +} // namespace mavlink - ip->send_message(hb); +static void send_heartbeat(MAVConnInterface * ip) +{ + using mavlink::common::MAV_AUTOPILOT; + using mavlink::common::MAV_MODE; + using mavlink::common::MAV_STATE; + using mavlink::common::MAV_TYPE; + + mavlink::common::msg::HEARTBEAT hb = {}; + hb.type = static_cast(MAV_TYPE::ONBOARD_CONTROLLER); + hb.autopilot = static_cast(MAV_AUTOPILOT::INVALID); + hb.base_mode = static_cast(MAV_MODE::MANUAL_ARMED); + hb.custom_mode = 0; + hb.system_status = static_cast(MAV_STATE::ACTIVE); + + ip->send_message(hb); } -class UDP : public ::testing::Test { +class UDP : public ::testing::Test +{ public: - std::mutex mutex; - std::condition_variable cond; - - msgid_t message_id; - - void recv_message(const mavlink_message_t *message, const Framing framing) { - //printf("Got message %u, len: %u, framing: %d\n", message->msgid, message->len, int(framing)); - message_id = message->msgid; - cond.notify_one(); - } - - bool wait_one() { - std::unique_lock lock(mutex); - return cond.wait_for(lock, std::chrono::seconds(2)) == std::cv_status::no_timeout; - } + std::mutex mutex; + std::condition_variable cond; + + msgid_t message_id; + + void recv_message(const mavlink_message_t * message, const Framing framing [[maybe_unused]]) + { + // printf("Got message %u, len: %u, framing: %d\n", message->msgid, message->len, int(framing)); + message_id = message->msgid; + cond.notify_one(); + } + + bool wait_one() + { + std::unique_lock lock(mutex); + return cond.wait_for(lock, std::chrono::seconds(2)) == std::cv_status::no_timeout; + } }; #if 0 // XXX(vooon): temparary disable that check - it don't work on Travis (with ICI) TEST_F(UDP, bind_error) { - MAVConnInterface::Ptr conns[2]; + MAVConnInterface::Ptr conns[2]; - conns[0] = std::make_shared(42, 200, "localhost", 45000); - ASSERT_THROW(conns[1] = std::make_shared(42, 200, "localhost", 45000), DeviceError); + conns[0] = std::make_shared(42, 200, "localhost", 45000); + ASSERT_THROW(conns[1] = std::make_shared(42, 200, "localhost", 45000), DeviceError); } #endif TEST_F(UDP, send_message) { - MAVConnInterface::Ptr echo, client; - - message_id = std::numeric_limits::max(); - auto msgid = mavlink::common::msg::HEARTBEAT::MSG_ID; - - // create echo server - echo = std::make_shared(42, 200, "0.0.0.0", 45002); - echo->connect([&](const mavlink_message_t * msg, const Framing framing) { - echo->send_message(msg); - }); - - // create client - client = std::make_shared(44, 200, "0.0.0.0", 45003, "localhost", 45002); - client->connect(std::bind(&UDP::recv_message, this, std::placeholders::_1, std::placeholders::_2)); - - // wait echo - send_heartbeat(client.get()); - send_heartbeat(client.get()); - EXPECT_EQ(wait_one(), true); - EXPECT_EQ(message_id, msgid); + MAVConnInterface::Ptr echo, client; + + message_id = std::numeric_limits::max(); + auto msgid = mavlink::common::msg::HEARTBEAT::MSG_ID; + + // create echo server + echo = std::make_shared(42, 200, "0.0.0.0", 45002); + echo->connect( + [&](const mavlink_message_t * msg, const Framing framing [[maybe_unused]]) { + echo->send_message(msg); + }); + + // create client + client = std::make_shared(44, 200, "0.0.0.0", 45003, "localhost", 45002); + client->connect( + std::bind( + &UDP::recv_message, this, std::placeholders::_1, + std::placeholders::_2)); + + // wait echo + send_heartbeat(client.get()); + send_heartbeat(client.get()); + EXPECT_EQ(wait_one(), true); + EXPECT_EQ(message_id, msgid); } -class TCP : public UDP {}; +class TCP : public UDP +{ +}; TEST_F(TCP, bind_error) { - MAVConnInterface::Ptr conns[2]; - - conns[0] = std::make_shared(42, 200, "localhost", 57600); - conns[0]->connect(MAVConnInterface::ReceivedCb()); - ASSERT_THROW(conns[1] = std::make_shared(42, 200, "localhost", 57600), DeviceError); + MAVConnInterface::Ptr conns[2]; + + conns[0] = std::make_shared(42, 200, "localhost", 57600); + conns[0]->connect(MAVConnInterface::ReceivedCb()); + ASSERT_THROW( + conns[1] = std::make_shared( + 42, 200, "localhost", + 57600), DeviceError); } TEST_F(TCP, connect_error) { - MAVConnInterface::Ptr client; - ASSERT_THROW(client = std::make_shared(42, 200, "localhost", 57666), DeviceError); + MAVConnInterface::Ptr client; + ASSERT_THROW( + client = std::make_shared(42, 200, "localhost", 57666), + DeviceError); } TEST_F(TCP, send_message) { - MAVConnInterface::Ptr echo_server, client; - - message_id = std::numeric_limits::max(); - auto msgid = mavlink::common::msg::HEARTBEAT::MSG_ID; - - // create echo server - echo_server = std::make_shared(42, 200, "0.0.0.0", 57602); - echo_server->connect([&](const mavlink_message_t * msg, const Framing framing) { - echo_server->send_message(msg); - }); - - // create client - client = std::make_shared(44, 200, "localhost", 57602); - client->connect(std::bind(&TCP::recv_message, this, std::placeholders::_1, std::placeholders::_2)); - - // wait echo - send_heartbeat(client.get()); - send_heartbeat(client.get()); - EXPECT_EQ(wait_one(), true); - EXPECT_EQ(message_id, msgid); + MAVConnInterface::Ptr echo_server, client; + + message_id = std::numeric_limits::max(); + auto msgid = mavlink::common::msg::HEARTBEAT::MSG_ID; + + // create echo server + echo_server = std::make_shared(42, 200, "0.0.0.0", 57602); + echo_server->connect( + [&](const mavlink_message_t * msg, const Framing framing [[maybe_unused]]) { + echo_server->send_message(msg); + }); + + // create client + client = std::make_shared(44, 200, "localhost", 57602); + client->connect( + std::bind( + &TCP::recv_message, this, std::placeholders::_1, + std::placeholders::_2)); + + // wait echo + send_heartbeat(client.get()); + send_heartbeat(client.get()); + EXPECT_EQ(wait_one(), true); + EXPECT_EQ(message_id, msgid); } TEST_F(TCP, client_reconnect) { - MAVConnInterface::Ptr echo_server; - MAVConnInterface::Ptr client1, client2; - - // create echo server - echo_server = std::make_shared(42, 200, "0.0.0.0", 57604); - echo_server->connect([&](const mavlink_message_t * msg, const Framing framing) { - echo_server->send_message(msg); - }); - - EXPECT_NO_THROW({ - client1 = std::make_shared(44, 200, "localhost", 57604); - }); - - EXPECT_NO_THROW({ - client2 = std::make_shared(45, 200, "localhost", 57604); - }); - - EXPECT_NO_THROW({ - client1 = std::make_shared(46, 200, "localhost", 57604); - }); + MAVConnInterface::Ptr echo_server; + MAVConnInterface::Ptr client1, client2; + + // create echo server + echo_server = std::make_shared(42, 200, "0.0.0.0", 57604); + echo_server->connect( + [&](const mavlink_message_t * msg, const Framing framing [[maybe_unused]]) { + echo_server->send_message(msg); + }); + + EXPECT_NO_THROW( + { + client1 = std::make_shared(44, 200, "localhost", 57604); + }); + + EXPECT_NO_THROW( + { + client2 = std::make_shared(45, 200, "localhost", 57604); + }); + + EXPECT_NO_THROW( + { + client1 = std::make_shared(46, 200, "localhost", 57604); + }); } TEST(SERIAL, open_error) { - MAVConnInterface::Ptr serial; - ASSERT_THROW(serial = std::make_shared(42, 200, "/some/magic/not/exist/path", 57600), DeviceError); + MAVConnInterface::Ptr serial; + ASSERT_THROW( + serial = std::make_shared( + 42, 200, "/some/magic/not/exist/path", + 57600), + DeviceError); } #if 0 TEST(URL, open_url_serial) { - MAVConnInterface::Ptr serial; - MAVConnSerial *serial_p; - - /* not best way to test tty access, - * but it does not require any preparation - * Disabled because it breaks terminal. - */ - EXPECT_NO_THROW({ - serial = MAVConnInterface::open_url("/dev/tty:115200"); - serial_p = dynamic_cast(serial.get()); - EXPECT_NE(serial_p, nullptr); - }); - - EXPECT_NO_THROW({ - serial = MAVConnInterface::open_url("serial:///dev/tty:115200?ids=2,240"); - serial_p = dynamic_cast(serial.get()); - EXPECT_NE(serial_p, nullptr); - }); + MAVConnInterface::Ptr serial; + MAVConnSerial * serial_p; + + /* not best way to test tty access, + * but it does not require any preparation + * Disabled because it breaks terminal. + */ + EXPECT_NO_THROW( + { + serial = MAVConnInterface::open_url("/dev/tty:115200"); + serial_p = dynamic_cast(serial.get()); + EXPECT_NE(serial_p, nullptr); + }); + + EXPECT_NO_THROW( + { + serial = MAVConnInterface::open_url("serial:///dev/tty:115200?ids=2,240"); + serial_p = dynamic_cast(serial.get()); + EXPECT_NE(serial_p, nullptr); + }); } #endif TEST(URL, open_url_udp) { - MAVConnInterface::Ptr udp; - MAVConnUDP *udp_p; - - EXPECT_NO_THROW({ - udp = MAVConnInterface::open_url("udp://localhost:45004@localhost:45005/?ids=2,241"); - udp_p = dynamic_cast(udp.get()); - EXPECT_NE(udp_p, nullptr); - }); - - EXPECT_NO_THROW({ - udp = MAVConnInterface::open_url("udp://@localhost:45005"); - udp_p = dynamic_cast(udp.get()); - EXPECT_NE(udp_p, nullptr); - }); - - EXPECT_NO_THROW({ - udp = MAVConnInterface::open_url("udp://localhost:45006@"); - udp_p = dynamic_cast(udp.get()); - EXPECT_NE(udp_p, nullptr); - }); - - EXPECT_THROW({ - udp = MAVConnInterface::open_url("udp://localhost:45008"); - }, DeviceError); + MAVConnInterface::Ptr udp; + MAVConnUDP * udp_p; + + EXPECT_NO_THROW( + { + udp = MAVConnInterface::open_url("udp://localhost:45004@localhost:45005/?ids=2,241"); + udp_p = dynamic_cast(udp.get()); + EXPECT_NE(udp_p, nullptr); + }); + + EXPECT_NO_THROW( + { + udp = MAVConnInterface::open_url("udp://@localhost:45005"); + udp_p = dynamic_cast(udp.get()); + EXPECT_NE(udp_p, nullptr); + }); + + EXPECT_NO_THROW( + { + udp = MAVConnInterface::open_url("udp://localhost:45006@"); + udp_p = dynamic_cast(udp.get()); + EXPECT_NE(udp_p, nullptr); + }); + + EXPECT_THROW( + { + udp = MAVConnInterface::open_url("udp://localhost:45008"); + }, + DeviceError); } TEST(URL, open_url_tcp) { - MAVConnInterface::Ptr tcp_server, tcp_client; - - MAVConnTCPServer *tcp_server_p; - MAVConnTCPClient *tcp_client_p; - - EXPECT_NO_THROW({ - tcp_server = MAVConnInterface::open_url("tcp-l://localhost:57606"); - tcp_server_p = dynamic_cast(tcp_server.get()); - EXPECT_NE(tcp_server_p, nullptr); - }); - - EXPECT_NO_THROW({ - tcp_client = MAVConnInterface::open_url("tcp://localhost:57606"); - tcp_client_p = dynamic_cast(tcp_client.get()); - EXPECT_NE(tcp_client_p, nullptr); - }); + MAVConnInterface::Ptr tcp_server, tcp_client; + + MAVConnTCPServer * tcp_server_p; + MAVConnTCPClient * tcp_client_p; + + EXPECT_NO_THROW( + { + tcp_server = MAVConnInterface::open_url("tcp-l://localhost:57606"); + tcp_server_p = dynamic_cast(tcp_server.get()); + EXPECT_NE(tcp_server_p, nullptr); + }); + + EXPECT_NO_THROW( + { + tcp_client = MAVConnInterface::open_url("tcp://localhost:57606"); + tcp_client_p = dynamic_cast(tcp_client.get()); + EXPECT_NE(tcp_client_p, nullptr); + }); } -int main(int argc, char **argv){ - //ros::init(argc, argv, "mavconn_test", ros::init_options::AnonymousName); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/mavros/CHANGELOG.rst b/mavros/CHANGELOG.rst index d13629af6..9a75790bd 100644 --- a/mavros/CHANGELOG.rst +++ b/mavros/CHANGELOG.rst @@ -2,15 +2,152 @@ Changelog for package mavros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.16.0 (2023-05-05) -------------------- -* Merge pull request `#1829 `_ from snwu1996/latched_gp_origin_pub - Made it such that the gp_origin topic publisher is latched. -* made it such that the gp_origin topic published latched. -* Merge pull request `#1817 `_ from lucasw/pluginlib_hpp - use hpp instead of deprecated .h pluginlib headers -* use hpp instead of deprecated .h pluginlib headers -* Contributors: Lucas Walter, Shu-Nong Wu, Vladimir Ermakov +2.5.0 (2023-05-05) +------------------ +* Merge pull request `#1852 `_ from robobe/fix-mavlink-header-stamp + Fix mavlink header stamp in convert_to_rosmsg method +* Delete test_convert_to_rosmsg.py +* fix flake8 +* fix flake8 +* fix conver_to_rosmsg method + fix header stamp field + move from rclpy.Time to + builtin_interfaces Time message +* Merge branch 'mavlink_payload64_fix' into ros2 +* Merge pull request `#1851 `_ from robobe/mavlink_payload64_fix + cast payload_octest to int +* cast payload_octest to int + test script , +* Merge pull request `#1838 `_ from vacabun/launch_namespace_fix + fix ROS2 launch parameters namespace and name. +* fix plugin denylist allowlist name, and fix parameters namespace. +* Merge pull request `#1835 `_ from vacabun/multi_uas_launch_fix + Multi-UAS launch +* Merge branch 'ros2' into multi_uas_launch_fix +* Merge pull request `#1836 `_ from eMrazSVK/ros2 + Fix apm_pluginlist.yaml and apm.launch +* naming +* Fix apm config and launch ROS2 +* Remove duplicate parameters +* fix parameters name tgt_system and tgt_component and add multi-uas launch. +* Merge pull request `#1834 `_ from vacabun/px4_launch_fix + fix px4 launch file for ROS2 +* change plugin odom parameters in apm launch. +* 1.Change part of parameters to be flat In ROS2. + 2.Remove plugin safety_area in launch parameters file. +* remove event_launcher.yaml mavlink_bridge.launch +* remove file 'apm2.lunch'. +* fix apm launch files to ROS2. +* fix plugin distance_sensor parses yaml from string parameter. +* fix px4 launch file on ROS2. +* Merge pull request `#1833 `_ from lopsided98/ftp-segfault + plugins: ftp: fix null pointer dereference +* plugins: ftp: fix null pointer dereference + df4e529 mistakenly switched PayloadHeader::data from using a non-standards + compliant (but accepted by most compilers) flexible array to a pointer. This + resulted in an attempt to dereference the uninitialized contents of the array. + This patch eliminates PayloadHeader::data and instead makes FTPRequest::data() + use pointer arithmetic to get the data buffer from within the payload buffer. +* Contributors: Ben Wolsieffer, Eduard Mraz, Vladimir Ermakov, robo, robobe, vacabun + +2.4.0 (2022-12-30) +------------------ +* ci: ignore xml lib warn +* Merge branch 'master' into ros2 + * master: + 1.15.0 + update changelog + ci: update actions + Implement debug float array handler + mavros_extras: Fix a sequence point warning + mavros_extras: Fix a comparison that shouldn't be bitwise + mavros: Fix some warnings + mavros_extras: Fix buggy check for lat/lon ignored + libmavconn: fix MAVLink v1.0 output selection +* 1.15.0 +* update changelog +* Merge pull request `#1806 `_ from scoutdi/fix-some-warnings + mavros: Fix some warnings +* mavros: Fix some warnings +* Contributors: Morten Fyhn Amundsen, Vladimir Ermakov + +2.3.0 (2022-09-24) +------------------ +* extras: fix linter errors +* mavros: remove custom find script, re-generate +* Merge branch 'master' into ros2 + * master: + 1.14.0 + update changelog + scripts: waypoint and param files are text, not binary + libmavconn: fix MAVLink v1.0 output selection + plugins: add guided_target to accept offboard position targets + add cmake module path for geographiclib on debian based systems + use already installed FindGeographicLib.cmake +* 1.14.0 +* update changelog +* scripts: waypoint and param files are text, not binary + Fix `#1784 `_ +* Merge pull request `#1780 `_ from snktshrma/master + guided_target: accept position-target-global-int messages +* plugins: add guided_target to accept offboard position targets + Update guided_target.cpp + Update guided_target.cpp + Update mavros_plugins.xml + Update CMakeLists.txt + Added offboard_position.cpp + Update apm_config.yaml + Update offboard_position.cpp + Update offboard_position.cpp + Rename offboard_position.cpp to guided_target.cpp + Update CMakeLists.txt + Update mavros_plugins.xml + Update apm_config.yaml + Update guided_target.cpp +* Merge pull request `#1775 `_ from acxz/find-geographiclib + use already installed FindGeographicLib.cmake +* add cmake module path for geographiclib on debian based systems +* Merge pull request `#1771 `_ from alehed/fix/update_comment + Put correct version in comment +* Put correct version in comment + Now that the change has been merged into master in pymavlink, + it will be in the next tagged release. +* Contributors: Alexander Hedges, Sanket Sharma, Vladimir Ermakov, acxz + +2.2.0 (2022-06-27) +------------------ +* extras: fix build +* Merge branch 'master' into ros2 + * master: + mount_control.cpp: detect MOUNT_ORIENTATION stale messages + ESCTelemetryItem.msg: correct RPM units + apm_config.yaml: add mount configuration + sys_status.cpp fix free memory for values > 64KiB + uncrustify cellular_status.cpp + Add CellularStatus plugin and message + *_config.yaml: document usage of multiple batteries diagnostics + sys_status.cpp: fix compilation + sys_status.cpp: support diagnostics on up-to 10 batteries + sys_status.cpp: do not use harcoded constants + sys_status.cpp: Timeout on MEMINFO and HWSTATUS mavlink messages and publish on the diagnostics + sys_status.cpp: fix enabling of mem_diag and hwst_diag + sys_status.cpp: Do not use battery1 voltage as voltage for all other batteries (bugfix). + sys_status.cpp: ignore sys_status mavlink messages from gimbals + mount_control.cpp: use mount_nh for params to keep similarities with other plugins set diag settings before add() + sys_status.cpp: remove deprecated BATTERY2 mavlink message support + Mount control plugin: add configurable diagnostics + Bugfix: increment_f had no value asigned when input LaserScan was bigger than obstacle.distances.size() + Bugfix: wrong interpolation when the reduction ratio (scale_factor) is not integer. + Disable startup_px4_usb_quirk in px4_config.yaml +* cmake: style fix +* cmake: downgrade to C++17 as 20 breaks something in rclcpp +* pylib: fix flake8 for checkid +* cmake: hide -std=c++2a +* Merge pull request `#1752 `_ from alehed/fix/make_compatible_with_mavlink_pr_666 + Make compatible with pymavlink type annotations PR +* Make compatible with pymavlink type annotations PR + In that PR, the attribute name is changed to msgname due to conflicts + with message instance variables. 1.15.0 (2022-12-30) ------------------- @@ -48,6 +185,11 @@ Changelog for package mavros correct MountConfigure response success correct constructor initialization order some gimbals send negated/inverted angle measurements, correct that to obey the MAVLink frame convention using run-time parameters +* Update global_position.py +* Merge pull request `#1745 `_ from antonyramsy/ros2 + fix subscribe_raw_salellites typo +* fix subscribe_raw_salellites typo + subscribe_raw_salellites -> subscribe_raw_satellites * Merge pull request `#1743 `_ from amilcarlucas/pr_apm_config apm_config.yaml: add mount configuration * apm_config.yaml: add mount configuration @@ -84,6 +226,516 @@ Changelog for package mavros * Merge pull request `#1696 `_ from okalachev/patch-2 Disable startup_px4_usb_quirk in px4_config.yaml * Disable startup_px4_usb_quirk in px4_config.yaml +* Contributors: Alexander Hedges, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Oleg Kalachev, Vladimir Ermakov, antonyramsy + +2.1.1 (2022-03-02) +------------------ +* Merge pull request `#1717 `_ from rob-clarke/fix--sys-status-callbacks + Maybe fix sys status callbacks +* uncrustify +* Cleanup for pr +* Initialise common client +* Add debug +* Use common client + Add debug +* plugins: Fix misprint + Fix `#1709 `_ +* Contributors: Rob Clarke, Vladimir Ermakov + +2.1.0 (2022-02-02) +------------------ +* plugin: sys_status: Add temporary hack from @Michel1968 + https://github.com/mavlink/mavros/issues/1588#issuecomment-1027699924 +* py-lib: make linters happy again +* py-lib: fix WPL loading +* py-lib: reformat with black, fix WPL +* py-lib: gracefull exiting - need to join to spinner thread +* py-lib: debug shutdown call +* py-lib: fix checkid +* sys: place service servers to separate callback group +* plugins: fix topic names to use prefix for namespaced ones +* py-lib: fix import +* uas: fix linter warnings +* uas: set executor timeout to 1s +* uas: use custom executor derived from MultiThreadedExecutor one +* uas: fix lambda +* ci: fix several lint warnings +* uas: log amount of executor threads +* command: add request header for possible future use +* Merge branch 'master' into ros2 + * master: + 1.13.0 + update changelog + py-lib: fix compatibility with py3 for Noetic + re-generate all coglets + test: add checks for ROTATION_CUSTOM + lib: Fix rotation search for CUSTOM + Removed CamelCase for class members. Publish to "report" + More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages + Fixed callback name to match `handle\_{MESSAGE_NAME.lower()}` convention + Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html + Fixed topic names to match more closely what other plugins use. Fixed a typo. + Add plugin for reporting terrain height estimate from FCU + 1.12.2 + update changelog + Set time/publish_sim_time to false by default + plugin: setpoint_raw: move getParam to initializer + extras: trajectory: backport `#1667 `_ +* 1.13.0 +* update changelog +* Merge pull request `#1690 `_ from mavlink/fix-enum_sensor_orientation + Fix enum sensor_orientation +* py-lib: fix compatibility with py3 for Noetic +* test: add checks for ROTATION_CUSTOM +* lib: Fix rotation search for CUSTOM + Fix `#1688 `_. +* 1.12.2 +* update changelog +* Merge pull request `#1672 `_ from okalachev/patch-1 + Set time/publish_sim_time to false by default +* Set time/publish_sim_time to false by default +* Merge pull request `#1669 `_ from Hs293Go/master + plugin: setpoint_raw: move getParam to initializer +* plugin: setpoint_raw: move getParam to initializer + Repeatedly getting the thrust_scaling parameter in a callback that can + be invoked from a fast control loop may fail spuriously and trigger a + fatal error +* Merge pull request `#1670 `_ from windelbouwman/fix-uninitialized-struct-member + Fix spurious bug because class field was uninitialized. +* Fix spurious bug because class field was uninitialized. +* Merge branch 'master' into ros2 + * master: + 1.12.1 + update changelog + mavconn: fix connection issue introduced by `#1658 `_ + mavros_extras: Fix some warnings + mavros: Fix some warnings +* 1.12.1 +* update changelog +* mavconn: fix connection issue introduced by `#1658 `_ +* Merge pull request `#1660 `_ from scoutdi/fix-warnings + Fix warnings +* mavros: Fix some warnings +* Contributors: Morten Fyhn Amundsen, Oleg Kalachev, Vladimir Ermakov, Windel Bouwman, hs293go + +2.0.5 (2021-11-28) +------------------ +* fix some build warnings; drop old copter vis +* router: fix `#1655 `_: use MAVConnInterface::connect() from `#1658 `_ +* Merge branch 'master' into ros2 + * master: + 1.12.0 + update changelog + Fix multiple bugs + lib: fix mission frame debug print + extras: distance_sensor: revert back to zero quaternion +* 1.12.0 +* update changelog +* Merge pull request `#1658 `_ from asherikov/as_bugfixes + Fix multiple bugs +* Fix multiple bugs + - fix bad_weak_ptr on connect and disconnect + - introduce new API to avoid thread race when assigning callbacks + - fix uninitialized variable in TCP client constructor which would + randomly block TCP server + This is an API breaking change: if client code creates connections using + make_shared<>() instead of open_url(), it is now necessary to call new + connect() method explicitly. +* extras: fix some more lint warns +* plugin: fix some compile warnings +* cmake: require C++20 to build all modules +* extras: port distance_sensor plugin +* lib: ignore MAVPACKED-related warnings from mavlink +* lib: fix mission frame debug print +* msgs: update conversion header +* Merge branch 'master' into ros2 + * master: + 1.11.1 + update changelog + lib: fix build +* 1.11.1 +* update changelog +* lib: fix build +* Merge branch 'master' into ros2 + * master: + 1.11.0 + update changelog + lib: fix ftf warnings + msgs: use pragmas to ignore unaligned pointer warnings + extras: landing_target: fix misprint + msgs: fix convert const + plugin: setpoint_raw: fix misprint + msgs: try to hide 'unaligned pointer' warning + plugin: sys: fix compillation error + plugin: initialize quaternions with identity + plugin: sys: Use wall timers for connection management + Use meters for relative altitude + distance_sensor: Initialize sensor orientation quaternion to zero + Address review comments + Add camera plugin for interfacing with mavlink camera protocol +* 1.11.0 +* update changelog +* lib: fix ftf warnings +* plugin: setpoint_raw: fix misprint +* plugin: sys: fix compillation error +* plugin: initialize quaternions with identity + Eigen::Quaternion[d|f] () does not initialize with zeroes or identity. + So we must initialize with identity vector objects that can be left + unassigned. + Related to `#1652 `_ +* plugin: sys: Use wall timers for connection management + Fixes `#1629 `_ +* Merge pull request `#1651 `_ from Jaeyoung-Lim/pr-image-capture-plugin + Add camera plugin for interfacing with mavlink camera protocol +* Add camera plugin for interfacing with mavlink camera protocol + Add camera image captured message for handling camera trigger information +* extras: port log_transfer +* Contributors: Alexander Sherikov, Jaeyoung-Lim, Vladimir Ermakov + +2.0.4 (2021-11-04) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.10.0 + prepare release +* 1.10.0 +* prepare release +* extras: port esc_status plugin +* plugins: update metadata xml +* mavros: port mav_controller_output plugin +* Merge branch 'master' into ros2 + * master: (25 commits) + Remove reference + Catch std::length_error in send_message + Show ENOTCONN error instead of crash + Tunnel: Check for invalid payload length + Tunnel.msg: Generate enum with cog + mavros_extras: Create tunnel plugin + mavros_msgs: Add Tunnel message + MountControl.msg: fix copy-paste + sys_time.cpp: typo + sys_time: publish /clock for simulation times + 1.9.0 + update changelog + Spelling corrections + Changed OverrideRCIn to 18 channels + This adds functionality to erase all logs on the SD card via mavlink + publish BATTERY2 message as /mavros/battery2 topic + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 + Added NAV_CONTROLLER_OUTPUT Plugin + Added GPS_INPUT plugin + Update esc_status plugin with datatype change on MAVLink. + ... +* Merge pull request `#1631 `_ from shubham-shahh/ros2 + rectified spelling and gramatical errors +* Update mission_protocol_base.cpp +* Update test_uas.cpp +* Update setpoint_raw.cpp +* Update param.cpp +* Update param.cpp +* Update mission_protocol_base.cpp +* Update ftp.cpp +* Update command.cpp +* Update param.py +* Merge pull request `#1626 `_ from valbok/crash_on_shutdown + Show ENOTCONN error instead of crash on socket's shutdown +* Merge pull request `#1627 `_ from marcelino-pensa/bug/ma-prevent-race-condition + Node dying when calling /mavros/param/pull +* Remove reference +* Catch std::length_error in send_message + Instead of crashing the process +* Merge pull request `#1623 `_ from amilcarlucas/pr/more-typo-fixes + More typo fixes +* sys_time.cpp: typo +* Merge pull request `#1622 `_ from dayjaby/sys_time_pub_clock + sys_time: publish /clock for simulation times +* sys_time: publish /clock for simulation times +* 1.9.0 +* update changelog +* Merge pull request `#1616 `_ from amilcarlucas/pr/RC_CHANNELS-mavlink2-extensions + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels.… +* Changed OverrideRCIn to 18 channels +* Merge pull request `#1617 `_ from amilcarlucas/pr/NAV_CONTROLLER_OUTPUT-plugin + Added NAV_CONTROLLER_OUTPUT Plugin +* Merge pull request `#1619 `_ from amilcarlucas/pr/BATTERY2-topic + publish BATTERY2 message as /mavros/battery2 topic +* publish BATTERY2 message as /mavros/battery2 topic +* Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 +* Added NAV_CONTROLLER_OUTPUT Plugin +* Merge branch 'master' into master +* plugins: fix lint error +* extras: fix build, add UAS::send_massage(msg, compid) +* extras: port companion_process_status +* style: apply ament_uncrustify --reformat +* Merge branch 'master' into ros2 + * master: + extras: esc_telemetry: fix build + extras: fix esc_telemetry centi-volt/amp conversion + extras: uncrustify all plugins + plugins: reformat xml + extras: reformat plugins xml + extras: fix apm esc_telemetry + msgs: fix types for apm's esc telemetry + actually allocate memory for the telemetry information + fixed some compile errors + added esc_telemetry plugin + Reset calibration flag when re-calibrating. Prevent wrong data output. + Exclude changes to launch files. + Delete debug files. + Apply uncrustify changes. + Set progress array to global to prevent erasing data. + Move Compass calibration report to extras. Rewrite code based on instructions. + Remove extra message from CMakeLists. + Add message and service definition. + Add compass calibration feedback status. Add service to call the 'Next' button in calibrations. +* plugins: reformat xml +* mavros_extras: ported landing_target plugin to ros2 +* sanitized code +* Exclude changes to launch files. +* Delete debug files. +* Apply uncrustify changes. +* Move Compass calibration report to extras. Rewrite code based on instructions. +* Add compass calibration feedback status. Add service to call the 'Next' button in calibrations. +* Contributors: André Filipe, BV-OpenSource, David Jablonski, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Marcelino Almeida, Shubham Shah, Val Doroshchuk, Vladimir Ermakov + +2.0.3 (2021-06-20) +------------------ +* param: fix Foxy build +* Contributors: Vladimir Ermakov + +2.0.2 (2021-06-20) +------------------ +* mavros: fix run on Galactic +* plugin: param: `#1579 `_: fix cpplint warnings +* plugin: param: `#1579 `_: implement std parameter events +* plugin: param: `#1579 `_: rewrite plugin to implement standard parameter services +* plugin: add ability to set node options +* lib: fix lint error +* plugin: fix build error +* lib: fix reorder warnings +* lib: fix ftf compilation warnings +* Contributors: Vladimir Ermakov + +2.0.1 (2021-06-06) +------------------ +* readme: update source build instruction +* Merge branch 'master' into ros2 + * master: + readme: update + 1.8.0 + update changelog + Create semgrep-analysis.yml + Create codeql-analysis.yml +* 1.8.0 +* update changelog +* Contributors: Vladimir Ermakov + +2.0.0 (2021-05-28) +------------------ +* pylib: fixing pep257 errors +* pylib: fixing pep257 errors +* pylib: fixing pep257 errors +* pylib: fixing pep257 errors +* pylib: fix flake8 +* pylib: fixing lint erorrs +* includes: include tf2 buffer +* pylib: fix ftp, add flags to wp +* pylib: port mavftp +* test: fix ParamDict test, yapf +* pylib: fix wp load/dump file +* pylib: port mavwp +* pylib: fix param plugin +* pylib: port mavparam +* pylib: add uas settings accessor +* pylib: fix set_mode +* plugin: fix sys_status ~/set_mode service +* pylib: porting mavsys +* pylib: fix checkid +* pylib: port checkid +* pylib: force-create mav script entry point, why console_scripts didn't work\? +* pylib: small fix for setup +* pylib: fix mavcmd trigger +* pylib: move cmd check to utils +* pylib: move wait flag to global group +* pylib: port mavsafety, drop safetyarea as it completely outdated +* pylib: fix script path +* pylib: wait for services by default +* pylib: add local position plugin +* pylib: port all mavcmd +* pylib: port most of mavcmd +* pylib: start porting mavcmd +* pylib: fix loading +* pylib: port ftp +* pylib: port mavlink helpers +* pylib: port setpoint plugin +* pylib: remove event_lanucher, ros2 should have different way to do the same +* pylib: test ParamFile +* pylib: test ParamDict +* pylib: port param +* pylib: add system module +* pylib: fix loading +* pylib: apply yapf +* tests: add simple plan file +* msgs: update command codes +* pylib: move to support ament_python +* pylib: start porting +* plugins: fix all cpplint errors +* plugins: fix some cpplint errors +* test: fix cpplint errors +* lib: fix lint errors +* lib: fixing cpplint +* plugins: waypoint: fix parameter exception +* plugins: geofence: port to ros2 +* plugins: rallypoint: port to ros2 +* plugins: waypoint: port to ros2 +* plugins: mission base ported to ros2 +* plugins: mission: noe step further +* mission proto: start port +* mavros: make cpplint happy about includes +* tests: make cpplint happy +* mavros: make cpplint happy +* lib: uncrustify +* Merge branch 'master' into ros2 + * master: + ci: github uses yaml parser which do not support anchors. surprise, surprise! + ci: install geographiclib datasets + extras: `#1370 `_: set obstacle aangle offset + lib: ftf: allow both Quaterniond and Quaternionf for quaternion_to_mavlink() + extras: distance_sensor: rename param for custom orientation, apply uncrustify + px4_config: Add distance_sensor parameters + distance_sensor: Add horizontal_fov_ratio, vertical_fov_ratio, sensor_orientation parameters + distance_sensor: Fill horizontal_fov, vertical_fov, quaternion +* lib: ftf: allow both Quaterniond and Quaternionf for quaternion_to_mavlink() +* extras: distance_sensor: rename param for custom orientation, apply uncrustify +* px4_config: Add distance_sensor parameters +* lib: fix misprint +* plugins: param: `#1567 `_: use parameters qos +* lib: more lint... +* lib: fix more linter warnings +* lib: fix some linter warnings +* lib: fix some linter warnings +* router: fix lint error, it invalidated after erase +* plugins: ftp: disable ll debug +* plugins: param: add use_sim_time to excluded ids +* plugins: param: set only allowed posparam +* plugins: param: ported to ros2 +* plugins: ftp: port to ros2 +* plugins: setpoint_position: port to ros2 +* plugins: setpoint_attitude: port to ros2 +* Merge branch 'master' into ros2 + * master: + convert whole expression to mm +* convert whole expression to mm +* plugins: setpoint_trajectory: port to ros2 +* plugins: setpoint_velocity: port to ros2 +* plugins: setpoint_accel: port to ros2 +* plugins: setpoint_raw: fix sefgault +* plugins: setpoint_raw: port to ros2 +* plugins: imu: port to ros2 +* plugins: global_position: port to ros2 +* plugin: local_position: port to ros2 +* plugins: wind_estimation: port to ros2 +* plugins: rc_io: port to ros2 +* plugins: port manual_control +* plugins: home_position: port to ros2 +* plugins: sys_status: update set_message_interval +* plugins: sys_status: implement autopilot version request +* plugins: port command to ros2 +* uas: add eigen aligned allocator +* plugin: altitude: port to ros2 +* uas: add convinient helpers +* plugins: actuator_control: port to ros2 +* uas: fix some more lint errors +* uas: fix some lint errors +* plugin: sys_status: fix some lint errors +* plugins: port sys_time +* Merge branch 'master' into ros2 + * master: + 1.7.1 + update changelog + re-generate all pymavlink enums + 1.7.0 + update changelog +* plugins: sys_status: fix connection timeout +* lib: update cog to match ament-uncrustify +* plugins: sys_status: fixing mav_type +* plugins: sys_state: declare parameters +* plugins: sys_state: ported most of things +* plugins: sys_status: port most of the parts +* plugins: start porting sys_status +* plugins: generate description xml +* plugins: port dummy +* mavros: generate plugin list +* Merge branch 'master' into ros2 + * master: + msgs: re-generate the code + lib: re-generate the code + plugins: mission: re-generate the code + MissionBase: correction to file information + MissionBase: add copyright from origional waypoint.cpp + uncrustify + whitespace + add rallypoint and geofence plugins to mavros plugins xml + add rallypoint and geofence plugins to CMakeList + Geofence: add geofence plugin + Rallypoint: add rallypoint plugin + Waypoint: inherit MissionBase class for mission protocol + MissionBase: breakout mission protocol from waypoint.cpp + README: Update PX4 Autopilot references + Fix https://github.com/mavlink/mavros/issues/849 +* uas: test multiple handlers for same message +* uas: implement test for plugin message router +* uas: fix is_plugin_allowed truth table +* uas: initial unittest +* uas: implement tf helpers +* uas: add parameters callback, testing helper +* node: disable intra process messaging because it's throws errors +* uas: it's compilling! +* uas: still fixing build... +* uas: split uas_data.cpp into smaller units +* uas: fix misprints +* uas: initial implementation porting +* uas: style fixes in headers +* uas: update plugin base class, add registration macro +* uas: begin implementation +* router: use common format for address +* router: add source id to UAS frame_id +* mavros_node: that binary would be similar to old mavros v1 node +* router: fix conponent loading +* router: add test for Endpoint::recv_message +* router: rename mavlink to/from to source/sink, i think that terms more descriptive +* router: add diagnostics updater +* router: fix incorrect get_msg_byte +* router: trying to deal with mock cleanup checks +* router: initial import of the test +* router: catch open erros on ROSEndpoint +* router: catch DeviceError +* router: remove debugging printf's +* router: weak_ptr segfaults, replace with shared_ptr +* router: implement routing, cleanup +* tools: remove our custom uncrustify, would use amend-uncrustiry instead +* mavros: ament-uncrustify all code. enen unused one +* router: implement params handler +* router: fix build +* router: add handler for parameters and reconnection timer +* router: add some code docs, ament-uncrustify +* router: implement basic part of Endpoint +* lib: add stub code for router +* mavros: router decl done +* mavros: prototyping router +* mavros: update tests +* lib: port most of utilities +* mnavros: lib: apply ament_uncrustify +* lib: port enum_to_string +* lib: update sensor_orientation +* mavros: add rcpputils +* mavros: fix cmake to build libmavros +* mavros: begin porting... +* Merge pull request `#1186 `_ from PickNikRobotics/ros2 + mavros_msgs Ros2 +* Merge branch 'ros2' into ros2 +* msgs: start porting to ROS2 +* disable all packages but messages * Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Oleg Kalachev, Sanket Sharma, Vladimir Ermakov, acxz 1.13.0 (2022-01-13) diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt index 994003d68..65eb973e6 100644 --- a/mavros/CMakeLists.txt +++ b/mavros/CMakeLists.txt @@ -1,42 +1,39 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(mavros) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - angles - diagnostic_msgs - diagnostic_updater - eigen_conversions - geographic_msgs - geometry_msgs - libmavconn - mavros_msgs - nav_msgs - pluginlib - rosconsole_bridge - roscpp - sensor_msgs - std_msgs - std_srvs - tf2_eigen - tf2_ros - trajectory_msgs -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) - -## Find Eigen -find_package(Eigen3) -if(NOT EIGEN3_FOUND) - # Fallback to cmake_modules - find_package(cmake_modules REQUIRED) - find_package(Eigen REQUIRED) - set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) - set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) +# Default to C++20 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++2a") endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # we dont use add_compile_options with pedantic in message packages + # because the Python C extensions dont comply with it + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wcomment") + +# Allow GNU extensions (-std=gnu++20) +set(CMAKE_C_EXTENSIONS ON) +set(CMAKE_CXX_EXTENSIONS ON) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +# find mavros dependencies +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rcpputils REQUIRED) +find_package(message_filters REQUIRED) + +find_package(mavlink REQUIRED) +find_package(libmavconn REQUIRED) +find_package(console_bridge REQUIRED) +#find_package(rosconsole_bridge REQUIRED) # XXX TODO: connect libmavconn loggers + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) ## Find GeographicLib # Append to CMAKE_MODULE_PATH since debian/ubuntu installs @@ -44,84 +41,89 @@ endif() set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib") find_package(GeographicLib REQUIRED) -## Check if the datasets are installed -include(CheckGeographicLibDatasets) +find_package(angles REQUIRED) +find_package(eigen_stl_containers REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_ros REQUIRED) -include(EnableCXX11) -include(MavrosMavlink) +find_package(diagnostic_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) -# detect if sensor_msgs has BatteryState.msg -# http://answers.ros.org/question/223769/how-to-check-that-message-exists-with-catkin-for-conditional-compilation-sensor_msgsbatterystate/ -list(FIND sensor_msgs_MESSAGE_FILES "msg/BatteryState.msg" BATTERY_STATE_MSG_IDX) -if(${BATTERY_STATE_MSG_IDX} GREATER -1) - add_definitions( - -DHAVE_SENSOR_MSGS_BATTERYSTATE_MSG - ) -endif() +find_package(geographic_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(trajectory_msgs REQUIRED) -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -catkin_python_setup() - - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES mavros - CATKIN_DEPENDS diagnostic_msgs diagnostic_updater eigen_conversions geographic_msgs geometry_msgs libmavconn mavros_msgs message_runtime nav_msgs pluginlib roscpp sensor_msgs std_msgs tf2_ros trajectory_msgs - DEPENDS Boost EIGEN3 GeographicLib +include_directories( + $ + $ ) -########### -## Build ## -########### - include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} ${mavlink_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${GeographicLib_INCLUDE_DIRS} ) -add_library(mavros +## Check if the datasets are installed +include(CheckGeographicLibDatasets) + +if(rclcpp_VERSION VERSION_LESS 9.0.0) + add_definitions( + -DUSE_OLD_DECLARE_PARAMETER + ) +endif() + +# [[[cog: +# import mavros_cog +# ]]] +# [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) + +add_library(mavros SHARED + # [[[cog: + # mavros_cog.outl_glob_files('src/lib') + # ]]] src/lib/enum_sensor_orientation.cpp src/lib/enum_to_string.cpp src/lib/ftf_frame_conversions.cpp src/lib/ftf_quaternion_utils.cpp - src/lib/mavlink_diag.cpp - src/lib/mavros.cpp - src/lib/rosconsole_bridge.cpp + src/lib/mavros_router.cpp + src/lib/mavros_uas.cpp + src/lib/plugin.cpp + src/lib/uas_ap.cpp src/lib/uas_data.cpp + src/lib/uas_executor.cpp src/lib/uas_stringify.cpp + src/lib/uas_tf.cpp src/lib/uas_timesync.cpp + # [[[end]]] (checksum: 3239d16fc87982964694f18425f297b1) ) -add_dependencies(mavros - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(mavros - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${GeographicLib_LIBRARIES} +ament_target_dependencies(mavros + rclcpp + rclcpp_components + rcpputils + #class_loader + sensor_msgs + pluginlib + mavros_msgs + libmavconn + #console_bridge + diagnostic_updater + tf2_ros + tf2_eigen + Eigen3 ) -if(NOT APPLE) - target_link_libraries(mavros - atomic - ) -endif() +target_link_libraries(mavros ${GeographicLib_LIBRARIES}) +rclcpp_components_register_nodes(mavros "mavros::router::Router" "mavros::uas::UAS") -add_library(mavros_plugins - src/plugins/3dr_radio.cpp +add_library(mavros_plugins SHARED + # [[[cog: + # mavros_cog.outl_glob_files('src/plugins') + # ]]] src/plugins/actuator_control.cpp src/plugins/altitude.cpp src/plugins/command.cpp @@ -129,7 +131,6 @@ add_library(mavros_plugins src/plugins/ftp.cpp src/plugins/geofence.cpp src/plugins/global_position.cpp - src/plugins/hil.cpp src/plugins/home_position.cpp src/plugins/imu.cpp src/plugins/local_position.cpp @@ -139,101 +140,137 @@ add_library(mavros_plugins src/plugins/param.cpp src/plugins/rallypoint.cpp src/plugins/rc_io.cpp - src/plugins/safety_area.cpp src/plugins/setpoint_accel.cpp src/plugins/setpoint_attitude.cpp src/plugins/setpoint_position.cpp src/plugins/setpoint_raw.cpp - src/plugins/setpoint_velocity.cpp src/plugins/setpoint_trajectory.cpp + src/plugins/setpoint_velocity.cpp src/plugins/sys_status.cpp src/plugins/sys_time.cpp - src/plugins/vfr_hud.cpp src/plugins/waypoint.cpp src/plugins/wind_estimation.cpp + # [[[end]]] (checksum: ccf56c1a56e9dccf8464483f7b1eab99) ) add_dependencies(mavros_plugins mavros ) target_link_libraries(mavros_plugins mavros - ${catkin_LIBRARIES} ) +ament_target_dependencies(mavros_plugins + angles + geometry_msgs + geographic_msgs + mavros_msgs + std_msgs + std_srvs + sensor_msgs + pluginlib + nav_msgs + trajectory_msgs + rclcpp + rclcpp_components + rcpputils + libmavconn + diagnostic_updater + tf2_ros + tf2_eigen + message_filters + Eigen3 +) +pluginlib_export_plugin_description_file(mavros mavros_plugins.xml) + +add_executable(mavros_node src/mavros_node.cpp) +target_link_libraries(mavros_node mavros) + +ament_python_install_package(${PROJECT_NAME}) -## Declare a cpp executable -add_executable(mavros_node - src/mavros_node.cpp +# NOTE(vooon): for some reason console_scripts do not work with ament_python! +install(PROGRAMS scripts/mav + DESTINATION lib/${PROJECT_NAME} ) -add_dependencies(mavros_node - mavros + +install(TARGETS mavros mavros_node mavros_plugins + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -target_link_libraries(mavros_node - mavros - ${catkin_LIBRARIES} + +install(PROGRAMS scripts/install_geographiclib_datasets.sh + DESTINATION lib/${PROJECT_NAME} ) -add_executable(gcs_bridge - src/gcs_bridge.cpp +install(DIRECTORY include/ + DESTINATION include ) -target_link_libraries(gcs_bridge - mavros - ${catkin_LIBRARIES} + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} ) -############# -## Install ## -############# +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(ament_cmake_pytest REQUIRED) -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + # NOTE(vooon): explicit call to find dependencies for ament_lint + # ament_find_gtest() + # ament_find_gmock() -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -file(GLOB SCRIPTS ${PROJECT_SOURCE_DIR}/scripts/*) -catkin_install_python(PROGRAMS - ${SCRIPTS} - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + # NOTE(vooon): without that cppcheck fails on tests cpp files + # list(APPEND ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS "${GMOCK_INCLUDE_DIRS}") + # list(APPEND ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS "${GTEST_INCLUDE_DIRS}") + # list(APPEND ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS "${EIGEN3_INCLUDE_DIRS}") + # set(ament_cmake_cppcheck_LANGUAGE "c++") -## Mark executables and/or libraries for installation -install(TARGETS gcs_bridge mavros mavros_node mavros_plugins - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + # NOTE(vooon): tired to provide more and more macro + list(APPEND ament_cmake_cppcheck_ADDITIONAL_EXCLUDE "./test/*.cpp") -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" -) + # NOTE(vooon): cpplint complains about generated lines too long. Nothing more. + # XXX(vooon): don't work for Foxy! + # list(APPEND ament_cmake_cpplint_ADDITIONAL_EXCLUDE "./src/lib/enum_to_string.cpp") -## Mark other files for installation (e.g. launch and bag files, etc.) -install(FILES - mavros_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) + find_package(ament_lint_auto REQUIRED) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) + # NOTE(vooon): Does not support our custom triple-license, tiered to make it to work. + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) -############# -## Testing ## -############# + ament_lint_auto_find_test_dependencies() -if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(libmavros-frame-conversions-test test/test_frame_conversions.cpp) + ament_add_gtest(libmavros-frame-conversions-test test/test_frame_conversions.cpp) target_link_libraries(libmavros-frame-conversions-test mavros) + #ament_target_dependencies(libmavros-frame-conversions-test mavros) - catkin_add_gtest(libmavros-sensor-orientation-test test/test_sensor_orientation.cpp) + ament_add_gtest(libmavros-sensor-orientation-test test/test_sensor_orientation.cpp) target_link_libraries(libmavros-sensor-orientation-test mavros) + #ament_target_dependencies(libmavros-sensor-orientation-test mavros) - catkin_add_gtest(libmavros-quaternion-utils-test test/test_quaternion_utils.cpp) + ament_add_gtest(libmavros-quaternion-utils-test test/test_quaternion_utils.cpp) target_link_libraries(libmavros-quaternion-utils-test mavros) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) + #ament_target_dependencies(libmavros-quaternion-utils-test mavros) + + ament_add_gmock(mavros-router-test test/test_router.cpp) + target_link_libraries(mavros-router-test mavros) + ament_target_dependencies(mavros-router-test mavros_msgs) + + ament_add_gmock(mavros-uas-test test/test_uas.cpp) + target_link_libraries(mavros-uas-test mavros) + ament_target_dependencies(mavros-uas-test mavros_msgs) + ament_add_pytest_test(mavros_py_test test/mavros_py + PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + ) endif() +#ament_export_dependencies(console_bridge) +ament_export_include_directories(include) +ament_export_libraries(mavros) +ament_export_dependencies(eigen3_cmake_module) +ament_export_dependencies(Eigen3) +#ament_export_targets(mavros_node) +ament_package() + # vim: ts=2 sw=2 et: diff --git a/mavros/README.md b/mavros/README.md index 8319644c5..d63710b3d 100644 --- a/mavros/README.md +++ b/mavros/README.md @@ -1,8 +1,8 @@ MAVROS ====== -MAVLink extendable communication node for ROS -with proxy for Ground Control Station (e.g. [QGroundControl][qgc]). +MAVLink extendable communication node for ROS2. + ROS API documentation moved to [wiki.ros.org][wiki]. @@ -23,8 +23,7 @@ Features Limitations ----------- -Only for Linux. Depends on [Boost library][boost], GCC 4.8+ (C++11 support). -Catkin build system required. +Only for Linux. This package are dependent on [ros-\*-mavlink][mlwiki] build from [mavlink-gbp-release][mlgbp]. It exists in ROS package index and usually updates each month. @@ -82,33 +81,40 @@ given that: `global_position` plugin. -Programs --------- +Composite nodes +--------------- -### mavros\_node -- main communication node +See also: https://docs.ros.org/en/foxy/Tutorials/Composition.html -Main node. Allow disable GCS proxy by setting empty URL. +### mavros::router::Router + +This is router node required to support connections to FCU(s), GCS(es) and UAS nodes. +The Router allows you to add/remove endpoints on the fly without node restart. -Run example (autopilot connected via USB at 921600 baud, GCS running on the host with IP 172.16.254.1): +### mavros::uas::UAS - rosrun mavros mavros_node _fcu_url:=/dev/ttyACM0:921600 _gcs_url:=udp://@172.16.254.1 +This node is a plugin container which manages all protocol plugins. +Each plugin is a subnode to this. -### gcs\_bridge -- additional proxy -Allows you to add a channel for GCS. -For example if you need to connect one GCS for HIL and the second on the tablet. +Programs +-------- -Example (SITL & QGroundControl): +### mavros\_node -- all-in-one container - rosrun mavros mavros_node _gcs_url:='udp://:14556@172.16.254.129:14551' & - rosrun mavros gcs_bridge _gcs_url:='udp://@172.16.254.129' +That is a preconfigured composite node contaier, which provides similar parameters as ROS1 mavros\_node. +That container loads Router, UAS and configures them to work together (sets uas\_link, etc.). +Main node. Allow disable GCS proxy by setting empty URL. + ros2 run mavros mavros_node --ros-args --params-file params.yaml Launch Files ------------ +**XXX TODO**! #1564 + Launch files are provided for use with common FCUs, in particular [Pixhawk](pixhawk): * [px4.launch](launch/px4.launch) -- for use with the PX4 Autopilot (for VTOL, multicopters and planes) @@ -157,58 +163,57 @@ respect ROS msg API. Not having the dataset available will shutdown the `mavros_ ROS repository has binary packages for Ubuntu x86, amd64 (x86\_64) and armhf (ARMv7). Kinetic also support Debian Jessie amd64 and arm64 (ARMv8). -Just use `apt-get` for installation: +Just use `apt` for installation: - sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras + sudo apt install ros-foxy-mavros Then install GeographicLib datasets by running the `install_geographiclib_datasets.sh` script: - wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh + ros2 run mavros install_geographiclib_datasets.sh + + # Alternative: + wget https://raw.githubusercontent.com/mavlink/mavros/ros2/mavros/scripts/install_geographiclib_datasets.sh ./install_geographiclib_datasets.sh ### Source installation -Use `wstool` utility for retrieving sources and [`catkin` tool][catkin] for build. +Use `vcs` utility for retrieving sources and `colcon` tool for build. -NOTE: The source installation instructions are for the ROS Kinetic release. +NOTE: The source installation instructions are for the ROS Foxy release. ```sh -sudo apt-get install python-catkin-tools python-rosinstall-generator -y -# For Noetic use that: -# sudo apt install python3-catkin-tools python3-rosinstall-generator python3-osrf-pycommon -y +sudo apt install -y python3-vcstool python3-rosinstall-generator python3-osrf-pycommon # 1. Create the workspace: unneeded if you already has workspace -mkdir -p ~/catkin_ws/src -cd ~/catkin_ws -catkin init -wstool init src +mkdir -p ~/ros2_ws/src +cd ~/ros2_ws # 2. Install MAVLink # we use the Kinetic reference for all ROS distros as it's not distro-specific and up to date -rosinstall_generator --rosdistro kinetic mavlink | tee /tmp/mavros.rosinstall +rosinstall_generator --format repos mavlink | tee /tmp/mavlink.repos # 3. Install MAVROS: get source (upstream - released) -rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall +rosinstall_generator --format repos --upstream mavros | tee -a /tmp/mavros.repos # alternative: latest source -# rosinstall_generator --upstream-development mavros | tee -a /tmp/mavros.rosinstall -# For fetching all the dependencies into your catkin_ws, just add '--deps' to the above scripts -# ex: rosinstall_generator --upstream mavros --deps | tee -a /tmp/mavros.rosinstall +# rosinstall_generator --format repos --upstream-development mavros | tee -a /tmp/mavros.repos +# For fetching all the dependencies into your ros2_ws, just add '--deps' to the above scripts +# ex: rosinstall_generator --format repos --upstream mavros --deps | tee -a /tmp/mavros.repos # 4. Create workspace & deps -wstool merge -t src /tmp/mavros.rosinstall -wstool update -t src -j4 +vcs import src < /tmp/mavlink.repos +vcs import src < /tmp/mavros.repos rosdep install --from-paths src --ignore-src -y # 5. Install GeographicLib datasets: ./src/mavros/mavros/scripts/install_geographiclib_datasets.sh # 6. Build source -catkin build +colcon build # 7. Make sure that you use setup.bash or setup.zsh from workspace. -# Else rosrun can't find nodes from this workspace. -source devel/setup.bash +# Else ros2 run can't find nodes from this workspace. +source install/setup.bash ``` *Build error*. if you has error with missing `mavlink*` then you need fresh mavlink package. diff --git a/mavros/include/mavros/frame_tf.h b/mavros/include/mavros/frame_tf.hpp similarity index 50% rename from mavros/include/mavros/frame_tf.h rename to mavros/include/mavros/frame_tf.hpp index bdd32f715..54644e82d 100644 --- a/mavros/include/mavros/frame_tf.h +++ b/mavros/include/mavros/frame_tf.hpp @@ -1,6 +1,14 @@ +/* + * Copyright 2016,2017,2021 Vladimir Ermakov. + * Copyright 2017,2018 Nuno Marques. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Frame transformation utilities - * @file frame_tf.h + * @file frame_tf.hpp * @author Vladimir Ermakov * @author Eddy Scott * @author Nuno Marques @@ -8,73 +16,82 @@ * @addtogroup nodelib * @{ */ -/* - * Copyright 2016,2017 Vladimir Ermakov. - * Copyright 2017,2018 Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ #pragma once +#ifndef MAVROS__FRAME_TF_HPP_ +#define MAVROS__FRAME_TF_HPP_ + #include -#include -#include -#include +#include +#include // NOLINT +#include // NOLINT +#include // NOLINT // for Covariance types -#include -#include -#include -#include -#include - -namespace mavros { -namespace ftf { +#include "sensor_msgs/msg/imu.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose_with_covariance.hpp" + +namespace mavros +{ +namespace ftf +{ //! Type matching rosmsg for 3x3 covariance matrix -using Covariance3d = sensor_msgs::Imu::_angular_velocity_covariance_type; +using Covariance3d = sensor_msgs::msg::Imu::_angular_velocity_covariance_type; //! Type matching rosmsg for 6x6 covariance matrix -using Covariance6d = geometry_msgs::PoseWithCovariance::_covariance_type; +using Covariance6d = geometry_msgs::msg::PoseWithCovariance::_covariance_type; //! Type matching rosmsg for 9x9 covariance matrix -using Covariance9d = boost::array; +using Covariance9d = std::array; //! Eigen::Map for Covariance3d -using EigenMapCovariance3d = Eigen::Map >; -using EigenMapConstCovariance3d = Eigen::Map >; +using EigenMapCovariance3d = Eigen::Map>; +using EigenMapConstCovariance3d = Eigen::Map>; //! Eigen::Map for Covariance6d -using EigenMapCovariance6d = Eigen::Map >; -using EigenMapConstCovariance6d = Eigen::Map >; +using EigenMapCovariance6d = Eigen::Map>; +using EigenMapConstCovariance6d = Eigen::Map>; //! Eigen::Map for Covariance9d -using EigenMapCovariance9d = Eigen::Map >; -using EigenMapConstCovariance9d = Eigen::Map >; +using EigenMapCovariance9d = Eigen::Map>; +using EigenMapConstCovariance9d = Eigen::Map>; /** * @brief Orientation transform options when applying rotations to data */ -enum class StaticTF { - NED_TO_ENU, //!< will change orientation from being expressed WRT NED frame to WRT ENU frame - ENU_TO_NED, //!< change from expressed WRT ENU frame to WRT NED frame - AIRCRAFT_TO_BASELINK, //!< change from expressed WRT aircraft frame to WRT to baselink frame - BASELINK_TO_AIRCRAFT, //!< change from expressed WRT baselnk to WRT aircraft - ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK,//!< change orientation from being expressed in aircraft frame to baselink frame in an absolute frame of reference. - ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT,//!< change orientation from being expressed in baselink frame to aircraft frame in an absolute frame of reference +enum class StaticTF +{ + //! will change orientation from being expressed WRT NED frame to WRT ENU frame + NED_TO_ENU, + //! change from expressed WRT ENU frame to WRT NED frame + ENU_TO_NED, + //! change from expressed WRT aircraft frame to WRT to baselink frame + AIRCRAFT_TO_BASELINK, + //! change from expressed WRT baselnk to WRT aircraft + BASELINK_TO_AIRCRAFT, + //! change orientation from being expressed in aircraft frame to + // baselink frame in an absolute frame of reference. + ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK, + //! change orientation from being expressed in baselink frame to + // aircraft frame in an absolute frame of reference + ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT, }; /** * @brief Orientation transform options when applying rotations to data, for ECEF. */ -enum class StaticEcefTF { - ECEF_TO_ENU, //!< change from expressed WRT ECEF frame to WRT ENU frame - ENU_TO_ECEF //!< change from expressed WRT ENU frame to WRT ECEF frame +enum class StaticEcefTF +{ + ECEF_TO_ENU, //!< change from expressed WRT ECEF frame to WRT ENU frame + ENU_TO_ECEF //!< change from expressed WRT ENU frame to WRT ECEF frame }; -namespace detail { +namespace detail +{ /** * @brief Transform representation of attitude from 1 frame to another * (e.g. transfrom attitude from representing from base_link -> NED @@ -82,63 +99,63 @@ namespace detail { * * General function. Please use specialized enu-ned and ned-enu variants. */ -Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const StaticTF transform); +Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond & q, const StaticTF transform); /** * @brief Transform data expressed in one frame to another frame. * * General function. Please use specialized enu-ned and ned-enu variants. */ -Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q); +Eigen::Vector3d transform_frame(const Eigen::Vector3d & vec, const Eigen::Quaterniond & q); /** * @brief Transform 3x3 convariance expressed in one frame to another * * General function. Please use specialized enu-ned and ned-enu variants. */ -Covariance3d transform_frame(const Covariance3d &cov, const Eigen::Quaterniond &q); +Covariance3d transform_frame(const Covariance3d & cov, const Eigen::Quaterniond & q); /** * @brief Transform 6x6 convariance expressed in one frame to another * * General function. Please use specialized enu-ned and ned-enu variants. */ -Covariance6d transform_frame(const Covariance6d &cov, const Eigen::Quaterniond &q); +Covariance6d transform_frame(const Covariance6d & cov, const Eigen::Quaterniond & q); /** * @brief Transform 9x9 convariance expressed in one frame to another * * General function. Please use specialized enu-ned and ned-enu variants. */ -Covariance9d transform_frame(const Covariance9d &cov, const Eigen::Quaterniond &q); +Covariance9d transform_frame(const Covariance9d & cov, const Eigen::Quaterniond & q); /** * @brief Transform data expressed in one frame to another frame. * * General function. Please use specialized variants. */ -Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticTF transform); +Eigen::Vector3d transform_static_frame(const Eigen::Vector3d & vec, const StaticTF transform); /** * @brief Transform 3d convariance expressed in one frame to another * * General function. Please use specialized enu-ned and ned-enu variants. */ -Covariance3d transform_static_frame(const Covariance3d &cov, const StaticTF transform); +Covariance3d transform_static_frame(const Covariance3d & cov, const StaticTF transform); /** * @brief Transform 6d convariance expressed in one frame to another * * General function. Please use specialized enu-ned and ned-enu variants. */ -Covariance6d transform_static_frame(const Covariance6d &cov, const StaticTF transform); +Covariance6d transform_static_frame(const Covariance6d & cov, const StaticTF transform); /** * @brief Transform 9d convariance expressed in one frame to another * * General function. Please use specialized enu-ned and ned-enu variants. */ -Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF transform); +Covariance9d transform_static_frame(const Covariance9d & cov, const StaticTF transform); /** * @brief Transform data expressed in one frame to another frame @@ -146,9 +163,12 @@ Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF tran * * General function. Please use specialized variants. */ -Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, const StaticEcefTF transform); +Eigen::Vector3d transform_static_frame( + const Eigen::Vector3d & vec, + const Eigen::Vector3d & map_origin, + const StaticEcefTF transform); -} // namespace detail +} // namespace detail // -*- frame tf -*- @@ -157,8 +177,9 @@ Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen:: * represented WRT ENU frame */ template -inline T transform_orientation_ned_enu(const T &in) { - return detail::transform_orientation(in, StaticTF::NED_TO_ENU); +inline T transform_orientation_ned_enu(const T & in) +{ + return detail::transform_orientation(in, StaticTF::NED_TO_ENU); } /** @@ -166,8 +187,9 @@ inline T transform_orientation_ned_enu(const T &in) { * attitude represented WRT NED frame */ template -inline T transform_orientation_enu_ned(const T &in) { - return detail::transform_orientation(in, StaticTF::ENU_TO_NED); +inline T transform_orientation_enu_ned(const T & in) +{ + return detail::transform_orientation(in, StaticTF::ENU_TO_NED); } /** @@ -175,8 +197,9 @@ inline T transform_orientation_enu_ned(const T &in) { * attitude represented WRT base_link frame */ template -inline T transform_orientation_aircraft_baselink(const T &in) { - return detail::transform_orientation(in, StaticTF::AIRCRAFT_TO_BASELINK); +inline T transform_orientation_aircraft_baselink(const T & in) +{ + return detail::transform_orientation(in, StaticTF::AIRCRAFT_TO_BASELINK); } /** @@ -184,36 +207,40 @@ inline T transform_orientation_aircraft_baselink(const T &in) { * attitude represented WRT body frame */ template -inline T transform_orientation_baselink_aircraft(const T &in) { - return detail::transform_orientation(in, StaticTF::BASELINK_TO_AIRCRAFT); +inline T transform_orientation_baselink_aircraft(const T & in) +{ + return detail::transform_orientation(in, StaticTF::BASELINK_TO_AIRCRAFT); } /** * @brief Transform from attitude represented WRT aircraft frame to - * attitude represented WRT base_link frame, treating aircraft frame + * attitude represented WRT base_link frame, treating aircraft frame * as in an absolute frame of reference (local NED). */ template -inline T transform_orientation_absolute_frame_aircraft_baselink(const T &in) { - return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK); +inline T transform_orientation_absolute_frame_aircraft_baselink(const T & in) +{ + return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK); } /** * @brief Transform from attitude represented WRT baselink frame to - * attitude represented WRT body frame, treating baselink frame + * attitude represented WRT body frame, treating baselink frame * as in an absolute frame of reference (local NED). */ template -inline T transform_orientation_absolute_frame_baselink_aircraft(const T &in) { - return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT); +inline T transform_orientation_absolute_frame_baselink_aircraft(const T & in) +{ + return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT); } /** * @brief Transform data expressed in NED to ENU frame. */ template -inline T transform_frame_ned_enu(const T &in) { - return detail::transform_static_frame(in, StaticTF::NED_TO_ENU); +inline T transform_frame_ned_enu(const T & in) +{ + return detail::transform_static_frame(in, StaticTF::NED_TO_ENU); } /** @@ -221,8 +248,9 @@ inline T transform_frame_ned_enu(const T &in) { * */ template -inline T transform_frame_enu_ned(const T &in) { - return detail::transform_static_frame(in, StaticTF::ENU_TO_NED); +inline T transform_frame_enu_ned(const T & in) +{ + return detail::transform_static_frame(in, StaticTF::ENU_TO_NED); } /** @@ -230,8 +258,9 @@ inline T transform_frame_enu_ned(const T &in) { * */ template -inline T transform_frame_aircraft_baselink(const T &in) { - return detail::transform_static_frame(in, StaticTF::AIRCRAFT_TO_BASELINK); +inline T transform_frame_aircraft_baselink(const T & in) +{ + return detail::transform_static_frame(in, StaticTF::AIRCRAFT_TO_BASELINK); } /** @@ -239,8 +268,9 @@ inline T transform_frame_aircraft_baselink(const T &in) { * */ template -inline T transform_frame_baselink_aircraft(const T &in) { - return detail::transform_static_frame(in, StaticTF::BASELINK_TO_AIRCRAFT); +inline T transform_frame_baselink_aircraft(const T & in) +{ + return detail::transform_static_frame(in, StaticTF::BASELINK_TO_AIRCRAFT); } /** @@ -251,8 +281,9 @@ inline T transform_frame_baselink_aircraft(const T &in) { * @returns local ENU coordinates [m]. */ template -inline T transform_frame_ecef_enu(const T &in, const T &map_origin) { - return detail::transform_static_frame(in, map_origin, StaticEcefTF::ECEF_TO_ENU); +inline T transform_frame_ecef_enu(const T & in, const T & map_origin) +{ + return detail::transform_static_frame(in, map_origin, StaticEcefTF::ECEF_TO_ENU); } /** @@ -263,8 +294,9 @@ inline T transform_frame_ecef_enu(const T &in, const T &map_origin) { * @returns local ECEF coordinates [m]. */ template -inline T transform_frame_enu_ecef(const T &in, const T &map_origin) { - return detail::transform_static_frame(in, map_origin, StaticEcefTF::ENU_TO_ECEF); +inline T transform_frame_enu_ecef(const T & in, const T & map_origin) +{ + return detail::transform_static_frame(in, map_origin, StaticEcefTF::ENU_TO_ECEF); } /** @@ -272,8 +304,9 @@ inline T transform_frame_enu_ecef(const T &in, const T &map_origin) { * Assumes quaternion represents rotation from aircraft frame to NED frame. */ template -inline T transform_frame_aircraft_ned(const T &in, const Eigen::Quaterniond &q) { - return detail::transform_frame(in, q); +inline T transform_frame_aircraft_ned(const T & in, const Eigen::Quaterniond & q) +{ + return detail::transform_frame(in, q); } /** @@ -281,8 +314,9 @@ inline T transform_frame_aircraft_ned(const T &in, const Eigen::Quaterniond &q) * Assumes quaternion represents rotation from NED to aircraft frame. */ template -inline T transform_frame_ned_aircraft(const T &in, const Eigen::Quaterniond &q) { - return detail::transform_frame(in, q); +inline T transform_frame_ned_aircraft(const T & in, const Eigen::Quaterniond & q) +{ + return detail::transform_frame(in, q); } /** @@ -290,8 +324,9 @@ inline T transform_frame_ned_aircraft(const T &in, const Eigen::Quaterniond &q) * Assumes quaternion represents rotation from aircraft frame to ENU frame. */ template -inline T transform_frame_aircraft_enu(const T &in, const Eigen::Quaterniond &q) { - return detail::transform_frame(in, q); +inline T transform_frame_aircraft_enu(const T & in, const Eigen::Quaterniond & q) +{ + return detail::transform_frame(in, q); } /** @@ -299,8 +334,9 @@ inline T transform_frame_aircraft_enu(const T &in, const Eigen::Quaterniond &q) * Assumes quaternion represents rotation from ENU to aircraft frame. */ template -inline T transform_frame_enu_aircraft(const T &in, const Eigen::Quaterniond &q) { - return detail::transform_frame(in, q); +inline T transform_frame_enu_aircraft(const T & in, const Eigen::Quaterniond & q) +{ + return detail::transform_frame(in, q); } /** @@ -308,8 +344,9 @@ inline T transform_frame_enu_aircraft(const T &in, const Eigen::Quaterniond &q) * Assumes quaternion represents rotation from ENU to base_link frame. */ template -inline T transform_frame_enu_baselink(const T &in, const Eigen::Quaterniond &q) { - return detail::transform_frame(in, q); +inline T transform_frame_enu_baselink(const T & in, const Eigen::Quaterniond & q) +{ + return detail::transform_frame(in, q); } /** @@ -317,8 +354,9 @@ inline T transform_frame_enu_baselink(const T &in, const Eigen::Quaterniond &q) * Assumes quaternion represents rotation from basel_link to ENU frame. */ template -inline T transform_frame_baselink_enu(const T &in, const Eigen::Quaterniond &q) { - return detail::transform_frame(in, q); +inline T transform_frame_baselink_enu(const T & in, const Eigen::Quaterniond & q) +{ + return detail::transform_frame(in, q); } // -*- utils -*- @@ -327,15 +365,18 @@ inline T transform_frame_baselink_enu(const T &in, const Eigen::Quaterniond &q) /** * @brief Convert euler angles to quaternion. */ -Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy); +Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d & rpy); /** * @brief Convert euler angles to quaternion. * * @return quaternion, same as @a tf::quaternionFromRPY() but in Eigen format. */ -inline Eigen::Quaterniond quaternion_from_rpy(const double roll, const double pitch, const double yaw) { - return quaternion_from_rpy(Eigen::Vector3d(roll, pitch, yaw)); +inline Eigen::Quaterniond quaternion_from_rpy( + const double roll, const double pitch, + const double yaw) +{ + return quaternion_from_rpy(Eigen::Vector3d(roll, pitch, yaw)); } /** @@ -343,17 +384,19 @@ inline Eigen::Quaterniond quaternion_from_rpy(const double roll, const double pi * * Reverse operation to @a quaternion_from_rpy() */ -Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q); +Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond & q); /** * @brief Convert quaternion to euler angles */ -inline void quaternion_to_rpy(const Eigen::Quaterniond &q, double &roll, double &pitch, double &yaw) +inline void quaternion_to_rpy( + const Eigen::Quaterniond & q, double & roll, double & pitch, + double & yaw) { - const auto rpy = quaternion_to_rpy(q); - roll = rpy.x(); - pitch = rpy.y(); - yaw = rpy.z(); + const auto rpy = quaternion_to_rpy(q); + roll = rpy.x(); + pitch = rpy.y(); + yaw = rpy.z(); } /** @@ -361,7 +404,7 @@ inline void quaternion_to_rpy(const Eigen::Quaterniond &q, double &roll, double * * Replacement function for @a tf::getYaw() */ -double quaternion_get_yaw(const Eigen::Quaterniond &q); +double quaternion_get_yaw(const Eigen::Quaterniond & q); /** * @brief Store Quaternion to MAVLink float[4] format @@ -369,50 +412,51 @@ double quaternion_get_yaw(const Eigen::Quaterniond &q); * MAVLink uses wxyz order, wile Eigen::Quaterniond uses xyzw internal order, * so it can't be stored to array using Eigen::Map. */ -template ::value, bool>::type = true> -inline void quaternion_to_mavlink(const Eigen::Quaternion<_Scalar> &q, std::array &qmsg) +template::value, bool> = true> +inline void quaternion_to_mavlink(const Eigen::Quaternion<_Scalar> & q, std::array & qmsg) { - qmsg[0] = q.w(); - qmsg[1] = q.x(); - qmsg[2] = q.y(); - qmsg[3] = q.z(); + qmsg[0] = q.w(); + qmsg[1] = q.x(); + qmsg[2] = q.y(); + qmsg[3] = q.z(); } /** * @brief Convert Mavlink float[4] quaternion to Eigen */ -inline Eigen::Quaterniond mavlink_to_quaternion(const std::array &q) +inline Eigen::Quaterniond mavlink_to_quaternion(const std::array & q) { - return Eigen::Quaterniond(q[0], q[1], q[2], q[3]); + return Eigen::Quaterniond(q[0], q[1], q[2], q[3]); } /** * @brief Convert covariance matrix to MAVLink float[n] format */ template -inline void covariance_to_mavlink(const T &cov, std::array &covmsg) +inline void covariance_to_mavlink(const T & cov, std::array & covmsg) { - std::copy(cov.cbegin(), cov.cend(), covmsg.begin()); + std::copy(cov.cbegin(), cov.cend(), covmsg.begin()); } /** * @brief Convert upper right triangular of a covariance matrix to MAVLink float[n] format */ template -inline void covariance_urt_to_mavlink(const T &covmap, std::array &covmsg) +inline void covariance_urt_to_mavlink(const T & covmap, std::array & covmsg) { - auto m = covmap; - std::size_t COV_SIZE = m.rows() * (m.rows() + 1) / 2; - ROS_ASSERT_MSG(COV_SIZE == ARR_SIZE, - "frame_tf: covariance matrix URT size (%lu) is different from Mavlink msg covariance field size (%lu)", - COV_SIZE, ARR_SIZE); - - auto out = covmsg.begin(); - - for (size_t x = 0; x < m.cols(); x++) { - for (size_t y = x; y < m.rows(); y++) - *out++ = m(y, x); - } + auto m = covmap; + std::size_t COV_SIZE = m.rows() * (m.rows() + 1) / 2; + rcpputils::assert_true( + COV_SIZE == ARR_SIZE, + "frame_tf: covariance matrix URT size is different from Mavlink msg covariance field size"); + + auto out = covmsg.begin(); + + for (Eigen::Index x = 0; x < m.cols(); x++) { + for (Eigen::Index y = x; y < m.rows(); y++) { + *out++ = m(y, x); + } + } } /** @@ -420,46 +464,54 @@ inline void covariance_urt_to_mavlink(const T &covmap, std::array full covariance matrix */ template -inline void mavlink_urt_to_covariance_matrix(const std::array &covmsg, T &covmat) +inline void mavlink_urt_to_covariance_matrix(const std::array & covmsg, T & covmat) { - std::size_t COV_SIZE = covmat.rows() * (covmat.rows() + 1) / 2; - ROS_ASSERT_MSG(COV_SIZE == ARR_SIZE, - "frame_tf: covariance matrix URT size (%lu) is different from Mavlink msg covariance field size (%lu)", - COV_SIZE, ARR_SIZE); - - auto in = covmsg.begin(); - - for (size_t x = 0; x < covmat.cols(); x++) { - for (size_t y = x; y < covmat.rows(); y++) { - covmat(x, y) = static_cast(*in++); - covmat(y, x) = covmat(x, y); - } - } + std::size_t COV_SIZE = covmat.rows() * (covmat.rows() + 1) / 2; + rcpputils::assert_true( + COV_SIZE == ARR_SIZE, + "frame_tf: covariance matrix URT size is different from Mavlink msg covariance field size"); + + auto in = covmsg.begin(); + + for (Eigen::Index x = 0; x < covmat.cols(); x++) { + for (Eigen::Index y = x; y < covmat.rows(); y++) { + covmat(x, y) = static_cast(*in++); + covmat(y, x) = covmat(x, y); + } + } } // [[[cog: // def make_to_eigen(te, tr, fields): -// cog.outl("""//! @brief Helper to convert common ROS geometry_msgs::{tr} to Eigen::{te}""".format(**locals())) -// cog.outl("""inline Eigen::{te} to_eigen(const geometry_msgs::{tr} r) {{""".format(**locals())) -// cog.outl("""\treturn Eigen::{te}({fl});""".format(te=te, fl=", ".join(["r." + f for f in fields]))) -// cog.outl("""}""") +// fl = ", ".join(["r." + f for f in fields]) +// cog.outl(f"""//! @brief Helper to convert common ROS geometry_msgs::{tr} to Eigen::{te}""") +// cog.outl(f"""inline Eigen::{te} to_eigen(const geometry_msgs::msg::{tr} r)""") +// cog.outl(f"""{{""") +// cog.outl(f""" return Eigen::{te}({fl});""") +// cog.outl(f"""}}""") // // make_to_eigen("Vector3d", "Point", "xyz") // make_to_eigen("Vector3d", "Vector3", "xyz") // make_to_eigen("Quaterniond", "Quaternion", "wxyz") // ]]] //! @brief Helper to convert common ROS geometry_msgs::Point to Eigen::Vector3d -inline Eigen::Vector3d to_eigen(const geometry_msgs::Point r) { - return Eigen::Vector3d(r.x, r.y, r.z); +inline Eigen::Vector3d to_eigen(const geometry_msgs::msg::Point r) +{ + return Eigen::Vector3d(r.x, r.y, r.z); } //! @brief Helper to convert common ROS geometry_msgs::Vector3 to Eigen::Vector3d -inline Eigen::Vector3d to_eigen(const geometry_msgs::Vector3 r) { - return Eigen::Vector3d(r.x, r.y, r.z); +inline Eigen::Vector3d to_eigen(const geometry_msgs::msg::Vector3 r) +{ + return Eigen::Vector3d(r.x, r.y, r.z); } //! @brief Helper to convert common ROS geometry_msgs::Quaternion to Eigen::Quaterniond -inline Eigen::Quaterniond to_eigen(const geometry_msgs::Quaternion r) { - return Eigen::Quaterniond(r.w, r.x, r.y, r.z); +inline Eigen::Quaterniond to_eigen(const geometry_msgs::msg::Quaternion r) +{ + return Eigen::Quaterniond(r.w, r.x, r.y, r.z); } -// [[[end]]] (checksum: 1b3ada1c4245d4e31dcae9768779b952) -} // namespace ftf -} // namespace mavros +// [[[end]]] (checksum: 2f12174368db2fc32ab814cb97b1bbec) + +} // namespace ftf +} // namespace mavros + +#endif // MAVROS__FRAME_TF_HPP_ diff --git a/mavros/include/mavros/mavlink_diag.h b/mavros/include/mavros/mavlink_diag.h deleted file mode 100644 index b2dff3930..000000000 --- a/mavros/include/mavros/mavlink_diag.h +++ /dev/null @@ -1,44 +0,0 @@ -/** - * @brief Mavlink diag class - * @file mavlink_diag.h - * @author Vladimir Ermakov - * - * @addtogroup nodelib - * @{ - */ -/* - * Copyright 2014 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include - -namespace mavros { -class MavlinkDiag : public diagnostic_updater::DiagnosticTask -{ -public: - explicit MavlinkDiag(std::string name); - - void run(diagnostic_updater::DiagnosticStatusWrapper &stat); - - void set_mavconn(const mavconn::MAVConnInterface::Ptr &link) { - weak_link = link; - } - - void set_connection_status(bool connected) { - is_connected = connected; - } - -private: - mavconn::MAVConnInterface::WeakPtr weak_link; - unsigned int last_drop_count; - std::atomic is_connected; -}; -}; // namespace mavros - diff --git a/mavros/include/mavros/mavros.h b/mavros/include/mavros/mavros.h deleted file mode 100644 index 0b4954902..000000000 --- a/mavros/include/mavros/mavros.h +++ /dev/null @@ -1,82 +0,0 @@ -/** - * @brief MavRos node implementation class - * @file mavros.h - * @author Vladimir Ermakov - * - * @addtogroup nodelib - * @{ - * @brief MAVROS node implementation - */ -/* - * Copyright 2014,2015 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace mavros { -/** - * @brief MAVROS node class - * - * This class implement mavros_node - */ -class MavRos -{ -public: - MavRos(); - ~MavRos() {}; - - void spin(); - -private: - ros::NodeHandle mavlink_nh; - // fcu_link stored in mav_uas - mavconn::MAVConnInterface::Ptr gcs_link; - bool gcs_quiet_mode; - ros::Time last_message_received_from_gcs; - ros::Duration conn_timeout; - - ros::Publisher mavlink_pub; - ros::Subscriber mavlink_sub; - - diagnostic_updater::Updater gcs_diag_updater; - MavlinkDiag fcu_link_diag; - MavlinkDiag gcs_link_diag; - - pluginlib::ClassLoader plugin_loader; - std::vector loaded_plugins; - - //! FCU link -> router -> plugin handler - std::unordered_map plugin_subscriptions; - - //! UAS object passed to all plugins - UAS mav_uas; - - //! fcu link -> ros - void mavlink_pub_cb(const mavlink::mavlink_message_t *mmsg, const mavconn::Framing framing); - //! ros -> fcu link - void mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg); - - //! message router - void plugin_route_cb(const mavlink::mavlink_message_t *mmsg, const mavconn::Framing framing); - - //! load plugin - void add_plugin(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist); - - //! start mavlink app on USB - void startup_px4_usb_quirk(); - void log_connect_change(bool connected); -}; -} // namespace mavros - diff --git a/mavros/include/mavros/mavros_plugin.h b/mavros/include/mavros/mavros_plugin.h deleted file mode 100644 index 8d8c2141b..000000000 --- a/mavros/include/mavros/mavros_plugin.h +++ /dev/null @@ -1,150 +0,0 @@ -/** - * @brief MAVROS Plugin base - * @file mavros_plugin.h - * @author Vladimir Ermakov - * - * @addtogroup plugin - * @{ - * @brief MAVROS Plugin system - */ -/* - * Copyright 2013,2014,2015 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace mavros { -namespace plugin { -using mavros::UAS; -typedef std::lock_guard lock_guard; -typedef std::unique_lock unique_lock; - -/** - * @brief MAVROS Plugin base class - */ -class PluginBase -{ -private: - PluginBase(const PluginBase&) = delete; - -public: - //! generic message handler callback - using HandlerCb = mavconn::MAVConnInterface::ReceivedCb; - //! Tuple: MSG ID, MSG NAME, message type into hash_code, message handler callback - using HandlerInfo = std::tuple; - //! Subscriptions vector - using Subscriptions = std::vector; - - // pluginlib return boost::shared_ptr - using Ptr = boost::shared_ptr; - using ConstPtr = boost::shared_ptr; - - virtual ~PluginBase() {}; - - /** - * @brief Plugin initializer - * - * @param[in] uas @p UAS instance - */ - virtual void initialize(UAS &uas) { - m_uas = &uas; - } - - /** - * @brief Return vector of MAVLink message subscriptions (handlers) - */ - virtual Subscriptions get_subscriptions() = 0; - -protected: - /** - * @brief Plugin constructor - * Should not do anything before initialize() - */ - PluginBase() : m_uas(nullptr) {}; - - UAS *m_uas; - - // TODO: filtered handlers - - /** - * Make subscription to raw message. - * - * @param[in] id message id - * @param[in] fn pointer to member function (handler) - */ - template - HandlerInfo make_handler(const mavlink::msgid_t id, void (_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing)) { - auto bfn = std::bind(fn, static_cast<_C*>(this), std::placeholders::_1, std::placeholders::_2); - const auto type_hash_ = typeid(mavlink::mavlink_message_t).hash_code(); - - return HandlerInfo{ id, nullptr, type_hash_, bfn }; - } - - /** - * Make subscription to message with automatic decoding. - * - * @param[in] fn pointer to member function (handler) - */ - template - HandlerInfo make_handler(void (_C::*fn)(const mavlink::mavlink_message_t*, _T &)) { - auto bfn = std::bind(fn, static_cast<_C*>(this), std::placeholders::_1, std::placeholders::_2); - const auto id = _T::MSG_ID; - const auto name = _T::NAME; - const auto type_hash_ = typeid(_T).hash_code(); - - return HandlerInfo{ - id, name, type_hash_, - [bfn](const mavlink::mavlink_message_t *msg, const mavconn::Framing framing) { - if (framing != mavconn::Framing::ok) - return; - - mavlink::MsgMap map(msg); - _T obj; - obj.deserialize(map); - - bfn(msg, obj); - } - }; - } - - /** - * Common callback called on connection change - */ - virtual void connection_cb(bool connected) { - ROS_BREAK(); - } - - /** - * Shortcut for connection_cb() registration - */ - inline void enable_connection_cb() { - m_uas->add_connection_change_handler(std::bind(&PluginBase::connection_cb, this, std::placeholders::_1)); - } - - /** - * Common callback called only when capabilities change - */ - virtual void capabilities_cb(UAS::MAV_CAP capabilities) { - ROS_BREAK(); - } - - /** - * Shortcut for capabilities_cb() registration - */ - void enable_capabilities_cb() { - m_uas->add_capabilities_change_handler(std::bind(&PluginBase::capabilities_cb, this, std::placeholders::_1)); - } -}; -} // namespace plugin -} // namespace mavros diff --git a/mavros/include/mavros/mavros_plugin_register_macro.hpp b/mavros/include/mavros/mavros_plugin_register_macro.hpp new file mode 100644 index 000000000..1fb4a4fa2 --- /dev/null +++ b/mavros/include/mavros/mavros_plugin_register_macro.hpp @@ -0,0 +1,35 @@ +/* + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief MAVROS Plugin register macro + * @file mavros_plugin_register_macro.hpp + * @author Vladimir Ermakov + * + * @addtogroup plugin + * @{ + * @brief MAVROS Plugin system + */ + +#pragma once + +#ifndef MAVROS__MAVROS_PLUGIN_REGISTER_MACRO_HPP_ +#define MAVROS__MAVROS_PLUGIN_REGISTER_MACRO_HPP_ + +#include +#include + +/** + * Register a plugin for mavros UAS. + */ +#define MAVROS_PLUGIN_REGISTER(PluginClass) \ + CLASS_LOADER_REGISTER_CLASS( \ + mavros::plugin::PluginFactoryTemplate, \ + mavros::plugin::PluginFactory) + + +#endif // MAVROS__MAVROS_PLUGIN_REGISTER_MACRO_HPP_ diff --git a/mavros/include/mavros/mavros_router.hpp b/mavros/include/mavros/mavros_router.hpp new file mode 100644 index 000000000..7d409f682 --- /dev/null +++ b/mavros/include/mavros/mavros_router.hpp @@ -0,0 +1,305 @@ +/* + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief MavRos node implementation class + * @file mavros_router.hpp + * @author Vladimir Ermakov + * + * @addtogroup nodelib + * @{ + * @brief MAVROS node implementation + */ + +#pragma once + +#ifndef MAVROS__MAVROS_ROUTER_HPP_ +#define MAVROS__MAVROS_ROUTER_HPP_ + +#include +#include +#include +#include +#include // NOLINT +#include +#include +#include +#include // NOLINT + +#include "mavconn/interface.hpp" +#include "mavconn/mavlink_dialect.hpp" +#include "mavros/utils.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/rclcpp.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" + +#include "mavros_msgs/msg/mavlink.hpp" +#include "mavros_msgs/srv/endpoint_add.hpp" +#include "mavros_msgs/srv/endpoint_del.hpp" + +namespace mavros +{ +namespace router +{ + +using id_t = uint32_t; +using addr_t = uint32_t; + +using mavconn::Framing; +using ::mavlink::mavlink_message_t; +using ::mavlink::msgid_t; + +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class Router; + +/** + * Endpoint base class + * + * Represents one network connection to the Router + * + * One endpoint could map to several remote devices, + * e.g. mesh radio for swarm. + */ +class Endpoint : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Endpoint) + + enum class Type + { + fcu = 0, + gcs = 1, + uas = 2, + }; + + Endpoint() + : parent(), + id(0), + link_type(Type::fcu), + url{}, + remote_addrs{}, + stale_addrs{} + { + const addr_t broadcast_addr = 0; + + // Accept broadcasts by default + remote_addrs.emplace(broadcast_addr); + } + + std::shared_ptr parent; + + uint32_t id; // id of the endpoint + Type link_type; // class of the endpoint + std::string url; // url to open that endpoint + std::set remote_addrs; // remotes that we heard there + std::set stale_addrs; // temporary storage for stale remote addrs + + virtual bool is_open() = 0; + virtual std::pair open() = 0; + virtual void close() = 0; + + virtual void send_message( + const mavlink_message_t * msg, const Framing framing = Framing::ok, + id_t src_id = 0) = 0; + virtual void recv_message(const mavlink_message_t * msg, const Framing framing = Framing::ok); + + virtual std::string diag_name(); + virtual void diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat) = 0; +}; + +/** + * Router class implements MAVROS Router node + * + * Router is essential part that connects several MAVLink devices together. + * In general there are three device classes that router uses: + * - a FCU endpoint - should be faced to autopilot firmware(s), + * - a GCS endpoint - that ep connects control software + * - a UAS endpoint - special type of endpoint that is used to connect MAVROS UAS node(s) to FCU(s) + * + * Some traffic rules: + * 1. FCU broadcast -> GCS, UAS, but not to any other FCU + * 2. FCU targeted system/component -> GCS/UAS endpoint that have matching address + * 3. FCU targeted system -> same as for p.2 + * 4. GCS broadcast -> FCU, UAS + * 5. GCS targeted -> FCU/UAS maching addr + * 6. UAS broadcast -> FCU, GCS + * 7. UAS targeted -> FCU/GCS + */ +class Router : public rclcpp::Node +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Router) + + using StrV = std::vector; + + explicit Router(const std::string & node_name = "mavros_router") + : Router(rclcpp::NodeOptions(), node_name) {} + + explicit Router( + const rclcpp::NodeOptions & options, + const std::string & node_name = "mavros_router") + : rclcpp::Node(node_name, + options /* rclcpp::NodeOptions(options).use_intra_process_comms(true) */), + endpoints{}, stat_msg_routed(0), stat_msg_sent(0), stat_msg_dropped(0), + diagnostic_updater(this, 1.0) + { + RCLCPP_DEBUG(this->get_logger(), "Start mavros::router::Router initialization..."); + + set_parameters_handle_ptr = + this->add_on_set_parameters_callback(std::bind(&Router::on_set_parameters_cb, this, _1)); + this->declare_parameter("fcu_urls", StrV()); + this->declare_parameter("gcs_urls", StrV()); + this->declare_parameter("uas_urls", StrV()); + + add_service = this->create_service( + "~/add_endpoint", + std::bind(&Router::add_endpoint, this, _1, _2)); + del_service = this->create_service( + "~/del_endpoint", + std::bind(&Router::del_endpoint, this, _1, _2)); + + // try to reconnect endpoint each 30 seconds + reconnect_timer = + this->create_wall_timer(30s, std::bind(&Router::periodic_reconnect_endpoints, this)); + + // collect garbage addrs each minute + stale_addrs_timer = + this->create_wall_timer(60s, std::bind(&Router::periodic_clear_stale_remote_addrs, this)); + + diagnostic_updater.setHardwareID("none"); // NOTE: router connects several hardwares + diagnostic_updater.add("MAVROS Router", this, &Router::diag_run); + + std::stringstream ss; + for (auto & s : mavconn::MAVConnInterface::get_known_dialects()) { + ss << " " << s; + } + + RCLCPP_INFO(get_logger(), "Built-in SIMD instructions: %s", Eigen::SimdInstructionSetsInUse()); + RCLCPP_INFO(get_logger(), "Built-in MAVLink package version: %s", mavlink::version); + RCLCPP_INFO(get_logger(), "Known MAVLink dialects:%s", ss.str().c_str()); + RCLCPP_INFO(get_logger(), "MAVROS Router started"); + } + + void route_message(Endpoint::SharedPtr src, const mavlink_message_t * msg, const Framing framing); + +private: + friend class Endpoint; + friend class TestRouter; + + static std::atomic id_counter; + + std::shared_timed_mutex mu; + + // map stores all routing endpoints + std::unordered_map endpoints; + + std::atomic stat_msg_routed; //!< amount of messages came to route_messages() + std::atomic stat_msg_sent; //!< amount of messages sent + std::atomic stat_msg_dropped; //!< amount of messages dropped + + rclcpp::Service::SharedPtr add_service; + rclcpp::Service::SharedPtr del_service; + rclcpp::TimerBase::SharedPtr reconnect_timer; + rclcpp::TimerBase::SharedPtr stale_addrs_timer; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr set_parameters_handle_ptr; + diagnostic_updater::Updater diagnostic_updater; + + void add_endpoint( + const mavros_msgs::srv::EndpointAdd::Request::SharedPtr request, + mavros_msgs::srv::EndpointAdd::Response::SharedPtr response); + void del_endpoint( + const mavros_msgs::srv::EndpointDel::Request::SharedPtr request, + mavros_msgs::srv::EndpointDel::Response::SharedPtr response); + + void periodic_reconnect_endpoints(); + void periodic_clear_stale_remote_addrs(); + + rcl_interfaces::msg::SetParametersResult on_set_parameters_cb( + const std::vector & parameters); + + void diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat); +}; + +/** + * MAVConnEndpoint implements Endpoint for FCU or GCS connection + * via good old libmavconn url's + * + * TODO(vooon): support multiple remotes on UDP, + * choose right TCP client instead of simple broadcast + * + * NOTE(vooon): do we still need PX4 USB quirk? + */ +class MAVConnEndpoint : public Endpoint +{ +public: + MAVConnEndpoint() + : Endpoint(), + stat_last_drop_count(0) + {} + + ~MAVConnEndpoint() + { + close(); + } + + mavconn::MAVConnInterface::Ptr link; // connection + size_t stat_last_drop_count; + + bool is_open() override; + std::pair open() override; + void close() override; + + void send_message( + const mavlink_message_t * msg, const Framing framing = Framing::ok, + id_t src_id = 0) override; + + void diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat) override; +}; + +/** + * ROSEndpoint implements Endpoint for UAS node + * + * That endpoint converts mavlink messages to ROS2 IDL + * and passes them trough DDL messaging or intra-process comms. + * + * Each drone would have separate UAS node + */ +class ROSEndpoint : public Endpoint +{ +public: + ROSEndpoint() + : Endpoint() + {} + + ~ROSEndpoint() + { + close(); + } + + rclcpp::Subscription::SharedPtr sink; // UAS -> FCU + rclcpp::Publisher::SharedPtr source; // FCU -> UAS + + bool is_open() override; + std::pair open() override; + void close() override; + + void send_message( + const mavlink_message_t * msg, const Framing framing = Framing::ok, + id_t src_id = 0) override; + + void diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat) override; + +private: + void ros_recv_message(const mavros_msgs::msg::Mavlink::SharedPtr rmsg); +}; + +} // namespace router +} // namespace mavros + +#endif // MAVROS__MAVROS_ROUTER_HPP_ diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h deleted file mode 100644 index d6d9044ac..000000000 --- a/mavros/include/mavros/mavros_uas.h +++ /dev/null @@ -1,476 +0,0 @@ -/** - * @brief MAVROS Plugin context - * @file mavros_uas.h - * @author Vladimir Ermakov - * @author Eddy Scott - * - * @addtogroup nodelib - * @{ - */ -/* - * Copyright 2014,2015,2016,2017 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - -namespace mavros { -/** - * @brief helper accessor to FCU link interface - */ -#define UAS_FCU(uasobjptr) \ - ((uasobjptr)->fcu_link) - -/** - * @brief helper accessor to diagnostic updater - */ -#define UAS_DIAG(uasobjptr) \ - ((uasobjptr)->diag_updater) - - -/** - * @brief UAS for plugins - * - * This class stores some useful data and - * provides fcu connection, mode stringify utilities. - * - * Currently it stores: - * - FCU link interface - * - FCU System & Component ID pair - * - Connection status (@a mavplugin::SystemStatusPlugin) - * - Autopilot type (@a mavplugin::SystemStatusPlugin) - * - Vehicle type (@a mavplugin::SystemStatusPlugin) - * - IMU data (@a mavplugin::IMUPubPlugin) - * - GPS data (@a mavplugin::GPSPlugin) - */ -class UAS { -public: - // common enums used by UAS - using MAV_TYPE = mavlink::minimal::MAV_TYPE; - using MAV_AUTOPILOT = mavlink::minimal::MAV_AUTOPILOT; - using MAV_MODE_FLAG = mavlink::minimal::MAV_MODE_FLAG; - using MAV_STATE = mavlink::minimal::MAV_STATE; - using MAV_CAP = mavlink::common::MAV_PROTOCOL_CAPABILITY; - using timesync_mode = utils::timesync_mode; - - // other UAS aliases - using ConnectionCb = std::function; - using CapabilitiesCb = std::function; - using lock_guard = std::lock_guard; - using unique_lock = std::unique_lock; - - UAS(); - ~UAS() {}; - - /** - * @brief MAVLink FCU device conection - */ - mavconn::MAVConnInterface::Ptr fcu_link; - - /** - * @brief Mavros diagnostic updater - */ - diagnostic_updater::Updater diag_updater; - - /** - * @brief Return connection status - */ - inline bool is_connected() { - return connected; - } - - /* -*- HEARTBEAT data -*- */ - - /** - * Update autopilot type on every HEARTBEAT - */ - void update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_); - - /** - * Update autopilot connection status (every HEARTBEAT/conn_timeout) - */ - void update_connection_status(bool conn_); - - /** - * @brief Add connection change handler callback - */ - void add_connection_change_handler(ConnectionCb cb); - - /** - * @brief Returns vehicle type - */ - inline MAV_TYPE get_type() { - std::underlying_type::type type_ = type; - return static_cast(type_); - } - - /** - * @brief Returns autopilot type - */ - inline MAV_AUTOPILOT get_autopilot() { - std::underlying_type::type autopilot_ = autopilot; - return static_cast(autopilot_); - } - - /** - * @brief Returns arming status - * - * @note There may be race condition between SET_MODE and HEARTBEAT. - */ - inline bool get_armed() { - uint8_t base_mode_ = base_mode; - return base_mode_ & utils::enum_value(MAV_MODE_FLAG::SAFETY_ARMED); - } - - /** - * @brief Returns HIL status - */ - inline bool get_hil_state() { - uint8_t base_mode_ = base_mode; - return base_mode_ & utils::enum_value(MAV_MODE_FLAG::HIL_ENABLED); - } - - /* -*- FCU target id pair -*- */ - - /** - * @brief Return communication target system - */ - inline uint8_t get_tgt_system() { - return target_system; // not changed after configuration - } - - /** - * @brief Return communication target component - */ - inline uint8_t get_tgt_component() { - return target_component;// not changed after configuration - } - - inline void set_tgt(uint8_t sys, uint8_t comp) { - target_system = sys; - target_component = comp; - } - - - /* -*- IMU data -*- */ - - /** - * @brief Store IMU data [ENU] - */ - void update_attitude_imu_enu(sensor_msgs::Imu::Ptr &imu); - - /** - * @brief Store IMU data [NED] - */ - void update_attitude_imu_ned(sensor_msgs::Imu::Ptr &imu); - - /** - * @brief Get IMU data [ENU] - */ - sensor_msgs::Imu::Ptr get_attitude_imu_enu(); - - /** - * @brief Get IMU data [NED] - */ - sensor_msgs::Imu::Ptr get_attitude_imu_ned(); - - /** - * @brief Get Attitude orientation quaternion - * @return orientation quaternion [ENU] - */ - geometry_msgs::Quaternion get_attitude_orientation_enu(); - - /** - * @brief Get Attitude orientation quaternion - * @return orientation quaternion [NED] - */ - geometry_msgs::Quaternion get_attitude_orientation_ned(); - - /** - * @brief Get angular velocity from IMU data - * @return vector3 [ENU] - */ - geometry_msgs::Vector3 get_attitude_angular_velocity_enu(); - - /** - * @brief Get angular velocity from IMU data - * @return vector3 [NED] - */ - geometry_msgs::Vector3 get_attitude_angular_velocity_ned(); - - - /* -*- GPS data -*- */ - - //! Store GPS RAW data - void update_gps_fix_epts(sensor_msgs::NavSatFix::Ptr &fix, - float eph, float epv, - int fix_type, int satellites_visible); - - //! Returns EPH, EPV, Fix type and satellites visible - void get_gps_epts(float &eph, float &epv, int &fix_type, int &satellites_visible); - - //! Retunrs last GPS RAW message - sensor_msgs::NavSatFix::Ptr get_gps_fix(); - - /* -*- GograpticLib utils -*- */ - - /** - * @brief Geoid dataset used to convert between AMSL and WGS-84 - * - * That class loads egm96_5 dataset to RAM, it is about 24 MiB. - */ - std::shared_ptr egm96_5; - - /** - * @brief Conversion from height above geoid (AMSL) - * to height above ellipsoid (WGS-84) - */ - template - inline double geoid_to_ellipsoid_height(T lla) - { - if (egm96_5) - return GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(lla->latitude, lla->longitude); - else - return 0.0; - } - - /** - * @brief Conversion from height above ellipsoid (WGS-84) - * to height above geoid (AMSL) - */ - template - inline double ellipsoid_to_geoid_height(T lla) - { - if (egm96_5) - return GeographicLib::Geoid::ELLIPSOIDTOGEOID * (*egm96_5)(lla->latitude, lla->longitude); - else - return 0.0; - } - - /* -*- transform -*- */ - - tf2_ros::Buffer tf2_buffer; - tf2_ros::TransformListener tf2_listener; - tf2_ros::TransformBroadcaster tf2_broadcaster; - tf2_ros::StaticTransformBroadcaster tf2_static_broadcaster; - - /** - * @brief Add static transform. To publish all static transforms at once, we stack them in a std::vector. - * - * @param frame_id parent frame for transform - * @param child_id child frame for transform - * @param tr transform - * @param vector vector of transforms - */ - void add_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr, std::vector& vector); - - /** - * @brief Publishes static transform. - * - * @param frame_id parent frame for transform - * @param child_id child frame for transform - * @param tr transform - */ - void publish_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr); - - /* -*- time sync -*- */ - - inline void set_time_offset(uint64_t offset_ns) { - time_offset = offset_ns; - } - - inline uint64_t get_time_offset(void) { - return time_offset; - } - - inline void set_timesync_mode(timesync_mode mode) { - tsync_mode = mode; - } - - inline timesync_mode get_timesync_mode(void) { - return tsync_mode; - } - - /* -*- autopilot version -*- */ - uint64_t get_capabilities(); - - /** - * @brief Function to check if the flight controller has a capability - * - * @param capabilities can accept a multiple capability params either in enum or int from - */ - template - bool has_capability(T capability){ - static_assert(std::is_enum::value, "Only query capabilities using the UAS::MAV_CAP enum."); - return get_capabilities() & utils::enum_value(capability); - } - - /** - * @brief Function to check if the flight controller has a set of capabilities - * - * @param capabilities can accept a multiple capability params either in enum or int from - */ - - template - bool has_capabilities(Ts ... capabilities){ - bool ret = true; - std::initializer_list capabilities_list{has_capability(capabilities) ...}; - for (auto has_cap : capabilities_list) ret &= has_cap; - return ret; - } - - /** - * @brief Update the capabilities if they've changed every VERSION/timeout - */ - void update_capabilities(bool known, uint64_t caps = 0); - - /** - * @brief Adds a function to the capabilities callback queue - * - * @param cb A void function that takes a single mavlink::common::MAV_PROTOCOL_CAPABILITY(MAV_CAP) param - */ - void add_capabilities_change_handler(CapabilitiesCb cb); - - /** - * @brief Compute FCU message time from time_boot_ms or time_usec field - * - * Uses time_offset for calculation - * - * @return FCU time if it is known else current wall time. - */ - ros::Time synchronise_stamp(uint32_t time_boot_ms); - ros::Time synchronise_stamp(uint64_t time_usec); - - /** - * @brief Create message header from time_boot_ms or time_usec stamps and frame_id. - * - * Setting frame_id and stamp are pretty common, this little helper should reduce LOC. - * - * @param[in] frame_id frame for header - * @param[in] time_stamp mavlink message time - * @return Header with syncronized stamp and frame id - */ - template - inline std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp) { - std_msgs::Header out; - out.frame_id = frame_id; - out.stamp = synchronise_stamp(time_stamp); - return out; - } - - /* -*- utils -*- */ - - /** - * Helper template to set target id's of message. - */ - template - inline void msg_set_target(_T &msg) { - msg.target_system = get_tgt_system(); - msg.target_component = get_tgt_component(); - } - - /** - * @brief Check that sys/comp id's is my target - */ - inline bool is_my_target(uint8_t sysid, uint8_t compid) { - return sysid == get_tgt_system() && compid == get_tgt_component(); - } - - /** - * @brief Check that system id is my target - */ - inline bool is_my_target(uint8_t sysid) { - return sysid == get_tgt_system(); - } - - /** - * @brief Check that FCU is APM - */ - inline bool is_ardupilotmega() { - return MAV_AUTOPILOT::ARDUPILOTMEGA == get_autopilot(); - } - - /** - * @brief Check that FCU is PX4 - */ - inline bool is_px4() { - return MAV_AUTOPILOT::PX4 == get_autopilot(); - } - - /** - * @brief Represent FCU mode as string - * - * Port pymavlink mavutil.mode_string_v10 - * - * Supported FCU's: - * - APM:Plane - * - APM:Copter - * - PX4 - * - * @param[in] base_mode base mode - * @param[in] custom_mode custom mode data - */ - std::string str_mode_v10(uint8_t base_mode, uint32_t custom_mode); - - /** - * @brief Lookup custom mode for given string - * - * Complimentary to @a str_mode_v10() - * - * @param[in] cmode_str string representation of mode - * @param[out] custom_mode decoded value - * @return true if success - */ - bool cmode_from_str(std::string cmode_str, uint32_t &custom_mode); - -private: - std::recursive_mutex mutex; - - std::atomic type; - std::atomic autopilot; - std::atomic base_mode; - - uint8_t target_system; - uint8_t target_component; - - std::atomic connected; - std::vector connection_cb_vec; - std::vector capabilities_cb_vec; - - sensor_msgs::Imu::Ptr imu_enu_data; - sensor_msgs::Imu::Ptr imu_ned_data; - - sensor_msgs::NavSatFix::Ptr gps_fix; - float gps_eph; - float gps_epv; - int gps_fix_type; - int gps_satellites_visible; - - std::atomic time_offset; - timesync_mode tsync_mode; - - std::atomic fcu_caps_known; - std::atomic fcu_capabilities; -}; -} // namespace mavros diff --git a/mavros/include/mavros/mavros_uas.hpp b/mavros/include/mavros/mavros_uas.hpp new file mode 100644 index 000000000..c6fb863ed --- /dev/null +++ b/mavros/include/mavros/mavros_uas.hpp @@ -0,0 +1,641 @@ +/* + * Copyright 2014,2015,2016,2017,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief MAVROS UAS Node + * @file mavros_uas.hpp + * @author Vladimir Ermakov + * @author Eddy Scott + * + * @addtogroup nodelib + * @{ + */ + +#pragma once + +#ifndef MAVROS__MAVROS_UAS_HPP_ +#define MAVROS__MAVROS_UAS_HPP_ + +#include // NOLINT +#include // NOLINT +#include // NOLINT +#include // NOLINT + +#include +#include +#include +#include +#include +#include +#include + +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "mavconn/interface.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/rclcpp.hpp" +#include "GeographicLib/Geoid.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" + +#include "mavros/utils.hpp" +#include "mavros/plugin.hpp" +#include "mavros/frame_tf.hpp" +#include "mavros/uas_executor.hpp" + +namespace mavros +{ +namespace uas +{ + +//! default source component +static constexpr auto MAV_COMP_ID_ONBOARD_COMPUTER = 191; + +using s_unique_lock = std::unique_lock; +using s_shared_lock = std::shared_lock; + +// common enums used by UAS +using MAV_TYPE = mavlink::minimal::MAV_TYPE; +using MAV_AUTOPILOT = mavlink::minimal::MAV_AUTOPILOT; +using MAV_MODE_FLAG = mavlink::minimal::MAV_MODE_FLAG; +using MAV_STATE = mavlink::minimal::MAV_STATE; +using MAV_CAP = mavlink::common::MAV_PROTOCOL_CAPABILITY; +using timesync_mode = utils::timesync_mode; + + +/** + * @brief UAS Node data + * + * This class stores some useful data; + * + * Currently it stores: + * - IMU data (@a mavplugin::IMUPubPlugin) + * - GPS data (@a mavplugin::GPSPlugin) + */ +class Data +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Data(); + ~Data() = default; + + /* -*- IMU data -*- */ + + /** + * @brief Store IMU data [ENU] + */ + void update_attitude_imu_enu(const sensor_msgs::msg::Imu & imu); + + /** + * @brief Store IMU data [NED] + */ + void update_attitude_imu_ned(const sensor_msgs::msg::Imu & imu); + + /** + * @brief Get IMU data [ENU] + */ + sensor_msgs::msg::Imu get_attitude_imu_enu(); + + /** + * @brief Get IMU data [NED] + */ + sensor_msgs::msg::Imu get_attitude_imu_ned(); + + /** + * @brief Get Attitude orientation quaternion + * @return orientation quaternion [ENU] + */ + geometry_msgs::msg::Quaternion get_attitude_orientation_enu(); + + /** + * @brief Get Attitude orientation quaternion + * @return orientation quaternion [NED] + */ + geometry_msgs::msg::Quaternion get_attitude_orientation_ned(); + + /** + * @brief Get angular velocity from IMU data + * @return vector3 [ENU] + */ + geometry_msgs::msg::Vector3 get_attitude_angular_velocity_enu(); + + /** + * @brief Get angular velocity from IMU data + * @return vector3 [NED] + */ + geometry_msgs::msg::Vector3 get_attitude_angular_velocity_ned(); + + + /* -*- GPS data -*- */ + + //! Store GPS RAW data + void update_gps_fix_epts( + const sensor_msgs::msg::NavSatFix & fix, + float eph, float epv, + int fix_type, int satellites_visible); + + //! Returns EPH, EPV, Fix type and satellites visible + void get_gps_epts(float & eph, float & epv, int & fix_type, int & satellites_visible); + + //! Retunrs last GPS RAW message + sensor_msgs::msg::NavSatFix get_gps_fix(); + + /* -*- GograpticLib utils -*- */ + + /** + * @brief Geoid dataset used to convert between AMSL and WGS-84 + * + * That class loads egm96_5 dataset to RAM, it is about 24 MiB. + */ + static std::shared_ptr egm96_5; + + /** + * @brief Conversion from height above geoid (AMSL) + * to height above ellipsoid (WGS-84) + */ + template::value, bool> = true> + inline double geoid_to_ellipsoid_height(const T lla) + { + if (egm96_5) { + return GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(lla->latitude, lla->longitude); + } else { + return 0.0; + } + } + + template::value, bool> = true> + inline double geoid_to_ellipsoid_height(const T & lla) + { + return geoid_to_ellipsoid_height(&lla); + } + + /** + * @brief Conversion from height above ellipsoid (WGS-84) + * to height above geoid (AMSL) + */ + template::value, bool> = true> + inline double ellipsoid_to_geoid_height(const T lla) + { + if (egm96_5) { + return GeographicLib::Geoid::ELLIPSOIDTOGEOID * (*egm96_5)(lla->latitude, lla->longitude); + } else { + return 0.0; + } + } + + template::value, bool> = true> + inline double ellipsoid_to_geoid_height(const T & lla) + { + return ellipsoid_to_geoid_height(&lla); + } + +private: + std::shared_timed_mutex mu; + + sensor_msgs::msg::Imu imu_enu_data; + sensor_msgs::msg::Imu imu_ned_data; + + sensor_msgs::msg::NavSatFix gps_fix; + float gps_eph; + float gps_epv; + int gps_fix_type; + int gps_satellites_visible; + + //! init_geographiclib() once flag + static std::once_flag init_flag; + + //! Initialize egm96-5 + static void init_geographiclib(); +}; + +/** + * @brief UAS Node + * + * This class implements main translation mode. + * It loads plugins to support various sub-protocols. + * + * Also each plugin can use that node to create sub-nodes or other RCLCPP objects. + * + * UAS Node provides: + * - FCU messaging link + * - FCU System & Component ID pair + * - Connection status (@a mavplugin::SystemStatusPlugin) + * - Autopilot type (@a mavplugin::SystemStatusPlugin) + * - Vehicle type (@a mavplugin::SystemStatusPlugin) + * - Additional data trough mavros::uas::Data class + */ +class UAS : public rclcpp::Node +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(UAS) + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + // other UAS aliases + using ConnectionCb = std::function; + using CapabilitiesCb = std::function; + + using StrV = std::vector; + + explicit UAS(const std::string & name_ = "mavros") + : UAS(rclcpp::NodeOptions(), name_) {} + + explicit UAS( + const rclcpp::NodeOptions & options_ = rclcpp::NodeOptions(), + const std::string & name_ = "mavros", + const std::string & uas_url_ = "/uas1", uint8_t target_system_ = 1, + uint8_t target_component_ = 1); + + ~UAS() = default; + + /** + * @brief Mavros diagnostic updater + */ + diagnostic_updater::Updater diagnostic_updater; + + //! Data that can be useful to pass between plugins + Data data; + + /** + * @brief Return connection status + */ + inline bool is_connected() + { + return connected; + } + + /* -*- HEARTBEAT data -*- */ + + /** + * Update autopilot type on every HEARTBEAT + */ + void update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_); + + /** + * Update autopilot connection status (every HEARTBEAT/conn_timeout) + */ + void update_connection_status(bool conn_); + + /** + * @brief Add connection change handler callback + */ + void add_connection_change_handler(ConnectionCb cb); + + /** + * @brief Returns vehicle type + */ + inline MAV_TYPE get_type() + { + std::underlying_type::type type_ = type; + return static_cast(type_); + } + + /** + * @brief Returns autopilot type + */ + inline MAV_AUTOPILOT get_autopilot() + { + std::underlying_type::type autopilot_ = autopilot; + return static_cast(autopilot_); + } + + /** + * @brief Returns arming status + * + * @note There may be race condition between SET_MODE and HEARTBEAT. + */ + inline bool get_armed() + { + uint8_t base_mode_ = base_mode; + return base_mode_ & utils::enum_value(MAV_MODE_FLAG::SAFETY_ARMED); + } + + /** + * @brief Returns HIL status + */ + inline bool get_hil_state() + { + uint8_t base_mode_ = base_mode; + return base_mode_ & utils::enum_value(MAV_MODE_FLAG::HIL_ENABLED); + } + + /* -*- FCU target id pair -*- */ + + /** + * @brief Return communication target system + */ + inline uint8_t get_tgt_system() + { + return target_system; + } + + /** + * @brief Return communication target component + */ + inline uint8_t get_tgt_component() + { + return target_component; + } + + inline void set_tgt(uint8_t sys, uint8_t comp) + { + target_system = sys; + target_component = comp; + } + + /* -*- transform -*- */ + + tf2_ros::Buffer tf2_buffer; + tf2_ros::TransformListener tf2_listener; + tf2_ros::TransformBroadcaster tf2_broadcaster; + tf2_ros::StaticTransformBroadcaster tf2_static_broadcaster; + + /** + * @brief Add static transform. To publish all static transforms at once, we stack them in a std::vector. + * + * @param frame_id parent frame for transform + * @param child_id child frame for transform + * @param tr transform + * @param vector vector of transforms + */ + void add_static_transform( + const std::string & frame_id, const std::string & child_id, + const Eigen::Affine3d & tr, + std::vector & vector); + + /** + * @brief Publishes static transform. + * + * @param frame_id parent frame for transform + * @param child_id child frame for transform + * @param tr transform + */ + void publish_static_transform( + const std::string & frame_id, const std::string & child_id, + const Eigen::Affine3d & tr); + + /* -*- time sync -*- */ + + inline void set_time_offset(uint64_t offset_ns) + { + time_offset = offset_ns; + } + + inline uint64_t get_time_offset(void) + { + return time_offset; + } + + inline void set_timesync_mode(timesync_mode mode) + { + tsync_mode = mode; + } + + inline timesync_mode get_timesync_mode(void) + { + return tsync_mode; + } + + /** + * @brief Compute FCU message time from time_boot_ms or time_usec field + * + * Uses time_offset for calculation + * + * @return FCU time if it is known else current wall time. + */ + rclcpp::Time synchronise_stamp(uint32_t time_boot_ms); + rclcpp::Time synchronise_stamp(uint64_t time_usec); + + /** + * @brief Create message header from time_boot_ms or time_usec stamps and frame_id. + * + * Setting frame_id and stamp are pretty common, this little helper should reduce LOC. + * + * @param[in] frame_id frame for header + * @param[in] time_stamp mavlink message time + * @return Header with syncronized stamp and frame id + */ + template + inline std_msgs::msg::Header synchronized_header(const std::string & frame_id, const T time_stamp) + { + std_msgs::msg::Header out; + out.frame_id = frame_id; + out.stamp = synchronise_stamp(time_stamp); + return out; + } + + /* -*- autopilot version -*- */ + + uint64_t get_capabilities(); + + /** + * @brief Function to check if the flight controller has a capability + * + * @param capabilities can accept a multiple capability params either in enum or int from + */ + template + bool has_capability(T capability) + { + static_assert( + std::is_enum::value, + "Only query capabilities using the UAS::MAV_CAP enum."); + return get_capabilities() & utils::enum_value(capability); + } + + /** + * @brief Function to check if the flight controller has a set of capabilities + * + * @param capabilities can accept a multiple capability params either in enum or int from + */ + template + bool has_capabilities(Ts ... capabilities) + { + bool ret = true; + std::initializer_list capabilities_list {has_capability(capabilities) ...}; + for (auto has_cap : capabilities_list) { + ret &= has_cap; + } + return ret; + } + + /** + * @brief Update the capabilities if they've changed every VERSION/timeout + */ + void update_capabilities(bool known, uint64_t caps = 0); + + /** + * @brief Adds a function to the capabilities callback queue + * + * @param cb A void function that takes a single mavlink::common::MAV_PROTOCOL_CAPABILITY(MAV_CAP) param + */ + void add_capabilities_change_handler(CapabilitiesCb cb); + + /* -*- utils -*- */ + + /** + * Helper template to set target id's of message. + */ + template + inline void msg_set_target(_T & msg) + { + msg.target_system = get_tgt_system(); + msg.target_component = get_tgt_component(); + } + + /** + * @brief Check that sys/comp id's is my target + */ + inline bool is_my_target(uint8_t sysid, uint8_t compid) + { + return sysid == get_tgt_system() && compid == get_tgt_component(); + } + + /** + * @brief Check that system id is my target + */ + inline bool is_my_target(uint8_t sysid) + { + return sysid == get_tgt_system(); + } + + /** + * @brief Check that FCU is APM + */ + inline bool is_ardupilotmega() + { + return MAV_AUTOPILOT::ARDUPILOTMEGA == get_autopilot(); + } + + /** + * @brief Check that FCU is PX4 + */ + inline bool is_px4() + { + return MAV_AUTOPILOT::PX4 == get_autopilot(); + } + + /** + * @brief Represent FCU mode as string + * + * Port pymavlink mavutil.mode_string_v10 + * + * Supported FCU's: + * - APM:Plane + * - APM:Copter + * - PX4 + * + * @param[in] base_mode base mode + * @param[in] custom_mode custom mode data + */ + std::string str_mode_v10(uint8_t base_mode, uint32_t custom_mode); + + /** + * @brief Lookup custom mode for given string + * + * Complimentary to @a str_mode_v10() + * + * @param[in] cmode_str string representation of mode + * @param[out] custom_mode decoded value + * @return true if success + */ + bool cmode_from_str(std::string cmode_str, uint32_t & custom_mode); + + /** + * @brief Send message to UAS + */ + inline void send_message(const mavlink::Message & msg) + { + send_message(msg, source_component); + } + + /** + * @brief Send message to UAS with custom component id + */ + void send_message(const mavlink::Message & msg, const uint8_t src_compid); + + //! sets protocol version + void set_protocol_version(mavconn::Protocol ver); + +private: + friend class TestUAS; + + // params + uint8_t source_system; + uint8_t source_component; + uint8_t target_system; + uint8_t target_component; + + std::string uas_url; + + StrV plugin_allowlist; + StrV plugin_denylist; + + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr set_parameters_handle_ptr; + rclcpp::TimerBase::SharedPtr startup_delay_timer; + + // XXX(vooon): we have to use own executor because Node::create_sub_node() doesn't work for us. + using thread_ptr = std::unique_ptr>; + thread_ptr exec_spin_thd; + // rclcpp::executors::MultiThreadedExecutor exec; + UASExecutor exec; + + // plugins + pluginlib::ClassLoader plugin_factory_loader; + std::vector loaded_plugins; + + //! UAS link -> router -> plugin handler + std::unordered_map plugin_subscriptions; + + std::shared_timed_mutex mu; + + // essential data + std::atomic type; + std::atomic autopilot; + std::atomic base_mode; + std::atomic fcu_caps_known; + std::atomic fcu_capabilities; + + std::atomic connected; + std::vector connection_cb_vec; + std::vector capabilities_cb_vec; + + std::atomic time_offset; + timesync_mode tsync_mode; + + // UAS -> Router connection + mavlink::mavlink_status_t mavlink_status; + rclcpp::Subscription::SharedPtr source; // FCU -> UAS + rclcpp::Publisher::SharedPtr sink; // UAS -> FCU + + //! initialize connection to the Router + void connect_to_router(); + + //! uas message receive handler + void recv_message(const mavros_msgs::msg::Mavlink::SharedPtr rmsg); + + //! message router + void plugin_route(const mavlink::mavlink_message_t * mmsg, const mavconn::Framing framing); + + //! check if plugin allowed to load + bool is_plugin_allowed(const std::string & pl_name); + + //! makes an instance of the plugin + virtual plugin::Plugin::SharedPtr create_plugin_instance(const std::string & pl_name); + + //! load plugin + void add_plugin(const std::string & pl_name); + + rcl_interfaces::msg::SetParametersResult on_set_parameters_cb( + const std::vector & parameters); + + void log_connect_change(bool connected); + + void diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat); +}; +} // namespace uas +} // namespace mavros + +#endif // MAVROS__MAVROS_UAS_HPP_ diff --git a/mavros/include/mavros/mission_protocol_base.h b/mavros/include/mavros/mission_protocol_base.h deleted file mode 100644 index 8c54b4a8f..000000000 --- a/mavros/include/mavros/mission_protocol_base.h +++ /dev/null @@ -1,603 +0,0 @@ -/** - * @brief Mission base plugin - * @file mission_protocol_base.h - * @author Vladimir Ermakov - * @author Charlie Burge - * - * @addtogroup plugin - * @{ - */ -/* - * Copyright 2014,2015,2016,2017,2018 Vladimir Ermakov. - * Copyright 2021 Charlie Burge. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -namespace mavros { -namespace plugin { -using mavlink::common::MAV_CMD; -using mavlink::common::MAV_FRAME; -using MRES = mavlink::common::MAV_MISSION_RESULT; -using utils::enum_value; -using mavlink::common::MAV_FRAME; -using WP_ITEM = mavlink::common::msg::MISSION_ITEM; -using WP_ITEM_INT = mavlink::common::msg::MISSION_ITEM_INT; -using WP_TYPE = mavlink::common::MAV_MISSION_TYPE; - -static double waypoint_encode_factor( const uint8_t &frame ){ - switch (frame) { - // [[[cog: - // from pymavlink.dialects.v20 import common - // e = common.enums['MAV_FRAME'] - // all_names = [ee.name[len('MAV_FRAME_'):] for ee in e.values()] - // all_names.pop() # remove ENUM_END - // global_names = [v for v in all_names if v.startswith('GLOBAL')] - // local_names = [v for v in all_names if v.startswith(('LOCAL', 'BODY', 'MOCAP', 'VISION', 'ESTIM'))] - // other_names = ['MISSION'] - // - // for names, factor in [(global_names, 10000000), (local_names, 10000), (other_names, 1)]: - // for name in names: - // cog.outl(f"case enum_value(MAV_FRAME::{name}):") - // cog.outl(f"\treturn {factor};") - // - // cog.outl("default:\n\treturn 1;") - // ]]] - case enum_value(MAV_FRAME::GLOBAL): - case enum_value(MAV_FRAME::GLOBAL_RELATIVE_ALT): - case enum_value(MAV_FRAME::GLOBAL_INT): - case enum_value(MAV_FRAME::GLOBAL_RELATIVE_ALT_INT): - case enum_value(MAV_FRAME::GLOBAL_TERRAIN_ALT): - case enum_value(MAV_FRAME::GLOBAL_TERRAIN_ALT_INT): - return 10000000; - case enum_value(MAV_FRAME::LOCAL_NED): - case enum_value(MAV_FRAME::LOCAL_ENU): - case enum_value(MAV_FRAME::LOCAL_OFFSET_NED): - case enum_value(MAV_FRAME::BODY_NED): - case enum_value(MAV_FRAME::BODY_OFFSET_NED): - case enum_value(MAV_FRAME::BODY_FRD): - case enum_value(MAV_FRAME::LOCAL_FRD): - case enum_value(MAV_FRAME::LOCAL_FLU): - return 10000; - case enum_value(MAV_FRAME::MISSION): - return 1; - default: - return 1; - // [[[end]]] (checksum: 152fba25e8f422b878caf990a551a6fd) - } -} - -template -mavros_msgs::Waypoint mav_to_msg(const ITEM &mav_msg) -{ - mavros_msgs::Waypoint ret; - - // [[[cog: - // waypoint_item_msg = [(v, v) if isinstance(v, str) else v for v in ( - // 'frame', - // 'command', - // ('is_current', 'current'), - // 'autocontinue', - // 'param1', - // 'param2', - // 'param3', - // 'param4', - // )] - // waypoint_coords = [ - // ('x_lat', 'x'), - // ('y_long', 'y'), - // ('z_alt', 'z'), - // ] - // for a, b in waypoint_item_msg + waypoint_coords: - // cog.outl(f"ret.{a} = mav_msg.{b};") - // ]]] - ret.frame = mav_msg.frame; - ret.command = mav_msg.command; - ret.is_current = mav_msg.current; - ret.autocontinue = mav_msg.autocontinue; - ret.param1 = mav_msg.param1; - ret.param2 = mav_msg.param2; - ret.param3 = mav_msg.param3; - ret.param4 = mav_msg.param4; - ret.x_lat = mav_msg.x; - ret.y_long = mav_msg.y; - ret.z_alt = mav_msg.z; - // [[[end]]] (checksum: 27badd1a5facc63f38cdd7aad3be9816) - - return ret; -} - - -template<> -inline mavros_msgs::Waypoint mav_to_msg(const WP_ITEM_INT &mav_msg){ - mavros_msgs::Waypoint ret; - - // [[[cog: - // for a, b in waypoint_item_msg + waypoint_coords: - // if a.startswith(('x', 'y')): - // cog.outl(f"ret.{a} = mav_msg.{b} / waypoint_encode_factor(mav_msg.frame);") - // else: - // cog.outl(f"ret.{a} = mav_msg.{b};") - // ]]] - ret.frame = mav_msg.frame; - ret.command = mav_msg.command; - ret.is_current = mav_msg.current; - ret.autocontinue = mav_msg.autocontinue; - ret.param1 = mav_msg.param1; - ret.param2 = mav_msg.param2; - ret.param3 = mav_msg.param3; - ret.param4 = mav_msg.param4; - ret.x_lat = mav_msg.x / waypoint_encode_factor(mav_msg.frame); - ret.y_long = mav_msg.y / waypoint_encode_factor(mav_msg.frame); - ret.z_alt = mav_msg.z; - // [[[end]]] (checksum: 6c82a18990af7aeeb1db9211e9b1bbf1) - - return ret; -} - - -template -ITEM mav_from_msg(const mavros_msgs::Waypoint &wp, const uint16_t seq, WP_TYPE type){ - ITEM ret{}; - - // [[[cog: - // for a, b in waypoint_item_msg + waypoint_coords: - // cog.outl(f"ret.{b} = wp.{a};") - // ]]] - ret.frame = wp.frame; - ret.command = wp.command; - ret.current = wp.is_current; - ret.autocontinue = wp.autocontinue; - ret.param1 = wp.param1; - ret.param2 = wp.param2; - ret.param3 = wp.param3; - ret.param4 = wp.param4; - ret.x = wp.x_lat; - ret.y = wp.y_long; - ret.z = wp.z_alt; - // [[[end]]] (checksum: c1b08cda34f1c4dc94129bda4743aaec) - - ret.seq = seq; - - // defaults to 0 -> MAV_MISSION_TYPE_MISSION - ret.mission_type = enum_value(type); - - return ret; -} - -template <> -inline WP_ITEM_INT mav_from_msg(const mavros_msgs::Waypoint &wp, const uint16_t seq, WP_TYPE type){ - WP_ITEM_INT ret{}; - - // [[[cog: - // for a, b in waypoint_item_msg + waypoint_coords: - // if b.startswith(('x', 'y')): - // cog.outl(f"ret.{b} = int32_t(wp.{a} * waypoint_encode_factor(wp.frame));") - // else: - // cog.outl(f"ret.{b} = wp.{a};") - // ]]] - ret.frame = wp.frame; - ret.command = wp.command; - ret.current = wp.is_current; - ret.autocontinue = wp.autocontinue; - ret.param1 = wp.param1; - ret.param2 = wp.param2; - ret.param3 = wp.param3; - ret.param4 = wp.param4; - ret.x = int32_t(wp.x_lat * waypoint_encode_factor(wp.frame)); - ret.y = int32_t(wp.y_long * waypoint_encode_factor(wp.frame)); - ret.z = wp.z_alt; - // [[[end]]] (checksum: 6315c451fe834dbf20a43ee112b8b5fe) - - ret.seq = seq; - - // defaults to 0 -> MAV_MISSION_TYPE_MISSION - ret.mission_type = enum_value(type); - - return ret; -} - -template -std::string waypoint_to_string(const ITEM &wp) -{ - std::stringstream ss; - ss.precision(7); - ss << '#' << wp.seq << (wp.current ? '*' : ' ') << " F:" << +wp.frame << " C:" << std::setw(3) << wp.command; - ss << " p: " << wp.param1 << ' ' << wp.param2 << ' ' << wp.param3 << ' ' << wp.param4 << " x: " << wp.x << " y: " << wp.y << " z: " << wp.z; - return ss.str(); -} - - -/** - * @brief Mission base plugin - */ -class MissionBase : public plugin::PluginBase { -public: - MissionBase(std::string _name) : - PluginBase(), - log_ns(_name), - wp_state(WP::IDLE), - wp_type(WP_TYPE::MISSION), - wp_count(0), - wp_cur_id(0), - wp_cur_active(0), - wp_set_active(0), - wp_retries(RETRIES_COUNT), - is_timedout(false), - do_pull_after_gcs(false), - enable_partial_push(false), - reschedule_pull(false), - BOOTUP_TIME_DT(BOOTUP_TIME_MS / 1000.0), - LIST_TIMEOUT_DT(LIST_TIMEOUT_MS / 1000.0), - WP_TIMEOUT_DT(WP_TIMEOUT_MS / 1000.0), - RESCHEDULE_DT(RESCHEDULE_MS / 1000.0) - { } - - virtual void initialize_with_nodehandle(ros::NodeHandle *_wp_nh) - { - wp_timer = _wp_nh->createTimer(WP_TIMEOUT_DT, &MissionBase::timeout_cb, this, true); - wp_timer.stop(); - - schedule_timer = _wp_nh->createTimer(BOOTUP_TIME_DT, &MissionBase::scheduled_pull_cb, this, true); - schedule_timer.stop(); - } - -protected: - using unique_lock = std::unique_lock; - using lock_guard = std::lock_guard; - - std::string log_ns; - - std::recursive_mutex mutex; - - std::vector waypoints; - std::vector send_waypoints; - - enum class WP { - IDLE, - RXLIST, - RXWP, - RXWPINT, - TXLIST, - TXPARTIAL, - TXWP, - TXWPINT, - CLEAR, - SET_CUR - }; - WP wp_state; - - WP_TYPE wp_type; - size_t wp_count; - size_t wp_start_id; - size_t wp_end_id; - size_t wp_cur_id; - size_t wp_cur_active; - size_t wp_set_active; - size_t wp_retries; - bool is_timedout; - std::mutex recv_cond_mutex; - std::mutex send_cond_mutex; - std::condition_variable list_receiving; - std::condition_variable list_sending; - - ros::Timer wp_timer; - ros::Timer schedule_timer; - bool do_pull_after_gcs; - bool enable_partial_push; - - bool reschedule_pull; - - bool use_mission_item_int; - bool mission_item_int_support_confirmed; - - static constexpr int BOOTUP_TIME_MS = 15000; //! system startup delay before start pull - static constexpr int LIST_TIMEOUT_MS = 30000; //! Timeout for pull/push operations - static constexpr int WP_TIMEOUT_MS = 1000; - static constexpr int RESCHEDULE_MS = 5000; - static constexpr int RETRIES_COUNT = 3; - static constexpr unsigned int MAV_PROTOCOL_CAPABILITY_MISSION_INT = 4; - - const ros::Duration BOOTUP_TIME_DT; - const ros::Duration LIST_TIMEOUT_DT; - const ros::Duration WP_TIMEOUT_DT; - const ros::Duration RESCHEDULE_DT; - - /* -*- rx handlers -*- */ - - /** - * @brief handle MISSION_ITEM_INT mavlink msg - * handles and stores mission items when pulling waypoints - * @param msg Received Mavlink msg - * @param wpi WaypointItemInt from msg - */ - void handle_mission_item_int(const mavlink::mavlink_message_t *msg, WP_ITEM_INT &wpi); - - - /** - * @brief handle MISSION_ITEM mavlink msg - * handles and stores mission items when pulling waypoints - * @param msg Received Mavlink msg - * @param wpi WaypointItem from msg - */ - void handle_mission_item(const mavlink::mavlink_message_t *msg, WP_ITEM &wpi); - - /** - * @brief checks for a sequence mismatch between a - * MISSION_REQUEST(_INT) sequence and the current - * waypoint that should be sent. - * @param seq The seq member of a MISSION_REQUEST(_INT) - * @return True if there is a sequence mismatch - */ - bool sequence_mismatch(const uint16_t &seq); - - /** - * @brief handle MISSION_REQUEST mavlink msg - * handles and acts on misison request from FCU - * @param msg Received Mavlink msg - * @param mreq MISSION_REQUEST from msg - */ - void handle_mission_request(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_REQUEST &mreq); - - /** - * @brief handle MISSION_REQUEST_INT mavlink msg - * handles and acts on misison request from FCU - * @param msg Received Mavlink msg - * @param mreq MISSION_REQUEST_INT from msg - */ - void handle_mission_request_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_REQUEST_INT &mreq); - - /** - * @brief handle MISSION_COUNT mavlink msg - * Handles a mission count from FCU in a Waypoint Pull - * Triggers a pull GCS seems to be requesting mission - * @param msg Received Mavlink msg - * @param mcnt MISSION_COUNT from msg - */ - void handle_mission_count(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_COUNT &mcnt); - - /** - * @brief handle MISSION_ACK mavlink msg - * Handles a MISSION_ACK which marks the end of a push, or a failure - * @param msg Received Mavlink msg - * @param mack MISSION_ACK from msg - */ - void handle_mission_ack(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_ACK &mack); - - /* -*- mid-level helpers -*- */ - - /** - * @brief Act on a timeout - * Resend the message that may have been lost - */ - void timeout_cb(const ros::TimerEvent &event); - - //! @brief Callback for scheduled waypoint pull - void scheduled_pull_cb(const ros::TimerEvent &event) - { - lock_guard lock(mutex); - if (wp_state != WP::IDLE) { - /* try later */ - ROS_DEBUG_NAMED(log_ns, "%s: busy, reschedule pull", log_ns.c_str()); - schedule_pull(RESCHEDULE_DT); - return; - } - - ROS_DEBUG_NAMED(log_ns, "%s: start scheduled pull", log_ns.c_str()); - wp_state = WP::RXLIST; - wp_count = 0; - restart_timeout_timer(); - mission_request_list(); - } - - //! @brief Send ACK back to FCU after pull - void request_mission_done(void) - { - /* possibly not needed if count == 0 (QGC impl) */ - mission_ack(MRES::ACCEPTED); - - go_idle(); - list_receiving.notify_all(); - ROS_INFO_NAMED(log_ns, "%s: mission received", log_ns.c_str()); - } - - void go_idle(void) - { - reschedule_pull = false; - wp_state = WP::IDLE; - wp_timer.stop(); - } - - void restart_timeout_timer(void) - { - wp_retries = RETRIES_COUNT; - restart_timeout_timer_int(); - } - - void restart_timeout_timer_int(void) - { - is_timedout = false; - wp_timer.stop(); - wp_timer.start(); - } - - void schedule_pull(const ros::Duration &dt) - { - schedule_timer.stop(); - schedule_timer.setPeriod(dt); - schedule_timer.start(); - } - - //! @brief send a single waypoint to FCU - template - void send_waypoint(size_t seq){ - if (seq < send_waypoints.size()) { - auto wp_msg = send_waypoints.at(seq); - auto wpi = mav_from_msg(wp_msg, seq, wp_type); - mission_send(wpi); - ROS_DEBUG_STREAM_NAMED(log_ns, log_ns << ": send item " << waypoint_to_string(wpi)); - } - } - - /** - * @brief wait until a waypoint pull is complete. - * Pull happens asynchronously, this function blocks until it is done. - */ - bool wait_fetch_all() - { - std::unique_lock lock(recv_cond_mutex); - return list_receiving.wait_for(lock, std::chrono::nanoseconds(LIST_TIMEOUT_DT.toNSec())) - == std::cv_status::no_timeout - && !is_timedout; - } - - /** - * @brief wait until a waypoint push is complete. - * Push happens asynchronously, this function blocks until it is done. - */ - bool wait_push_all() - { - std::unique_lock lock(send_cond_mutex); - - return list_sending.wait_for(lock, std::chrono::nanoseconds(LIST_TIMEOUT_DT.toNSec())) - == std::cv_status::no_timeout - && !is_timedout; - } - - //! @brief set the FCU current waypoint - void set_current_waypoint(size_t seq) - { - size_t i = 0; - for (auto &it : waypoints) { - it.is_current = (i == seq) ? true : false; - i++; - } - } - - //! @brief publish the updated waypoint list after operation - virtual void publish_waypoints() = 0; - - /* -*- low-level send functions -*- */ - - template - void mission_send(ITEM &wp) - { - auto wpi = wp; - m_uas->msg_set_target(wpi); - UAS_FCU(m_uas)->send_message_ignore_drop(wpi); - } - - void mission_request(uint16_t seq) - { - ROS_DEBUG_NAMED(log_ns, "%s:m: request #%u", log_ns.c_str(), seq); - - mavlink::common::msg::MISSION_REQUEST mrq {}; - mrq.mission_type = enum_value(wp_type); - m_uas->msg_set_target(mrq); - mrq.seq = seq; - - UAS_FCU(m_uas)->send_message_ignore_drop(mrq); - } - - void mission_request_int(uint16_t seq) - { - ROS_DEBUG_NAMED(log_ns, "%s:m: request_int #%u", log_ns.c_str(), seq); - - mavlink::common::msg::MISSION_REQUEST_INT mrq {}; - mrq.mission_type = enum_value(wp_type); - m_uas->msg_set_target(mrq); - mrq.seq = seq; - - UAS_FCU(m_uas)->send_message_ignore_drop(mrq); - } - - void mission_set_current(uint16_t seq) - { - ROS_DEBUG_NAMED(log_ns, "%s:m: set current #%u", log_ns.c_str(), seq); - - mavlink::common::msg::MISSION_SET_CURRENT msc {}; - m_uas->msg_set_target(msc); - msc.seq = seq; - - UAS_FCU(m_uas)->send_message_ignore_drop(msc); - } - - void mission_request_list() - { - ROS_DEBUG_NAMED(log_ns, "%s:m: request list", log_ns.c_str()); - - mavlink::common::msg::MISSION_REQUEST_LIST mrl {}; - mrl.mission_type = enum_value(wp_type); - m_uas->msg_set_target(mrl); - - UAS_FCU(m_uas)->send_message_ignore_drop(mrl); - } - - void mission_count(uint16_t cnt) - { - ROS_INFO_NAMED(log_ns, "%s:m: count %u", log_ns.c_str(), cnt); - - mavlink::common::msg::MISSION_COUNT mcnt {}; - mcnt.mission_type = enum_value(wp_type); - mcnt.count = cnt; - m_uas->msg_set_target(mcnt); - - UAS_FCU(m_uas)->send_message_ignore_drop(mcnt); - } - - void mission_write_partial_list(uint16_t start_index, uint16_t end_index) - { - ROS_DEBUG_NAMED(log_ns, "%s:m: write partial list %u - %u", - log_ns.c_str(),start_index, end_index); - - mavlink::common::msg::MISSION_WRITE_PARTIAL_LIST mwpl {}; - mwpl.start_index = start_index; - mwpl.end_index = end_index; - mwpl.mission_type = enum_value(wp_type); - m_uas->msg_set_target(mwpl); - - UAS_FCU(m_uas)->send_message_ignore_drop(mwpl); - } - - void mission_clear_all() - { - ROS_DEBUG_NAMED(log_ns, "%s:m: clear all", log_ns.c_str()); - - mavlink::common::msg::MISSION_CLEAR_ALL mclr {}; - mclr.mission_type = enum_value(wp_type); - m_uas->msg_set_target(mclr); - - UAS_FCU(m_uas)->send_message_ignore_drop(mclr); - } - - void mission_ack(MRES type) { - ROS_DEBUG_NAMED(log_ns, "%s:m: ACK %u", log_ns.c_str(), enum_value(type)); - - mavlink::common::msg::MISSION_ACK mack {}; - mack.type = enum_value(type); - mack.mission_type = enum_value(wp_type); - m_uas->msg_set_target(mack); - - UAS_FCU(m_uas)->send_message_ignore_drop(mack); - } -}; -} // namespace plugin -} // namespace mavros diff --git a/mavros/include/mavros/mission_protocol_base.hpp b/mavros/include/mavros/mission_protocol_base.hpp new file mode 100644 index 000000000..77701ca51 --- /dev/null +++ b/mavros/include/mavros/mission_protocol_base.hpp @@ -0,0 +1,654 @@ +/* + * Copyright 2014,2015,2016,2017,2018,2021 Vladimir Ermakov. + * Copyright 2021 Charlie Burge. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief Mission base plugin + * @file mission_protocol_base.h + * @author Vladimir Ermakov + * @author Charlie Burge + * + * @addtogroup plugin + * @{ + */ + +#pragma once + +#ifndef MAVROS__MISSION_PROTOCOL_BASE_HPP_ +#define MAVROS__MISSION_PROTOCOL_BASE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/waypoint_list.hpp" +#include "mavros_msgs/srv/waypoint_clear.hpp" +#include "mavros_msgs/srv/waypoint_pull.hpp" +#include "mavros_msgs/srv/waypoint_push.hpp" + +namespace mavros +{ +namespace plugin +{ +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT + +using utils::enum_value; +using mavlink::common::MAV_CMD; +using mavlink::common::MAV_FRAME; +using mavlink::common::MAV_PROTOCOL_CAPABILITY; +using mavlink::common::msg::MISSION_ITEM; +using mavlink::common::msg::MISSION_ITEM_INT; +using MRES = mavlink::common::MAV_MISSION_RESULT; +using MTYPE = mavlink::common::MAV_MISSION_TYPE; +using MFilter = plugin::filter::SystemAndOk; + +// [[[cog: +// +// from pymavlink.dialects.v20 import common +// e = common.enums['MAV_FRAME'] +// all_names = [ee.name[len('MAV_FRAME_'):] for ee in e.values()] +// all_names.pop() # remove ENUM_END +// global_names = [v for v in all_names if v.startswith('GLOBAL')] +// local_names = [v for v in all_names if v.startswith(('LOCAL', 'BODY', 'MOCAP', +// 'VISION', 'ESTIM'))] +// other_names = ['MISSION'] +// +// waypoint_item_msg = [(v, v) if isinstance(v, str) else v for v in ( +// 'frame', +// 'command', +// ('is_current', 'current'), +// 'autocontinue', +// 'param1', +// 'param2', +// 'param3', +// 'param4', +// )] +// waypoint_coords = [ +// ('x_lat', 'x'), +// ('y_long', 'y'), +// ('z_alt', 'z'), +// ] +// mission_item_msg = [ +// ('seq', 'seq'), +// ('mission_type', 'mission_type'), +// ] +// ]]] +// [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) + +//! Thin wrapper for Waypoint message +class MissionItem : public mavros_msgs::msg::Waypoint +{ +public: + uint16_t seq; //!< sequence number, not a part of ros message + uint8_t mission_type; //!< MAV_MISSION_TYPE + + static constexpr double encode_factor(const uint8_t frame) + { + switch (frame) { + // [[[cog: + // for names, factor in [(global_names, 10000000), (local_names, 10000), (other_names, 1)]: + // for name in names: + // cog.outl(f"case enum_value(MAV_FRAME::{name}):") + // cog.outl(f" return {factor:1.1f};") + // + // cog.outl("default:\n return 1.0;") + // ]]] + case enum_value(MAV_FRAME::GLOBAL): + case enum_value(MAV_FRAME::GLOBAL_RELATIVE_ALT): + case enum_value(MAV_FRAME::GLOBAL_INT): + case enum_value(MAV_FRAME::GLOBAL_RELATIVE_ALT_INT): + case enum_value(MAV_FRAME::GLOBAL_TERRAIN_ALT): + case enum_value(MAV_FRAME::GLOBAL_TERRAIN_ALT_INT): + return 10000000.0; + case enum_value(MAV_FRAME::LOCAL_NED): + case enum_value(MAV_FRAME::LOCAL_ENU): + case enum_value(MAV_FRAME::LOCAL_OFFSET_NED): + case enum_value(MAV_FRAME::BODY_NED): + case enum_value(MAV_FRAME::BODY_OFFSET_NED): + case enum_value(MAV_FRAME::BODY_FRD): + case enum_value(MAV_FRAME::LOCAL_FRD): + case enum_value(MAV_FRAME::LOCAL_FLU): + return 10000.0; + case enum_value(MAV_FRAME::MISSION): + return 1.0; + default: + return 1.0; + // [[[end]]] (checksum: 62e411e9acae54305a129fdf57f5b732) + } + } + + explicit MissionItem(const MISSION_ITEM & wpi) + : mavros_msgs::msg::Waypoint() + { + // [[[cog: + // fields = mission_item_msg + waypoint_item_msg + waypoint_coords + // for i, (a, b) in enumerate(fields): + // cog.outl(f"{a} = wpi.{b};") + // ]]] + seq = wpi.seq; + mission_type = wpi.mission_type; + frame = wpi.frame; + command = wpi.command; + is_current = wpi.current; + autocontinue = wpi.autocontinue; + param1 = wpi.param1; + param2 = wpi.param2; + param3 = wpi.param3; + param4 = wpi.param4; + x_lat = wpi.x; + y_long = wpi.y; + z_alt = wpi.z; + // [[[end]]] (checksum: 64a5d4f7b3428e8e72d5ce216d51935c) + } + + explicit MissionItem(const MISSION_ITEM_INT & wpi) + : mavros_msgs::msg::Waypoint() + { + // [[[cog: + // fields = mission_item_msg + waypoint_item_msg + waypoint_coords + // for i, (a, b) in enumerate(fields): + // if a.startswith(('x', 'y')): + // cog.outl(f"{a} = wpi.{b} / encode_factor(wpi.frame);") + // else: + // cog.outl(f"{a} = wpi.{b};") + // ]]] + seq = wpi.seq; + mission_type = wpi.mission_type; + frame = wpi.frame; + command = wpi.command; + is_current = wpi.current; + autocontinue = wpi.autocontinue; + param1 = wpi.param1; + param2 = wpi.param2; + param3 = wpi.param3; + param4 = wpi.param4; + x_lat = wpi.x / encode_factor(wpi.frame); + y_long = wpi.y / encode_factor(wpi.frame); + z_alt = wpi.z; + // [[[end]]] (checksum: 2e01c028ecde023cc049aba04f6a6df5) + } + + explicit MissionItem(const mavros_msgs::msg::Waypoint & other) + { + *this = other; + } + + MissionItem & operator=(const mavros_msgs::msg::Waypoint & other) + { + // [[[cog: + // fields = waypoint_item_msg + waypoint_coords + // for a, b in fields: + // cog.outl(f"{a} = other.{a};") + // ]]] + frame = other.frame; + command = other.command; + is_current = other.is_current; + autocontinue = other.autocontinue; + param1 = other.param1; + param2 = other.param2; + param3 = other.param3; + param4 = other.param4; + x_lat = other.x_lat; + y_long = other.y_long; + z_alt = other.z_alt; + // [[[end]]] (checksum: 6879b829d6e6e1e28a879dd2ca3afdac) + + return *this; + } + + void to_msg(MISSION_ITEM & out) const + { + // [[[cog: + // fields = mission_item_msg + waypoint_item_msg + waypoint_coords + // for a, b in fields: + // cog.outl(f"out.{b} = {a};") + // ]]] + out.seq = seq; + out.mission_type = mission_type; + out.frame = frame; + out.command = command; + out.current = is_current; + out.autocontinue = autocontinue; + out.param1 = param1; + out.param2 = param2; + out.param3 = param3; + out.param4 = param4; + out.x = x_lat; + out.y = y_long; + out.z = z_alt; + // [[[end]]] (checksum: 799ed1c97af022223371c42c8c1f09d4) + } + + void to_msg(MISSION_ITEM_INT & out) const + { + // [[[cog: + // fields = mission_item_msg + waypoint_item_msg + waypoint_coords + // for a, b in fields: + // if b.startswith(('x', 'y')): + // cog.outl(f"out.{b} = int32_t({a} * encode_factor(frame));") + // else: + // cog.outl(f"out.{b} = {a};") + // ]]] + out.seq = seq; + out.mission_type = mission_type; + out.frame = frame; + out.command = command; + out.current = is_current; + out.autocontinue = autocontinue; + out.param1 = param1; + out.param2 = param2; + out.param3 = param3; + out.param4 = param4; + out.x = int32_t(x_lat * encode_factor(frame)); + out.y = int32_t(y_long * encode_factor(frame)); + out.z = z_alt; + // [[[end]]] (checksum: b79ee415a09746786ba2a7cec99c5ab5) + } + + friend std::ostream & operator<<(std::ostream & os, const MissionItem & mi); +}; + +std::ostream & operator<<(std::ostream & os, const MissionItem & mi); + + +/** + * @brief Mission protocol base plugin + */ +class MissionBase : public plugin::Plugin +{ +public: + MissionBase( + plugin::UASPtr uas_, const std::string & name_, MTYPE mission_type_ = MTYPE::MISSION, + const char * log_prefix_ = "WP", + const std::chrono::nanoseconds bootup_time_ = 15s) + : Plugin(uas_, name_), + mission_type(mission_type_), + log_prefix(log_prefix_), + wp_state(WP::IDLE), + wp_count(0), + wp_start_id(0), + wp_end_id(0), + wp_cur_id(0), + wp_cur_active(0), + wp_set_active(0), + wp_retries(RETRIES_COUNT), + is_timedout(false), + reschedule_pull(false), + do_pull_after_gcs(false), + enable_partial_push(false), + use_mission_item_int(false), + mission_item_int_support_confirmed(false), + BOOTUP_TIME(bootup_time_), + LIST_TIMEOUT(30s), + WP_TIMEOUT(1s), + RESCHEDULE_TIME(5s) + { + timeout_timer = node->create_wall_timer(WP_TIMEOUT, std::bind(&MissionBase::timeout_cb, this)); + timeout_timer->cancel(); + } + + Subscriptions get_subscriptions() override + { + Subscriptions ret{ + make_handler(&MissionBase::handle_mission_item), + make_handler(&MissionBase::handle_mission_item_int), + make_handler(&MissionBase::handle_mission_request), + make_handler(&MissionBase::handle_mission_request_int), + make_handler(&MissionBase::handle_mission_count), + make_handler(&MissionBase::handle_mission_ack), + }; + + // NOTE(vooon): those messages do not have mission_type and only needed for waypoint plugin + if (mission_type == MTYPE::MISSION) { + ret.push_back(make_handler(&MissionBase::handle_mission_current)); + ret.push_back(make_handler(&MissionBase::handle_mission_item_reached)); + } + + return ret; + } + +protected: + using unique_lock = std::unique_lock; + using lock_guard = std::lock_guard; + + std::recursive_mutex mutex; + std::vector waypoints; + std::vector send_waypoints; + + const MTYPE mission_type; + const char * log_prefix; + + enum class WP + { + IDLE, + RXLIST, + RXWP, + RXWPINT, + TXLIST, + TXPARTIAL, + TXWP, + TXWPINT, + CLEAR, + SET_CUR + }; + + WP wp_state; + size_t wp_count; + size_t wp_start_id; + size_t wp_end_id; + size_t wp_cur_id; + size_t wp_cur_active; + size_t wp_set_active; + size_t wp_retries; + + bool is_timedout; + std::mutex recv_cond_mutex; + std::mutex send_cond_mutex; + std::condition_variable list_receiving; + std::condition_variable list_sending; + + rclcpp::TimerBase::SharedPtr timeout_timer; + rclcpp::TimerBase::SharedPtr schedule_timer; + + bool reschedule_pull; + + bool do_pull_after_gcs; + bool enable_partial_push; + bool use_mission_item_int; + bool mission_item_int_support_confirmed; + + static constexpr int RETRIES_COUNT = 3; + const std::chrono::nanoseconds BOOTUP_TIME; + const std::chrono::nanoseconds LIST_TIMEOUT; + const std::chrono::nanoseconds WP_TIMEOUT; + const std::chrono::nanoseconds RESCHEDULE_TIME; + + /* -*- rx handlers -*- */ + + /** + * @brief filters messages not suitable for that plugin + */ + template + bool filter_message(const MsgT & m) + { + return m.mission_type != enum_value(mission_type); + } + + /** + * @brief checks for a sequence mismatch between a + * MISSION_REQUEST(_INT) sequence and the current + * waypoint that should be sent. + * @param seq The seq member of a MISSION_REQUEST(_INT) + * @return True if there is a sequence mismatch + */ + template + bool sequence_mismatch(const MsgT & m) + { + if (m.seq != wp_cur_id && m.seq != wp_cur_id + 1) { + RCLCPP_WARN( + get_logger(), "%s: Seq mismatch, dropping %s (%d != %zu)", log_prefix, + m.get_name().c_str(), m.seq, wp_cur_id); + return true; + } + + return false; + } + + /** + * @brief handle MISSION_ITEM mavlink msg + * handles and stores mission items when pulling waypoints + * @param msg Received Mavlink msg + * @param wpi WaypointItem from msg + */ + virtual void handle_mission_item( + const mavlink::mavlink_message_t * msg, + MISSION_ITEM & wpi, + MFilter filter); + + /** + * @brief handle MISSION_ITEM_INT mavlink msg + * handles and stores mission items when pulling waypoints + * @param msg Received Mavlink msg + * @param wpi WaypointItemInt from msg + */ + virtual void handle_mission_item_int( + const mavlink::mavlink_message_t * msg, + MISSION_ITEM_INT & wpi, + MFilter filter); + + /** + * @brief handle MISSION_REQUEST mavlink msg + * handles and acts on misison request from FCU + * @param msg Received Mavlink msg + * @param mreq MISSION_REQUEST from msg + */ + virtual void handle_mission_request( + const mavlink::mavlink_message_t * msg, + mavlink::common::msg::MISSION_REQUEST & mreq, + MFilter filter); + + /** + * @brief handle MISSION_REQUEST_INT mavlink msg + * handles and acts on misison request from FCU + * @param msg Received Mavlink msg + * @param mreq MISSION_REQUEST_INT from msg + */ + virtual void handle_mission_request_int( + const mavlink::mavlink_message_t * msg, + mavlink::common::msg::MISSION_REQUEST_INT & mreq, + MFilter filter); + + /** + * @brief handle MISSION_COUNT mavlink msg + * Handles a mission count from FCU in a Waypoint Pull + * Triggers a pull GCS seems to be requesting mission + * @param msg Received Mavlink msg + * @param mcnt MISSION_COUNT from msg + */ + virtual void handle_mission_count( + const mavlink::mavlink_message_t * msg, + mavlink::common::msg::MISSION_COUNT & mcnt, + MFilter filter); + + /** + * @brief handle MISSION_ACK mavlink msg + * Handles a MISSION_ACK which marks the end of a push, or a failure + * @param msg Received Mavlink msg + * @param mack MISSION_ACK from msg + */ + virtual void handle_mission_ack( + const mavlink::mavlink_message_t * msg, + mavlink::common::msg::MISSION_ACK & mack, + MFilter filter); + + /** + * @brief handle MISSION_CURRENT mavlink msg + * This confirms a SET_CUR action + * @param msg Received Mavlink msg + * @param mcur MISSION_CURRENT from msg + */ + virtual void handle_mission_current( + const mavlink::mavlink_message_t * msg, + mavlink::common::msg::MISSION_CURRENT & mcur, + MFilter filter); + + /** + * @brief handle MISSION_ITEM_REACHED mavlink msg + * @param msg Received Mavlink msg + * @param mitr MISSION_ITEM_REACHED from msg + */ + virtual void handle_mission_item_reached( + const mavlink::mavlink_message_t * msg, + mavlink::common::msg::MISSION_ITEM_REACHED & mitr, + MFilter filter); + + /* -*- mid-level helpers -*- */ + + /** + * @brief Act on a timeout + * Resend the message that may have been lost + */ + void timeout_cb(); + + //! @brief Callback for scheduled waypoint pull + void scheduled_pull_cb() + { + lock_guard lock(mutex); + + // run once + schedule_timer->cancel(); + + if (wp_state != WP::IDLE) { + /* try later */ + RCLCPP_DEBUG(get_logger(), "%s: busy, reschedule pull", log_prefix); + schedule_pull(RESCHEDULE_TIME); + return; + } + + RCLCPP_DEBUG(get_logger(), "%s: start scheduled pull", log_prefix); + wp_state = WP::RXLIST; + wp_count = 0; + restart_timeout_timer(); + mission_request_list(); + } + + //! @brief Send ACK back to FCU after pull + void request_mission_done(void) + { + /* possibly not needed if count == 0 (QGC impl) */ + mission_ack(MRES::ACCEPTED); + + go_idle(); + list_receiving.notify_all(); + RCLCPP_INFO(get_logger(), "%s: mission received", log_prefix); + } + + void go_idle(void) + { + reschedule_pull = false; + wp_state = WP::IDLE; + timeout_timer->cancel(); + } + + void restart_timeout_timer(void) + { + wp_retries = RETRIES_COUNT; + restart_timeout_timer_int(); + } + + void restart_timeout_timer_int(void) + { + is_timedout = false; + timeout_timer->reset(); + } + + void schedule_pull(const std::chrono::nanoseconds & dt) + { + if (schedule_timer) { + schedule_timer->cancel(); + schedule_timer.reset(); + } + + schedule_timer = node->create_wall_timer(dt, std::bind(&MissionBase::scheduled_pull_cb, this)); + } + + //! @brief send a single waypoint to FCU + template + void send_waypoint(size_t seq) + { + static_assert( + std::is_same::value || std::is_same::value, "wrong type"); + + if (seq < send_waypoints.size()) { + MsgT wpi{}; + + auto wp_msg = send_waypoints.at(seq); + wp_msg.seq = seq; + wp_msg.mission_type = enum_value(mission_type); + wp_msg.to_msg(wpi); + + RCLCPP_DEBUG_STREAM(get_logger(), log_prefix << ": send item " << wp_msg); + mission_send(wpi); + } + } + + /** + * @brief wait until a waypoint pull is complete. + * Pull happens asynchronously, this function blocks until it is done. + */ + bool wait_fetch_all() + { + std::unique_lock lock(recv_cond_mutex); + + return list_receiving.wait_for(lock, LIST_TIMEOUT) == std::cv_status::no_timeout && + !is_timedout; + } + + /** + * @brief wait until a waypoint push is complete. + * Push happens asynchronously, this function blocks until it is done. + */ + bool wait_push_all() + { + std::unique_lock lock(send_cond_mutex); + + return list_sending.wait_for(lock, LIST_TIMEOUT) == std::cv_status::no_timeout && + !is_timedout; + } + + //! @brief set the FCU current waypoint + void set_current_waypoint(size_t seq) + { + size_t i = 0; + for (auto & it : waypoints) { + it.is_current = !!(i++ == seq); + } + } + + //! @brief publish the updated waypoint list after operation + virtual void publish_waypoints() = 0; + + //! @brief publish mission item reached seq + virtual void publish_reached(const uint16_t seq) = 0; + + /* -*- low-level send functions -*- */ + + template + void mission_send(MsgT & wpi) + { + static_assert( + std::is_same::value || std::is_same::value, "wrong type"); + + uas->msg_set_target(wpi); + uas->send_message(wpi); + } + + void mission_request(const uint16_t seq); + void mission_request_int(const uint16_t seq); + void mission_set_current(const uint16_t seq); + void mission_request_list(); + void mission_count(const uint16_t cnt); + void mission_write_partial_list(const uint16_t start_index, const uint16_t end_index); + void mission_clear_all(); + void mission_ack(const MRES type); +}; + +} // namespace plugin +} // namespace mavros + +#endif // MAVROS__MISSION_PROTOCOL_BASE_HPP_ diff --git a/mavros/include/mavros/plugin.hpp b/mavros/include/mavros/plugin.hpp new file mode 100644 index 000000000..ee4c48bba --- /dev/null +++ b/mavros/include/mavros/plugin.hpp @@ -0,0 +1,309 @@ +/* + * Copyright 2013,2014,2015,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief MAVROS Plugin base + * @file plugin.h + * @author Vladimir Ermakov + * + * @addtogroup plugin + * @{ + * @brief MAVROS Plugin system + */ + +#pragma once + +#ifndef MAVROS__PLUGIN_HPP_ +#define MAVROS__PLUGIN_HPP_ + +#include +#include +#include +#include +#include // NOLINT +#include +#include +#include + +#include "mavconn/interface.hpp" +#include "mavros/mavros_uas.hpp" + +namespace mavros +{ +namespace uas +{ +class UAS; +using MAV_CAP = mavlink::common::MAV_PROTOCOL_CAPABILITY; +} // namespace uas + +namespace plugin +{ + +using mavros::uas::UAS; +using UASPtr = std::shared_ptr; +using r_unique_lock = std::unique_lock; +using s_unique_lock = std::unique_lock; +using s_shared_lock = std::shared_lock; + +class Filter +{ + virtual bool operator()( + UASPtr uas, const mavlink::mavlink_message_t * cmsg, + const mavconn::Framing framing) = 0; +}; + +/** + * @brief MAVROS Plugin base class + */ +class Plugin : public std::enable_shared_from_this +{ +private: + explicit Plugin(const Plugin &) = delete; + +public: + RCLCPP_SMART_PTR_DEFINITIONS(Plugin) + + //! generic message handler callback + using HandlerCb = mavconn::MAVConnInterface::ReceivedCb; + //! Tuple: MSG ID, MSG NAME, message type into hash_code, message handler callback + using HandlerInfo = std::tuple; + //! Subscriptions vector + using Subscriptions = std::vector; + + explicit Plugin(UASPtr uas_) + : uas(uas_), node(std::dynamic_pointer_cast(uas_)) + {} + + explicit Plugin( + UASPtr uas_, const std::string & subnode, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : uas(uas_), + // node(std::dynamic_pointer_cast(uas_)->create_sub_node(subnode)) // https://github.com/ros2/rclcpp/issues/731 + node(rclcpp::Node::make_shared(subnode, + std::dynamic_pointer_cast(uas_)->get_fully_qualified_name(), options)) + {} + + virtual ~Plugin() = default; + + /** + * @brief Return vector of MAVLink message subscriptions (handlers) + */ + virtual Subscriptions get_subscriptions() = 0; + + virtual rclcpp::Node::SharedPtr get_node() const + { + return node; + } + + virtual rclcpp::Logger get_logger() const + { + return node->get_logger(); + } + + virtual rclcpp::Clock::SharedPtr get_clock() const + { + return node->get_clock(); + } + +protected: + UASPtr uas; // uas back link + rclcpp::Node::SharedPtr node; // most of plugins uses sub-node + + using SetParametersResult = rcl_interfaces::msg::SetParametersResult; + using ParameterFunctorT = std::function; + + std::unordered_map node_watch_parameters; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr node_set_parameters_handle_ptr; + + /** + * Make subscription to raw message. + * + * @param[in] id message id + * @param[in] fn pointer to member function (handler) + */ + template + HandlerInfo make_handler( + const mavlink::msgid_t id, void (_C::* fn)( + const mavlink::mavlink_message_t * msg, const mavconn::Framing framing)) + { + auto bfn = std::bind( + fn, std::static_pointer_cast<_C>(shared_from_this()), std::placeholders::_1, + std::placeholders::_2); + const auto type_hash_ = typeid(mavlink::mavlink_message_t).hash_code(); + + return HandlerInfo {id, nullptr, type_hash_, bfn}; + } + + /** + * Make subscription to message with automatic decoding. + * + * @param[in] fn pointer to member function (handler) + */ + template + HandlerInfo make_handler(void (_C::* fn)(const mavlink::mavlink_message_t *, _T &, _F)) + { + static_assert( + std::is_base_of::value, + "Filter class should be derived from mavros::plugin::Filter"); + + auto bfn = std::bind( + fn, std::static_pointer_cast<_C>(shared_from_this()), std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3); + const auto id = _T::MSG_ID; + const auto name = _T::NAME; + const auto type_hash_ = typeid(_T).hash_code(); + auto uas_ = this->uas; + + return HandlerInfo { + id, name, type_hash_, + [bfn, uas_](const mavlink::mavlink_message_t * msg, const mavconn::Framing framing) { + auto filter = _F(); + if (!filter(uas_, msg, framing)) { + return; + } + + mavlink::MsgMap map(msg); + _T obj; + obj.deserialize(map); + + bfn(msg, obj, filter); + } + }; + } + + /** + * Common callback called on connection change + */ + virtual void connection_cb(bool connected) + { + (void)connected; + assert(false); + } + + /** + * Shortcut for connection_cb() registration + */ + void enable_connection_cb(); + + /** + * Common callback called only when capabilities change + */ + virtual void capabilities_cb(uas::MAV_CAP capabilities) + { + (void)capabilities; + assert(false); + } + + /** + * Shortcut for capabilities_cb() registration + */ + void enable_capabilities_cb(); + + /** + * Default implmentation of that watch would use watch_parameters + */ + virtual SetParametersResult node_on_set_parameters_cb( + const std::vector & parameters); + + /** + * Shourtcut to enable default parameters watch impl + */ + void enable_node_watch_parameters(); + + /** + * Adds parameter to watch and declares it + */ + template + auto node_declare_and_watch_parameter( + const std::string & name, ParameterFunctorT cb, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false + ) + { + node_watch_parameters[name] = cb; + +#ifdef USE_OLD_DECLARE_PARAMETER + // NOTE(vooon): for Foxy: + return node->declare_parameter( + name, + rclcpp::ParameterValue(), parameter_descriptor, ignore_override); +#else + // NOTE(vooon): for Galactic: + return node->declare_parameter(name, parameter_descriptor, ignore_override); +#endif + } + + template + auto node_declare_and_watch_parameter( + const std::string & name, const ParameterT & default_value, + ParameterFunctorT cb, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false + ) + { + node_watch_parameters[name] = cb; + return node->declare_parameter(name, default_value, parameter_descriptor, ignore_override); + } + + //! Helper to convert ros time to mavlink time_usec field + inline uint64_t get_time_usec(const builtin_interfaces::msg::Time & t) + { + return rclcpp::Time(t).nanoseconds() / 1000; + } + + //! Helper to convert ros now time to mavlink time_usec field + inline uint64_t get_time_usec() + { + return get_time_usec(node->now()); + } + + //! Helper to convert ros time to mavlink time_boot_ms field + inline uint32_t get_time_boot_ms(const builtin_interfaces::msg::Time & t) + { + return rclcpp::Time(t).nanoseconds() / 1000000; + } + + //! Helper to convert ros now time to mavlink time_boot_ms field + inline uint32_t get_time_boot_ms() + { + return get_time_boot_ms(node->now()); + } +}; + +//! A factory class to help initialize plugin node from constructor +class PluginFactory +{ +public: + PluginFactory() = default; + virtual ~PluginFactory() = default; + + virtual Plugin::SharedPtr create_plugin_instance(UASPtr uas) = 0; +}; + +//! Helper template to make plugin factories +template +class PluginFactoryTemplate : public PluginFactory +{ +public: + PluginFactoryTemplate() = default; + virtual ~PluginFactoryTemplate() = default; + + Plugin::SharedPtr create_plugin_instance(UASPtr uas) override + { + static_assert( + std::is_base_of::value, + "Plugin should be derived from mavros::plugin::Plugin"); + return std::make_shared<_T>(uas); + } +}; + +} // namespace plugin +} // namespace mavros + +#endif // MAVROS__PLUGIN_HPP_ diff --git a/mavros/include/mavros/plugin_filter.hpp b/mavros/include/mavros/plugin_filter.hpp new file mode 100644 index 000000000..3d3b28a17 --- /dev/null +++ b/mavros/include/mavros/plugin_filter.hpp @@ -0,0 +1,83 @@ +/* + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief MAVROS Plugin message filters + * @file plugin.h + * @author Vladimir Ermakov + * + * @addtogroup plugin + * @{ + * @brief MAVROS Plugin system + */ + +#pragma once + +#ifndef MAVROS__PLUGIN_FILTER_HPP_ +#define MAVROS__PLUGIN_FILTER_HPP_ + +#include +#include +#include + +namespace mavros +{ +namespace plugin +{ +namespace filter +{ +using mavros::plugin::Filter; +using mavros::uas::UAS; +using UASPtr = UAS::SharedPtr; +using mavconn::Framing; + + +//! AnyOk filter passes all messages with Framing::ok +class AnyOk : public Filter +{ +public: + inline bool operator()( + UASPtr uas, const mavlink::mavlink_message_t * cmsg, + const Framing framing) override + { + (void)uas; + (void)cmsg; + return framing == Framing::ok; + } +}; + +//! SystemAndOk filter passes only messages with Framing::ok and matching target system id +class SystemAndOk : public Filter +{ +public: + inline bool operator()( + UASPtr uas, const mavlink::mavlink_message_t * cmsg, + const Framing framing) override + { + return framing == Framing::ok && uas->is_my_target(cmsg->sysid); + } +}; + +//! ComponentAndOk filter passes only messages with Framing::ok and matching target +// system/component ids +class ComponentAndOk : public Filter +{ +public: + inline bool operator()( + UASPtr uas, const mavlink::mavlink_message_t * cmsg, + const Framing framing) override + { + return framing == Framing::ok && uas->is_my_target(cmsg->sysid, cmsg->compid); + } +}; + +} // namespace filter +} // namespace plugin +} // namespace mavros + + +#endif // MAVROS__PLUGIN_FILTER_HPP_ diff --git a/mavros/include/mavros/px4_custom_mode.h b/mavros/include/mavros/px4_custom_mode.hpp similarity index 70% rename from mavros/include/mavros/px4_custom_mode.h rename to mavros/include/mavros/px4_custom_mode.hpp index f00db38da..3fb19338b 100644 --- a/mavros/include/mavros/px4_custom_mode.h +++ b/mavros/include/mavros/px4_custom_mode.hpp @@ -43,9 +43,13 @@ #pragma once -//#include +#ifndef MAVROS__PX4_CUSTOM_MODE_HPP_ +#define MAVROS__PX4_CUSTOM_MODE_HPP_ -namespace px4 { +// #include + +namespace px4 +{ /** * @brief PX4 custom mode * @@ -53,48 +57,52 @@ namespace px4 { * and uint32_t SET_MODE.custom_mode. */ union custom_mode { - enum MAIN_MODE : uint8_t { - MAIN_MODE_MANUAL = 1, - MAIN_MODE_ALTCTL, - MAIN_MODE_POSCTL, - MAIN_MODE_AUTO, - MAIN_MODE_ACRO, - MAIN_MODE_OFFBOARD, - MAIN_MODE_STABILIZED, - MAIN_MODE_RATTITUDE - }; + enum MAIN_MODE : uint8_t + { + MAIN_MODE_MANUAL = 1, + MAIN_MODE_ALTCTL, + MAIN_MODE_POSCTL, + MAIN_MODE_AUTO, + MAIN_MODE_ACRO, + MAIN_MODE_OFFBOARD, + MAIN_MODE_STABILIZED, + MAIN_MODE_RATTITUDE + }; - enum SUB_MODE_AUTO : uint8_t { - SUB_MODE_AUTO_READY = 1, - SUB_MODE_AUTO_TAKEOFF, - SUB_MODE_AUTO_LOITER, - SUB_MODE_AUTO_MISSION, - SUB_MODE_AUTO_RTL, - SUB_MODE_AUTO_LAND, - SUB_MODE_AUTO_RTGS, - SUB_MODE_AUTO_FOLLOW_TARGET, - SUB_MODE_AUTO_PRECLAND - }; + enum SUB_MODE_AUTO : uint8_t + { + SUB_MODE_AUTO_READY = 1, + SUB_MODE_AUTO_TAKEOFF, + SUB_MODE_AUTO_LOITER, + SUB_MODE_AUTO_MISSION, + SUB_MODE_AUTO_RTL, + SUB_MODE_AUTO_LAND, + SUB_MODE_AUTO_RTGS, + SUB_MODE_AUTO_FOLLOW_TARGET, + SUB_MODE_AUTO_PRECLAND + }; - struct { - uint16_t reserved; - uint8_t main_mode; - uint8_t sub_mode; - }; - uint32_t data; - float data_float; + struct mode_type + { + uint16_t reserved; + uint8_t main_mode; + uint8_t sub_mode; + } mode; + uint32_t data; + float data_float; - custom_mode() : data(0) - { } + custom_mode() : data(0) + { + } - explicit custom_mode(uint32_t val) : data(val) - { } + explicit custom_mode(uint32_t val) : data(val) + { + } - constexpr custom_mode(uint8_t mm, uint8_t sm) : - reserved(0), - main_mode(mm), - sub_mode(sm) - { } + constexpr custom_mode(uint8_t mm, uint8_t sm) + : mode{0, mm, sm} + { + } }; /** @@ -104,8 +112,9 @@ union custom_mode { * @param sm sub mode (currently used only in auto mode) * @return uint32_t representation */ -constexpr uint32_t define_mode(enum custom_mode::MAIN_MODE mm, uint8_t sm = 0) { - return custom_mode(mm, sm).data; +constexpr uint32_t define_mode(enum custom_mode::MAIN_MODE mm, uint8_t sm = 0) +{ + return custom_mode(mm, sm).data; } /** @@ -116,7 +125,11 @@ constexpr uint32_t define_mode(enum custom_mode::MAIN_MODE mm, uint8_t sm = 0) { * @param sm auto sub mode * @return uint32_t representation */ -constexpr uint32_t define_mode_auto(enum custom_mode::SUB_MODE_AUTO sm) { - return define_mode(custom_mode::MAIN_MODE_AUTO, sm); +constexpr uint32_t define_mode_auto(enum custom_mode::SUB_MODE_AUTO sm) +{ + return define_mode(custom_mode::MAIN_MODE_AUTO, sm); } -}; // namespace px4 +} // namespace px4 + + +#endif // MAVROS__PX4_CUSTOM_MODE_HPP_ diff --git a/mavros/include/mavros/setpoint_mixin.h b/mavros/include/mavros/setpoint_mixin.h deleted file mode 100644 index 9156f1e66..000000000 --- a/mavros/include/mavros/setpoint_mixin.h +++ /dev/null @@ -1,249 +0,0 @@ -/** - * @brief Mixin for setpoint plugins - * @file setpoint_mixin.h - * @author Vladimir Ermakov - * - * @addtogroup plugin - * @{ - */ -/* - * Copyright 2014 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include -#include - -#include - -#include "tf2_ros/message_filter.h" -#include - - -namespace mavros { -namespace plugin { -/** - * @brief This mixin adds set_position_target_local_ned() - */ -template -class SetPositionTargetLocalNEDMixin { -public: - //! Message specification: @p https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED - void set_position_target_local_ned(uint32_t time_boot_ms, uint8_t coordinate_frame, - uint16_t type_mask, - Eigen::Vector3d p, - Eigen::Vector3d v, - Eigen::Vector3d af, - float yaw, float yaw_rate) - { - mavros::UAS *m_uas_ = static_cast(this)->m_uas; - mavlink::common::msg::SET_POSITION_TARGET_LOCAL_NED sp = {}; - - m_uas_->msg_set_target(sp); - - // [[[cog: - // for f in ('time_boot_ms', 'coordinate_frame', 'type_mask', 'yaw', 'yaw_rate'): - // cog.outl("sp.%s = %s;" % (f, f)) - // for fp, vp in (('', 'p'), ('v', 'v'), ('af', 'af')): - // for a in ('x', 'y', 'z'): - // cog.outl("sp.%s%s = %s.%s();" % (fp, a, vp, a)) - // ]]] - sp.time_boot_ms = time_boot_ms; - sp.coordinate_frame = coordinate_frame; - sp.type_mask = type_mask; - sp.yaw = yaw; - sp.yaw_rate = yaw_rate; - sp.x = p.x(); - sp.y = p.y(); - sp.z = p.z(); - sp.vx = v.x(); - sp.vy = v.y(); - sp.vz = v.z(); - sp.afx = af.x(); - sp.afy = af.y(); - sp.afz = af.z(); - // [[[end]]] (checksum: 6a9b9dacbcf85c5d428d754c20afe110) - - UAS_FCU(m_uas_)->send_message_ignore_drop(sp); - } -}; - -/** - * @brief This mixin adds set_position_target_global_int() - */ -template -class SetPositionTargetGlobalIntMixin { -public: - //! Message specification: @p https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT - void set_position_target_global_int(uint32_t time_boot_ms, uint8_t coordinate_frame, - uint16_t type_mask, - int32_t lat_int, int32_t lon_int, float alt, - Eigen::Vector3d v, - Eigen::Vector3d af, - float yaw, float yaw_rate) - { - mavros::UAS *m_uas_ = static_cast(this)->m_uas; - mavlink::common::msg::SET_POSITION_TARGET_GLOBAL_INT sp = {}; - - m_uas_->msg_set_target(sp); - - // [[[cog: - // for f in ('time_boot_ms', 'coordinate_frame', 'type_mask', 'lat_int', 'lon_int', 'alt', 'yaw', 'yaw_rate'): - // cog.outl("sp.%s = %s;" % (f, f)) - // for fp, vp in (('v', 'v'), ('af', 'af')): - // for a in ('x', 'y', 'z'): - // cog.outl("sp.%s%s = %s.%s();" % (fp, a, vp, a)) - // ]]] - sp.time_boot_ms = time_boot_ms; - sp.coordinate_frame = coordinate_frame; - sp.type_mask = type_mask; - sp.lat_int = lat_int; - sp.lon_int = lon_int; - sp.alt = alt; - sp.yaw = yaw; - sp.yaw_rate = yaw_rate; - sp.vx = v.x(); - sp.vy = v.y(); - sp.vz = v.z(); - sp.afx = af.x(); - sp.afy = af.y(); - sp.afz = af.z(); - // [[[end]]] (checksum: 30c9629ad309d488df1f63b683dac6a4) - - UAS_FCU(m_uas_)->send_message_ignore_drop(sp); - } -}; - -/** - * @brief This mixin adds set_attitude_target() - */ -template -class SetAttitudeTargetMixin { -public: - //! Message sepecification: @p https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET - void set_attitude_target(uint32_t time_boot_ms, - uint8_t type_mask, - Eigen::Quaterniond orientation, - Eigen::Vector3d body_rate, - float thrust) - { - mavros::UAS *m_uas_ = static_cast(this)->m_uas; - mavlink::common::msg::SET_ATTITUDE_TARGET sp = {}; - - m_uas_->msg_set_target(sp); - mavros::ftf::quaternion_to_mavlink(orientation, sp.q); - - // [[[cog: - // for f in ('time_boot_ms', 'type_mask', 'thrust'): - // cog.outl("sp.%s = %s;" % (f, f)) - // for f, v in (('roll', 'x'), ('pitch', 'y'), ('yaw', 'z')): - // cog.outl("sp.body_%s_rate = body_rate.%s();" % (f, v)) - // ]]] - sp.time_boot_ms = time_boot_ms; - sp.type_mask = type_mask; - sp.thrust = thrust; - sp.body_roll_rate = body_rate.x(); - sp.body_pitch_rate = body_rate.y(); - sp.body_yaw_rate = body_rate.z(); - // [[[end]]] (checksum: aa941484927bb7a7d39a2c31d08fcfc1) - - UAS_FCU(m_uas_)->send_message_ignore_drop(sp); - } -}; - -/** - * @brief This mixin adds TF2 listener thread to plugin - * - * It requires tf_frame_id, tf_child_frame_id strings - * tf_rate double and uas object pointer - */ -template -class TF2ListenerMixin { -public: - std::thread tf_thread; - std::string tf_thd_name; - - /** - * @brief start tf listener - * - * @param _thd_name listener thread name - * @param cbp plugin callback function - */ - void tf2_start(const char *_thd_name, void (D::*cbp)(const geometry_msgs::TransformStamped &) ) - { - tf_thd_name = _thd_name; - auto tf_transform_cb = std::bind(cbp, static_cast(this), std::placeholders::_1); - - tf_thread = std::thread([this, tf_transform_cb]() { - mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str()); - - mavros::UAS *m_uas_ = static_cast(this)->m_uas; - std::string &_frame_id = static_cast(this)->tf_frame_id; - std::string &_child_frame_id = static_cast(this)->tf_child_frame_id; - - ros::Rate rate(static_cast(this)->tf_rate); - while (ros::ok()) { - // Wait up to 3s for transform - if (m_uas_->tf2_buffer.canTransform(_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0))) { - try { - auto transform = m_uas_->tf2_buffer.lookupTransform( - _frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0)); - tf_transform_cb(transform); - } - catch (tf2::LookupException &ex) { - ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name.c_str(), ex.what()); - } - } - rate.sleep(); - } - }); - } - - /** - * @brief start tf listener syncronized with another topic - * - * @param _thd_name listener thread name - * @param cbp plugin callback function - */ - template - void tf2_start(const char *_thd_name, message_filters::Subscriber &topic_sub, void (D::*cbp)(const geometry_msgs::TransformStamped &, const typename T::ConstPtr &)) - { - tf_thd_name = _thd_name; - - tf_thread = std::thread([this, cbp, &topic_sub]() { - mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str()); - - mavros::UAS *m_uas_ = static_cast(this)->m_uas; - ros::NodeHandle &_sp_nh = static_cast(this)->sp_nh; - std::string &_frame_id = static_cast(this)->tf_frame_id; - std::string &_child_frame_id = static_cast(this)->tf_child_frame_id; - - tf2_ros::MessageFilter tf2_filter(topic_sub, m_uas_->tf2_buffer, _frame_id, 10, _sp_nh); - - ros::Rate rate(static_cast(this)->tf_rate); - while (ros::ok()) { - // Wait up to 3s for transform - if (m_uas_->tf2_buffer.canTransform(_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0))) { - try { - auto transform = m_uas_->tf2_buffer.lookupTransform( - _frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0)); - - tf2_filter.registerCallback(boost::bind(cbp, static_cast(this), transform, _1)); - } - catch (tf2::LookupException &ex) { - ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name.c_str(), ex.what()); - } - } - rate.sleep(); - } - }); - } -}; -} // namespace plugin -} // namespace mavros diff --git a/mavros/include/mavros/setpoint_mixin.hpp b/mavros/include/mavros/setpoint_mixin.hpp new file mode 100644 index 000000000..dc9c3c6bd --- /dev/null +++ b/mavros/include/mavros/setpoint_mixin.hpp @@ -0,0 +1,293 @@ +/** + * @brief Mixin for setpoint plugins + * @file setpoint_mixin.h + * @author Vladimir Ermakov + * + * @addtogroup plugin + * @{ + */ +/* + * Copyright 2014 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ + +#pragma once + +#ifndef MAVROS__SETPOINT_MIXIN_HPP_ +#define MAVROS__SETPOINT_MIXIN_HPP_ + +#include +#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" + +#include "geometry_msgs/msg/transform_stamped.hpp" + +namespace mavros +{ +namespace plugin +{ + +/** + * @brief This mixin adds set_position_target_local_ned() + */ +template +class SetPositionTargetLocalNEDMixin +{ +public: + //! Message specification: @p https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED + void set_position_target_local_ned( + uint32_t time_boot_ms, uint8_t coordinate_frame, + uint16_t type_mask, + Eigen::Vector3d p, + Eigen::Vector3d v, + Eigen::Vector3d af, + float yaw, float yaw_rate) + { + static_assert( + std::is_base_of::value, + "SetPositionTargetLocalNEDMixin should be used by mavros::plugin::Plugin child"); + + plugin::UASPtr uas_ = static_cast(this)->uas; + + mavlink::common::msg::SET_POSITION_TARGET_LOCAL_NED sp = {}; + uas_->msg_set_target(sp); + + // [[[cog: + // for f in ('time_boot_ms', 'coordinate_frame', 'type_mask', 'yaw', 'yaw_rate'): + // cog.outl(f"sp.{f} = {f};") + // for fp, vp in (('', 'p'), ('v', 'v'), ('af', 'af')): + // for a in ('x', 'y', 'z'): + // cog.outl(f"sp.{fp}{a} = {vp}.{a}();") + // ]]] + sp.time_boot_ms = time_boot_ms; + sp.coordinate_frame = coordinate_frame; + sp.type_mask = type_mask; + sp.yaw = yaw; + sp.yaw_rate = yaw_rate; + sp.x = p.x(); + sp.y = p.y(); + sp.z = p.z(); + sp.vx = v.x(); + sp.vy = v.y(); + sp.vz = v.z(); + sp.afx = af.x(); + sp.afy = af.y(); + sp.afz = af.z(); + // [[[end]]] (checksum: f8942bb13a7463a2cbadc9b745df25d0) + + uas_->send_message(sp); + } +}; + +/** + * @brief This mixin adds set_position_target_global_int() + */ +template +class SetPositionTargetGlobalIntMixin +{ +public: + //! Message specification: @p https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT + void set_position_target_global_int( + uint32_t time_boot_ms, uint8_t coordinate_frame, + uint16_t type_mask, + int32_t lat_int, int32_t lon_int, float alt, + Eigen::Vector3d v, + Eigen::Vector3d af, + float yaw, float yaw_rate) + { + static_assert( + std::is_base_of::value, + "SetPositionTargetGlobalIntMixin should be used by mavros::plugin::Plugin child"); + + plugin::UASPtr uas_ = static_cast(this)->uas; + + mavlink::common::msg::SET_POSITION_TARGET_GLOBAL_INT sp = {}; + uas_->msg_set_target(sp); + + // [[[cog: + // for f in ('time_boot_ms', 'coordinate_frame', 'type_mask', + // 'lat_int', 'lon_int', 'alt', 'yaw', 'yaw_rate'): + // cog.outl(f"sp.{f} = {f};") + // for fp, vp in (('v', 'v'), ('af', 'af')): + // for a in ('x', 'y', 'z'): + // cog.outl(f"sp.{fp}{a} = {vp}.{a}();") + // ]]] + sp.time_boot_ms = time_boot_ms; + sp.coordinate_frame = coordinate_frame; + sp.type_mask = type_mask; + sp.lat_int = lat_int; + sp.lon_int = lon_int; + sp.alt = alt; + sp.yaw = yaw; + sp.yaw_rate = yaw_rate; + sp.vx = v.x(); + sp.vy = v.y(); + sp.vz = v.z(); + sp.afx = af.x(); + sp.afy = af.y(); + sp.afz = af.z(); + // [[[end]]] (checksum: 82301ecd7936657d65e006cf7525e82a) + + uas_->send_message(sp); + } +}; + +/** + * @brief This mixin adds set_attitude_target() + */ +template +class SetAttitudeTargetMixin +{ +public: + //! Message sepecification: @p https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET + void set_attitude_target( + uint32_t time_boot_ms, + uint8_t type_mask, + Eigen::Quaterniond orientation, + Eigen::Vector3d body_rate, + float thrust) + { + static_assert( + std::is_base_of::value, + "SetAttitudeTargetMixin should be used by mavros::plugin::Plugin child"); + + plugin::UASPtr uas_ = static_cast(this)->uas; + + mavlink::common::msg::SET_ATTITUDE_TARGET sp = {}; + + uas_->msg_set_target(sp); + mavros::ftf::quaternion_to_mavlink(orientation, sp.q); + + // [[[cog: + // for f in ('time_boot_ms', 'type_mask', 'thrust'): + // cog.outl(f"sp.{f} = {f};") + // for f, v in (('roll', 'x'), ('pitch', 'y'), ('yaw', 'z')): + // cog.outl(f"sp.body_{f}_rate = body_rate.{v}();") + // ]]] + sp.time_boot_ms = time_boot_ms; + sp.type_mask = type_mask; + sp.thrust = thrust; + sp.body_roll_rate = body_rate.x(); + sp.body_pitch_rate = body_rate.y(); + sp.body_yaw_rate = body_rate.z(); + // [[[end]]] (checksum: d0910b0f92d233024163ebf957a3d642) + + uas_->send_message(sp); + } +}; + +/** + * @brief This mixin adds TF2 listener thread to plugin + * + * It requires tf_frame_id, tf_child_frame_id strings + * tf_rate double and uas object pointer + */ +template +class TF2ListenerMixin +{ +public: + std::string tf_thd_name; + rclcpp::TimerBase::SharedPtr timer_; + + /** + * @brief start tf listener + * + * @param _thd_name listener thread name + * @param cbp plugin callback function + */ + void tf2_start( + const char * _thd_name, void (D::* cbp)( + const geometry_msgs::msg::TransformStamped &) ) + { + tf_thd_name = _thd_name; + D * base = static_cast(this); + auto tf_transform_cb = std::bind(cbp, base, std::placeholders::_1); + + auto timer_callback = [this, base, tf_transform_cb]() -> void { + plugin::UASPtr _uas = base->uas; + std::string & _frame_id = base->tf_frame_id; + std::string & _child_frame_id = base->tf_child_frame_id; + if (_uas->tf2_buffer.canTransform( + _frame_id, _child_frame_id, tf2::TimePoint(), + tf2::durationFromSec(3.0))) + { + try { + auto transform = _uas->tf2_buffer.lookupTransform( + _frame_id, _child_frame_id, tf2::TimePoint(), tf2::durationFromSec(3.0)); + tf_transform_cb(transform); + } catch (tf2::LookupException & ex) { + RCLCPP_ERROR( + _uas->get_logger(), "%s: %s", tf_thd_name.c_str(), + ex.what()); + } + } + }; + + timer_ = + create_timer( + base->node, base->uas->get_clock(), + rclcpp::Duration::from_seconds(1.0 / base->tf_rate), timer_callback); + } + +#if 0 // XXX TODO(vooon): port me pls + /** + * @brief start tf listener syncronized with another topic + * + * @param _thd_name listener thread name + * @param cbp plugin callback function + */ + template + void tf2_start( + const char * _thd_name, message_filters::Subscriber & topic_sub, void (D::* cbp)( + const geometry_msgs::TransformStamped &, const typename T::ConstPtr &)) + { + tf_thd_name = _thd_name; + + tf_thread = std::thread( + [this, cbp, &topic_sub]() { + mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str()); + + mavros::UAS * _uas = static_cast(this)->uas; + auto & _node = static_cast(this)->node; + std::string & _frame_id = static_cast(this)->tf_frame_id; + std::string & _child_frame_id = static_cast(this)->tf_child_frame_id; + + tf2_ros::MessageFilter tf2_filter(topic_sub, m_uas_->tf2_buffer, _frame_id, 10, _node); + + rate = ros::Rate(static_cast(this)->tf_rate); + while (ros::ok()) { + // Wait up to 3s for transform + if (m_uas_->tf2_buffer.canTransform( + _frame_id, _child_frame_id, ros::Time(0), + ros::Duration(3.0))) + { + try { + auto transform = m_uas_->tf2_buffer.lookupTransform( + _frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0)); + + tf2_filter.registerCallback( + std::bind( + cbp, static_cast(this), transform, + std::placeholders::_1)); + } catch (tf2::LookupException & ex) { + RCLCPP_ERROR("tf2_buffer", "%s: %s", tf_thd_name.c_str(), ex.what()); + } + } + rate.sleep(); + } + }); + } +#endif +}; + +} // namespace plugin +} // namespace mavros + +#endif // MAVROS__SETPOINT_MIXIN_HPP_ diff --git a/mavros/include/mavros/uas_executor.hpp b/mavros/include/mavros/uas_executor.hpp new file mode 100644 index 000000000..87b8cbedb --- /dev/null +++ b/mavros/include/mavros/uas_executor.hpp @@ -0,0 +1,60 @@ +/* + * Copyright 2022 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief MAVROS UAS Node Executor + * @file uas_executor.hpp + * @author Vladimir Ermakov + * + * @addtogroup nodelib + * @{ + */ + +#pragma once + +#ifndef MAVROS__UAS_EXECUTOR_HPP_ +#define MAVROS__UAS_EXECUTOR_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "mavros/utils.hpp" + +namespace mavros +{ +namespace uas +{ + +/** + * Executor for UAS Plugin nodes + */ +class UASExecutor : public rclcpp::executors::MultiThreadedExecutor +{ +public: + explicit UASExecutor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); + ~UASExecutor() = default; + + void set_ids(uint8_t sysid, uint8_t compid); + +protected: + void run(size_t thread_id); + +private: + RCLCPP_DISABLE_COPY(UASExecutor) + + static size_t select_number_of_threads(); + + uint8_t source_system, source_component; +}; + +} // namespace uas +} // namespace mavros + +#endif // MAVROS__UAS_EXECUTOR_HPP_ diff --git a/mavros/include/mavros/utils.h b/mavros/include/mavros/utils.hpp similarity index 67% rename from mavros/include/mavros/utils.h rename to mavros/include/mavros/utils.hpp index c670ce2c3..8c3deab33 100644 --- a/mavros/include/mavros/utils.h +++ b/mavros/include/mavros/utils.hpp @@ -1,51 +1,58 @@ +/* + * Copyright 2014,2015,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief some useful utils - * @file utils.h + * @file utils.hpp * @author Vladimir Ermakov * * @addtogroup mavutils * @{ * @brief Some useful utils */ -/* - * Copyright 2014,2015,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ #pragma once -#include -#include -#include -#include +#ifndef MAVROS__UTILS_HPP_ +#define MAVROS__UTILS_HPP_ -#include +#include +#include // NOLINT + +#include "mavconn/thread_utils.hpp" +#include "mavros_msgs/mavlink_convert.hpp" +#include "mavconn/mavlink_dialect.hpp" // OS X compat: missing error codes #ifdef __APPLE__ -#define EBADE 50 /* Invalid exchange */ -#define EBADFD 81 /* File descriptor in bad state */ -#define EBADRQC 54 /* Invalid request code */ -#define EBADSLT 55 /* Invalid slot */ +#define EBADE 50 /* Invalid exchange */ +#define EBADFD 81 /* File descriptor in bad state */ +#define EBADRQC 54 /* Invalid request code */ +#define EBADSLT 55 /* Invalid slot */ #endif -namespace mavros { -namespace utils { +namespace mavros +{ +namespace utils +{ using mavconn::utils::format; +using mavconn::utils::set_this_thread_name; /** * Possible modes of timesync operation * * Used by UAS class, but it can't be defined inside because enum is used in utils. */ -enum class timesync_mode { - NONE = 0, //!< Disabled - MAVLINK, //!< Via TIMESYNC message - ONBOARD, - PASSTHROUGH, +enum class timesync_mode +{ + NONE = 0, //!< Disabled + MAVLINK, //!< Via TIMESYNC message + ONBOARD, + PASSTHROUGH, }; /** @@ -54,7 +61,7 @@ enum class timesync_mode { template constexpr typename std::underlying_type<_T>::type enum_value(_T e) { - return static_cast::type>(e); + return static_cast::type>(e); } /** @@ -84,9 +91,11 @@ std::string to_string(mavlink::common::LANDING_TARGET_TYPE e); template std::string to_string_enum(int e) { - return to_string(static_cast<_T>(e)); + return to_string(static_cast<_T>(e)); } +std::string enum_to_name(mavlink::minimal::MAV_TYPE e); + /** * @brief Function to match the received orientation received by MAVLink msg * and the rotation of the sensor relative to the FCU. @@ -96,27 +105,30 @@ Eigen::Quaterniond sensor_orientation_matching(mavlink::common::MAV_SENSOR_ORIEN /** * @brief Retrieve sensor orientation number from alias name. */ -int sensor_orientation_from_str(const std::string &sensor_orientation); +int sensor_orientation_from_str(const std::string & sensor_orientation); /** * @brief Retrieve timesync mode from name */ -timesync_mode timesync_mode_from_str(const std::string &mode); +timesync_mode timesync_mode_from_str(const std::string & mode); /** * @brief Retreive MAV_FRAME from name */ -mavlink::common::MAV_FRAME mav_frame_from_str(const std::string &mav_frame); +mavlink::common::MAV_FRAME mav_frame_from_str(const std::string & mav_frame); /** * @brief Retreive MAV_TYPE from name */ -mavlink::minimal::MAV_TYPE mav_type_from_str(const std::string &mav_type); +mavlink::minimal::MAV_TYPE mav_type_from_str(const std::string & mav_type); /** * @brief Retrieve landing target type from alias name */ -mavlink::common::LANDING_TARGET_TYPE landing_target_type_from_str(const std::string &landing_target_type); +mavlink::common::LANDING_TARGET_TYPE landing_target_type_from_str( + const std::string & landing_target_type); + +} // namespace utils +} // namespace mavros -} // namespace utils -} // namespace mavros +#endif // MAVROS__UTILS_HPP_ diff --git a/mavros/launch/apm.launch b/mavros/launch/apm.launch index 5061b4c3f..49c6f57bd 100644 --- a/mavros/launch/apm.launch +++ b/mavros/launch/apm.launch @@ -9,17 +9,16 @@ - - - - - - - - - - - - + + + + + + + + + + + diff --git a/mavros/launch/apm2.launch b/mavros/launch/apm2.launch deleted file mode 120000 index ad4fd45df..000000000 --- a/mavros/launch/apm2.launch +++ /dev/null @@ -1 +0,0 @@ -apm.launch \ No newline at end of file diff --git a/mavros/launch/apm_config.yaml b/mavros/launch/apm_config.yaml index 63eac7aa2..e1f3599b9 100644 --- a/mavros/launch/apm_config.yaml +++ b/mavros/launch/apm_config.yaml @@ -1,42 +1,49 @@ # Common configuration for APM2 autopilot # # node: -startup_px4_usb_quirk: false +/mavros/**: + ros__parameters: + startup_px4_usb_quirk: false # --- system plugins --- # sys_status & sys_time connection options -conn: - heartbeat_rate: 1.0 # send heartbeat rate in Hertz - heartbeat_mav_type: "ONBOARD_CONTROLLER" - timeout: 10.0 # heartbeat timeout in seconds - timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) - system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) +/mavros/**/conn: + ros__parameters: + heartbeat_rate: 1.0 # send heartbeat rate in Hertz + heartbeat_mav_type: "ONBOARD_CONTROLLER" + timeout: 10.0 # heartbeat timeout in seconds + timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) + system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) # sys_status -sys: - min_voltage: 10.0 # diagnostics min voltage, use a vector i.e. [16.2, 16.0] for multiple batteries, up-to 10 are supported - # to achieve the same on a ROS launch file do: [16.2, 16.0] - disable_diag: false # disable all sys_status diagnostics, except heartbeat +/mavros/**/sys: + ros__parameters: + min_voltage: [10.0] # diagnostics min voltage, use a vector i.e. [16.2, 16.0] for multiple batteries, up-to 10 are supported + # to achieve the same on a ROS launch file do: [16.2, 16.0] + disable_diag: false # disable all sys_status diagnostics, except heartbeat # sys_time -time: - time_ref_source: "fcu" # time_reference source - timesync_mode: MAVLINK - timesync_avg_alpha: 0.6 # timesync averaging factor +/mavros/**/time: + ros__parameters: + time_ref_source: "fcu" # time_reference source + timesync_mode: MAVLINK + timesync_avg_alpha: 0.6 # timesync averaging factor # --- mavros plugins (alphabetical order) --- # 3dr_radio -tdr_radio: - low_rssi: 40 # raw rssi lower level for diagnostics +/mavros/**/tdr_radio: + ros__parameters: + low_rssi: 40 # raw rssi lower level for diagnostics # actuator_control # None # command -cmd: - use_comp_id_system_control: false # quirk for some old FCUs +/mavros/**/cmd: + ros__parameters: + use_comp_id_system_control: false # quirk for some old FCUs # dummy # None @@ -45,35 +52,36 @@ cmd: # None # global_position -global_position: - frame_id: "map" # origin frame - child_frame_id: "base_link" # body-fixed frame - rot_covariance: 99999.0 # covariance for attitude? - gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m) - use_relative_alt: true # use relative altitude for local coordinates - tf: - send: false # send TF? - frame_id: "map" # TF frame_id - global_frame_id: "earth" # TF earth frame_id - child_frame_id: "base_link" # TF child_frame_id +/mavros/**/global_position: + ros__parameters: + frame_id: "map" # origin frame + child_frame_id: "base_link" # body-fixed frame + rot_covariance: 99999.0 # covariance for attitude? + gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m) + use_relative_alt: true # use relative altitude for local coordinates + tf.send: false # send TF? + tf.frame_id: "map" # TF frame_id + tf.global_frame_id: "earth" # TF earth frame_id + tf.child_frame_id: "base_link" # TF child_frame_id # imu_pub -imu: - frame_id: "base_link" - # need find actual values - linear_acceleration_stdev: 0.0003 - angular_velocity_stdev: 0.0003490659 // 0.02 degrees - orientation_stdev: 1.0 - magnetic_stdev: 0.0 +/mavros/**/imu: + ros__parameters: + frame_id: "base_link" + # need find actual values + linear_acceleration_stdev: 0.0003 + angular_velocity_stdev: 0.0003490659 # 0.02 degrees + orientation_stdev: 1.0 + magnetic_stdev: 0.0 # local_position -local_position: - frame_id: "map" - tf: - send: false +/mavros/**/local_position: + ros__parameters: frame_id: "map" - child_frame_id: "base_link" - send_fcu: false + tf.send: false + tf.frame_id: "map" + tf.child_frame_id: "base_link" + tf.send_fcu: false # param # None, used for FCU params @@ -81,58 +89,57 @@ local_position: # rc_io # None -# safety_area -safety_area: - p1: {x: 1.0, y: 1.0, z: 1.0} - p2: {x: -1.0, y: -1.0, z: -1.0} - # setpoint_accel -setpoint_accel: - send_force: false +/mavros/**/setpoint_accel: + ros__parameters: + send_force: false # setpoint_attitude -setpoint_attitude: - reverse_thrust: false # allow reversed thrust - use_quaternion: false # enable PoseStamped topic subscriber - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "map" - child_frame_id: "target_attitude" - rate_limit: 50.0 +/mavros/**/setpoint_attitude: + ros__parameters: + reverse_thrust: false # allow reversed thrust + use_quaternion: false # enable PoseStamped topic subscriber + tf.listen: false # enable tf listener (disable topic subscribers) + tf.frame_id: "map" + tf.child_frame_id: "target_attitude" + tf.rate_limit: 50.0 # setpoint_raw -setpoint_raw: - thrust_scaling: 1.0 # specify thrust scaling (normalized, 0 to 1) for thrust (like PX4) +/mavros/**/setpoint_raw: + ros__parameters: + thrust_scaling: 1.0 # specify thrust scaling (normalized, 0 to 1) for thrust (like PX4) # setpoint_position -setpoint_position: - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "map" - child_frame_id: "target_position" - rate_limit: 50.0 - mav_frame: LOCAL_NED - +/mavros/**/setpoint_position: + ros__parameters: + tf.listen: false # enable tf listener (disable topic subscribers) + tf.frame_id: "map" + tf.child_frame_id: "target_position" + tf.rate_limit: 50.0 + mav_frame: LOCAL_NED + # guided_target -guided_target: - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "map" - child_frame_id: "target_position" - rate_limit: 50.0 +/mavros/**/guided_target: + ros__parameters: + tf.listen: false # enable tf listener (disable topic subscribers) + tf.frame_id: "map" + tf.child_frame_id: "target_position" + tf.rate_limit: 50.0 # setpoint_velocity -setpoint_velocity: - mav_frame: LOCAL_NED +/mavros/**/setpoint_velocity: + ros__parameters: + mav_frame: LOCAL_NED # vfr_hud # None # waypoint -mission: - pull_after_gcs: true # update mission if gcs updates - use_mission_item_int: true # use the MISSION_ITEM_INT message instead of MISSION_ITEM - # for uploading waypoints to FCU +/mavros/**/mission: + ros__parameters: + pull_after_gcs: true # update mission if gcs updates + use_mission_item_int: true # use the MISSION_ITEM_INT message instead of MISSION_ITEM + # for uploading waypoints to FCU # --- mavros extras plugins (same order) --- @@ -144,131 +151,136 @@ mission: # distance_sensor ## Currently available orientations: -# Check http://wiki.ros.org/mavros/Enumerations +# Check http://wiki.ros.org/mavros/**/Enumerations ## -distance_sensor: - rangefinder_pub: - id: 0 - frame_id: "lidar" - #orientation: PITCH_270 # sended by FCU - field_of_view: 0.0 # XXX TODO - send_tf: false - sensor_position: {x: 0.0, y: 0.0, z: -0.1} - rangefinder_sub: - subscriber: true - id: 1 - orientation: PITCH_270 # only that orientation are supported by APM 3.4+ +/mavros/**/distance_sensor: + ros__parameters: + config: | + rangefinder_pub: + id: 0 + frame_id: "lidar" + #orientation: PITCH_270 # sended by FCU + field_of_view: 0.0 # XXX TODO + send_tf: false + sensor_position: {x: 0.0, y: 0.0, z: -0.1} + rangefinder_sub: + subscriber: true + id: 1 + orientation: PITCH_270 # only that orientation are supported by APM 3.4+ # image_pub -image: - frame_id: "px4flow" +/mavros/**/image: + ros__parameters: + frame_id: "px4flow" # fake_gps -fake_gps: - # select data source - use_mocap: true # ~mocap/pose - mocap_transform: false # ~mocap/tf instead of pose - mocap_withcovariance: false # ~mocap/pose uses PoseWithCovarianceStamped Message - use_vision: false # ~vision (pose) - use_hil_gps: true - gps_id: 4 - # origin (default: Zürich) - geo_origin: - lat: 47.3667 # latitude [degrees] - lon: 8.5500 # longitude [degrees] - alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters] - eph: 2.0 - epv: 2.0 - horiz_accuracy: 0.5 - vert_accuracy: 0.5 - speed_accuracy: 0.0 - satellites_visible: 6 # virtual number of visible satellites - fix_type: 3 # type of GPS fix (default: 3D) - tf: - listen: false - send: false # send TF? - frame_id: "map" # TF frame_id - child_frame_id: "fix" # TF child_frame_id - rate_limit: 10.0 # TF rate - gps_rate: 5.0 # GPS data publishing rate +/mavros/**/fake_gps: + ros__parameters: + # select data source + use_mocap: true # ~mocap/pose + mocap_transform: false # ~mocap/tf instead of pose + mocap_withcovariance: false # ~mocap/pose uses PoseWithCovarianceStamped Message + use_vision: false # ~vision (pose) + use_hil_gps: true + gps_id: 4 + # origin (default: Zürich) + geo_origin.lat: 47.3667 # latitude [degrees] + geo_origin.lon: 8.5500 # longitude [degrees] + geo_origin.alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters] + eph: 2.0 + epv: 2.0 + horiz_accuracy: 0.5 + vert_accuracy: 0.5 + speed_accuracy: 0.0 + satellites_visible: 6 # virtual number of visible satellites + fix_type: 3 # type of GPS fix (default: 3D) + tf.listen: false + tf.send: false # send TF? + tf.frame_id: "map" # TF frame_id + tf.child_frame_id: "fix" # TF child_frame_id + tf.rate_limit: 10.0 # TF rate + gps_rate: 5.0 # GPS data publishing rate # landing_target -landing_target: - listen_lt: false - mav_frame: "LOCAL_NED" - land_target_type: "VISION_FIDUCIAL" - image: - width: 640 # [pixels] - height: 480 - camera: - fov_x: 2.0071286398 # default: 115 [degrees] - fov_y: 2.0071286398 - tf: - send: true - listen: false - frame_id: "landing_target" - child_frame_id: "camera_center" - rate_limit: 10.0 - target_size: {x: 0.3, y: 0.3} +/mavros/**/landing_target: + ros__parameters: + listen_lt: false + mav_frame: "LOCAL_NED" + land_target_type: "VISION_FIDUCIAL" + image.width: 640 # [pixels] + image.height: 480 + camera.fov_x: 2.0071286398 # default: 115 [degrees] + camera.fov_y: 2.0071286398 + tf.send: true + tf.listen: false + tf.frame_id: "landing_target" + tf.child_frame_id: "camera_center" + tf.rate_limit: 10.0 + target_size: {x: 0.3, y: 0.3} # mocap_pose_estimate -mocap: - # select mocap source - use_tf: false # ~mocap/tf - use_pose: true # ~mocap/pose +/mavros/**/mocap: + ros__parameters: + # select mocap source + use_tf: false # ~mocap/tf + use_pose: true # ~mocap/pose # mount_control -mount: - debounce_s: 4.0 - err_threshold_deg: 10.0 - negate_measured_roll: false - negate_measured_pitch: false - negate_measured_yaw: false +/mavros/**/mount: + ros__parameters: + debounce_s: 4.0 + err_threshold_deg: 10.0 + negate_measured_roll: false + negate_measured_pitch: false + negate_measured_yaw: false # odom -odometry: - frame_tf: - desired_frame: "ned" - estimator_type: 3 # check enum MAV_ESTIMATOR_TYPE in +/mavros/**/odometry: + ros__parameters: + fcu.odom_parent_id_des: "map" # desired parent frame rotation of the FCU's odometry + fcu.odom_child_id_des: "map" # desired child frame rotation of the FCU's odometry # px4flow -px4flow: - frame_id: "px4flow" - ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter - ranger_min_range: 0.3 # meters - ranger_max_range: 5.0 # meters +/mavros/**/px4flow: + ros__parameters: + frame_id: "px4flow" + ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter + ranger_min_range: 0.3 # meters + ranger_max_range: 5.0 # meters # vision_pose_estimate -vision_pose: - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "map" - child_frame_id: "vision_estimate" - rate_limit: 10.0 +/mavros/**/vision_pose: + ros__parameters: + tf.listen: false # enable tf listener (disable topic subscribers) + tf.frame_id: "map" + tf.child_frame_id: "vision_estimate" + tf.rate_limit: 10.0 # vision_speed_estimate -vision_speed: - listen_twist: true # enable listen to twist topic, else listen to vec3d topic - twist_cov: true # enable listen to twist with covariance topic +/mavros/**/vision_speed: + ros__parameters: + listen_twist: true # enable listen to twist topic, else listen to vec3d topic + twist_cov: true # enable listen to twist with covariance topic # vibration -vibration: - frame_id: "base_link" +/mavros/**/vibration: + ros__parameters: + frame_id: "base_link" # wheel_odometry -wheel_odometry: - count: 2 # number of wheels to compute odometry - use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry - wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) - wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) - send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance) - send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry - frame_id: "map" # origin frame - child_frame_id: "base_link" # body-fixed frame - vel_error: 0.1 # wheel velocity measurement error 1-std (m/s) - tf: - send: true - frame_id: "map" - child_frame_id: "base_link" +/mavros/**/wheel_odometry: + ros__parameters: + count: 2 # number of wheels to compute odometry + use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry + wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) + wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) + send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance) + send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry + frame_id: "map" # origin frame + child_frame_id: "base_link" # body-fixed frame + vel_error: 0.1 # wheel velocity measurement error 1-std (m/s) + tf.send: true + tf.frame_id: "map" + tf.child_frame_id: "base_link" # vim:set ts=2 sw=2 et: diff --git a/mavros/launch/apm_pluginlists.yaml b/mavros/launch/apm_pluginlists.yaml index 4f11ed516..e8d2d3ad1 100644 --- a/mavros/launch/apm_pluginlists.yaml +++ b/mavros/launch/apm_pluginlists.yaml @@ -1,17 +1,18 @@ -plugin_blacklist: -# common -- actuator_control -- ftp -- safety_area -- hil -# extras -- altitude -- debug_value -- image_pub -- px4flow -- vibration -- vision_speed_estimate -- wheel_odometry +/mavros/**: + ros__parameters: + plugin_denylist: + # common + - actuator_control + - ftp + - hil + # extras + - altitude + - debug_value + - image_pub + - px4flow + - vibration + - vision_speed_estimate + - wheel_odometry -plugin_whitelist: [] -#- 'sys_*' + # plugin_allowlist: + # - 'sys_*' diff --git a/mavros/launch/event_launcher.yaml b/mavros/launch/event_launcher.yaml deleted file mode 100644 index b32c667b7..000000000 --- a/mavros/launch/event_launcher.yaml +++ /dev/null @@ -1,69 +0,0 @@ -# Example configuration for event_launcher -# vim:set ts=2 sw=2 et: - -# Notify on arming change -arming_notify: # name of process, used only for logging - event: armed, disarmed # prefer to split by comma - action: run run # but space are fine too - shell: notify-send 'UAV Arming status' 'Has changed' # maybe later that will provide some environment variables - -# you may define custom action source (std_srvs/Trigger) -mytrigger: # this was name of event source - service: ~trigger_notify - -trigger_notify: - event: mytrigger - action: run # trigger generate only one event - shell: notify-send 'My Trigger' 'Has called' - -# you may use several event sources -othertrigger: - service: ~trigger_stop - -trigger_notify2: - event: mytrigger, othertrigger - action: run, stop # stop does nothin in this example, because notify-send don't blocks - shell: # array will simplify parsing step - - notify-send - - Trigger \#2 - - Started - -# rosbag example -start_recorder: - service: start_recorder - -start_talker: - service: start_talker - -start_launch: - service: start_launch - -stop_all: - service: stop_all - -wireshark: - service: run_wireshark - -# env-variables are expanded for first element -trigger_wireshark: - event: wireshark, stop_all - action: run, stop - shell: - - $HOME/bin/al76-wireshark.sh # will expand - - $PWD # nope - -rosbag_armed: - event: [armed, disarmed, start_recorder, stop_all] # yaml array is fine too - action: run, stop, run, stop - shell: rosrun rosbag record -a -x '/mavlink/.*' --lz4 -o 'uav' - -rosrun_talker: - event: start_talker, stop_all - action: run, stop - shell: rosrun roscpp_tutorials talker - -roslaunch_triggered: - event: start_launch, stop_all - action: run, stop - logfile: /tmp/launch.log - shell: roslaunch roscpp_tutorials talker_listener.launch diff --git a/mavros/launch/mavlink_bridge.launch b/mavros/launch/mavlink_bridge.launch deleted file mode 100644 index 4cfe194f9..000000000 --- a/mavros/launch/mavlink_bridge.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/mavros/launch/multi_uas.launch b/mavros/launch/multi_uas.launch new file mode 100644 index 000000000..73c820cc6 --- /dev/null +++ b/mavros/launch/multi_uas.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/mavros/launch/node.launch b/mavros/launch/node.launch index 1b4b22046..fee029e06 100644 --- a/mavros/launch/node.launch +++ b/mavros/launch/node.launch @@ -11,16 +11,16 @@ + - - - - - - - + + + + + + - - + + diff --git a/mavros/launch/px4.launch b/mavros/launch/px4.launch index 6a37f5c11..860ad2903 100644 --- a/mavros/launch/px4.launch +++ b/mavros/launch/px4.launch @@ -1,7 +1,4 @@ - - - @@ -9,17 +6,18 @@ + - - - - - - - - - - - + + + + + + + + + + + diff --git a/mavros/launch/px4_config.yaml b/mavros/launch/px4_config.yaml index 6b2913c94..942c8dffe 100644 --- a/mavros/launch/px4_config.yaml +++ b/mavros/launch/px4_config.yaml @@ -1,41 +1,44 @@ # Common configuration for PX4 autopilot # -# node: -startup_px4_usb_quirk: false - -# --- system plugins --- +/mavros/**: + ros__parameters: + startup_px4_usb_quirk: false # sys_status & sys_time connection options -conn: - heartbeat_rate: 1.0 # send heartbeat rate in Hertz - timeout: 10.0 # heartbeat timeout in seconds - timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) - system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) +/mavros/**/conn: + ros__parameters: + heartbeat_rate: 1.0 # send heartbeat rate in Hertz + timeout: 10.0 # heartbeat timeout in seconds + timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) + system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) # sys_status -sys: - min_voltage: 10.0 # diagnostics min voltage, use a vector i.e. [16.2, 16.0] for multiple batteries, up-to 10 are supported - # to achieve the same on a ROS launch file do: [16.2, 16.0] - disable_diag: false # disable all sys_status diagnostics, except heartbeat +/mavros/**/sys: + ros__parameters: + min_voltage: [10.0] # diagnostics min voltage, use a vector i.e. [16.2, 16.0] for multiple batteries, up-to 10 are supported to achieve the same on a ROS launch file do: [16.2, 16.0] + disable_diag: false # disable all sys_status diagnostics, except heartbeat # sys_time -time: - time_ref_source: "fcu" # time_reference source - timesync_mode: MAVLINK - timesync_avg_alpha: 0.6 # timesync averaging factor +/mavros/**/time: + ros__parameters: + time_ref_source: fcu # time_reference source + timesync_mode: MAVLINK + timesync_avg_alpha: 0.6 # timesync averaging factor # --- mavros plugins (alphabetical order) --- # 3dr_radio -tdr_radio: - low_rssi: 40 # raw rssi lower level for diagnostics +/mavros/**/tdr_radio: + ros__parameters: + low_rssi: 40 # raw rssi lower level for diagnostics # actuator_control # None # command -cmd: - use_comp_id_system_control: false # quirk for some old FCUs +/mavros/**/cmd: + ros__parameters: + use_comp_id_system_control: false # quirk for some old FCUs # dummy # None @@ -44,35 +47,36 @@ cmd: # None # global_position -global_position: - frame_id: "map" # origin frame - child_frame_id: "base_link" # body-fixed frame - rot_covariance: 99999.0 # covariance for attitude? - gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m) - use_relative_alt: true # use relative altitude for local coordinates - tf: - send: false # send TF? - frame_id: "map" # TF frame_id - global_frame_id: "earth" # TF earth frame_id - child_frame_id: "base_link" # TF child_frame_id +/mavros/**/global_position: + ros__parameters: + frame_id: "map" # origin frame + child_frame_id: "base_link" # body-fixed frame + rot_covariance: 99999.0 # covariance for attitude? + gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m) + use_relative_alt: true # use relative altitude for local coordinates + tf.send: false # send TF? + tf.frame_id: "map" # TF frame_id + tf.global_frame_id: "earth" # TF earth frame_id + tf.child_frame_id: "base_link" # TF child_frame_id # imu_pub -imu: - frame_id: "base_link" - # need find actual values - linear_acceleration_stdev: 0.0003 - angular_velocity_stdev: 0.0003490659 // 0.02 degrees - orientation_stdev: 1.0 - magnetic_stdev: 0.0 +/mavros/**/imu: + ros__parameters: + frame_id: base_link + # need find actual values + linear_acceleration_stdev: 0.0003 + angular_velocity_stdev: 0.0003490659 # 0.02 degrees + orientation_stdev: 1.0 + magnetic_stdev: 0.0 # local_position -local_position: - frame_id: "map" - tf: - send: false +/mavros/**/local_position: + ros__parameters: frame_id: "map" - child_frame_id: "base_link" - send_fcu: false + tf.send: false + tf.frame_id: "map" + tf.child_frame_id: "base_link" + tf.send_fcu: false # param # None, used for FCU params @@ -80,51 +84,51 @@ local_position: # rc_io # None -# safety_area -safety_area: - p1: {x: 1.0, y: 1.0, z: 1.0} - p2: {x: -1.0, y: -1.0, z: -1.0} - # setpoint_accel -setpoint_accel: - send_force: false +/mavros/**/setpoint_accel: + ros__parameters: + send_force: false # setpoint_attitude -setpoint_attitude: - reverse_thrust: false # allow reversed thrust - use_quaternion: false # enable PoseStamped topic subscriber - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "map" - child_frame_id: "target_attitude" - rate_limit: 50.0 - -setpoint_raw: - thrust_scaling: 1.0 # used in setpoint_raw attitude callback. - # Note: PX4 expects normalized thrust values between 0 and 1, which means that - # the scaling needs to be unitary and the inputs should be 0..1 as well. +/mavros/**/setpoint_attitude: + ros__parameters: + reverse_thrust: false # allow reversed thrust + use_quaternion: false # enable PoseStamped topic subscriber + tf.listen: false # enable tf listener (disable topic subscribers) + tf.frame_id: "map" + tf.child_frame_id: "target_attitude" + tf.rate_limit: 50.0 + +/mavros/**/setpoint_raw: + ros__parameters: + thrust_scaling: 1.0 # used in setpoint_raw attitude callback. + # Note: PX4 expects normalized thrust values between 0 and 1, which means that + # the scaling needs to be unitary and the inputs should be 0..1 as well. # setpoint_position -setpoint_position: - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "map" - child_frame_id: "target_position" - rate_limit: 50.0 - mav_frame: LOCAL_NED +/mavros/**/setpoint_position: + ros__parameters: + tf.listen: false # enable tf listener (disable topic subscribers) + tf.frame_id: "map" + tf.child_frame_id: "target_position" + tf.rate_limit: 50.0 + mav_frame: LOCAL_NED # setpoint_velocity -setpoint_velocity: - mav_frame: LOCAL_NED +/mavros/**/setpoint_velocity: + ros__parameters: + mav_frame: LOCAL_NED # vfr_hud # None # waypoint -mission: - pull_after_gcs: true # update mission if gcs updates - use_mission_item_int: true # use the MISSION_ITEM_INT message instead of MISSION_ITEM - # for uploading waypoints to FCU +/mavros/**/mission: + ros__parameters: + pull_after_gcs: true # update mission if gcs updates + use_mission_item_int: true # use the MISSION_ITEM_INT message instead of MISSION_ITEM + # for uploading waypoints to FCU + # --- mavros extras plugins (same order) --- @@ -136,146 +140,155 @@ mission: # distance_sensor ## Currently available orientations: -# Check http://wiki.ros.org/mavros/Enumerations +# Check http://wiki.ros.org/mavros/**/Enumerations ## -distance_sensor: - hrlv_ez4_pub: - id: 0 - frame_id: "hrlv_ez4_sonar" - orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing - field_of_view: 0.0 # XXX TODO - send_tf: true - sensor_position: {x: 0.0, y: 0.0, z: -0.1} - lidarlite_pub: - id: 1 - frame_id: "lidarlite_laser" - orientation: PITCH_270 - field_of_view: 0.0 # XXX TODO - send_tf: true - sensor_position: {x: 0.0, y: 0.0, z: -0.1} - sonar_1_sub: - subscriber: true - id: 2 - orientation: PITCH_270 - horizontal_fov_ratio: 1.0 # horizontal_fov = horizontal_fov_ratio * msg.field_of_view - vertical_fov_ratio: 1.0 # vertical_fov = vertical_fov_ratio * msg.field_of_view - # custom_orientation: # Used for orientation == CUSTOM - # roll: 0 - # pitch: 270 - # yaw: 0 - laser_1_sub: - subscriber: true - id: 3 - orientation: PITCH_270 +/mavros/**/distance_sensor: + ros__parameters: + config: | + hrlv_ez4_pub: + id: 0 + frame_id: "hrlv_ez4_sonar" + orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing + field_of_view: 0.0 # XXX TODO + send_tf: true + sensor_position: {x: 0.0, y: 0.0, z: -0.1} + lidarlite_pub: + id: 1 + frame_id: "lidarlite_laser" + orientation: PITCH_270 + field_of_view: 0.0 # XXX TODO + send_tf: true + sensor_position: {x: 0.0, y: 0.0, z: -0.1} + sonar_1_sub: + subscriber: true + id: 2 + orientation: PITCH_270 + horizontal_fov_ratio: 1.0 # horizontal_fov = horizontal_fov_ratio * msg.field_of_view + vertical_fov_ratio: 1.0 # vertical_fov = vertical_fov_ratio * msg.field_of_view + # custom_orientation: # Used for orientation == CUSTOM + # roll: 0 + # pitch: 270 + # yaw: 0 + laser_1_sub: + subscriber: true + id: 3 + orientation: PITCH_270 # image_pub -image: - frame_id: "px4flow" +/mavros/**/image: + ros__parameters: + frame_id: "px4flow" + # fake_gps -fake_gps: - # select data source - use_mocap: true # ~mocap/pose - mocap_transform: true # ~mocap/tf instead of pose - use_vision: false # ~vision (pose) - # origin (default: Zürich) - geo_origin: - lat: 47.3667 # latitude [degrees] - lon: 8.5500 # longitude [degrees] - alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters] - eph: 2.0 - epv: 2.0 - satellites_visible: 5 # virtual number of visible satellites - fix_type: 3 # type of GPS fix (default: 3D) - tf: - listen: false - send: false # send TF? - frame_id: "map" # TF frame_id - child_frame_id: "fix" # TF child_frame_id - rate_limit: 10.0 # TF rate - gps_rate: 5.0 # GPS data publishing rate +/mavros/**/fake_gps: + ros__parameters: + # select data source + use_mocap: true # ~mocap/pose + mocap_transform: true # ~mocap/tf instead of pose + use_vision: false # ~vision (pose) + # origin (default: Zürich) + geo_origin.lat: 47.3667 # latitude [degrees] + geo_origin.lon: 8.5500 # longitude [degrees] + geo_origin.alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters] + eph: 2.0 + epv: 2.0 + satellites_visible: 5 # virtual number of visible satellites + fix_type: 3 # type of GPS fix (default: 3D) + tf.listen: false + tf.send: false # send TF? + tf.frame_id: "map" # TF frame_id + tf.child_frame_id: "fix" # TF child_frame_id + tf.rate_limit: 10.0 # TF rate + gps_rate: 5.0 # GPS data publishing rate + # landing_target -landing_target: - listen_lt: false - mav_frame: "LOCAL_NED" - land_target_type: "VISION_FIDUCIAL" - image: - width: 640 # [pixels] - height: 480 - camera: - fov_x: 2.0071286398 # default: 115 [degrees] - fov_y: 2.0071286398 - tf: - send: true - listen: false - frame_id: "landing_target" - child_frame_id: "camera_center" - rate_limit: 10.0 - target_size: {x: 0.3, y: 0.3} +/mavros/**/landing_target: + ros__parameters: + listen_lt: false + mav_frame: "LOCAL_NED" + land_target_type: "VISION_FIDUCIAL" + image.width: 640 # [pixels] + image.height: 480 + camera.fov_x: 2.0071286398 # default: 115 [degrees] + camera.fov_y: 2.0071286398 + tf.send: true + tf.listen: false + tf.frame_id: "landing_target" + tf.child_frame_id: "camera_center" + tf.rate_limit: 10.0 + target_size: {x: 0.3, y: 0.3} + # mocap_pose_estimate -mocap: - # select mocap source - use_tf: false # ~mocap/tf - use_pose: true # ~mocap/pose +/mavros/**/mocap: + ros__parameters: + # select mocap source + use_tf: false # ~mocap/tf + use_pose: true # ~mocap/pose # mount_control -mount: - debounce_s: 4.0 - err_threshold_deg: 10.0 - negate_measured_roll: false - negate_measured_pitch: false - negate_measured_yaw: false +/mavros/**/mount: + ros__parameters: + debounce_s: 4.0 + err_threshold_deg: 10.0 + negate_measured_roll: false + negate_measured_pitch: false + negate_measured_yaw: false # odom -odometry: - fcu: - odom_parent_id_des: "map" # desired parent frame rotation of the FCU's odometry - odom_child_id_des: "base_link" # desired child frame rotation of the FCU's odometry +/mavros/**/odometry: + ros__parameters: + fcu.odom_parent_id_des: "map" # desired parent frame rotation of the FCU's odometry + fcu.odom_child_id_des: "base_link" # desired child frame rotation of the FCU's odometry # px4flow -px4flow: - frame_id: "px4flow" - ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter - ranger_min_range: 0.3 # meters - ranger_max_range: 5.0 # meters +/mavros/**/px4flow: + ros__parameters: + frame_id: "px4flow" + ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter + ranger_min_range: 0.3 # meters + ranger_max_range: 5.0 # meters # vision_pose_estimate -vision_pose: - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "odom" - child_frame_id: "vision_estimate" - rate_limit: 10.0 +/mavros/**/vision_pose: + ros__parameters: + tf.listen: false # enable tf listener (disable topic subscribers) + tf.frame_id: "odom" + tf.child_frame_id: "vision_estimate" + tf.rate_limit: 10.0 # vision_speed_estimate -vision_speed: - listen_twist: true # enable listen to twist topic, else listen to vec3d topic - twist_cov: true # enable listen to twist with covariance topic +/mavros/**/vision_speed: + ros__parameters: + listen_twist: true # enable listen to twist topic, else listen to vec3d topic + twist_cov: true # enable listen to twist with covariance topic # vibration -vibration: - frame_id: "base_link" +/mavros/**/vibration: + ros__parameters: + frame_id: "base_link" + # wheel_odometry -wheel_odometry: - count: 2 # number of wheels to compute odometry - use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry - wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) - wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) - send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance) - send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry - frame_id: "odom" # origin frame - child_frame_id: "base_link" # body-fixed frame - vel_error: 0.1 # wheel velocity measurement error 1-std (m/s) - tf: - send: false - frame_id: "odom" - child_frame_id: "base_link" +/mavros/**/wheel_odometry: + ros__parameters: + count: 2 # number of wheels to compute odometry + use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry + wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) + wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) + send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance) + send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry + frame_id: "odom" # origin frame + child_frame_id: "base_link" # body-fixed frame + vel_error: 0.1 # wheel velocity measurement error 1-std (m/s) + tf.send: false + tf.frame_id: "odom" + tf.child_frame_id: "base_link" # camera -camera: - frame_id: "base_link" - -# vim:set ts=2 sw=2 et: +/mavros/**/camera: + ros__parameters: + frame_id: "base_link" + diff --git a/mavros/launch/px4_pluginlists.yaml b/mavros/launch/px4_pluginlists.yaml index 92cfacac7..41a3a22e7 100644 --- a/mavros/launch/px4_pluginlists.yaml +++ b/mavros/launch/px4_pluginlists.yaml @@ -1,13 +1,14 @@ -plugin_blacklist: -# common -- safety_area -# extras -- image_pub -- vibration -- distance_sensor -- rangefinder -- wheel_odometry +/mavros/**: + ros__parameters: + plugin_denylist: + # common + # extras + - image_pub + - vibration + - distance_sensor + - rangefinder + - wheel_odometry -plugin_whitelist: [] -#- 'sys_*' + # plugin_allowlist: + # - 'sys_*' diff --git a/mavros/mavros/__init__.py b/mavros/mavros/__init__.py new file mode 100644 index 000000000..61785ab74 --- /dev/null +++ b/mavros/mavros/__init__.py @@ -0,0 +1,84 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: + +from . import ( + command, + ftp, + global_position, + local_position, + mission, + param, + setpoint, + system, +) +from .base import BaseNode, cached_property + + +class Client(BaseNode): + """ + Client provides some convinient methods to work with MAVROS API. + + Plugin interfaces are lazy constructed. + + NOTE: Client is a ROS2 Node. + """ + + @cached_property + def system(self) -> system.SystemPlugin: + return system.SystemPlugin(self) + + @cached_property + def command(self) -> command.CommandPlugin: + return command.CommandPlugin(self) + + @cached_property + def param(self) -> param.ParamPlugin: + return param.ParamPlugin(self) + + @cached_property + def waypoint(self) -> mission.WaypointPlugin: + return mission.WaypointPlugin(self) + + @cached_property + def geofence(self) -> mission.GeofencePlugin: + return mission.GeofencePlugin(self) + + @cached_property + def rallypoint(self) -> mission.RallypointPlugin: + return mission.RallypointPlugin(self) + + @cached_property + def setpoint_accel(self) -> setpoint.SetpointAccelPlugin: + return setpoint.SetpointAccelPlugin(self) + + @cached_property + def setpoint_attitude(self) -> setpoint.SetpointAttitudePlugin: + return setpoint.SetpointAttitudePlugin(self) + + @cached_property + def setpoint_position(self) -> setpoint.SetpointPositionPlugin: + return setpoint.SetpointPositionPlugin(self) + + @cached_property + def setpoint_raw(self) -> setpoint.SetpointRawPlugin: + return setpoint.SetpointRawPlugin(self) + + @cached_property + def setpoint_trajectory(self) -> setpoint.SetpointTrajectoryPlugin: + return setpoint.SetpointTrajectoryPlugin(self) + + @cached_property + def setpoint_velocity(self) -> setpoint.SetpointVelocityPlugin: + return setpoint.SetpointVelocityPlugin(self) + + @cached_property + def ftp(self) -> ftp.FTPPlugin: + return ftp.FTPPlugin(self) + + @cached_property + def global_position(self) -> global_position.GlobalPositionPlugin: + return global_position.GlobalPositionPlugin(self) + + @cached_property + def local_position(self) -> local_position.LocalPositionPlugin: + return local_position.LocalPositionPlugin(self) diff --git a/mavros/mavros/base.py b/mavros/mavros/base.py new file mode 100644 index 000000000..1b05a31f1 --- /dev/null +++ b/mavros/mavros/base.py @@ -0,0 +1,210 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md + +import random +import string +import threading +import typing +from dataclasses import dataclass, field, fields +from functools import cached_property + +import rclpy # noqa F401 +import rclpy.node +import rclpy.qos + +DEFAULT_NAMESPACE = "mavros" +DEFAULT_NODE_NAME_PREFIX = "mavpy_" + +SERVICE_WAIT_TIMEOUT = 5.0 + +# STATE_QOS used for state topics, like ~/state, ~/mission/waypoints etc. +STATE_QOS = rclpy.qos.QoSProfile( + depth=10, durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL +) + +# SENSOR_QOS used for most of sensor streams +SENSOR_QOS = rclpy.qos.qos_profile_sensor_data + +# PARAMETERS_QOS used for parameter streams +PARAMETERS_QOS = rclpy.qos.qos_profile_parameters + +TopicType = typing.Union[typing.Tuple, str] +QoSType = typing.Union[rclpy.qos.QoSProfile, int] +ServiceCallable = rclpy.node.Callable[ + [rclpy.node.SrvTypeRequest, rclpy.node.SrvTypeResponse], rclpy.node.SrvTypeResponse +] +SubscriptionCallable = rclpy.node.Callable[[rclpy.node.MsgType], None] + + +class ServiceWaitTimeout(RuntimeError): + pass + + +def wait_for_service(client: rclpy.node.Client, lg: typing.Optional[typing.Any]): + ready = client.wait_for_service(timeout_sec=SERVICE_WAIT_TIMEOUT) + if not ready: + topic = client.srv_name + if lg: + lg.error("wait for service time out: {topic}") + raise ServiceWaitTimeout(topic) + + +@dataclass +class UASParams: + uas_url: str = "" + fcu_protocol: str = "v2.0" + system_id: int = 1 + component_id: int = 191 + target_system_id: int = 1 + target_component_id: int = 1 + plugin_allowlist: typing.List[str] = field(default_factory=list) + plugin_denylist: typing.List[str] = field(default_factory=list) + + @property + def uas_ids(self) -> (int, int): + return (self.system_id, self.component_id) + + @property + def target_ids(self) -> (int, int): + return (self.target_system_id, self.target_component_id) + + +class BaseNode(rclpy.node.Node): + """ + Base class for mavros client object. + + It's used to hide plugin parameters. + """ + + _ns: str + + def __init__( + self, node_name: typing.Optional[str] = None, mavros_ns: str = DEFAULT_NAMESPACE + ): + """ + Constructor. + + :param node_name: name of the node, would be random if None + :param mavros_ns: node name of mavros::UAS + """ + if node_name is None: + node_name = DEFAULT_NODE_NAME_PREFIX + "".join( + random.choices(string.ascii_lowercase + string.digits, k=4) + ) + + super().__init__(node_name) + self._ns = mavros_ns + + @property + def mavros_ns(self) -> str: + return self._ns + + @cached_property + def uas_settings(self) -> UASParams: + from .utils import call_get_parameters + + lg = self.get_logger() + + names = [f.name for f in fields(UASParams)] + lg.debug(f"Getting UAS parameters: {', '.join(names)}") + pd = call_get_parameters(node=self, node_name=self.mavros_ns, names=names) + + return UASParams(**{k: v.value for k, v in pd.items()}) + + def get_topic(self, *args: str) -> str: + return "/".join((self._ns,) + args) + + def start_spinner(self) -> threading.Thread: + def run(): + lg = self.get_logger() + while rclpy.ok(): + lg.debug("starting spinning client node") + rclpy.spin(self) + + lg.debug("stopped client node spinner") + + thd = threading.Thread(target=run, name=f"mavros_py_spin_{self.get_name()}") + thd.daemon = True + thd.start() + return thd + + +class PluginModule: + """ + PluginModule is a base class for modules used to talk to mavros plugins. + + Provides some helper functions. + """ + + _node: BaseNode + + def __init__(self, parent_node: BaseNode): + self._node = parent_node + + @property + def node(self) -> BaseNode: + return self._node + + def get_logger(self, *args, **kwargs): + return self.node.get_logger(*args, **kwargs) + + def create_publisher( + self, + msg_type: rclpy.node.MsgType, + topic: TopicType, + qos_profile: QoSType, + **kwargs, + ) -> rclpy.node.Publisher: + if isinstance(topic, str): + topic = (topic,) + + return self._node.create_publisher( + msg_type, self._node.get_topic(*topic), qos_profile, **kwargs + ) + + def create_subscription( + self, + msg_type: rclpy.node.MsgType, + topic: TopicType, + callback: SubscriptionCallable, + qos_profile: QoSType, + **kwargs, + ) -> rclpy.node.Subscription: + if isinstance(topic, str): + topic = (topic,) + + return self._node.create_subscription( + msg_type, self._node.get_topic(*topic), callback, qos_profile, **kwargs + ) + + def create_client( + self, srv_type: rclpy.node.SrvType, srv_name: TopicType, **kwargs + ) -> rclpy.node.Client: + if isinstance(srv_name, str): + srv_name = (srv_name,) + + cli = self._node.create_client( + srv_type, self._node.get_topic(*srv_name), **kwargs + ) + wait_for_service(cli, self.get_logger()) + return cli + + def create_service( + self, + srv_type: rclpy.node.SrvType, + srv_name: TopicType, + callback: ServiceCallable, + **kwargs, + ) -> rclpy.node.Service: + if isinstance(srv_name, str): + srv_name = (srv_name,) + + return self._node.create_service( + srv_type, self._node.get_topic(*srv_name), callback, **kwargs + ) diff --git a/mavros/mavros/cmd/__init__.py b/mavros/mavros/cmd/__init__.py new file mode 100644 index 000000000..a35a6b538 --- /dev/null +++ b/mavros/mavros/cmd/__init__.py @@ -0,0 +1,132 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +"""MAVROS helper tools.""" + +import pathlib +import typing + +import click +import rclpy + +from .. import Client +from ..base import DEFAULT_NAMESPACE + + +class CliClient: + def __init__( + self, + *, + node_name: typing.Optional[str] = None, + mavros_ns: str = DEFAULT_NAMESPACE, + verbose: bool = False, + ): + self.cli = Client(node_name=node_name, mavros_ns=mavros_ns) + self.verbose = verbose + + def __getattr__(self, key: str): + try: + return object.__getattribute__(self, key) + except AttributeError: + return getattr(self.cli, key) + + def __repr__(self) -> str: + return f"" + + def verbose_echo(self, *args, **kwargs): + if self.verbose: + click.echo(*args, **kwargs) + + def verbose_secho(self, *args, **kwargs): + if self.verbose: + click.secho(*args, **kwargs) + + +pass_client = click.make_pass_decorator(CliClient) + + +def print_version(ctx, param_, value): + if not value or ctx.resilient_parsing: + return + + import xml.etree.ElementTree as ET # nosemgrep + + import ament_index_python as aip + + share_dir = aip.get_package_share_directory("mavros") + package_xml = pathlib.Path(share_dir) / "package.xml" + + tree = ET.parse(package_xml) + versions = tree.getroot().findall("version") + + click.echo(f"MAVROS Version: {versions[0].text}") + ctx.exit() + + +@click.group() +@click.option( + "--node-name", + type=str, + default=None, + envvar="MAVCLI_NODE_NAME", + help="Set node name to cli's Node, default: random", +) +@click.option( + "--mavros-ns", + type=str, + default=DEFAULT_NAMESPACE, + envvar="MAVCLI_MAVROS_NS", + help="Set namespace of mavros::UAS Node", +) +@click.option( + "--verbose", + type=bool, + default=False, + is_flag=True, + envvar="MAVCLI_VERBOSE", + help="Verbose output", +) +@click.option( + "--wait-fcu", + type=float, + is_flag=False, + flag_value=None, + envvar="MAVCLI_WAIT_FCU", + default=False, + help="Wait for establishing FCU connection", +) +@click.option( + "--version", + is_flag=True, + callback=print_version, + expose_value=False, + is_eager=True, + help="Show program version and exit", +) +@click.pass_context +def cli(ctx, node_name, mavros_ns, verbose, wait_fcu): + """MAVROS tools entry point.""" + spinner_thd = None + + def on_close(): + rclpy.shutdown() + if spinner_thd: + spinner_thd.join() + + rclpy.init() + ctx.call_on_close(on_close) + + ctx.obj = CliClient(node_name=node_name, mavros_ns=mavros_ns, verbose=verbose) + spinner_thd = ctx.obj.start_spinner() + + if wait_fcu or wait_fcu is None: + ctx.obj.verbose_echo("Waiting connection to the FCU...") + ctx.obj.system.wait_fcu_connection(wait_fcu) + + +from . import checkid, cmd, ftp, mission, param, safety, system # NOQA diff --git a/mavros/scripts/event_launcher b/mavros/mavros/cmd/__main__.py old mode 100755 new mode 100644 similarity index 64% rename from mavros/scripts/event_launcher rename to mavros/mavros/cmd/__main__.py index b3a03f7c7..1cb1ddba4 --- a/mavros/scripts/event_launcher +++ b/mavros/mavros/cmd/__main__.py @@ -1,11 +1,13 @@ -#!/usr/bin/env python +# -*- coding: utf-8 -*- # vim:set ts=4 sw=4 et: # -# Copyright 2015 Vladimir Ermakov. +# Copyright 2021 Vladimir Ermakov. # # This file is part of the mavros package and subject to the license terms # in the top-level LICENSE file of the mavros repository. # https://github.com/mavlink/mavros/tree/master/LICENSE.md +"""MAVROS helper tools entry point.""" -from mavros import event_launcher -event_launcher.main() +from . import cli + +cli() diff --git a/mavros/mavros/cmd/checkid.py b/mavros/mavros/cmd/checkid.py new file mode 100644 index 000000000..a65713a7f --- /dev/null +++ b/mavros/mavros/cmd/checkid.py @@ -0,0 +1,130 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2015,2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +""" +Check ID diagnostic. + +This script listens to devices connected to mavros and +checks against system & component id mismatch errors. +""" + +import threading +import typing + +import click +import rclpy.qos + +from mavros_msgs.msg import Mavlink + +from . import CliClient, cli, pass_client +from .utils import common_dialect + +ROUTER_QOS = rclpy.qos.QoSProfile( + depth=1000, + reliability=rclpy.qos.QoSReliabilityPolicy.BEST_EFFORT, + durability=rclpy.qos.QoSDurabilityPolicy.VOLATILE, +) + + +class Checker: + def __init__(self, *, client: CliClient, follow: bool, watch_time: float): + # dict of sets: (sysid, compid) -> set[msgid...] + self.message_sources = {} + self.messages_received = 0 + self.reports = 0 + self.client = client + self.follow = follow + self.event = threading.Event() + + self.tgt_ids = client.uas_settings.target_ids + uas_url = client.uas_settings.uas_url + source_topic = f"{uas_url}/mavlink_source" + + click.secho( + f"Router topic: {source_topic}, target: {self.fmt_ids(self.tgt_ids)}", + fg="cyan", + ) + + self.source_sub = client.create_subscription( + Mavlink, source_topic, self.mavlink_source_cb, ROUTER_QOS + ) + self.timer = client.create_timer(watch_time, self.timer_cb) + + def fmt_ids(self, ids: typing.Tuple[int]) -> str: + return f"{'.'.join(f'{v}' for v in ids)}" + + def mavlink_source_cb(self, msg: Mavlink): + self.client.verbose_secho(f"Msg: {msg}", fg="magenta") + + ids = (msg.sysid, msg.compid) + if ids in self.message_sources: + self.message_sources[ids].add(msg.msgid) + else: + self.message_sources[ids] = set((msg.msgid,)) + + self.messages_received += 1 + + def timer_cb(self): + if self.reports > 0: + click.echo("-" * 80) + + self.reports += 1 + str_tgt_ids = self.fmt_ids(self.tgt_ids) + + if self.tgt_ids in self.message_sources: + click.secho(f"OK. I got messages from {str_tgt_ids}.", fg="green") + else: + click.secho( + f"ERROR. I got {len(self.message_sources)} addresses, " + f"but not your target {str_tgt_ids}", + fg="red", + ) + + click.secho("---", fg="cyan") + click.secho( + f"Received {self.messages_received}, from {len(self.message_sources)} addresses", + fg="cyan", + ) + click.secho("address list of messages", fg="cyan") + for address, messages in self.message_sources.items(): + + def fmt_msgid(msgid: int) -> str: + if common_dialect is None: + return f"{msgid}" + + msg = common_dialect.mavlink_map.get(msgid) + if msg is None: + return f"{msgid}" + else: + msg_name = "" + # Since pymavlink version 2.4.32 `name` is renamed to `msgname`. + # We want to stay compatible with prior versions of pymavlink. + if hasattr(msg, "msgname"): + msg_name = msg.msgname + else: + msg_name = msg.name + return f"{msgid} ({msg_name})" + + str_ids = self.fmt_ids(address) + click.secho( + f"{str_ids:>7s} {', '.join(fmt_msgid(msgid) for msgid in messages)}", + fg="white", + ) + + if not self.follow: + self.event.set() + + +@cli.command() +@click.option("-f", "--follow", is_flag=True, help="do not exit after first report.") +@click.option("--watch-time", type=float, default=15.0, help="watch period") +@pass_client +def checkid(client, follow, watch_time): + """Tool to verify target address and list messages coming to mavros UAS.""" + checker = Checker(client=client, follow=follow, watch_time=watch_time) + checker.event.wait() diff --git a/mavros/mavros/cmd/cmd.py b/mavros/mavros/cmd/cmd.py new file mode 100644 index 000000000..b9165216b --- /dev/null +++ b/mavros/mavros/cmd/cmd.py @@ -0,0 +1,350 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2014,2015,2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +""" +mav cmd command. + +Note: arming service provided by mav safety. +""" + +import threading + +import click +from sensor_msgs.msg import NavSatFix + +from mavros_msgs.srv import ( + CommandHome, + CommandInt, + CommandLong, + CommandTOL, + CommandTriggerControl, + CommandTriggerInterval, +) + +from . import cli, pass_client +from .utils import bool2int, check_cmd_ret + + +@cli.group() +@pass_client +def cmd(client): + """Tool to send commands to MAVLink device.""" + + +@cmd.command() +@click.option( + "-c", "--confirmation", default=False, is_flag=True, help="Require confirmation" +) +@click.option( + "-b", "--broadcast", default=False, is_flag=True, help="Broadcast command" +) +@click.argument("command", type=int) +@click.argument("param1", type=float) +@click.argument("param2", type=float) +@click.argument("param3", type=float) +@click.argument("param4", type=float) +@click.argument("param5", type=float) +@click.argument("param6", type=float) +@click.argument("param7", type=float) +@pass_client +@click.pass_context +def long( + ctx, + client, + confirmation, + broadcast, + command, + param1, + param2, + param3, + param4, + param5, + param6, + param7, +): + """Send any command (COMMAND_LONG).""" + req = CommandLong.Request( + broadcast=broadcast, + command=command, + confirmation=bool2int(confirmation), + param1=param1, + param2=param2, + param3=param3, + param4=param4, + param5=param5, + param6=param6, + param7=param7, + ) + + client.verbose_echo(f"Calling: {req}") + ret = client.command.cli_long.call(req) + check_cmd_ret(ctx, client, ret) + + +@cmd.command() +@click.option("-c", "--current", default=False, is_flag=True, help="Is current?") +@click.option( + "-a", "--autocontinue", default=False, is_flag=True, help="Is autocontinue?" +) +@click.option("-f", "--frame", type=int, default=3, help="Frame Code") +@click.option( + "-b", "--broadcast", default=False, is_flag=True, help="Broadcast command" +) +@click.argument("command", type=int) +@click.argument("param1", type=float) +@click.argument("param2", type=float) +@click.argument("param3", type=float) +@click.argument("param4", type=float) +@click.argument("x", type=int) +@click.argument("y", type=int) +@click.argument("z", type=float) +@pass_client +@click.pass_context +def int( + ctx, + client, + current, + autocontinue, + broadcast, + frame, + command, + param1, + param2, + param3, + param4, + x, + y, + z, +): + """Send any command (COMMAND_INT).""" + req = CommandInt.Request( + broadcast=broadcast, + command=command, + frame=frame, + current=bool2int(current), + autocontinue=bool2int(autocontinue), + param1=param1, + param2=param2, + param3=param3, + param4=param4, + x=x, + y=y, + z=z, + ) + + client.verbose_echo(f"Calling: {req}") + ret = client.command.cli_int.call(req) + check_cmd_ret(ctx, client, ret) + + +@cmd.command() +@click.option( + "-c", + "--current-gps", + is_flag=True, + help="Use current GPS location (use 0 0 0 for location args)", +) +@click.argument("latitude", type=float) +@click.argument("longitude", type=float) +@click.argument("altitude", type=float) +@pass_client +@click.pass_context +def set_home(ctx, client, current_gps, latitude, longitude, altitude): + """Request change home position.""" + req = CommandHome.Request( + current_gps=current_gps, + latitude=latitude, + longitude=longitude, + altitude=altitude, + ) + + client.verbose_echo(f"Calling: {req}") + ret = client.command.cli_set_home.call(req) + check_cmd_ret(ctx, client, ret) + + +@cmd.command() +@click.option("--min-pitch", type=float, required=True, help="Min pitch") +@click.option("--yaw", type=float, required=True, help="Desired Yaw") +@click.argument("latitude", type=float) +@click.argument("longitude", type=float) +@click.argument("altitude", type=float) +@pass_client +@click.pass_context +def takeoff(ctx, client, min_pitch, yaw, latitude, longitude, altitude): + """Request takeoff.""" + req = CommandTOL.Request( + min_pitch=min_pitch, + yaw=yaw, + latitude=latitude, + longitude=longitude, + altitude=altitude, + ) + + client.verbose_echo(f"Calling: {req}") + ret = client.command.cli_takeoff.call(req) + check_cmd_ret(ctx, client, ret) + + +@cmd.command() +@click.option("--yaw", type=float, required=True, help="Desired Yaw") +@click.argument("latitude", type=float) +@click.argument("longitude", type=float) +@click.argument("altitude", type=float) +@pass_client +@click.pass_context +def land(ctx, client, yaw, latitude, longitude, altitude): + """Request land.""" + req = CommandTOL.Request( + min_pitch=0.0, + yaw=yaw, + latitude=latitude, + longitude=longitude, + altitude=altitude, + ) + + client.verbose_echo(f"Calling: {req}") + ret = client.command.cli_land.call(req) + check_cmd_ret(ctx, client, ret) + + +@cmd.command() +@click.option("--min-pitch", type=float, required=True, help="Min pitch") +@click.option("--yaw", type=float, required=True, help="Desired Yaw") +@click.argument("altitude", type=float) +@pass_client +@click.pass_context +def takeoff_cur(ctx, client, min_pitch, yaw, altitude): + """Request takeoff from current GPS coordinates.""" + done_evt = threading.Event() + + def fix_cb(fix: NavSatFix): + click.echo( + f"Taking-off from current coord: " + f"Lat: {fix.latitude}, Long: {fix.longitude}" + ) + client.verbose_echo( + f"With desired Altitude: {altitude}, " + f"Yaw: {yaw}, Pitch angle: {min_pitch}" + ) + + req = CommandTOL.Request( + min_pitch=min_pitch, + yaw=yaw, + latitude=fix.latitude, + longitude=fix.longitude, + altitude=altitude, + ) + + client.verbose_echo(f"Calling: {req}") + ret = client.command.cli_takeoff.call(req) + check_cmd_ret(ctx, client, ret) + + done_evt.set() + + client.global_position.subscribe_fix(fix_cb) + if not done_evt.wait(10.0): + click.echo("Something went wrong. Topic timed out.") + ctx.exit(1) + + +@cmd.command() +@click.option("--yaw", type=float, required=True, help="Desired Yaw") +@click.argument("altitude", type=float) +@pass_client +@click.pass_context +def land_cur(ctx, client, yaw, altitude): + """Request land on current GPS coordinates.""" + done_evt = threading.Event() + + def fix_cb(fix: NavSatFix): + click.echo( + f"Landing on current coord: " f"Lat: {fix.latitude}, Long: {fix.longitude}" + ) + client.verbose_echo(f"With desired Altitude: {altitude}, Yaw: {yaw}") + + req = CommandTOL.Request( + min_pitch=0.0, + yaw=yaw, + latitude=fix.latitude, + longitude=fix.longitude, + altitude=altitude, + ) + + client.verbose_echo(f"Calling: {req}") + ret = client.command.cli_land.call(req) + check_cmd_ret(ctx, client, ret) + + done_evt.set() + + client.global_position.subscribe_fix(fix_cb) + if not done_evt.wait(10.0): + click.echo("Something went wrong. Topic timed out.") + ctx.exit(1) + + +@cmd.command() +@click.option( + "-e", + "--enable", + "trigger_enable", + flag_value=True, + default=True, + help="Enable camera trigger", +) +@click.option( + "-d", "--disable", "trigger_enable", flag_value=False, help="Disable camera trigger" +) +@click.option( + "-r", "--sequence-reset", is_flag=True, default=False, help="Reset trigger sequence" +) +@click.option( + "-p", "--trigger-pause", is_flag=True, default=False, help="Pause triggering" +) +@pass_client +@click.pass_context +def trigger_control(ctx, client, trigger_enable, sequence_reset, trigger_pause): + """Control onboard camera triggering system (PX4).""" + req = CommandTriggerControl.Request( + trigger_enable=trigger_enable, + sequence_reset=sequence_reset, + trigger_pause=trigger_pause, + ) + + client.verbose_echo(f"Calling: {req}") + ret = client.command.cli_trigger_control.call(req) + check_cmd_ret(ctx, client, ret) + + +@cmd.command() +@click.option( + "-c", + "--cycle-time", + default=0.0, + type=float, + help="Camera trigger cycle time. Zero to use current onboard value", +) +@click.option( + "-i", + "--integration-time", + default=0.0, + type=float, + help="Camera shutter intergration time. Zero to ignore", +) +@pass_client +@click.pass_context +def trigger_interval(ctx, client, cycle_time, integration_time): + """Control onboard camera triggering system (PX4).""" + req = CommandTriggerInterval.Request( + cycle_time=cycle_time, + integration_time=integration_time, + ) + + client.verbose_echo(f"Calling: {req}") + ret = client.command.cli_trigger_interval.call(req) + check_cmd_ret(ctx, client, ret) diff --git a/mavros/mavros/cmd/ftp.py b/mavros/mavros/cmd/ftp.py new file mode 100644 index 000000000..9258faba6 --- /dev/null +++ b/mavros/mavros/cmd/ftp.py @@ -0,0 +1,299 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2014,2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +"""mav ftp command.""" + +import os +import pathlib +import typing + +import click + +from mavros_msgs.msg import FileEntry + +from ..nuttx_crc32 import nuttx_crc32 +from . import cli, pass_client +from .utils import fault_echo + +# optimized transfer size for FTP message payload +# XXX: bug in ftp.cpp cause a doubling request of last package. +# -1 fixes that. +FTP_PAGE_SIZE = 239 * 18 - 1 +FTP_PWD_FILE = pathlib.Path("/tmp/.mavftp_pwd") + + +class ProgressBar: + """Wrapper class for hiding file transfer brogressbar construction.""" + + def __init__(self, quiet: bool, label: str, maxval: int): + if quiet or maxval == 0: + if maxval == 0: + click.echo("Can't show progressbar for unknown file size", err=True) + self.pbar = None + return + + self.pbar = click.progressbar( + label=label, + length=maxval, + show_percent=True, + show_eta=True, + ) + + def update(self, value: int): + if self.pbar: + self.pbar.update(value) + + def __enter__(self): + if self.pbar: + self.pbar.__enter__() + + return self + + def __exit__(self, type, value, traceback): + if self.pbar: + self.pbar.__exit__(type, value, traceback) + + +@cli.group() +@pass_client +def ftp(client): + """File manipulation tool for MAVLink-FTP.""" + + +def resolve_path(path: typing.Union[None, str, pathlib.Path] = None) -> pathlib.Path: + """Resolve FTP path using PWD file.""" + if FTP_PWD_FILE.exists(): + with FTP_PWD_FILE.open("r") as fd: + pwd = fd.readline() + else: + # default home location is root directory + pwd = os.environ.get("MAVFTP_HOME", "/") + + pwd = pathlib.Path(pwd) + + if not path: + return pwd.resolve() # no path - PWD location + elif path.startswith("/"): + return pathlib.Path(path).resolve() # absolute path + else: + return (pwd / path).resolve() + + +@ftp.command("cd") +@click.argument("path", type=click.Path(exists=False), nargs=1, required=False) +@pass_client +@click.pass_context +def change_directory(ctx, client, path): + """Change directory.""" + if path: + path = resolve_path(path) + if path and not path.is_absolute(): + fault_echo(ctx, f"Path is not absolute: {path}") + + if path: + with FTP_PWD_FILE.open("w") as fd: + fd.write(str(pathlib.Path(path).resolve())) + else: + if FTP_PWD_FILE.exists(): + FTP_PWD_FILE.unlink() + + +@ftp.command("ls") +@click.argument("path", type=click.Path(exists=False), nargs=1, required=False) +@pass_client +@click.pass_context +def list(ctx, client, path): + """List files and directories.""" + path = resolve_path(path) + for ent in client.ftp.listdir(str(path)): + isdir = ent.type == FileEntry.TYPE_DIRECTORY + click.echo(f"{ent.name}{isdir and '/' or ''}\t{ent.size}") + + +@ftp.command() +@click.argument("path", type=click.Path(exists=False), nargs=1, required=False) +@pass_client +@click.pass_context +def cat(ctx, client, path): + """Cat file from FCU.""" + ctx.invoke( + download, + src=path, + dest=click.open_file("-", "wb"), + progressbar=True, + verify=False, + ) + + +@ftp.command("rm") +@click.argument( + "path", type=click.Path(exists=False, file_okay=True), nargs=1, required=True +) +@pass_client +@click.pass_context +def remove(ctx, client, path): + """Remove file.""" + path = resolve_path(path) + client.ftp.unlink(str(path)) + + +@ftp.command() +@pass_client +def reset(client): + """Reset ftp server.""" + client.ftp.reset_server() + + +@ftp.command() +@click.argument( + "path", type=click.Path(exists=False, dir_okay=True), nargs=1, required=True +) +@pass_client +@click.pass_context +def mkdir(ctx, client, path): + """Create directory.""" + path = resolve_path(path) + client.ftp.mkdir(str(path)) + + +@ftp.command() +@click.argument( + "path", type=click.Path(exists=False, dir_okay=True), nargs=1, required=True +) +@pass_client +@click.pass_context +def rmdir(ctx, client, path): + """Remove directory.""" + path = resolve_path(path) + client.ftp.rmdir(str(path)) + + +@ftp.command() +@click.option( + "--progressbar/--no-progressbar", " /-q", default=True, help="show progress bar" +) +@click.option("--verify/--no-verify", " /-v", default=True, help="perform verify step") +@click.argument( + "src", type=click.Path(exists=False, file_okay=True), nargs=1, required=True +) +@click.argument("dest", type=click.File("wb"), required=False) +@pass_client +@click.pass_context +def download(ctx, client, src, dest, progressbar, verify): + """Download file.""" + local_crc = 0 + src = resolve_path(src) + + if not dest: + # if file argument is not set, use $PWD/basename + dest = click.open_file(pathlib.Path(src.name).name, "wb") + + client.verbose_echo(f"Downloading from {src} to {dest.name}", err=True) + + with dest as to_fd, click.ftp.open(src, "r") as from_fd, ProgressBar( + not progressbar, "Downloading:", from_fd.size + ) as bar: + while True: + buf = from_fd.read(FTP_PAGE_SIZE) + if len(buf) == 0: + break + + local_crc = nuttx_crc32(buf, local_crc) + to_fd.write(buf) + bar.update(from_fd.tell()) + + if verify: + click.verbose_echo("Verifying...", err=True) + remote_crc = click.ftp.checksum(str(src)) + if local_crc != remote_crc: + fault_echo(f"Verification failed: 0x{local_crc:08x} != 0x{remote_crc:08x}") + + +@ftp.command() +@click.option( + "--progressbar/--no-progressbar", " /-q", default=True, help="show progress bar" +) +@click.option("--verify/--no-verify", " /-v", default=True, help="perform verify step") +@click.option( + "--overwrite/--no-overwrite", + " /-W", + default=True, + help="is it allowed to overwrite file", +) +@click.argument("src", type=click.File("rb"), nargs=1, required=True) +@click.argument("dest", type=click.Path(exists=False, file_okay=True), required=False) +@pass_client +@click.pass_context +def upload(ctx, client, src, dest, progressbar, verify, overwrite): + """Upload file.""" + mode = "cw" if not overwrite else "w" + local_crc = 0 + + if dest: + dest = resolve_path(dest) + else: + dest = resolve_path(pathlib.Path(src.name).name) + + client.verbose_echo(f"Uploading from {src} to {dest}", err=True) + + # for stdin it is 0 + from_size = os.fstat(src.fileno()).st_size + + with src as from_fd, client.ftp.open(str(dest), mode) as to_fd, ProgressBar( + not progressbar, "Uploading:", from_size + ) as bar: + while True: + buf = from_fd.read(FTP_PAGE_SIZE) + if len(buf) == 0: + break + + local_crc = nuttx_crc32(buf, local_crc) + to_fd.write(buf) + bar.update(to_fd.tell()) + + if verify: + client.verbose_echo("Verifying...", err=True) + remote_crc = client.ftp.checksum(str(dest)) + if local_crc != remote_crc: + fault_echo(f"Verification failed: 0x{local_crc:08x} != 0x{remote_crc:08x}") + + +@ftp.command() +@click.argument("local", type=click.File("rb"), nargs=1, required=True) +@click.argument("remote", type=click.Path(exists=False, file_okay=True), required=False) +@pass_client +@click.pass_context +def verify(ctx, client, local, remote): + """Verify files.""" + local_crc = 0 + + if remote: + remote = resolve_path(remote) + else: + remote = resolve_path(pathlib.Path(local.name).name) + + client.verbose_echo(f"Verifying {local} and {remote}", err=True) + + with local as fd: + while True: + buf = fd.read(4096 * 32) # use 128k block for CRC32 calculation + if len(buf) == 0: + break + + local_crc = nuttx_crc32(buf, local_crc) + + remote_crc = client.ftp.checksum(str(remote)) + + client.verbose_echo("CRC32 for local and remote files:") + client.verbose_echo(f"0x{local_crc:08x} {local.name}") + client.verbose_echo(f"0x{remote_crc:08x} {remote.name}") + + if local_crc != remote_crc: + fault_echo(ctx, f"{local.name}: FAULT") + else: + click.echo(f"{local.name}: OK") diff --git a/mavros/mavros/cmd/mission.py b/mavros/mavros/cmd/mission.py new file mode 100644 index 000000000..92293d512 --- /dev/null +++ b/mavros/mavros/cmd/mission.py @@ -0,0 +1,454 @@ +# vim:set ts=4 sw=4 et: +# +# Copyright 2014,2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +"""mav wp command.""" + +import threading +import typing + +import click + +from mavros_msgs.msg import Waypoint, WaypointList +from mavros_msgs.srv import ( + WaypointClear, + WaypointPull, + WaypointPush, + WaypointSetCurrent, +) + +from ..mission import ( + FRAMES, + NAV_CMDS, + MissionPluginBase, + PlanFile, + QGroundControlPlan, + QGroundControlWPL, +) +from . import CliClient, cli, pass_client +from .utils import apply_options, fault_echo + +no_prettytable = False +try: + from prettytable import PrettyTable +except ImportError: + no_prettytable = True + + +@cli.group() +@pass_client +def wp(client): + """Tool to manipulate missions on MAVLink device.""" + if no_prettytable: + click.echo( + "Waring: 'show' action disabled. Please install python3-prettytable", + err=True, + ) + + +def _add_format_options(f): + return apply_options( + f, + click.option( + "-wpl", + "--qgc-wpl", + "file_format", + flag_value="wpl", + help="Select QGroundControl WPL mission file format (old CSV)", + ), + click.option( + "-plan", + "--qgc-plan", + "file_format", + flag_value="plan", + help="Select QGroundControl Plan mission file format", + ), + ) + + +def get_wp_file_io( + client: CliClient, file_format: typing.Optional[str], file_: typing.TextIO +) -> PlanFile: + if file_format == "wpl": + return QGroundControlWPL() + elif file_format == "plan": + return QGroundControlPlan() + else: + if file_.name.endswith(".plan"): + return QGroundControlPlan() + else: + return QGroundControlWPL() + + +def fmt_accessor(accessor: MissionPluginBase): + return f"{accessor.__class__.__name__.replace('Plugin', '')}" + + +@wp.command() +@click.option( + "--mission/--no-mission", + "-m/-M", + "pull_mission", + default=True, + help="Pull mission points", +) +@click.option( + "--fence/--no-fence", "-f/-F", "pull_fence", default=True, help="Pull fence points" +) +@click.option( + "--rally/--no-rally", "-r/-R", "pull_rally", default=True, help="Pull rally points" +) +@pass_client +@click.pass_context +def pull(ctx, client, pull_mission, pull_fence, pull_rally): + """Pull mission from FCU.""" + _ = 1 # yapf + + def pull_if(cond: bool, accessor: MissionPluginBase): + if not cond: + return + + req = WaypointPull.Request() + ret = accessor.cli_pull.call(req) + + if not ret.success: + fault_echo(ctx, "Request failed. Check mavros logs") + + client.verbose_echo(f"{fmt_accessor(accessor)}(s) received: {ret.wp_received}") + + pull_if(pull_mission, client.waypoint) + pull_if(pull_fence, client.geofence) + pull_if(pull_rally, client.rallypoint) + + +@wp.command() +@click.option( + "--mission", "accessor", flag_value="waypoint", default=True, help="Show mission" +) +@click.option("--fence", "accessor", flag_value="geofence", help="Show geo fence") +@click.option("--rally", "accessor", flag_value="rallypoint", help="Show rallypoints") +@click.option( + "-p", + "--pull", + "pull_flag", + is_flag=True, + default=False, + help="Pull points from FCU before show", +) +@click.option( + "-f", "--follow", is_flag=True, default=False, help="Watch and show new data" +) +@pass_client +@click.pass_context +def show(ctx, client, accessor, pull_flag, follow): + """ + Show current points. + + You can select type by setting flag: + --mission - mission (default), + --fence - geofence, + --rally - rallypoints + """ + if no_prettytable: + fault_echo(ctx, "Show command require prettytable module!") + + def str_bool(x: bool) -> str: + return "Yes" if x else "No" + + def str_frame(f: int) -> str: + return f"{FRAMES.get(f, 'UNK')} ({f})" + + def str_command(c: int) -> str: + return f"{NAV_CMDS.get(c, 'UNK')} ({c})" + + done_evt = threading.Event() + + def show_table(topic: WaypointList): + pt = PrettyTable( + ( + "#", + "Curr", + "Auto", + "Frame", + "Command", + "P1", + "P2", + "P3", + "P4", + "X Lat", + "Y Long", + "Z Alt", + ) + ) + + for seq, w in enumerate(topic.waypoints): + pt.add_row( + ( + seq, + str_bool(w.is_current), + str_bool(w.autocontinue), + str_frame(w.frame), + str_command(w.command), + w.param1, + w.param2, + w.param3, + w.param4, + w.x_lat, + w.y_long, + w.z_alt, + ) + ) + + click.echo(pt) + if not follow: + done_evt.set() + + if pull_flag: + ctx.invoke( + pull, + pull_mission=accessor == "waypoint", + pull_fence=accessor == "geofence", + pull_rally=accessor == "rallypoint", + ) + + # Waypoints topic is latched type, and it updates after pull + sub = getattr(client, accessor).subscribe_points(show_table) + + if follow: + done_evt.wait() + elif not done_evt.wait(30.0): + fault_echo(ctx, "Something went wrong. Topic timed out.") + + del sub + + +@wp.command() +@_add_format_options +@click.option( + "-p", + "--preserve-home", + is_flag=True, + default=False, + help="Preserve home location (WP 0, APM only)", +) +@click.option( + "-s", + "--start-index", + type=int, + default=0, + help="Waypoint start index for partial update (APM only)", +) +@click.option( + "-e", + "--end-index", + type=int, + default=0, + help="Waypoint end index for partial update " "(APM only, default: last element)", +) +@click.option( + "--no-mission", "-M", is_flag=True, default=False, help="Don't load mission points" +) +@click.option( + "--no-fence", "-F", is_flag=True, default=False, help="Don't load fence points" +) +@click.option( + "--no-rally", "-R", is_flag=True, default=False, help="Don't load rally points" +) +@click.argument("file_", metavar="FILE", type=click.File("r")) +@pass_client +@click.pass_context +def load( + ctx, + client, + file_format, + preserve_home, + start_index, + end_index, + no_mission, + no_fence, + no_rally, + file_, +): + """Load mission from file.""" + mission_file = get_wp_file_io(client, file_format, file_) + mission_file.load(file_) + + def call_push( + *, + accessor: MissionPluginBase, + points: typing.Optional[typing.List[Waypoint]], + start_index: int = 0, + no_send: bool = False, + ): + if points is None or no_send: + return + + req = WaypointPush.Request( + waypoints=points.waypoints, + start_index=start_index, + ) + ret = accessor.cli_push(req) + + if not ret.success: + fault_echo("Request failed. Check mavros logs") + + client.verbose_echo( + f"{fmt_accessor(accessor)}(s) transfered: {ret.wp_transfered}" + ) + + done_evt = threading.Event() + + def fix_wp0(topic: WaypointList): + if len(topic.waypoints) > 0: + wp0 = topic.waypoints[0] + mission_file.mission[0] = wp0 + client.verbose_echo( + f"HOME location: latitude: {wp0.x_lat}, " + f"longitude: {wp0.y_long}, altitude: {wp0.z_alt}" + ) + else: + click.echo("Failed to get WP0! WP0 will be loaded from file.", err=True) + + done_evt.set() + + if start_index > 0: + # Push partial + if start_index == end_index: + end_index += 1 + + end_index = end_index or len(mission_file.mission) + + call_push( + accessor=client.waypoint, + points=mission_file.mission[start_index:end_index], + start_index=start_index, + ) + return + + if preserve_home: + client.waypoint.subscribe_points(fix_wp0) + if not done_evt.wait(30.0): + fault_echo("Something went wrong. Topic timed out.") + + call_push( + accessor=client.waypoint, + points=mission_file.mission, + start_index=0, + no_send=no_mission, + ) + call_push( + accessor=client.geofence, + points=mission_file.fence, + start_index=0, + no_send=no_fence, + ) + call_push( + accessor=client.rallypoint, + points=mission_file.geofence, + start_index=0, + no_send=no_rally, + ) + + +@wp.command() +@_add_format_options +@click.option( + "--no-mission", "-M", is_flag=True, default=False, help="Don't dump mission points" +) +@click.option( + "--no-fence", "-F", is_flag=True, default=False, help="Don't dump fence points" +) +@click.option( + "--no-rally", "-R", is_flag=True, default=False, help="Don't dump rally points" +) +@click.argument("file_", metavar="FILE", type=click.File("w")) +@pass_client +@click.pass_context +def dump(ctx, client, file_format, no_mission, no_fence, no_rally, file_): + """Dump mission to file.""" + mission_file = get_wp_file_io(client, file_format, file_) + + fetch_mission = not no_mission + fetch_fence = isinstance(mission_file, QGroundControlPlan) and not no_fence + fetch_rally = isinstance(mission_file, QGroundControlPlan) and not no_rally + + ctx.invoke( + pull, + pull_mission=fetch_mission, + pull_fence=fetch_fence, + pull_rally=fetch_rally, + ) + + if fetch_mission: + mission_file.mission = client.waypoint.points + + if fetch_fence: + mission_file.fence = client.geofence.points + + if fetch_rally: + mission_file.rally = client.rallypoint.points + + mission_file.save(file_) + + +@wp.command() +@click.option( + "--mission/--no-mission", + "-m/-M", + "clear_mission", + default=True, + help="Clear mission points", +) +@click.option( + "--fence/--no-fence", + "-f/-F", + "clear_fence", + default=True, + help="Clear fence points", +) +@click.option( + "--rally/--no-rally", + "-r/-R", + "clear_rally", + default=True, + help="Clear rally points", +) +@pass_client +@click.pass_context +def clear(ctx, client, clear_mission, clear_fence, clear_rally): + """Clear mission from FCU.""" + _ = 1 # yapf + + def clear_if(cond: bool, accessor: MissionPluginBase): + if not cond: + return + + req = WaypointClear.Request() + ret = accessor.cli_clear.call(req) + + if not ret.success: + fault_echo(ctx, "Request failed. Check mavros logs") + + client.verbose_echo(f"{fmt_accessor(accessor)}(s) cleared") + + clear_if(clear_mission, client.waypoint) + clear_if(clear_fence, client.geofence) + clear_if(clear_rally, client.rallypoint) + + +@wp.command() +@click.argument("seq", type=int) +@pass_client +@click.pass_context +def setcur(ctx, client, seq): + """Set current mission item.""" + req = WaypointSetCurrent.Request(wp_seq=seq) + ret = client.waypoint.cli_set_current.call(req) + + if not ret.success: + fault_echo("Request failed, Check mavros logs") + + client.verbose_echo("Set current done.") diff --git a/mavros/mavros/cmd/param.py b/mavros/mavros/cmd/param.py new file mode 100644 index 000000000..fbcbbc4b6 --- /dev/null +++ b/mavros/mavros/cmd/param.py @@ -0,0 +1,144 @@ +# vim:set ts=4 sw=4 et: +# +# Copyright 2014,2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +"""mav param command.""" + +import typing + +import click + +from ..param import MavProxyParam, MissionPlannerParam, ParamFile, QGroundControlParam +from . import CliClient, cli, pass_client +from .utils import apply_options + + +@cli.group() +@pass_client +def param(client): + """Tool to manipulate parameters of the MAVLink device.""" + + +def _add_format_options(f): + return apply_options( + f, + click.option( + "-mp", + "--mission-planner", + "file_format", + flag_value="mp", + help="Select Mission Planner param file format", + ), + click.option( + "-qgc", + "--qgroundcontrol", + "file_format", + flag_value="qgc", + help="Select QGroundControl param file format", + ), + click.option( + "-mpx", + "-mavpx", + "--mavproxy", + "file_format", + flag_value="mpx", + help="Select MAVProxy param file format", + ), + ) + + +def get_param_file_io( + client: CliClient, file_format: typing.Optional[str], file_: typing.TextIO +) -> ParamFile: + if file_format == "mp": + client.verbose_echo("MissionPlanner format") + pf = MissionPlannerParam() + + elif file_format == "qgc": + client.verbose_echo("QGroundControl format") + pf = QGroundControlParam() + + elif file_format == "mpx": + client.verbose_echo("MavProxy format") + pf = MavProxyParam() + + else: + if file_.name.endswith(".txt"): + client.verbose_echo("Suggestion: QGroundControl format") + pf = QGroundControlParam() + else: + client.verbose_echo("Suggestion: MissionPlanner format") + pf = MissionPlannerParam() + + pf.tgt_system = client.uas_settings.target_system_id + pf.tgt_component = client.uas_settings.target_component_id + + return pf + + +@param.command() +@_add_format_options +@click.argument("file_", type=click.File("r"), metavar="FILE") +@pass_client +def load(client, file_format, file_): + """Load parameters from file.""" + param_file = get_param_file_io(client, file_format, file_) + param_file.load(file_) + + client.param.values.update(param_file.parameters) + + client.verbose_echo(f"Prameters sent: {len(param_file.parameters)}") + + +@param.command() +@_add_format_options +@click.option( + "-f", + "--force", + is_flag=True, + default=False, + help="Force pull params form FCU, update cache", +) +@click.argument("file_", type=click.File("w"), metavar="FILE") +@pass_client +def dump(client, file_format, force, file_): + """Dump parameters to file.""" + # NOTE(vooon): hidden magic - create ParamDict and get ref + values = client.param.values + + if force: + client.param.call_pull(force=True) + + client.verbose_echo(f"Parameters received: {len(values)}") + + param_file = get_param_file_io(client, file_format, file_) + param_file.parameters = values + param_file.save(file_) + + +@param.command() +@click.argument("param_id", type=str) +@pass_client +def get(client, param_id): + """Print one parameter value.""" + # XXX(vooon): ineffecient + click.echo(f"{client.param.values[param_id].value}") + + +@param.command() +@click.argument("param_id", type=str) +@click.argument("value", type=str) +@pass_client +def set(client, param_id, value): + """Set one parameter.""" + if "." in value: + val = float(value) + else: + val = int(value) + + # XXX(vooon): ineffecient + client.param.values[param_id] = val + click.echo(f"{client.param.values[param_id].value}") diff --git a/mavros/mavros/cmd/safety.py b/mavros/mavros/cmd/safety.py new file mode 100644 index 000000000..29a33ef51 --- /dev/null +++ b/mavros/mavros/cmd/safety.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python +# vim:set ts=4 sw=4 et: +# +# Copyright 2014,2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +""" +mav safety command. + +Allow user to Arm, Disarm and Kill vehicle motors. +""" + +import click + +from mavros_msgs.srv import CommandBool, CommandLong + +from . import cli, pass_client +from .utils import check_cmd_ret + + +@cli.group() +@pass_client +def safety(client): + """Tool to send safety commands to MAVLink device.""" + + +def _arm(ctx, client, state: bool): + req = CommandBool.Request(value=state) + + client.verbose_echo(f"Calling: {req}") + ret = client.command.cli_arming.call(req) + check_cmd_ret(ctx, client, ret) + + +@safety.command() +@pass_client +@click.pass_context +def arm(ctx, client): + """Arm motors.""" + _arm(ctx, client, True) + + +@safety.command() +@pass_client +@click.pass_context +def disarm(ctx, client): + """Disarm motors.""" + _arm(ctx, client, False) + + +@safety.command() +@pass_client +@click.pass_context +def kill(ctx, client): + """Kill motors.""" + req = CommandLong.Request(command=400, param2=21196.0) + + client.verbose_echo(f"Calling: {req}") + ret = client.command.cli_long.call(req) + check_cmd_ret(ctx, client, ret) diff --git a/mavros/mavros/cmd/system.py b/mavros/mavros/cmd/system.py new file mode 100644 index 000000000..ce33b3503 --- /dev/null +++ b/mavros/mavros/cmd/system.py @@ -0,0 +1,156 @@ +# vim:set ts=4 sw=4 et: +# +# Copyright 2014,2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +"""mav sys command.""" + +import threading +import typing + +import click + +from mavros_msgs.msg import State +from mavros_msgs.srv import MessageInterval, SetMode, StreamRate + +from . import cli, pass_client +from .utils import apply_options, fault_echo + + +@cli.group() +@pass_client +def sys(client): + """Tool to change mode and rate on MAVLink device.""" + + +@sys.command() +@click.option("-b", "--base-mode", type=int, default=0, help="Base mode code.") +@click.option( + "-c", + "--custom-mode", + type=str, + default="", + help="Custom mode string (same as in ~/state topic)", +) +@pass_client +@click.pass_context +def mode(ctx, client, base_mode, custom_mode): + """Set mode.""" + custom_mode = custom_mode.upper() + + done_evt = threading.Event() + + def state_cb(state: State): + client.verbose_echo(f"Current mode: {state.mode}") + if state.mode == custom_mode: + click.echo("Mode changed.") + done_evt.set() + + if custom_mode != "" and not custom_mode.isdigit(): + # with correct custom mode we can wait until it changes + client.system.subscribe_state(state_cb) + else: + done_evt.set() + + req = SetMode.Request(base_mode=base_mode, custom_mode=custom_mode) + ret = client.system.cli_set_mode.call(req) + + if not ret.mode_sent: + fault_echo(ctx, "Request failed. Check mavros logs") + + if not done_evt.wait(5): + fault_echo(ctx, "Timed out!") + + +def _add_rate_options(*options: typing.List[str]): + opts = [ + click.option( + f"--{option.lower().replace(' ', '-')}", + type=int, + metavar="RATE", + default=None, + help=f"{option} stream", + ) + for option in options + ] + + def wrap(f): + return apply_options(f, *opts) + + return wrap + + +@sys.command() +@_add_rate_options( + "All", + "Raw sensors", + "Ext status", + "RC channels", + "Raw controller", + "Position", + "Extra1", + "Extra2", + "Extra3", +) +@click.option( + "--stream-id", + type=int, + nargs=2, + metavar="ID, RATE", + default=None, + help="custom stream stream", +) +@pass_client +@click.pass_context +def rate( + ctx, + client, + all, + raw_sensors, + ext_status, + rc_channels, + raw_controller, + position, + extra1, + extra2, + extra3, + stream_id, +): + """Set stream rate.""" + _ = 1 # yapf + + def set_rate(rate_arg: typing.Optional[int], id_: int): + if rate_arg is None: + return + + req = StreamRate.Request( + stream_id=id_, + message_rate=rate_arg, + on_off=(rate_arg != 0), + ) + client.system.cli_set_stream_rate.call(req) + + set_rate(all, StreamRate.Request.STREAM_ALL) + set_rate(raw_sensors, StreamRate.Request.STREAM_RAW_SENSORS) + set_rate(ext_status, StreamRate.Request.STREAM_EXTENDED_STATUS) + set_rate(rc_channels, StreamRate.Request.STREAM_RC_CHANNELS) + set_rate(raw_controller, StreamRate.Request.STREAM_RAW_CONTROLLER) + set_rate(position, StreamRate.Request.STREAM_POSITION) + set_rate(extra1, StreamRate.Request.STREAM_EXTRA1) + set_rate(extra2, StreamRate.Request.STREAM_EXTRA2) + set_rate(extra3, StreamRate.Request.STREAM_EXTRA3) + + if stream_id: + set_rate(stream_id[1], stream_id[0]) + + +@sys.command() +@click.option("--id", type=int, metavar="MSGID", required=True, help="message id") +@click.option("--rate", type=float, metavar="RATE", required=True, help="message rate") +@pass_client +def message_interval(client, id, rate): + """Set message interval.""" + req = MessageInterval.Request(message_id=id, message_rate=rate) + client.system.cli_set_message_interval.call(req) diff --git a/mavros/mavros/cmd/utils.py b/mavros/mavros/cmd/utils.py new file mode 100644 index 000000000..4f89b26ef --- /dev/null +++ b/mavros/mavros/cmd/utils.py @@ -0,0 +1,80 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md +"""Utilities for cli.""" + +import functools +import typing + +import click + +try: + import pymavlink.dialects.v20.common as common_dialect +except ImportError: + common_dialect = None + + +def fault_echo(ctx, *args, **kwargs): + kwargs["err"] = True + click.echo(*args, **kwargs) + ctx.exit(1) + + +def fault_secho(ctx, *args, **kwargs): + kwargs["err"] = True + click.secho(*args, **kwargs) + ctx.exit(1) + + +def check_cmd_ret(ctx, client, ret): + # NOTE(vooon): APM returns MAV_RESULT + + # https://mavlink.io/en/messages/common.html#MAV_CMD_ACK + # ename = 'MAV_CMD_ACK' + # https://mavlink.io/en/messages/common.html#MAV_RESULT + ename = "MAV_RESULT" + + ackstr = "" + if hasattr(ret, "result"): + if common_dialect is not None: + ack = common_dialect.enums[ename].get( + ret.result, common_dialect.EnumEntry("unknown", "") + ) + ackstr = f" ACK: {ret.result} ({ack.name})" + else: + ackstr = f" ACK: {ret.result}" + + if not ret.success: + fault_echo(ctx, f"Request failed. Check mavros logs.{ackstr}") + + if client.verbose: + if hasattr(ret, "result"): + click.echo(f"Command done.{ackstr}") + else: + click.echo("Request done.") + + +def bool2int(b: bool) -> int: + """ + Convert bool to 1 or 0. + + I had an exception "TypeError: 'bool' object is not iterable" + for code like int(confirmation). + + Why? Who knows... + """ + if b: + return 1 + return 0 + + +def apply_options( + func: typing.Callable, *opts: typing.List[typing.Callable] +) -> typing.Callable: + """Apply several click.option to the same function.""" + return functools.reduce(lambda x, opt: opt(x), reversed(opts), func) diff --git a/mavros/mavros/command.py b/mavros/mavros/command.py new file mode 100644 index 000000000..3ec1180eb --- /dev/null +++ b/mavros/mavros/command.py @@ -0,0 +1,58 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md + +import rclpy + +from mavros_msgs.srv import ( + CommandBool, + CommandHome, + CommandInt, + CommandLong, + CommandTOL, + CommandTriggerControl, + CommandTriggerInterval, +) + +from .base import PluginModule, cached_property + + +class CommandPlugin(PluginModule): + """Interface to command plugin.""" + + @cached_property + def cli_long(self) -> rclpy.node.Client: + return self.create_client(CommandLong, ("cmd", "command")) + + @cached_property + def cli_int(self) -> rclpy.node.Client: + return self.create_client(CommandInt, ("cmd", "command_int")) + + @cached_property + def cli_arming(self) -> rclpy.node.Client: + return self.create_client(CommandBool, ("cmd", "arming")) + + @cached_property + def cli_set_home(self) -> rclpy.node.Client: + return self.create_client(CommandHome, ("cmd", "set_home")) + + @cached_property + def cli_takeoff(self) -> rclpy.node.Client: + return self.create_client(CommandTOL, ("cmd", "takeoff")) + + @cached_property + def cli_land(self) -> rclpy.node.Client: + return self.create_client(CommandTOL, ("cmd", "land")) + + @cached_property + def cli_trigger_control(self) -> rclpy.node.Client: + return self.create_client(CommandTriggerControl, ("cmd", "trigger_control")) + + @cached_property + def cli_trigger_interval(self) -> rclpy.node.Client: + return self.create_client(CommandTriggerInterval, ("cmd", "trigger_interval")) diff --git a/mavros/mavros/ftp.py b/mavros/mavros/ftp.py new file mode 100644 index 000000000..3f6420596 --- /dev/null +++ b/mavros/mavros/ftp.py @@ -0,0 +1,233 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2014,2015,2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md + +import os +import typing + +import rclpy +from std_srvs.srv import Empty + +from mavros_msgs.msg import FileEntry +from mavros_msgs.srv import ( + FileChecksum, + FileClose, + FileList, + FileMakeDir, + FileOpen, + FileRead, + FileRemove, + FileRemoveDir, + FileRename, + FileTruncate, + FileWrite, +) + +from .base import PluginModule, cached_property + + +def _check_raise_errno(ret): + if not ret.success: + raise IOError(ret.r_errno, os.strerror(ret.r_errno)) + + +class FTPFile: + """ + FCU file object. + + Note that current PX4 firmware only support two connections simultaneously. + """ + + _fm: "FTPPlugin" + + def __init__(self, *, fm, name, mode): + self._fm = fm + self.name = None + self.mode = mode + self.open(name, mode) + + def __del__(self): + self.close() + + def open(self, path: str, mode: str): + """ + Call open. + + Supported modes: + - 'w': write binary + - 'r': read binary + - 'cw': create excl & write + """ + if mode == "w" or mode == "wb": + m = FileOpen.Request.MODE_WRITE + elif mode == "r" or mode == "rb": + m = FileOpen.Request.MODE_READ + elif mode == "cw": + m = FileOpen.Request.MODE_CREATE + else: + raise ValueError("Unknown open mode: {}".format(m)) + + req = FileOpen.Request( + file_path=path, + mode=m, + ) + + ret = self._fm.cli_open.call(req) + _check_raise_errno(ret) + + self.name = path + self.mode = mode + self.size = ret.size + self.offset = 0 + + def close(self): + if self.closed: + return + + req = FileClose.Request(file_path=self.name) + ret = self._fm.cli_close(req) + self.name = None + _check_raise_errno(ret) + + def read(self, size: int = 1) -> bytearray: + req = FileRead.Request(file_path=self.name, offset=self.offset, size=size) + ret = self._fm.cli_read.call(req) + _check_raise_errno(ret) + self.offset += len(ret.data) + return bytearray(ret.data) + + def write(self, bin_data: typing.Union[bytes, bytearray]): + data_len = len(bin_data) + + req = FileWrite.Request(file_path=self.name, offset=self.offset, data=bin_data) + ret = self._fm.cli_write.call(req) + _check_raise_errno(ret) + self.offset += data_len + if self.offset > self.size: + self.size = self.offset + + def tell(self): + return self.offset + + def seek(self, offset, whence=os.SEEK_SET): + if whence is os.SEEK_SET: + self.offset = offset + elif whence is os.SEEK_END: + self.offset = offset + self.size + elif whence is os.SEEK_CUR: + self.offset += offset + else: + raise ValueError("Unknown whence") + + def truncate(self, size: int = 0): + req = FileTruncate.Request(file_path=self.name, length=size) + ret = self._fm.cli_truncate.call(req) + _check_raise_errno(ret) + + @property + def closed(self): + return self.name is None + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_value, traceback): + self.close() + + +class FTPPlugin(PluginModule): + """FTP plugin interface.""" + + @cached_property + def cli_open(self) -> rclpy.node.Client: + return self.create_client(FileOpen, ("ftp", "open")) + + @cached_property + def cli_close(self) -> rclpy.node.Client: + return self.create_client(FileClose, ("ftp", "close")) + + @cached_property + def cli_read(self) -> rclpy.node.Client: + return self.create_client(FileRead, ("ftp", "read")) + + @cached_property + def cli_write(self) -> rclpy.node.Client: + return self.create_client(FileWrite, ("ftp", "write")) + + @cached_property + def cli_truncate(self) -> rclpy.node.Client: + return self.create_client(FileTruncate, ("ftp", "truncate")) + + @cached_property + def cli_listdir(self) -> rclpy.node.Client: + return self.create_client(FileList, ("ftp", "list")) + + @cached_property + def cli_unlink(self) -> rclpy.node.Client: + return self.create_client(FileRemove, ("ftp", "remove")) + + @cached_property + def cli_mkdir(self) -> rclpy.node.Client: + return self.create_client(FileMakeDir, ("ftp", "mkdir")) + + @cached_property + def cli_rmdir(self) -> rclpy.node.Client: + return self.create_client(FileRemoveDir, ("ftp", "rmdir")) + + @cached_property + def cli_rename(self) -> rclpy.node.Client: + return self.create_client(FileRename, ("ftp", "rename")) + + @cached_property + def cli_checksum(self) -> rclpy.node.Client: + return self.create_client(FileChecksum, ("ftp", "checksum")) + + @cached_property + def cli_reset(self) -> rclpy.node.Client: + return self.create_client(Empty, ("ftp", "reset")) + + def open(self, path: str, mode: str = "r") -> FTPFile: + return FTPFile(fm=self, name=path, mode=mode) + + def listdir(self, dir_path: str) -> typing.List[FileEntry]: + req = FileList.Request(dir_path=dir_path) + ret = self.cli_listdir.call(req) + _check_raise_errno(ret) + return ret.list + + def unlink(self, path: str): + req = FileRemove.Request(file_path=path) + ret = self.cli_unlink.call(req) + _check_raise_errno(ret) + + def mkdir(self, path: str): + req = FileMakeDir.Request(dir_path=path) + ret = self.cli_mkdir.call(req) + _check_raise_errno(ret) + + def rmdir(self, path: str): + req = FileRemoveDir.Request(dir_path=path) + ret = self.cli_rmdir.call(req) + _check_raise_errno(ret) + + def rename(self, old_path: str, new_path: str): + req = FileRename.Request(old_path=old_path, new_path=new_path) + ret = self.cli_rename.call(req) + _check_raise_errno(ret) + + def checksum(self, path: str) -> int: + req = FileChecksum.Request(file_path=path) + ret = self.cli_checksum.call(req) + _check_raise_errno(ret) + return ret.crc32 + + def reset_server( + self, + ): + req = Empty.Request() + self.cli_reset.call(req) diff --git a/mavros/mavros/global_position.py b/mavros/mavros/global_position.py new file mode 100644 index 000000000..08b79f2b0 --- /dev/null +++ b/mavros/mavros/global_position.py @@ -0,0 +1,69 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md + +import rclpy +from geometry_msgs.msg import TwistStamped +from nav_msgs.msg import Odometry +from sensor_msgs.msg import NavSatFix +from std_msgs.msg import Float64, UInt32 + +from .base import SENSOR_QOS, PluginModule, SubscriptionCallable + + +class GlobalPositionPlugin(PluginModule): + """Global position plugin.""" + + def subscribe_raw_fix( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + NavSatFix, ("global_position", "raw", "fix"), callback, qos_profile + ) + + def subscribe_raw_gps_vel( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + TwistStamped, ("global_position", "raw", "gps_vel"), callback, qos_profile + ) + + def subscribe_raw_satellites( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + UInt32, ("global_position", "raw", "satellites"), callback, qos_profile + ) + + def subscribe_fix( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + NavSatFix, ("global_position", "global"), callback, qos_profile + ) + + def subscribe_odom( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + Odometry, ("global_position", "local"), callback, qos_profile + ) + + def subscribe_rel_alt( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + Float64, ("global_position", "rel_alt"), callback, qos_profile + ) + + def subscribe_compass_hdg( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + Float64, ("global_position", "compass_hdg"), callback, qos_profile + ) diff --git a/mavros/mavros/local_position.py b/mavros/mavros/local_position.py new file mode 100644 index 000000000..c56d80b72 --- /dev/null +++ b/mavros/mavros/local_position.py @@ -0,0 +1,82 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md + +import rclpy +from geometry_msgs.msg import ( + AccelWithCovarianceStamped, + PoseStamped, + PoseWithCovarianceStamped, + TwistStamped, + TwistWithCovarianceStamped, +) +from nav_msgs.msg import Odometry + +from .base import SENSOR_QOS, PluginModule, SubscriptionCallable + + +class LocalPositionPlugin(PluginModule): + """Local position plugin.""" + + def subscribe_pose( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + PoseStamped, ("local_position", "pose"), callback, qos_profile + ) + + def subscribe_pose_cov( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + PoseWithCovarianceStamped, + ("local_position", "pose_cov"), + callback, + qos_profile, + ) + + def subscribe_velocity_local( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + TwistStamped, ("local_position", "velocity_local"), callback, qos_profile + ) + + def subscribe_velocity_body( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + TwistStamped, ("local_position", "velocity_body"), callback, qos_profile + ) + + def subscribe_velocity_body_cov( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + TwistWithCovarianceStamped, + ("local_position", "velocity_body_cov"), + callback, + qos_profile, + ) + + def subscribe_accel( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + AccelWithCovarianceStamped, + ("local_position", "accel"), + callback, + qos_profile, + ) + + def subscribe_odom( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + Odometry, ("local_position", "local"), callback, qos_profile + ) diff --git a/mavros/src/mavros/mavlink.py b/mavros/mavros/mavlink.py similarity index 57% rename from mavros/src/mavros/mavlink.py rename to mavros/mavros/mavlink.py index fa5175f76..a008e12fc 100644 --- a/mavros/src/mavros/mavlink.py +++ b/mavros/mavros/mavlink.py @@ -1,21 +1,28 @@ # -*- coding: utf-8 -*- # vim:set ts=4 sw=4 et: # -# Copyright 2015 Vladimir Ermakov. +# Copyright 2015,2021 Vladimir Ermakov. # # This file is part of the mavros package and subject to the license terms # in the top-level LICENSE file of the mavros repository. # https://github.com/mavlink/mavros/tree/master/LICENSE.md -import rospy import struct +import typing + +import rclpy.time from pymavlink import mavutil -from pymavlink.generator.mavcrc import x25crc -from mavros_msgs.msg import Mavlink +from pymavlink.generator.mavcrc import x25crc # noqa F401 from std_msgs.msg import Header +from builtin_interfaces.msg import Time +from mavros_msgs.msg import Mavlink +from .utils import system_now -def convert_to_bytes(msg): +MAVLink_message = typing.TypeVar("MAVLink_message") + + +def convert_to_bytes(msg: Mavlink) -> bytearray: """ Re-builds the MAVLink byte stream from mavros_msgs/Mavlink messages. @@ -29,25 +36,41 @@ def convert_to_bytes(msg): msg_len = 6 + msg.len # header + payload length msgdata = bytearray( struct.pack( - '> 8) & 0xff, (msg.msgid >> 16) & 0xff, - *msg.payload64)) + "> 8) & 0xFF, + (msg.msgid >> 16) & 0xFF, + *msg.payload64, + ) + ) if payload_octets != msg.len / 8: # message is shorter than payload octets msgdata = msgdata[:msg_len] # finalize - msgdata += struct.pack(' typing.List[int]: + """Convert payload bytes to Mavlink.payload64.""" payload_bytes = bytearray(payload_bytes) payload_len = len(payload_bytes) - payload_octets = payload_len / 8 + payload_octets = int(payload_len / 8) if payload_len % 8 > 0: payload_octets += 1 - payload_bytes += b'\0' * (8 - payload_len % 8) + payload_bytes += b"\0" * (8 - payload_len % 8) - return struct.unpack('<%dQ' % payload_octets, payload_bytes) + return struct.unpack(f"<{payload_octets}Q", payload_bytes) -def convert_to_rosmsg(mavmsg, stamp=None): +def convert_to_rosmsg( + mavmsg: MAVLink_message, stamp: typing.Optional[rclpy.time.Time] = None +) -> Mavlink: """ - Convert pymavlink message to Mavlink.msg + Convert pymavlink message to Mavlink.msg. - Currently supports MAVLink v1.0 only. + Currently supports both MAVLink v1.0 and v2.0, + but without signing. """ if stamp is not None: header = Header(stamp=stamp) else: - header = Header(stamp=rospy.get_rostime()) + stamp = Time() + stamp.sec, stamp.nanosec = system_now().seconds_nanoseconds() + header = Header(stamp=stamp) if mavutil.mavlink20(): # XXX Need some api to retreive signature block. if mavmsg.get_signed(): - rospy.logerr("Signed message can't be converted to rosmsg.") + raise ValueError("Signed message can't be converted to rosmsg.") hdr = mavmsg.get_header() return Mavlink( @@ -98,7 +126,7 @@ def convert_to_rosmsg(mavmsg, stamp=None): msgid=hdr.msgId, checksum=mavmsg.get_crc(), payload64=convert_to_payload64(mavmsg.get_payload()), - signature=None, # FIXME #569 + signature=None, # FIXME #569 ) else: @@ -112,5 +140,5 @@ def convert_to_rosmsg(mavmsg, stamp=None): compid=mavmsg.get_srcComponent(), msgid=mavmsg.get_msgId(), checksum=mavmsg.get_crc(), - payload64=convert_to_payload64(mavmsg.get_payload()) + payload64=convert_to_payload64(mavmsg.get_payload()), ) diff --git a/mavros/mavros/mission.py b/mavros/mavros/mission.py new file mode 100644 index 000000000..dfd57c3cb --- /dev/null +++ b/mavros/mavros/mission.py @@ -0,0 +1,245 @@ +# -*- python -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2014,2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md + +import csv +import itertools +import threading +import typing +from collections import OrderedDict + +import rclpy + +from mavros_msgs.msg import CommandCode, Waypoint, WaypointList +from mavros_msgs.srv import ( + WaypointClear, + WaypointPull, + WaypointPush, + WaypointSetCurrent, +) + +from .base import ( + SERVICE_WAIT_TIMEOUT, + STATE_QOS, + PluginModule, + ServiceWaitTimeout, + SubscriptionCallable, + cached_property, +) + +FRAMES = { + Waypoint.FRAME_GLOBAL: "GAA", + Waypoint.FRAME_GLOBAL_REL_ALT: "GRA", + Waypoint.FRAME_LOCAL_ENU: "LOC-ENU", + Waypoint.FRAME_LOCAL_NED: "LOC-NED", + Waypoint.FRAME_MISSION: "MIS", +} + +NAV_CMDS = { + CommandCode.NAV_LAND: "LAND", + CommandCode.NAV_LOITER_TIME: "LOITER-TIME", + CommandCode.NAV_LOITER_TURNS: "LOITER-TURNS", + CommandCode.NAV_LOITER_UNLIM: "LOITER-UNLIM", + CommandCode.NAV_RETURN_TO_LAUNCH: "RTL", + CommandCode.NAV_TAKEOFF: "TAKEOFF", + CommandCode.NAV_WAYPOINT: "WAYPOINT", + CommandCode.CONDITION_DELAY: "COND-DELAY", + CommandCode.CONDITION_CHANGE_ALT: "COND-CHANGE-ALT", + CommandCode.CONDITION_DISTANCE: "COND-DISTANCE", + CommandCode.CONDITION_YAW: "COND-YAW", + # CommandCode.CONDITION_GATE: 'COND-GATE', # XXX(vooon): gone? not present in pymavlink 2.4.19 + CommandCode.DO_JUMP: "DO-JUMP", + CommandCode.DO_CHANGE_SPEED: "DO-CHANGE-SPEED", + CommandCode.DO_SET_RELAY: "DO-SET-RELAY", + CommandCode.DO_REPEAT_RELAY: "DO-REPEAT-RELAY", + CommandCode.DO_SET_SERVO: "DO-SET-SERVO", + CommandCode.DO_REPEAT_SERVO: "DO-REPEAT-SERVO", + CommandCode.DO_SET_ROI: "DO-SET-ROI", + CommandCode.NAV_FENCE_RETURN_POINT: "FENCE-RETURN", + CommandCode.NAV_FENCE_POLYGON_VERTEX_INCLUSION: "FENCE-VERTEX-INC", + CommandCode.NAV_FENCE_POLYGON_VERTEX_EXCLUSION: "FENCE-VERTEX-EXC", + CommandCode.NAV_FENCE_CIRCLE_INCLUSION: "FENCE-CIRCLE-INC", + CommandCode.NAV_FENCE_CIRCLE_EXCLUSION: "FENCE-CIRCLE-EXC", + CommandCode.NAV_RALLY_POINT: "RALLY", +} + + +class PlanFile: + """Base class for waypoint file parsers.""" + + mission: typing.Optional[typing.List[Waypoint]] = None + fence: typing.Optional[typing.List[Waypoint]] = None + rally: typing.Optional[typing.List[Waypoint]] = None + + def load(self, file_: typing.TextIO) -> "PlanFile": + """Return a iterable of waypoints.""" + raise NotImplementedError + + def save(self, file_: typing.TextIO): + """Write waypoints to file.""" + raise NotImplementedError + + +class QGroundControlWPL(PlanFile): + """Parse QGC waypoint file.""" + + file_header = "QGC WPL 120" + known_versions = (110, 120) + + FieldTypes = typing.Union[bool, int, float] + fields_map: typing.Mapping[ + str, typing.Callable[[typing.Any], FieldTypes] + ] = OrderedDict( + is_current=lambda x: bool(int(x)), + frame=int, + command=int, + param1=float, + param2=float, + param3=float, + param4=float, + x_lat=float, + y_long=float, + z_alt=float, + autocontinue=lambda x: bool(int(x)), + ) + + class CSVDialect(csv.Dialect): + delimiter = "\t" + doublequote = False + skipinitialspace = True + lineterminator = "\r\n" + quoting = csv.QUOTE_NONE + + def _parse_wpl_file(self, file_: typing.TextIO): + got_header = False + reader = csv.reader( + file_, + self.CSVDialect, + ) + for data in reader: + if data[0].startswith("#"): + continue + + if not got_header: + qgc, wpl, ver = data[0].split(" ", 3) + if qgc == "QGC" and wpl == "WPL" and int(ver) in self.known_versions: + got_header = True + + else: + yield Waypoint( + **{ + k: v(data[i]) + for i, (k, v) in enumerate(self.fields_map.items(), start=1) + } + ) + + def load(self, file_: typing.TextIO) -> PlanFile: + self.mission = list(self._parse_wpl_file(file_)) + self.fence = None + self.rally = None + return self + + def save(self, file_: typing.TextIO): + assert self.mission is not None, "empty mission" + assert self.fence is None, "WPL do not support geofences" + assert self.rally is None, "WPL do not support rallypoints" + + def flt_bool(x): + if isinstance(x, bool): + return int(x) + return x + + writer = csv.writer(file_, self.CSVDialect) + writer.writerow((self.file_header,)) + for seq, w in enumerate(self.mission): + row = itertools.chain( + (seq,), (flt_bool(getattr(w, k)) for k in self.fields_map.keys()) + ) + writer.writerow(row) + + +class QGroundControlPlan(PlanFile): + pass # TODO(vooon): implement me! + + +class MissionPluginBase(PluginModule): + + _plugin_ns = "mission" + _plugin_list_topic = "waypoints" + + _points: typing.List[Waypoint] = [] + _points_sub = None + + @cached_property + def cli_pull(self) -> rclpy.node.Client: + return self.create_client(WaypointPull, (self._plugin_ns, "pull")) + + @cached_property + def cli_push(self) -> rclpy.node.Client: + return self.create_client(WaypointPush, (self._plugin_ns, "push")) + + @cached_property + def cli_clear(self) -> rclpy.node.Client: + return self.create_client(WaypointClear, (self._plugin_ns, "clear")) + + def subscribe_points( + self, + callback: SubscriptionCallable, + qos_profile: rclpy.qos.QoSProfile = STATE_QOS, + ) -> rclpy.node.Subscription: + """Subscribe to points list (waypoints, fences, rallypoints).""" + return self.create_subscription( + WaypointList, + (self._plugin_ns, self._plugin_list_topic), + callback, + qos_profile, + ) + + @property + def points(self) -> typing.List[Waypoint]: + """Subscribe and return points cache.""" + if self._points_sub is not None: + return self._points + + done_evt = threading.Event() + + def handler(ml: WaypointList): + self._points = ml.waypoints + done_evt.set() + + self._points_sub = self.subscribe_points(handler) + if not done_evt.wait(SERVICE_WAIT_TIMEOUT): + raise ServiceWaitTimeout( + f"timeout waiting for {self._points_sub.topic_name}" + ) + + return self._points + + +class WaypointPlugin(MissionPluginBase): + """Interface to waypoint plugin.""" + + @cached_property + def cli_set_current(self) -> rclpy.node.Client: + return self._node.create_client( + WaypointSetCurrent, (self._plugin_ns, "set_current") + ) + + +class GeofencePlugin(MissionPluginBase): + """Interface to geofence plugin.""" + + _plugin_ns = "geofence" + _plugin_list_topic = "fences" + + +class RallypointPlugin(MissionPluginBase): + """Interface to rallypoint plugin.""" + + _plugin_ns = "rallypoint" + _plugin_list_topic = "rallypoints" diff --git a/mavros/mavros/nuttx_crc32.py b/mavros/mavros/nuttx_crc32.py new file mode 100644 index 000000000..b4a88f6e1 --- /dev/null +++ b/mavros/mavros/nuttx_crc32.py @@ -0,0 +1,277 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2014 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md + +__all__ = ("nuttx_crc32",) + +CRC32_TAB = ( + 0x00000000, + 0x77073096, + 0xEE0E612C, + 0x990951BA, + 0x076DC419, + 0x706AF48F, + 0xE963A535, + 0x9E6495A3, + 0x0EDB8832, + 0x79DCB8A4, + 0xE0D5E91E, + 0x97D2D988, + 0x09B64C2B, + 0x7EB17CBD, + 0xE7B82D07, + 0x90BF1D91, + 0x1DB71064, + 0x6AB020F2, + 0xF3B97148, + 0x84BE41DE, + 0x1ADAD47D, + 0x6DDDE4EB, + 0xF4D4B551, + 0x83D385C7, + 0x136C9856, + 0x646BA8C0, + 0xFD62F97A, + 0x8A65C9EC, + 0x14015C4F, + 0x63066CD9, + 0xFA0F3D63, + 0x8D080DF5, + 0x3B6E20C8, + 0x4C69105E, + 0xD56041E4, + 0xA2677172, + 0x3C03E4D1, + 0x4B04D447, + 0xD20D85FD, + 0xA50AB56B, + 0x35B5A8FA, + 0x42B2986C, + 0xDBBBC9D6, + 0xACBCF940, + 0x32D86CE3, + 0x45DF5C75, + 0xDCD60DCF, + 0xABD13D59, + 0x26D930AC, + 0x51DE003A, + 0xC8D75180, + 0xBFD06116, + 0x21B4F4B5, + 0x56B3C423, + 0xCFBA9599, + 0xB8BDA50F, + 0x2802B89E, + 0x5F058808, + 0xC60CD9B2, + 0xB10BE924, + 0x2F6F7C87, + 0x58684C11, + 0xC1611DAB, + 0xB6662D3D, + 0x76DC4190, + 0x01DB7106, + 0x98D220BC, + 0xEFD5102A, + 0x71B18589, + 0x06B6B51F, + 0x9FBFE4A5, + 0xE8B8D433, + 0x7807C9A2, + 0x0F00F934, + 0x9609A88E, + 0xE10E9818, + 0x7F6A0DBB, + 0x086D3D2D, + 0x91646C97, + 0xE6635C01, + 0x6B6B51F4, + 0x1C6C6162, + 0x856530D8, + 0xF262004E, + 0x6C0695ED, + 0x1B01A57B, + 0x8208F4C1, + 0xF50FC457, + 0x65B0D9C6, + 0x12B7E950, + 0x8BBEB8EA, + 0xFCB9887C, + 0x62DD1DDF, + 0x15DA2D49, + 0x8CD37CF3, + 0xFBD44C65, + 0x4DB26158, + 0x3AB551CE, + 0xA3BC0074, + 0xD4BB30E2, + 0x4ADFA541, + 0x3DD895D7, + 0xA4D1C46D, + 0xD3D6F4FB, + 0x4369E96A, + 0x346ED9FC, + 0xAD678846, + 0xDA60B8D0, + 0x44042D73, + 0x33031DE5, + 0xAA0A4C5F, + 0xDD0D7CC9, + 0x5005713C, + 0x270241AA, + 0xBE0B1010, + 0xC90C2086, + 0x5768B525, + 0x206F85B3, + 0xB966D409, + 0xCE61E49F, + 0x5EDEF90E, + 0x29D9C998, + 0xB0D09822, + 0xC7D7A8B4, + 0x59B33D17, + 0x2EB40D81, + 0xB7BD5C3B, + 0xC0BA6CAD, + 0xEDB88320, + 0x9ABFB3B6, + 0x03B6E20C, + 0x74B1D29A, + 0xEAD54739, + 0x9DD277AF, + 0x04DB2615, + 0x73DC1683, + 0xE3630B12, + 0x94643B84, + 0x0D6D6A3E, + 0x7A6A5AA8, + 0xE40ECF0B, + 0x9309FF9D, + 0x0A00AE27, + 0x7D079EB1, + 0xF00F9344, + 0x8708A3D2, + 0x1E01F268, + 0x6906C2FE, + 0xF762575D, + 0x806567CB, + 0x196C3671, + 0x6E6B06E7, + 0xFED41B76, + 0x89D32BE0, + 0x10DA7A5A, + 0x67DD4ACC, + 0xF9B9DF6F, + 0x8EBEEFF9, + 0x17B7BE43, + 0x60B08ED5, + 0xD6D6A3E8, + 0xA1D1937E, + 0x38D8C2C4, + 0x4FDFF252, + 0xD1BB67F1, + 0xA6BC5767, + 0x3FB506DD, + 0x48B2364B, + 0xD80D2BDA, + 0xAF0A1B4C, + 0x36034AF6, + 0x41047A60, + 0xDF60EFC3, + 0xA867DF55, + 0x316E8EEF, + 0x4669BE79, + 0xCB61B38C, + 0xBC66831A, + 0x256FD2A0, + 0x5268E236, + 0xCC0C7795, + 0xBB0B4703, + 0x220216B9, + 0x5505262F, + 0xC5BA3BBE, + 0xB2BD0B28, + 0x2BB45A92, + 0x5CB36A04, + 0xC2D7FFA7, + 0xB5D0CF31, + 0x2CD99E8B, + 0x5BDEAE1D, + 0x9B64C2B0, + 0xEC63F226, + 0x756AA39C, + 0x026D930A, + 0x9C0906A9, + 0xEB0E363F, + 0x72076785, + 0x05005713, + 0x95BF4A82, + 0xE2B87A14, + 0x7BB12BAE, + 0x0CB61B38, + 0x92D28E9B, + 0xE5D5BE0D, + 0x7CDCEFB7, + 0x0BDBDF21, + 0x86D3D2D4, + 0xF1D4E242, + 0x68DDB3F8, + 0x1FDA836E, + 0x81BE16CD, + 0xF6B9265B, + 0x6FB077E1, + 0x18B74777, + 0x88085AE6, + 0xFF0F6A70, + 0x66063BCA, + 0x11010B5C, + 0x8F659EFF, + 0xF862AE69, + 0x616BFFD3, + 0x166CCF45, + 0xA00AE278, + 0xD70DD2EE, + 0x4E048354, + 0x3903B3C2, + 0xA7672661, + 0xD06016F7, + 0x4969474D, + 0x3E6E77DB, + 0xAED16A4A, + 0xD9D65ADC, + 0x40DF0B66, + 0x37D83BF0, + 0xA9BCAE53, + 0xDEBB9EC5, + 0x47B2CF7F, + 0x30B5FFE9, + 0xBDBDF21C, + 0xCABAC28A, + 0x53B39330, + 0x24B4A3A6, + 0xBAD03605, + 0xCDD70693, + 0x54DE5729, + 0x23D967BF, + 0xB3667A2E, + 0xC4614AB8, + 0x5D681B02, + 0x2A6F2B94, + 0xB40BBE37, + 0xC30C8EA1, + 0x5A05DF1B, + 0x2D02EF8D, +) + + +def nuttx_crc32(b: bytes, crc32val: int = 0) -> int: + """CRC32 algo from NuttX.""" + for b in bytearray(b): + crc32val = CRC32_TAB[(crc32val ^ b) & 0xFF] ^ (crc32val >> 8) + + return crc32val diff --git a/mavros/mavros/param.py b/mavros/mavros/param.py new file mode 100644 index 000000000..ce44b4d13 --- /dev/null +++ b/mavros/mavros/param.py @@ -0,0 +1,347 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2014,2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md + +import csv +import datetime +import typing + +import rclpy +from rcl_interfaces.msg import Parameter as ParameterMsg +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import GetParameters, ListParameters, SetParameters +from rclpy.parameter import Parameter + +from mavros_msgs.msg import ParamEvent +from mavros_msgs.srv import ParamPull, ParamSetV2 + +from .base import PARAMETERS_QOS, PluginModule, SubscriptionCallable, cached_property +from .utils import ( + call_get_parameters, + call_list_parameters, + call_set_parameters_check_and_raise, + parameter_from_parameter_value, +) + + +class ParamFile: + """Base class for param file parsers.""" + + parameters: typing.Optional[typing.Dict[str, Parameter]] = None + stamp: typing.Optional[datetime.datetime] = None + tgt_system: int = 1 + tgt_component: int = 1 + + def load(self, file_: typing.TextIO) -> "ParamFile": + """Load Parameters from a file.""" + raise NotImplementedError + + def save(self, file_: typing.TextIO): + """Write Parameters to a file.""" + raise NotImplementedError + + +class MavProxyParam(ParamFile): + """Parse MavProxy parm file.""" + + class CSVDialect(csv.Dialect): + delimiter = " " + doublequote = False + skipinitialspace = True + lineterminator = "\r\n" + quoting = csv.QUOTE_NONE + escapechar = "" + + def _parse_param_file(self, file_: typing.TextIO): + def to_numeric(x): + return float(x) if "." in x else int(x) + + for data in csv.reader(file_, self.CSVDialect): + if data[0].startswith("#"): + continue # skip comments + + if len(data) != 2: + raise ValueError("wrong field count") + + yield Parameter(data[0].strip(), value=to_numeric(data[1])) + + def load(self, file_: typing.TextIO) -> ParamFile: + self.parameters = {p.name: p for p in self._parse_param_file(file_)} + return self + + def save(self, file_: typing.TextIO): + if self.stamp is None: + self.stamp = datetime.datetime.now() + + writer = csv.writer(file_, self.CSVDialect) + file_.write( + f"""#NOTE: {self.stamp.strftime("%d.%m.%Y %T")}{self.CSVDialect.lineterminator}""" + ) + for k, p in self.parameters.items(): + writer.writerow((p.name, p.value)) + + +class MissionPlannerParam(MavProxyParam): + """Parse MissionPlanner param file.""" + + class CSVDialect(csv.Dialect): + delimiter = "," + doublequote = False + skipinitialspace = True + lineterminator = "\r\n" + quoting = csv.QUOTE_NONE + escapechar = "" + + +class QGroundControlParam(ParamFile): + """Parse QGC param file.""" + + class CSVDialect(csv.Dialect): + delimiter = "\t" + doublequote = False + skipinitialspace = True + lineterminator = "\n" + quoting = csv.QUOTE_NONE + escapechar = "" + + def _parse_param_file(self, file_: typing.TextIO): + def to_numeric(x): + return float(x) if "." in x else int(x) + + for data in csv.reader(file_, self.CSVDialect): + if data[0].startswith("#"): + continue # skip comments + + if len(data) != 5: + raise ValueError("wrong field count") + + yield Parameter(data[2].strip(), value=to_numeric(data[3])) + + def load(self, file_: typing.TextIO) -> ParamFile: + self.parameters = {p.name: p for p in self._parse_param_file(file_)} + return self + + def save(self, file_: typing.TextIO): + def to_type(x): + if isinstance(x, float): + return 9 # REAL32 + elif isinstance(x, int): + return 6 # INT32 + else: + raise ValueError(f"unknown type: {type(x):r}") + + if self.stamp is None: + self.stamp = datetime.datetime.now() + + writer = csv.writer(file_, self.CSVDialect) + writer.writerow((f"""# NOTE: {self.stamp.strftime("%d.%m.%Y %T")}""",)) + writer.writerow( + ( + f"# Onboard parameters saved by " + f"mavparam for ({self.tgt_system}.{self.tgt_component})", + ) + ) + writer.writerow(("# MAV ID", "COMPONENT ID", "PARAM NAME", "VALUE", "(TYPE)")) + for k, p in self.parameters.items(): + writer.writerow( + ( + self.tgt_system, + self.tgt_component, + p.name, + p.value, + to_type(p.value), + ) + ) + + +class ParamPlugin(PluginModule): + """Parameter plugin interface.""" + + timeout_sec: float = 5.0 + _parameters = None + _event_sub = None + + @cached_property + def cli_list_parameters(self) -> rclpy.node.Client: + """Client for ListParameters service.""" + return self.create_client(ListParameters, ("param", "list_parameters")) + + @cached_property + def cli_get_parameters(self) -> rclpy.node.Client: + """Client for GetParameters service.""" + return self.create_client(GetParameters, ("param", "get_parameters")) + + @cached_property + def cli_set_parameters(self) -> rclpy.node.Client: + """Client for SetParameters service.""" + return self.create_client(SetParameters, ("param", "set_parameters")) + + @cached_property + def cli_pull(self) -> rclpy.node.Client: + """Client for ParamPull service.""" + return self.create_client(ParamPull, ("param", "pull")) + + @cached_property + def cli_set(self) -> rclpy.node.Client: + """Client for ParamSetV2 service.""" + return self.create_client(ParamSetV2, ("param", "set")) + + def subscribe_events( + self, + callback: SubscriptionCallable, + qos_profile: rclpy.qos.QoSProfile = PARAMETERS_QOS, + ) -> rclpy.node.Subscription: + """Subscribe to parameter events.""" + return self.create_subscription( + ParamEvent, ("param", "event"), callback, qos_profile + ) + + def call_pull(self, *, force_pull: bool = False) -> ParamPull.Response: + """Do a call to ParamPull service.""" + lg = self.get_logger() + + req = ParamPull.Request(force_pull=force_pull) + resp = self.cli_pull.call(req) + lg.debug(f"pull result: {resp}") + return resp + + @property + def values(self) -> "ParamDict": + """Provide current state of parameters and allows to change them.""" + if self._parameters is not None: + return self._parameters + + pm = ParamDict() + pm._pm = self + + # 1. subscribe for parameter updates + self._event_sub = self.subscribe_events(pm._event_handler) + self._parameters = pm + + # 2. pull parameters, if it isn't yet done + # we'll get bunch of events + self.call_pull() + + # 3. if too small events come, request whole list + if len(pm) < 10: + names = call_list_parameters( + node=self._node, client=self.cli_list_parameters + ) + for k, v in call_get_parameters( + node=self._node, client=self.cli_get_parameters, names=names + ).items(): + pm.setdefault(k, v) + + return pm + + +class ParamDict(dict): + """ + ParamDict wrapper. + + That class holds states of parameters + and allows to upload new items. + """ + + class NoSet: + """Wrapper to mark values we do not want to send set request for.""" + + value: Parameter + + def __init__(self, p): + self.value = p + + _pm: "ParamPlugin" = None + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + def __getitem__(self, key: str) -> Parameter: + return super().__getitem__(key) + + def __setitem__(self, key: str, value): + do_call_set, value = self._set_item(key, value) + if do_call_set: + call_set_parameters_check_and_raise( + node=self._pm._node, + client=self._pm.cli_set_parameters, + parameters=[value], + ) + + def _set_item(self, key: str, value) -> (bool, Parameter): + is_no_set = False + if isinstance(value, ParamDict.NoSet): + is_no_set = True + value = value.value + + if isinstance(value, Parameter): + pass + elif isinstance(value, ParameterValue): + value = parameter_from_parameter_value(key, value) + elif isinstance(value, ParameterMsg): + value = Parameter.from_parameter_msg(value) + else: + value = Parameter(name=key, value=value) + + assert key == value.name + + do_call_set = not is_no_set and self.get(key, Parameter(name=key)) != value + super().__setitem__(key, value) + return do_call_set, value + + def __getattr__(self, key: str): + try: + return object.__getattribute__(self, key) + except AttributeError: + try: + return self[key] + except KeyError: + raise AttributeError(key) + + def __setattr__(self, key: str, value): + try: + object.__getattribute__(self, key) + except AttributeError: + try: + self[key] = value + except Exception as ex: + raise AttributeError(f"{key}: {ex}") + else: + object.__setattr__(self, key, value) + + def __delattr__(self, key: str): + try: + object.__getattribute__(self, key) + except AttributeError: + try: + del self[key] + except KeyError: + raise AttributeError(key) + else: + object.__delattr__(self, key) + + def update(self, *args, **kwargs): + keys_to_set = [] + for k, v in dict(*args, **kwargs).items(): + do_call_set, _ = self._set_item(k, v) + if do_call_set: + keys_to_set.append(k) + + if keys_to_set: + call_set_parameters_check_and_raise( + node=self._pm._node, + client=self._pm.cli_set_parameters, + parameters=[self[k] for k in keys_to_set], + ) + + def setdefault(self, key: str, value=None): + if key not in self: + self[key] = ParamDict.NoSet(value) + + def _event_handler(self, msg: ParamEvent): + self[msg.param_id] = parameter_from_parameter_value(msg.param_id, msg.value) diff --git a/mavros/mavros/setpoint.py b/mavros/mavros/setpoint.py new file mode 100644 index 000000000..9b2d1aa72 --- /dev/null +++ b/mavros/mavros/setpoint.py @@ -0,0 +1,136 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2015,2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md + +import rclpy +from geographic_msgs.msg import GeoPoseStamped +from geometry_msgs.msg import PoseStamped, Twist, TwistStamped, Vector3Stamped +from nav_msgs.msg import Path +from std_srvs.srv import Trigger +from trajectory_msgs.msg import MultiDOFJointTrajectory + +from mavros_msgs.msg import AttitudeTarget, GlobalPositionTarget, PositionTarget, Thrust + +from .base import SENSOR_QOS, PluginModule, SubscriptionCallable, cached_property + +QOS = SENSOR_QOS + + +class SetpointAccelPlugin(PluginModule): + @cached_property + def pub_accel(self) -> rclpy.node.Publisher: + return self.create_publisher(Vector3Stamped, ("setpoint_accel", "accel"), QOS) + + +class SetpointAttitudePlugin(PluginModule): + @cached_property + def pub_attitude(self) -> rclpy.node.Publisher: + return self.create_publisher( + PoseStamped, ("setpoint_attitude", "attitude"), QOS + ) + + @cached_property + def pub_cmd_vel(self) -> rclpy.node.Publisher: + return self.create_publisher( + TwistStamped, ("setpoint_attitude", "cmd_vel"), QOS + ) + + @cached_property + def pub_thrust(self) -> rclpy.node.Publisher: + return self.create_publisher(Thrust, ("setpoint_attitude", "thrust"), QOS) + + +class SetpointPositionPlugin(PluginModule): + @cached_property + def pub_local(self) -> rclpy.node.Publisher: + return self.create_publisher(PoseStamped, ("setpoint_position", "local"), QOS) + + @cached_property + def pub_global(self) -> rclpy.node.Publisher: + return self.create_publisher( + GeoPoseStamped, ("setpoint_position", "global"), QOS + ) + + @cached_property + def pub_global_to_local(self) -> rclpy.node.Publisher: + return self.create_publisher( + GeoPoseStamped, ("setpoint_position", "global_to_local"), QOS + ) + + +class SetpointRawPlugin(PluginModule): + @cached_property + def pub_local(self) -> rclpy.node.Publisher: + return self.create_publisher(PositionTarget, ("setpoint_raw", "local"), QOS) + + @cached_property + def pub_global(self) -> rclpy.node.Publisher: + return self.create_publisher( + GlobalPositionTarget, ("setpoint_raw", "global"), QOS + ) + + @cached_property + def pub_attitude(self) -> rclpy.node.Publisher: + return self.create_publisher(AttitudeTarget, ("setpoint_raw", "attitude"), QOS) + + def subscribe_target_local( + self, callback: SubscriptionCallable, qos_profile=QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + PositionTarget, ("setpoint_raw", "target_local"), callback, qos_profile + ) + + def subscribe_target_global( + self, callback: SubscriptionCallable, qos_profile=QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + GlobalPositionTarget, + ("setpoint_raw", "target_global"), + callback, + qos_profile, + ) + + def subscribe_target_attitude( + self, callback: SubscriptionCallable, qos_profile=QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + AttitudeTarget, ("setpoint_raw", "target_attitude"), callback, qos_profile + ) + + +class SetpointTrajectoryPlugin(PluginModule): + @cached_property + def pub_local(self) -> rclpy.node.Publisher: + return self.create_publisher( + MultiDOFJointTrajectory, ("setpoint_trajectory", "local"), QOS + ) + + def subscribe_desired( + self, callback: SubscriptionCallable, qos_profile=QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + Path, ("setpoint_trajectory", "desired"), callback, qos_profile + ) + + @cached_property + def reset(self) -> rclpy.node.Client: + return self.create_client(Trigger, ("setpoint_trajectory", "reset")) + + +class SetpointVelocityPlugin(PluginModule): + @cached_property + def pub_cmd_vel(self) -> rclpy.node.Publisher: + return self.create_publisher( + TwistStamped, ("setpoint_velocity", "cmd_vel"), QOS + ) + + @cached_property + def pub_cmd_vel_unstamped(self) -> rclpy.node.Publisher: + return self.create_publisher( + Twist, ("setpoint_velocity", "cmd_vel_unstamped"), QOS + ) diff --git a/mavros/mavros/system.py b/mavros/mavros/system.py new file mode 100644 index 000000000..84c38a161 --- /dev/null +++ b/mavros/mavros/system.py @@ -0,0 +1,94 @@ +# -*- python -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md + +import threading +import typing + +import rclpy +from sensor_msgs.msg import BatteryState + +from mavros_msgs.msg import EstimatorStatus, ExtendedState, State, StatusText +from mavros_msgs.srv import MessageInterval, SetMode, StreamRate, VehicleInfoGet + +from .base import ( + SENSOR_QOS, + STATE_QOS, + PluginModule, + SubscriptionCallable, + cached_property, +) + + +class SystemPlugin(PluginModule): + """System plugin.""" + + def subscribe_state( + self, callback: SubscriptionCallable, qos_profile=STATE_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription(State, "state", callback, qos_profile) + + def subscribe_extended_state( + self, callback: SubscriptionCallable, qos_profile=STATE_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + ExtendedState, "extended_state", callback, qos_profile + ) + + def subscribe_estimator_status( + self, callback: SubscriptionCallable, qos_profile=STATE_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + EstimatorStatus, "estimator_status", callback, qos_profile + ) + + def subscribe_battery_state( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription(BatteryState, "battery", callback, qos_profile) + + def subscribe_statustest( + self, callback: SubscriptionCallable, qos_profile=SENSOR_QOS + ) -> rclpy.node.Subscription: + return self.create_subscription( + StatusText, ("statustext", "recv"), callback, qos_profile + ) + + @cached_property + def pub_statustext( + self, + ) -> rclpy.node.Publisher: + return self.create_publisher(StatusText, ("statustext", "send"), SENSOR_QOS) + + @cached_property + def cli_set_mode(self) -> rclpy.node.Client: + return self.create_client(SetMode, "set_mode") + + @cached_property + def cli_set_stream_rate(self) -> rclpy.node.Client: + return self.create_client(StreamRate, "set_stream_rate") + + @cached_property + def cli_set_message_interval(self) -> rclpy.node.Client: + return self.create_client(MessageInterval, "set_message_interval") + + @cached_property + def cli_get_vehicle_info(self) -> rclpy.node.Client: + return self.create_client(VehicleInfoGet, "vehicle_info_get") + + def wait_fcu_connection(self, timeout: typing.Optional[float] = None) -> bool: + """Wait until establishing FCU connection.""" + connected = threading.Event() + + def handler(msg: State): + self.get_logger().debug(f"got state: {msg}") + if msg.connected: + connected.set() + + self.subscribe_state(handler) + return connected.wait(timeout) diff --git a/mavros/mavros/utils.py b/mavros/mavros/utils.py new file mode 100644 index 000000000..d12e7c5ad --- /dev/null +++ b/mavros/mavros/utils.py @@ -0,0 +1,113 @@ +# -*- coding: utf-8 -*- +# vim:set ts=4 sw=4 et: +# +# Copyright 2021 Vladimir Ermakov. +# +# This file is part of the mavros package and subject to the license terms +# in the top-level LICENSE file of the mavros repository. +# https://github.com/mavlink/mavros/tree/master/LICENSE.md + +import functools +import typing + +import rclpy +import rclpy.clock +import rclpy.time +from rcl_interfaces.msg import Parameter as ParameterMsg +from rcl_interfaces.msg import ParameterValue, SetParametersResult +from rcl_interfaces.srv import GetParameters, ListParameters, SetParameters +from rclpy.parameter import Parameter + +from .base import wait_for_service + + +@functools.lru_cache(maxsize=None) +def system_clock() -> rclpy.clock.Clock: + return rclpy.clock.Clock() + + +def system_now() -> rclpy.time.Time: + return system_clock().now() + + +def parameter_from_parameter_value( + name: str, parameter_value: ParameterValue +) -> Parameter: + pmsg = ParameterMsg(name=name, value=parameter_value) + return Parameter.from_parameter_msg(pmsg) + + +def call_list_parameters( + *, + node: rclpy.node.Node, + node_name: typing.Optional[str] = None, + client: typing.Optional[rclpy.node.Client] = None, + prefixes: typing.List[str] = [], +) -> typing.List[str]: + lg = node.get_logger() + + if client is None: + assert node_name is not None + client = node.create_client(ListParameters, f"{node_name}/list_parameters") + + wait_for_service(client, lg) + + req = ListParameters.Request(prefixes=prefixes) + resp = client.call(req) + lg.debug(f"list result: {resp}") + + return resp.result.names + + +def call_get_parameters( + *, + node: rclpy.node.Node, + node_name: typing.Optional[str] = None, + client: typing.Optional[rclpy.node.Client] = None, + names: typing.List[str] = [], +) -> typing.Dict[str, Parameter]: + lg = node.get_logger() + + if client is None: + assert node_name is not None + client = node.create_client(GetParameters, f"{node_name}/get_parameters") + + wait_for_service(client, lg) + + req = GetParameters.Request(names=names) + resp = client.call(req) + lg.debug(f"get result: {resp}") + + return { + name: parameter_from_parameter_value(name, value) + for name, value in zip(names, resp.values) + } + + +def call_set_parameters( + *, + node: rclpy.node.Node, + node_name: typing.Optional[str] = None, + client: typing.Optional[rclpy.node.Client] = None, + parameters: typing.List[Parameter] = [], +) -> typing.Dict[str, SetParametersResult]: + lg = node.get_logger() + + if client is None: + assert node_name is not None + client = node.create_client(SetParameters, f"{node_name}/set_parameters") + + wait_for_service(client, lg) + + req = SetParameters.Request(parameters=[p.to_parameter_msg() for p in parameters]) + resp = client.call(req) + lg.debug(f"set result: {resp}") + + return dict(zip((p.name for p in parameters), resp.results)) + + +def call_set_parameters_check_and_raise(**kwargs): + results = call_set_parameters(**kwargs) + msg = ";".join(f"{k}: {r.reason}" for k, r in results.items() if not r.successful) + if msg: + raise ValueError(msg) diff --git a/mavros/mavros_plugins.xml b/mavros/mavros_plugins.xml index e65aa9507..c93e25f31 100644 --- a/mavros/mavros_plugins.xml +++ b/mavros/mavros_plugins.xml @@ -1,90 +1,141 @@ - - - - Publish system status. - - - Publish IMU data. - - - Access to FCU parameters. - - - Access to FCU mission waypoints. - - - Access to FCU mission rally points. - - - Access to FCU mission Geofence. - - - Publish RC IO state. - - - Send commands to FCU. - - - Publish FCU local position. - - - Publish FCU fused global position and raw GPS fix. - - - Send to FCU external position setpoint. - - - Publish Nav_Controller_Output messages - - - Send to FCU safety allowed area. - - - Publish 3DR Radio modem status. - - - Send to FCU external velocity setpoint. - - - Send to FCU external acceleration/force setpoint. - - - Send to FCU external attitude setpoint. - - - Access to raw POSITION_TARGET_LOCAL_NED messages. - - - Send to FCU setpoints from trajectory. - - - Publish VFR HUD data and WIND estimations. - - - Access to files on FCU via MAVLink-FTP. - - - Synchronise clocks with FCU. - - - Send direct controls values to the actuators - - - Publish manual control values - - - Publish altitude values - - - Publish and subscribe HIL controls, state, GPS and sensor values - - - Publish home position - - - Publish wind estimates + + + + + @brief ActuatorControl plugin +@plugin actuator_control + +Sends actuator controls to FCU controller. + + + @brief Altitude plugin. +@plugin altitude + + + @brief Command plugin. +@plugin command + +Send any command via COMMAND_LONG + + + @brief FTP plugin. +@plugin ftp + + + @brief Geofence manipulation plugin +@plugin geofence + + + @brief Global position plugin. +@plugin global_position + +Publishes global position. Conversion from GPS LLA to ECEF allows +publishing local position to TF and PoseWithCovarianceStamped. + + + @brief home position plugin. +@plugin home_position + +Publishes home position. + + + @brief IMU and attitude data publication plugin +@plugin imu + + + @brief Local position plugin. +@plugin local_position + +Publish local position to TF, PositionStamped, TwistStamped +and Odometry + + + @brief Manual Control plugin +@plugin manual_control + + + @brief nav controller output plugin. +@plugin nav_controller_output + +Publishes nav_controller_output message https://mavlink.io/en/messages/common.html#NAV_CONTROLLER_OUTPUT + + + @brief Parameter manipulation plugin +@plugin param + + + @brief Rallypoint manipulation plugin +@plugin rallypoint + + + @brief RC IO plugin +@plugin rc_io + + + @brief Setpoint acceleration/force plugin +@plugin setpoint_accel + +Send setpoint accelerations/forces to FCU controller. + + + @brief Setpoint attitude plugin +@plugin setpoint_attitude + +Send setpoint attitude/orientation/thrust to FCU controller. + + + @brief Setpoint position plugin +@plugin setpoint_position + +Send setpoint positions to FCU controller. + + + @brief Setpoint RAW plugin +@plugin setpoint_raw + +Send position setpoints and publish current state (return loop). +User can decide what set of filed needed for operation via IGNORE bits. + + + @brief Setpoint TRAJECTORY plugin +@plugin setpoint_trajectory + +Receive trajectory setpoints and send setpoint_raw setpoints along the trajectory. + + + @brief Setpoint velocity plugin +@plugin setpoint_velocity + +Send setpoint velocities to FCU controller. + + + @brief System status plugin. +@plugin sys_status + +Required by all plugins. + + + @brief System time plugin +@plugin sys_time + + + @brief Mission manupulation plugin +@plugin waypoint + + + @brief Wind estimation plugin. +@plugin wind_estimation + diff --git a/mavros/package.xml b/mavros/package.xml index 18b7991b6..8ac15a8cd 100644 --- a/mavros/package.xml +++ b/mavros/package.xml @@ -1,14 +1,14 @@ mavros - 1.16.0 + 2.5.0 MAVROS -- MAVLink extendable communication node for ROS with proxy for Ground Control Station. Vladimir Ermakov - Vladimir Ermakov + GPLv3 LGPLv3 BSD @@ -17,10 +17,14 @@ https://github.com/mavlink/mavros.git https://github.com/mavlink/mavros/issues - catkin + Vladimir Ermakov + + ament_cmake + ament_cmake_python + eigen3_cmake_module + eigen3_cmake_module - boost eigen eigen mavlink @@ -35,17 +39,18 @@ angles - cmake_modules diagnostic_updater - eigen_conversions + message_filters + eigen_stl_containers libmavconn pluginlib - rosconsole_bridge - roscpp + + libconsole-bridge-dev tf2_ros tf2_eigen - message_runtime - rospy + rclcpp + rclcpp_components + rcpputils diagnostic_msgs @@ -58,10 +63,21 @@ std_msgs std_srvs - rosunit + rosidl_default_runtime + rclpy + python3-click + + ament_cmake_gtest + ament_cmake_gmock + ament_cmake_pytest + ament_lint_auto + ament_lint_common gtest + google-mock + ament_cmake + diff --git a/mavros/pytest.ini b/mavros/pytest.ini new file mode 100644 index 000000000..fe55d2ed6 --- /dev/null +++ b/mavros/pytest.ini @@ -0,0 +1,2 @@ +[pytest] +junit_family=xunit2 diff --git a/mavros/scripts/checkid b/mavros/scripts/checkid deleted file mode 100755 index bdeac3cc0..000000000 --- a/mavros/scripts/checkid +++ /dev/null @@ -1,109 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# vim:set ts=4 sw=4 et: -# -# Copyright 2015 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -""" -This script listens to devices connected to mavros and checks against system & component id mismatch errors. -""" - -from __future__ import print_function - -import argparse - -import os -import rospy -import mavros -from mavros.utils import * -from mavros_msgs.msg import Mavlink - - -class Checker(object): - def __init__(self, args): - # dict of sets: (sysid, compid) -> set[msgid...] - self.message_sources = {} - self.messages_received = 0 - self.reports = 0 - self.args = args - - self.sub = rospy.Subscriber("mavlink/from", Mavlink, self.mavlink_from_cb, queue_size=10) - self.timer = rospy.Timer(rospy.Duration(15.0), self.timer_cb, oneshot=False) - - def mavlink_from_cb(self, msg): - ids = (msg.sysid, msg.compid) - if ids in self.message_sources: - self.message_sources[ids].add(msg.msgid) - else: - self.message_sources[ids] = set((msg.msgid, )) - - self.messages_received += 1 - - def timer_cb(self, event): - if self.reports > 0: - print() - - self.reports += 1 - - param_not_exist = False - tgt_ids = [1, 1] - for idx, key in enumerate(( - '/'.join((self.args.mavros_ns, "target_system_id")), - '/'.join((self.args.mavros_ns, "target_component_id")) - )): - try: - tgt_ids[idx] = rospy.get_param(key) - except KeyError: - print("WARNING: %s not set. Used default value: %s" % (key, tgt_ids[idx])) - param_not_exist = True - - if param_not_exist: - print("NOTE: Target parameters may be unset, " - "but that may be result of incorrect --mavros-ns option." - "Double check results!") - - # TODO: add something like colorama to highlight ERROR message - if tuple(tgt_ids) in self.message_sources: - print("OK. I got messages from %d:%d." % tuple(tgt_ids)) - else: - print("ERROR. I got %d addresses, but not your target %d:%d" % - (len(self.message_sources), - tgt_ids[0], tgt_ids[1])) - - print("\n---\nReceived %d messages, from %d addresses" % - (self.messages_received, len(self.message_sources))) - - # TODO: prettytable? - print("sys:comp list of messages") - for address, messages in self.message_sources.items(): - print("% 3d:%-3d %s" % ( - address[0], address[1], - ", ".join(("%d" % msgid for msgid in messages)) - )) - - if not self.args.follow: - # timer will stop after that request - rospy.signal_shutdown("done") - - -def main(): - # NOTE: in this particular script we do not need any plugin topic (which uses mavros private namespace) - # And will use mavlink topics from global namespace (default "/"). You may define at run _ns:=NS - parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) - #parser.add_argument('-v', '--verbose', action='store_true', help="verbose output") - parser.add_argument('-f', '--follow', action='store_true', help="do not exit after first report (report each 15 sec).") - - args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) - - rospy.init_node("checkid", anonymous=True) - checker = Checker(args) - rospy.spin() - - -if __name__ == '__main__': - main() diff --git a/mavros/scripts/install_geographiclib_datasets.sh b/mavros/scripts/install_geographiclib_datasets.sh index b3fbadc3d..65a9238e0 100755 --- a/mavros/scripts/install_geographiclib_datasets.sh +++ b/mavros/scripts/install_geographiclib_datasets.sh @@ -21,7 +21,7 @@ run_get() { echo "Installing GeographicLib $tool $model" geographiclib-get-$tool $model >/dev/null 2>&1 - + files=$(shopt -s nullglob dotglob; echo /usr/share/GeographicLib/$dir/$model* /usr/local/share/GeographicLib/$dir/$model*) if (( ! ${#files} )); then echo "Error while installing GeographicLib $tool $model" diff --git a/mavros/scripts/mav b/mavros/scripts/mav new file mode 100755 index 000000000..5a6716cfd --- /dev/null +++ b/mavros/scripts/mav @@ -0,0 +1,14 @@ +#!/usr/bin/python3 + +if False: + import sys + + from pkg_resources import load_entry_point + + if __name__ == '__main__': + sys.exit(load_entry_point('mavros', 'console_scripts', 'mav')()) + +else: + from mavros.cmd import cli + + cli() diff --git a/mavros/scripts/mavcmd b/mavros/scripts/mavcmd deleted file mode 100755 index 4d8a81651..000000000 --- a/mavros/scripts/mavcmd +++ /dev/null @@ -1,287 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# vim:set ts=4 sw=4 et: -# -# Copyright 2014,2015 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -from __future__ import print_function - -import argparse -import threading - -import rospy -import mavros -from mavros.utils import * -from mavros import command -from sensor_msgs.msg import NavSatFix - - -def _check_ret(args, ret): - if not ret.success: - fault("Request failed. Check mavros logs. ACK:", ret.result) - - print_if(args.verbose, "Command ACK:", ret.result) - - -def do_long(args): - try: - ret = command.long( - broadcast=args.broadcast, - command=args.command, confirmation=int(args.confirmation), - param1=args.param1, - param2=args.param2, - param3=args.param3, - param4=args.param4, - param5=args.param5, - param6=args.param6, - param7=args.param7) - except rospy.ServiceException as ex: - fault(ex) - - _check_ret(args, ret) - - -def do_int(args): - try: - ret = command.int(frame=args.frame, command=args.command, - current=int(args.current), - autocontinue=int(args.autocontinue), - param1=args.param1, - param2=args.param2, - param3=args.param3, - param4=args.param4, - x=args.x, - y=args.y, - z=args.z) - except rospy.ServiceException as ex: - fault(ex) - - if not ret.success: - fault("Request failed. Check mavros logs.") - - print_if(args.verbose, "Request done.") - - -def do_set_home(args): - try: - ret = command.set_home(current_gps=args.current_gps, - latitude=args.latitude, - longitude=args.longitude, - altitude=args.altitude) - except rospy.ServiceException as ex: - fault(ex) - - _check_ret(args, ret) - - -def do_takeoff(args): - try: - ret = command.takeoff(min_pitch=args.min_pitch, - yaw=args.yaw, - latitude=args.latitude, - longitude=args.longitude, - altitude=args.altitude) - except rospy.ServiceException as ex: - fault(ex) - - _check_ret(args, ret) - - -def do_land(args): - try: - ret = command.land(min_pitch=0.0, - yaw=args.yaw, - latitude=args.latitude, - longitude=args.longitude, - altitude=args.altitude) - except rospy.ServiceException as ex: - fault(ex) - - _check_ret(args, ret) - - -def _find_gps_topic(args, op_name): - # XXX: since 0.13 global position always exists. need redo that. - global_fix = mavros.get_topic('global_position', 'global') - gps_fix = mavros.get_topic('global_position', 'raw', 'fix') - - topics = rospy.get_published_topics() - # need find more elegant way - if len([topic for topic, type_ in topics if topic == global_fix]): - return global_fix - elif len([topic for topic, type_ in topics if topic == gps_fix]): - print_if(args.verbose, "Use GPS_RAW_INT data!") - return gps_fix - elif args.any_gps: - t = [topic for topic, type_ in topics if type_ == 'sensor_msgs/NavSatFix'] - if len(t) > 0: - print("Use", t[0], "NavSatFix topic for", op_name) - return t[0] - - return None - - -def do_takeoff_cur_gps(args): - done_evt = threading.Event() - def fix_cb(fix): - print("Taking-off from current coord: Lat:", fix.latitude, - "Long:", fix.longitude) - print_if(args.verbose, "With desired Altitude:", args.altitude, - "Yaw:", args.yaw, "Pitch angle:", args.min_pitch) - - try: - ret = command.takeoff(min_pitch=args.min_pitch, - yaw=args.yaw, - latitude=fix.latitude, - longitude=fix.longitude, - altitude=args.altitude) - except rospy.ServiceException as ex: - fault(ex) - - _check_ret(args, ret) - done_evt.set() - - topic = _find_gps_topic(args, "takeoff") - if topic is None: - fault("NavSatFix topic not exist") - - sub = rospy.Subscriber(topic, NavSatFix, fix_cb) - if not done_evt.wait(10.0): - fault("Something went wrong. Topic timed out.") - - -def do_land_cur_gps(args): - done_evt = threading.Event() - def fix_cb(fix): - print("Landing on current coord: Lat:", fix.latitude, - "Long:", fix.longitude) - print_if(args.verbose, "With desired Altitude:", args.altitude, - "Yaw:", args.yaw) - - try: - ret = command.land(min_pitch=0.0, - yaw=args.yaw, - latitude=fix.latitude, - longitude=fix.longitude, - altitude=args.altitude) - except rospy.ServiceException as ex: - fault(ex) - - _check_ret(args, ret) - done_evt.set() - - topic = _find_gps_topic(args, "landing") - if topic is None: - fault("NavSatFix topic not exist") - - sub = rospy.Subscriber(topic, NavSatFix, fix_cb) - if not done_evt.wait(10.0): - fault("Something went wrong. Topic timed out.") - - -def do_trigger_control(args): - try: - ret = command.trigger_control(trigger_enable=args.trigger_enable, - cycle_time=args.cycle_time) - except rospy.ServiceException as ex: - fault(ex) - - _check_ret(args, ret) - - -def main(): - parser = argparse.ArgumentParser(description="Command line tool for sending commands to MAVLink device.") - parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) - parser.add_argument('-v', '--verbose', action='store_true', help="Verbose output") - parser.add_argument('--wait', action='store_true', help="Wait for establishing FCU connection") - subarg = parser.add_subparsers() - - long_args = subarg.add_parser('long', help="Send any command (COMMAND_LONG)") - long_args.set_defaults(func=do_long) - long_args.add_argument('-c', '--confirmation', action='store_true', help="Require confirmation") - long_args.add_argument('-b', '--broadcast', action='store_true', help="Broadcast command") - long_args.add_argument('command', type=int, help="Command Code") - long_args.add_argument('param1', type=float, help="param1") - long_args.add_argument('param2', type=float, help="param2") - long_args.add_argument('param3', type=float, help="param3") - long_args.add_argument('param4', type=float, help="param4") - long_args.add_argument('param5', type=float, help="param5 / x_lat") - long_args.add_argument('param6', type=float, help="param6 / y_long") - long_args.add_argument('param7', type=float, help="param7 / z_alt") - - int_args = subarg.add_parser('int', help="Send any command (COMMAND_INT)") - int_args.set_defaults(func=do_int) - int_args.add_argument('-c', '--current', action='store_true', help="Is current?") - int_args.add_argument('-a', '--autocontinue', action='store_true', help="Is autocontinue?") - int_args.add_argument('-f', '--frame', type=int, default=3, help="Frame Code (default: %(default)s)") - int_args.add_argument('command', type=int, help="Command Code") - int_args.add_argument('param1', type=float, help="param1") - int_args.add_argument('param2', type=float, help="param2") - int_args.add_argument('param3', type=float, help="param3") - int_args.add_argument('param4', type=float, help="param4") - int_args.add_argument('x', type=int, help="Latitude in deg*1E7 or X*1E4 m") - int_args.add_argument('y', type=int, help="Longitude in deg*1E7 or Y*1E4 m") - int_args.add_argument('z', type=float, help="Altitude in m, depending on frame") - - # Note: arming service provided by mavsafety - - set_home_args = subarg.add_parser('sethome', help="Request change home position") - set_home_args.set_defaults(func=do_set_home) - set_home_args.add_argument('-c', '--current-gps', action='store_true', - help="Use current GPS location (use 0 0 0 for location args)") - set_home_args.add_argument('latitude', type=float, help="Latitude") - set_home_args.add_argument('longitude', type=float, help="Longitude") - set_home_args.add_argument('altitude', type=float, help="Altitude") - - takeoff_args = subarg.add_parser('takeoff', help="Request takeoff") - takeoff_args.set_defaults(func=do_takeoff) - takeoff_args.add_argument('min_pitch', type=float, help="Min pitch") - takeoff_args.add_argument('yaw', type=float, help="Desired Yaw") - takeoff_args.add_argument('latitude', type=float, help="Latitude") - takeoff_args.add_argument('longitude', type=float, help="Longitude") - takeoff_args.add_argument('altitude', type=float, help="Altitude") - - land_args = subarg.add_parser('land', help="Request land") - land_args.set_defaults(func=do_land) - land_args.add_argument('yaw', type=float, help="Desired Yaw") - land_args.add_argument('latitude', type=float, help="Latitude") - land_args.add_argument('longitude', type=float, help="Longitude") - land_args.add_argument('altitude', type=float, help="Altitude") - - takeoff_cur_args = subarg.add_parser('takeoffcur', help="Request takeoff from current GPS coordinates") - takeoff_cur_args.set_defaults(func=do_takeoff_cur_gps) - takeoff_cur_args.add_argument('-a', '--any-gps', action="store_true", help="Try to find GPS topic (warn: could be dangerous!)") - takeoff_cur_args.add_argument('min_pitch', type=float, help="Min pitch") - takeoff_cur_args.add_argument('yaw', type=float, help="Desired Yaw") - takeoff_cur_args.add_argument('altitude', type=float, help="Altitude") - - land_cur_args = subarg.add_parser('landcur', help="Request land on current GPS coordinates") - land_cur_args.set_defaults(func=do_land_cur_gps) - land_cur_args.add_argument('-a', '--any-gps', action="store_true", help="Try to find GPS topic (warn: could be dangerous!)") - land_cur_args.add_argument('yaw', type=float, help="Desired Yaw") - land_cur_args.add_argument('altitude', type=float, help="Altitude") - - trigger_ctrl_args = subarg.add_parser('trigger_control', help="Control onboard camera triggering system (PX4)") - trigger_ctrl_args.set_defaults(func=do_trigger_control) - trigger_en_group = trigger_ctrl_args.add_mutually_exclusive_group() - trigger_en_group.add_argument('-e', '--enable', dest='trigger_enable', action='store_true', default=True, help="Enable camera trigger (default)") - trigger_en_group.add_argument('-d', '--disable', dest='trigger_enable', action='store_false', help="Disable camera trigger") - trigger_ctrl_args.add_argument('-c', '--cycle_time', default=0.0, type=float, required=False, help="Camera trigger cycle time. Zero to use current onboard value") - - args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) - - rospy.init_node("mavcmd", anonymous=True) - mavros.set_namespace(args.mavros_ns) - - if args.wait: - wait_fcu_connection() - - args.func(args) - - -if __name__ == '__main__': - main() diff --git a/mavros/scripts/mavftp b/mavros/scripts/mavftp deleted file mode 100755 index 57196f54f..000000000 --- a/mavros/scripts/mavftp +++ /dev/null @@ -1,296 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# vim:set ts=4 sw=4 et: -# -# Copyright 2014 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -from __future__ import print_function - -import argparse - -import os -import rospy -import mavros -from mavros.utils import * -from mavros.nuttx_crc32 import * -from mavros import ftp - -no_progressbar = False -try: - import progressbar as pbar -except ImportError: - print("Prigressbar disabled. install python-progressbar", file=sys.stderr) - no_progressbar = True - -# optimized transfer size for FTP message payload -# XXX: bug in ftp.cpp cause a doubling request of last package. -# -1 fixes that. -FTP_PAGE_SIZE = 239 * 18 - 1 -FTP_PWD_FILE = '/tmp/.mavftp_pwd' - - -def _resolve_path(path = None): - """ - Resolve FTP path using PWD file - """ - if os.path.exists(FTP_PWD_FILE): - with open(FTP_PWD_FILE, 'r') as fd: - pwd = fd.readline() - else: - # default home location is root directory - pwd = os.environ.get('MAVFTP_HOME', '/') - - if not path: - return os.path.normpath(pwd) # no path - PWD location - elif path.startswith('/'): - return os.path.normpath(path) # absolute path - else: - return os.path.normpath(os.path.join(pwd, path)) - - -class ProgressBar: - """ - Wrapper class for hiding file transfer brogressbar construction - """ - def __init__(self, quiet, operation, maxval): - if no_progressbar or quiet or maxval == 0: - print_if(maxval == 0, "Can't show progressbar for unknown file size", file=sys.stderr) - self.pbar = None - return - - self.pbar = pbar.ProgressBar( - widgets=[operation, pbar.Percentage(), ' ', pbar.Bar(), ' ', pbar.ETA(), ' ', pbar.FileTransferSpeed()], - maxval=maxval).start() - - def update(self, value): - if self.pbar: - self.pbar.update(value) - - def __enter__(self): - if self.pbar: - self.pbar.start() - - return self - - def __exit__(self, type, value, traceback): - if self.pbar: - self.pbar.finish() - - -def do_change_directory(args): - # TODO: check that path is exist - path = args.path or os.environ.get('MAVFTP_HOME') - if args.path and not path.startswith('/'): - path = _resolve_path(path) - if path and not path.startswith('/'): - fault("Path is not absolute:", path) - - if path: - with open(FTP_PWD_FILE, 'w') as fd: - fd.write(os.path.normpath(path)) - else: - if os.path.exists(FTP_PWD_FILE): - os.unlink(FTP_PWD_FILE) - - -def do_list(args): - args.path = _resolve_path(args.path) - for ent in ftp.listdir(args.path): - n = ent.name - if ent.type == ftp.FileEntry.TYPE_DIRECTORY: - n += '/' - else: - n += '\t{}'.format(ent.size) - - print(n) - - -def do_cat(args): - args.no_progressbar = True - args.no_verify = False - args.file = sys.stdout - do_download(args) - - -def do_remove(args): - args.path = _resolve_path(args.path) - ftp.unlink(args.path) - - -def do_reset(args): - ftp.reset_server() - - -def do_mkdir(args): - args.path = _resolve_path(args.path) - ftp.mkdir(args.path) - - -def do_rmdir(args): - args.path = _resolve_path(args.path) - ftp.rmdir(args.path) - - -def do_download(args): - local_crc = 0 - args.path = _resolve_path(args.path) - - if not args.file: - # if file argument is not set, use $PWD/basename - args.file = open(os.path.basename(args.path), 'wb') - - print_if(args.verbose, "Downloading from", args.path, "to", args.file.name, file=sys.stderr) - - with args.file as to_fd, \ - ftp.open(args.path, 'r') as from_fd, \ - ProgressBar(args.no_progressbar, "Downloading: ", from_fd.size) as bar: - while True: - buf = from_fd.read(FTP_PAGE_SIZE) - if len(buf) == 0: - break - - local_crc = nuttx_crc32(buf, local_crc) - to_fd.write(buf) - bar.update(from_fd.tell()) - - if not args.no_verify: - print_if(args.verbose, "Verifying...", file=sys.stderr) - remote_crc = ftp.checksum(args.path) - if local_crc != remote_crc: - fault("Verification failed: 0x{local_crc:08x} != 0x{remote_crc:08x}".format(**locals())) - - -def do_upload(args): - mode = 'cw' if args.no_overwrite else 'w' - local_crc = 0 - - if args.path: - args.path = _resolve_path(args.path) - else: - args.path = _resolve_path(os.path.basename(args.file.name)) - - print_if(args.verbose, "Uploading from", args.file.name, "to", args.path, file=sys.stderr) - - # for stdin it is 0 - from_size = os.fstat(args.file.fileno()).st_size - - with args.file as from_fd, \ - ftp.open(args.path, mode) as to_fd, \ - ProgressBar(args.no_progressbar, "Uploading: ", from_size) as bar: - while True: - buf = from_fd.read(FTP_PAGE_SIZE) - if len(buf) == 0: - break - - local_crc = nuttx_crc32(buf, local_crc) - to_fd.write(buf) - bar.update(to_fd.tell()) - - if not args.no_verify: - print_if(args.verbose, "Verifying...", file=sys.stderr) - remote_crc = ftp.checksum(args.path) - if local_crc != remote_crc: - fault("Verification failed: 0x{local_crc:08x} != 0x{remote_crc:08x}".format(**locals())) - - -def do_verify(args): - local_crc = 0 - - if args.path: - args.path = _resolve_path(args.path) - else: - args.path = _resolve_path(os.path.basename(args.file.name)) - - print_if(args.verbose, "Verifying", args.file.name, "and", args.path, file=sys.stderr) - - with args.file as fd: - while True: - buf = fd.read(4096 * 32) # use 128k block for CRC32 calculation - if len(buf) == 0: - break - - local_crc = nuttx_crc32(buf, local_crc) - - remote_crc = ftp.checksum(args.path) - - print_if(args.verbose, "CRC32 for local and remote files:") - print_if(args.verbose, "0x{local_crc:08x} {args.file.name}".format(**locals())) - print_if(args.verbose, "0x{remote_crc:08x} {args.path}".format(**locals())) - - if local_crc != remote_crc: - print("{args.file.name}: FAULT".format(**locals())) - sys.exit(1) - else: - print("{args.file.name}: OK".format(**locals())) - - -def main(): - parser = argparse.ArgumentParser(description="File manipulation tool for MAVLink-FTP.") - parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) - parser.add_argument('-v', '--verbose', action='store_true', help="verbose output") - subarg = parser.add_subparsers() - - # argparse from python2 don't support subparser aliases - # there exist a hack, but i don't want it now. - - cd_args = subarg.add_parser('cd', help="change directory") - cd_args.set_defaults(func=do_change_directory) - cd_args.add_argument('path', type=str, nargs='?', help="directory path") - - list_args = subarg.add_parser('list', help="list files and dirs") - list_args.set_defaults(func=do_list) - list_args.add_argument('path', type=str, nargs='?', help="directory path") - - cat_args = subarg.add_parser('cat', help="cat file") - cat_args.set_defaults(func=do_cat) - cat_args.add_argument('path', type=str, help="file path") - - remove_args = subarg.add_parser('remove', help="remove file") - remove_args.set_defaults(func=do_remove) - remove_args.add_argument('path', type=str, help="file path") - - mkdir_args = subarg.add_parser('mkdir', help="create direcotory") - mkdir_args.set_defaults(func=do_mkdir) - # mkdir_args.add_argument('-p', action='store_true', help="dir path") - mkdir_args.add_argument('path', type=str, help="dir path") - - rmdir_args = subarg.add_parser('rmdir', help="remove directory") - rmdir_args.set_defaults(func=do_rmdir) - rmdir_args.add_argument('path', type=str, help="dir path") - - download_args = subarg.add_parser('download', help="download file") - download_args.set_defaults(func=do_download) - download_args.add_argument('path', type=str, help="file to send") - download_args.add_argument('file', type=argparse.FileType('wb'), nargs='?', help="save path") - download_args.add_argument('-q', '--no-progressbar', action="store_true", help="do not show progressbar") - download_args.add_argument('--no-verify', action="store_true", help="do not perform verify step") - - upload_args = subarg.add_parser('upload', help="upload file") - upload_args.set_defaults(func=do_upload) - upload_args.add_argument('file', type=argparse.FileType('rb'), help="file to send") - upload_args.add_argument('path', type=str, nargs='?', help="save path") - upload_args.add_argument('-n', '--no-overwrite', action="store_true", help="do not overwrite existing file") - upload_args.add_argument('-q', '--no-progressbar', action="store_true", help="do not show progressbar") - upload_args.add_argument('--no-verify', action="store_true", help="do not perform verify step") - - verify_args = subarg.add_parser('verify', help="verify files") - verify_args.set_defaults(func=do_verify) - verify_args.add_argument('file', type=argparse.FileType('rb'), help="local file") - verify_args.add_argument('path', type=str, nargs='?', help="remote file") - - reset_args = subarg.add_parser('reset', help="reset") - reset_args.set_defaults(func=do_reset) - - args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) - - rospy.init_node("mavftp", anonymous=True) - mavros.set_namespace(args.mavros_ns) - args.func(args) - - -if __name__ == '__main__': - main() diff --git a/mavros/scripts/mavparam b/mavros/scripts/mavparam deleted file mode 100755 index bff5bc0e0..000000000 --- a/mavros/scripts/mavparam +++ /dev/null @@ -1,110 +0,0 @@ -#!/usr/bin/env python -# vim:set ts=4 sw=4 et: -# -# Copyright 2014 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -import argparse - -import rospy -import mavros -from mavros.utils import * -from mavros.param import * - - -def get_param_file_io(args): - if args.mission_planner: - print_if(args.verbose, "MissionPlanner format") - return MissionPlannerParam(args) - - elif args.qgroundcontrol: - print_if(args.verbose, "QGroundControl format") - return QGroundControlParam(args) - - elif args.mavproxy: - print_if(args.verbose, "MavProxy format") - return MavProxyParam(args) - - else: - if args.file.name.endswith('.txt'): - print_if(args.verbose, "Suggestion: QGroundControl format") - return QGroundControlParam(args) - else: - print_if(args.verbose, "Suggestion: MissionPlanner format") - return MissionPlannerParam(args) - - -def do_load(args): - param_file = get_param_file_io(args) - with args.file: - param_transfered = param_set_list(param_file.read(args.file)) - - print_if(args.verbose, "Parameters transfered:", param_transfered) - - -def do_dump(args): - param_received, param_list = param_get_all(args.force) - print_if(args.verbose, "Parameters received:", param_received) - - param_file = get_param_file_io(args) - with args.file: - param_file.write(args.file, param_list) - - -def do_get(args): - print(param_get(args.param_id)) - - -def do_set(args): - if '.' in args.value: - val = float(args.value) - else: - val = int(args.value) - - print(param_set(args.param_id, val)) - - -def main(): - parser = argparse.ArgumentParser(description="Command line tool for getting and setting parameters from MAVLink device.") - parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) - parser.add_argument('-v', '--verbose', action='store_true', help="Verbose output") - subarg = parser.add_subparsers() - - load_args = subarg.add_parser('load', help="Load parameters from file") - load_args.set_defaults(func=do_load) - load_args.add_argument('file', type=argparse.FileType('r'), help="Input file") - load_format = load_args.add_mutually_exclusive_group() - load_format.add_argument('-mp', '--mission-planner', action="store_true", help="Select MissionPlanner param file format") - load_format.add_argument('-qgc', '--qgroundcontrol', action="store_true", help="Select QGroundControl param file format") - load_format.add_argument('-mavpx', '--mavproxy', action="store_true", help="Select MavProxy param file format") - - dump_args = subarg.add_parser('dump', help="Dump parameters to file") - dump_args.set_defaults(func=do_dump) - dump_args.add_argument('file', type=argparse.FileType('w'), help="Output file") - dump_args.add_argument('-f', '--force', action="store_true", help="Force pull params from FCU, not cache") - dump_format = dump_args.add_mutually_exclusive_group() - dump_format.add_argument('-mp', '--mission-planner', action="store_true", help="Select MissionPlanner param file format") - dump_format.add_argument('-qgc', '--qgroundcontrol', action="store_true", help="Select QGroundControl param file format") - dump_format.add_argument('-mavpx', '--mavproxy', action="store_true", help="Select MavProxy param file format") - - get_args = subarg.add_parser('get', help="Get parameter") - get_args.set_defaults(func=do_get) - get_args.add_argument('param_id', help="Parameter ID string") - - set_args = subarg.add_parser('set', help="Set parameter") - set_args.set_defaults(func=do_set) - set_args.add_argument('param_id', help="Parameter ID string") - set_args.add_argument('value', help="New value") - - args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) - - rospy.init_node("mavparam", anonymous=True) - mavros.set_namespace(args.mavros_ns) - args.func(args) - - -if __name__ == '__main__': - main() diff --git a/mavros/scripts/mavsafety b/mavros/scripts/mavsafety deleted file mode 100755 index 2f46ad36b..000000000 --- a/mavros/scripts/mavsafety +++ /dev/null @@ -1,108 +0,0 @@ -#!/usr/bin/env python -# vim:set ts=4 sw=4 et: -# -# Copyright 2014 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -from __future__ import print_function - -import argparse - -import rospy -import mavros -from mavros.utils import * -from geometry_msgs.msg import PolygonStamped, Point32 -from mavros import command - - -def _arm(args, state): - try: - ret = command.arming(value=state) - except rospy.ServiceException as ex: - fault(ex) - - if not ret.success: - fault("Request failed. Check mavros logs") - - print_if(args.verbose, "Command result:", ret.result) - return ret - - -def do_arm(args): - _arm(args, True) - - -def do_disarm(args): - _arm(args, False) - -def do_kill(args): - try: - ret = command.long(command=400, param2=21196) - except rospy.ServiceException as ex: - fault(ex) - - if not ret.success: - fault("Request failed. Check mavros logs") - - print_if(args.verbose, "Command result:", ret.result) - return ret - -_ONCE_DELAY = 3 -def do_safetyarea(args): - set_topic = mavros.get_topic('safety_area', 'set') - pub = rospy.Publisher(set_topic, PolygonStamped, - queue_size=10, latch=True) - - poly = PolygonStamped() - poly.header.frame_id = 'mavsafety' - poly.header.stamp = rospy.get_rostime() - poly.polygon.points = [ - Point32(x=args.p1[0], y=args.p1[1], z=args.p1[2]), - Point32(x=args.p2[0], y=args.p2[1], z=args.p2[2]), - ] - - pub.publish(poly) - # XXX maybe not worked - print_if(pub.get_num_connections() < 1, - "Mavros not started, nobody subcsribes to ", set_topic) - - # stick around long enough for others to grab - timeout_t = rospy.get_time() + _ONCE_DELAY - while not rospy.is_shutdown() and rospy.get_time() < timeout_t: - rospy.sleep(0.2) - - -def main(): - parser = argparse.ArgumentParser(description="Command line tool for manipulating safety on MAVLink device.") - parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) - parser.add_argument('-v', '--verbose', action='store_true', help="Verbose output") - subarg = parser.add_subparsers() - - arm_args = subarg.add_parser('arm', help="Arm motors") - arm_args.set_defaults(func=do_arm) - - disarm_args = subarg.add_parser('disarm', help="Disarm motors") - disarm_args.set_defaults(func=do_disarm) - - kill_args = subarg.add_parser('kill', help="Kill motors") - kill_args.set_defaults(func=do_kill) - - safety_area_args = subarg.add_parser('safetyarea', help="Send safety area") - safety_area_args.set_defaults(func=do_safetyarea) - safety_area_args.add_argument('-p1', type=float, nargs=3, metavar=('x', 'y', 'z'), - required=True, help="Corner 1") - safety_area_args.add_argument('-p2', type=float, nargs=3, metavar=('x', 'y', 'z'), - required=True, help="Corner 2") - - args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) - - rospy.init_node("mavsafety", anonymous=True) - mavros.set_namespace(args.mavros_ns) - args.func(args) - - -if __name__ == '__main__': - main() diff --git a/mavros/scripts/mavsetp b/mavros/scripts/mavsetp deleted file mode 100755 index 57ce352d6..000000000 --- a/mavros/scripts/mavsetp +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python -# vim:set ts=4 sw=4 et: -# -# Copyright 2014,2016 Nuno Marques, Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -from __future__ import print_function - -import argparse -import math - -import rospy -import mavros - -from mavros import command, setpoint as sp -from mavros.utils import * -from mavros_msgs.srv import SetMode -from tf.transformations import quaternion_from_euler - -_ONCE_DELAY = 3 - - -def publish_once(args, pub, msg): - pub.publish(msg) - rospy.sleep(0.2) - enable_offboard() - - # stick around long enough for others to grab - timeout_t = rospy.get_time() + _ONCE_DELAY - while not rospy.is_shutdown() and rospy.get_time() < timeout_t: - rospy.sleep(0.2) - print_if(pub.get_num_connections() < 1, - "Mavros not started, nobody subcsribes to", pub.name) - - -def enable_offboard(): - print("Requesting OFFBOARD mode...") - - try: - set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode) - ret = set_mode(custom_mode="OFFBOARD") - except rospy.ServiceException as ex: - fault(ex) - - if not ret.mode_sent: - print("Request failed. Check mavros logs") - else: - print("OFFBOARD mode are set.") - - -def do_local_pos(args): - pub = sp.get_pub_position_local(queue_size=10, latch=True) - - pos = sp.PoseStamped(header=sp.Header(frame_id='mavsetp', stamp=rospy.get_rostime())) - pos.pose.position = sp.Point(x=args.position[0], y=args.position[1], z=args.position[2]) - - yaw = math.radians(args.position[3]) if args.degrees else args.position[3] - q = quaternion_from_euler(0, 0, yaw) - pos.pose.orientation = sp.Quaternion(*q) - - publish_once(args, pub, pos) - - -def do_local_vel(args): - pub = sp.get_pub_velocity_cmd_vel(queue_size=10, latch=True) - - vel = sp.TwistStamped(header=sp.Header(frame_id='mavsetp', stamp=rospy.get_rostime())) - vel.twist.linear = sp.Vector3(x=args.velocity[0], y=args.velocity[1], z=args.velocity[2]) - vel.twist.angular = sp.Vector3(z=args.velocity[3]) - - publish_once(args, pub, vel) - - -def do_local_accel(args): - pub = sp.get_pub_accel_accel(queue_size=10, latch=True) - - vel = sp.Vector3Stamped(header=sp.Header(frame_id='mavsetp', stamp=rospy.get_rostime())) - vel.vector = sp.Vector3(x=args.acceleration[0], y=args.acceleration[1], z=args.acceleration[2]) - - publish_once(args, pub, vel) - - -def do_local_selector(args): - if args.position is not None: - do_local_pos(args) - elif args.velocity is not None: - do_local_vel(args) - else: - do_local_accel(args) - - -def main(): - parser = argparse.ArgumentParser(description="Command line tool for controlling the device by setpoints.") - parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) - parser.add_argument('-V', '--verbose', action='store_true', help="Verbose output") - subarg = parser.add_subparsers() - - local_args = subarg.add_parser('local', help="Send local setpoint") - local_args.add_argument('-d', '--degrees', action='store_true', help="Yaw angle in degrees") - local_args.set_defaults(func=do_local_selector) - local_group = local_args.add_mutually_exclusive_group(required=True) - - local_group.add_argument('-p', '--position', type=float, nargs=4, - metavar=('x', 'y', 'z', 'y_ang'), help="Local position & desired yaw angle") - - local_group.add_argument('-v', '--velocity', type=float, nargs=4, - metavar=('vx', 'vy', 'vz', 'y_rate'), help="Linear velocity & desired yaw rate") - - local_group.add_argument('-a', '--acceleration', type=float, nargs=3, - metavar=('afx', 'afy', 'afz'), help="Linear acceleration/force") - - args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) - - rospy.init_node("mavsetp", anonymous=True) - mavros.set_namespace(args.mavros_ns) - args.func(args) - - -if __name__ == '__main__': - main() diff --git a/mavros/scripts/mavsys b/mavros/scripts/mavsys deleted file mode 100755 index ab8f8b9e3..000000000 --- a/mavros/scripts/mavsys +++ /dev/null @@ -1,143 +0,0 @@ -#!/usr/bin/env python -# vim:set ts=4 sw=4 et: -# -# Copyright 2014 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -from __future__ import print_function - -import argparse -import threading - -import rospy -import mavros -from mavros.utils import * -from mavros_msgs.msg import State -from mavros_msgs.srv import SetMode, StreamRate, StreamRateRequest, MessageInterval - -def do_mode(args): - base_mode = 0 - custom_mode = '' - - if args.custom_mode is not None: - custom_mode = args.custom_mode.upper() - if args.base_mode is not None: - base_mode = args.base_mode - - done_evt = threading.Event() - def state_cb(state): - print_if(args.verbose, "Current mode:", state.mode) - if state.mode == custom_mode: - print("Mode changed.") - done_evt.set() - - if custom_mode != '' and not custom_mode.isdigit(): - # with correct custom mode we can wait until it changes - sub = rospy.Subscriber(mavros.get_topic('state'), State, state_cb) - else: - done_evt.set() - - try: - set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode) - ret = set_mode(base_mode=base_mode, custom_mode=custom_mode) - except rospy.ServiceException as ex: - fault(ex) - - if not ret.mode_sent: - fault("Request failed. Check mavros logs") - - if not done_evt.wait(5): - fault("Timed out!") - - - -def do_rate(args): - set_rate = rospy.ServiceProxy(mavros.get_topic('set_stream_rate'), StreamRate) - def _test_set_rate(rate_arg, id): - if rate_arg is not None: - try: - set_rate(stream_id=id, message_rate=rate_arg, on_off=(rate_arg != 0)) - except rospy.ServiceException as ex: - fault(ex) - - _test_set_rate(args.all, StreamRateRequest.STREAM_ALL) - _test_set_rate(args.raw_sensors, StreamRateRequest.STREAM_RAW_SENSORS) - _test_set_rate(args.ext_status, StreamRateRequest.STREAM_EXTENDED_STATUS) - _test_set_rate(args.rc_channels, StreamRateRequest.STREAM_RC_CHANNELS) - _test_set_rate(args.raw_controller, StreamRateRequest.STREAM_RAW_CONTROLLER) - _test_set_rate(args.position, StreamRateRequest.STREAM_POSITION) - _test_set_rate(args.extra1, StreamRateRequest.STREAM_EXTRA1) - _test_set_rate(args.extra2, StreamRateRequest.STREAM_EXTRA2) - _test_set_rate(args.extra3, StreamRateRequest.STREAM_EXTRA3) - - if args.stream_id is not None: - _test_set_rate(args.stream_id[1], args.stream_id[0]) - - - -def do_message_interval(args): - - if args.id is None: - fault("id not specified") - return - - if args.rate is None: - fault("rate not specified") - return - - try: - set_message_interval = rospy.ServiceProxy(mavros.get_topic('set_message_interval'), MessageInterval) - set_message_interval(message_id=args.id, message_rate=args.rate) - except rospy.ServiceException as ex: - fault(ex) - - - -def main(): - parser = argparse.ArgumentParser(description="Change mode and rate on MAVLink device.") - parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) - parser.add_argument('-v', '--verbose', action='store_true', help="verbose output") - parser.add_argument('--wait', action='store_true', help="Wait for establishing FCU connection") - subarg = parser.add_subparsers() - - mode_args = subarg.add_parser('mode', help="Set mode", formatter_class=argparse.RawTextHelpFormatter) - mode_args.set_defaults(func=do_mode) - mode_group = mode_args.add_mutually_exclusive_group(required=True) - mode_group.add_argument('-b', '--base-mode', type=int, help="Base mode code") - mode_group.add_argument('-c', '--custom-mode', type=str, help="Custom mode string (same as in ~/state topic)\ - \n\nNOTE: For PX4 Pro AUTO.LOITER, disable RC failsafe, which can be done by setting 'NAV_RCL_ACT' parameter to 0.") - - rate_args = subarg.add_parser('rate', help="Set stream rate") - rate_args.set_defaults(func=do_rate) - rate_args.add_argument('--all', type=int, metavar='rate', help="All streams") - rate_args.add_argument('--raw-sensors', type=int, metavar='rate', help="raw sensors stream") - rate_args.add_argument('--ext-status', type=int, metavar='rate', help="extended status stream") - rate_args.add_argument('--rc-channels', type=int, metavar='rate', help="RC channels stream") - rate_args.add_argument('--raw-controller', type=int, metavar='rate', help="raw conptoller stream") - rate_args.add_argument('--position', type=int, metavar='rate', help="position stream") - rate_args.add_argument('--extra1', type=int, metavar='rate', help="Extra 1 stream") - rate_args.add_argument('--extra2', type=int, metavar='rate', help="Extra 2 stream") - rate_args.add_argument('--extra3', type=int, metavar='rate', help="Extra 3 stream") - rate_args.add_argument('--stream-id', type=int, nargs=2, metavar=('id', 'rate'), help="any stream") - - message_interval_args = subarg.add_parser('message_interval', help="Set message interval") - message_interval_args.set_defaults(func=do_message_interval) - message_interval_args.add_argument('--id', type=int, metavar='id', help="message id") - message_interval_args.add_argument('--rate', type=float, metavar='rate', help="message rate") - - args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) - - rospy.init_node("mavsys", anonymous=True) - mavros.set_namespace(args.mavros_ns) - - if args.wait: - wait_fcu_connection() - - args.func(args) - - -if __name__ == '__main__': - main() diff --git a/mavros/scripts/mavwp b/mavros/scripts/mavwp deleted file mode 100755 index 9f39e7d95..000000000 --- a/mavros/scripts/mavwp +++ /dev/null @@ -1,223 +0,0 @@ -#!/usr/bin/env python -# vim:set ts=4 sw=4 et: -# -# Copyright 2014 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -from __future__ import print_function - -import argparse -import threading - -import rospy -import mavros -from mavros.utils import * -from mavros import mission as M - -no_prettytable = False -try: - from prettytable import PrettyTable -except ImportError: - print("Waring: 'show' action disabled. install python-prettytable", file=sys.stderr) - no_prettytable = True - - -def get_wp_file_io(args): - return M.QGroundControlWP() - - -def _pull(args): - try: - ret = M.pull() - except rospy.ServiceException as ex: - fault(ex) - - if not ret.success: - fault("Request failed. Check mavros logs") - - print_if(args.verbose, "Waypoints received:", ret.wp_received) - return ret - - -def do_pull(args): - _pull(args) - - -def do_show(args): - str_bool = lambda x: 'Yes' if x else 'No' - str_frame = lambda f: M.FRAMES.get(f, 'UNK') + ' ({})'.format(f) - str_command = lambda c: M.NAV_CMDS.get(c, 'UNK') + ' ({})'.format(c) - - done_evt = threading.Event() - def _show_table(topic): - pt = PrettyTable(('#', 'Curr', 'Auto', - 'Frame', 'Command', - 'P1', 'P2', 'P3', 'P4', - 'X Lat', 'Y Long', 'Z Alt')) - - for seq, w in enumerate(topic.waypoints): - pt.add_row(( - seq, - str_bool(w.is_current), - str_bool(w.autocontinue), - str_frame(w.frame), - str_command(w.command), - w.param1, - w.param2, - w.param3, - w.param4, - w.x_lat, - w.y_long, - w.z_alt - )) - - print(pt, file=sys.stdout) - sys.stdout.flush() - done_evt.set() - - if args.pull: - _pull(args) - - # Waypoints topic is latched type, and it updates after pull - sub = M.subscribe_waypoints(_show_table) - if args.follow: - rospy.spin() - elif not done_evt.wait(30.0): - fault("Something went wrong. Topic timed out.") - - -def do_load(args): - wps = [] - wps_file = get_wp_file_io(args) - with args.file: - wps = [w for w in wps_file.read(args.file)] - - def _load_call(start, waypoint_list): - try: - ret = M.push(start_index=start, waypoints=waypoint_list) - except rospy.ServiceException as ex: - fault(ex) - - if not ret.success: - fault("Request failed. Check mavros logs") - - print_if(args.verbose, "Waypoints transfered:", ret.wp_transfered) - - done_evt = threading.Event() - def _fix_wp0(topic): - if len(topic.waypoints) > 0: - wps[0] = topic.waypoints[0] - print_if(args.verbose, "HOME location: latitude:", wps[0].x_lat, - "longitude:", wps[0].y_long, "altitude:", wps[0].z_alt) - else: - print("Failed to get WP0! WP0 will be loaded from file.", file=sys.stderr) - - done_evt.set() - - if args.start_index > 0: - # Push partial - if args.start_index == args.end_index: - args.end_index += 1 - - end_index = args.end_index or len(wps) - _load_call(args.start_index, wps[args.start_index:end_index]) - else: - # Full push - if not args.preserve_home: - _load_call(0, wps) - else: - # Note: _load_call() emit publish on this topic, so callback only changes - # waypoint 0, and signal done event. - sub = M.subscribe_waypoints(_fix_wp0) - if not done_evt.wait(30.0): - fault("Something went wrong. Topic timed out.") - else: - sub.unregister() - _load_call(0, wps) - - -def do_dump(args): - done_evt = threading.Event() - def _write_file(topic): - wps_file = get_wp_file_io(args) - with args.file: - wps_file.write(args.file, topic.waypoints) - done_evt.set() - - # Waypoints topic is latched type, and it updates after pull - _pull(args) - sub = M.subscribe_waypoints(_write_file) - if not done_evt.wait(30.0): - fault("Something went wrong. Topic timed out.") - - -def do_clear(args): - try: - ret = M.clear() - except rospy.ServiceException as ex: - fault(ex) - - if not ret.success: - fault("Request failed, Check mavros logs") - else: - print_if(args.verbose, "Waypoints cleared") - - -def do_set_current(args): - try: - ret = M.set_current(wp_seq=args.seq) - except rospy.ServiceException as ex: - fault(ex) - - if not ret.success: - fault("Request failed, Check mavros logs") - else: - print_if(args.verbose, "Set current done.") - - -def main(): - parser = argparse.ArgumentParser(description="Command line tool for manipulating missions on MAVLink device.") - parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) - parser.add_argument('-v', '--verbose', action='store_true', help="Verbose output") - subarg = parser.add_subparsers() - - if not no_prettytable: - show_args = subarg.add_parser('show', help="Show waypoints") - show_args.add_argument('-f', '--follow', action='store_true', help="Watch and show new data") - show_args.add_argument('-p', '--pull', action='store_true', help="Pull waypoints from FCU before show") - show_args.set_defaults(func=do_show) - - load_args = subarg.add_parser('load', help="Load waypoints from file") - load_args.set_defaults(func=do_load) - load_args.add_argument('-p', '--preserve-home', action='store_true', help="Preserve home location (WP 0, APM only)") - load_args.add_argument('-s', '--start-index', type=int, default=0, help="Waypoint start index for partial updating (APM only)") - load_args.add_argument('-e', '--end-index', type=int, default=0, help="Waypoint end index for partial updating (APM only, default: last element in waypoint list)") - load_args.add_argument('file', type=argparse.FileType('r'), help="Input file (QGC/MP format)") - - pull_args = subarg.add_parser('pull', help="Pull waypoints from FCU") - pull_args.set_defaults(func=do_pull) - - dump_args = subarg.add_parser('dump', help="Dump waypoints to file") - dump_args.set_defaults(func=do_dump) - dump_args.add_argument('file', type=argparse.FileType('w'), help="Output file (QGC format)") - - clear_args = subarg.add_parser('clear', help="Clear waypoints on device") - clear_args.set_defaults(func=do_clear) - - setcur_args = subarg.add_parser('setcur', help="Set current waypoints on device") - setcur_args.add_argument('seq', type=int, help="Waypoint seq id") - setcur_args.set_defaults(func=do_set_current) - - args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) - - rospy.init_node("mavwp", anonymous=True) - mavros.set_namespace(args.mavros_ns) - - args.func(args) - - -if __name__ == '__main__': - main() diff --git a/mavros/setup.cfg b/mavros/setup.cfg new file mode 100644 index 000000000..f332eeeeb --- /dev/null +++ b/mavros/setup.cfg @@ -0,0 +1,30 @@ +[metadata] +name = 'mavros' +description = 'Helper scripts and module for MAVROS' +license = 'Triple licensed under GPLv3, LGPLv3 and BSD' +author = 'Vladimir Ermakov' +author-email = 'vooon341@gmail.com' +maintainer = 'Vladimir Ermakov' +maintainer_email = 'vooon341@gmail.com' +keywords = 'ROS' +classifiers = + 'Intended Audience :: Developers' + 'License :: OSI Approved :: GNU General Public License v3 (GPLv3)' + 'License :: OSI Approved :: GNU Lesser General Public License v3 (LGPLv3)' + 'License :: OSI Approved :: BSD License' + 'Programming Language :: Python' + 'Topic :: Software Development' +tests_require = 'pytest' + +[options.entry_points] +console_scripts = + mav=mavros.cmd:cli + +[develop] +script-dir=$base/lib/mavros + +[install] +install-scripts=$base/lib/mavros + +[yapf] +blank_line_before_nested_class_or_def = True diff --git a/mavros/setup.py b/mavros/setup.py index dc8f1556a..b17bf0727 100644 --- a/mavros/setup.py +++ b/mavros/setup.py @@ -1,11 +1,38 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +from setuptools import setup -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +package_name = "mavros" -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['mavros'], - package_dir={'': 'src'}) - -setup(**setup_args) +setup( + name=package_name, + version="1.6.0", + packages=[package_name], + # package_dir={'': 'src'}, + # data_files=[ + # ('share/ament_index/resource_index/packages', + # ['resource/' + package_name]), + # ('share/' + package_name, ['package.xml']), + # ], + install_requires=["setuptools"], + zip_safe=True, + author="Vladimir Ermakov", + author_email="vooon341@gmail.com", + maintainer="Vladimir Ermakov", + maintainer_email="vooon341@gmail.com", + keywords=["ROS"], + classifiers=[ + "Intended Audience :: Developers", + "License :: OSI Approved :: GNU General Public License v3 (GPLv3)", + "License :: OSI Approved :: GNU Lesser General Public License v3 (LGPLv3)", + "License :: OSI Approved :: BSD License", + "Programming Language :: Python", + "Topic :: Software Development", + ], + description="Helper scripts and module for MAVROS", + license="Triple licensed under GPLv3, LGPLv3 and BSD", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "mav = mavros.cmd:cli", + ], + }, +) diff --git a/mavros/src/gcs_bridge.cpp b/mavros/src/gcs_bridge.cpp deleted file mode 100644 index 57eb98420..000000000 --- a/mavros/src/gcs_bridge.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/** - * @brief MAVROS GCS proxy - * @file gcs_bridge.cpp - * @author Vladimir Ermakov - */ -/* - * Copyright 2014,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include -#include - -#include -#include -#include - -using namespace mavros; -using namespace mavconn; - -ros::Publisher mavlink_pub; -ros::Subscriber mavlink_sub; -MAVConnInterface::Ptr gcs_link; - - -void mavlink_pub_cb(const mavlink::mavlink_message_t *mmsg, const mavconn::Framing framing) -{ - auto rmsg = boost::make_shared(); - - rmsg->header.stamp = ros::Time::now(); - mavros_msgs::mavlink::convert(*mmsg, *rmsg, mavros::utils::enum_value(framing)); - mavlink_pub.publish(rmsg); -} - -void mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg) -{ - mavlink::mavlink_message_t mmsg; - - if (mavros_msgs::mavlink::convert(*rmsg, mmsg)) - gcs_link->send_message_ignore_drop(&mmsg); - else - ROS_ERROR("Packet drop: convert error."); -} - -int main(int argc, char *argv[]) -{ - ros::init(argc, argv, "gcs_bridge"); - ros::NodeHandle priv_nh("~"); - ros::NodeHandle mavlink_nh("mavlink"); - diagnostic_updater::Updater updater; - mavros::MavlinkDiag gcs_link_diag("GCS bridge"); - - std::string gcs_url; - priv_nh.param("gcs_url", gcs_url, "udp://@"); - - try { - mavlink_pub = mavlink_nh.advertise("to", 10); - gcs_link = MAVConnInterface::open_url(gcs_url, 1, MAV_COMP_ID_UDP_BRIDGE, mavlink_pub_cb); - gcs_link_diag.set_mavconn(gcs_link); - gcs_link_diag.set_connection_status(true); - } - catch (mavconn::DeviceError &ex) { - ROS_FATAL("GCS: %s", ex.what()); - return 1; - } - - // prefer UDPROS, but allow TCPROS too - mavlink_sub = mavlink_nh.subscribe("from", 10, mavlink_sub_cb, - ros::TransportHints() - .unreliable().maxDatagramSize(1024) - .reliable()); - - // setup updater - updater.setHardwareID(gcs_url); - updater.add(gcs_link_diag); - - // updater spinner - auto diag_timer = priv_nh.createTimer(ros::Duration(0.5), - [&](const ros::TimerEvent &evt) { - updater.update(); - }); - diag_timer.start(); - - ros::spin(); - return 0; -} diff --git a/mavros/src/lib/enum_sensor_orientation.cpp b/mavros/src/lib/enum_sensor_orientation.cpp index 006da5704..54a383d88 100644 --- a/mavros/src/lib/enum_sensor_orientation.cpp +++ b/mavros/src/lib/enum_sensor_orientation.cpp @@ -14,28 +14,39 @@ * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ +#include +#include #include -#include -#include -#include +#include "mavros/utils.hpp" +#include "mavros/frame_tf.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace mavros +{ +namespace utils +{ -namespace mavros { -namespace utils { using mavlink::common::MAV_SENSOR_ORIENTATION; // internal type: name - rotation using OrientationPair = std::pair; +static auto logger = rclcpp::get_logger("uas.enum"); + // internal data initializer -static const OrientationPair make_orientation(const std::string &name, - const double roll, - const double pitch, - const double yaw) +static const OrientationPair make_orientation( + const std::string & name, + const double roll, + const double pitch, + const double yaw) { - constexpr auto DEG_TO_RAD = (M_PI / 180.0); - const Eigen::Quaterniond rot = ftf::quaternion_from_rpy(Eigen::Vector3d(roll, pitch, yaw) * DEG_TO_RAD); - return std::make_pair(name, rot); + constexpr auto DEG_TO_RAD = (M_PI / 180.0); + const Eigen::Quaterniond rot = ftf::quaternion_from_rpy( + Eigen::Vector3d( + roll, pitch, + yaw) * DEG_TO_RAD); + return std::make_pair(name, rot); } // [[[cog: @@ -62,120 +73,124 @@ static const OrientationPair make_orientation(const std::string &name, // } // return cls(**pairs) // except Exception as ex: -// print(f"Parse Error: {ex}, desc: {desc}") +// cog.msg(f"Parse Error: {ex}, desc: {desc}") // return cls() // -// cog.outl(f"static const std::unordered_map::type, const OrientationPair> sensor_orientations{{{{") +// def __str__(self): +// return f"{self.Roll}, {self.Pitch}, {self.Yaw}" +// +// cog.outl( +// f"static const std::unordered_map::type,\n" +// f" const OrientationPair> sensor_orientations{{{{") // for k, e in enum: // name_short = e.name[len(pfx2):] // vec = Vector3.parse_rpy(e.description) -// whitespace = ' ' * (27 - len(name_short)) -// cog.outl(f"""{{ {k:>3}, make_orientation("{name_short}",{whitespace}{vec.Roll:>5}, {vec.Pitch:>5}, {vec.Yaw:>5}) }},""") +// cog.outl(f""" {{{k}, make_orientation("{name_short}", {vec})}},""") // // cog.outl("}};") // ]]] -static const std::unordered_map::type, const OrientationPair> sensor_orientations{{ -{ 0, make_orientation("NONE", 0.0, 0.0, 0.0) }, -{ 1, make_orientation("YAW_45", 0.0, 0.0, 45.0) }, -{ 2, make_orientation("YAW_90", 0.0, 0.0, 90.0) }, -{ 3, make_orientation("YAW_135", 0.0, 0.0, 135.0) }, -{ 4, make_orientation("YAW_180", 0.0, 0.0, 180.0) }, -{ 5, make_orientation("YAW_225", 0.0, 0.0, 225.0) }, -{ 6, make_orientation("YAW_270", 0.0, 0.0, 270.0) }, -{ 7, make_orientation("YAW_315", 0.0, 0.0, 315.0) }, -{ 8, make_orientation("ROLL_180", 180.0, 0.0, 0.0) }, -{ 9, make_orientation("ROLL_180_YAW_45", 180.0, 0.0, 45.0) }, -{ 10, make_orientation("ROLL_180_YAW_90", 180.0, 0.0, 90.0) }, -{ 11, make_orientation("ROLL_180_YAW_135", 180.0, 0.0, 135.0) }, -{ 12, make_orientation("PITCH_180", 0.0, 180.0, 0.0) }, -{ 13, make_orientation("ROLL_180_YAW_225", 180.0, 0.0, 225.0) }, -{ 14, make_orientation("ROLL_180_YAW_270", 180.0, 0.0, 270.0) }, -{ 15, make_orientation("ROLL_180_YAW_315", 180.0, 0.0, 315.0) }, -{ 16, make_orientation("ROLL_90", 90.0, 0.0, 0.0) }, -{ 17, make_orientation("ROLL_90_YAW_45", 90.0, 0.0, 45.0) }, -{ 18, make_orientation("ROLL_90_YAW_90", 90.0, 0.0, 90.0) }, -{ 19, make_orientation("ROLL_90_YAW_135", 90.0, 0.0, 135.0) }, -{ 20, make_orientation("ROLL_270", 270.0, 0.0, 0.0) }, -{ 21, make_orientation("ROLL_270_YAW_45", 270.0, 0.0, 45.0) }, -{ 22, make_orientation("ROLL_270_YAW_90", 270.0, 0.0, 90.0) }, -{ 23, make_orientation("ROLL_270_YAW_135", 270.0, 0.0, 135.0) }, -{ 24, make_orientation("PITCH_90", 0.0, 90.0, 0.0) }, -{ 25, make_orientation("PITCH_270", 0.0, 270.0, 0.0) }, -{ 26, make_orientation("PITCH_180_YAW_90", 0.0, 180.0, 90.0) }, -{ 27, make_orientation("PITCH_180_YAW_270", 0.0, 180.0, 270.0) }, -{ 28, make_orientation("ROLL_90_PITCH_90", 90.0, 90.0, 0.0) }, -{ 29, make_orientation("ROLL_180_PITCH_90", 180.0, 90.0, 0.0) }, -{ 30, make_orientation("ROLL_270_PITCH_90", 270.0, 90.0, 0.0) }, -{ 31, make_orientation("ROLL_90_PITCH_180", 90.0, 180.0, 0.0) }, -{ 32, make_orientation("ROLL_270_PITCH_180", 270.0, 180.0, 0.0) }, -{ 33, make_orientation("ROLL_90_PITCH_270", 90.0, 270.0, 0.0) }, -{ 34, make_orientation("ROLL_180_PITCH_270", 180.0, 270.0, 0.0) }, -{ 35, make_orientation("ROLL_270_PITCH_270", 270.0, 270.0, 0.0) }, -{ 36, make_orientation("ROLL_90_PITCH_180_YAW_90", 90.0, 180.0, 90.0) }, -{ 37, make_orientation("ROLL_90_YAW_270", 90.0, 0.0, 270.0) }, -{ 38, make_orientation("ROLL_90_PITCH_68_YAW_293", 90.0, 68.0, 293.0) }, -{ 39, make_orientation("PITCH_315", 0.0, 315.0, 0.0) }, -{ 40, make_orientation("ROLL_90_PITCH_315", 90.0, 315.0, 0.0) }, -{ 100, make_orientation("CUSTOM", 0.0, 0.0, 0.0) }, +static const std::unordered_map::type, + const OrientationPair> sensor_orientations{{ + {0, make_orientation("NONE", 0.0, 0.0, 0.0)}, + {1, make_orientation("YAW_45", 0.0, 0.0, 45.0)}, + {2, make_orientation("YAW_90", 0.0, 0.0, 90.0)}, + {3, make_orientation("YAW_135", 0.0, 0.0, 135.0)}, + {4, make_orientation("YAW_180", 0.0, 0.0, 180.0)}, + {5, make_orientation("YAW_225", 0.0, 0.0, 225.0)}, + {6, make_orientation("YAW_270", 0.0, 0.0, 270.0)}, + {7, make_orientation("YAW_315", 0.0, 0.0, 315.0)}, + {8, make_orientation("ROLL_180", 180.0, 0.0, 0.0)}, + {9, make_orientation("ROLL_180_YAW_45", 180.0, 0.0, 45.0)}, + {10, make_orientation("ROLL_180_YAW_90", 180.0, 0.0, 90.0)}, + {11, make_orientation("ROLL_180_YAW_135", 180.0, 0.0, 135.0)}, + {12, make_orientation("PITCH_180", 0.0, 180.0, 0.0)}, + {13, make_orientation("ROLL_180_YAW_225", 180.0, 0.0, 225.0)}, + {14, make_orientation("ROLL_180_YAW_270", 180.0, 0.0, 270.0)}, + {15, make_orientation("ROLL_180_YAW_315", 180.0, 0.0, 315.0)}, + {16, make_orientation("ROLL_90", 90.0, 0.0, 0.0)}, + {17, make_orientation("ROLL_90_YAW_45", 90.0, 0.0, 45.0)}, + {18, make_orientation("ROLL_90_YAW_90", 90.0, 0.0, 90.0)}, + {19, make_orientation("ROLL_90_YAW_135", 90.0, 0.0, 135.0)}, + {20, make_orientation("ROLL_270", 270.0, 0.0, 0.0)}, + {21, make_orientation("ROLL_270_YAW_45", 270.0, 0.0, 45.0)}, + {22, make_orientation("ROLL_270_YAW_90", 270.0, 0.0, 90.0)}, + {23, make_orientation("ROLL_270_YAW_135", 270.0, 0.0, 135.0)}, + {24, make_orientation("PITCH_90", 0.0, 90.0, 0.0)}, + {25, make_orientation("PITCH_270", 0.0, 270.0, 0.0)}, + {26, make_orientation("PITCH_180_YAW_90", 0.0, 180.0, 90.0)}, + {27, make_orientation("PITCH_180_YAW_270", 0.0, 180.0, 270.0)}, + {28, make_orientation("ROLL_90_PITCH_90", 90.0, 90.0, 0.0)}, + {29, make_orientation("ROLL_180_PITCH_90", 180.0, 90.0, 0.0)}, + {30, make_orientation("ROLL_270_PITCH_90", 270.0, 90.0, 0.0)}, + {31, make_orientation("ROLL_90_PITCH_180", 90.0, 180.0, 0.0)}, + {32, make_orientation("ROLL_270_PITCH_180", 270.0, 180.0, 0.0)}, + {33, make_orientation("ROLL_90_PITCH_270", 90.0, 270.0, 0.0)}, + {34, make_orientation("ROLL_180_PITCH_270", 180.0, 270.0, 0.0)}, + {35, make_orientation("ROLL_270_PITCH_270", 270.0, 270.0, 0.0)}, + {36, make_orientation("ROLL_90_PITCH_180_YAW_90", 90.0, 180.0, 90.0)}, + {37, make_orientation("ROLL_90_YAW_270", 90.0, 0.0, 270.0)}, + {38, make_orientation("ROLL_90_PITCH_68_YAW_293", 90.0, 68.0, 293.0)}, + {39, make_orientation("PITCH_315", 0.0, 315.0, 0.0)}, + {40, make_orientation("ROLL_90_PITCH_315", 90.0, 315.0, 0.0)}, + {100, make_orientation("CUSTOM", 0.0, 0.0, 0.0)}, }}; -// [[[end]]] (checksum: 92ae7d7d313456c3645ce7c189d2b6df) - +// [[[end]]] (checksum: ddb7b21315b0f0636d264ad2d6b3856e) std::string to_string(MAV_SENSOR_ORIENTATION orientation) { - const auto idx = enum_value(orientation); - auto it = sensor_orientations.find(idx); + const auto idx = enum_value(orientation); + auto it = sensor_orientations.find(idx); - if (it == sensor_orientations.end()) { - ROS_ERROR_NAMED("uas", "SENSOR: wrong orientation index: %d", idx); - return std::to_string(idx); - } + if (it == sensor_orientations.end()) { + RCLCPP_ERROR(logger, "SENSOR: wrong orientation index: %d", idx); + return std::to_string(idx); + } - return it->second.first; + return it->second.first; } Eigen::Quaterniond sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation) { - const auto idx = enum_value(orientation); - auto it = sensor_orientations.find(idx); + const auto idx = enum_value(orientation); + auto it = sensor_orientations.find(idx); - if (it == sensor_orientations.end()) { - ROS_ERROR_NAMED("uas", "SENSOR: wrong orientation index: %d", idx); - return Eigen::Quaterniond::Identity(); - } + if (it == sensor_orientations.end()) { + RCLCPP_ERROR(logger, "SENSOR: wrong orientation index: %d", idx); + return Eigen::Quaterniond::Identity(); + } - return it->second.second; + return it->second.second; } -int sensor_orientation_from_str(const std::string &sensor_orientation) +int sensor_orientation_from_str(const std::string & sensor_orientation) { - // XXX bsearch - - // 1. try to find by name - for (const auto& kv : sensor_orientations) { - if (kv.second.first == sensor_orientation) - return kv.first; - } - - // 2. try convert integer - // fallback for old configs that uses numeric orientation. - try { - int idx = std::stoi(sensor_orientation, 0, 0); - if (0 > idx || sensor_orientations.find(idx) == sensor_orientations.end()) { - ROS_ERROR_NAMED("uas", "SENSOR: orientation index out of bound: %d", idx); - return -1; - } - else - return idx; - } - catch (std::invalid_argument &ex) { - // failed - } - - ROS_ERROR_STREAM_NAMED("uas", "SENSOR: wrong orientation str: " << sensor_orientation); - - return -1; + // XXX bsearch + + // 1. try to find by name + for (const auto & kv : sensor_orientations) { + if (kv.second.first == sensor_orientation) { + return kv.first; + } + } + + // 2. try convert integer + // fallback for old configs that uses numeric orientation. + try { + int idx = std::stoi(sensor_orientation, 0, 0); + if (0 > idx || sensor_orientations.find(idx) == sensor_orientations.end()) { + RCLCPP_ERROR(logger, "SENSOR: orientation index out of bound: %d", idx); + return -1; + } else { + return idx; + } + } catch (std::invalid_argument & ex) { + // failed + } + + RCLCPP_ERROR_STREAM(logger, "SENSOR: wrong orientation str: " << sensor_orientation); + + return -1; } -} // namespace utils -} // namespace mavros +} // namespace utils +} // namespace mavros diff --git a/mavros/src/lib/enum_to_string.cpp b/mavros/src/lib/enum_to_string.cpp index c3d8b1ff1..bfaa60cb3 100644 --- a/mavros/src/lib/enum_to_string.cpp +++ b/mavros/src/lib/enum_to_string.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2016,2021 Valdimir Ermakov + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief enum stringify helpers * @file enum_to_string.cpp @@ -6,21 +13,18 @@ * @addtogroup nodelib * @{ */ -/* - * Copyright 2016 Valdimir Ermakov - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ #include +#include #include -#include -#include -namespace mavros { -namespace utils { +#include "mavros/utils.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace mavros +{ +namespace utils +{ using mavlink::minimal::MAV_AUTOPILOT; using mavlink::minimal::MAV_TYPE; using mavlink::minimal::MAV_STATE; @@ -34,14 +38,22 @@ using mavlink::common::MAV_FRAME; using mavlink::common::MAV_DISTANCE_SENSOR; using mavlink::common::LANDING_TARGET_TYPE; +static auto logger = rclcpp::get_logger("uas.enum"); + // [[[cog: // import pymavlink.dialects.v20.common as common // +// # NOTE(vooon): Foxy couldn't exclude that file from cpplint +// # So in order to pass lint test i have to remove +// # description comments. +// EMIT_DESCRIPTION = False +// // def get_enum(ename): // enum = sorted(common.enums[ename].items()) // enum.pop() # remove ENUM_END // return enum // +// // def split_by(delim, s): // for c in delim: // if c in s: @@ -49,249 +61,270 @@ using mavlink::common::LANDING_TARGET_TYPE; // // return s.strip() // +// // def make_whitespace(l, v): // d = l - len(v) -// return ' ' * d if d > 0 else ' ' +// return ' ' * d if d > 2 else ' ' +// // // def ename_array_name(ename, suffix=None): // l = ename.rsplit('::', 1) // return (l[1] if len(l) > 1 else l[0]).lower() + (suffix or '_strings') // +// // def array_outl(name, enum, suffix=None): // array = ename_array_name(name, suffix) -// cog.outl(f"""\ +// cog.outl(f""" // //! {name} values // static const std::array {array}{{{{""") // +// // def to_string_outl(ename, funcname='to_string', suffix=None): // array = ename_array_name(ename, suffix) -// cog.outl(f"""\ +// cog.outl(f""" // std::string {funcname}({ename} e) // {{ -// size_t idx = enum_value(e); -// if (idx >= {array}.size()) -// return std::to_string(idx); +// size_t idx = enum_value(e); +// if (idx >= {array}.size()) {{ +// return std::to_string(idx); +// }} // -// return {array}[idx]; +// return {array}[idx]; // }}""") // -// def enum_name_is_value_outl(ename, suffix=None, funcname='to_string'): +// +// def enum_value_is_description_outl(ename, suffix=None, +// split_by_delim='-,/.', funcname='to_string'): +// enum = get_enum(ename) +// +// array_outl(ename, enum, suffix) +// for k, e in enum: +// value = split_by(split_by_delim, e.description) +// sp = make_whitespace(30, value) +// if EMIT_DESCRIPTION and e.description: +// cog.outl(f"""/* {k:>2} */ "{value}",{sp}// {e.description}""") +// else: +// cog.outl(f"""/* {k:>2} */ "{value}",""") +// +// cog.outl("}};") +// cog.outl() +// to_string_outl(ename, funcname, suffix) +// +// +// def enum_value_is_name_outl(ename, suffix=None, funcname='to_string'): // enum = get_enum(ename) // // array_outl(ename, enum, suffix) // for k, e in enum: // name_short = e.name[len(ename) + 1:] // sp = make_whitespace(30, name_short) -// if e.description: +// if EMIT_DESCRIPTION and e.description: // cog.outl(f"""/* {k:>2} */ "{name_short}",{sp}// {e.description}""") // else: // cog.outl(f"""/* {k:>2} */ "{name_short}",""") // // cog.outl("}};") -// cog.outl() // to_string_outl(ename, funcname, suffix) -// +// ]]] +// [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) + +// [[[cog: // ename = 'MAV_AUTOPILOT' -// enum = get_enum(ename) -// -// array_outl(ename, enum) -// for k, e in enum: -// value = split_by('-,/.', e.description) -// sp = make_whitespace(30, value) -// cog.outl(f"""/* {k:>2} */ "{value}",{sp}// {e.description}""") -// -// cog.outl("}};") -// cog.outl() -// to_string_outl(ename) +// enum_value_is_description_outl(ename) // ]]] + //! MAV_AUTOPILOT values -static const std::array mav_autopilot_strings{{ -/* 0 */ "Generic autopilot", // Generic autopilot, full support for everything -/* 1 */ "Reserved for future use", // Reserved for future use. -/* 2 */ "SLUGS autopilot", // SLUGS autopilot, http://slugsuav.soe.ucsc.edu -/* 3 */ "ArduPilot", // ArduPilot - Plane/Copter/Rover/Sub/Tracker, https://ardupilot.org -/* 4 */ "OpenPilot", // OpenPilot, http://openpilot.org -/* 5 */ "Generic autopilot only supporting simple waypoints", // Generic autopilot only supporting simple waypoints -/* 6 */ "Generic autopilot supporting waypoints and other simple navigation commands", // Generic autopilot supporting waypoints and other simple navigation commands -/* 7 */ "Generic autopilot supporting the full mission command set", // Generic autopilot supporting the full mission command set -/* 8 */ "No valid autopilot", // No valid autopilot, e.g. a GCS or other MAVLink component -/* 9 */ "PPZ UAV", // PPZ UAV - http://nongnu.org/paparazzi -/* 10 */ "UAV Dev Board", // UAV Dev Board -/* 11 */ "FlexiPilot", // FlexiPilot -/* 12 */ "PX4 Autopilot", // PX4 Autopilot - http://px4.io/ -/* 13 */ "SMACCMPilot", // SMACCMPilot - http://smaccmpilot.org -/* 14 */ "AutoQuad", // AutoQuad -- http://autoquad.org -/* 15 */ "Armazila", // Armazila -- http://armazila.com -/* 16 */ "Aerob", // Aerob -- http://aerob.ru -/* 17 */ "ASLUAV autopilot", // ASLUAV autopilot -- http://www.asl.ethz.ch -/* 18 */ "SmartAP Autopilot", // SmartAP Autopilot - http://sky-drones.com -/* 19 */ "AirRails", // AirRails - http://uaventure.com +static const std::array mav_autopilot_strings{{ +/* 0 */ "Generic autopilot", +/* 1 */ "Reserved for future use", +/* 2 */ "SLUGS autopilot", +/* 3 */ "ArduPilot", +/* 4 */ "OpenPilot", +/* 5 */ "Generic autopilot only supporting simple waypoints", +/* 6 */ "Generic autopilot supporting waypoints and other simple navigation commands", +/* 7 */ "Generic autopilot supporting the full mission command set", +/* 8 */ "No valid autopilot", +/* 9 */ "PPZ UAV", +/* 10 */ "UAV Dev Board", +/* 11 */ "FlexiPilot", +/* 12 */ "PX4 Autopilot", +/* 13 */ "SMACCMPilot", +/* 14 */ "AutoQuad", +/* 15 */ "Armazila", +/* 16 */ "Aerob", +/* 17 */ "ASLUAV autopilot", +/* 18 */ "SmartAP Autopilot", +/* 19 */ "AirRails", +/* 20 */ "Fusion Reflex", }}; + std::string to_string(MAV_AUTOPILOT e) { - size_t idx = enum_value(e); - if (idx >= mav_autopilot_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= mav_autopilot_strings.size()) { + return std::to_string(idx); + } - return mav_autopilot_strings[idx]; + return mav_autopilot_strings[idx]; } -// [[[end]]] (checksum: c1feb82117da0447594aacbe5c52f97b) +// [[[end]]] (checksum: 68013f2988194a55231693f1d7fa9726) // [[[cog: // ename = 'MAV_TYPE' -// enum = get_enum(ename) -// -// array_outl(ename, enum) -// for k, e in enum: -// value = split_by(',-/.', e.description) -// sp = make_whitespace(30, value) -// cog.outl(f"""/* {k:>2} */ "{value}",{sp}// {e.description}""") -// -// cog.outl("}};") -// cog.outl() -// to_string_outl(ename) +// enum_value_is_description_outl(ename) // ]]] + //! MAV_TYPE values -static const std::array mav_type_strings{{ -/* 0 */ "Generic micro air vehicle", // Generic micro air vehicle -/* 1 */ "Fixed wing aircraft", // Fixed wing aircraft. -/* 2 */ "Quadrotor", // Quadrotor -/* 3 */ "Coaxial helicopter", // Coaxial helicopter -/* 4 */ "Normal helicopter with tail rotor", // Normal helicopter with tail rotor. -/* 5 */ "Ground installation", // Ground installation -/* 6 */ "Operator control unit", // Operator control unit / ground control station -/* 7 */ "Airship", // Airship, controlled -/* 8 */ "Free balloon", // Free balloon, uncontrolled -/* 9 */ "Rocket", // Rocket -/* 10 */ "Ground rover", // Ground rover -/* 11 */ "Surface vessel", // Surface vessel, boat, ship -/* 12 */ "Submarine", // Submarine -/* 13 */ "Hexarotor", // Hexarotor -/* 14 */ "Octorotor", // Octorotor -/* 15 */ "Tricopter", // Tricopter -/* 16 */ "Flapping wing", // Flapping wing -/* 17 */ "Kite", // Kite -/* 18 */ "Onboard companion controller", // Onboard companion controller -/* 19 */ "Two", // Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. -/* 20 */ "Quad", // Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. -/* 21 */ "Tiltrotor VTOL", // Tiltrotor VTOL -/* 22 */ "VTOL reserved 2", // VTOL reserved 2 -/* 23 */ "VTOL reserved 3", // VTOL reserved 3 -/* 24 */ "VTOL reserved 4", // VTOL reserved 4 -/* 25 */ "VTOL reserved 5", // VTOL reserved 5 -/* 26 */ "Gimbal", // Gimbal -/* 27 */ "ADSB system", // ADSB system -/* 28 */ "Steerable", // Steerable, nonrigid airfoil -/* 29 */ "Dodecarotor", // Dodecarotor -/* 30 */ "Camera", // Camera -/* 31 */ "Charging station", // Charging station -/* 32 */ "FLARM collision avoidance system", // FLARM collision avoidance system -/* 33 */ "Servo", // Servo -/* 34 */ "Open Drone ID. See https:", // Open Drone ID. See https://mavlink.io/en/services/opendroneid.html. -/* 35 */ "Decarotor", // Decarotor +static const std::array mav_type_strings{{ +/* 0 */ "Generic micro air vehicle", +/* 1 */ "Fixed wing aircraft", +/* 2 */ "Quadrotor", +/* 3 */ "Coaxial helicopter", +/* 4 */ "Normal helicopter with tail rotor", +/* 5 */ "Ground installation", +/* 6 */ "Operator control unit", +/* 7 */ "Airship", +/* 8 */ "Free balloon", +/* 9 */ "Rocket", +/* 10 */ "Ground rover", +/* 11 */ "Surface vessel", +/* 12 */ "Submarine", +/* 13 */ "Hexarotor", +/* 14 */ "Octorotor", +/* 15 */ "Tricopter", +/* 16 */ "Flapping wing", +/* 17 */ "Kite", +/* 18 */ "Onboard companion controller", +/* 19 */ "Two", +/* 20 */ "Quad", +/* 21 */ "Tiltrotor VTOL", +/* 22 */ "VTOL reserved 2", +/* 23 */ "VTOL reserved 3", +/* 24 */ "VTOL reserved 4", +/* 25 */ "VTOL reserved 5", +/* 26 */ "Gimbal", +/* 27 */ "ADSB system", +/* 28 */ "Steerable", +/* 29 */ "Dodecarotor", +/* 30 */ "Camera", +/* 31 */ "Charging station", +/* 32 */ "FLARM collision avoidance system", +/* 33 */ "Servo", +/* 34 */ "Open Drone ID. See https:", +/* 35 */ "Decarotor", +/* 36 */ "Battery", +/* 37 */ "Parachute", +/* 38 */ "Log", +/* 39 */ "OSD", +/* 40 */ "IMU", +/* 41 */ "GPS", +/* 42 */ "Winch", }}; + std::string to_string(MAV_TYPE e) { - size_t idx = enum_value(e); - if (idx >= mav_type_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= mav_type_strings.size()) { + return std::to_string(idx); + } - return mav_type_strings[idx]; + return mav_type_strings[idx]; } -// [[[end]]] (checksum: f44b083f4b6be34f26d75692072f09bf) +// [[[end]]] (checksum: 5bcb1f5fb05dbb71467360a932e5b182) // [[[cog: // ename = 'MAV_TYPE' -// enum_name_is_value_outl(ename, funcname='to_name', suffix='_names') +// enum_value_is_name_outl(ename, funcname='enum_to_name', suffix='_names') // ]]] + //! MAV_TYPE values -static const std::array mav_type_names{{ -/* 0 */ "GENERIC", // Generic micro air vehicle -/* 1 */ "FIXED_WING", // Fixed wing aircraft. -/* 2 */ "QUADROTOR", // Quadrotor -/* 3 */ "COAXIAL", // Coaxial helicopter -/* 4 */ "HELICOPTER", // Normal helicopter with tail rotor. -/* 5 */ "ANTENNA_TRACKER", // Ground installation -/* 6 */ "GCS", // Operator control unit / ground control station -/* 7 */ "AIRSHIP", // Airship, controlled -/* 8 */ "FREE_BALLOON", // Free balloon, uncontrolled -/* 9 */ "ROCKET", // Rocket -/* 10 */ "GROUND_ROVER", // Ground rover -/* 11 */ "SURFACE_BOAT", // Surface vessel, boat, ship -/* 12 */ "SUBMARINE", // Submarine -/* 13 */ "HEXAROTOR", // Hexarotor -/* 14 */ "OCTOROTOR", // Octorotor -/* 15 */ "TRICOPTER", // Tricopter -/* 16 */ "FLAPPING_WING", // Flapping wing -/* 17 */ "KITE", // Kite -/* 18 */ "ONBOARD_CONTROLLER", // Onboard companion controller -/* 19 */ "VTOL_DUOROTOR", // Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. -/* 20 */ "VTOL_QUADROTOR", // Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. -/* 21 */ "VTOL_TILTROTOR", // Tiltrotor VTOL -/* 22 */ "VTOL_RESERVED2", // VTOL reserved 2 -/* 23 */ "VTOL_RESERVED3", // VTOL reserved 3 -/* 24 */ "VTOL_RESERVED4", // VTOL reserved 4 -/* 25 */ "VTOL_RESERVED5", // VTOL reserved 5 -/* 26 */ "GIMBAL", // Gimbal -/* 27 */ "ADSB", // ADSB system -/* 28 */ "PARAFOIL", // Steerable, nonrigid airfoil -/* 29 */ "DODECAROTOR", // Dodecarotor -/* 30 */ "CAMERA", // Camera -/* 31 */ "CHARGING_STATION", // Charging station -/* 32 */ "FLARM", // FLARM collision avoidance system -/* 33 */ "SERVO", // Servo -/* 34 */ "ODID", // Open Drone ID. See https://mavlink.io/en/services/opendroneid.html. -/* 35 */ "DECAROTOR", // Decarotor +static const std::array mav_type_names{{ +/* 0 */ "GENERIC", +/* 1 */ "FIXED_WING", +/* 2 */ "QUADROTOR", +/* 3 */ "COAXIAL", +/* 4 */ "HELICOPTER", +/* 5 */ "ANTENNA_TRACKER", +/* 6 */ "GCS", +/* 7 */ "AIRSHIP", +/* 8 */ "FREE_BALLOON", +/* 9 */ "ROCKET", +/* 10 */ "GROUND_ROVER", +/* 11 */ "SURFACE_BOAT", +/* 12 */ "SUBMARINE", +/* 13 */ "HEXAROTOR", +/* 14 */ "OCTOROTOR", +/* 15 */ "TRICOPTER", +/* 16 */ "FLAPPING_WING", +/* 17 */ "KITE", +/* 18 */ "ONBOARD_CONTROLLER", +/* 19 */ "VTOL_DUOROTOR", +/* 20 */ "VTOL_QUADROTOR", +/* 21 */ "VTOL_TILTROTOR", +/* 22 */ "VTOL_RESERVED2", +/* 23 */ "VTOL_RESERVED3", +/* 24 */ "VTOL_RESERVED4", +/* 25 */ "VTOL_RESERVED5", +/* 26 */ "GIMBAL", +/* 27 */ "ADSB", +/* 28 */ "PARAFOIL", +/* 29 */ "DODECAROTOR", +/* 30 */ "CAMERA", +/* 31 */ "CHARGING_STATION", +/* 32 */ "FLARM", +/* 33 */ "SERVO", +/* 34 */ "ODID", +/* 35 */ "DECAROTOR", +/* 36 */ "BATTERY", +/* 37 */ "PARACHUTE", +/* 38 */ "LOG", +/* 39 */ "OSD", +/* 40 */ "IMU", +/* 41 */ "GPS", +/* 42 */ "WINCH", }}; -std::string to_name(MAV_TYPE e) +std::string enum_to_name(MAV_TYPE e) { - size_t idx = enum_value(e); - if (idx >= mav_type_names.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= mav_type_names.size()) { + return std::to_string(idx); + } - return mav_type_names[idx]; + return mav_type_names[idx]; } -// [[[end]]] (checksum: 3775b5a8763b4e66287a471af939ef6c) +// [[[end]]] (checksum: dfc98695f046912ae73ec2bd4508e9c1) // [[[cog: // ename = 'MAV_STATE' -// enum = get_enum(ename) -// -// array_outl(ename, enum) -// for k, e in enum: -// value = e.name[len(ename) + 1:].title() -// sp = make_whitespace(30, value) -// cog.outl("""/* {k:>2} */ "{value}",{sp}// {e.description}""".format(**locals())) -// -// cog.outl("}};") -// cog.outl() -// to_string_outl(ename) +// enum_value_is_name_outl(ename) // ]]] + //! MAV_STATE values static const std::array mav_state_strings{{ -/* 0 */ "Uninit", // Uninitialized system, state is unknown. -/* 1 */ "Boot", // System is booting up. -/* 2 */ "Calibrating", // System is calibrating and not flight-ready. -/* 3 */ "Standby", // System is grounded and on standby. It can be launched any time. -/* 4 */ "Active", // System is active and might be already airborne. Motors are engaged. -/* 5 */ "Critical", // System is in a non-normal flight mode. It can however still navigate. -/* 6 */ "Emergency", // System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. -/* 7 */ "Poweroff", // System just initialized its power-down sequence, will shut down now. -/* 8 */ "Flight_Termination", // System is terminating itself. +/* 0 */ "UNINIT", +/* 1 */ "BOOT", +/* 2 */ "CALIBRATING", +/* 3 */ "STANDBY", +/* 4 */ "ACTIVE", +/* 5 */ "CRITICAL", +/* 6 */ "EMERGENCY", +/* 7 */ "POWEROFF", +/* 8 */ "FLIGHT_TERMINATION", }}; std::string to_string(MAV_STATE e) { - size_t idx = enum_value(e); - if (idx >= mav_state_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= mav_state_strings.size()) { + return std::to_string(idx); + } - return mav_state_strings[idx]; + return mav_state_strings[idx]; } -// [[[end]]] (checksum: 8af1e6916d0229c193aab7d3dc2c97e9) +// [[[end]]] (checksum: e953b14d18e31abb45db4fe72ebb749f) // [[[cog: // ename = "timesync_mode" @@ -299,12 +332,13 @@ std::string to_string(MAV_STATE e) // // array_outl(ename, ent) // for k, e in enumerate(ent): -// cog.outl("""/* {k:>2} */ "{e}",""".format(**locals())) +// cog.outl(f"""/* {k:>2} */ "{e}",""") // // cog.outl("}};") // cog.outl() // to_string_outl(ename) // ]]] + //! timesync_mode values static const std::array timesync_mode_strings{{ /* 0 */ "NONE", @@ -313,53 +347,58 @@ static const std::array timesync_mode_strings{{ /* 3 */ "PASSTHROUGH", }}; + std::string to_string(timesync_mode e) { - size_t idx = enum_value(e); - if (idx >= timesync_mode_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= timesync_mode_strings.size()) { + return std::to_string(idx); + } - return timesync_mode_strings[idx]; + return timesync_mode_strings[idx]; } -// [[[end]]] (checksum: 2796eaa4f9361c2d7ca87f63e0401d4d) +// [[[end]]] (checksum: 7a286bcf12006fdeff1bd9fca8ce4176) -timesync_mode timesync_mode_from_str(const std::string &mode) +timesync_mode timesync_mode_from_str(const std::string & mode) { - for (size_t idx = 0; idx < timesync_mode_strings.size(); idx++) { - if (timesync_mode_strings[idx] == mode) { - std::underlying_type::type rv = idx; - return static_cast(rv); - } - } - - ROS_ERROR_STREAM_NAMED("uas", "TM: Unknown mode: " << mode); - return timesync_mode::NONE; + for (size_t idx = 0; idx < timesync_mode_strings.size(); idx++) { + if (timesync_mode_strings[idx] == mode) { + std::underlying_type::type rv = idx; + return static_cast(rv); + } + } + + RCLCPP_ERROR_STREAM(logger, "TM: Unknown mode: " << mode); + return timesync_mode::NONE; } // [[[cog: // ename = 'ADSB_ALTITUDE_TYPE' -// enum_name_is_value_outl(ename) +// enum_value_is_name_outl(ename) // ]]] + //! ADSB_ALTITUDE_TYPE values static const std::array adsb_altitude_type_strings{{ -/* 0 */ "PRESSURE_QNH", // Altitude reported from a Baro source using QNH reference -/* 1 */ "GEOMETRIC", // Altitude reported from a GNSS source +/* 0 */ "PRESSURE_QNH", +/* 1 */ "GEOMETRIC", }}; std::string to_string(ADSB_ALTITUDE_TYPE e) { - size_t idx = enum_value(e); - if (idx >= adsb_altitude_type_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= adsb_altitude_type_strings.size()) { + return std::to_string(idx); + } - return adsb_altitude_type_strings[idx]; + return adsb_altitude_type_strings[idx]; } -// [[[end]]] (checksum: dc127bf29aefa513471d13c5a0e1e6ec) +// [[[end]]] (checksum: 2e8d87a6e603b105ded642f34978fd55) // [[[cog: // ename = 'ADSB_EMITTER_TYPE' -// enum_name_is_value_outl(ename) +// enum_value_is_name_outl(ename) // ]]] + //! ADSB_EMITTER_TYPE values static const std::array adsb_emitter_type_strings{{ /* 0 */ "NO_INFO", @@ -386,391 +425,410 @@ static const std::array adsb_emitter_type_strings{{ std::string to_string(ADSB_EMITTER_TYPE e) { - size_t idx = enum_value(e); - if (idx >= adsb_emitter_type_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= adsb_emitter_type_strings.size()) { + return std::to_string(idx); + } - return adsb_emitter_type_strings[idx]; + return adsb_emitter_type_strings[idx]; } -// [[[end]]] (checksum: 713e0304603321e421131d8552d0f8e0) +// [[[end]]] (checksum: 342e71a579408cf35a4dbf8b42bd099a) // [[[cog: // ename = 'MAV_ESTIMATOR_TYPE' -// enum_name_is_value_outl(ename) +// enum_value_is_name_outl(ename) // ]]] + //! MAV_ESTIMATOR_TYPE values static const std::array mav_estimator_type_strings{{ -/* 0 */ "UNKNOWN", // Unknown type of the estimator. -/* 1 */ "NAIVE", // This is a naive estimator without any real covariance feedback. -/* 2 */ "VISION", // Computer vision based estimate. Might be up to scale. -/* 3 */ "VIO", // Visual-inertial estimate. -/* 4 */ "GPS", // Plain GPS estimate. -/* 5 */ "GPS_INS", // Estimator integrating GPS and inertial sensing. -/* 6 */ "MOCAP", // Estimate from external motion capturing system. -/* 7 */ "LIDAR", // Estimator based on lidar sensor input. -/* 8 */ "AUTOPILOT", // Estimator on autopilot. +/* 0 */ "UNKNOWN", +/* 1 */ "NAIVE", +/* 2 */ "VISION", +/* 3 */ "VIO", +/* 4 */ "GPS", +/* 5 */ "GPS_INS", +/* 6 */ "MOCAP", +/* 7 */ "LIDAR", +/* 8 */ "AUTOPILOT", }}; std::string to_string(MAV_ESTIMATOR_TYPE e) { - size_t idx = enum_value(e); - if (idx >= mav_estimator_type_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= mav_estimator_type_strings.size()) { + return std::to_string(idx); + } - return mav_estimator_type_strings[idx]; + return mav_estimator_type_strings[idx]; } -// [[[end]]] (checksum: 78a66e6898ff8c5dafb482dbf264a489) +// [[[end]]] (checksum: 451e5fe0a2c760a5c9d4efed0f97553d) // [[[cog: // ename = 'GPS_FIX_TYPE' -// enum_name_is_value_outl(ename) +// enum_value_is_name_outl(ename) // ]]] + //! GPS_FIX_TYPE values static const std::array gps_fix_type_strings{{ -/* 0 */ "NO_GPS", // No GPS connected -/* 1 */ "NO_FIX", // No position information, GPS is connected -/* 2 */ "2D_FIX", // 2D position -/* 3 */ "3D_FIX", // 3D position -/* 4 */ "DGPS", // DGPS/SBAS aided 3D position -/* 5 */ "RTK_FLOAT", // RTK float, 3D position -/* 6 */ "RTK_FIXED", // RTK Fixed, 3D position -/* 7 */ "STATIC", // Static fixed, typically used for base stations -/* 8 */ "PPP", // PPP, 3D position. +/* 0 */ "NO_GPS", +/* 1 */ "NO_FIX", +/* 2 */ "2D_FIX", +/* 3 */ "3D_FIX", +/* 4 */ "DGPS", +/* 5 */ "RTK_FLOAT", +/* 6 */ "RTK_FIXED", +/* 7 */ "STATIC", +/* 8 */ "PPP", }}; std::string to_string(GPS_FIX_TYPE e) { - size_t idx = enum_value(e); - if (idx >= gps_fix_type_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= gps_fix_type_strings.size()) { + return std::to_string(idx); + } - return gps_fix_type_strings[idx]; + return gps_fix_type_strings[idx]; } -// [[[end]]] (checksum: 7569b73b2d68ed1412bf0c36afeb131c) +// [[[end]]] (checksum: 260b7022824b9717be95db4e4e28a8d5) // [[[cog: // ename = 'MAV_MISSION_RESULT' -// enum = get_enum(ename) -// -// array_outl(ename, enum) -// for k, e in enum: -// value = e.description -// sp = make_whitespace(30, value) -// cog.outl("""/* {k:>2} */ "{value}",{sp}// {e.description}""".format(**locals())) -// -// cog.outl("}};") -// cog.outl() -// to_string_outl(ename) +// enum_value_is_description_outl(ename, split_by_delim='') // ]]] + //! MAV_MISSION_RESULT values static const std::array mav_mission_result_strings{{ -/* 0 */ "mission accepted OK", // mission accepted OK -/* 1 */ "Generic error / not accepting mission commands at all right now.", // Generic error / not accepting mission commands at all right now. -/* 2 */ "Coordinate frame is not supported.", // Coordinate frame is not supported. -/* 3 */ "Command is not supported.", // Command is not supported. -/* 4 */ "Mission items exceed storage space.", // Mission items exceed storage space. -/* 5 */ "One of the parameters has an invalid value.", // One of the parameters has an invalid value. -/* 6 */ "param1 has an invalid value.", // param1 has an invalid value. -/* 7 */ "param2 has an invalid value.", // param2 has an invalid value. -/* 8 */ "param3 has an invalid value.", // param3 has an invalid value. -/* 9 */ "param4 has an invalid value.", // param4 has an invalid value. -/* 10 */ "x / param5 has an invalid value.", // x / param5 has an invalid value. -/* 11 */ "y / param6 has an invalid value.", // y / param6 has an invalid value. -/* 12 */ "z / param7 has an invalid value.", // z / param7 has an invalid value. -/* 13 */ "Mission item received out of sequence", // Mission item received out of sequence -/* 14 */ "Not accepting any mission commands from this communication partner.", // Not accepting any mission commands from this communication partner. -/* 15 */ "Current mission operation cancelled (e.g. mission upload, mission download).", // Current mission operation cancelled (e.g. mission upload, mission download). +/* 0 */ "mission accepted OK", +/* 1 */ "Generic error / not accepting mission commands at all right now.", +/* 2 */ "Coordinate frame is not supported.", +/* 3 */ "Command is not supported.", +/* 4 */ "Mission items exceed storage space.", +/* 5 */ "One of the parameters has an invalid value.", +/* 6 */ "param1 has an invalid value.", +/* 7 */ "param2 has an invalid value.", +/* 8 */ "param3 has an invalid value.", +/* 9 */ "param4 has an invalid value.", +/* 10 */ "x / param5 has an invalid value.", +/* 11 */ "y / param6 has an invalid value.", +/* 12 */ "z / param7 has an invalid value.", +/* 13 */ "Mission item received out of sequence", +/* 14 */ "Not accepting any mission commands from this communication partner.", +/* 15 */ "Current mission operation cancelled (e.g. mission upload, mission download).", }}; + std::string to_string(MAV_MISSION_RESULT e) { - size_t idx = enum_value(e); - if (idx >= mav_mission_result_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= mav_mission_result_strings.size()) { + return std::to_string(idx); + } - return mav_mission_result_strings[idx]; + return mav_mission_result_strings[idx]; } -// [[[end]]] (checksum: bf3c500065b1c65a1e822c70da56d2d5) +// [[[end]]] (checksum: ddddde7cdf04ebd988f019d5b93eadbe) // [[[cog: // ename = 'MAV_FRAME' -// enum_name_is_value_outl(ename) +// enum_value_is_name_outl(ename) // ]]] + //! MAV_FRAME values static const std::array mav_frame_strings{{ -/* 0 */ "GLOBAL", // Global (WGS84) coordinate frame + MSL altitude. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). -/* 1 */ "LOCAL_NED", // Local coordinate frame, Z-down (x: North, y: East, z: Down). -/* 2 */ "MISSION", // NOT a coordinate frame, indicates a mission command. -/* 3 */ "GLOBAL_RELATIVE_ALT", // Global (WGS84) coordinate frame + altitude relative to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. -/* 4 */ "LOCAL_ENU", // Local coordinate frame, Z-up (x: East, y: North, z: Up). -/* 5 */ "GLOBAL_INT", // Global (WGS84) coordinate frame (scaled) + MSL altitude. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL). -/* 6 */ "GLOBAL_RELATIVE_ALT_INT", // Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location. -/* 7 */ "LOCAL_OFFSET_NED", // Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. -/* 8 */ "BODY_NED", // Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. -/* 9 */ "BODY_OFFSET_NED", // Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. -/* 10 */ "GLOBAL_TERRAIN_ALT", // Global (WGS84) coordinate frame with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. -/* 11 */ "GLOBAL_TERRAIN_ALT_INT", // Global (WGS84) coordinate frame (scaled) with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. -/* 12 */ "BODY_FRD", // Body fixed frame of reference, Z-down (x: Forward, y: Right, z: Down). -/* 13 */ "RESERVED_13", // MAV_FRAME_BODY_FLU - Body fixed frame of reference, Z-up (x: Forward, y: Left, z: Up). -/* 14 */ "RESERVED_14", // MAV_FRAME_MOCAP_NED - Odometry local coordinate frame of data given by a motion capture system, Z-down (x: North, y: East, z: Down). -/* 15 */ "RESERVED_15", // MAV_FRAME_MOCAP_ENU - Odometry local coordinate frame of data given by a motion capture system, Z-up (x: East, y: North, z: Up). -/* 16 */ "RESERVED_16", // MAV_FRAME_VISION_NED - Odometry local coordinate frame of data given by a vision estimation system, Z-down (x: North, y: East, z: Down). -/* 17 */ "RESERVED_17", // MAV_FRAME_VISION_ENU - Odometry local coordinate frame of data given by a vision estimation system, Z-up (x: East, y: North, z: Up). -/* 18 */ "RESERVED_18", // MAV_FRAME_ESTIM_NED - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-down (x: North, y: East, z: Down). -/* 19 */ "RESERVED_19", // MAV_FRAME_ESTIM_ENU - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-up (x: East, y: North, z: Up). -/* 20 */ "LOCAL_FRD", // Forward, Right, Down coordinate frame. This is a local frame with Z-down and arbitrary F/R alignment (i.e. not aligned with NED/earth frame). -/* 21 */ "LOCAL_FLU", // Forward, Left, Up coordinate frame. This is a local frame with Z-up and arbitrary F/L alignment (i.e. not aligned with ENU/earth frame). +/* 0 */ "GLOBAL", +/* 1 */ "LOCAL_NED", +/* 2 */ "MISSION", +/* 3 */ "GLOBAL_RELATIVE_ALT", +/* 4 */ "LOCAL_ENU", +/* 5 */ "GLOBAL_INT", +/* 6 */ "GLOBAL_RELATIVE_ALT_INT", +/* 7 */ "LOCAL_OFFSET_NED", +/* 8 */ "BODY_NED", +/* 9 */ "BODY_OFFSET_NED", +/* 10 */ "GLOBAL_TERRAIN_ALT", +/* 11 */ "GLOBAL_TERRAIN_ALT_INT", +/* 12 */ "BODY_FRD", +/* 13 */ "RESERVED_13", +/* 14 */ "RESERVED_14", +/* 15 */ "RESERVED_15", +/* 16 */ "RESERVED_16", +/* 17 */ "RESERVED_17", +/* 18 */ "RESERVED_18", +/* 19 */ "RESERVED_19", +/* 20 */ "LOCAL_FRD", +/* 21 */ "LOCAL_FLU", }}; std::string to_string(MAV_FRAME e) { - size_t idx = enum_value(e); - if (idx >= mav_frame_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= mav_frame_strings.size()) { + return std::to_string(idx); + } - return mav_frame_strings[idx]; + return mav_frame_strings[idx]; } -// [[[end]]] (checksum: f7783e4d7764c236021e92fc4a1c16a1) +// [[[end]]] (checksum: 9e2018e38b2c586263f10adba00d2ca6) // [[[cog: // ename = 'MAV_COMPONENT' // suffix = 'MAV_COMP_ID' // enum = get_enum(ename) // -// cog.outl(f"static const std::unordered_map::type, const std::string> {suffix.lower()}_strings{{{{") +// cog.outl( +// f"static const std::unordered_map::type,\n" +// f" const std::string> {suffix.lower()}_strings{{{{") // for k, e in enum: // name_short = e.name[len(suffix) + 1:] -// sp = make_whitespace(30, name_short) -// if e.description: -// cog.outl(f"""{{ {k:>3}, "{name_short}" }},{sp}// {e.description}""") +// entry = f"""{{{k}, "{name_short}"}},""" +// if EMIT_DESCRIPTION and e.description: +// cog.outl(f""" {entry:<39} // {e.description}""") // else: -// cog.outl(f"""{{ {k:>3}, "{name_short}" }},""") +// cog.outl(f""" {entry}""") // // cog.outl("}};") // ]]] -static const std::unordered_map::type, const std::string> mav_comp_id_strings{{ -{ 0, "ALL" }, // Target id (target_component) used to broadcast messages to all components of the receiving system. Components should attempt to process messages with this component ID and forward to components on any other interfaces. Note: This is not a valid *source* component id for a message. -{ 1, "AUTOPILOT1" }, // System flight controller component ("autopilot"). Only one autopilot is expected in a particular system. -{ 25, "USER1" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 26, "USER2" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 27, "USER3" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 28, "USER4" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 29, "USER5" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 30, "USER6" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 31, "USER7" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 32, "USER8" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 33, "USER9" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 34, "USER10" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 35, "USER11" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 36, "USER12" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 37, "USER13" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 38, "USER14" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 39, "USER15" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 40, "USER16" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 41, "USER17" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 42, "USER18" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 43, "USER19" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 44, "USER20" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 45, "USER21" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 46, "USER22" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 47, "USER23" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 48, "USER24" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 49, "USER25" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 50, "USER26" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 51, "USER27" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 52, "USER28" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 53, "USER29" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 54, "USER30" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 55, "USER31" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 56, "USER32" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 57, "USER33" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 58, "USER34" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 59, "USER35" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 60, "USER36" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 61, "USER37" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 62, "USER38" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 63, "USER39" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 64, "USER40" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 65, "USER41" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 66, "USER42" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 67, "USER43" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 68, "TELEMETRY_RADIO" }, // Telemetry radio (e.g. SiK radio, or other component that emits RADIO_STATUS messages). -{ 69, "USER45" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 70, "USER46" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 71, "USER47" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 72, "USER48" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 73, "USER49" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 74, "USER50" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 75, "USER51" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 76, "USER52" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 77, "USER53" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 78, "USER54" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 79, "USER55" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 80, "USER56" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 81, "USER57" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 82, "USER58" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 83, "USER59" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 84, "USER60" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 85, "USER61" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 86, "USER62" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 87, "USER63" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 88, "USER64" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 89, "USER65" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 90, "USER66" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 91, "USER67" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 92, "USER68" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 93, "USER69" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 94, "USER70" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 95, "USER71" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 96, "USER72" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 97, "USER73" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 98, "USER74" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 99, "USER75" }, // Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. -{ 100, "CAMERA" }, // Camera #1. -{ 101, "CAMERA2" }, // Camera #2. -{ 102, "CAMERA3" }, // Camera #3. -{ 103, "CAMERA4" }, // Camera #4. -{ 104, "CAMERA5" }, // Camera #5. -{ 105, "CAMERA6" }, // Camera #6. -{ 140, "SERVO1" }, // Servo #1. -{ 141, "SERVO2" }, // Servo #2. -{ 142, "SERVO3" }, // Servo #3. -{ 143, "SERVO4" }, // Servo #4. -{ 144, "SERVO5" }, // Servo #5. -{ 145, "SERVO6" }, // Servo #6. -{ 146, "SERVO7" }, // Servo #7. -{ 147, "SERVO8" }, // Servo #8. -{ 148, "SERVO9" }, // Servo #9. -{ 149, "SERVO10" }, // Servo #10. -{ 150, "SERVO11" }, // Servo #11. -{ 151, "SERVO12" }, // Servo #12. -{ 152, "SERVO13" }, // Servo #13. -{ 153, "SERVO14" }, // Servo #14. -{ 154, "GIMBAL" }, // Gimbal #1. -{ 155, "LOG" }, // Logging component. -{ 156, "ADSB" }, // Automatic Dependent Surveillance-Broadcast (ADS-B) component. -{ 157, "OSD" }, // On Screen Display (OSD) devices for video links. -{ 158, "PERIPHERAL" }, // Generic autopilot peripheral component ID. Meant for devices that do not implement the parameter microservice. -{ 159, "QX1_GIMBAL" }, // Gimbal ID for QX1. -{ 160, "FLARM" }, // FLARM collision alert component. -{ 171, "GIMBAL2" }, // Gimbal #2. -{ 172, "GIMBAL3" }, // Gimbal #3. -{ 173, "GIMBAL4" }, // Gimbal #4 -{ 174, "GIMBAL5" }, // Gimbal #5. -{ 175, "GIMBAL6" }, // Gimbal #6. -{ 190, "MISSIONPLANNER" }, // Component that can generate/supply a mission flight plan (e.g. GCS or developer API). -{ 191, "ONBOARD_COMPUTER" }, // Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. -{ 195, "PATHPLANNER" }, // Component that finds an optimal path between points based on a certain constraint (e.g. minimum snap, shortest path, cost, etc.). -{ 196, "OBSTACLE_AVOIDANCE" }, // Component that plans a collision free path between two points. -{ 197, "VISUAL_INERTIAL_ODOMETRY" }, // Component that provides position estimates using VIO techniques. -{ 198, "PAIRING_MANAGER" }, // Component that manages pairing of vehicle and GCS. -{ 200, "IMU" }, // Inertial Measurement Unit (IMU) #1. -{ 201, "IMU_2" }, // Inertial Measurement Unit (IMU) #2. -{ 202, "IMU_3" }, // Inertial Measurement Unit (IMU) #3. -{ 220, "GPS" }, // GPS #1. -{ 221, "GPS2" }, // GPS #2. -{ 236, "ODID_TXRX_1" }, // Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). -{ 237, "ODID_TXRX_2" }, // Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). -{ 238, "ODID_TXRX_3" }, // Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). -{ 240, "UDP_BRIDGE" }, // Component to bridge MAVLink to UDP (i.e. from a UART). -{ 241, "UART_BRIDGE" }, // Component to bridge to UART (i.e. from UDP). -{ 242, "TUNNEL_NODE" }, // Component handling TUNNEL messages (e.g. vendor specific GUI of a component). -{ 250, "SYSTEM_CONTROL" }, // Component for handling system messages (e.g. to ARM, takeoff, etc.). +static const std::unordered_map::type, + const std::string> mav_comp_id_strings{{ + {0, "ALL"}, + {1, "AUTOPILOT1"}, + {25, "USER1"}, + {26, "USER2"}, + {27, "USER3"}, + {28, "USER4"}, + {29, "USER5"}, + {30, "USER6"}, + {31, "USER7"}, + {32, "USER8"}, + {33, "USER9"}, + {34, "USER10"}, + {35, "USER11"}, + {36, "USER12"}, + {37, "USER13"}, + {38, "USER14"}, + {39, "USER15"}, + {40, "USER16"}, + {41, "USER17"}, + {42, "USER18"}, + {43, "USER19"}, + {44, "USER20"}, + {45, "USER21"}, + {46, "USER22"}, + {47, "USER23"}, + {48, "USER24"}, + {49, "USER25"}, + {50, "USER26"}, + {51, "USER27"}, + {52, "USER28"}, + {53, "USER29"}, + {54, "USER30"}, + {55, "USER31"}, + {56, "USER32"}, + {57, "USER33"}, + {58, "USER34"}, + {59, "USER35"}, + {60, "USER36"}, + {61, "USER37"}, + {62, "USER38"}, + {63, "USER39"}, + {64, "USER40"}, + {65, "USER41"}, + {66, "USER42"}, + {67, "USER43"}, + {68, "TELEMETRY_RADIO"}, + {69, "USER45"}, + {70, "USER46"}, + {71, "USER47"}, + {72, "USER48"}, + {73, "USER49"}, + {74, "USER50"}, + {75, "USER51"}, + {76, "USER52"}, + {77, "USER53"}, + {78, "USER54"}, + {79, "USER55"}, + {80, "USER56"}, + {81, "USER57"}, + {82, "USER58"}, + {83, "USER59"}, + {84, "USER60"}, + {85, "USER61"}, + {86, "USER62"}, + {87, "USER63"}, + {88, "USER64"}, + {89, "USER65"}, + {90, "USER66"}, + {91, "USER67"}, + {92, "USER68"}, + {93, "USER69"}, + {94, "USER70"}, + {95, "USER71"}, + {96, "USER72"}, + {97, "USER73"}, + {98, "USER74"}, + {99, "USER75"}, + {100, "CAMERA"}, + {101, "CAMERA2"}, + {102, "CAMERA3"}, + {103, "CAMERA4"}, + {104, "CAMERA5"}, + {105, "CAMERA6"}, + {140, "SERVO1"}, + {141, "SERVO2"}, + {142, "SERVO3"}, + {143, "SERVO4"}, + {144, "SERVO5"}, + {145, "SERVO6"}, + {146, "SERVO7"}, + {147, "SERVO8"}, + {148, "SERVO9"}, + {149, "SERVO10"}, + {150, "SERVO11"}, + {151, "SERVO12"}, + {152, "SERVO13"}, + {153, "SERVO14"}, + {154, "GIMBAL"}, + {155, "LOG"}, + {156, "ADSB"}, + {157, "OSD"}, + {158, "PERIPHERAL"}, + {159, "QX1_GIMBAL"}, + {160, "FLARM"}, + {161, "PARACHUTE"}, + {171, "GIMBAL2"}, + {172, "GIMBAL3"}, + {173, "GIMBAL4"}, + {174, "GIMBAL5"}, + {175, "GIMBAL6"}, + {180, "BATTERY"}, + {181, "BATTERY2"}, + {189, "MAVCAN"}, + {190, "MISSIONPLANNER"}, + {191, "ONBOARD_COMPUTER"}, + {192, "ONBOARD_COMPUTER2"}, + {193, "ONBOARD_COMPUTER3"}, + {194, "ONBOARD_COMPUTER4"}, + {195, "PATHPLANNER"}, + {196, "OBSTACLE_AVOIDANCE"}, + {197, "VISUAL_INERTIAL_ODOMETRY"}, + {198, "PAIRING_MANAGER"}, + {200, "IMU"}, + {201, "IMU_2"}, + {202, "IMU_3"}, + {220, "GPS"}, + {221, "GPS2"}, + {236, "ODID_TXRX_1"}, + {237, "ODID_TXRX_2"}, + {238, "ODID_TXRX_3"}, + {240, "UDP_BRIDGE"}, + {241, "UART_BRIDGE"}, + {242, "TUNNEL_NODE"}, + {250, "SYSTEM_CONTROL"}, }}; -// [[[end]]] (checksum: 2dc119e3e20f7668a71e8649ba3f67b6) +// [[[end]]] (checksum: 3a66ba989a8794aff36d5dbadae852d8) std::string to_string(MAV_COMPONENT e) { - auto idx = enum_value(e); - auto it = mav_comp_id_strings.find(idx); + size_t idx = enum_value(e); + auto it = mav_comp_id_strings.find(idx); - if (it == mav_comp_id_strings.end()) - return std::to_string(idx); + if (it == mav_comp_id_strings.end()) { + return std::to_string(idx); + } - return it->second; + return it->second; } -MAV_FRAME mav_frame_from_str(const std::string &mav_frame) +MAV_FRAME mav_frame_from_str(const std::string & mav_frame) { - for (size_t idx = 0; idx < mav_frame_strings.size(); idx++) { - if (mav_frame_strings[idx] == mav_frame) { - std::underlying_type::type rv = idx; - return static_cast(rv); - } - } - - ROS_ERROR_STREAM_NAMED("uas", "FRAME: Unknown MAV_FRAME: " << mav_frame); - return MAV_FRAME::LOCAL_NED; + for (size_t idx = 0; idx < mav_frame_strings.size(); idx++) { + if (mav_frame_strings[idx] == mav_frame) { + std::underlying_type::type rv = idx; + return static_cast(rv); + } + } + + RCLCPP_ERROR_STREAM(logger, "FRAME: Unknown MAV_FRAME: " << mav_frame); + return MAV_FRAME::LOCAL_NED; } -MAV_TYPE mav_type_from_str(const std::string &mav_type) +MAV_TYPE mav_type_from_str(const std::string & mav_type) { - for (size_t idx = 0; idx < mav_type_names.size(); idx++) { - if (mav_type_names[idx] == mav_type) { - std::underlying_type::type rv = idx; - return static_cast(rv); - } - } - ROS_ERROR_STREAM_NAMED("uas", "TYPE: Unknown MAV_TYPE: " << mav_type); - return MAV_TYPE::GENERIC; + for (size_t idx = 0; idx < mav_type_names.size(); idx++) { + if (mav_type_names[idx] == mav_type) { + std::underlying_type::type rv = idx; + return static_cast(rv); + } + } + RCLCPP_ERROR_STREAM(logger, "TYPE: Unknown MAV_TYPE: " << mav_type); + return MAV_TYPE::GENERIC; } // [[[cog: // ename = 'MAV_DISTANCE_SENSOR' -// enum_name_is_value_outl(ename) +// enum_value_is_name_outl(ename) // ]]] + //! MAV_DISTANCE_SENSOR values static const std::array mav_distance_sensor_strings{{ -/* 0 */ "LASER", // Laser rangefinder, e.g. LightWare SF02/F or PulsedLight units -/* 1 */ "ULTRASOUND", // Ultrasound rangefinder, e.g. MaxBotix units -/* 2 */ "INFRARED", // Infrared rangefinder, e.g. Sharp units -/* 3 */ "RADAR", // Radar type, e.g. uLanding units -/* 4 */ "UNKNOWN", // Broken or unknown type, e.g. analog units +/* 0 */ "LASER", +/* 1 */ "ULTRASOUND", +/* 2 */ "INFRARED", +/* 3 */ "RADAR", +/* 4 */ "UNKNOWN", }}; std::string to_string(MAV_DISTANCE_SENSOR e) { - size_t idx = enum_value(e); - if (idx >= mav_distance_sensor_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= mav_distance_sensor_strings.size()) { + return std::to_string(idx); + } - return mav_distance_sensor_strings[idx]; + return mav_distance_sensor_strings[idx]; } -// [[[end]]] (checksum: 3f792ad01cdb3f2315a8907f578ab5b3) +// [[[end]]] (checksum: dda871f638e51a30d2ecd3b0d063c0de) // [[[cog: // ename = 'LANDING_TARGET_TYPE' -// enum_name_is_value_outl(ename) +// enum_value_is_name_outl(ename) // ]]] + //! LANDING_TARGET_TYPE values static const std::array landing_target_type_strings{{ -/* 0 */ "LIGHT_BEACON", // Landing target signaled by light beacon (ex: IR-LOCK) -/* 1 */ "RADIO_BEACON", // Landing target signaled by radio beacon (ex: ILS, NDB) -/* 2 */ "VISION_FIDUCIAL", // Landing target represented by a fiducial marker (ex: ARTag) -/* 3 */ "VISION_OTHER", // Landing target represented by a pre-defined visual shape/feature (ex: X-marker, H-marker, square) +/* 0 */ "LIGHT_BEACON", +/* 1 */ "RADIO_BEACON", +/* 2 */ "VISION_FIDUCIAL", +/* 3 */ "VISION_OTHER", }}; std::string to_string(LANDING_TARGET_TYPE e) { - size_t idx = enum_value(e); - if (idx >= landing_target_type_strings.size()) - return std::to_string(idx); + size_t idx = enum_value(e); + if (idx >= landing_target_type_strings.size()) { + return std::to_string(idx); + } - return landing_target_type_strings[idx]; + return landing_target_type_strings[idx]; } -// [[[end]]] (checksum: a42789c10cbebd5bc253abca2a07289b) +// [[[end]]] (checksum: f582577481c6b17014ed9925665f7634) -LANDING_TARGET_TYPE landing_target_type_from_str(const std::string &landing_target_type) +LANDING_TARGET_TYPE landing_target_type_from_str(const std::string & landing_target_type) { - for (size_t idx = 0; idx < landing_target_type_strings.size(); idx++) { - if (landing_target_type_strings[idx] == landing_target_type) { - std::underlying_type::type rv = idx; - return static_cast(rv); - } - } - ROS_ERROR_STREAM_NAMED("uas", "TYPE: Unknown LANDING_TARGET_TYPE: " << landing_target_type << ". Defaulting to LIGHT_BEACON"); - return LANDING_TARGET_TYPE::LIGHT_BEACON; + for (size_t idx = 0; idx < landing_target_type_strings.size(); idx++) { + if (landing_target_type_strings[idx] == landing_target_type) { + std::underlying_type::type rv = idx; + return static_cast(rv); + } + } + + RCLCPP_ERROR_STREAM( + logger, + "TYPE: Unknown LANDING_TARGET_TYPE: " << landing_target_type << + ". Defaulting to LIGHT_BEACON"); + return LANDING_TARGET_TYPE::LIGHT_BEACON; } -} // namespace utils -} // namespace mavros +} // namespace utils +} // namespace mavros diff --git a/mavros/src/lib/ftf_frame_conversions.cpp b/mavros/src/lib/ftf_frame_conversions.cpp index e4fa9ac4e..3f689403c 100644 --- a/mavros/src/lib/ftf_frame_conversions.cpp +++ b/mavros/src/lib/ftf_frame_conversions.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2015 Nuno Marques. + * Copyright 2016,2021 Vladimir Ermakov + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Frame conversions helper functions * @file uas_frame_conversions.cpp @@ -7,21 +15,16 @@ * @addtogroup nodelib * @{ */ -/* - * Copyright 2015 Nuno Marques. - * Copyright 2016 Vladimir Ermakov - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include #include -namespace mavros { -namespace ftf { -namespace detail { +namespace mavros +{ +namespace ftf +{ +namespace detail +{ /** * @brief Static quaternion needed for rotating between ENU and NED frames * NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East) @@ -62,8 +65,8 @@ static const auto AIRCRAFT_BASELINK_R = AIRCRAFT_BASELINK_Q.normalized().toRotat * to avoid NaN/Inf floating point pollution across different axes * since in NED <-> ENU the axes are perfectly aligned. */ -static const Eigen::PermutationMatrix<3> NED_ENU_REFLECTION_XY(Eigen::Vector3i(1,0,2)); -static const Eigen::DiagonalMatrix NED_ENU_REFLECTION_Z(1,1,-1); +static const Eigen::PermutationMatrix<3> NED_ENU_REFLECTION_XY(Eigen::Vector3i(1, 0, 2)); +static const Eigen::DiagonalMatrix NED_ENU_REFLECTION_Z(1, 1, -1); /** * @brief Auxiliar matrices to Covariance transforms @@ -72,236 +75,245 @@ using Matrix6d = Eigen::Matrix; using Matrix9d = Eigen::Matrix; -Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const StaticTF transform) +Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond & q, const StaticTF transform) { - // Transform the attitude representation from frame to frame. - // The proof for this transform can be seen - // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ - switch (transform) { - case StaticTF::NED_TO_ENU: - case StaticTF::ENU_TO_NED: - return NED_ENU_Q * q; - - case StaticTF::AIRCRAFT_TO_BASELINK: - case StaticTF::BASELINK_TO_AIRCRAFT: - return q * AIRCRAFT_BASELINK_Q; - - case StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK: - case StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT: - return AIRCRAFT_BASELINK_Q * q; - - default: - ROS_FATAL("unsupported StaticTF mode"); - return q; - } + // Transform the attitude representation from frame to frame. + // The proof for this transform can be seen + // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ + switch (transform) { + case StaticTF::NED_TO_ENU: + case StaticTF::ENU_TO_NED: + return NED_ENU_Q * q; + + case StaticTF::AIRCRAFT_TO_BASELINK: + case StaticTF::BASELINK_TO_AIRCRAFT: + return q * AIRCRAFT_BASELINK_Q; + + case StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK: + case StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT: + return AIRCRAFT_BASELINK_Q * q; + + default: + rcpputils::require_true(false, "unsupported transform arg"); + return q; + } } -Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticTF transform) +Eigen::Vector3d transform_static_frame(const Eigen::Vector3d & vec, const StaticTF transform) { - switch (transform) { - case StaticTF::NED_TO_ENU: - case StaticTF::ENU_TO_NED: - return NED_ENU_REFLECTION_XY * (NED_ENU_REFLECTION_Z * vec); - - case StaticTF::AIRCRAFT_TO_BASELINK: - case StaticTF::BASELINK_TO_AIRCRAFT: - return AIRCRAFT_BASELINK_AFFINE * vec; - - default: - ROS_FATAL("unsupported StaticTF mode"); - return vec; - } + switch (transform) { + case StaticTF::NED_TO_ENU: + case StaticTF::ENU_TO_NED: + return NED_ENU_REFLECTION_XY * (NED_ENU_REFLECTION_Z * vec); + + case StaticTF::AIRCRAFT_TO_BASELINK: + case StaticTF::BASELINK_TO_AIRCRAFT: + return AIRCRAFT_BASELINK_AFFINE * vec; + + default: + rcpputils::require_true(false, "unsupported transform arg"); + return vec; + } } -Covariance3d transform_static_frame(const Covariance3d &cov, const StaticTF transform) +Covariance3d transform_static_frame(const Covariance3d & cov, const StaticTF transform) { - Covariance3d cov_out_; - EigenMapConstCovariance3d cov_in(cov.data()); - EigenMapCovariance3d cov_out(cov_out_.data()); - - switch (transform) { - case StaticTF::NED_TO_ENU: - case StaticTF::ENU_TO_NED: - cov_out = NED_ENU_REFLECTION_XY * (NED_ENU_REFLECTION_Z * cov_in * NED_ENU_REFLECTION_Z) * - NED_ENU_REFLECTION_XY.transpose(); - return cov_out_; - - case StaticTF::AIRCRAFT_TO_BASELINK: - case StaticTF::BASELINK_TO_AIRCRAFT: - cov_out = cov_in * AIRCRAFT_BASELINK_Q; - return cov_out_; - - default: - ROS_FATAL("unsupported StaticTF mode"); - return cov; - } + Covariance3d cov_out_; + EigenMapConstCovariance3d cov_in(cov.data()); + EigenMapCovariance3d cov_out(cov_out_.data()); + + switch (transform) { + case StaticTF::NED_TO_ENU: + case StaticTF::ENU_TO_NED: + cov_out = NED_ENU_REFLECTION_XY * (NED_ENU_REFLECTION_Z * cov_in * NED_ENU_REFLECTION_Z) * + NED_ENU_REFLECTION_XY.transpose(); + return cov_out_; + + case StaticTF::AIRCRAFT_TO_BASELINK: + case StaticTF::BASELINK_TO_AIRCRAFT: + cov_out = cov_in * AIRCRAFT_BASELINK_Q; + return cov_out_; + + default: + rcpputils::require_true(false, "unsupported transform arg"); + return cov_out_; + } } -Covariance6d transform_static_frame(const Covariance6d &cov, const StaticTF transform) +Covariance6d transform_static_frame(const Covariance6d & cov, const StaticTF transform) { - Covariance6d cov_out_; - Matrix6d R = Matrix6d::Zero(); // not `auto` because Zero ret is const - - EigenMapConstCovariance6d cov_in(cov.data()); - EigenMapCovariance6d cov_out(cov_out_.data()); - - switch (transform) { - case StaticTF::NED_TO_ENU: - case StaticTF::ENU_TO_NED: - { - Eigen::PermutationMatrix<6> NED_ENU_REFLECTION_XY_6 (NED_ENU_REFLECTION_XY.indices().replicate<2,1>()); - NED_ENU_REFLECTION_XY_6.indices().middleRows<3>(3).array() += 3; - Eigen::DiagonalMatrix NED_ENU_REFLECTION_Z_6(NED_ENU_REFLECTION_Z.diagonal().replicate<2,1>()); - - cov_out = NED_ENU_REFLECTION_XY_6 * (NED_ENU_REFLECTION_Z_6 * cov_in * NED_ENU_REFLECTION_Z_6) * - NED_ENU_REFLECTION_XY_6.transpose(); - - return cov_out_; - } - case StaticTF::AIRCRAFT_TO_BASELINK: - case StaticTF::BASELINK_TO_AIRCRAFT: - R.block<3, 3>(0, 0) = - R.block<3, 3>(3, 3) = AIRCRAFT_BASELINK_R; - - cov_out = R * cov_in * R.transpose(); - return cov_out_; - - default: - ROS_FATAL("unsupported StaticTF mode"); - return cov; - } + Covariance6d cov_out_; + Matrix6d R = Matrix6d::Zero(); // not `auto` because Zero ret is const + + EigenMapConstCovariance6d cov_in(cov.data()); + EigenMapCovariance6d cov_out(cov_out_.data()); + + switch (transform) { + case StaticTF::NED_TO_ENU: + case StaticTF::ENU_TO_NED: + { + Eigen::PermutationMatrix<6> NED_ENU_REFLECTION_XY_6( + NED_ENU_REFLECTION_XY.indices().replicate<2, 1>()); + NED_ENU_REFLECTION_XY_6.indices().middleRows<3>(3).array() += 3; + Eigen::DiagonalMatrix NED_ENU_REFLECTION_Z_6( + NED_ENU_REFLECTION_Z.diagonal().replicate<2, 1>()); + + cov_out = NED_ENU_REFLECTION_XY_6 * + (NED_ENU_REFLECTION_Z_6 * cov_in * NED_ENU_REFLECTION_Z_6) * + NED_ENU_REFLECTION_XY_6.transpose(); + + return cov_out_; + } + case StaticTF::AIRCRAFT_TO_BASELINK: + case StaticTF::BASELINK_TO_AIRCRAFT: + R.block<3, 3>(0, 0) = + R.block<3, 3>(3, 3) = AIRCRAFT_BASELINK_R; + + cov_out = R * cov_in * R.transpose(); + return cov_out_; + + default: + rcpputils::require_true(false, "unsupported transform arg"); + return cov_out_; + } } -Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF transform) +Covariance9d transform_static_frame(const Covariance9d & cov, const StaticTF transform) { - Covariance9d cov_out_; - Matrix9d R = Matrix9d::Zero(); - - EigenMapConstCovariance9d cov_in(cov.data()); - EigenMapCovariance9d cov_out(cov_out_.data()); - - switch (transform) { - case StaticTF::NED_TO_ENU: - case StaticTF::ENU_TO_NED: - { - Eigen::PermutationMatrix<9> NED_ENU_REFLECTION_XY_9 (NED_ENU_REFLECTION_XY.indices().replicate<3,1>()); - NED_ENU_REFLECTION_XY_9.indices().middleRows<3>(3).array() += 3; - NED_ENU_REFLECTION_XY_9.indices().middleRows<3>(6).array() += 6; - Eigen::DiagonalMatrix NED_ENU_REFLECTION_Z_9(NED_ENU_REFLECTION_Z.diagonal().replicate<3,1>()); - - cov_out = NED_ENU_REFLECTION_XY_9 * (NED_ENU_REFLECTION_Z_9 * cov_in * NED_ENU_REFLECTION_Z_9) * - NED_ENU_REFLECTION_XY_9.transpose(); - - return cov_out_; - } - case StaticTF::AIRCRAFT_TO_BASELINK: - case StaticTF::BASELINK_TO_AIRCRAFT: - R.block<3, 3>(0, 0) = - R.block<3, 3>(3, 3) = - R.block<3, 3>(6, 6) = AIRCRAFT_BASELINK_R; - - cov_out = R * cov_in * R.transpose(); - return cov_out_; - - default: - ROS_FATAL("unsupported StaticTF mode"); - return cov; - } + Covariance9d cov_out_; + Matrix9d R = Matrix9d::Zero(); + + EigenMapConstCovariance9d cov_in(cov.data()); + EigenMapCovariance9d cov_out(cov_out_.data()); + + switch (transform) { + case StaticTF::NED_TO_ENU: + case StaticTF::ENU_TO_NED: + { + Eigen::PermutationMatrix<9> NED_ENU_REFLECTION_XY_9( + NED_ENU_REFLECTION_XY.indices().replicate<3, 1>()); + NED_ENU_REFLECTION_XY_9.indices().middleRows<3>(3).array() += 3; + NED_ENU_REFLECTION_XY_9.indices().middleRows<3>(6).array() += 6; + Eigen::DiagonalMatrix NED_ENU_REFLECTION_Z_9( + NED_ENU_REFLECTION_Z.diagonal().replicate<3, 1>()); + + cov_out = NED_ENU_REFLECTION_XY_9 * + (NED_ENU_REFLECTION_Z_9 * cov_in * NED_ENU_REFLECTION_Z_9) * + NED_ENU_REFLECTION_XY_9.transpose(); + + return cov_out_; + } + case StaticTF::AIRCRAFT_TO_BASELINK: + case StaticTF::BASELINK_TO_AIRCRAFT: + R.block<3, 3>(0, 0) = + R.block<3, 3>(3, 3) = + R.block<3, 3>(6, 6) = AIRCRAFT_BASELINK_R; + + cov_out = R * cov_in * R.transpose(); + return cov_out_; + + default: + rcpputils::require_true(false, "unsupported transform arg"); + return cov_out_; + } } -Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, const StaticEcefTF transform) +Eigen::Vector3d transform_static_frame( + const Eigen::Vector3d & vec, + const Eigen::Vector3d & map_origin, + const StaticEcefTF transform) { - //! Degrees to radians - static constexpr double DEG_TO_RAD = (M_PI / 180.0); - - // Don't forget to convert from degrees to radians - const double sin_lat = std::sin(map_origin.x() * DEG_TO_RAD); - const double sin_lon = std::sin(map_origin.y() * DEG_TO_RAD); - const double cos_lat = std::cos(map_origin.x() * DEG_TO_RAD); - const double cos_lon = std::cos(map_origin.y() * DEG_TO_RAD); - - /** - * @brief Compute transform from ECEF to ENU: - * http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates - * ϕ = latitude - * λ = longitude - * The rotation is composed by a counter-clockwise rotation over the Z-axis - * by an angle of 90 + λ followed by a counter-clockwise rotation over the east-axis by - * an angle of 90 - ϕ. - * R = [-sinλ cosλ 0.0 - * -cosλ*sinϕ -sinλ*sinϕ cosϕ - * cosλ*cosϕ sinλ*cosϕ sinϕ ] - * [East, North, Up] = R * [∂x, ∂y, ∂z] - * where both [East, North, Up] and [∂x, ∂y, ∂z] are local coordinates relative to map origin. - */ - Eigen::Matrix3d R; - R << -sin_lon, cos_lon, 0.0, - -cos_lon * sin_lat, -sin_lon * sin_lat, cos_lat, - cos_lon * cos_lat, sin_lon * cos_lat, sin_lat; - - switch (transform) { - case StaticEcefTF::ECEF_TO_ENU: - return R * vec; - - case StaticEcefTF::ENU_TO_ECEF: - // ENU to ECEF rotation is just an inverse rotation from ECEF to ENU, which means transpose. - R.transposeInPlace(); - return R * vec; - - default: - ROS_FATAL("unsupported StaticTF mode"); - return vec; - } + //! Degrees to radians + static constexpr double DEG_TO_RAD = (M_PI / 180.0); + + // Don't forget to convert from degrees to radians + const double sin_lat = std::sin(map_origin.x() * DEG_TO_RAD); + const double sin_lon = std::sin(map_origin.y() * DEG_TO_RAD); + const double cos_lat = std::cos(map_origin.x() * DEG_TO_RAD); + const double cos_lon = std::cos(map_origin.y() * DEG_TO_RAD); + + /** + * @brief Compute transform from ECEF to ENU: + * http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates + * ϕ = latitude + * λ = longitude + * The rotation is composed by a counter-clockwise rotation over the Z-axis + * by an angle of 90 + λ followed by a counter-clockwise rotation over the east-axis by + * an angle of 90 - ϕ. + * R = [-sinλ cosλ 0.0 + * -cosλ*sinϕ -sinλ*sinϕ cosϕ + * cosλ*cosϕ sinλ*cosϕ sinϕ ] + * [East, North, Up] = R * [∂x, ∂y, ∂z] + * where both [East, North, Up] and [∂x, ∂y, ∂z] are local coordinates relative to map origin. + */ + Eigen::Matrix3d R; + R << -sin_lon, cos_lon, 0.0, + -cos_lon * sin_lat, -sin_lon * sin_lat, cos_lat, + cos_lon * cos_lat, sin_lon * cos_lat, sin_lat; + + switch (transform) { + case StaticEcefTF::ECEF_TO_ENU: + return R * vec; + + case StaticEcefTF::ENU_TO_ECEF: + // ENU to ECEF rotation is just an inverse rotation from ECEF to ENU, which means transpose. + R.transposeInPlace(); + return R * vec; + + default: + rcpputils::require_true(false, "unsupported transform arg"); + return vec; + } } -Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q) +Eigen::Vector3d transform_frame(const Eigen::Vector3d & vec, const Eigen::Quaterniond & q) { - Eigen::Affine3d transformation(q); - return transformation * vec; + Eigen::Affine3d transformation(q); + return transformation * vec; } -Covariance3d transform_frame(const Covariance3d &cov, const Eigen::Quaterniond &q) +Covariance3d transform_frame(const Covariance3d & cov, const Eigen::Quaterniond & q) { - Covariance3d cov_out_; - EigenMapConstCovariance3d cov_in(cov.data()); - EigenMapCovariance3d cov_out(cov_out_.data()); + Covariance3d cov_out_; + EigenMapConstCovariance3d cov_in(cov.data()); + EigenMapCovariance3d cov_out(cov_out_.data()); - cov_out = cov_in * q; - return cov_out_; + cov_out = cov_in * q; + return cov_out_; } -Covariance6d transform_frame(const Covariance6d &cov, const Eigen::Quaterniond &q) +Covariance6d transform_frame(const Covariance6d & cov, const Eigen::Quaterniond & q) { - Covariance6d cov_out_; - Matrix6d R = Matrix6d::Zero(); + Covariance6d cov_out_; + Matrix6d R = Matrix6d::Zero(); - EigenMapConstCovariance6d cov_in(cov.data()); - EigenMapCovariance6d cov_out(cov_out_.data()); + EigenMapConstCovariance6d cov_in(cov.data()); + EigenMapCovariance6d cov_out(cov_out_.data()); - R.block<3, 3>(0, 0) = - R.block<3, 3>(3, 3) = q.normalized().toRotationMatrix(); + R.block<3, 3>(0, 0) = + R.block<3, 3>(3, 3) = q.normalized().toRotationMatrix(); - cov_out = R * cov_in * R.transpose(); - return cov_out_; + cov_out = R * cov_in * R.transpose(); + return cov_out_; } -Covariance9d transform_frame(const Covariance9d &cov, const Eigen::Quaterniond &q) +Covariance9d transform_frame(const Covariance9d & cov, const Eigen::Quaterniond & q) { - Covariance9d cov_out_; - Matrix9d R = Matrix9d::Zero(); + Covariance9d cov_out_; + Matrix9d R = Matrix9d::Zero(); - EigenMapConstCovariance9d cov_in(cov.data()); - EigenMapCovariance9d cov_out(cov_out_.data()); + EigenMapConstCovariance9d cov_in(cov.data()); + EigenMapCovariance9d cov_out(cov_out_.data()); - R.block<3, 3>(0, 0) = - R.block<3, 3>(3, 3) = - R.block<3, 3>(6, 6) = q.normalized().toRotationMatrix(); + R.block<3, 3>(0, 0) = + R.block<3, 3>(3, 3) = + R.block<3, 3>(6, 6) = q.normalized().toRotationMatrix(); - cov_out = R * cov_in * R.transpose(); - return cov_out_; + cov_out = R * cov_in * R.transpose(); + return cov_out_; } -} // namespace detail -} // namespace ftf -} // namespace mavros +} // namespace detail +} // namespace ftf +} // namespace mavros diff --git a/mavros/src/lib/ftf_quaternion_utils.cpp b/mavros/src/lib/ftf_quaternion_utils.cpp index 257a65295..e546c08dd 100644 --- a/mavros/src/lib/ftf_quaternion_utils.cpp +++ b/mavros/src/lib/ftf_quaternion_utils.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2015,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Eigen::Quaternion helter functions * @file uas_quaternion_utils.cpp @@ -6,17 +13,13 @@ * @addtogroup nodelib * @{ */ -/* - * Copyright 2015,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -namespace mavros { -namespace ftf { +#include + +namespace mavros +{ +namespace ftf +{ /* * Note: order of axis are match tf2::LinearMath (bullet). @@ -24,33 +27,33 @@ namespace ftf { * Compatibility checked by unittests. */ -Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy) +Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d & rpy) { - // YPR - ZYX - return Eigen::Quaterniond( - Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()) - ); + // YPR - ZYX + return Eigen::Quaterniond( + Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()) + ); } -Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q) +Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond & q) { - // YPR - ZYX - return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse(); + // YPR - ZYX + return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse(); } -double quaternion_get_yaw(const Eigen::Quaterniond &q) +double quaternion_get_yaw(const Eigen::Quaterniond & q) { - // to match equation from: - // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles - const double &q0 = q.w(); - const double &q1 = q.x(); - const double &q2 = q.y(); - const double &q3 = q.z(); - - return std::atan2(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3)); + // to match equation from: + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + const double & q0 = q.w(); + const double & q1 = q.x(); + const double & q2 = q.y(); + const double & q3 = q.z(); + + return std::atan2(2. * (q0 * q3 + q1 * q2), 1. - 2. * (q2 * q2 + q3 * q3)); } -} // namesapace ftf -} // namesapace mavros +} // namespace ftf +} // namespace mavros diff --git a/mavros/src/lib/mavlink_diag.cpp b/mavros/src/lib/mavlink_diag.cpp deleted file mode 100644 index c5f9ee885..000000000 --- a/mavros/src/lib/mavlink_diag.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/** - * @brief Mavlink diag class - * @file mavlink_diag.cpp - * @author Vladimir Ermakov - */ -/* - * Copyright 2014,2015 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include - -using namespace mavros; - -MavlinkDiag::MavlinkDiag(std::string name) : - diagnostic_updater::DiagnosticTask(name), - last_drop_count(0), - is_connected(false) -{ }; - -void MavlinkDiag::run(diagnostic_updater::DiagnosticStatusWrapper &stat) -{ - if (auto link = weak_link.lock()) { - auto mav_status = link->get_status(); - auto iostat = link->get_iostat(); - - stat.addf("Received packets:", "%u", mav_status.packet_rx_success_count); - stat.addf("Dropped packets:", "%u", mav_status.packet_rx_drop_count); - stat.addf("Buffer overruns:", "%u", mav_status.buffer_overrun); - stat.addf("Parse errors:", "%u", mav_status.parse_error); - stat.addf("Rx sequence number:", "%u", mav_status.current_rx_seq); - stat.addf("Tx sequence number:", "%u", mav_status.current_tx_seq); - - stat.addf("Rx total bytes:", "%u", iostat.rx_total_bytes); - stat.addf("Tx total bytes:", "%u", iostat.tx_total_bytes); - stat.addf("Rx speed:", "%f", iostat.rx_speed); - stat.addf("Tx speed:", "%f", iostat.tx_speed); - - if (mav_status.packet_rx_drop_count > last_drop_count) - stat.summaryf(1, "%d packeges dropped since last report", - mav_status.packet_rx_drop_count - last_drop_count); - else if (is_connected) - stat.summary(0, "connected"); - else - // link operational, but not connected - stat.summary(1, "not connected"); - - last_drop_count = mav_status.packet_rx_drop_count; - } else { - stat.summary(2, "not connected"); - } -} - diff --git a/mavros/src/lib/mavros.cpp b/mavros/src/lib/mavros.cpp deleted file mode 100644 index aff0987e6..000000000 --- a/mavros/src/lib/mavros.cpp +++ /dev/null @@ -1,403 +0,0 @@ -/** - * @brief MAVROS class - * @file mavros.cpp - * @author Vladimir Ermakov - */ -/* - * Copyright 2013,2014,2015,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include -#include -#include -#include - -// MAVLINK_VERSION string -#include -#include - -#include -#include - -using namespace mavros; -using mavconn::MAVConnInterface; -using mavconn::Framing; -using mavlink::mavlink_message_t; -using plugin::PluginBase; -using utils::enum_value; - - -MavRos::MavRos() : - mavlink_nh("mavlink"), // allow to namespace it - last_message_received_from_gcs(0), - fcu_link_diag("FCU connection"), - gcs_link_diag("GCS bridge"), - plugin_loader("mavros", "mavros::plugin::PluginBase"), - plugin_subscriptions{} -{ - std::string fcu_url, gcs_url; - std::string fcu_protocol; - int system_id, component_id; - int tgt_system_id, tgt_component_id; - bool px4_usb_quirk; - double conn_timeout_d; - ros::V_string plugin_blacklist{}, plugin_whitelist{}; - MAVConnInterface::Ptr fcu_link; - - ros::NodeHandle nh("~"); - - nh.param("fcu_url", fcu_url, "serial:///dev/ttyACM0"); - nh.param("gcs_url", gcs_url, "udp://@"); - nh.param("gcs_quiet_mode", gcs_quiet_mode, false); - nh.param("conn/timeout", conn_timeout_d, 30.0); - - nh.param("fcu_protocol", fcu_protocol, "v2.0"); - nh.param("system_id", system_id, 1); - nh.param("component_id", component_id, mavconn::MAV_COMP_ID_UDP_BRIDGE); - nh.param("target_system_id", tgt_system_id, 1); - nh.param("target_component_id", tgt_component_id, 1); - nh.param("startup_px4_usb_quirk", px4_usb_quirk, false); - nh.getParam("plugin_blacklist", plugin_blacklist); - nh.getParam("plugin_whitelist", plugin_whitelist); - - conn_timeout = ros::Duration(conn_timeout_d); - - // Now we use FCU URL as a hardware Id - UAS_DIAG(&mav_uas).setHardwareID(fcu_url); - - ROS_INFO_STREAM("FCU URL: " << fcu_url); - try { - fcu_link = MAVConnInterface::open_url_no_connect(fcu_url, system_id, component_id); - // may be overridden by URL - system_id = fcu_link->get_system_id(); - component_id = fcu_link->get_component_id(); - - fcu_link_diag.set_mavconn(fcu_link); - UAS_DIAG(&mav_uas).add(fcu_link_diag); - } - catch (mavconn::DeviceError &ex) { - ROS_FATAL("FCU: %s", ex.what()); - ros::shutdown(); - return; - } - - if (fcu_protocol == "v1.0") { - fcu_link->set_protocol_version(mavconn::Protocol::V10); - } - else if (fcu_protocol == "v2.0") { - fcu_link->set_protocol_version(mavconn::Protocol::V20); - } - //else if (fcu_protocol == "auto") { // XXX TODO - // fcu_link->set_protocol_version(mavconn::Protocol::V20); - //} - else { - ROS_WARN("Unknown FCU protocol: \"%s\", should be: \"v1.0\" or \"v2.0\". Used default v1.0.", fcu_protocol.c_str()); - fcu_link->set_protocol_version(mavconn::Protocol::V10); - } - - if (gcs_url != "") { - ROS_INFO_STREAM("GCS URL: " << gcs_url); - try { - gcs_link = MAVConnInterface::open_url_no_connect(gcs_url, system_id, component_id); - - gcs_link_diag.set_mavconn(gcs_link); - gcs_diag_updater.setHardwareID(gcs_url); - gcs_diag_updater.add(gcs_link_diag); - } - catch (mavconn::DeviceError &ex) { - ROS_FATAL("GCS: %s", ex.what()); - ros::shutdown(); - return; - } - } - else - ROS_INFO("GCS bridge disabled"); - - // ROS mavlink bridge - mavlink_pub = mavlink_nh.advertise("from", 100); - mavlink_sub = mavlink_nh.subscribe("to", 100, &MavRos::mavlink_sub_cb, this, - ros::TransportHints() - .unreliable().maxDatagramSize(1024) - .reliable()); - - // setup UAS and diag - mav_uas.set_tgt(tgt_system_id, tgt_component_id); - UAS_FCU(&mav_uas) = fcu_link; - - mav_uas.add_connection_change_handler(std::bind(&MavlinkDiag::set_connection_status, &fcu_link_diag, std::placeholders::_1)); - mav_uas.add_connection_change_handler(std::bind(&MavRos::log_connect_change, this, std::placeholders::_1)); - - // prepare plugin lists - // issue #257 2: assume that all plugins blacklisted - if (plugin_blacklist.empty() and !plugin_whitelist.empty()) - plugin_blacklist.emplace_back("*"); - - for (auto &name : plugin_loader.getDeclaredClasses()) - add_plugin(name, plugin_blacklist, plugin_whitelist); - - // connect FCU link - - // XXX TODO: move workers to ROS Spinner, let mavconn threads to do only IO - fcu_link->connect( - [this](const mavlink_message_t *msg, const Framing framing) { - mavlink_pub_cb(msg, framing); - plugin_route_cb(msg, framing); - - if (gcs_link) { - if (this->gcs_quiet_mode && msg->msgid != mavlink::minimal::msg::HEARTBEAT::MSG_ID && - (ros::Time::now() - this->last_message_received_from_gcs > this->conn_timeout)) { - return; - } - - gcs_link->send_message_ignore_drop(msg); - } - }, - []() { - ROS_ERROR("FCU connection closed, mavros will be terminated."); - ros::requestShutdown(); - }); - - if (gcs_link) { - // setup GCS link bridge - gcs_link->connect([this, fcu_link](const mavlink_message_t *msg, const Framing framing) { - this->last_message_received_from_gcs = ros::Time::now(); - fcu_link->send_message_ignore_drop(msg); - }); - - gcs_link_diag.set_connection_status(true); - } - - if (px4_usb_quirk) - startup_px4_usb_quirk(); - - std::stringstream ss; - for (auto &s : mavconn::MAVConnInterface::get_known_dialects()) - ss << " " << s; - - ROS_INFO("Built-in SIMD instructions: %s", Eigen::SimdInstructionSetsInUse()); - ROS_INFO("Built-in MAVLink package version: %s", MAVLINK_VERSION); - ROS_INFO("Known MAVLink dialects:%s", ss.str().c_str()); - ROS_INFO("MAVROS started. MY ID %u.%u, TARGET ID %u.%u", - system_id, component_id, - tgt_system_id, tgt_component_id); -} - -void MavRos::spin() -{ - ros::AsyncSpinner spinner(4 /* threads */); - - auto diag_timer = mavlink_nh.createTimer( - ros::Duration(0.5), - [&](const ros::TimerEvent &) { - UAS_DIAG(&mav_uas).update(); - - if (gcs_link) - gcs_diag_updater.update(); - }); - diag_timer.start(); - - auto remote_endpoint_timer = mavlink_nh.createTimer( - ros::Duration(1.0), [&](const ros::TimerEvent&) { - static auto pub = mavlink_nh.advertise("gcs_ip", 1, true /* latch */); - static auto finished = false; - - if (finished) - return; - - if (const auto* p = dynamic_cast(gcs_link.get())) { - const auto endpoint = p->get_remote_endpoint(); - const auto endpoint_valid = endpoint.find("255.255.255.255") == std::string::npos; - - if (endpoint_valid) { - std_msgs::String msg; - msg.data = endpoint.substr(0, endpoint.find(":")); - pub.publish(msg); - finished = true; - } - } - }); - remote_endpoint_timer.start(); - - spinner.start(); - ros::waitForShutdown(); - - ROS_INFO("Stopping mavros..."); - spinner.stop(); -} - -void MavRos::mavlink_pub_cb(const mavlink_message_t *mmsg, Framing framing) -{ - auto rmsg = boost::make_shared(); - - if (mavlink_pub.getNumSubscribers() == 0) - return; - - rmsg->header.stamp = ros::Time::now(); - mavros_msgs::mavlink::convert(*mmsg, *rmsg, enum_value(framing)); - mavlink_pub.publish(rmsg); -} - -void MavRos::mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg) -{ - mavlink_message_t mmsg; - - if (mavros_msgs::mavlink::convert(*rmsg, mmsg)) - UAS_FCU(&mav_uas)->send_message_ignore_drop(&mmsg); - else - ROS_ERROR("Drop mavlink packet: convert error."); -} - -void MavRos::plugin_route_cb(const mavlink_message_t *mmsg, const Framing framing) -{ - auto it = plugin_subscriptions.find(mmsg->msgid); - if (it == plugin_subscriptions.end()) - return; - - for (auto &info : it->second) - std::get<3>(info)(mmsg, framing); -} - -static bool pattern_match(std::string &pattern, std::string &pl_name) -{ - int cmp = fnmatch(pattern.c_str(), pl_name.c_str(), FNM_CASEFOLD); - if (cmp == 0) - return true; - else if (cmp != FNM_NOMATCH) { - // never see that, i think that it is fatal error. - ROS_FATAL("Plugin list check error! fnmatch('%s', '%s', FNM_CASEFOLD) -> %d", - pattern.c_str(), pl_name.c_str(), cmp); - ros::shutdown(); - } - - return false; -} - -/** - * @brief Checks that plugin blacklisted - * - * Operation algo: - * - * 1. if blacklist and whitelist is empty: load all - * 2. if blacklist is empty and whitelist non empty: assume blacklist is ["*"] - * 3. if blacklist non empty: usual blacklist behavior - * 4. if whitelist non empty: override blacklist - * - * @note Issue #257. - */ -static bool is_blacklisted(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist) -{ - for (auto &bl_pattern : blacklist) { - if (pattern_match(bl_pattern, pl_name)) { - for (auto &wl_pattern : whitelist) { - if (pattern_match(wl_pattern, pl_name)) - return false; - } - - return true; - } - } - - return false; -} - -inline bool is_mavlink_message_t(const size_t rt) -{ - static const auto h = typeid(mavlink_message_t).hash_code(); - return h == rt; -} - -/** - * @brief Loads plugin (if not blacklisted) - */ -void MavRos::add_plugin(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist) -{ - if (is_blacklisted(pl_name, blacklist, whitelist)) { - ROS_INFO_STREAM("Plugin " << pl_name << " blacklisted"); - return; - } - - try { - auto plugin = plugin_loader.createInstance(pl_name); - - ROS_INFO_STREAM("Plugin " << pl_name << " loaded"); - - for (auto &info : plugin->get_subscriptions()) { - auto msgid = std::get<0>(info); - auto msgname = std::get<1>(info); - auto type_hash_ = std::get<2>(info); - - std::string log_msgname; - - if (is_mavlink_message_t(type_hash_)) - log_msgname = utils::format("MSG-ID (%u) <%zu>", msgid, type_hash_); - else - log_msgname = utils::format("%s (%u) <%zu>", msgname, msgid, type_hash_); - - ROS_DEBUG_STREAM("Route " << log_msgname << " to " << pl_name); - - auto it = plugin_subscriptions.find(msgid); - if (it == plugin_subscriptions.end()) { - // new entry - - ROS_DEBUG_STREAM(log_msgname << " - new element"); - plugin_subscriptions[msgid] = PluginBase::Subscriptions{{info}}; - } - else { - // existing: check handler message type - - bool append_allowed = is_mavlink_message_t(type_hash_); - if (!append_allowed) { - append_allowed = true; - for (auto &e : it->second) { - auto t2 = std::get<2>(e); - if (!is_mavlink_message_t(t2) && t2 != type_hash_) { - ROS_ERROR_STREAM(log_msgname << " routed to different message type (hash: " << t2 << ")"); - append_allowed = false; - } - } - } - - if (append_allowed) { - ROS_DEBUG_STREAM(log_msgname << " - emplace"); - it->second.emplace_back(info); - } - else - ROS_ERROR_STREAM(log_msgname << " handler dropped because this ID are used for another message type"); - } - } - - plugin->initialize(mav_uas); - loaded_plugins.push_back(plugin); - - ROS_INFO_STREAM("Plugin " << pl_name << " initialized"); - } catch (pluginlib::PluginlibException &ex) { - ROS_ERROR_STREAM("Plugin " << pl_name << " load exception: " << ex.what()); - } -} - -void MavRos::startup_px4_usb_quirk() -{ - /* sample code from QGC */ - const uint8_t init[] = {0x0d, 0x0d, 0x0d, 0}; - const uint8_t nsh[] = "sh /etc/init.d/rc.usb\n"; - - ROS_INFO("Autostarting mavlink via USB on PX4"); - UAS_FCU(&mav_uas)->send_bytes(init, 3); - UAS_FCU(&mav_uas)->send_bytes(nsh, sizeof(nsh) - 1); - UAS_FCU(&mav_uas)->send_bytes(init, 4); /* NOTE in original init[3] */ -} - -void MavRos::log_connect_change(bool connected) -{ - auto ap = utils::to_string(mav_uas.get_autopilot()); - - /* note: sys_status plugin required */ - if (connected) - ROS_INFO("CON: Got HEARTBEAT, connected. FCU: %s", ap.c_str()); - else - ROS_WARN("CON: Lost connection, HEARTBEAT timed out."); -} diff --git a/mavros/src/lib/mavros_router.cpp b/mavros/src/lib/mavros_router.cpp new file mode 100644 index 000000000..ebb34601b --- /dev/null +++ b/mavros/src/lib/mavros_router.cpp @@ -0,0 +1,552 @@ +/* + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief Mavros Router class + * @file mavros_router.cpp + * @author Vladimir Ermakov + */ + +#include +#include +#include +#include +#include + +#include "mavros/mavros_router.hpp" +#include "rcpputils/asserts.hpp" + +using namespace mavros::router; // NOLINT +using rclcpp::QoS; + +using unique_lock = std::unique_lock; +using shared_lock = std::shared_lock; + +std::atomic Router::id_counter {1000}; + +static inline uint8_t get_msg_byte(const mavlink_message_t * msg, uint8_t offset) +{ + return _MAV_PAYLOAD(msg)[offset]; +} + +void Router::route_message( + Endpoint::SharedPtr src, const mavlink_message_t * msg, + const Framing framing) +{ + shared_lock lock(mu); + this->stat_msg_routed++; + + // find message destination target + addr_t target_addr = 0; + auto msg_entry = ::mavlink::mavlink_get_msg_entry(msg->msgid); + if (msg_entry) { + if (msg_entry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) { + target_addr |= get_msg_byte(msg, msg_entry->target_system_ofs) << 8; + } + if (msg_entry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) { + target_addr |= get_msg_byte(msg, msg_entry->target_component_ofs); + } + } + + size_t sent_cnt = 0, retry_cnt = 0; +retry: + for (auto & kv : this->endpoints) { + auto & dest = kv.second; + + if (src->id == dest->id) { + continue; // do not echo message + } + if (src->link_type == dest->link_type) { + continue; // drop messages between same type FCU/GCS/UAS + } + + // NOTE(vooon): current router do not allow to speak drone-to-drone. + // if it is needed perhaps better to add mavlink-router in front of mavros-router. + + bool has_target = dest->remote_addrs.find(target_addr) != dest->remote_addrs.end(); + + if (has_target) { + dest->send_message(msg, framing, src->id); + sent_cnt++; + } + } + + // if message haven't been sent retry broadcast it + if (sent_cnt == 0 && retry_cnt < 2) { + target_addr = 0; + retry_cnt++; + goto retry; + } + + // update stats + this->stat_msg_sent.fetch_add(sent_cnt); + if (sent_cnt == 0) { + this->stat_msg_dropped++; + + auto lg = get_logger(); + auto clock = get_clock(); + + RCLCPP_WARN_THROTTLE( + lg, + *clock, 10000, "Message dropped: msgid: %d, source: %d.%d, target: %d.%d", msg->msgid, + msg->sysid, msg->compid, target_addr >> 8, + target_addr & 0xff); + } +} + +void Router::add_endpoint( + const mavros_msgs::srv::EndpointAdd::Request::SharedPtr request, + mavros_msgs::srv::EndpointAdd::Response::SharedPtr response) +{ + unique_lock lock(mu); + auto lg = get_logger(); + + RCLCPP_INFO( + lg, "Requested to add endpoint: type: %d, url: %s", request->type, + request->url.c_str()); + + if (request->type > mavros_msgs::srv::EndpointAdd::Request::TYPE_UAS) { + RCLCPP_ERROR(lg, "Unknown endpoint type"); + response->successful = false; + response->reason = "unknown link type"; + return; + } + + id_t id = id_counter.fetch_add(1); + + Endpoint::SharedPtr ep; + if (request->type == mavros_msgs::srv::EndpointAdd::Request::TYPE_UAS) { + ep = std::make_shared(); + } else { + ep = std::make_shared(); + } + + // NOTE(vooon): has type std::shared_ptr + auto shared_this = shared_from_this(); + + ep->parent = std::static_pointer_cast(shared_this); + ep->id = id; + ep->link_type = static_cast(request->type); + ep->url = request->url; + + this->endpoints[id] = ep; + this->diagnostic_updater.add(ep->diag_name(), std::bind(&Endpoint::diag_run, ep, _1)); + RCLCPP_INFO(lg, "Endpoint link[%d] created", id); + + auto result = ep->open(); + if (result.first) { + RCLCPP_INFO(lg, "link[%d] opened successfully", id); + } else { + RCLCPP_ERROR(lg, "link[%d] open failed: %s", id, result.second.c_str()); + } + + response->successful = result.first; + response->reason = result.second; + response->id = id; +} + +void Router::del_endpoint( + const mavros_msgs::srv::EndpointDel::Request::SharedPtr request, + mavros_msgs::srv::EndpointDel::Response::SharedPtr response) +{ + unique_lock lock(mu); + auto lg = get_logger(); + + if (request->id != 0) { + RCLCPP_INFO(lg, "Requested to del endpoint id: %d", request->id); + auto it = this->endpoints.find(request->id); + if (it != this->endpoints.end() ) { + it->second->close(); + this->diagnostic_updater.removeByName(it->second->diag_name()); + this->endpoints.erase(it); + response->successful = true; + } + return; + } + + RCLCPP_INFO( + lg, "Requested to del endpoint type: %d url: %s", request->type, + request->url.c_str()); + for (auto it = this->endpoints.cbegin(); it != this->endpoints.cend(); it++) { + if (it->second->url == request->url && + it->second->link_type == static_cast( request->type)) + { + it->second->close(); + this->diagnostic_updater.removeByName(it->second->diag_name()); + this->endpoints.erase(it); + response->successful = true; + return; + } + } +} + +rcl_interfaces::msg::SetParametersResult Router::on_set_parameters_cb( + const std::vector & parameters) +{ + auto lg = get_logger(); + rcl_interfaces::msg::SetParametersResult result{}; + + RCLCPP_DEBUG(lg, "params callback"); + + using Type = Endpoint::Type; + + auto get_existing_set = [this](Type type) -> std::set { + shared_lock lock(this->mu); + + std::set ret; + + for (const auto & kv : this->endpoints) { + if (kv.second->link_type != type) { + continue; + } + + ret.emplace(kv.second->url); + } + + return ret; + }; + + auto update_endpoints = [&, this](const rclcpp::Parameter & parameter, Type type) { + RCLCPP_DEBUG(lg, "Processing urls parameter: %s", parameter.get_name().c_str()); + + auto urls = parameter.as_string_array(); + std::set urls_set(urls.begin(), urls.end()); + auto existing_set = get_existing_set(type); + + std::set to_add{}, to_del{}; + std::set_difference( + urls_set.begin(), urls_set.end(), existing_set.begin(), + existing_set.end(), std::inserter(to_add, to_add.begin())); + std::set_difference( + existing_set.begin(), existing_set.end(), urls_set.begin(), + urls_set.end(), std::inserter(to_del, to_del.begin())); + + for (auto url : to_add) { + auto req = std::make_shared(); + auto resp = std::make_shared(); + + req->type = utils::enum_value(type); + req->url = url; + + this->add_endpoint(req, resp); + } + + for (auto url : to_del) { + auto req = std::make_shared(); + auto resp = std::make_shared(); + + req->type = utils::enum_value(type); + req->url = url; + + this->del_endpoint(req, resp); + } + }; + + result.successful = true; + for (const auto & parameter : parameters) { + const auto name = parameter.get_name(); + if (name == "fcu_urls") { + update_endpoints(parameter, Type::fcu); + } else if (name == "gcs_urls") { + update_endpoints(parameter, Type::gcs); + } else if (name == "uas_urls") { + update_endpoints(parameter, Type::uas); + } else { + result.successful = false; + result.reason = "unknown parameter"; + } + } + + return result; +} + +void Router::periodic_reconnect_endpoints() +{ + shared_lock lock(mu); + auto lg = get_logger(); + + RCLCPP_DEBUG(lg, "start reconnecting..."); + + for (auto & kv : this->endpoints) { + auto & p = kv.second; + + if (p->is_open()) { + continue; + } + + RCLCPP_INFO(lg, "link[%d] trying to reconnect...", p->id); + auto result = p->open(); + + if (result.first) { + RCLCPP_INFO(lg, "link[%d] reconnected", p->id); + } else { + RCLCPP_ERROR(lg, "link[%d] reconnect failed: %s", p->id, result.second.c_str()); + } + } +} + +void Router::periodic_clear_stale_remote_addrs() +{ + unique_lock lock(mu); + auto lg = get_logger(); + + RCLCPP_DEBUG(lg, "clear stale remotes"); + for (auto & kv : this->endpoints) { + auto & p = kv.second; + + // Step 1: remove any stale addrs that still there + // (hadn't been removed by Endpoint::recv_message()) + for (auto addr : p->stale_addrs) { + if (addr != 0) { + p->remote_addrs.erase(addr); + RCLCPP_INFO( + lg, "link[%d] removed stale remote address %d.%d", p->id, addr >> 8, + addr & 0xff); + } + } + + // Step 2: re-initiate stale_addrs + p->stale_addrs.clear(); + p->stale_addrs.insert(p->remote_addrs.begin(), p->remote_addrs.end()); + } +} + +void Router::diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + auto endpoints_len = [this]() -> auto { + shared_lock lock(this->mu); + return this->endpoints.size(); + } (); + + stat.addf("Endpoints", "%zu", endpoints_len); + stat.addf("Messages routed", "%zu", stat_msg_routed.load()); + stat.addf("Messages sent", "%zu", stat_msg_sent.load()); + stat.addf("Messages dropped", "%zu", stat_msg_dropped.load()); + + if (endpoints_len < 2) { + stat.summary(2, "not enough endpoints"); + } else { + stat.summary(0, "ok"); + } +} + +void Endpoint::recv_message(const mavlink_message_t * msg, const Framing framing) +{ + rcpputils::assert_true(msg, "msg not nullptr"); + // rcpputils::assert_true(this->parent, "parent not nullptr"); + + const addr_t sysid_addr = msg->sysid << 8; + const addr_t sysid_compid_addr = (msg->sysid << 8) | msg->compid; + + // save source addr to remote_addrs + auto sp = this->remote_addrs.emplace(sysid_addr); + auto scp = this->remote_addrs.emplace(sysid_compid_addr); + + // and delete it from stale_addrs + this->stale_addrs.erase(sysid_addr); + this->stale_addrs.erase(sysid_compid_addr); + + auto & nh = this->parent; + if (sp.second || scp.second) { + RCLCPP_INFO( + nh->get_logger(), "link[%d] detected remote address %d.%d", this->id, msg->sysid, + msg->compid); + } + + nh->route_message(shared_from_this(), msg, framing); +} + +std::string Endpoint::diag_name() +{ + return utils::format("endpoint %d: %s", this->id, this->url.c_str()); +} + +bool MAVConnEndpoint::is_open() +{ + if (!this->link) { + return false; + } + + return this->link->is_open(); +} + +std::pair MAVConnEndpoint::open() +{ + try { + this->link = mavconn::MAVConnInterface::open_url( + this->url, 1, mavconn::MAV_COMP_ID_UDP_BRIDGE, std::bind( + &MAVConnEndpoint::recv_message, + shared_from_this(), _1, _2)); + } catch (mavconn::DeviceError & ex) { + return {false, ex.what()}; + } + + // not necessary because router would not serialize mavlink::Message + // but that is a good default + this->link->set_protocol_version(mavconn::Protocol::V20); + + // TODO(vooon): message signing? + + return {true, ""}; +} + +void MAVConnEndpoint::close() +{ + if (!this->link) { + return; + } + + this->link->close(); + this->link.reset(); +} + +void MAVConnEndpoint::send_message( + const mavlink_message_t * msg, const Framing framing, + id_t src_id [[maybe_unused]]) +{ + (void)framing; + + if (!this->link) { + return; + } + + this->link->send_message_ignore_drop(msg); +} + +void MAVConnEndpoint::diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (!this->link) { + stat.summary(2, "closed"); + return; + } + + auto mav_status = this->link->get_status(); + auto iostat = this->link->get_iostat(); + + stat.addf("Received packets", "%u", mav_status.packet_rx_success_count); + stat.addf("Dropped packets", "%u", mav_status.packet_rx_drop_count); + stat.addf("Buffer overruns", "%u", mav_status.buffer_overrun); + stat.addf("Parse errors", "%u", mav_status.parse_error); + stat.addf("Rx sequence number", "%u", mav_status.current_rx_seq); + stat.addf("Tx sequence number", "%u", mav_status.current_tx_seq); + + stat.addf("Rx total bytes", "%u", iostat.rx_total_bytes); + stat.addf("Tx total bytes", "%u", iostat.tx_total_bytes); + stat.addf("Rx speed", "%f", iostat.rx_speed); + stat.addf("Tx speed", "%f", iostat.tx_speed); + + stat.addf("Remotes count", "%zu", this->remote_addrs.size()); + size_t idx = 0; + for (auto addr : this->remote_addrs) { + stat.addf(utils::format("Remote [%d]", idx++), "%d.%d", addr >> 8, addr & 0xff); + } + + if (mav_status.packet_rx_drop_count > stat_last_drop_count) { + stat.summaryf( + 1, "%d packeges dropped since last report", + mav_status.packet_rx_drop_count - stat_last_drop_count); + } else { + stat.summary(0, "ok"); + } + + stat_last_drop_count = mav_status.packet_rx_drop_count; +} + +bool ROSEndpoint::is_open() +{ + return this->source && this->sink; +} + +std::pair ROSEndpoint::open() +{ + auto & nh = this->parent; + if (!nh) { + return {false, "parent not set"}; + } + + try { + auto qos = QoS( + 1000).best_effort().durability_volatile(); + this->source = + nh->create_publisher( + utils::format( + "%s/%s", this->url.c_str(), + "mavlink_source"), qos); + this->sink = nh->create_subscription( + utils::format("%s/%s", this->url.c_str(), "mavlink_sink"), qos, + std::bind(&ROSEndpoint::ros_recv_message, this, _1)); + } catch (rclcpp::exceptions::InvalidTopicNameError & ex) { + return {false, ex.what()}; + } + + return {true, ""}; +} + +void ROSEndpoint::close() +{ + this->source.reset(); + this->sink.reset(); +} + +void ROSEndpoint::send_message(const mavlink_message_t * msg, const Framing framing, id_t src_id) +{ + rcpputils::assert_true(msg, "msg not null"); + + auto rmsg = mavros_msgs::msg::Mavlink(); + auto ok = mavros_msgs::mavlink::convert(*msg, rmsg, utils::enum_value(framing)); + + // don't fail if endpoint closed + if (!this->source) { + return; + } + + rmsg.header.stamp = this->parent->now(); + rmsg.header.frame_id = utils::format("ep:%d", src_id); + + if (ok) { + this->source->publish(rmsg); + } else if (auto & nh = this->parent) { + RCLCPP_ERROR(nh->get_logger(), "message conversion error"); + } +} + +void ROSEndpoint::ros_recv_message(const mavros_msgs::msg::Mavlink::SharedPtr rmsg) +{ + rcpputils::assert_true(!!rmsg, "rmsg not nullptr"); + + mavlink::mavlink_message_t mmsg; + + auto ok = mavros_msgs::mavlink::convert(*rmsg, mmsg); + auto framing = static_cast(rmsg->framing_status); + + if (ok) { + recv_message(&mmsg, framing); + } else if (auto & nh = this->parent) { + RCLCPP_ERROR(nh->get_logger(), "message conversion error"); + } +} + +void ROSEndpoint::diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + // TODO(vooon): make some diagnostics + + stat.addf("Remotes count", "%zu", this->remote_addrs.size()); + size_t idx = 0; + for (auto addr : this->remote_addrs) { + stat.addf(utils::format("Remote [%d]", idx++), "%d.%d", addr >> 8, addr & 0xff); + } + + if (this->is_open()) { + stat.summary(0, "ok"); + } else { + stat.summary(2, "closed"); + } +} + + +#include // NOLINT +RCLCPP_COMPONENTS_REGISTER_NODE(mavros::router::Router) diff --git a/mavros/src/lib/mavros_uas.cpp b/mavros/src/lib/mavros_uas.cpp new file mode 100644 index 000000000..d6c6e3411 --- /dev/null +++ b/mavros/src/lib/mavros_uas.cpp @@ -0,0 +1,442 @@ +/* + * Copyright 2013,2014,2015,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief MAVROS UAS Node class + * @file mavros_uas.cpp + * @author Vladimir Ermakov + */ + +#include +#include +#include +#include +#include // NOLINT + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/utils.hpp" + +using namespace mavros; // NOLINT +using namespace mavros::uas; // NOLINT +using namespace std::chrono_literals; // NOLINT +using mavconn::MAVConnInterface; +using mavconn::Framing; +using mavlink::mavlink_message_t; +using plugin::Plugin; +using plugin::PluginFactory; +using utils::enum_value; + +UAS::UAS( + const rclcpp::NodeOptions & options_, + const std::string & name_, + const std::string & uas_url_, uint8_t target_system_, + uint8_t target_component_) +: rclcpp::Node(name_, options_ /* rclcpp::NodeOptions(options_).use_intra_process_comms(true) */), + diagnostic_updater(this, 1.0), + data(), + tf2_buffer(get_clock(), tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME)), + tf2_listener(tf2_buffer, true), + tf2_broadcaster(this), + tf2_static_broadcaster(this), + source_system(target_system_), + source_component(MAV_COMP_ID_ONBOARD_COMPUTER), + target_system(target_system_), + target_component(target_component_), + uas_url(uas_url_), + plugin_factory_loader("mavros", "mavros::plugin::PluginFactory"), + loaded_plugins{}, + plugin_subscriptions{}, + type(enum_value(MAV_TYPE::GENERIC)), + autopilot(enum_value(MAV_AUTOPILOT::GENERIC)), + base_mode(0), + fcu_caps_known(false), + fcu_capabilities(0), + connected(false), + time_offset(0), + tsync_mode(timesync_mode::NONE), + mavlink_status({}) +{ + // XXX TODO(vooon): should i use LifecycleNode? + + set_parameters_handle_ptr = + this->add_on_set_parameters_callback( + std::bind( + &UAS::on_set_parameters_cb, this, + std::placeholders::_1)); + + this->declare_parameter("uas_url", uas_url); + this->declare_parameter("fcu_protocol", "v2.0"); + this->declare_parameter("system_id", source_system); + this->declare_parameter("component_id", source_component); + this->declare_parameter("target_system_id", target_system); + this->declare_parameter("target_component_id", target_component); + this->declare_parameter("plugin_allowlist", plugin_allowlist); + this->declare_parameter("plugin_denylist", plugin_denylist); + + // NOTE(vooon): we couldn't add_plugin() in constructor because it needs shared_from_this() + startup_delay_timer = this->create_wall_timer( + 10ms, [this]() { + startup_delay_timer->cancel(); + + std::string fcu_protocol; + int tgt_system, tgt_component; + this->get_parameter("uas_url", uas_url); + this->get_parameter("fcu_protocol", fcu_protocol); + this->get_parameter("system_id", source_system); + this->get_parameter("component_id", source_component); + this->get_parameter("target_system_id", tgt_system); + this->get_parameter("target_component_id", tgt_component); + this->get_parameter("plugin_allowlist", plugin_allowlist); + this->get_parameter("plugin_denylist", plugin_denylist); + + exec_spin_thd = thread_ptr( + new std::thread( + [this]() { + utils::set_this_thread_name("uas-exec/%d.%d", source_system, source_component); + auto lg = this->get_logger(); + + RCLCPP_INFO( + lg, "UAS Executor started, threads: %zu", + this->exec.get_number_of_threads()); + this->exec.spin(); + RCLCPP_WARN(lg, "UAS Executor terminated"); + }), + [this](std::thread * t) { + this->exec.cancel(); + t->join(); + delete t; + }); + + // setup diag + diagnostic_updater.setHardwareID(utils::format("uas://%s", uas_url.c_str())); + diagnostic_updater.add("MAVROS UAS", this, &UAS::diag_run); + + // setup uas link + if (fcu_protocol == "v1.0") { + set_protocol_version(mavconn::Protocol::V10); + } else if (fcu_protocol == "v2.0") { + set_protocol_version(mavconn::Protocol::V20); + } else { + RCLCPP_WARN( + get_logger(), + "Unknown FCU protocol: \"%s\", should be: \"v1.0\" or \"v2.0\". Used default v2.0.", + fcu_protocol.c_str()); + set_protocol_version(mavconn::Protocol::V20); + } + + // setup source and target + set_tgt(tgt_system, tgt_component); + + add_connection_change_handler( + std::bind( + &UAS::log_connect_change, this, + std::placeholders::_1)); + + // prepare plugin lists + // issue #257 2: assume that all plugins blacklisted + if (plugin_denylist.empty() && !plugin_allowlist.empty()) { + plugin_denylist.emplace_back("*"); + } + + for (auto & name : plugin_factory_loader.getDeclaredClasses()) { + add_plugin(name); + } + + connect_to_router(); + + // Publish helper TFs used for frame transformation in the odometry plugin + { + std::vector transform_vector; + add_static_transform( + "map", "map_ned", Eigen::Affine3d( + ftf::quaternion_from_rpy( + M_PI, 0, + M_PI_2)), + transform_vector); + add_static_transform( + "odom", "odom_ned", Eigen::Affine3d( + ftf::quaternion_from_rpy( + M_PI, 0, + M_PI_2)), + transform_vector); + add_static_transform( + "base_link", "base_link_frd", + Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)), transform_vector); + + tf2_static_broadcaster.sendTransform(transform_vector); + } + + std::stringstream ss; + for (auto & s : mavconn::MAVConnInterface::get_known_dialects()) { + ss << " " << s; + } + + RCLCPP_INFO( + get_logger(), "Built-in SIMD instructions: %s", + Eigen::SimdInstructionSetsInUse()); + RCLCPP_INFO(get_logger(), "Built-in MAVLink package version: %s", mavlink::version); + RCLCPP_INFO(get_logger(), "Known MAVLink dialects:%s", ss.str().c_str()); + RCLCPP_INFO( + get_logger(), "MAVROS UAS via %s started. MY ID %u.%u, TARGET ID %u.%u", + uas_url.c_str(), + source_system, source_component, + target_system, target_component); + }); +} + +void UAS::plugin_route(const mavlink_message_t * mmsg, const Framing framing) +{ + auto it = plugin_subscriptions.find(mmsg->msgid); + if (it == plugin_subscriptions.end()) { + return; + } + + for (auto & info : it->second) { + std::get<3>(info)(mmsg, framing); + } +} + +static bool pattern_match(const std::string & pattern, const std::string & pl_name) +{ + int cmp = fnmatch(pattern.c_str(), pl_name.c_str(), FNM_CASEFOLD); + rcpputils::require_true( + cmp == 0 || cmp == FNM_NOMATCH, + "fnmatch(pattern, pl_name, FNM_CASEFOLD) error"); + + return cmp == 0; +} + +/** + * @brief Checks that plugin blacklisted + * + * Operation algo: + * + * 1. if blacklist and whitelist is empty: load all + * 2. if blacklist is empty and whitelist non empty: assume blacklist is ["*"] + * 3. if blacklist non empty: usual blacklist behavior + * 4. if whitelist non empty: override blacklist + * + * @note Issue #257. + */ +bool UAS::is_plugin_allowed( + const std::string & pl_name) +{ + for (auto & bl_pattern : plugin_denylist) { + if (pattern_match(bl_pattern, pl_name)) { + for (auto & wl_pattern : plugin_allowlist) { + if (pattern_match(wl_pattern, pl_name)) { + return true; + } + } + + return false; + } + } + + return true; +} + +inline bool is_mavlink_message_t(const size_t rt) +{ + static const auto h = typeid(mavlink_message_t).hash_code(); + return h == rt; +} + +plugin::Plugin::SharedPtr UAS::create_plugin_instance(const std::string & pl_name) +{ + auto plugin_factory = plugin_factory_loader.createSharedInstance(pl_name); + + return + plugin_factory->create_plugin_instance(std::static_pointer_cast(shared_from_this())); +} + +void UAS::add_plugin(const std::string & pl_name) +{ + auto lg = get_logger(); + + if (!is_plugin_allowed(pl_name)) { + RCLCPP_INFO_STREAM(lg, "Plugin " << pl_name << " ignored"); + return; + } + + try { + auto plugin = create_plugin_instance(pl_name); + + RCLCPP_INFO_STREAM(lg, "Plugin " << pl_name << " created"); + + for (auto & info : plugin->get_subscriptions()) { + auto msgid = std::get<0>(info); + auto msgname = std::get<1>(info); + auto type_hash_ = std::get<2>(info); + + std::string log_msgname; + + if (is_mavlink_message_t(type_hash_)) { + log_msgname = utils::format("MSG-ID (%u) <%zu>", msgid, type_hash_); + } else { + log_msgname = utils::format("%s (%u) <%zu>", msgname, msgid, type_hash_); + } + + RCLCPP_DEBUG_STREAM(lg, "Route " << log_msgname << " to " << pl_name); + + auto it = plugin_subscriptions.find(msgid); + if (it == plugin_subscriptions.end()) { + // new entry + + RCLCPP_DEBUG_STREAM(lg, log_msgname << " - new element"); + plugin_subscriptions[msgid] = Plugin::Subscriptions{{info}}; + } else { + // existing: check handler message type + + bool append_allowed = is_mavlink_message_t(type_hash_); + if (!append_allowed) { + append_allowed = true; + for (auto & e : it->second) { + auto t2 = std::get<2>(e); + if (!is_mavlink_message_t(t2) && t2 != type_hash_) { + RCLCPP_ERROR_STREAM( + lg, + log_msgname << " routed to different message type (hash: " << t2 << ")"); + append_allowed = false; + } + } + } + + if (append_allowed) { + RCLCPP_DEBUG_STREAM(lg, log_msgname << " - emplace"); + it->second.emplace_back(info); + } else { + RCLCPP_ERROR_STREAM( + lg, + log_msgname << " handler dropped because this ID are used for another message type"); + } + } + } + + loaded_plugins.push_back(plugin); + + auto pl_node = plugin->get_node(); + if (pl_node && pl_node.get() != this) { + RCLCPP_DEBUG_STREAM(lg, "Plugin " << pl_name << " added to executor"); + exec.add_node(pl_node); + } + + RCLCPP_INFO_STREAM(lg, "Plugin " << pl_name << " initialized"); + } catch (pluginlib::PluginlibException & ex) { + RCLCPP_ERROR_STREAM(lg, "Plugin " << pl_name << " load exception: " << ex.what()); + } +} + +rcl_interfaces::msg::SetParametersResult UAS::on_set_parameters_cb( + const std::vector & parameters) +{ + auto lg = get_logger(); + rcl_interfaces::msg::SetParametersResult result{}; + + RCLCPP_DEBUG(lg, "params callback"); + + result.successful = true; + for (const auto & parameter : parameters) { + const auto name = parameter.get_name(); + if (true) { + // XXX TODO + } else { + result.successful = false; + result.reason = "unknown parameter"; + } + } + + return result; +} + +void UAS::connect_to_router() +{ + auto qos = rclcpp::QoS( + 1000).best_effort().durability_volatile(); + + this->sink = + this->create_publisher( + utils::format( + "%s/%s", this->uas_url.c_str(), + "mavlink_sink"), qos); + + this->source = this->create_subscription( + utils::format("%s/%s", this->uas_url.c_str(), "mavlink_source"), qos, + std::bind(&UAS::recv_message, this, std::placeholders::_1)); +} + +void UAS::recv_message(const mavros_msgs::msg::Mavlink::SharedPtr rmsg) +{ + mavlink::mavlink_message_t msg; + + auto ok = mavros_msgs::mavlink::convert(*rmsg, msg); + rcpputils::assert_true(ok, "conversion error"); + + if (ok) { + plugin_route(&msg, static_cast(rmsg->framing_status)); + } +} + +void UAS::send_message(const mavlink::Message & obj, const uint8_t src_compid) +{ + mavlink::mavlink_message_t msg; + mavlink::MsgMap map(msg); + + auto mi = obj.get_message_info(); + + obj.serialize(map); + mavlink::mavlink_finalize_message_buffer( + &msg, source_system, src_compid, &mavlink_status, mi.min_length, mi.length, + mi.crc_extra); + + mavros_msgs::msg::Mavlink rmsg{}; + auto ok = mavros_msgs::mavlink::convert(msg, rmsg); + + rmsg.header.stamp = this->now(); + rmsg.header.frame_id = this->get_name(); + + if (this->sink && ok) { + this->sink->publish(rmsg); + } +} + +void UAS::set_protocol_version(mavconn::Protocol pver) +{ + if (pver == mavconn::Protocol::V10) { + mavlink_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } else { + mavlink_status.flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); + } +} + +void UAS::log_connect_change(bool connected) +{ + auto ap = utils::to_string(get_autopilot()); + + /* note: sys_status plugin required */ + if (connected) { + RCLCPP_INFO(get_logger(), "CON: Got HEARTBEAT, connected. FCU: %s", ap.c_str()); + } else { + RCLCPP_WARN(get_logger(), "CON: Lost connection, HEARTBEAT timed out."); + } +} + +void UAS::diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + // TODO(vooon): add some fields + + if (connected) { + stat.summary(0, "connected"); + } else { + stat.summary(2, "disconnected"); + } +} + + +#include // NOLINT +RCLCPP_COMPONENTS_REGISTER_NODE(mavros::uas::UAS) diff --git a/mavros/src/lib/plugin.cpp b/mavros/src/lib/plugin.cpp new file mode 100644 index 000000000..bb8dd1923 --- /dev/null +++ b/mavros/src/lib/plugin.cpp @@ -0,0 +1,66 @@ +/* + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief MAVROS Plugin methods + * @file plugin.cpp + * @author Vladimir Ermakov + */ + +#include + +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" + +using mavros::plugin::Plugin; + +void Plugin::enable_connection_cb() +{ + uas->add_connection_change_handler( + std::bind( + &Plugin::connection_cb, this, + std::placeholders::_1)); +} + +void Plugin::enable_capabilities_cb() +{ + uas->add_capabilities_change_handler( + std::bind( + &Plugin::capabilities_cb, this, + std::placeholders::_1)); +} + +Plugin::SetParametersResult Plugin::node_on_set_parameters_cb( + const std::vector & parameters) +{ + SetParametersResult result; + + result.successful = true; + + for (auto & p : parameters) { + auto it = node_watch_parameters.find(p.get_name()); + if (it != node_watch_parameters.end()) { + try { + it->second(p); + } catch (std::exception & ex) { + result.successful = false; + result.reason = ex.what(); + break; + } + } + } + + return result; +} + +void Plugin::enable_node_watch_parameters() +{ + node_set_parameters_handle_ptr = + node->add_on_set_parameters_callback( + std::bind( + &Plugin::node_on_set_parameters_cb, this, std::placeholders::_1)); +} diff --git a/mavros/src/lib/rosconsole_bridge.cpp b/mavros/src/lib/rosconsole_bridge.cpp deleted file mode 100644 index c635faa4f..000000000 --- a/mavros/src/lib/rosconsole_bridge.cpp +++ /dev/null @@ -1,16 +0,0 @@ -/** - * @brief Enable console bridge - * @file rosconsole_bridge.cpp - * @author Vladimir Ermakov - */ -/* - * Copyright 2014 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include - -REGISTER_ROSCONSOLE_BRIDGE; diff --git a/mavros/src/lib/uas_ap.cpp b/mavros/src/lib/uas_ap.cpp new file mode 100644 index 000000000..d10f4faea --- /dev/null +++ b/mavros/src/lib/uas_ap.cpp @@ -0,0 +1,102 @@ +/* + * Copyright 2014,2015,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief MAVROS UAS autipilot info methods + * @file uas_data.cpp + * @author Vladimir Ermakov + */ + +#include +#include +#include + +#include "mavros/mavros_uas.hpp" +#include "mavros/utils.hpp" +#include "mavros/px4_custom_mode.hpp" + +using namespace mavros; // NOLINT +using namespace mavros::uas; // NOLINT + +/* -*- heartbeat handlers -*- */ + +void UAS::update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_) +{ + type = type_; + autopilot = autopilot_; + base_mode = base_mode_; +} + +void UAS::update_connection_status(bool conn_) +{ + if (conn_ != connected) { + connected = conn_; + + s_shared_lock lock(mu); + + // call all change cb's + for (auto & cb : connection_cb_vec) { + cb(conn_); + } + } +} + +void UAS::add_connection_change_handler(UAS::ConnectionCb cb) +{ + s_unique_lock lock(mu); + connection_cb_vec.push_back(cb); +} + +/* -*- autopilot version -*- */ + +static uint64_t get_default_caps([[maybe_unused]] uas::MAV_AUTOPILOT ap_type) +{ + // TODO(vooon): return default caps mask for known FCU's + return 0; +} + +uint64_t UAS::get_capabilities() +{ + if (fcu_caps_known) { + uint64_t caps = fcu_capabilities; + return caps; + } else { + return get_default_caps(get_autopilot()); + } +} + +// This function may need a mutex now +void UAS::update_capabilities(bool known, uint64_t caps) +{ + bool process_cb_queue = false; + + if (known != fcu_caps_known) { + if (!fcu_caps_known) { + process_cb_queue = true; + } + fcu_caps_known = known; + } else if (fcu_caps_known) { // Implies fcu_caps_known == known + if (caps != fcu_capabilities) { + process_cb_queue = true; + } + } else {} // Capabilities werent known before and arent known after update + + if (process_cb_queue) { + fcu_capabilities = caps; + + s_shared_lock lock(mu); + for (auto & cb : capabilities_cb_vec) { + cb(static_cast(caps)); + } + } +} + +void UAS::add_capabilities_change_handler(UAS::CapabilitiesCb cb) +{ + s_unique_lock lock(mu); + capabilities_cb_vec.push_back(cb); +} diff --git a/mavros/src/lib/uas_data.cpp b/mavros/src/lib/uas_data.cpp index 6dcfa5604..29a5ca308 100644 --- a/mavros/src/lib/uas_data.cpp +++ b/mavros/src/lib/uas_data.cpp @@ -1,277 +1,158 @@ -/** - * @brief MAVROS UAS manager (data part) - * @file uas.cpp - * @author Vladimir Ermakov - */ /* - * Copyright 2014,2015 Vladimir Ermakov. + * Copyright 2014,2015,2021 Vladimir Ermakov. * * This file is part of the mavros package and subject to the license terms * in the top-level LICENSE file of the mavros repository. * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ +/** + * @brief MAVROS UAS manager (data part) + * @file uas_data.cpp + * @author Vladimir Ermakov + */ #include +#include #include #include -#include -#include -#include - -using namespace mavros; -using utils::enum_value; - -UAS::UAS() : - tf2_listener(tf2_buffer, true), - type(enum_value(MAV_TYPE::GENERIC)), - autopilot(enum_value(MAV_AUTOPILOT::GENERIC)), - base_mode(0), - target_system(1), - target_component(1), - connected(false), - gps_eph(NAN), - gps_epv(NAN), - gps_fix_type(0), - gps_satellites_visible(0), - time_offset(0), - tsync_mode(UAS::timesync_mode::NONE), - fcu_caps_known(false), - fcu_capabilities(0) -{ - try { - // Using smallest dataset with 5' grid, - // From default location, - // Use cubic interpolation, Thread safe - egm96_5 = std::make_shared("egm96-5", "", true, true); - } - catch (const std::exception &e) { - // catch exception and shutdown node - ROS_FATAL_STREAM("UAS: GeographicLib exception: " << e.what() << - " | Run install_geographiclib_dataset.sh script in order to install Geoid Model dataset!"); - ros::shutdown(); - } - - // Publish helper TFs used for frame transformation in the odometry plugin - std::vector transform_vector; - add_static_transform("map", "map_ned", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, M_PI_2)),transform_vector); - add_static_transform("odom", "odom_ned", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, M_PI_2)),transform_vector); - add_static_transform("base_link", "base_link_frd", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)),transform_vector); - - tf2_static_broadcaster.sendTransform(transform_vector); -} - -/* -*- heartbeat handlers -*- */ -void UAS::update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_) -{ - type = type_; - autopilot = autopilot_; - base_mode = base_mode_; -} +#include "mavros/mavros_uas.hpp" +#include "mavros/utils.hpp" +#include "mavros/px4_custom_mode.hpp" -void UAS::update_connection_status(bool conn_) -{ - if (conn_ != connected) { - connected = conn_; - - // call all change cb's - for (auto &cb : connection_cb_vec) - cb(conn_); - } -} +using namespace mavros::uas; // NOLINT -void UAS::add_connection_change_handler(UAS::ConnectionCb cb) -{ - lock_guard lock(mutex); - connection_cb_vec.push_back(cb); -} +std::once_flag Data::init_flag; +std::shared_ptr Data::egm96_5; -/* -*- autopilot version -*- */ -static uint64_t get_default_caps(UAS::MAV_AUTOPILOT ap_type) +Data::Data() +: imu_enu_data{}, + imu_ned_data{}, + gps_fix{}, + gps_eph(NAN), + gps_epv(NAN), + gps_fix_type(0), + gps_satellites_visible(0) { - // TODO: return default caps mask for known FCU's - return 0; -} + auto & nq = imu_ned_data.orientation; + auto & eq = imu_enu_data.orientation; + auto & nv = imu_ned_data.angular_velocity; + auto & ev = imu_enu_data.angular_velocity; -uint64_t UAS::get_capabilities() -{ - if (fcu_caps_known) { - uint64_t caps = fcu_capabilities; - return caps; - } - else { - return get_default_caps(get_autopilot()); - } -} + nq.w = 1.0, nq.x = nq.y = nq.z = 0.0; + eq.w = 1.0, eq.x = eq.y = eq.z = 0.0; -// This function may need a mutex now -void UAS::update_capabilities(bool known, uint64_t caps) -{ - bool process_cb_queue = false; + nv.x = nv.y = nv.z = 0.0; + ev.x = ev.y = ev.z = 0.0; - if (known != fcu_caps_known) { - if (!fcu_caps_known) { - process_cb_queue = true; - } - fcu_caps_known = known; - } else if (fcu_caps_known) { // Implies fcu_caps_known == known - if (caps != fcu_capabilities) { - process_cb_queue = true; - } - } - else {} // Capabilities werent known before and arent known after update + gps_fix.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + gps_fix.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + gps_fix.position_covariance.fill(0.0); + gps_fix.position_covariance[0] = -1.0; + gps_fix.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; - if (process_cb_queue) { - fcu_capabilities = caps; - for (auto &cb : capabilities_cb_vec) { - cb(static_cast(caps)); - } - } + std::call_once(init_flag, init_geographiclib); } -void UAS::add_capabilities_change_handler(UAS::CapabilitiesCb cb) +void Data::init_geographiclib() { - lock_guard lock(mutex); - capabilities_cb_vec.push_back(cb); + try { + // Using smallest dataset with 5' grid, + // From default location, + // Use cubic interpolation, Thread safe + egm96_5 = std::make_shared("egm96-5", "", true, true); + } catch (const std::exception & e) { + rcpputils::require_true( + false, utils::format( + "UAS: GeographicLib exception: %s " + "| Run install_geographiclib_dataset.sh script in order to install Geoid Model dataset!", + e.what())); + } } /* -*- IMU data -*- */ -void UAS::update_attitude_imu_enu(sensor_msgs::Imu::Ptr &imu) +void Data::update_attitude_imu_enu(const sensor_msgs::msg::Imu & imu) { - lock_guard lock(mutex); - imu_enu_data = imu; + s_unique_lock lock(mu); + imu_enu_data = imu; } -void UAS::update_attitude_imu_ned(sensor_msgs::Imu::Ptr &imu) +void Data::update_attitude_imu_ned(const sensor_msgs::msg::Imu & imu) { - lock_guard lock(mutex); - imu_ned_data = imu; + s_unique_lock lock(mu); + imu_ned_data = imu; } -sensor_msgs::Imu::Ptr UAS::get_attitude_imu_enu() +sensor_msgs::msg::Imu Data::get_attitude_imu_enu() { - lock_guard lock(mutex); - return imu_enu_data; + s_shared_lock lock(mu); + return imu_enu_data; } -sensor_msgs::Imu::Ptr UAS::get_attitude_imu_ned() +sensor_msgs::msg::Imu Data::get_attitude_imu_ned() { - lock_guard lock(mutex); - return imu_ned_data; + s_shared_lock lock(mu); + return imu_ned_data; } -geometry_msgs::Quaternion UAS::get_attitude_orientation_enu() +geometry_msgs::msg::Quaternion Data::get_attitude_orientation_enu() { - lock_guard lock(mutex); - if (imu_enu_data) - return imu_enu_data->orientation; - else { - // fallback - return identity - geometry_msgs::Quaternion q; - q.w = 1.0; q.x = q.y = q.z = 0.0; - return q; - } + s_shared_lock lock(mu); + + return imu_enu_data.orientation; } -geometry_msgs::Quaternion UAS::get_attitude_orientation_ned() +geometry_msgs::msg::Quaternion Data::get_attitude_orientation_ned() { - lock_guard lock(mutex); - if (imu_ned_data) - return imu_ned_data->orientation; - else { - // fallback - return identity - geometry_msgs::Quaternion q; - q.w = 1.0; q.x = q.y = q.z = 0.0; - return q; - } + s_shared_lock lock(mu); + + return imu_ned_data.orientation; } -geometry_msgs::Vector3 UAS::get_attitude_angular_velocity_enu() +geometry_msgs::msg::Vector3 Data::get_attitude_angular_velocity_enu() { - lock_guard lock(mutex); - if (imu_enu_data) - return imu_enu_data->angular_velocity; - else { - // fallback - geometry_msgs::Vector3 v; - v.x = v.y = v.z = 0.0; - return v; - } + s_shared_lock lock(mu); + return imu_enu_data.angular_velocity; } -geometry_msgs::Vector3 UAS::get_attitude_angular_velocity_ned() +geometry_msgs::msg::Vector3 Data::get_attitude_angular_velocity_ned() { - lock_guard lock(mutex); - if (imu_ned_data) - return imu_ned_data->angular_velocity; - else { - // fallback - geometry_msgs::Vector3 v; - v.x = v.y = v.z = 0.0; - return v; - } + s_shared_lock lock(mu); + return imu_ned_data.angular_velocity; } - /* -*- GPS data -*- */ -void UAS::update_gps_fix_epts(sensor_msgs::NavSatFix::Ptr &fix, - float eph, float epv, - int fix_type, int satellites_visible) +void Data::update_gps_fix_epts( + const sensor_msgs::msg::NavSatFix & fix, + float eph, float epv, + int fix_type, int satellites_visible) { - lock_guard lock(mutex); + s_unique_lock lock(mu); - gps_fix = fix; - gps_eph = eph; - gps_epv = epv; - gps_fix_type = fix_type; - gps_satellites_visible = satellites_visible; + gps_fix = fix; + gps_eph = eph; + gps_epv = epv; + gps_fix_type = fix_type; + gps_satellites_visible = satellites_visible; } //! Returns EPH, EPV, Fix type and satellites visible -void UAS::get_gps_epts(float &eph, float &epv, int &fix_type, int &satellites_visible) +void Data::get_gps_epts(float & eph, float & epv, int & fix_type, int & satellites_visible) { - lock_guard lock(mutex); + s_shared_lock lock(mu); - eph = gps_eph; - epv = gps_epv; - fix_type = gps_fix_type; - satellites_visible = gps_satellites_visible; + eph = gps_eph; + epv = gps_epv; + fix_type = gps_fix_type; + satellites_visible = gps_satellites_visible; } //! Retunrs last GPS RAW message -sensor_msgs::NavSatFix::Ptr UAS::get_gps_fix() -{ - lock_guard lock(mutex); - return gps_fix; -} - -/* -*- transform -*- */ - -//! Stack static transform into vector -void UAS::add_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr, std::vector& vector) -{ - geometry_msgs::TransformStamped static_transform; - - static_transform.header.stamp = ros::Time::now(); - static_transform.header.frame_id = frame_id; - static_transform.child_frame_id = child_id; - tf::transformEigenToMsg(tr, static_transform.transform); - - vector.emplace_back(static_transform); -} - -//! Publishes static transform -void UAS::publish_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr) +sensor_msgs::msg::NavSatFix Data::get_gps_fix() { - geometry_msgs::TransformStamped static_transformStamped; - - static_transformStamped.header.stamp = ros::Time::now(); - static_transformStamped.header.frame_id = frame_id; - static_transformStamped.child_frame_id = child_id; - tf::transformEigenToMsg(tr, static_transformStamped.transform); - - tf2_static_broadcaster.sendTransform(static_transformStamped); + s_shared_lock lock(mu); + return gps_fix; } diff --git a/mavros/src/lib/uas_executor.cpp b/mavros/src/lib/uas_executor.cpp new file mode 100644 index 000000000..567b15a84 --- /dev/null +++ b/mavros/src/lib/uas_executor.cpp @@ -0,0 +1,45 @@ +/* + * Copyright 2022 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief MAVROS UAS Node Executor + * @file uas_executor.hpp + * @author Vladimir Ermakov + */ + +#include + +#include "mavros/uas_executor.hpp" + +using namespace mavros; // NOLINT +using namespace mavros::uas; // NOLINT +using namespace std::chrono_literals; // NOLINT + +UASExecutor::UASExecutor(const rclcpp::ExecutorOptions & options) +: MultiThreadedExecutor(options, select_number_of_threads(), true, 1000ms), + source_system(0), + source_component(0) +{ +} + +size_t UASExecutor::select_number_of_threads() +{ + // return std::max(16, std::min(std::thread::hardware_concurrency(), 4)); + return std::clamp(std::thread::hardware_concurrency(), 4, 16); +} + +void UASExecutor::set_ids(uint8_t sysid, uint8_t compid) +{ + source_system = sysid; + source_component = compid; +} + +void UASExecutor::run(size_t thread_id) +{ + utils::set_this_thread_name("uas-exec/%d.%d/%zu", source_system, source_component, thread_id); + MultiThreadedExecutor::run(thread_id); +} diff --git a/mavros/src/lib/uas_stringify.cpp b/mavros/src/lib/uas_stringify.cpp index a626df5b8..34131a826 100644 --- a/mavros/src/lib/uas_stringify.cpp +++ b/mavros/src/lib/uas_stringify.cpp @@ -1,23 +1,27 @@ -/** - * @brief MAVROS UAS manager - * @file uas.cpp - * @author Vladimir Ermakov - */ /* - * Copyright 2014 Vladimir Ermakov. + * Copyright 2014,2021 Vladimir Ermakov. * * This file is part of the mavros package and subject to the license terms * in the top-level LICENSE file of the mavros repository. * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ +/** + * @brief MAVROS UAS manager + * @file uas.cpp + * @author Vladimir Ermakov + */ #include #include #include -#include -#include +#include +#include -using namespace mavros; +#include "mavros/mavros_uas.hpp" +#include "mavros/px4_custom_mode.hpp" + +using namespace mavros; // NOLINT +using namespace mavros::uas; // NOLINT using mavros::utils::enum_value; @@ -30,26 +34,26 @@ typedef std::unordered_map cmode_map; * ArduPlane/defines.h */ static const cmode_map arduplane_cmode_map{{ - { 0, "MANUAL" }, - { 1, "CIRCLE" }, - { 2, "STABILIZE" }, - { 3, "TRAINING" }, - { 4, "ACRO" }, - { 5, "FBWA" }, - { 6, "FBWB" }, - { 7, "CRUISE" }, - { 8, "AUTOTUNE" }, - { 10, "AUTO" }, - { 11, "RTL" }, - { 12, "LOITER" }, - { 14, "LAND" }, // not in list - { 15, "GUIDED" }, - { 16, "INITIALISING" }, - { 17, "QSTABILIZE" }, // QuadPlane - { 18, "QHOVER" }, - { 19, "QLOITER" }, - { 20, "QLAND" }, - { 21, "QRTL" } + {0, "MANUAL"}, + {1, "CIRCLE"}, + {2, "STABILIZE"}, + {3, "TRAINING"}, + {4, "ACRO"}, + {5, "FBWA"}, + {6, "FBWB"}, + {7, "CRUISE"}, + {8, "AUTOTUNE"}, + {10, "AUTO"}, + {11, "RTL"}, + {12, "LOITER"}, + {14, "LAND"}, // not in list + {15, "GUIDED"}, + {16, "INITIALISING"}, + {17, "QSTABILIZE"}, // QuadPlane + {18, "QHOVER"}, + {19, "QLOITER"}, + {20, "QLAND"}, + {21, "QRTL"} }}; /** APM:Copter custom mode -> string @@ -57,26 +61,26 @@ static const cmode_map arduplane_cmode_map{{ * ArduCopter/defines.h */ static const cmode_map arducopter_cmode_map{{ - { 0, "STABILIZE" }, - { 1, "ACRO" }, - { 2, "ALT_HOLD" }, - { 3, "AUTO" }, - { 4, "GUIDED" }, - { 5, "LOITER" }, - { 6, "RTL" }, - { 7, "CIRCLE" }, - { 8, "POSITION" }, // not in list - { 9, "LAND" }, - { 10, "OF_LOITER" }, - { 11, "DRIFT" }, // renamed, prev name: APPROACH - { 13, "SPORT" }, - { 14, "FLIP" }, - { 15, "AUTOTUNE" }, - { 16, "POSHOLD" }, - { 17, "BRAKE" }, - { 18, "THROW" }, - { 19, "AVOID_ADSB" }, - { 20, "GUIDED_NOGPS" } + {0, "STABILIZE"}, + {1, "ACRO"}, + {2, "ALT_HOLD"}, + {3, "AUTO"}, + {4, "GUIDED"}, + {5, "LOITER"}, + {6, "RTL"}, + {7, "CIRCLE"}, + {8, "POSITION"}, // not in list + {9, "LAND"}, + {10, "OF_LOITER"}, + {11, "DRIFT"}, // renamed, prev name: APPROACH + {13, "SPORT"}, + {14, "FLIP"}, + {15, "AUTOTUNE"}, + {16, "POSHOLD"}, + {17, "BRAKE"}, + {18, "THROW"}, + {19, "AVOID_ADSB"}, + {20, "GUIDED_NOGPS"} }}; /** APM:Rover custom mode -> string @@ -84,18 +88,18 @@ static const cmode_map arducopter_cmode_map{{ * APMrover2/defines.h */ static const cmode_map apmrover2_cmode_map{{ - { 0, "MANUAL" }, - { 1, "ACRO" }, - { 3, "STEERING" }, - { 4, "HOLD" }, - { 5, "LOITER" }, - { 6, "FOLLOW" }, - { 7, "SIMPLE" }, - { 10, "AUTO" }, - { 11, "RTL" }, - { 12, "SMART_RTL" }, - { 15, "GUIDED" }, - { 16, "INITIALISING" } + {0, "MANUAL"}, + {1, "ACRO"}, + {3, "STEERING"}, + {4, "HOLD"}, + {5, "LOITER"}, + {6, "FOLLOW"}, + {7, "SIMPLE"}, + {10, "AUTO"}, + {11, "RTL"}, + {12, "SMART_RTL"}, + {15, "GUIDED"}, + {16, "INITIALISING"} }}; /** ArduSub custom mode -> string @@ -105,174 +109,185 @@ static const cmode_map apmrover2_cmode_map{{ * ArduSub/defines.h */ static const cmode_map ardusub_cmode_map{{ - { 0, "STABILIZE" }, - { 1, "ACRO" }, - { 2, "ALT_HOLD" }, - { 3, "AUTO" }, // n/a - { 4, "GUIDED" }, // n/a - { 5, "VELHOLD" }, - { 6, "RTL" }, // n/a - { 7, "CIRCLE" }, // n/a - { 9, "SURFACE" }, - { 10, "OF_LOITER" }, // deprecated - { 11, "DRIFT" }, // n/a - { 13, "TRANSECT" }, - { 14, "FLIP" }, // n/a - { 15, "AUTOTUNE" }, // n/a - { 16, "POSHOLD" }, - { 17, "BRAKE" }, // n/a - { 18, "THROW" }, - { 19, "MANUAL" } + {0, "STABILIZE"}, + {1, "ACRO"}, + {2, "ALT_HOLD"}, + {3, "AUTO"}, // n/a + {4, "GUIDED"}, // n/a + {5, "VELHOLD"}, + {6, "RTL"}, // n/a + {7, "CIRCLE"}, // n/a + {9, "SURFACE"}, + {10, "OF_LOITER"}, // deprecated + {11, "DRIFT"}, // n/a + {13, "TRANSECT"}, + {14, "FLIP"}, // n/a + {15, "AUTOTUNE"}, // n/a + {16, "POSHOLD"}, + {17, "BRAKE"}, // n/a + {18, "THROW"}, + {19, "MANUAL"} }}; //! PX4 custom mode -> string static const cmode_map px4_cmode_map{{ - { px4::define_mode(px4::custom_mode::MAIN_MODE_MANUAL), "MANUAL" }, - { px4::define_mode(px4::custom_mode::MAIN_MODE_ACRO), "ACRO" }, - { px4::define_mode(px4::custom_mode::MAIN_MODE_ALTCTL), "ALTCTL" }, - { px4::define_mode(px4::custom_mode::MAIN_MODE_POSCTL), "POSCTL" }, - { px4::define_mode(px4::custom_mode::MAIN_MODE_OFFBOARD), "OFFBOARD" }, - { px4::define_mode(px4::custom_mode::MAIN_MODE_STABILIZED), "STABILIZED" }, - { px4::define_mode(px4::custom_mode::MAIN_MODE_RATTITUDE), "RATTITUDE" }, - { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_MISSION), "AUTO.MISSION" }, - { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_LOITER), "AUTO.LOITER" }, - { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_RTL), "AUTO.RTL" }, - { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_LAND), "AUTO.LAND" }, - { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_RTGS), "AUTO.RTGS" }, - { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_READY), "AUTO.READY" }, - { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_TAKEOFF), "AUTO.TAKEOFF" }, - { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_FOLLOW_TARGET), "AUTO.FOLLOW_TARGET" }, - { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_PRECLAND), "AUTO.PRECLAND" }, + {px4::define_mode(px4::custom_mode::MAIN_MODE_MANUAL), "MANUAL"}, + {px4::define_mode(px4::custom_mode::MAIN_MODE_ACRO), "ACRO"}, + {px4::define_mode(px4::custom_mode::MAIN_MODE_ALTCTL), "ALTCTL"}, + {px4::define_mode(px4::custom_mode::MAIN_MODE_POSCTL), "POSCTL"}, + {px4::define_mode(px4::custom_mode::MAIN_MODE_OFFBOARD), "OFFBOARD"}, + {px4::define_mode(px4::custom_mode::MAIN_MODE_STABILIZED), "STABILIZED"}, + {px4::define_mode(px4::custom_mode::MAIN_MODE_RATTITUDE), "RATTITUDE"}, + {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_MISSION), "AUTO.MISSION"}, + {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_LOITER), "AUTO.LOITER"}, + {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_RTL), "AUTO.RTL"}, + {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_LAND), "AUTO.LAND"}, + {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_RTGS), "AUTO.RTGS"}, + {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_READY), "AUTO.READY"}, + {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_TAKEOFF), "AUTO.TAKEOFF"}, + {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_FOLLOW_TARGET), "AUTO.FOLLOW_TARGET"}, + {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_PRECLAND), "AUTO.PRECLAND"}, }}; -static inline std::string str_base_mode(int base_mode) { - return utils::format("MODE(0x%2X)", base_mode); +static inline std::string str_base_mode(int base_mode) +{ + return utils::format("MODE(0x%2X)", base_mode); } -static std::string str_custom_mode(uint32_t custom_mode) { - return utils::format("CMODE(%u)", custom_mode); +static std::string str_custom_mode(uint32_t custom_mode) +{ + return utils::format("CMODE(%u)", custom_mode); } -static std::string str_mode_cmap(const cmode_map &cmap, uint32_t custom_mode) +static std::string str_mode_cmap(const cmode_map & cmap, uint32_t custom_mode) { - auto it = cmap.find(custom_mode); - if (it != cmap.end()) - return it->second; - else - return str_custom_mode(custom_mode); + auto it = cmap.find(custom_mode); + if (it != cmap.end()) { + return it->second; + } else { + return str_custom_mode(custom_mode); + } } static inline std::string str_mode_px4(uint32_t custom_mode_int) { - px4::custom_mode custom_mode(custom_mode_int); + px4::custom_mode custom_mode(custom_mode_int); - // clear fields - custom_mode.reserved = 0; - if (custom_mode.main_mode != px4::custom_mode::MAIN_MODE_AUTO) { - ROS_WARN_COND_NAMED(custom_mode.sub_mode != 0, "uas", "PX4: Unknown sub-mode %d.%d", - custom_mode.main_mode, custom_mode.sub_mode); - custom_mode.sub_mode = 0; - } + // clear fields + custom_mode.mode.reserved = 0; + if (custom_mode.mode.main_mode != px4::custom_mode::MAIN_MODE_AUTO) { + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger("uas"), + custom_mode.mode.sub_mode != 0, "PX4: Unknown sub-mode %d.%d", + custom_mode.mode.main_mode, custom_mode.mode.sub_mode); + custom_mode.mode.sub_mode = 0; + } - return str_mode_cmap(px4_cmode_map, custom_mode.data); + return str_mode_cmap(px4_cmode_map, custom_mode.data); } -static inline bool is_apm_copter(UAS::MAV_TYPE type) +static inline bool is_apm_copter(uas::MAV_TYPE type) { - return type == UAS::MAV_TYPE::QUADROTOR || - type == UAS::MAV_TYPE::HEXAROTOR || - type == UAS::MAV_TYPE::OCTOROTOR || - type == UAS::MAV_TYPE::TRICOPTER || - type == UAS::MAV_TYPE::COAXIAL; + return type == uas::MAV_TYPE::QUADROTOR || + type == uas::MAV_TYPE::HEXAROTOR || + type == uas::MAV_TYPE::OCTOROTOR || + type == uas::MAV_TYPE::TRICOPTER || + type == uas::MAV_TYPE::COAXIAL; } std::string UAS::str_mode_v10(uint8_t base_mode, uint32_t custom_mode) { - if (!(base_mode & enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED))) - return str_base_mode(base_mode); + if (!(base_mode & enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED))) { + return str_base_mode(base_mode); + } - auto type = get_type(); - auto ap = get_autopilot(); - if (MAV_AUTOPILOT::ARDUPILOTMEGA == ap) { - if (is_apm_copter(type)) - return str_mode_cmap(arducopter_cmode_map, custom_mode); - else if (type == MAV_TYPE::FIXED_WING) - return str_mode_cmap(arduplane_cmode_map, custom_mode); - else if (type == MAV_TYPE::GROUND_ROVER) - return str_mode_cmap(apmrover2_cmode_map, custom_mode); - else if (type == MAV_TYPE::SURFACE_BOAT) - return str_mode_cmap(apmrover2_cmode_map, custom_mode); // NOTE: #1051 for now (19.06.2018) boat is same as rover - else if (type == MAV_TYPE::SUBMARINE) - return str_mode_cmap(ardusub_cmode_map, custom_mode); - else { - ROS_WARN_THROTTLE_NAMED(30, "uas", "MODE: Unknown APM based FCU! Type: %d", enum_value(type)); - return str_custom_mode(custom_mode); - } - } - else if (MAV_AUTOPILOT::PX4 == ap) - return str_mode_px4(custom_mode); - else - /* TODO: other autopilot */ - return str_custom_mode(custom_mode); + auto type = get_type(); + auto ap = get_autopilot(); + if (MAV_AUTOPILOT::ARDUPILOTMEGA == ap) { + if (is_apm_copter(type)) { + return str_mode_cmap(arducopter_cmode_map, custom_mode); + } else if (type == MAV_TYPE::FIXED_WING) { + return str_mode_cmap(arduplane_cmode_map, custom_mode); + } else if (type == MAV_TYPE::GROUND_ROVER) { + return str_mode_cmap(apmrover2_cmode_map, custom_mode); + } else if (type == MAV_TYPE::SURFACE_BOAT) { + // NOTE(vooon): #1051 for now (19.06.2018) boat is same as rover + return str_mode_cmap(apmrover2_cmode_map, custom_mode); + } else if (type == MAV_TYPE::SUBMARINE) { + return str_mode_cmap(ardusub_cmode_map, custom_mode); + } else { + RCLCPP_WARN_THROTTLE( + get_logger(), + *get_clock(), 30000, "MODE: Unknown APM based FCU! Type: %d", enum_value(type)); + return str_custom_mode(custom_mode); + } + } else if (MAV_AUTOPILOT::PX4 == ap) { + return str_mode_px4(custom_mode); + } else { + /* TODO(vooon): other autopilot */ + return str_custom_mode(custom_mode); + } } /* XXX TODO * Add a fallback CMODE(dec) decoder for unknown FCU's */ -static bool cmode_find_cmap(const cmode_map &cmap, std::string &cmode_str, uint32_t &cmode) +static bool cmode_find_cmap(const cmode_map & cmap, std::string & cmode_str, uint32_t & cmode) { - // 1. try find by name - for (auto &mode : cmap) { - if (mode.second == cmode_str) { - cmode = mode.first; - return true; - } - } + // 1. try find by name + for (auto & mode : cmap) { + if (mode.second == cmode_str) { + cmode = mode.first; + return true; + } + } - // 2. try convert integer - //! @todo parse CMODE(dec) - try { - cmode = std::stoi(cmode_str, 0, 0); - return true; - } - catch (std::invalid_argument &ex) { - // failed - } + // 2. try convert integer + //! @todo parse CMODE(dec) + try { + cmode = std::stoi(cmode_str, 0, 0); + return true; + } catch (std::invalid_argument & ex) { + // failed + } - // Debugging output. - std::ostringstream os; - for (auto &mode : cmap) - os << " " << mode.second; + // Debugging output. + std::ostringstream os; + for (auto & mode : cmap) { + os << " " << mode.second; + } - ROS_ERROR_STREAM_NAMED("uas", "MODE: Unknown mode: " << cmode_str); - ROS_INFO_STREAM_NAMED("uas", "MODE: Known modes are:" << os.str()); + auto lg = rclcpp::get_logger("uas"); + RCLCPP_ERROR_STREAM(lg, "MODE: Unknown mode: " << cmode_str); + RCLCPP_INFO_STREAM(lg, "MODE: Known modes are:" << os.str()); - return false; + return false; } -bool UAS::cmode_from_str(std::string cmode_str, uint32_t &custom_mode) +bool UAS::cmode_from_str(std::string cmode_str, uint32_t & custom_mode) { - // upper case - std::transform(cmode_str.begin(), cmode_str.end(), cmode_str.begin(), std::ref(toupper)); + // upper case + std::transform(cmode_str.begin(), cmode_str.end(), cmode_str.begin(), std::ref(toupper)); - auto type = get_type(); - auto ap = get_autopilot(); - if (MAV_AUTOPILOT::ARDUPILOTMEGA == ap) { - if (is_apm_copter(type)) - return cmode_find_cmap(arducopter_cmode_map, cmode_str, custom_mode); - else if (type == MAV_TYPE::FIXED_WING) - return cmode_find_cmap(arduplane_cmode_map, cmode_str, custom_mode); - else if (type == MAV_TYPE::GROUND_ROVER) - return cmode_find_cmap(apmrover2_cmode_map, cmode_str, custom_mode); - else if (type == MAV_TYPE::SURFACE_BOAT) - return cmode_find_cmap(apmrover2_cmode_map, cmode_str, custom_mode); - else if (type == MAV_TYPE::SUBMARINE) - return cmode_find_cmap(ardusub_cmode_map, cmode_str, custom_mode); - } - else if (MAV_AUTOPILOT::PX4 == ap) - return cmode_find_cmap(px4_cmode_map, cmode_str, custom_mode); + auto type = get_type(); + auto ap = get_autopilot(); + if (MAV_AUTOPILOT::ARDUPILOTMEGA == ap) { + if (is_apm_copter(type)) { + return cmode_find_cmap(arducopter_cmode_map, cmode_str, custom_mode); + } else if (type == MAV_TYPE::FIXED_WING) { + return cmode_find_cmap(arduplane_cmode_map, cmode_str, custom_mode); + } else if (type == MAV_TYPE::GROUND_ROVER) { + return cmode_find_cmap(apmrover2_cmode_map, cmode_str, custom_mode); + } else if (type == MAV_TYPE::SURFACE_BOAT) { + return cmode_find_cmap(apmrover2_cmode_map, cmode_str, custom_mode); + } else if (type == MAV_TYPE::SUBMARINE) { + return cmode_find_cmap(ardusub_cmode_map, cmode_str, custom_mode); + } + } else if (MAV_AUTOPILOT::PX4 == ap) { + return cmode_find_cmap(px4_cmode_map, cmode_str, custom_mode); + } - ROS_ERROR_NAMED("uas", "MODE: Unsupported FCU"); - return false; + RCLCPP_ERROR(get_logger(), "MODE: Unsupported FCU"); + return false; } diff --git a/mavros/src/lib/uas_tf.cpp b/mavros/src/lib/uas_tf.cpp new file mode 100644 index 000000000..0c8c74d30 --- /dev/null +++ b/mavros/src/lib/uas_tf.cpp @@ -0,0 +1,49 @@ +/* + * Copyright 2014,2015,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief MAVROS UAS TF part + * @file uas_tf.cpp + * @author Vladimir Ermakov + */ + +#include +#include +#include + +#include "mavros/mavros_uas.hpp" +#include "tf2_eigen/tf2_eigen.h" + +using namespace mavros::uas; // NOLINT + + +void UAS::add_static_transform( + const std::string & frame_id, const std::string & child_id, + const Eigen::Affine3d & tr, + std::vector & vector) +{ + geometry_msgs::msg::TransformStamped static_transform = tf2::eigenToTransform(tr); + + static_transform.header.stamp = this->now(); + static_transform.header.frame_id = frame_id; + static_transform.child_frame_id = child_id; + + vector.emplace_back(static_transform); +} + +void UAS::publish_static_transform( + const std::string & frame_id, const std::string & child_id, + const Eigen::Affine3d & tr) +{ + geometry_msgs::msg::TransformStamped static_transform_stamped = tf2::eigenToTransform(tr); + + static_transform_stamped.header.stamp = this->now(); + static_transform_stamped.header.frame_id = frame_id; + static_transform_stamped.child_frame_id = child_id; + + tf2_static_broadcaster.sendTransform(static_transform_stamped); +} diff --git a/mavros/src/lib/uas_timesync.cpp b/mavros/src/lib/uas_timesync.cpp index a5c0b008a..c5cad3494 100644 --- a/mavros/src/lib/uas_timesync.cpp +++ b/mavros/src/lib/uas_timesync.cpp @@ -4,51 +4,49 @@ * @author Vladimir Ermakov */ /* - * Copyright 2014,2017 Vladimir Ermakov, M.H.Kabir. + * Copyright 2014,2017,2021 Vladimir Ermakov, M.H.Kabir. * * This file is part of the mavros package and subject to the license terms * in the top-level LICENSE file of the mavros repository. * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ -#include -#include -#include -#include -#include -#include +#include "mavros/mavros_uas.hpp" -using namespace mavros; +using namespace mavros; // NOLINT +using namespace mavros::uas; // NOLINT /* -*- time syncronise functions -*- */ -static inline ros::Time ros_time_from_ns(const uint64_t stamp_ns) { - return ros::Time( - stamp_ns / 1000000000UL, // t_sec - stamp_ns % 1000000000UL); // t_nsec +static inline rclcpp::Time ros_time_from_ns(const uint64_t stamp_ns) +{ + return rclcpp::Time( + stamp_ns / 1000000000UL, // t_sec + stamp_ns % 1000000000UL); // t_nsec } -ros::Time UAS::synchronise_stamp(uint32_t time_boot_ms) { - // copy offset from atomic var - uint64_t offset_ns = time_offset; - - if (offset_ns > 0 || tsync_mode == timesync_mode::PASSTHROUGH) { - uint64_t stamp_ns = static_cast(time_boot_ms) * 1000000UL + offset_ns; - return ros_time_from_ns(stamp_ns); - } - else - return ros::Time::now(); +rclcpp::Time UAS::synchronise_stamp(uint32_t time_boot_ms) +{ + // copy offset from atomic var + uint64_t offset_ns = time_offset; + + if (offset_ns > 0 || tsync_mode == timesync_mode::PASSTHROUGH) { + uint64_t stamp_ns = static_cast(time_boot_ms) * 1000000UL + offset_ns; + return ros_time_from_ns(stamp_ns); + } else { + return this->now(); + } } -ros::Time UAS::synchronise_stamp(uint64_t time_usec) { - uint64_t offset_ns = time_offset; +rclcpp::Time UAS::synchronise_stamp(uint64_t time_usec) +{ + uint64_t offset_ns = time_offset; - if (offset_ns > 0 || tsync_mode == timesync_mode::PASSTHROUGH) { - uint64_t stamp_ns = time_usec * 1000UL + offset_ns; - return ros_time_from_ns(stamp_ns); - } - else - return ros::Time::now(); + if (offset_ns > 0 || tsync_mode == timesync_mode::PASSTHROUGH) { + uint64_t stamp_ns = time_usec * 1000UL + offset_ns; + return ros_time_from_ns(stamp_ns); + } else { + return this->now(); + } } - diff --git a/mavros/src/mavros/__init__.py b/mavros/src/mavros/__init__.py deleted file mode 100644 index ccb27a03b..000000000 --- a/mavros/src/mavros/__init__.py +++ /dev/null @@ -1,53 +0,0 @@ -# -*- coding: utf-8 -*- -# vim:set ts=4 sw=4 et: - -__all__ = ( - 'get_namespace', - 'set_namespace', - 'get_topic', - 'DEFAULT_NAMESPACE' -) - -DEFAULT_NAMESPACE = 'mavros' - -# global namespace storage -_mavros_ns = None -_mavros_ns_update = [] - - -def get_namespace(): - """ - Returns mavros node namespace - """ - global _mavros_ns - if _mavros_ns is None: - raise RuntimeError("mavros namespace is uninitialized! " - "Call mavros.set_namespace() first!") - return _mavros_ns - - -def set_namespace(ns=DEFAULT_NAMESPACE): - """ - Sets namespace of mavros node - """ - global _mavros_ns, _mavros_ns_update - _mavros_ns = ns - - for cb in _mavros_ns_update: - if callable(cb): - cb() - - -def register_on_namespace_update(cb): - """ - Call callback after namespace update - """ - global _mavros_ns_update - _mavros_ns_update.append(cb) - - -def get_topic(*args): - """ - Create topic name for mavros node - """ - return '/'+'/'.join((get_namespace(), ) + args) diff --git a/mavros/src/mavros/command.py b/mavros/src/mavros/command.py deleted file mode 100644 index 2d62e57ad..000000000 --- a/mavros/src/mavros/command.py +++ /dev/null @@ -1,52 +0,0 @@ -# -*- coding: utf-8 -*- -# vim:set ts=4 sw=4 et: -# -# Copyright 2014,2015 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -__all__ = ( - 'long', - 'int', - 'arming', - 'set_home', - 'takeoff', - 'land', - 'trigger_control', -) - -import rospy -import mavros - -from mavros_msgs.srv import CommandLong, CommandInt, CommandBool, CommandHome, CommandTOL, CommandTriggerControl, CommandTriggerInterval - - -def _get_proxy(service, type): - return rospy.ServiceProxy(mavros.get_topic('cmd', service), type) - - -long = None -int = None -arming = None -set_home = None -takeoff = None -land = None -trigger_control = None - - -def _setup_services(): - global long, int, arming, set_home, takeoff, land, trigger_control - long = _get_proxy('command', CommandLong) - int = _get_proxy('command_int', CommandInt) - arming = _get_proxy('arming', CommandBool) - set_home = _get_proxy('set_home', CommandHome) - takeoff = _get_proxy('takeoff', CommandTOL) - land = _get_proxy('land', CommandTOL) - trigger_control = _get_proxy('trigger_control', CommandTriggerControl) - trigger_interval = _get_proxy('trigger_interval', CommandTriggerInterval) - - -# register updater -mavros.register_on_namespace_update(_setup_services) diff --git a/mavros/src/mavros/event_launcher.py b/mavros/src/mavros/event_launcher.py deleted file mode 100644 index 86a11f19f..000000000 --- a/mavros/src/mavros/event_launcher.py +++ /dev/null @@ -1,311 +0,0 @@ -# -*- coding: utf-8 -*- -# vim:set ts=4 sw=4 et: -# -# Copyright 2015 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -from __future__ import print_function - -import os -import sys -import time -import shlex -import rospy -import mavros -import signal -import argparse -import threading -import subprocess - -from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse -from mavros_msgs.msg import State - - -class EventHandler(object): - """ - Base class for event handlers - """ - - __slots__ = [ - 'name', 'events', 'actions', 'lock', - ] - - def __init__(self, name, events=[], actions=[]): - self.name = name - self.events = events - self.actions = actions - self.lock = threading.RLock() - - def __str__(self): - return self.name - - def __call__(self, event): - if event not in self.events: - return # not our event - - idx = self.events.index(event) - action = self.actions[idx] - - if hasattr(self, 'action_' + action): - getattr(self, 'action_' + action)() - else: - rospy.logerr("Misconfiguration of %s, there no action '%s'", - self, action) - - def spin_once(self): - raise NotImplementedError - - -class ShellHandler(EventHandler): - """ - Simple program runner - """ - - # XXX TODO: - # simple readlines() from Popen.stdout and .stderr simply don't work - it blocks - # so need to find a way for such logging, but for now i don't process process output. - - __slots__ = [ - 'process', 'command', 'args', 'logfile', - ] - - def __init__(self, name, command, args=[], logfile=None, events=[], actions=[]): - super(ShellHandler, self).__init__(name, events, actions) - self.process = None - self.command = command - self.args = args - self.logfile = logfile - - def action_run(self): - with self.lock: - if self.process: - rospy.loginfo("%s: process still active, terminating before run", str(self)) - self.action_stop() - - rospy.loginfo("%s: starting...", self) - - args = [self.command] + self.args - if self.logfile: - child_stdout = open(self.logfile, 'a') - child_stderr = subprocess.STDOUT - else: - child_stdout = subprocess.PIPE - child_stderr = subprocess.PIPE - - if hasattr(child_stdout, 'write'): - child_stdout.write("\n--- run cut: %s ---\n" % time.ctime()) - - self.process = subprocess.Popen(args, stdout=child_stdout, stderr=child_stderr, - close_fds=True, preexec_fn=os.setsid) - - poll_result = self.process.poll() - if poll_result is None or poll_result == 0: - rospy.loginfo("%s: started, pid: %s", self, self.process.pid) - else: - rospy.logerr("Failed to start '%s'", self) - self.process = None - - def action_stop(self): - with self.lock: - if not self.process: - return - - rospy.loginfo("%s: stoppig...", self) - - # code from roslaunch.nodeprocess _stop_unix - pid = self.process.pid - pgid = os.getpgid(pid) - - def poll_timeout(timeout): - timeout_t = time.time() + timeout - retcode = self.process.poll() - while time.time() < timeout_t and retcode is None: - time.sleep(0.1) - retcode = self.process.poll() - return retcode - - try: - rospy.loginfo("%s: sending SIGINT to pid [%s] pgid [%s]", self, pid, pgid) - os.killpg(pgid, signal.SIGINT) - - retcode = poll_timeout(15.0) - if retcode is None: - rospy.logwarn("%s: escalating to SIGTERM", self) - os.killpg(pgid, signal.SIGTERM) - - retcode = poll_timeout(2.0) - if retcode is None: - rospy.logerr("%s: escalating to SIGKILL, may still be running", self) - try: - os.killpg(pgid, signal.SIGKILL) - except OSError as ex: - rospy.logerr("%s: %s", self, ex) - - if retcode is not None: - rospy.loginfo("%s: process (pid %s) terminated, exit code: %s", - self, pid, retcode) - finally: - self.process = None - - def spin_once(self): - with self.lock: - if self.process: - poll_result = self.process.poll() - if poll_result is not None: - rospy.loginfo("%s: process (pid %s) terminated, exit code: %s", - self, self.process.pid, poll_result) - self.process = None - - -# NOTE: here was RosrunHandler and RoslaunchHandler -# but roslaunch.scriptapi can't launch node from worker thread -# so i decided to leave only ShellHandler. - - -class Launcher(object): - __slots__ = [ - 'handlers', - 'known_events', - 'triggers', - 'prev_armed', - 'state_sub', - ] - - def __init__(self): - self.handlers = [] - self.known_events = ['armed', 'disarmed'] - self.triggers = {} - self.prev_armed = False - - try: - params = rospy.get_param('~') - if not isinstance(params, dict): - raise KeyError("bad configuration") - except KeyError as e: - rospy.logerr('Config error: %s', e) - return - - # load configuration - for k, v in params.items(): - # TODO: add checks for mutually exclusive options - - if 'service' in v: - self._load_trigger(k, v) - elif 'shell' in v: - self._load_shell(k, v) - else: - rospy.logwarn("Skipping unknown section: %s", k) - - # check that events are known - rospy.loginfo("Known events: %s", ', '.join(self.known_events)) - for h in self.handlers: - for evt in h.events: - if evt not in self.known_events: - rospy.logwarn("%s: unknown event: %s", h.name, evt) - - # config loaded, we may subscribe - self.state_sub = rospy.Subscriber( - mavros.get_topic('state'), - State, - self.mavros_state_cb) - - def _load_trigger(self, name, params): - rospy.logdebug("Loading trigger: %s", name) - - def gen_cb(event): - def cb(req): - self(event) - return TriggerResponse(success=True) # try later to check success - return cb - - self.known_events.append(name) - self.triggers[name] = rospy.Service(params['service'], Trigger, gen_cb(name)) - rospy.loginfo("Trigger: %s (%s)", name, params['service']) - - def _load_shell(self, name, params): - rospy.logdebug("Loading shell: %s", name) - - events, actions = self._get_evt_act(params) - - def expandpath(p): - return os.path.expanduser(os.path.expandvars(p)) - - args = params['shell'] - if not isinstance(args, list): - args = shlex.split(args) - - command = expandpath(args[0]) - args = args[1:] - - logfile = params.get('logfile') - if logfile: - logfile = expandpath(logfile) - - rospy.loginfo("Shell: %s (%s)", name, ' '.join([command] + [repr(v) for v in args])) - if logfile: - rospy.loginfo("Log: %s -> %s", name, logfile) - - handler = ShellHandler(name, command, args, logfile, events, actions) - self.handlers.append(handler) - - def _get_evt_act(self, params): - evt = self._param_to_list(params['event']) - act = self._param_to_list(params['action']) - if len(evt) != len(act): - raise ValueError("event and action fileds has different size!") - return evt, act - - def _param_to_list(self, str_or_list): - if isinstance(str_or_list, list): - return [it.strip() for it in str_or_list] - else: - ret = [] - for it in str_or_list.split(): - ret.extend([v.strip() for v in it.split(',') if v]) - return ret - - def __call__(self, event): - rospy.logdebug('Event: %s', event) - for h in self.handlers: - try: - h(event) - except Exception as ex: - import traceback - rospy.logerr("Event %s -> %s exception: %s", event, h, ex) - rospy.logerr(traceback.format_exc()) - - def spin(self): - if not self.handlers: - rospy.logwarn("No event handlers defined, terminating.") - return - - rate = rospy.Rate(1.0) - while not rospy.is_shutdown(): - for h in self.handlers: - h.spin_once() - - rate.sleep() - - def mavros_state_cb(self, msg): - if msg.armed != self.prev_armed: - self.prev_armed = msg.armed - self('armed' if msg.armed else 'disarmed') - - -def main(): - parser = argparse.ArgumentParser( - description="This node launch/terminate processes on events.") - parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) - - args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) - - rospy.init_node("event_launcher") - mavros.set_namespace(args.mavros_ns) - - rospy.loginfo("Starting event launcher...") - - launcher = Launcher() - launcher.spin() diff --git a/mavros/src/mavros/ftp.py b/mavros/src/mavros/ftp.py deleted file mode 100644 index b46d6c772..000000000 --- a/mavros/src/mavros/ftp.py +++ /dev/null @@ -1,234 +0,0 @@ -# -*- coding: utf-8 -*- -# vim:set ts=4 sw=4 et: -# -# Copyright 2014,2015 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -__all__ = ( - 'FTPFile', - 'open', - 'listdir', - 'unlink', - 'mkdir', - 'rmdir', - 'rename', - 'checksum', - 'reset_server' -) - -import os -import rospy -import mavros - -from std_srvs.srv import Empty -from mavros_msgs.msg import FileEntry -from mavros_msgs.srv import FileOpen, FileClose, FileRead, FileList, FileOpenRequest, \ - FileMakeDir, FileRemoveDir, FileRemove, FileWrite, FileTruncate, FileRename, \ - FileChecksum - - -def _get_proxy(service, type): - return rospy.ServiceProxy(mavros.get_topic('ftp', service), type) - - -def _check_raise_errno(ret): - if not ret.success: - raise IOError(ret.r_errno, os.strerror(ret.r_errno)) - - -class FTPFile(object): - """ - FCU file object. - Note that current PX4 firmware only support two connections simultaneously. - """ - - def __init__(self, name, mode): - self.name = None - self.mode = mode - self.open(name, mode) - - def __del__(self): - self.close() - - def open(self, path, mode): - """ - Supported modes: - - 'w': write binary - - 'r': read binary - - 'cw': create excl & write - """ - if mode == 'w' or mode == 'wb': - m = FileOpenRequest.MODE_WRITE - elif mode == 'r' or mode == 'rb': - m = FileOpenRequest.MODE_READ - elif mode == 'cw': - m = FileOpenRequest.MODE_CREATE - else: - raise ValueError("Unknown open mode: {}".format(m)) - - open_ = _get_proxy('open', FileOpen) - try: - ret = open_(file_path=path, mode=m) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - _check_raise_errno(ret) - - self._read = _get_proxy('read', FileRead) - self._write = _get_proxy('write', FileWrite) - - self.name = path - self.mode = mode - self.size = ret.size - self.offset = 0 - - def close(self): - if self.closed: - return - - close_ = _get_proxy('close', FileClose) - try: - ret = close_(file_path=self.name) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - self.name = None - _check_raise_errno(ret) - - def read(self, size=1): - try: - ret = self._read(file_path=self.name, offset=self.offset, size=size) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - _check_raise_errno(ret) - self.offset += len(ret.data) - return bytearray(ret.data) - - def write(self, bin_data): - data_len = len(bin_data) - try: - ret = self._write(file_path=self.name, offset=self.offset, data=bin_data) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - _check_raise_errno(ret) - self.offset += data_len - if self.offset > self.size: - self.size = self.offset - - def tell(self): - return self.offset - - def seek(self, offset, whence=os.SEEK_SET): - if whence is os.SEEK_SET: - self.offset = offset - elif whence is os.SEEK_END: - self.offset = offset + self.size - elif whence is os.SEEK_CUR: - self.offset += offset - else: - raise ValueError("Unknown whence") - - def truncate(self, size=0): - truncate_ = _get_proxy('truncate', FileTruncate) - try: - ret = truncate_(file_path=self.name, length=size) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - _check_raise_errno(ret) - - @property - def closed(self): - return self.name is None - - def __enter__(self): - return self - - def __exit__(self, exc_type, exc_value, traceback): - self.close() - - -def open(path, mode): - """Open file on FCU""" - return FTPFile(path, mode) - - -def listdir(path): - """List directory :path: contents""" - try: - list_ = _get_proxy('list', FileList) - ret = list_(dir_path=path) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - _check_raise_errno(ret) - return ret.list - - -def unlink(path): - """Remove :path: file""" - remove = _get_proxy('remove', FileRemove) - try: - ret = remove(file_path=path) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - _check_raise_errno(ret) - - -def mkdir(path): - """Create directory :path:""" - mkdir_ = _get_proxy('mkdir', FileMakeDir) - try: - ret = mkdir_(dir_path=path) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - _check_raise_errno(ret) - - -def rmdir(path): - """Remove directory :path:""" - rmdir_ = _get_proxy('rmdir', FileRemoveDir) - try: - ret = rmdir_(dir_path=path) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - _check_raise_errno(ret) - - -def rename(old_path, new_path): - """Rename :old_path: to :new_path:""" - rename_ = _get_proxy('rename', FileRename) - try: - ret = rename_(old_path=old_path, new_path=new_path) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - _check_raise_errno(ret) - - -def checksum(path): - """Calculate CRC32 for :path:""" - checksum_ = _get_proxy('checksum', FileChecksum) - try: - ret = checksum_(file_path=path) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - _check_raise_errno(ret) - return ret.crc32 - - -def reset_server(): - reset = _get_proxy('reset', Empty) - try: - reset() - except rospy.ServiceException as ex: - raise IOError(str(ex)) diff --git a/mavros/src/mavros/mission.py b/mavros/src/mavros/mission.py deleted file mode 100644 index 65e1bfae4..000000000 --- a/mavros/src/mavros/mission.py +++ /dev/null @@ -1,147 +0,0 @@ -# -*- python -*- -# vim:set ts=4 sw=4 et: -# -# Copyright 2014 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -import csv -import time - -import rospy -import mavros - -from mavros_msgs.msg import Waypoint, WaypointList, CommandCode -from mavros_msgs.srv import WaypointPull, WaypointPush, WaypointClear, \ - WaypointSetCurrent - - -FRAMES = { - Waypoint.FRAME_GLOBAL: 'GAA', - Waypoint.FRAME_GLOBAL_REL_ALT: 'GRA', - Waypoint.FRAME_LOCAL_ENU: 'LOC-ENU', - Waypoint.FRAME_LOCAL_NED: 'LOC-NED', - Waypoint.FRAME_MISSION: 'MIS' -} - -NAV_CMDS = { - CommandCode.NAV_LAND: 'LAND', - CommandCode.NAV_LOITER_TIME: 'LOITER-TIME', - CommandCode.NAV_LOITER_TURNS: 'LOITER-TURNS', - CommandCode.NAV_LOITER_UNLIM: 'LOITER-UNLIM', - CommandCode.NAV_RETURN_TO_LAUNCH: 'RTL', - CommandCode.NAV_TAKEOFF: 'TAKEOFF', - CommandCode.NAV_WAYPOINT: 'WAYPOINT', - # Maybe later i will add this enum to message - 112: 'COND-DELAY', - 113: 'COND-CHANGE-ALT', - 114: 'COND-DISTANCE', - 115: 'COND-YAW', - 177: 'DO-JUMP', - 178: 'DO-CHANGE-SPEED', - 181: 'DO-SET-RELAY', - 182: 'DO-REPEAT-RELAY', - 183: 'DO-SET-SERVO', - 184: 'DO-REPEAT-SERVO', - 201: 'DO-SET-ROI', -} - - -class WaypointFile(object): - """Base class for waypoint file parsers""" - def read(self, file_): - """Returns a iterable of waypoints""" - raise NotImplementedError - - def write(self, file_, waypoints): - """Writes waypoints to file""" - raise NotImplementedError - - -class QGroundControlWP(WaypointFile): - """Parse QGC waypoint file""" - - file_header = 'QGC WPL 120' - known_versions = (110, 120) - - class CSVDialect(csv.Dialect): - delimiter = '\t' - doublequote = False - skipinitialspace = True - lineterminator = '\r\n' - quoting = csv.QUOTE_NONE - - def read(self, file_): - got_header = False - for data in csv.reader(file_, self.CSVDialect): - if data[0].startswith('#'): - continue; # skip comments (i think in next format version they add this) - - if not got_header: - qgc, wpl, ver = data[0].split(' ', 3) - ver = int(ver) - if qgc == 'QGC' and wpl == 'WPL' and ver in self.known_versions: - got_header = True - - else: - yield Waypoint( - is_current = bool(int(data[1])), - frame = int(data[2]), - command = int(data[3]), - param1 = float(data[4]), - param2 = float(data[5]), - param3 = float(data[6]), - param4 = float(data[7]), - x_lat = float(data[8]), - y_long = float(data[9]), - z_alt = float(data[10]), - autocontinue = bool(int(data[11])) - ) - - def write(self, file_, waypoints): - writer = csv.writer(file_, self.CSVDialect) - writer.writerow((self.file_header ,)) - for seq, w in enumerate(waypoints): - writer.writerow(( - seq, - int(w.is_current), - w.frame, - w.command, - w.param1, - w.param2, - w.param3, - w.param4, - w.x_lat, - w.y_long, - w.z_alt, - int(w.autocontinue) - )) - - - -pull = None -push = None -clear = None -set_current = None - - -def subscribe_waypoints(cb, **kvargs): - return rospy.Subscriber(mavros.get_topic('mission', 'waypoints'), WaypointList, cb, **kvargs) - - -def _setup_services(): - global pull, push, clear, set_current - - def _get_proxy(name, type): - return rospy.ServiceProxy(mavros.get_topic('mission', name), type) - - pull = _get_proxy('pull', WaypointPull) - push = _get_proxy('push', WaypointPush) - clear = _get_proxy('clear', WaypointClear) - set_current = _get_proxy('set_current', WaypointSetCurrent) - - -# register updater -mavros.register_on_namespace_update(_setup_services) diff --git a/mavros/src/mavros/nuttx_crc32.py b/mavros/src/mavros/nuttx_crc32.py deleted file mode 100644 index 975a1578d..000000000 --- a/mavros/src/mavros/nuttx_crc32.py +++ /dev/null @@ -1,57 +0,0 @@ -# -*- coding: utf-8 -*- -# vim:set ts=4 sw=4 et: -# -# Copyright 2014 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -__all__ = ( - 'nuttx_crc32', -) - -CRC32_TAB = ( - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d -) - - -def nuttx_crc32(bytes, crc32val=0): - """ - CRC32 algo from NuttX. - """ - for b in bytearray(bytes): - crc32val = CRC32_TAB[(crc32val ^ b) & 0xff] ^ (crc32val >> 8) - - return crc32val diff --git a/mavros/src/mavros/param.py b/mavros/src/mavros/param.py deleted file mode 100644 index ea44f6a8d..000000000 --- a/mavros/src/mavros/param.py +++ /dev/null @@ -1,216 +0,0 @@ -# -*- coding: utf-8 -*- -# vim:set ts=4 sw=4 et: -# -# Copyright 2014 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -import csv -import time -import rospy -import mavros - -from mavros_msgs.msg import ParamValue -from mavros_msgs.srv import ParamPull, ParamPush, ParamGet, ParamSet - - -class Parameter(object): - """Class representing one parameter""" - def __init__(self, param_id, param_value=0): - self.param_id = param_id - self.param_value = param_value - - def __repr__(self): - return "".format(self.param_id, self.param_value) - - -class ParamFile(object): - """Base class for param file parsers""" - def __init__(self, args): - pass - - def read(self, file_): - """Returns a iterable of Parameters""" - raise NotImplementedError - - def write(self, file_, parametes): - """Writes Parameters to file""" - raise NotImplementedError - - -class MavProxyParam(ParamFile): - """Parse MavProxy param files""" - - class CSVDialect(csv.Dialect): - delimiter = ' ' - doublequote = False - skipinitialspace = True - lineterminator = '\r\n' - quoting = csv.QUOTE_NONE - - def read(self, file_): - to_numeric = lambda x: float(x) if '.' in x else int(x) - - for data in csv.reader(file_, self.CSVDialect): - if data[0].startswith('#'): - continue # skip comments - - if len(data) != 2: - raise ValueError("wrong field count") - - yield Parameter(data[0].strip(), to_numeric(data[1])); - - def write(self, file_, parameters): - writer = csv.writer(file_, self.CSVDialect) - file_.write("#NOTE: " + time.strftime("%d.%m.%Y %T") + self.CSVDialect.lineterminator) - for p in parameters: - writer.writerow((p.param_id, p.param_value)) - - -class MissionPlannerParam(ParamFile): - """Parse MissionPlanner param files""" - - class CSVDialect(csv.Dialect): - delimiter = ',' - doublequote = False - skipinitialspace = True - lineterminator = '\r\n' - quoting = csv.QUOTE_NONE - - def read(self, file_): - to_numeric = lambda x: float(x) if '.' in x else int(x) - - for data in csv.reader(file_, self.CSVDialect): - if data[0].startswith('#'): - continue # skip comments - - if len(data) != 2: - raise ValueError("wrong field count") - - yield Parameter(data[0].strip(), to_numeric(data[1])); - - def write(self, file_, parameters): - writer = csv.writer(file_, self.CSVDialect) - writer.writerow(("#NOTE: " + time.strftime("%d.%m.%Y %T") ,)) - for p in parameters: - writer.writerow((p.param_id, p.param_value)) - - -class QGroundControlParam(ParamFile): - """Parse QGC param files""" - - class CSVDialect(csv.Dialect): - delimiter = '\t' - doublequote = False - skipinitialspace = True - lineterminator = '\n' - quoting = csv.QUOTE_NONE - - def read(self, file_): - to_numeric = lambda x: float(x) if '.' in x else int(x) - - for data in csv.reader(file_, self.CSVDialect): - if data[0].startswith('#'): - continue # skip comments - - if len(data) != 5: - raise ValueError("wrong field count") - - yield Parameter(data[2].strip(), to_numeric(data[3])); - - def write(self, file_, parameters): - def to_type(x): - if isinstance(x, float): - return 9 # REAL32 - elif isinstance(x, int): - return 6 # INT32 - else: - raise ValueError("unknown type: " + repr(type(x))) - - sysid = rospy.get_param(mavros.get_topic('target_system_id'), 1) - compid = rospy.get_param(mavros.get_topic('target_component_id'), 1) - - writer = csv.writer(file_, self.CSVDialect) - writer.writerow(("# NOTE: " + time.strftime("%d.%m.%Y %T"), )) - writer.writerow(("# Onboard parameters saved by mavparam for ({}, {})".format(sysid, compid), )) - writer.writerow(("# MAV ID" , "COMPONENT ID", "PARAM NAME", "VALUE", "(TYPE)")) - for p in parameters: - writer.writerow((sysid, compid, p.param_id, p.param_value, to_type(p.param_value), )) # XXX - - -def param_ret_value(ret): - if ret.value.integer != 0: - return ret.value.integer - elif ret.value.real != 0.0: - return ret.value.real - else: - return 0 - - -def param_get(param_id): - try: - get = rospy.ServiceProxy(mavros.get_topic('param', 'get'), ParamGet) - ret = get(param_id=param_id) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - if not ret.success: - raise IOError("Request failed.") - - return param_ret_value(ret) - - -def param_set(param_id, value): - if isinstance(value, float): - val = ParamValue(integer=0, real=value) - else: - val = ParamValue(integer=value, real=0.0) - - try: - set = rospy.ServiceProxy(mavros.get_topic('param', 'set'), ParamSet) - ret = set(param_id=param_id, value=val) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - if not ret.success: - raise IOError("Request failed.") - - return param_ret_value(ret) - - -def param_get_all(force_pull=False): - try: - pull = rospy.ServiceProxy(mavros.get_topic('param', 'pull'), ParamPull) - ret = pull(force_pull=force_pull) - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - if not ret.success: - raise IOError("Request failed.") - - params = rospy.get_param(mavros.get_topic('param')) - - return (ret.param_received, - list(sorted((Parameter(k, v) for k, v in params.items()), - key=lambda p: p.param_id)) - ) - - -def param_set_list(param_list): - # 1. load parameters to parameter server - for p in param_list: - rospy.set_param(mavros.get_topic('param', p.param_id), p.param_value) - - # 2. request push all - try: - push = rospy.ServiceProxy(mavros.get_topic('param', 'push'), ParamPush) - ret = push() - except rospy.ServiceException as ex: - raise IOError(str(ex)) - - if not ret.success: - raise IOError("Request failed.") - - return ret.param_transfered diff --git a/mavros/src/mavros/setpoint.py b/mavros/src/mavros/setpoint.py deleted file mode 100644 index 3822d7429..000000000 --- a/mavros/src/mavros/setpoint.py +++ /dev/null @@ -1,66 +0,0 @@ -# -*- coding: utf-8 -*- -# vim:set ts=4 sw=4 et: -# -# Copyright 2015 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -import rospy -import mavros - -from std_msgs.msg import Header, Float64 -from geometry_msgs.msg import TwistStamped, PoseStamped, PoseWithCovarianceStamped, \ - Vector3, Vector3Stamped, Point, Quaternion - - -def get_pub_accel_accel(**kvargs): - """ - Returns publisher for :setpoint_accel: plugin, :accel: topic - """ - return rospy.Publisher(mavros.get_topic('setpoint_accel', 'accel'), Vector3Stamped, **kvargs) - - -def get_pub_attitude_cmd_vel(**kvargs): - """ - Returns publisher for :setpoint_attitude: plugin, :cmd_vel: topic - """ - return rospy.Publisher(mavros.get_topic('setpoint_attitude', 'cmd_vel'), PoseStamped, **kvargs) - - -def get_pub_attitude_throttle(**kvargs): - """ - Returns publisher for :setpoint_attitude: plugin, :cmd_vel: topic - """ - return rospy.Publisher(mavros.get_topic('setpoint_attitude', 'att_throttle'), Float64, **kvargs) - - -def get_pub_attitude_pose(**kvargs): - """ - Returns publisher for :setpoint_attitude: plugin, :attituse: topic - """ - return rospy.Publisher(mavros.get_topic('setpoint_attitude', 'attitude'), PoseStamped, **kvargs) - - -def get_pub_attitude_posecov(**kvargs): - """ - Returns publisher for :setpoint_attitude: plugin, :attituse: topic (with covariance) - """ - raise DeprecationWarning("PoseWithCovarianceStamped subscriber removed.") - - -def get_pub_position_local(**kvargs): - """ - Returns publisher for :setpoint_position: plugin, :local: topic - """ - return rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped, **kvargs) - - -def get_pub_velocity_cmd_vel(**kvargs): - """ - Returns publisher for :setpoint_velocity: plugin, :cmd_vel: topic - """ - return rospy.Publisher(mavros.get_topic('setpoint_velocity', 'cmd_vel'), TwistStamped, **kvargs) - - diff --git a/mavros/src/mavros/utils.py b/mavros/src/mavros/utils.py deleted file mode 100644 index 9e80e9732..000000000 --- a/mavros/src/mavros/utils.py +++ /dev/null @@ -1,54 +0,0 @@ -# -*- coding: utf-8 -*- -# vim:set ts=4 sw=4 et: -# -# Copyright 2014 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -from __future__ import print_function - -import os -import sys - -import rospy -import mavros -import threading - - -def print_if(cond, *args, **kvargs): - if cond: - print(*args, **kvargs) - - -def fault(*args, **kvargs): - kvargs['file'] = sys.stderr - print(*args, **kvargs) - sys.exit(1) - - -def wait_fcu_connection(timeout=None): - """ - Wait until establishing FCU connection - """ - from mavros_msgs.msg import State - try: - msg = rospy.wait_for_message(mavros.get_topic('state'), State, timeout) - if msg.connected: - return True - except rospy.ROSException as e: - return False - - connected = threading.Event() - def handler(msg): - if msg.connected: - connected.set() - - sub = rospy.Subscriber( - mavros.get_topic('state'), - State, - handler - ) - - return connected.wait(timeout) diff --git a/mavros/src/mavros_node.cpp b/mavros/src/mavros_node.cpp index 9178a9160..93057032a 100644 --- a/mavros/src/mavros_node.cpp +++ b/mavros/src/mavros_node.cpp @@ -1,25 +1,84 @@ -/** - * @brief MAVROS Node - * @file mavros_node.cpp - * @author Vladimir Ermakov - */ /* - * Copyright 2013,2014,2015 Vladimir Ermakov. + * Copyright 2013,2014,2015,2021 Vladimir Ermakov. * * This file is part of the mavros package and subject to the license terms * in the top-level LICENSE file of the mavros repository. * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ +/** + * @brief MAVROS Node + * @file mavros_node.cpp + * @author Vladimir Ermakov + */ -#include +#include +#include +#include -int main(int argc, char *argv[]) +#include "mavros/mavros_router.hpp" +#include "mavros/mavros_uas.hpp" +#include "rclcpp/rclcpp.hpp" + +/** + * MAVROS Node is a transition helper, a component loader preconfigured + * to work similar to mavros v1. + */ +int main(int argc, char * argv[]) { - ros::init(argc, argv, "mavros"); + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 2); - mavros::MavRos mavros; - mavros.spin(); + rclcpp::NodeOptions options; + // options.use_intra_process_comms(true); - return 0; -} + std::string fcu_url, gcs_url, uas_url; + int tgt_system = 1, tgt_component = 1; + + auto node = std::make_shared("mavros_node", options); + exec.add_node(node); + + node->declare_parameter("fcu_url", fcu_url); + node->declare_parameter("gcs_url", gcs_url); + node->declare_parameter("tgt_system", tgt_system); + node->declare_parameter("tgt_component", tgt_component); + + node->get_parameter("fcu_url", fcu_url); + node->get_parameter("gcs_url", gcs_url); + node->get_parameter("tgt_system", tgt_system); + node->get_parameter("tgt_component", tgt_component); + + uas_url = mavros::utils::format("/uas%d", tgt_system); + + RCLCPP_INFO(node->get_logger(), "Starting mavros_node container"); + RCLCPP_INFO(node->get_logger(), "FCU URL: %s", fcu_url.c_str()); + RCLCPP_INFO(node->get_logger(), "GCS URL: %s", gcs_url.c_str()); + RCLCPP_INFO(node->get_logger(), "UAS Prefix: %s", uas_url.c_str()); + RCLCPP_INFO(node->get_logger(), "Starting mavros router node"); + auto router_node = std::make_shared(options, "mavros_router"); + exec.add_node(router_node); + + { + std::vector router_params{}; + + if (fcu_url != "") { + router_params.emplace_back("fcu_urls", std::vector{fcu_url}); + } + if (gcs_url != "") { + router_params.emplace_back("gcs_urls", std::vector{gcs_url}); + } + router_params.emplace_back("uas_urls", std::vector{uas_url}); + + router_node->set_parameters(router_params); + } + + RCLCPP_INFO(node->get_logger(), "Starting mavros uas node"); + auto uas_node = std::make_shared( + options, "mavros", uas_url, tgt_system, + tgt_component); + exec.add_node(uas_node); + + exec.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/mavros/src/plugins/3dr_radio.cpp b/mavros/src/plugins/3dr_radio.cpp deleted file mode 100644 index c17c96cfd..000000000 --- a/mavros/src/plugins/3dr_radio.cpp +++ /dev/null @@ -1,160 +0,0 @@ -/** - * @brief 3DR Radio status plugin - * @file 3dr_radio.cpp - * @author Vladimir Ermakov - * - * @addtogroup plugin - * @{ - */ -/* - * Copyright 2014,2015,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include - -#include - -namespace mavros { -namespace std_plugins { -/** - * @brief 3DR Radio plugin. - */ -class TDRRadioPlugin : public plugin::PluginBase { -public: - TDRRadioPlugin() : PluginBase(), - nh("~"), - has_radio_status(false), - diag_added(false), - low_rssi(0) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - nh.param("tdr_radio/low_rssi", low_rssi, 40); - - status_pub = nh.advertise("radio_status", 10); - - enable_connection_cb(); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&TDRRadioPlugin::handle_radio_status), - make_handler(&TDRRadioPlugin::handle_radio), - }; - } - -private: - ros::NodeHandle nh; - - bool has_radio_status; - bool diag_added; - int low_rssi; - - ros::Publisher status_pub; - - std::mutex diag_mutex; - mavros_msgs::RadioStatus::Ptr last_status; - - /* -*- message handlers -*- */ - - void handle_radio_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RADIO_STATUS &rst) - { - has_radio_status = true; - handle_message(msg, rst); - } - - void handle_radio(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::RADIO &rst) - { - if (has_radio_status) - return; - - // actually the same data, but from earlier modems - handle_message(msg, rst); - } - - template - void handle_message(const mavlink::mavlink_message_t *mmsg, msgT &rst) - { - if (mmsg->sysid != '3' || mmsg->compid != 'D') - ROS_WARN_THROTTLE_NAMED(30, "radio", "RADIO_STATUS not from 3DR modem?"); - - auto msg = boost::make_shared(); - - msg->header.stamp = ros::Time::now(); - -#define RST_COPY(field) msg->field = rst.field - RST_COPY(rssi); - RST_COPY(remrssi); - RST_COPY(txbuf); - RST_COPY(noise); - RST_COPY(remnoise); - RST_COPY(rxerrors); - RST_COPY(fixed); -#undef RST_COPY - - // valid for 3DR modem - msg->rssi_dbm = (rst.rssi / 1.9) - 127; - msg->remrssi_dbm = (rst.remrssi / 1.9) - 127; - - // add diag at first event - if (!diag_added) { - UAS_DIAG(m_uas).add("3DR Radio", this, &TDRRadioPlugin::diag_run); - diag_added = true; - } - - // store last status for diag - { - std::lock_guard lock(diag_mutex); - last_status = msg; - } - - status_pub.publish(msg); - } - - - void diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) - { - std::lock_guard lock(diag_mutex); - - if (!last_status) { - stat.summary(2, "No data"); - return; - } - else if (last_status->rssi < low_rssi) - stat.summary(1, "Low RSSI"); - else if (last_status->remrssi < low_rssi) - stat.summary(1, "Low remote RSSI"); - else - stat.summary(0, "Normal"); - - stat.addf("RSSI", "%u", last_status->rssi); - stat.addf("RSSI (dBm)", "%.1f", last_status->rssi_dbm); - stat.addf("Remote RSSI", "%u", last_status->remrssi); - stat.addf("Remote RSSI (dBm)", "%.1f", last_status->remrssi_dbm); - stat.addf("Tx buffer (%)", "%u", last_status->txbuf); - stat.addf("Noice level", "%u", last_status->noise); - stat.addf("Remote noice level", "%u", last_status->remnoise); - stat.addf("Rx errors", "%u", last_status->rxerrors); - stat.addf("Fixed", "%u", last_status->fixed); - } - - void connection_cb(bool connected) override - { - UAS_DIAG(m_uas).removeByName("3DR Radio"); - diag_added = false; - } - -}; -} // namespace std_plugins -} // namespace mavros - -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::TDRRadioPlugin, mavros::plugin::PluginBase) diff --git a/mavros/src/plugins/actuator_control.cpp b/mavros/src/plugins/actuator_control.cpp index ec12e5df7..fe68189f8 100644 --- a/mavros/src/plugins/actuator_control.cpp +++ b/mavros/src/plugins/actuator_control.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2015 Marcel Stüttgen + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief ActuatorControl plugin * @file actuator_control.cpp @@ -6,84 +14,86 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2015 Marcel Stüttgen - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -#include +#include "mavros_msgs/msg/actuator_control.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace std_plugins { /** * @brief ActuatorControl plugin + * @plugin actuator_control * * Sends actuator controls to FCU controller. */ -class ActuatorControlPlugin : public plugin::PluginBase { +class ActuatorControlPlugin : public plugin::Plugin +{ public: - ActuatorControlPlugin() : PluginBase(), - nh("~") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - target_actuator_control_pub = nh.advertise("target_actuator_control", 10); - actuator_control_sub = nh.subscribe("actuator_control", 10, &ActuatorControlPlugin::actuator_control_cb, this); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&ActuatorControlPlugin::handle_actuator_control_target), - }; - } + explicit ActuatorControlPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "actuator_control") + { + auto sensor_qos = rclcpp::SensorDataQoS(); + + target_actuator_control_pub = node->create_publisher( + "target_actuator_control", sensor_qos); + actuator_control_sub = node->create_subscription( + "actuator_control", sensor_qos, std::bind( + &ActuatorControlPlugin::actuator_control_cb, this, _1)); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&ActuatorControlPlugin::handle_actuator_control_target), + }; + } private: - ros::NodeHandle nh; - - ros::Publisher target_actuator_control_pub; - ros::Subscriber actuator_control_sub; - - /* -*- rx handlers -*- */ - - void handle_actuator_control_target(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ACTUATOR_CONTROL_TARGET &actuator_control_target) - { - auto actuator_control_target_msg = boost::make_shared(); - actuator_control_target_msg->header.stamp = m_uas->synchronise_stamp(actuator_control_target.time_usec); - - actuator_control_target_msg->group_mix = actuator_control_target.group_mlx; - const auto &arr = actuator_control_target.controls; - std::copy(arr.cbegin(), arr.cend(), actuator_control_target_msg->controls.begin()); - - target_actuator_control_pub.publish(actuator_control_target_msg); - } - - /* -*- callbacks -*- */ - - void actuator_control_cb(const mavros_msgs::ActuatorControl::ConstPtr &req) { - //! about groups, mixing and channels: @p https://pixhawk.org/dev/mixing - //! message definiton here: @p https://mavlink.io/en/messages/common.html#SET_ACTUATOR_CONTROL_TARGET - mavlink::common::msg::SET_ACTUATOR_CONTROL_TARGET act{}; - - act.time_usec = req->header.stamp.toNSec() / 1000; - act.group_mlx = req->group_mix; - act.target_system = m_uas->get_tgt_system(); - act.target_component = m_uas->get_tgt_component(); - std::copy(req->controls.begin(), req->controls.end(), act.controls.begin()); // std::array = boost::array - - UAS_FCU(m_uas)->send_message_ignore_drop(act); - } + rclcpp::Publisher::SharedPtr target_actuator_control_pub; + rclcpp::Subscription::SharedPtr actuator_control_sub; + + /* -*- rx handlers -*- */ + + void handle_actuator_control_target( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::ACTUATOR_CONTROL_TARGET & act, + plugin::filter::ComponentAndOk filter [[maybe_unused]]) + { + auto ract = mavros_msgs::msg::ActuatorControl(); + ract.header.stamp = uas->synchronise_stamp(act.time_usec); + ract.group_mix = act.group_mlx; + ract.controls = act.controls; + + target_actuator_control_pub->publish(ract); + } + + /* -*- callbacks -*- */ + + void actuator_control_cb(const mavros_msgs::msg::ActuatorControl::SharedPtr req) + { + //! about groups, mixing and channels: @p https://pixhawk.org/dev/mixing + //! message definiton here: @p https://mavlink.io/en/messages/common.html#SET_ACTUATOR_CONTROL_TARGET + mavlink::common::msg::SET_ACTUATOR_CONTROL_TARGET act{}; + + act.time_usec = get_time_usec(req->header.stamp); + act.group_mlx = req->group_mix; + uas->msg_set_target(act); + act.controls = req->controls; + + uas->send_message(act); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::ActuatorControlPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::ActuatorControlPlugin) diff --git a/mavros/src/plugins/altitude.cpp b/mavros/src/plugins/altitude.cpp index e76bab865..4ec63c169 100644 --- a/mavros/src/plugins/altitude.cpp +++ b/mavros/src/plugins/altitude.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2015 Andreas Antener . + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Altitude plugin * @file altitude.cpp @@ -6,70 +14,74 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2015 Andreas Antener . - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -#include +#include "mavros_msgs/msg/altitude.hpp" + +namespace mavros +{ +namespace std_plugins +{ -namespace mavros { -namespace std_plugins { /** * @brief Altitude plugin. + * @plugin altitude */ -class AltitudePlugin : public plugin::PluginBase { +class AltitudePlugin : public plugin::Plugin +{ public: - AltitudePlugin() : PluginBase(), - nh("~") - { } + explicit AltitudePlugin(plugin::UASPtr uas_) + : Plugin(uas_, "altitude") + { + enable_node_watch_parameters(); - /** - * Plugin initializer. Constructor should not do this. - */ - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); + node_declare_and_watch_parameter( + "frame_id", "map", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); - nh.param("frame_id", frame_id, "map"); - altitude_pub = nh.advertise("altitude", 10); - } + auto sensor_qos = rclcpp::SensorDataQoS(); - Subscriptions get_subscriptions() override - { - return { - make_handler(&AltitudePlugin::handle_altitude), - }; - } + altitude_pub = node->create_publisher("altitude", sensor_qos); + } -private: - ros::NodeHandle nh; - std::string frame_id; + Subscriptions get_subscriptions() override + { + return { + make_handler(&AltitudePlugin::handle_altitude), + }; + } - ros::Publisher altitude_pub; +private: + std::string frame_id; - void handle_altitude(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ALTITUDE &altitude) - { - auto ros_msg = boost::make_shared(); - ros_msg->header = m_uas->synchronized_header(frame_id, altitude.time_usec); + rclcpp::Publisher::SharedPtr altitude_pub; - ros_msg->monotonic = altitude.altitude_monotonic; - ros_msg->amsl = altitude.altitude_amsl; - ros_msg->local = altitude.altitude_local; - ros_msg->relative = altitude.altitude_relative; - ros_msg->terrain = altitude.altitude_terrain; - ros_msg->bottom_clearance = altitude.bottom_clearance; + void handle_altitude( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::ALTITUDE & altitude, plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto ros_msg = mavros_msgs::msg::Altitude(); + ros_msg.header = uas->synchronized_header(frame_id, altitude.time_usec); + ros_msg.monotonic = altitude.altitude_monotonic; + ros_msg.amsl = altitude.altitude_amsl; + ros_msg.local = altitude.altitude_local; + ros_msg.relative = altitude.altitude_relative; + ros_msg.terrain = altitude.altitude_terrain; + ros_msg.bottom_clearance = altitude.bottom_clearance; - altitude_pub.publish(ros_msg); - } + altitude_pub->publish(ros_msg); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::AltitudePlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::AltitudePlugin) diff --git a/mavros/src/plugins/command.cpp b/mavros/src/plugins/command.cpp index ff0a71c56..b50db296b 100644 --- a/mavros/src/plugins/command.cpp +++ b/mavros/src/plugins/command.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2014,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Command plugin * @file command.cpp @@ -6,452 +13,492 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ +#include #include #include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace mavros { -namespace std_plugins { -static constexpr double ACK_TIMEOUT_DEFAULT = 5.0; +#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/srv/command_long.hpp" +#include "mavros_msgs/srv/command_int.hpp" +#include "mavros_msgs/srv/command_bool.hpp" +#include "mavros_msgs/srv/command_home.hpp" +#include "mavros_msgs/srv/command_tol.hpp" +#include "mavros_msgs/srv/command_trigger_control.hpp" +#include "mavros_msgs/srv/command_trigger_interval.hpp" +#include "mavros_msgs/srv/command_vtol_transition.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT + +static constexpr std::chrono::nanoseconds ACK_TIMEOUT_DEFAULT = 5000ms; using utils::enum_value; using lock_guard = std::lock_guard; using unique_lock = std::unique_lock; -class CommandTransaction { +class CommandTransaction +{ public: - std::mutex cond_mutex; - std::condition_variable ack; - uint16_t expected_command; - uint8_t result; - - explicit CommandTransaction(uint16_t command) : - ack(), - expected_command(command), - // Default result if wait ack timeout - result(enum_value(mavlink::common::MAV_RESULT::FAILED)) - { } + uint16_t expected_command; + std::promise promise; + + explicit CommandTransaction(uint16_t command) + : expected_command(command) + {} }; /** * @brief Command plugin. + * @plugin command * * Send any command via COMMAND_LONG */ -class CommandPlugin : public plugin::PluginBase { +class CommandPlugin : public plugin::Plugin +{ public: - CommandPlugin() : PluginBase(), - cmd_nh("~cmd"), - use_comp_id_system_control(false) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - double command_ack_timeout; - - cmd_nh.param("command_ack_timeout", command_ack_timeout, ACK_TIMEOUT_DEFAULT); - cmd_nh.param("use_comp_id_system_control", use_comp_id_system_control, false); - - command_ack_timeout_dt = ros::Duration(command_ack_timeout); - - command_long_srv = cmd_nh.advertiseService("command", &CommandPlugin::command_long_cb, this); - command_ack_srv = cmd_nh.advertiseService("command_ack", &CommandPlugin::command_ack_cb, this); - command_int_srv = cmd_nh.advertiseService("command_int", &CommandPlugin::command_int_cb, this); - arming_srv = cmd_nh.advertiseService("arming", &CommandPlugin::arming_cb, this); - set_home_srv = cmd_nh.advertiseService("set_home", &CommandPlugin::set_home_cb, this); - takeoff_srv = cmd_nh.advertiseService("takeoff", &CommandPlugin::takeoff_cb, this); - land_srv = cmd_nh.advertiseService("land", &CommandPlugin::land_cb, this); - trigger_control_srv = cmd_nh.advertiseService("trigger_control", &CommandPlugin::trigger_control_cb, this); - trigger_interval_srv = cmd_nh.advertiseService("trigger_interval", &CommandPlugin::trigger_interval_cb, this); - vtol_transition_srv = cmd_nh.advertiseService("vtol_transition", &CommandPlugin::vtol_transition_cb, this); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&CommandPlugin::handle_command_ack) - }; - } + explicit CommandPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "cmd"), + use_comp_id_system_control(false), + command_ack_timeout_dt(ACK_TIMEOUT_DEFAULT) + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "command_ack_timeout", command_ack_timeout_dt.seconds(), [&](const rclcpp::Parameter & p) { + command_ack_timeout_dt = rclcpp::Duration::from_seconds(p.as_double()); + }); + + node_declare_and_watch_parameter( + "use_comp_id_system_control", false, [&](const rclcpp::Parameter & p) { + use_comp_id_system_control = p.as_bool(); + }); + + srv_cg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + command_long_srv = + node->create_service( + "~/command", + std::bind( + &CommandPlugin::command_long_cb, this, _1, _2, + _3), rmw_qos_profile_services_default, srv_cg); + command_int_srv = + node->create_service( + "~/command_int", + std::bind( + &CommandPlugin::command_int_cb, this, _1, _2, + _3), rmw_qos_profile_services_default, srv_cg); + arming_srv = + node->create_service( + "~/arming", + std::bind( + &CommandPlugin::arming_cb, this, _1, _2, + _3), rmw_qos_profile_services_default, srv_cg); + set_home_srv = + node->create_service( + "~/set_home", + std::bind( + &CommandPlugin::set_home_cb, this, _1, _2, + _3), rmw_qos_profile_services_default, srv_cg); + takeoff_srv = + node->create_service( + "~/takeoff", + std::bind( + &CommandPlugin::takeoff_cb, this, _1, _2, + _3), rmw_qos_profile_services_default, srv_cg); + land_srv = + node->create_service( + "~/land", + std::bind(&CommandPlugin::land_cb, this, _1, _2, _3), rmw_qos_profile_services_default, + srv_cg); + trigger_control_srv = node->create_service( + "~/trigger_control", std::bind( + &CommandPlugin::trigger_control_cb, this, _1, _2, + _3), rmw_qos_profile_services_default, srv_cg); + trigger_interval_srv = node->create_service( + "~/trigger_interval", std::bind( + &CommandPlugin::trigger_interval_cb, this, _1, _2, + _3), rmw_qos_profile_services_default, srv_cg); + vtol_transition_srv = node->create_service( + "~/vtol_transition", std::bind( + &CommandPlugin::vtol_transition_cb, this, _1, _2, + _3), rmw_qos_profile_services_default, srv_cg); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&CommandPlugin::handle_command_ack) + }; + } private: - using L_CommandTransaction = std::list; - - std::mutex mutex; - - ros::NodeHandle cmd_nh; - ros::ServiceServer command_long_srv; - ros::ServiceServer command_int_srv; - ros::ServiceServer command_ack_srv; - ros::ServiceServer arming_srv; - ros::ServiceServer set_home_srv; - ros::ServiceServer takeoff_srv; - ros::ServiceServer land_srv; - ros::ServiceServer trigger_control_srv; - ros::ServiceServer trigger_interval_srv; - ros::ServiceServer vtol_transition_srv; - - bool use_comp_id_system_control; - - L_CommandTransaction ack_waiting_list; - ros::Duration command_ack_timeout_dt; - - /* -*- message handlers -*- */ - - void handle_command_ack(const mavlink::mavlink_message_t *msg, mavlink::common::msg::COMMAND_ACK &ack) - { - lock_guard lock(mutex); - - // XXX(vooon): place here source ids check - - for (auto &tr : ack_waiting_list) { - if (tr.expected_command == ack.command) { - tr.result = ack.result; - tr.ack.notify_all(); - return; - } - } - - ROS_WARN_THROTTLE_NAMED(10, "cmd", "CMD: Unexpected command %u, result %u", - ack.command, ack.result); - } - - /* -*- mid-level functions -*- */ - - bool wait_ack_for(CommandTransaction &tr) { - unique_lock lock(tr.cond_mutex); - if (tr.ack.wait_for(lock, std::chrono::nanoseconds(command_ack_timeout_dt.toNSec())) == std::cv_status::timeout) { - ROS_WARN_NAMED("cmd", "CMD: Command %u -- wait ack timeout", tr.expected_command); - return false; - } else { - return true; - } - } - - /** - * Common function for command service callbacks. - * - * NOTE: success is bool in messages, but has unsigned char type in C++ - */ - bool send_command_long_and_wait(bool broadcast, - uint16_t command, uint8_t confirmation, - float param1, float param2, - float param3, float param4, - float param5, float param6, - float param7, - unsigned char &success, uint8_t &result) - { - using mavlink::common::MAV_RESULT; - - unique_lock lock(mutex); - - L_CommandTransaction::iterator ack_it; - - /* check transactions */ - for (const auto &tr : ack_waiting_list) { - if (tr.expected_command == command) { - ROS_WARN_THROTTLE_NAMED(10, "cmd", "CMD: Command %u already in progress", command); - return false; - } - } - - /** - * @note APM & PX4 master always send COMMAND_ACK. Old PX4 never. - * Don't expect any ACK in broadcast mode. - */ - bool is_ack_required = (confirmation != 0 || m_uas->is_ardupilotmega() || m_uas->is_px4()) && !broadcast; - if (is_ack_required) - ack_it = ack_waiting_list.emplace(ack_waiting_list.end(), command); - - command_long(broadcast, - command, confirmation, - param1, param2, - param3, param4, - param5, param6, - param7); - - if (is_ack_required) { - lock.unlock(); - bool is_not_timeout = wait_ack_for(*ack_it); - lock.lock(); - - success = is_not_timeout && ack_it->result == enum_value(MAV_RESULT::ACCEPTED); - result = ack_it->result; - - ack_waiting_list.erase(ack_it); - } - else { - success = true; - result = enum_value(MAV_RESULT::ACCEPTED); - } - - return true; - } - - /** - * Common function for COMMAND_INT service callbacks. - */ - bool send_command_int(bool broadcast, - uint8_t frame, uint16_t command, - uint8_t current, uint8_t autocontinue, - float param1, float param2, - float param3, float param4, - int32_t x, int32_t y, - float z, - unsigned char &success) - { - /* Note: seems that COMMAND_INT don't produce COMMAND_ACK - * so wait don't needed. - */ - command_int(broadcast, - frame, command, current, autocontinue, - param1, param2, - param3, param4, - x, y, z); - - success = true; - return true; - } - - bool send_command_ack( uint16_t command, uint8_t req_result, uint8_t progress, int32_t result_param2, - unsigned char &success, uint8_t &res_result) - { - using mavlink::common::MAV_RESULT; - - command_ack(command, - req_result, progress, - result_param2); - - - success = true; - res_result = enum_value(MAV_RESULT::ACCEPTED); - - return true; - } - - - /* -*- low-level send -*- */ - - template - inline void set_target(MsgT &cmd, bool broadcast) - { - using mavlink::minimal::MAV_COMPONENT; - - const uint8_t tgt_sys_id = (broadcast) ? 0 : m_uas->get_tgt_system(); - const uint8_t tgt_comp_id = (broadcast) ? 0 : - (use_comp_id_system_control) ? - enum_value(MAV_COMPONENT::COMP_ID_SYSTEM_CONTROL) : m_uas->get_tgt_component(); - - cmd.target_system = tgt_sys_id; - cmd.target_component = tgt_comp_id; - } - - void command_long(bool broadcast, - uint16_t command, uint8_t confirmation, - float param1, float param2, - float param3, float param4, - float param5, float param6, - float param7) - { - const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation; - - mavlink::common::msg::COMMAND_LONG cmd {}; - set_target(cmd, broadcast); - - cmd.command = command; - cmd.confirmation = confirmation_fixed; - cmd.param1 = param1; - cmd.param2 = param2; - cmd.param3 = param3; - cmd.param4 = param4; - cmd.param5 = param5; - cmd.param6 = param6; - cmd.param7 = param7; - - UAS_FCU(m_uas)->send_message_ignore_drop(cmd); - } - - void command_int(bool broadcast, - uint8_t frame, uint16_t command, - uint8_t current, uint8_t autocontinue, - float param1, float param2, - float param3, float param4, - int32_t x, int32_t y, - float z) - { - mavlink::common::msg::COMMAND_INT cmd {}; - set_target(cmd, broadcast); - - cmd.frame = frame; - cmd.command = command; - cmd.current = current; - cmd.autocontinue = autocontinue; - cmd.param1 = param1; - cmd.param2 = param2; - cmd.param3 = param3; - cmd.param4 = param4; - cmd.x = x; - cmd.y = y; - cmd.z = z; - - UAS_FCU(m_uas)->send_message_ignore_drop(cmd); - } - - void command_ack( uint16_t command, uint8_t result, - uint8_t progress, int32_t result_param2) - { - mavlink::common::msg::COMMAND_ACK cmd {}; - set_target(cmd, false); - - cmd.command = command; - cmd.result = result; - cmd.progress = progress; - cmd.result_param2 = result_param2; - - UAS_FCU(m_uas)->send_message_ignore_drop(cmd); - } - - - /* -*- callbacks -*- */ - - bool command_long_cb(mavros_msgs::CommandLong::Request &req, - mavros_msgs::CommandLong::Response &res) - { - return send_command_long_and_wait(req.broadcast, - req.command, req.confirmation, - req.param1, req.param2, - req.param3, req.param4, - req.param5, req.param6, - req.param7, - res.success, res.result); - } - - bool command_int_cb(mavros_msgs::CommandInt::Request &req, - mavros_msgs::CommandInt::Response &res) - { - return send_command_int(req.broadcast, - req.frame, req.command, - req.current, req.autocontinue, - req.param1, req.param2, - req.param3, req.param4, - req.x, req.y, req.z, - res.success); - } - - bool command_ack_cb(mavros_msgs::CommandAck::Request &req, - mavros_msgs::CommandAck::Response &res) - { - return send_command_ack(req.command, req.result, - req.progress, req.result_param2, - res.success, res.result); - } - - - bool arming_cb(mavros_msgs::CommandBool::Request &req, - mavros_msgs::CommandBool::Response &res) - { - using mavlink::common::MAV_CMD; - return send_command_long_and_wait(false, - enum_value(MAV_CMD::COMPONENT_ARM_DISARM), 1, - (req.value) ? 1.0 : 0.0, - 0, 0, 0, 0, 0, 0, - res.success, res.result); - } - - bool set_home_cb(mavros_msgs::CommandHome::Request &req, - mavros_msgs::CommandHome::Response &res) - { - using mavlink::common::MAV_CMD; - return send_command_long_and_wait(false, - enum_value(MAV_CMD::DO_SET_HOME), 1, - (req.current_gps) ? 1.0 : 0.0, - 0, 0, req.yaw, req.latitude, req.longitude, req.altitude, - res.success, res.result); - } - - bool takeoff_cb(mavros_msgs::CommandTOL::Request &req, - mavros_msgs::CommandTOL::Response &res) - { - using mavlink::common::MAV_CMD; - return send_command_long_and_wait(false, - enum_value(MAV_CMD::NAV_TAKEOFF), 1, - req.min_pitch, - 0, 0, - req.yaw, - req.latitude, req.longitude, req.altitude, - res.success, res.result); - } - - bool land_cb(mavros_msgs::CommandTOL::Request &req, - mavros_msgs::CommandTOL::Response &res) - { - using mavlink::common::MAV_CMD; - return send_command_long_and_wait(false, - enum_value(MAV_CMD::NAV_LAND), 1, - 0, 0, 0, - req.yaw, - req.latitude, req.longitude, req.altitude, - res.success, res.result); - } - - bool trigger_control_cb(mavros_msgs::CommandTriggerControl::Request &req, - mavros_msgs::CommandTriggerControl::Response &res) - { - using mavlink::common::MAV_CMD; - return send_command_long_and_wait(false, - enum_value(MAV_CMD::DO_TRIGGER_CONTROL), 1, - (req.trigger_enable) ? 1.0 : 0.0, - (req.sequence_reset) ? 1.0 : 0.0, - (req.trigger_pause) ? 1.0 : 0.0, - 0, 0, 0, 0, - res.success, res.result); - } - - bool trigger_interval_cb(mavros_msgs::CommandTriggerInterval::Request &req, - mavros_msgs::CommandTriggerInterval::Response &res) - { - using mavlink::common::MAV_CMD; - - // trigger interval can only be set when triggering is disabled - return send_command_long_and_wait(false, - enum_value(MAV_CMD::DO_SET_CAM_TRIGG_INTERVAL), 1, - req.cycle_time, - req.integration_time, - 0, 0, 0, 0, 0, - res.success, res.result); - } - - bool vtol_transition_cb(mavros_msgs::CommandVtolTransition::Request &req, - mavros_msgs::CommandVtolTransition::Response &res) - { - using mavlink::common::MAV_CMD; - return send_command_long_and_wait(false, - enum_value(MAV_CMD::DO_VTOL_TRANSITION), false, - req.state, - 0, 0, 0, 0, 0, 0, - res.success, res.result); - } + using L_CommandTransaction = std::list; + + std::mutex mutex; + + rclcpp::CallbackGroup::SharedPtr srv_cg; + rclcpp::Service::SharedPtr command_long_srv; + rclcpp::Service::SharedPtr command_int_srv; + rclcpp::Service::SharedPtr arming_srv; + rclcpp::Service::SharedPtr set_home_srv; + rclcpp::Service::SharedPtr takeoff_srv; + rclcpp::Service::SharedPtr land_srv; + rclcpp::Service::SharedPtr trigger_control_srv; + rclcpp::Service::SharedPtr trigger_interval_srv; + rclcpp::Service::SharedPtr vtol_transition_srv; + + bool use_comp_id_system_control; + + L_CommandTransaction ack_waiting_list; + rclcpp::Duration command_ack_timeout_dt; + + /* -*- message handlers -*- */ + + void handle_command_ack( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::COMMAND_ACK & ack, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + lock_guard lock(mutex); + + for (auto & tr : ack_waiting_list) { + if (tr.expected_command == ack.command) { + tr.promise.set_value(ack.result); + return; + } + } + + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), + 10000, "CMD: Unexpected command %u, result %u", + ack.command, ack.result); + } + + /* -*- mid-level functions -*- */ + + bool wait_ack_for(CommandTransaction & tr, uint8_t & result) + { + auto future = tr.promise.get_future(); + + auto wres = future.wait_for(command_ack_timeout_dt.to_chrono()); + if (wres != std::future_status::ready) { + RCLCPP_WARN(get_logger(), "CMD: Command %u -- ack timeout", tr.expected_command); + return false; + } else { + result = future.get(); + return true; + } + } + + /** + * Common function for command service callbacks. + * + * NOTE: success is bool in messages, but has unsigned char type in C++ + */ + void send_command_long_and_wait( + bool broadcast, + uint16_t command, uint8_t confirmation, + float param1, float param2, + float param3, float param4, + float param5, float param6, + float param7, + bool & success, uint8_t & result) + { + using mavlink::common::MAV_RESULT; + + unique_lock lock(mutex); + + L_CommandTransaction::iterator ack_it; + + /* check transactions */ + for (const auto & tr : ack_waiting_list) { + if (tr.expected_command == command) { + RCLCPP_WARN_THROTTLE( + get_logger(), + *get_clock(), 10000, "CMD: Command %u already in progress", command); + throw std::logic_error("operation in progress"); + } + } + + /** + * @note APM & PX4 master always send COMMAND_ACK. Old PX4 never. + * Don't expect any ACK in broadcast mode. + */ + bool is_ack_required = (confirmation != 0 || uas->is_ardupilotmega() || uas->is_px4()) && + !broadcast; + if (is_ack_required) { + ack_it = ack_waiting_list.emplace(ack_waiting_list.end(), command); + } + + command_long( + broadcast, + command, confirmation, + param1, param2, + param3, param4, + param5, param6, + param7); + + if (is_ack_required) { + lock.unlock(); + bool is_not_timeout = wait_ack_for(*ack_it, result); + lock.lock(); + + success = is_not_timeout && result == enum_value(MAV_RESULT::ACCEPTED); + + ack_waiting_list.erase(ack_it); + } else { + success = true; + result = enum_value(MAV_RESULT::ACCEPTED); + } + } + + /** + * Common function for COMMAND_INT service callbacks. + */ + void send_command_int( + bool broadcast, + uint8_t frame, uint16_t command, + uint8_t current, uint8_t autocontinue, + float param1, float param2, + float param3, float param4, + int32_t x, int32_t y, + float z, + bool & success) + { + /* Note: seems that COMMAND_INT don't produce COMMAND_ACK + * so wait is not needed. + */ + command_int( + broadcast, + frame, command, current, autocontinue, + param1, param2, + param3, param4, + x, y, z); + + success = true; + } + + /* -*- low-level send -*- */ + + template + inline void set_target(MsgT & cmd, bool broadcast) + { + using mavlink::minimal::MAV_COMPONENT; + + const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system(); + const uint8_t tgt_comp_id = (broadcast) ? 0 : + (use_comp_id_system_control) ? + enum_value(MAV_COMPONENT::COMP_ID_SYSTEM_CONTROL) : uas->get_tgt_component(); + + cmd.target_system = tgt_sys_id; + cmd.target_component = tgt_comp_id; + } + + void command_long( + bool broadcast, + uint16_t command, uint8_t confirmation, + float param1, float param2, + float param3, float param4, + float param5, float param6, + float param7) + { + const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation; + + mavlink::common::msg::COMMAND_LONG cmd {}; + set_target(cmd, broadcast); + + cmd.command = command; + cmd.confirmation = confirmation_fixed; + cmd.param1 = param1; + cmd.param2 = param2; + cmd.param3 = param3; + cmd.param4 = param4; + cmd.param5 = param5; + cmd.param6 = param6; + cmd.param7 = param7; + + uas->send_message(cmd); + } + + void command_int( + bool broadcast, + uint8_t frame, uint16_t command, + uint8_t current, uint8_t autocontinue, + float param1, float param2, + float param3, float param4, + int32_t x, int32_t y, + float z) + { + mavlink::common::msg::COMMAND_INT cmd {}; + set_target(cmd, broadcast); + + cmd.frame = frame; + cmd.command = command; + cmd.current = current; + cmd.autocontinue = autocontinue; + cmd.param1 = param1; + cmd.param2 = param2; + cmd.param3 = param3; + cmd.param4 = param4; + cmd.x = x; + cmd.y = y; + cmd.z = z; + + uas->send_message(cmd); + } + + /* -*- callbacks -*- */ + + void command_long_cb( + const std::shared_ptr req_header [[maybe_unused]], + const mavros_msgs::srv::CommandLong::Request::SharedPtr req, + mavros_msgs::srv::CommandLong::Response::SharedPtr res) + { + // TODO(vooon): rewrite to use async service server + send_command_long_and_wait( + req->broadcast, + req->command, req->confirmation, + req->param1, req->param2, + req->param3, req->param4, + req->param5, req->param6, + req->param7, + res->success, res->result); + } + + void command_int_cb( + const std::shared_ptr req_header [[maybe_unused]], + const mavros_msgs::srv::CommandInt::Request::SharedPtr req, + mavros_msgs::srv::CommandInt::Response::SharedPtr res) + { + send_command_int( + req->broadcast, + req->frame, req->command, + req->current, req->autocontinue, + req->param1, req->param2, + req->param3, req->param4, + req->x, req->y, req->z, + res->success); + } + + void arming_cb( + const std::shared_ptr req_header [[maybe_unused]], + const mavros_msgs::srv::CommandBool::Request::SharedPtr req, + mavros_msgs::srv::CommandBool::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + send_command_long_and_wait( + false, + enum_value(MAV_CMD::COMPONENT_ARM_DISARM), 1, + (req->value) ? 1.0 : 0.0, + 0, 0, 0, 0, 0, 0, + res->success, res->result); + } + + void set_home_cb( + const std::shared_ptr req_header [[maybe_unused]], + const mavros_msgs::srv::CommandHome::Request::SharedPtr req, + mavros_msgs::srv::CommandHome::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + send_command_long_and_wait( + false, + enum_value(MAV_CMD::DO_SET_HOME), 1, + (req->current_gps) ? 1.0 : 0.0, + 0, 0, req->yaw, req->latitude, req->longitude, req->altitude, + res->success, res->result); + } + + void takeoff_cb( + const std::shared_ptr req_header [[maybe_unused]], + const mavros_msgs::srv::CommandTOL::Request::SharedPtr req, + mavros_msgs::srv::CommandTOL::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + send_command_long_and_wait( + false, + enum_value(MAV_CMD::NAV_TAKEOFF), 1, + req->min_pitch, + 0, 0, + req->yaw, + req->latitude, req->longitude, req->altitude, + res->success, res->result); + } + + void land_cb( + const std::shared_ptr req_header [[maybe_unused]], + const mavros_msgs::srv::CommandTOL::Request::SharedPtr req, + mavros_msgs::srv::CommandTOL::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + send_command_long_and_wait( + false, + enum_value(MAV_CMD::NAV_LAND), 1, + 0, 0, 0, + req->yaw, + req->latitude, req->longitude, req->altitude, + res->success, res->result); + } + + void trigger_control_cb( + const std::shared_ptr req_header [[maybe_unused]], + const mavros_msgs::srv::CommandTriggerControl::Request::SharedPtr req, + mavros_msgs::srv::CommandTriggerControl::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + send_command_long_and_wait( + false, + enum_value(MAV_CMD::DO_TRIGGER_CONTROL), 1, + (req->trigger_enable) ? 1.0 : 0.0, + (req->sequence_reset) ? 1.0 : 0.0, + (req->trigger_pause) ? 1.0 : 0.0, + 0, 0, 0, 0, + res->success, res->result); + } + + void trigger_interval_cb( + const std::shared_ptr req_header [[maybe_unused]], + const mavros_msgs::srv::CommandTriggerInterval::Request::SharedPtr req, + mavros_msgs::srv::CommandTriggerInterval::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + + // trigger interval can only be set when triggering is disabled + send_command_long_and_wait( + false, + enum_value(MAV_CMD::DO_SET_CAM_TRIGG_INTERVAL), 1, + req->cycle_time, + req->integration_time, + 0, 0, 0, 0, 0, + res->success, res->result); + } + + void vtol_transition_cb( + const std::shared_ptr req_header [[maybe_unused]], + const mavros_msgs::srv::CommandVtolTransition::Request::SharedPtr req, + mavros_msgs::srv::CommandVtolTransition::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + send_command_long_and_wait( + false, + enum_value(MAV_CMD::DO_VTOL_TRANSITION), false, + req->state, + 0, 0, 0, 0, 0, 0, + res->success, res->result); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::CommandPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::CommandPlugin) diff --git a/mavros/src/plugins/dummy.cpp b/mavros/src/plugins/dummy.cpp index a54619be8..18d0b5b71 100644 --- a/mavros/src/plugins/dummy.cpp +++ b/mavros/src/plugins/dummy.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2013,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Dummy plugin * @file dummy.cpp @@ -7,81 +14,121 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2013,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -namespace mavros { -namespace std_plugins { +namespace mavros +{ +namespace std_plugins +{ /** * @brief Dummy plugin. + * @plugin dummy + * @example_plugin * * Example and "how to" for users. */ -class DummyPlugin : public plugin::PluginBase { +class DummyPlugin : public plugin::Plugin +{ public: - DummyPlugin() : PluginBase(), - nh("~") - { } + /** + * Plugin constructor. + * It must first call initialization of base class. + * + * When second argument is passed, it's used as a sub-node name. + * `node` class member will be initialized with it. + * + * You can use `node->create_xyz()` functions to make any node interfaces. + */ + explicit DummyPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "dummy") + { + } - /** - * Plugin initializer. Constructor should not do this. - */ - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - ROS_INFO_NAMED("dummy", "Dummy::initialize"); - } - - /** - * This function returns message subscriptions. - * - * Each subscription made by PluginBase::make_handler() template. - * Two variations: - * - With automatic decoding and framing error filtering (see handle_heartbeat) - * - Raw message with framig status (see handle_systemtext) - */ - Subscriptions get_subscriptions() override { - return { - /* automatic message deduction by second argument */ - make_handler(&DummyPlugin::handle_heartbeat), - make_handler(&DummyPlugin::handle_sys_status), - /* handle raw message, check framing! */ - make_handler(mavlink::common::msg::STATUSTEXT::MSG_ID, &DummyPlugin::handle_statustext_raw), - make_handler(&DummyPlugin::handle_statustext), - }; - } + /** + * This function returns message subscriptions. + * + * Each subscription made by PluginBase::make_handler() template. + * Two variations: + * - With automatic decoding and framing error filtering (see handle_heartbeat) + * - Raw message with framig status (see handle_systemtext) + */ + Subscriptions get_subscriptions() override + { + return { + /* automatic message deduction by second argument */ + make_handler(&DummyPlugin::handle_heartbeat), + make_handler(&DummyPlugin::handle_sys_status), + /* handle raw message, check framing! */ + make_handler(mavlink::common::msg::STATUSTEXT::MSG_ID, &DummyPlugin::handle_statustext_raw), + make_handler(&DummyPlugin::handle_statustext), + }; + } private: - ros::NodeHandle nh; - - void handle_heartbeat(const mavlink::mavlink_message_t *msg, mavlink::minimal::msg::HEARTBEAT &hb) { - ROS_INFO_STREAM_NAMED("dummy", "Dummy::handle_heartbeat: " << hb.to_yaml()); - } + /** + * This function will be called to handle HEARTBEAT from any source + * when framing is ok. + * + * @param[in] msg raw message frame + * @param[in] hb decoded message (require a type from mavlink c++11 library) + * @param[in] filter an instance of that filter will determine conditions to call that function + */ + void handle_heartbeat( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::minimal::msg::HEARTBEAT & hb, + plugin::filter::AnyOk filter [[maybe_unused]]) + { + RCLCPP_INFO_STREAM(get_logger(), "Dummy::handle_heartbeat: " << hb.to_yaml()); + } - void handle_sys_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYS_STATUS &st) { - ROS_INFO_STREAM_NAMED("dummy", "Dummy::handle_sys_status: " << st.to_yaml()); - } + /** + * This function subscribes for SYS_STATUS message. + * It will be only called when message came from our target system and framing is ok. + * + * @p mavros::plugin::filter::SystemAndOk + */ + void handle_sys_status( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::SYS_STATUS & st, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + RCLCPP_INFO_STREAM(get_logger(), "Dummy::handle_sys_status: " << st.to_yaml()); + } - void handle_statustext(const mavlink::mavlink_message_t *msg, mavlink::common::msg::STATUSTEXT &st) { - ROS_INFO_STREAM_NAMED("dummy", "Dummy::handle_statustext: " << st.to_yaml()); - } + /** + * This function subscribes for STATUSTEXT message. + * It will be only called when message came from out target system & component and framing is ok. + * + * @p mavros::plugin::filter::ComponentAndOk + */ + void handle_statustext( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::STATUSTEXT & st, + plugin::filter::ComponentAndOk filter [[maybe_unused]]) + { + RCLCPP_INFO_STREAM(get_logger(), "Dummy::handle_statustext: " << st.to_yaml()); + } - void handle_statustext_raw(const mavlink::mavlink_message_t *msg, const mavconn::Framing f) { - ROS_INFO_NAMED("dummy", "Dummy::handle_statustext_raw(%p, %d) from %u.%u", (void *)msg, utils::enum_value(f), msg->sysid, msg->compid); - } + /** + * This function will be called for any message with same ID passed to make_handler(), + * and with any framing status (not only ok). + */ + void handle_statustext_raw(const mavlink::mavlink_message_t * msg, const mavconn::Framing f) + { + RCLCPP_INFO( + get_logger(), + "Dummy::handle_statustext_raw(%p, %d) from %u.%u", static_cast(msg), + utils::enum_value(f), msg->sysid, msg->compid); + } }; -} // namespace std_plugins -} // namespace mavros +} // namespace std_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::DummyPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::DummyPlugin) diff --git a/mavros/src/plugins/ftp.cpp b/mavros/src/plugins/ftp.cpp index d50091578..5ce6054f6 100644 --- a/mavros/src/plugins/ftp.cpp +++ b/mavros/src/plugins/ftp.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2014,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief FTP plugin * @file ftp.cpp @@ -6,1070 +13,1184 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ #include #include #include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "std_srvs/srv/empty.hpp" +#include "mavros_msgs/msg/file_entry.hpp" +#include "mavros_msgs/srv/file_list.hpp" +#include "mavros_msgs/srv/file_open.hpp" +#include "mavros_msgs/srv/file_close.hpp" +#include "mavros_msgs/srv/file_read.hpp" +#include "mavros_msgs/srv/file_write.hpp" +#include "mavros_msgs/srv/file_remove.hpp" +#include "mavros_msgs/srv/file_make_dir.hpp" +#include "mavros_msgs/srv/file_remove_dir.hpp" +#include "mavros_msgs/srv/file_truncate.hpp" +#include "mavros_msgs/srv/file_rename.hpp" +#include "mavros_msgs/srv/file_checksum.hpp" // enable debugging messages -//#define FTP_LL_DEBUG +// #define FTP_LL_DEBUG -namespace mavros { -namespace std_plugins { +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT using utils::enum_value; /** * @brief FTP Request message abstraction class * - * @note This class not portable, and works on little-endian machines only. + * @note This class is not portable, and works on little-endian machines only. */ -class FTPRequest : public mavlink::common::msg::FILE_TRANSFER_PROTOCOL { +class FTPRequest : public mavlink::common::msg::FILE_TRANSFER_PROTOCOL +{ public: - /// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. - /// We pad the structure ourselves to 32 bit alignment to avoid usage of any pack pragmas. - struct PayloadHeader { - uint16_t seqNumber; ///< sequence number for message - uint8_t session; ///< Session id for read and write commands - uint8_t opcode; ///< Command opcode - uint8_t size; ///< Size of data - uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - uint8_t padding[2]; ///< 32 bit aligment padding - uint32_t offset; ///< Offsets for List and Read commands - uint8_t data[]; ///< command data, varies by Opcode - }; - - /// @brief Command opcodes - enum Opcode : uint8_t { - kCmdNone, ///< ignored, always acked - kCmdTerminateSession, ///< Terminates open Read session - kCmdResetSessions, ///< Terminates all open Read sessions - kCmdListDirectory, ///< List files in from - kCmdOpenFileRO, ///< Opens file at for reading, returns - kCmdReadFile, ///< Reads bytes from in - kCmdCreateFile, ///< Creates file at for writing, returns - kCmdWriteFile, ///< Writes bytes to in - kCmdRemoveFile, ///< Remove file at - kCmdCreateDirectory, ///< Creates directory at - kCmdRemoveDirectory, ///< Removes Directory at , must be empty - kCmdOpenFileWO, ///< Opens file at for writing, returns - kCmdTruncateFile, ///< Truncate file at to length - kCmdRename, ///< Rename to - kCmdCalcFileCRC32, ///< Calculate CRC32 for file at - kCmdBurstReadFile, ///< Burst download session file - - kRspAck = 128, ///< Ack response - kRspNak ///< Nak response - }; - - /// @brief Error codes returned in Nak response. - enum ErrorCode : uint8_t { - kErrNone, - kErrFail, ///< Unknown failure - kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] - kErrInvalidDataSize, ///< PayloadHeader.size is invalid - kErrInvalidSession, ///< Session is not currently open - kErrNoSessionsAvailable, ///< All available Sessions in use - kErrEOF, ///< Offset past end of file for List and Read commands - kErrUnknownCommand, ///< Unknown command opcode - kErrFailFileExists, ///< File exists already - kErrFailFileProtected ///< File is write protected - }; - - static const char DIRENT_FILE = 'F'; - static const char DIRENT_DIR = 'D'; - static const char DIRENT_SKIP = 'S'; - //! payload.size() - header bytes - static const uint8_t DATA_MAXSZ = 251 - sizeof(PayloadHeader); - - uint8_t *raw_payload() { - return payload.data(); - } - - inline PayloadHeader *header() { - return reinterpret_cast(payload.data()); - } - - uint8_t *data() { - return header()->data; - } - - char *data_c() { - return reinterpret_cast(header()->data); - } - - uint32_t *data_u32() { - return reinterpret_cast(header()->data); - } - - /** - * @brief Copy string to payload - * - * @param[in] s payload string - * @note this function allow null termination inside string - * it used to send multiple strings in one message - */ - void set_data_string(std::string &s) - { - size_t sz = (s.size() < DATA_MAXSZ - 1) ? s.size() : DATA_MAXSZ - 1; - - memcpy(data_c(), s.c_str(), sz); - data_c()[sz] = '\0'; - header()->size = sz; - } - - uint8_t get_target_system_id() { - return target_system; - } - - /** - * @brief Decode and check target system - */ - bool decode_valid(UAS *uas) - { + /// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. + /// We pad the structure ourselves to 32 bit alignment to avoid usage of any pack pragmas. + struct PayloadHeader + { + uint16_t seqNumber; ///< sequence number for message + uint8_t session; ///< Session id for read and write commands + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message + uint8_t padding[2]; ///< 32 bit aligment padding + uint32_t offset; ///< Offsets for List and Read commands + }; + + /// @brief Command opcodes + enum Opcode : uint8_t + { + kCmdNone, ///< ignored, always acked + kCmdTerminateSession, ///< Terminates open Read session + kCmdResetSessions, ///< Terminates all open Read sessions + kCmdListDirectory, ///< List files in from + kCmdOpenFileRO, ///< Opens file at for reading, returns + kCmdReadFile, ///< Reads bytes from in + kCmdCreateFile, ///< Creates file at for writing, returns + kCmdWriteFile, ///< Writes bytes to in + kCmdRemoveFile, ///< Remove file at + kCmdCreateDirectory, ///< Creates directory at + kCmdRemoveDirectory, ///< Removes Directory at , must be empty + kCmdOpenFileWO, ///< Opens file at for writing, returns + kCmdTruncateFile, ///< Truncate file at to length + kCmdRename, ///< Rename to + kCmdCalcFileCRC32, ///< Calculate CRC32 for file at + kCmdBurstReadFile, ///< Burst download session file + + kRspAck = 128, ///< Ack response + kRspNak ///< Nak response + }; + + /// @brief Error codes returned in Nak response. + enum ErrorCode : uint8_t + { + kErrNone, + kErrFail, ///< Unknown failure + kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] + kErrInvalidDataSize, ///< PayloadHeader.size is invalid + kErrInvalidSession, ///< Session is not currently open + kErrNoSessionsAvailable, ///< All available Sessions in use + kErrEOF, ///< Offset past end of file for List and Read commands + kErrUnknownCommand, ///< Unknown command opcode + kErrFailFileExists, ///< File exists already + kErrFailFileProtected ///< File is write protected + }; + + static const char DIRENT_FILE = 'F'; + static const char DIRENT_DIR = 'D'; + static const char DIRENT_SKIP = 'S'; + //! payload.size() - header bytes + static const uint8_t DATA_MAXSZ = 251 - sizeof(PayloadHeader); + + inline const uint8_t * raw_payload() const + { + return payload.data(); + } + + inline uint8_t * raw_payload() + { + return payload.data(); + } + + inline const PayloadHeader * header() const + { + return reinterpret_cast(payload.data()); + } + + inline PayloadHeader * header() + { + return reinterpret_cast(payload.data()); + } + + inline const uint8_t * data() const + { + return payload.data() + sizeof(PayloadHeader); + } + + inline uint8_t * data() + { + return payload.data() + sizeof(PayloadHeader); + } + + inline const char * data_c() const + { + return reinterpret_cast(data()); + } + + inline char * data_c() + { + return reinterpret_cast(data()); + } + + inline const uint32_t * data_u32() const + { + return reinterpret_cast(data()); + } + + inline uint32_t * data_u32() + { + return reinterpret_cast(data()); + } + + /** + * @brief Copy string to payload + * + * @param[in] s payload string + * @note this function allows null termination inside string + * it is used to send multiple strings in one message + */ + void set_data_string(const std::string & s) + { + size_t sz = (s.size() < DATA_MAXSZ - 1) ? s.size() : DATA_MAXSZ - 1; + + memcpy(data_c(), s.c_str(), sz); + data_c()[sz] = '\0'; + header()->size = sz; + } + + uint8_t get_target_system_id() const + { + return target_system; + } + + /** + * @brief Decode and check target system + */ + bool decode_valid([[maybe_unused]] plugin::UASPtr uas) + { #ifdef FTP_LL_DEBUG - auto hdr = header(); - ROS_DEBUG_NAMED("ftp", "FTP:rm: SEQ(%u) SESS(%u) OPCODE(%u) RQOP(%u) SZ(%u) OFF(%u)", - hdr->seqNumber, hdr->session, hdr->opcode, hdr->req_opcode, hdr->size, hdr->offset); + auto hdr = header(); + RCLCPP_DEBUG( + rclcpp::get_logger("ftp.request"), + "FTP:rm: SEQ(%u) SESS(%u) OPCODE(%u) RQOP(%u) SZ(%u) OFF(%u)", + hdr->seqNumber, hdr->session, hdr->opcode, hdr->req_opcode, hdr->size, hdr->offset); #endif - return UAS_FCU(uas)->get_system_id() == target_system; - } + // return uas->get_system_id() == target_system; + return true; // XXX TODO(vooon): probably whole method is not needed anymore + } - /** - * @brief Encode and send message - */ - void send(UAS *uas, uint16_t seqNumber) - { - target_network = 0; - target_system = uas->get_tgt_system(); - target_component = uas->get_tgt_component(); + /** + * @brief Encode and send message + */ + void send(plugin::UASPtr uas, uint16_t seqNumber) + { + target_network = 0; + target_system = uas->get_tgt_system(); + target_component = uas->get_tgt_component(); - auto hdr = header(); - hdr->seqNumber = seqNumber; - hdr->req_opcode = kCmdNone; + auto hdr = header(); + hdr->seqNumber = seqNumber; + hdr->req_opcode = kCmdNone; #ifdef FTP_LL_DEBUG - ROS_DEBUG_NAMED("ftp", "FTP:sm: SEQ(%u) SESS(%u) OPCODE(%u) SZ(%u) OFF(%u)", - hdr->seqNumber, hdr->session, hdr->opcode, hdr->size, hdr->offset); + RCLCPP_DEBUG( + rclcpp::get_logger("ftp.request"), "FTP:sm: SEQ(%u) SESS(%u) OPCODE(%u) SZ(%u) OFF(%u)", + hdr->seqNumber, hdr->session, hdr->opcode, hdr->size, hdr->offset); #endif - UAS_FCU(uas)->send_message_ignore_drop(*this); - } + uas->send_message(*this); + } - FTPRequest() : - mavlink::common::msg::FILE_TRANSFER_PROTOCOL{} - { } + FTPRequest() + : mavlink::common::msg::FILE_TRANSFER_PROTOCOL{} + {} - explicit FTPRequest(Opcode op, uint8_t session = 0) : - mavlink::common::msg::FILE_TRANSFER_PROTOCOL{} - { - header()->session = session; - header()->opcode = op; - } + explicit FTPRequest(Opcode op, uint8_t session = 0) + : mavlink::common::msg::FILE_TRANSFER_PROTOCOL{} + { + header()->session = session; + header()->opcode = op; + } }; /** * @brief FTP plugin. + * @plugin ftp */ -class FTPPlugin : public plugin::PluginBase { +class FTPPlugin : public plugin::Plugin +{ public: - FTPPlugin() : PluginBase(), - ftp_nh("~ftp"), - op_state(OP::IDLE), - last_send_seqnr(0), - active_session(0), - is_error(false), - r_errno(0), - list_offset(0), - open_size(0), - read_size(0), - read_offset(0), - read_buffer {}, - write_offset(0), - checksum_crc32(0) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - // since C++ generator do not produce field length defs make check explicit. - FTPRequest r; - ROS_ASSERT(r.payload.size() - sizeof(FTPRequest::PayloadHeader) == r.DATA_MAXSZ); - - list_srv = ftp_nh.advertiseService("list", &FTPPlugin::list_cb, this); - open_srv = ftp_nh.advertiseService("open", &FTPPlugin::open_cb, this); - close_srv = ftp_nh.advertiseService("close", &FTPPlugin::close_cb, this); - read_srv = ftp_nh.advertiseService("read", &FTPPlugin::read_cb, this); - write_srv = ftp_nh.advertiseService("write", &FTPPlugin::write_cb, this); - mkdir_srv = ftp_nh.advertiseService("mkdir", &FTPPlugin::mkdir_cb, this); - rmdir_srv = ftp_nh.advertiseService("rmdir", &FTPPlugin::rmdir_cb, this); - remove_srv = ftp_nh.advertiseService("remove", &FTPPlugin::remove_cb, this); - truncate_srv = ftp_nh.advertiseService("truncate", &FTPPlugin::truncate_cb, this); - reset_srv = ftp_nh.advertiseService("reset", &FTPPlugin::reset_cb, this); - rename_srv = ftp_nh.advertiseService("rename", &FTPPlugin::rename_cb, this); - checksum_srv = ftp_nh.advertiseService("checksum", &FTPPlugin::checksum_cb, this); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&FTPPlugin::handle_file_transfer_protocol), - }; - } + explicit FTPPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "ftp"), + op_state(OP::IDLE), + last_send_seqnr(0), + active_session(0), + is_error(false), + r_errno(0), + list_offset(0), + open_size(0), + read_size(0), + read_offset(0), + read_buffer{}, + write_offset(0), + write_buffer{}, + write_it{}, + checksum_crc32(0) + { + // since C++ generator do not produce field length defs make check explicit. + FTPRequest r; + rcpputils::assert_true((r.payload.size() - sizeof(FTPRequest::PayloadHeader)) == r.DATA_MAXSZ); + + list_srv = + node->create_service( + "~/list", + std::bind(&FTPPlugin::list_cb, this, _1, _2)); + open_srv = + node->create_service( + "~/open", + std::bind(&FTPPlugin::open_cb, this, _1, _2)); + close_srv = + node->create_service( + "~/close", + std::bind(&FTPPlugin::close_cb, this, _1, _2)); + read_srv = + node->create_service( + "~/read", + std::bind(&FTPPlugin::read_cb, this, _1, _2)); + write_srv = + node->create_service( + "~/write", + std::bind(&FTPPlugin::write_cb, this, _1, _2)); + mkdir_srv = + node->create_service( + "~/mkdir", + std::bind(&FTPPlugin::mkdir_cb, this, _1, _2)); + rmdir_srv = + node->create_service( + "~/rmdir", + std::bind(&FTPPlugin::rmdir_cb, this, _1, _2)); + remove_srv = + node->create_service( + "~/remove", + std::bind(&FTPPlugin::remove_cb, this, _1, _2)); + truncate_srv = + node->create_service( + "~/truncate", + std::bind(&FTPPlugin::truncate_cb, this, _1, _2)); + reset_srv = + node->create_service( + "~/reset", + std::bind(&FTPPlugin::reset_cb, this, _1, _2)); + rename_srv = + node->create_service( + "~/rename", + std::bind(&FTPPlugin::rename_cb, this, _1, _2)); + checksum_srv = + node->create_service( + "~/checksum", + std::bind(&FTPPlugin::checksum_cb, this, _1, _2)); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&FTPPlugin::handle_file_transfer_protocol), + }; + } private: - ros::NodeHandle ftp_nh; - ros::ServiceServer list_srv; - ros::ServiceServer open_srv; - ros::ServiceServer close_srv; - ros::ServiceServer read_srv; - ros::ServiceServer write_srv; - ros::ServiceServer mkdir_srv; - ros::ServiceServer rmdir_srv; - ros::ServiceServer remove_srv; - ros::ServiceServer rename_srv; - ros::ServiceServer truncate_srv; - ros::ServiceServer reset_srv; - ros::ServiceServer checksum_srv; - - //! This type used in servicies to store 'data' fileds. - typedef std::vector V_FileData; - - enum class OP { - IDLE, - ACK, - LIST, - OPEN, - READ, - WRITE, - CHECKSUM - }; - - OP op_state; - uint16_t last_send_seqnr; //!< seqNumber for send. - uint32_t active_session; //!< session id of current operation - - std::mutex cond_mutex; - std::condition_variable cond; //!< wait condvar - bool is_error; //!< error signaling flag (timeout/proto error) - int r_errno; //!< store errno from server - - // FTP:List - uint32_t list_offset; - std::string list_path; - std::vector list_entries; - - // FTP:Open / FTP:Close - std::string open_path; - size_t open_size; - std::map session_file_map; - - // FTP:Read - size_t read_size; - uint32_t read_offset; - V_FileData read_buffer; - - // FTP:Write - uint32_t write_offset; - V_FileData write_buffer; - V_FileData::iterator write_it; - - // FTP:CalcCRC32 - uint32_t checksum_crc32; - - // Timeouts, - // computed as x4 time that needed for transmission of - // one message at 57600 baud rate - static constexpr int LIST_TIMEOUT_MS = 5000; - static constexpr int OPEN_TIMEOUT_MS = 200; - static constexpr int CHUNK_TIMEOUT_MS = 200; - - //! Maximum difference between allocated space and used - static constexpr size_t MAX_RESERVE_DIFF = 0x10000; - - //! @todo exchange speed calculation - //! @todo diagnostics - //! @todo multisession not present anymore - - /* -*- message handler -*- */ - - //! handler for mavlink::common::msg::FILE_TRANSFER_PROTOCOL - void handle_file_transfer_protocol(const mavlink::mavlink_message_t *msg, FTPRequest &req) - { - if (!req.decode_valid(m_uas)) { - ROS_DEBUG_NAMED("ftp", "FTP: Wrong System Id, MY %u, TGT %u", - UAS_FCU(m_uas)->get_system_id(), req.get_target_system_id()); - return; - } - - const uint16_t incoming_seqnr = req.header()->seqNumber; - const uint16_t expected_seqnr = last_send_seqnr + 1; - if (incoming_seqnr != expected_seqnr) { - ROS_WARN_NAMED("ftp", "FTP: Lost sync! seqnr: %u != %u", - incoming_seqnr, expected_seqnr); - go_idle(true, EILSEQ); - return; - } - - last_send_seqnr = incoming_seqnr; - - // logic from QGCUASFileManager.cc - if (req.header()->opcode == FTPRequest::kRspAck) - handle_req_ack(req); - else if (req.header()->opcode == FTPRequest::kRspNak) - handle_req_nack(req); - else { - ROS_ERROR_NAMED("ftp", "FTP: Unknown request response: %u", req.header()->opcode); - go_idle(true, EBADRQC); - } - } - - void handle_req_ack(FTPRequest &req) - { - switch (op_state) { - case OP::IDLE: send_reset(); break; - case OP::ACK: go_idle(false); break; - case OP::LIST: handle_ack_list(req); break; - case OP::OPEN: handle_ack_open(req); break; - case OP::READ: handle_ack_read(req); break; - case OP::WRITE: handle_ack_write(req); break; - case OP::CHECKSUM: handle_ack_checksum(req); break; - default: - ROS_ERROR_NAMED("ftp", "FTP: wrong op_state"); - go_idle(true, EBADRQC); - } - } - - void handle_req_nack(FTPRequest &req) - { - auto hdr = req.header(); - auto error_code = static_cast(req.data()[0]); - auto prev_op = op_state; - - ROS_ASSERT(hdr->size == 1 || (error_code == FTPRequest::kErrFailErrno && hdr->size == 2)); - - op_state = OP::IDLE; - if (error_code == FTPRequest::kErrFailErrno) - r_errno = req.data()[1]; - // translate other protocol errors to errno - else if (error_code == FTPRequest::kErrFail) - r_errno = EFAULT; - else if (error_code == FTPRequest::kErrInvalidDataSize) - r_errno = EMSGSIZE; - else if (error_code == FTPRequest::kErrInvalidSession) - r_errno = EBADFD; - else if (error_code == FTPRequest::kErrNoSessionsAvailable) - r_errno = EMFILE; - else if (error_code == FTPRequest::kErrUnknownCommand) - r_errno = ENOSYS; - - if (prev_op == OP::LIST && error_code == FTPRequest::kErrEOF) { - /* dir list done */ - list_directory_end(); - return; - } - else if (prev_op == OP::READ && error_code == FTPRequest::kErrEOF) { - /* read done */ - read_file_end(); - return; - } - - ROS_ERROR_NAMED("ftp", "FTP: NAK: %u Opcode: %u State: %u Errno: %d (%s)", - error_code, hdr->req_opcode, enum_value(prev_op), r_errno, strerror(r_errno)); - go_idle(true); - } - - void handle_ack_list(FTPRequest &req) - { - auto hdr = req.header(); - - ROS_DEBUG_NAMED("ftp", "FTP:m: ACK List SZ(%u) OFF(%u)", hdr->size, hdr->offset); - if (hdr->offset != list_offset) { - ROS_ERROR_NAMED("ftp", "FTP: Wrong list offset, req %u, ret %u", - list_offset, hdr->offset); - go_idle(true, EBADE); - return; - } - - uint8_t off = 0; - uint32_t n_list_entries = 0; - - while (off < hdr->size) { - const char *ptr = req.data_c() + off; - const size_t bytes_left = hdr->size - off; - - size_t slen = strnlen(ptr, bytes_left); - if ((ptr[0] == FTPRequest::DIRENT_SKIP && slen > 1) || - (ptr[0] != FTPRequest::DIRENT_SKIP && slen < 2)) { - ROS_ERROR_NAMED("ftp", "FTP: Incorrect list entry: %s", ptr); - go_idle(true, ERANGE); - return; - } - else if (slen == bytes_left) { - ROS_ERROR_NAMED("ftp", "FTP: Missing NULL termination in list entry"); - go_idle(true, EOVERFLOW); - return; - } - - if (ptr[0] == FTPRequest::DIRENT_FILE || - ptr[0] == FTPRequest::DIRENT_DIR) { - add_dirent(ptr, slen); - } - else if (ptr[0] == FTPRequest::DIRENT_SKIP) { - // do nothing - } - else { - ROS_WARN_NAMED("ftp", "FTP: Unknown list entry: %s", ptr); - } - - off += slen + 1; - n_list_entries++; - } - - if (hdr->size == 0) { - // dir empty, we are done - list_directory_end(); - } - else { - ROS_ASSERT_MSG(n_list_entries > 0, "FTP:List don't parse entries"); - // Possibly more to come, try get more - list_offset += n_list_entries; - send_list_command(); - } - } - - void handle_ack_open(FTPRequest &req) - { - auto hdr = req.header(); - - ROS_DEBUG_NAMED("ftp", "FTP:m: ACK Open OPCODE(%u)", hdr->req_opcode); - ROS_ASSERT(hdr->size == sizeof(uint32_t)); - open_size = *req.data_u32(); - - ROS_INFO_NAMED("ftp", "FTP:Open %s: success, session %u, size %zu", - open_path.c_str(), hdr->session, open_size); - session_file_map.insert(std::make_pair(open_path, hdr->session)); - go_idle(false); - } - - void handle_ack_read(FTPRequest &req) - { - auto hdr = req.header(); - - ROS_DEBUG_NAMED("ftp", "FTP:m: ACK Read SZ(%u)", hdr->size); - if (hdr->session != active_session) { - ROS_ERROR_NAMED("ftp", "FTP:Read unexpected session"); - go_idle(true, EBADSLT); - return; - } - - if (hdr->offset != read_offset) { - ROS_ERROR_NAMED("ftp", "FTP:Read different offset"); - go_idle(true, EBADE); - return; - } - - // kCmdReadFile return cunks of DATA_MAXSZ or smaller (last chunk) - // We requested specific amount of data, that can be smaller, - // but not larger. - const size_t bytes_left = read_size - read_buffer.size(); - const size_t bytes_to_copy = std::min(bytes_left, hdr->size); - - read_buffer.insert(read_buffer.end(), req.data(), req.data() + bytes_to_copy); - - if (bytes_to_copy == FTPRequest::DATA_MAXSZ) { - // Possibly more data - read_offset += bytes_to_copy; - send_read_command(); - } - else - read_file_end(); - } - - void handle_ack_write(FTPRequest &req) - { - auto hdr = req.header(); - - ROS_DEBUG_NAMED("ftp", "FTP:m: ACK Write SZ(%u)", hdr->size); - if (hdr->session != active_session) { - ROS_ERROR_NAMED("ftp", "FTP:Write unexpected session"); - go_idle(true, EBADSLT); - return; - } - - if (hdr->offset != write_offset) { - ROS_ERROR_NAMED("ftp", "FTP:Write different offset"); - go_idle(true, EBADE); - return; - } - - ROS_ASSERT(hdr->size == sizeof(uint32_t)); - const size_t bytes_written = *req.data_u32(); - - // check that reported size not out of range - const size_t bytes_left_before_advance = std::distance(write_it, write_buffer.end()); - ROS_ASSERT_MSG(bytes_written <= bytes_left_before_advance, "Bad write size"); - ROS_ASSERT(bytes_written != 0); - - // move iterator to written size - std::advance(write_it, bytes_written); - - const size_t bytes_to_copy = write_bytes_to_copy(); - if (bytes_to_copy > 0) { - // More data to write - write_offset += bytes_written; - send_write_command(bytes_to_copy); - } - else - write_file_end(); - } - - void handle_ack_checksum(FTPRequest &req) - { - auto hdr = req.header(); - - ROS_DEBUG_NAMED("ftp", "FTP:m: ACK CalcFileCRC32 OPCODE(%u)", hdr->req_opcode); - ROS_ASSERT(hdr->size == sizeof(uint32_t)); - checksum_crc32 = *req.data_u32(); - - ROS_DEBUG_NAMED("ftp", "FTP:Checksum: success, crc32: 0x%08x", checksum_crc32); - go_idle(false); - } - - /* -*- send helpers -*- */ - - /** - * @brief Go to IDLE mode - * - * @param is_error_ mark that caused in error case - * @param r_errno_ set r_errno in error case - */ - void go_idle(bool is_error_, int r_errno_ = 0) - { - op_state = OP::IDLE; - is_error = is_error_; - if (is_error && r_errno_ != 0) r_errno = r_errno_; - else if (!is_error) r_errno = 0; - cond.notify_all(); - } - - void send_reset() - { - ROS_DEBUG_NAMED("ftp", "FTP:m: kCmdResetSessions"); - if (!session_file_map.empty()) { - ROS_WARN_NAMED("ftp", "FTP: Reset closes %zu sessons", - session_file_map.size()); - session_file_map.clear(); - } - - op_state = OP::ACK; - FTPRequest req(FTPRequest::kCmdResetSessions); - req.send(m_uas, last_send_seqnr); - } - - /// Send any command with string payload (usually file/dir path) - inline void send_any_path_command(FTPRequest::Opcode op, const std::string &debug_msg, std::string &path, uint32_t offset) - { - ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: " << debug_msg << path << " off: " << offset); - FTPRequest req(op); - req.header()->offset = offset; - req.set_data_string(path); - req.send(m_uas, last_send_seqnr); - } - - void send_list_command() { - send_any_path_command(FTPRequest::kCmdListDirectory, "kCmdListDirectory: ", list_path, list_offset); - } - - void send_open_ro_command() { - send_any_path_command(FTPRequest::kCmdOpenFileRO, "kCmdOpenFileRO: ", open_path, 0); - } - - void send_open_wo_command() { - send_any_path_command(FTPRequest::kCmdOpenFileWO, "kCmdOpenFileWO: ", open_path, 0); - } - - void send_create_command() { - send_any_path_command(FTPRequest::kCmdCreateFile, "kCmdCreateFile: ", open_path, 0); - } - - void send_terminate_command(uint32_t session) - { - ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: kCmdTerminateSession: " << session); - FTPRequest req(FTPRequest::kCmdTerminateSession, session); - req.header()->offset = 0; - req.header()->size = 0; - req.send(m_uas, last_send_seqnr); - } - - void send_read_command() - { - // read operation always try read DATA_MAXSZ block (hdr->size ignored) - ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: kCmdReadFile: " << active_session << " off: " << read_offset); - FTPRequest req(FTPRequest::kCmdReadFile, active_session); - req.header()->offset = read_offset; - req.header()->size = 0 /* FTPRequest::DATA_MAXSZ */; - req.send(m_uas, last_send_seqnr); - } - - void send_write_command(const size_t bytes_to_copy) - { - // write chunk from write_buffer [write_it..bytes_to_copy] - ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: kCmdWriteFile: " << active_session << " off: " << write_offset << " sz: " << bytes_to_copy); - FTPRequest req(FTPRequest::kCmdWriteFile, active_session); - req.header()->offset = write_offset; - req.header()->size = bytes_to_copy; - std::copy(write_it, write_it + bytes_to_copy, req.data()); - req.send(m_uas, last_send_seqnr); - } - - void send_remove_command(std::string &path) { - send_any_path_command(FTPRequest::kCmdRemoveFile, "kCmdRemoveFile: ", path, 0); - } - - bool send_rename_command(std::string &old_path, std::string &new_path) - { - std::ostringstream os; - os << old_path; - os << '\0'; - os << new_path; - - std::string paths = os.str(); - if (paths.size() >= FTPRequest::DATA_MAXSZ) { - ROS_ERROR_NAMED("ftp", "FTP: rename file paths is too long: %zu", paths.size()); - r_errno = ENAMETOOLONG; - return false; - } - - send_any_path_command(FTPRequest::kCmdRename, "kCmdRename: ", paths, 0); - return true; - } - - void send_truncate_command(std::string &path, size_t length) { - send_any_path_command(FTPRequest::kCmdTruncateFile, "kCmdTruncateFile: ", path, length); - } - - void send_create_dir_command(std::string &path) { - send_any_path_command(FTPRequest::kCmdCreateDirectory, "kCmdCreateDirectory: ", path, 0); - } - - void send_remove_dir_command(std::string &path) { - send_any_path_command(FTPRequest::kCmdRemoveDirectory, "kCmdRemoveDirectory: ", path, 0); - } - - void send_calc_file_crc32_command(std::string &path) { - send_any_path_command(FTPRequest::kCmdCalcFileCRC32, "kCmdCalcFileCRC32: ", path, 0); - } - - /* -*- helpers -*- */ - - void add_dirent(const char *ptr, size_t slen) - { - mavros_msgs::FileEntry ent; - ent.size = 0; - - if (ptr[0] == FTPRequest::DIRENT_DIR) { - ent.name.assign(ptr + 1, slen - 1); - ent.type = mavros_msgs::FileEntry::TYPE_DIRECTORY; - - ROS_DEBUG_STREAM_NAMED("ftp", "FTP:List Dir: " << ent.name); - } - else { - // ptr[0] == FTPRequest::DIRENT_FILE - std::string name_size(ptr + 1, slen - 1); - - auto sep_it = std::find(name_size.begin(), name_size.end(), '\t'); - ent.name.assign(name_size.begin(), sep_it); - ent.type = mavros_msgs::FileEntry::TYPE_FILE; - - if (sep_it != name_size.end()) { - name_size.erase(name_size.begin(), sep_it + 1); - if (name_size.size() != 0) - ent.size = std::stoi(name_size); - } - - ROS_DEBUG_STREAM_NAMED("ftp", "FTP:List File: " << ent.name << " SZ: " << ent.size); - } - - list_entries.push_back(ent); - } - - void list_directory_end() { - ROS_DEBUG_NAMED("ftp", "FTP:List done"); - go_idle(false); - } - - void list_directory(std::string &path) - { - list_offset = 0; - list_path = path; - list_entries.clear(); - op_state = OP::LIST; - - send_list_command(); - } - - bool open_file(std::string &path, int mode) - { - open_path = path; - open_size = 0; - op_state = OP::OPEN; - - if (mode == mavros_msgs::FileOpenRequest::MODE_READ) - send_open_ro_command(); - else if (mode == mavros_msgs::FileOpenRequest::MODE_WRITE) - send_open_wo_command(); - else if (mode == mavros_msgs::FileOpenRequest::MODE_CREATE) - send_create_command(); - else { - ROS_ERROR_NAMED("ftp", "FTP: Unsupported open mode: %d", mode); - op_state = OP::IDLE; - r_errno = EINVAL; - return false; - } - - return true; - } - - bool close_file(std::string &path) - { - auto it = session_file_map.find(path); - if (it == session_file_map.end()) { - ROS_ERROR_NAMED("ftp", "FTP:Close %s: not opened", path.c_str()); - r_errno = EBADF; - return false; - } - - op_state = OP::ACK; - send_terminate_command(it->second); - session_file_map.erase(it); - return true; - } - - void read_file_end() { - ROS_DEBUG_NAMED("ftp", "FTP:Read done"); - go_idle(false); - } - - bool read_file(std::string &path, size_t off, size_t len) - { - auto it = session_file_map.find(path); - if (it == session_file_map.end()) { - ROS_ERROR_NAMED("ftp", "FTP:Read %s: not opened", path.c_str()); - r_errno = EBADF; - return false; - } - - op_state = OP::READ; - active_session = it->second; - read_size = len; - read_offset = off; - read_buffer.clear(); - if (read_buffer.capacity() < len || - read_buffer.capacity() > len + MAX_RESERVE_DIFF) { - // reserve memory - read_buffer.reserve(len); - } - - send_read_command(); - return true; - } - - void write_file_end() { - ROS_DEBUG_NAMED("ftp", "FTP:Write done"); - go_idle(false); - } - - bool write_file(std::string &path, size_t off, V_FileData &data) - { - auto it = session_file_map.find(path); - if (it == session_file_map.end()) { - ROS_ERROR_NAMED("ftp", "FTP:Write %s: not opened", path.c_str()); - r_errno = EBADF; - return false; - } - - op_state = OP::WRITE; - active_session = it->second; - write_offset = off; - write_buffer = std::move(data); - write_it = write_buffer.begin(); - - send_write_command(write_bytes_to_copy()); - return true; - } - - void remove_file(std::string &path) { - op_state = OP::ACK; - send_remove_command(path); - } - - bool rename_(std::string &old_path, std::string &new_path) { - op_state = OP::ACK; - return send_rename_command(old_path, new_path); - } - - void truncate_file(std::string &path, size_t length) { - op_state = OP::ACK; - send_truncate_command(path, length); - } - - void create_directory(std::string &path) { - op_state = OP::ACK; - send_create_dir_command(path); - } - - void remove_directory(std::string &path) { - op_state = OP::ACK; - send_remove_dir_command(path); - } - - void checksum_crc32_file(std::string &path) { - op_state = OP::CHECKSUM; - checksum_crc32 = 0; - send_calc_file_crc32_command(path); - } - - static constexpr int compute_rw_timeout(size_t len) { - return CHUNK_TIMEOUT_MS * (len / FTPRequest::DATA_MAXSZ + 1); - } - - size_t write_bytes_to_copy() { - return std::min(std::distance(write_it, write_buffer.end()), - FTPRequest::DATA_MAXSZ); - } - - bool wait_completion(const int msecs) - { - std::unique_lock lock(cond_mutex); - - bool is_timedout = cond.wait_for(lock, std::chrono::milliseconds(msecs)) - == std::cv_status::timeout; - - if (is_timedout) { - // If timeout occurs don't forget to reset state - op_state = OP::IDLE; - r_errno = ETIMEDOUT; - return false; - } - else - // if go_idle() occurs before timeout - return !is_error; - } - - /* -*- service callbacks -*- */ - - /** - * Service handler common header code. - */ -#define SERVICE_IDLE_CHECK() \ - if (op_state != OP::IDLE) { \ - ROS_ERROR_NAMED("ftp", "FTP: Busy"); \ - return false; \ - } - - bool list_cb(mavros_msgs::FileList::Request &req, - mavros_msgs::FileList::Response &res) - { - SERVICE_IDLE_CHECK(); - - list_directory(req.dir_path); - res.success = wait_completion(LIST_TIMEOUT_MS); - res.r_errno = r_errno; - if (res.success) { - res.list = std::move(list_entries); - list_entries.clear(); // not shure that it's needed - } - - return true; - } - - bool open_cb(mavros_msgs::FileOpen::Request &req, - mavros_msgs::FileOpen::Response &res) - { - SERVICE_IDLE_CHECK(); - - // only one session per file - auto it = session_file_map.find(req.file_path); - if (it != session_file_map.end()) { - ROS_ERROR_NAMED("ftp", "FTP: File %s: already opened", - req.file_path.c_str()); - return false; - } - - res.success = open_file(req.file_path, req.mode); - if (res.success) { - res.success = wait_completion(OPEN_TIMEOUT_MS); - res.size = open_size; - } - res.r_errno = r_errno; - - return true; - } - - bool close_cb(mavros_msgs::FileClose::Request &req, - mavros_msgs::FileClose::Response &res) - { - SERVICE_IDLE_CHECK(); - - res.success = close_file(req.file_path); - if (res.success) { - res.success = wait_completion(OPEN_TIMEOUT_MS); - } - res.r_errno = r_errno; - - return true; - } - - bool read_cb(mavros_msgs::FileRead::Request &req, - mavros_msgs::FileRead::Response &res) - { - SERVICE_IDLE_CHECK(); - - res.success = read_file(req.file_path, req.offset, req.size); - if (res.success) - res.success = wait_completion(compute_rw_timeout(req.size)); - if (res.success) { - res.data = std::move(read_buffer); - read_buffer.clear(); // same as for list_entries - } - res.r_errno = r_errno; - - return true; - } - - bool write_cb(mavros_msgs::FileWrite::Request &req, - mavros_msgs::FileWrite::Response &res) - { - SERVICE_IDLE_CHECK(); - - const size_t data_size = req.data.size(); - res.success = write_file(req.file_path, req.offset, req.data); - if (res.success) { - res.success = wait_completion(compute_rw_timeout(data_size)); - } - write_buffer.clear(); - res.r_errno = r_errno; - - return true; - } - - bool remove_cb(mavros_msgs::FileRemove::Request &req, - mavros_msgs::FileRemove::Response &res) - { - SERVICE_IDLE_CHECK(); - - remove_file(req.file_path); - res.success = wait_completion(OPEN_TIMEOUT_MS); - res.r_errno = r_errno; - - return true; - } - - bool rename_cb(mavros_msgs::FileRename::Request &req, - mavros_msgs::FileRename::Response &res) - { - SERVICE_IDLE_CHECK(); - - res.success = rename_(req.old_path, req.new_path); - if (res.success) { - res.success = wait_completion(OPEN_TIMEOUT_MS); - } - res.r_errno = r_errno; - - return true; - } - - - bool truncate_cb(mavros_msgs::FileTruncate::Request &req, - mavros_msgs::FileTruncate::Response &res) - { - SERVICE_IDLE_CHECK(); - - // Note: emulated truncate() can take a while - truncate_file(req.file_path, req.length); - res.success = wait_completion(LIST_TIMEOUT_MS * 5); - res.r_errno = r_errno; - - return true; - } - - bool mkdir_cb(mavros_msgs::FileMakeDir::Request &req, - mavros_msgs::FileMakeDir::Response &res) - { - SERVICE_IDLE_CHECK(); - - create_directory(req.dir_path); - res.success = wait_completion(OPEN_TIMEOUT_MS); - res.r_errno = r_errno; - - return true; - } - - bool rmdir_cb(mavros_msgs::FileRemoveDir::Request &req, - mavros_msgs::FileRemoveDir::Response &res) - { - SERVICE_IDLE_CHECK(); - - remove_directory(req.dir_path); - res.success = wait_completion(OPEN_TIMEOUT_MS); - res.r_errno = r_errno; - - return true; - } - - bool checksum_cb(mavros_msgs::FileChecksum::Request &req, - mavros_msgs::FileChecksum::Response &res) - { - SERVICE_IDLE_CHECK(); - - checksum_crc32_file(req.file_path); - res.success = wait_completion(LIST_TIMEOUT_MS); - res.crc32 = checksum_crc32; - res.r_errno = r_errno; - - return true; - } + rclcpp::Service::SharedPtr list_srv; + rclcpp::Service::SharedPtr open_srv; + rclcpp::Service::SharedPtr close_srv; + rclcpp::Service::SharedPtr read_srv; + rclcpp::Service::SharedPtr write_srv; + rclcpp::Service::SharedPtr mkdir_srv; + rclcpp::Service::SharedPtr rmdir_srv; + rclcpp::Service::SharedPtr remove_srv; + rclcpp::Service::SharedPtr rename_srv; + rclcpp::Service::SharedPtr truncate_srv; + rclcpp::Service::SharedPtr reset_srv; + rclcpp::Service::SharedPtr checksum_srv; + + //! This type used in servicies to store 'data' fileds. + typedef std::vector V_FileData; + + enum class OP + { + IDLE, + ACK, + LIST, + OPEN, + READ, + WRITE, + CHECKSUM + }; + + OP op_state; + uint16_t last_send_seqnr; //!< seqNumber for send. + uint32_t active_session; //!< session id of current operation + + std::mutex cond_mutex; + std::condition_variable cond; //!< wait condvar + bool is_error; //!< error signaling flag (timeout/proto error) + int r_errno; //!< store errno from server + + // FTP:List + uint32_t list_offset; + std::string list_path; + std::vector list_entries; + + // FTP:Open / FTP:Close + std::string open_path; + size_t open_size; + std::map session_file_map; + + // FTP:Read + size_t read_size; + uint32_t read_offset; + V_FileData read_buffer; + + // FTP:Write + uint32_t write_offset; + V_FileData write_buffer; + V_FileData::iterator write_it; + + // FTP:CalcCRC32 + uint32_t checksum_crc32; + + // Timeouts, + // computed as x4 time that needed for transmission of + // one message at 57600 baud rate + static constexpr int LIST_TIMEOUT_MS = 5000; + static constexpr int OPEN_TIMEOUT_MS = 200; + static constexpr int CHUNK_TIMEOUT_MS = 200; + + //! Maximum difference between allocated space and used + static constexpr size_t MAX_RESERVE_DIFF = 0x10000; + + //! @todo exchange speed calculation + //! @todo diagnostics + //! @todo multisession not present anymore + + /* -*- message handler -*- */ + + //! handler for mavlink::common::msg::FILE_TRANSFER_PROTOCOL + void handle_file_transfer_protocol( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + FTPRequest & req, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + if (!req.decode_valid(uas)) { + // RCLCPP_DEBUG(get_logger(), "FTP: Wrong System Id, MY %u, TGT %u", + // uas->get_system_id(), req.get_target_system_id()); + return; + } + + const uint16_t incoming_seqnr = req.header()->seqNumber; + const uint16_t expected_seqnr = last_send_seqnr + 1; + if (incoming_seqnr != expected_seqnr) { + RCLCPP_WARN( + get_logger(), "FTP: Lost sync! seqnr: %u != %u", + incoming_seqnr, expected_seqnr); + go_idle(true, EILSEQ); + return; + } + + last_send_seqnr = incoming_seqnr; + + // logic from QGCUASFileManager.cc + if (req.header()->opcode == FTPRequest::kRspAck) { + handle_req_ack(req); + } else if (req.header()->opcode == FTPRequest::kRspNak) { + handle_req_nack(req); + } else { + RCLCPP_ERROR(get_logger(), "FTP: Unknown request response: %u", req.header()->opcode); + go_idle(true, EBADRQC); + } + } + + void handle_req_ack(const FTPRequest & req) + { + switch (op_state) { + case OP::IDLE: send_reset(); break; + case OP::ACK: go_idle(false); break; + case OP::LIST: handle_ack_list(req); break; + case OP::OPEN: handle_ack_open(req); break; + case OP::READ: handle_ack_read(req); break; + case OP::WRITE: handle_ack_write(req); break; + case OP::CHECKSUM: handle_ack_checksum(req); break; + default: + RCLCPP_ERROR(get_logger(), "FTP: wrong op_state"); + go_idle(true, EBADRQC); + } + } + + void handle_req_nack(const FTPRequest & req) + { + auto hdr = req.header(); + auto error_code = static_cast(req.data()[0]); + auto prev_op = op_state; + + rcpputils::require_true( + hdr->size == 1 || + (error_code == FTPRequest::kErrFailErrno && hdr->size == 2)); + + op_state = OP::IDLE; + if (error_code == FTPRequest::kErrFailErrno) { + r_errno = req.data()[1]; + + /* translate other protocol errors to errno */ + } else if (error_code == FTPRequest::kErrFail) { + r_errno = EFAULT; + } else if (error_code == FTPRequest::kErrInvalidDataSize) { + r_errno = EMSGSIZE; + } else if (error_code == FTPRequest::kErrInvalidSession) { + r_errno = EBADFD; + } else if (error_code == FTPRequest::kErrNoSessionsAvailable) { + r_errno = EMFILE; + } else if (error_code == FTPRequest::kErrUnknownCommand) { + r_errno = ENOSYS; + } + + if (prev_op == OP::LIST && error_code == FTPRequest::kErrEOF) { + /* dir list done */ + list_directory_end(); + return; + } else if (prev_op == OP::READ && error_code == FTPRequest::kErrEOF) { + /* read done */ + read_file_end(); + return; + } + + RCLCPP_ERROR( + get_logger(), "FTP: NAK: %u Opcode: %u State: %u Errno: %d (%s)", + error_code, hdr->req_opcode, enum_value(prev_op), r_errno, strerror(r_errno)); + go_idle(true); + } + + void handle_ack_list(const FTPRequest & req) + { + auto hdr = req.header(); + + RCLCPP_DEBUG(get_logger(), "FTP:m: ACK List SZ(%u) OFF(%u)", hdr->size, hdr->offset); + if (hdr->offset != list_offset) { + RCLCPP_ERROR( + get_logger(), "FTP: Wrong list offset, req %u, ret %u", + list_offset, hdr->offset); + go_idle(true, EBADE); + return; + } + + uint8_t off = 0; + uint32_t n_list_entries = 0; + + while (off < hdr->size) { + const char * ptr = req.data_c() + off; + const size_t bytes_left = hdr->size - off; + + size_t slen = strnlen(ptr, bytes_left); + if ((ptr[0] == FTPRequest::DIRENT_SKIP && slen > 1) || + (ptr[0] != FTPRequest::DIRENT_SKIP && slen < 2)) + { + RCLCPP_ERROR(get_logger(), "FTP: Incorrect list entry: %s", ptr); + go_idle(true, ERANGE); + return; + } else if (slen == bytes_left) { + RCLCPP_ERROR(get_logger(), "FTP: Missing NULL termination in list entry"); + go_idle(true, EOVERFLOW); + return; + } + + if (ptr[0] == FTPRequest::DIRENT_FILE || + ptr[0] == FTPRequest::DIRENT_DIR) + { + add_dirent(ptr, slen); + } else if (ptr[0] == FTPRequest::DIRENT_SKIP) { + // do nothing + } else { + RCLCPP_WARN(get_logger(), "FTP: Unknown list entry: %s", ptr); + } + + off += slen + 1; + n_list_entries++; + } + + if (hdr->size == 0) { + // dir empty, we are done + list_directory_end(); + } else { + rcpputils::assert_true(n_list_entries > 0, "FTP:List don't parse entries"); + // Possibly more to come, try get more + list_offset += n_list_entries; + send_list_command(); + } + } + + void handle_ack_open(const FTPRequest & req) + { + auto hdr = req.header(); + + RCLCPP_DEBUG(get_logger(), "FTP:m: ACK Open OPCODE(%u)", hdr->req_opcode); + rcpputils::require_true(hdr->size == sizeof(uint32_t)); + open_size = *req.data_u32(); + + RCLCPP_INFO( + get_logger(), "FTP:Open %s: success, session %u, size %zu", + open_path.c_str(), hdr->session, open_size); + session_file_map.insert(std::make_pair(open_path, hdr->session)); + go_idle(false); + } + + void handle_ack_read(const FTPRequest & req) + { + auto hdr = req.header(); + auto lg = get_logger(); + + RCLCPP_DEBUG(lg, "FTP:m: ACK Read SZ(%u)", hdr->size); + if (hdr->session != active_session) { + RCLCPP_ERROR(lg, "FTP:Read unexpected session"); + go_idle(true, EBADSLT); + return; + } + + if (hdr->offset != read_offset) { + RCLCPP_ERROR(lg, "FTP:Read different offset"); + go_idle(true, EBADE); + return; + } + + // kCmdReadFile return cunks of DATA_MAXSZ or smaller (last chunk) + // We requested specific amount of data, that can be smaller, + // but not larger. + const size_t bytes_left = read_size - read_buffer.size(); + const size_t bytes_to_copy = std::min(bytes_left, hdr->size); + + read_buffer.insert(read_buffer.end(), req.data(), req.data() + bytes_to_copy); + + if (bytes_to_copy == FTPRequest::DATA_MAXSZ) { + // Possibly more data + read_offset += bytes_to_copy; + send_read_command(); + } else { + read_file_end(); + } + } + + void handle_ack_write(const FTPRequest & req) + { + auto hdr = req.header(); + auto lg = get_logger(); + + RCLCPP_DEBUG(lg, "FTP:m: ACK Write SZ(%u)", hdr->size); + if (hdr->session != active_session) { + RCLCPP_ERROR(lg, "FTP:Write unexpected session"); + go_idle(true, EBADSLT); + return; + } + + if (hdr->offset != write_offset) { + RCLCPP_ERROR(lg, "FTP:Write different offset"); + go_idle(true, EBADE); + return; + } + + rcpputils::require_true(hdr->size == sizeof(uint32_t)); + const size_t bytes_written = *req.data_u32(); + + // check that reported size not out of range + const size_t bytes_left_before_advance = std::distance(write_it, write_buffer.end()); + rcpputils::assert_true(bytes_written <= bytes_left_before_advance, "Bad write size"); + rcpputils::assert_true(bytes_written != 0); + + // move iterator to written size + std::advance(write_it, bytes_written); + + const size_t bytes_to_copy = write_bytes_to_copy(); + if (bytes_to_copy > 0) { + // More data to write + write_offset += bytes_written; + send_write_command(bytes_to_copy); + } else { + write_file_end(); + } + } + + void handle_ack_checksum(const FTPRequest & req) + { + auto hdr = req.header(); + auto lg = get_logger(); + + RCLCPP_DEBUG(lg, "FTP:m: ACK CalcFileCRC32 OPCODE(%u)", hdr->req_opcode); + rcpputils::assert_true(hdr->size == sizeof(uint32_t)); + checksum_crc32 = *req.data_u32(); + + RCLCPP_DEBUG(lg, "FTP:Checksum: success, crc32: 0x%08x", checksum_crc32); + go_idle(false); + } + + /* -*- send helpers -*- */ + + /** + * @brief Go to IDLE mode + * + * @param is_error_ mark that caused in error case + * @param r_errno_ set r_errno in error case + */ + void go_idle(bool is_error_, int r_errno_ = 0) + { + op_state = OP::IDLE; + is_error = is_error_; + if (is_error && r_errno_ != 0) { + r_errno = r_errno_; + } else if (!is_error) { + r_errno = 0; + } + cond.notify_all(); + } + + void send_reset() + { + RCLCPP_DEBUG(get_logger(), "FTP:m: kCmdResetSessions"); + if (!session_file_map.empty()) { + RCLCPP_WARN( + get_logger(), "FTP: Reset closes %zu sessons", + session_file_map.size()); + session_file_map.clear(); + } + + op_state = OP::ACK; + FTPRequest req(FTPRequest::kCmdResetSessions); + req.send(uas, last_send_seqnr); + } + + /// Send any command with string payload (usually file/dir path) + inline void send_any_path_command( + const FTPRequest::Opcode op, const std::string & debug_msg, + const std::string & path, const uint32_t offset) + { + RCLCPP_DEBUG_STREAM(get_logger(), "FTP:m: " << debug_msg << path << " off: " << offset); + FTPRequest req(op); + req.header()->offset = offset; + req.set_data_string(path); + req.send(uas, last_send_seqnr); + } + + void send_list_command() + { + send_any_path_command( + FTPRequest::kCmdListDirectory, "kCmdListDirectory: ", list_path, + list_offset); + } + + void send_open_ro_command() + { + send_any_path_command(FTPRequest::kCmdOpenFileRO, "kCmdOpenFileRO: ", open_path, 0); + } + + void send_open_wo_command() + { + send_any_path_command(FTPRequest::kCmdOpenFileWO, "kCmdOpenFileWO: ", open_path, 0); + } + + void send_create_command() + { + send_any_path_command(FTPRequest::kCmdCreateFile, "kCmdCreateFile: ", open_path, 0); + } + + void send_terminate_command(uint32_t session) + { + RCLCPP_DEBUG_STREAM(get_logger(), "FTP:m: kCmdTerminateSession: " << session); + FTPRequest req(FTPRequest::kCmdTerminateSession, session); + req.header()->offset = 0; + req.header()->size = 0; + req.send(uas, last_send_seqnr); + } + + void send_read_command() + { + // read operation always try read DATA_MAXSZ block (hdr->size ignored) + RCLCPP_DEBUG_STREAM( + get_logger(), "FTP:m: kCmdReadFile: " << active_session << " off: " << read_offset); + FTPRequest req(FTPRequest::kCmdReadFile, active_session); + req.header()->offset = read_offset; + req.header()->size = 0 /* FTPRequest::DATA_MAXSZ */; + req.send(uas, last_send_seqnr); + } + + void send_write_command(const size_t bytes_to_copy) + { + // write chunk from write_buffer [write_it..bytes_to_copy] + RCLCPP_DEBUG_STREAM( + get_logger(), "FTP:m: kCmdWriteFile: " << active_session << " off: " << write_offset << + " sz: " << bytes_to_copy); + FTPRequest req(FTPRequest::kCmdWriteFile, active_session); + req.header()->offset = write_offset; + req.header()->size = bytes_to_copy; + std::copy(write_it, write_it + bytes_to_copy, req.data()); + req.send(uas, last_send_seqnr); + } + + void send_remove_command(const std::string & path) + { + send_any_path_command(FTPRequest::kCmdRemoveFile, "kCmdRemoveFile: ", path, 0); + } + + bool send_rename_command(const std::string & old_path, const std::string & new_path) + { + std::ostringstream os; + os << old_path; + os << '\0'; + os << new_path; + + std::string paths = os.str(); + if (paths.size() >= FTPRequest::DATA_MAXSZ) { + RCLCPP_ERROR(get_logger(), "FTP: rename file paths is too long: %zu", paths.size()); + r_errno = ENAMETOOLONG; + return false; + } + + send_any_path_command(FTPRequest::kCmdRename, "kCmdRename: ", paths, 0); + return true; + } + + void send_truncate_command(const std::string & path, size_t length) + { + send_any_path_command(FTPRequest::kCmdTruncateFile, "kCmdTruncateFile: ", path, length); + } + + void send_create_dir_command(const std::string & path) + { + send_any_path_command(FTPRequest::kCmdCreateDirectory, "kCmdCreateDirectory: ", path, 0); + } + + void send_remove_dir_command(const std::string & path) + { + send_any_path_command(FTPRequest::kCmdRemoveDirectory, "kCmdRemoveDirectory: ", path, 0); + } + + void send_calc_file_crc32_command(const std::string & path) + { + send_any_path_command(FTPRequest::kCmdCalcFileCRC32, "kCmdCalcFileCRC32: ", path, 0); + } + + /* -*- helpers -*- */ + + void add_dirent(const char * ptr, size_t slen) + { + mavros_msgs::msg::FileEntry ent; + ent.size = 0; + + if (ptr[0] == FTPRequest::DIRENT_DIR) { + ent.name.assign(ptr + 1, slen - 1); + ent.type = mavros_msgs::msg::FileEntry::TYPE_DIRECTORY; + + RCLCPP_DEBUG_STREAM(get_logger(), "FTP:List Dir: " << ent.name); + } else { + // ptr[0] == FTPRequest::DIRENT_FILE + std::string name_size(ptr + 1, slen - 1); + + auto sep_it = std::find(name_size.begin(), name_size.end(), '\t'); + ent.name.assign(name_size.begin(), sep_it); + ent.type = mavros_msgs::msg::FileEntry::TYPE_FILE; + + if (sep_it != name_size.end()) { + name_size.erase(name_size.begin(), sep_it + 1); + if (name_size.size() != 0) { + ent.size = std::stoi(name_size); + } + } + + RCLCPP_DEBUG_STREAM(get_logger(), "FTP:List File: " << ent.name << " SZ: " << ent.size); + } + + list_entries.push_back(ent); + } + + void list_directory_end() + { + RCLCPP_DEBUG(get_logger(), "FTP:List done"); + go_idle(false); + } + + void list_directory(const std::string & path) + { + list_offset = 0; + list_path = path; + list_entries.clear(); + op_state = OP::LIST; + + send_list_command(); + } + + bool open_file(const std::string & path, int mode) + { + open_path = path; + open_size = 0; + op_state = OP::OPEN; + + if (mode == mavros_msgs::srv::FileOpen::Request::MODE_READ) { + send_open_ro_command(); + } else if (mode == mavros_msgs::srv::FileOpen::Request::MODE_WRITE) { + send_open_wo_command(); + } else if (mode == mavros_msgs::srv::FileOpen::Request::MODE_CREATE) { + send_create_command(); + } else { + RCLCPP_ERROR(get_logger(), "FTP: Unsupported open mode: %d", mode); + op_state = OP::IDLE; + r_errno = EINVAL; + return false; + } + + return true; + } + + bool close_file(const std::string & path) + { + auto it = session_file_map.find(path); + if (it == session_file_map.end()) { + RCLCPP_ERROR(get_logger(), "FTP:Close %s: not opened", path.c_str()); + r_errno = EBADF; + return false; + } + + op_state = OP::ACK; + send_terminate_command(it->second); + session_file_map.erase(it); + return true; + } + + void read_file_end() + { + RCLCPP_DEBUG(get_logger(), "FTP:Read done"); + go_idle(false); + } + + bool read_file(const std::string & path, size_t off, size_t len) + { + auto it = session_file_map.find(path); + if (it == session_file_map.end()) { + RCLCPP_ERROR(get_logger(), "FTP:Read %s: not opened", path.c_str()); + r_errno = EBADF; + return false; + } + + op_state = OP::READ; + active_session = it->second; + read_size = len; + read_offset = off; + read_buffer.clear(); + if (read_buffer.capacity() < len || + read_buffer.capacity() > len + MAX_RESERVE_DIFF) + { + // reserve memory + read_buffer.reserve(len); + } + + send_read_command(); + return true; + } + + void write_file_end() + { + RCLCPP_DEBUG(get_logger(), "FTP:Write done"); + go_idle(false); + } + + bool write_file(const std::string & path, size_t off, V_FileData & data) + { + auto it = session_file_map.find(path); + if (it == session_file_map.end()) { + RCLCPP_ERROR(get_logger(), "FTP:Write %s: not opened", path.c_str()); + r_errno = EBADF; + return false; + } + + op_state = OP::WRITE; + active_session = it->second; + write_offset = off; + write_buffer = std::move(data); + write_it = write_buffer.begin(); + + send_write_command(write_bytes_to_copy()); + return true; + } + + void remove_file(const std::string & path) + { + op_state = OP::ACK; + send_remove_command(path); + } + + bool rename_(const std::string & old_path, const std::string & new_path) + { + op_state = OP::ACK; + return send_rename_command(old_path, new_path); + } + + void truncate_file(const std::string & path, size_t length) + { + op_state = OP::ACK; + send_truncate_command(path, length); + } + + void create_directory(const std::string & path) + { + op_state = OP::ACK; + send_create_dir_command(path); + } + + void remove_directory(const std::string & path) + { + op_state = OP::ACK; + send_remove_dir_command(path); + } + + void checksum_crc32_file(const std::string & path) + { + op_state = OP::CHECKSUM; + checksum_crc32 = 0; + send_calc_file_crc32_command(path); + } + + static constexpr int compute_rw_timeout(size_t len) + { + return CHUNK_TIMEOUT_MS * (len / FTPRequest::DATA_MAXSZ + 1); + } + + size_t write_bytes_to_copy() + { + return std::min( + std::distance(write_it, write_buffer.end()), + FTPRequest::DATA_MAXSZ); + } + + bool wait_completion(const int msecs) + { + std::unique_lock lock(cond_mutex); + + bool is_timedout = cond.wait_for(lock, std::chrono::milliseconds(msecs)) == + std::cv_status::timeout; + + if (is_timedout) { + // If timeout occurs don't forget to reset state + op_state = OP::IDLE; + r_errno = ETIMEDOUT; + return false; + } else { + // if go_idle() occurs before timeout + return !is_error; + } + } + + /* -*- service callbacks -*- */ + + /** + * Service handler common header code. + */ +#define SERVICE_IDLE_CHECK() \ + if (op_state != OP::IDLE) { \ + RCLCPP_ERROR(get_logger(), "FTP: Busy"); \ + throw std::runtime_error("ftp busy"); \ + } + + void list_cb( + const mavros_msgs::srv::FileList::Request::SharedPtr req, + mavros_msgs::srv::FileList::Response::SharedPtr res) + { + SERVICE_IDLE_CHECK(); + + list_directory(req->dir_path); + res->success = wait_completion(LIST_TIMEOUT_MS); + res->r_errno = r_errno; + if (res->success) { + res->list = std::move(list_entries); + list_entries.clear(); // not sure that it's needed + } + } + + void open_cb( + const mavros_msgs::srv::FileOpen::Request::SharedPtr req, + mavros_msgs::srv::FileOpen::Response::SharedPtr res) + { + SERVICE_IDLE_CHECK(); + + // only one session per file + auto it = session_file_map.find(req->file_path); + if (it != session_file_map.end()) { + RCLCPP_ERROR( + get_logger(), "FTP: File %s: already opened", + req->file_path.c_str()); + throw std::runtime_error("file already opened"); + } + + res->success = open_file(req->file_path, req->mode); + if (res->success) { + res->success = wait_completion(OPEN_TIMEOUT_MS); + res->size = open_size; + } + res->r_errno = r_errno; + } + + void close_cb( + const mavros_msgs::srv::FileClose::Request::SharedPtr req, + mavros_msgs::srv::FileClose::Response::SharedPtr res) + { + SERVICE_IDLE_CHECK(); + + res->success = close_file(req->file_path); + if (res->success) { + res->success = wait_completion(OPEN_TIMEOUT_MS); + } + res->r_errno = r_errno; + } + + void read_cb( + const mavros_msgs::srv::FileRead::Request::SharedPtr req, + mavros_msgs::srv::FileRead::Response::SharedPtr res) + { + SERVICE_IDLE_CHECK(); + + res->success = read_file(req->file_path, req->offset, req->size); + if (res->success) { + res->success = wait_completion(compute_rw_timeout(req->size)); + } + if (res->success) { + res->data = std::move(read_buffer); + read_buffer.clear(); // same as for list_entries + } + res->r_errno = r_errno; + } + + void write_cb( + const mavros_msgs::srv::FileWrite::Request::SharedPtr req, + mavros_msgs::srv::FileWrite::Response::SharedPtr res) + { + SERVICE_IDLE_CHECK(); + + const size_t data_size = req->data.size(); + res->success = write_file(req->file_path, req->offset, req->data); + if (res->success) { + res->success = wait_completion(compute_rw_timeout(data_size)); + } + write_buffer.clear(); + res->r_errno = r_errno; + } + + void remove_cb( + const mavros_msgs::srv::FileRemove::Request::SharedPtr req, + mavros_msgs::srv::FileRemove::Response::SharedPtr res) + { + SERVICE_IDLE_CHECK(); + + remove_file(req->file_path); + res->success = wait_completion(OPEN_TIMEOUT_MS); + res->r_errno = r_errno; + } + + void rename_cb( + const mavros_msgs::srv::FileRename::Request::SharedPtr req, + mavros_msgs::srv::FileRename::Response::SharedPtr res) + { + SERVICE_IDLE_CHECK(); + + res->success = rename_(req->old_path, req->new_path); + if (res->success) { + res->success = wait_completion(OPEN_TIMEOUT_MS); + } + res->r_errno = r_errno; + } + + void truncate_cb( + const mavros_msgs::srv::FileTruncate::Request::SharedPtr req, + mavros_msgs::srv::FileTruncate::Response::SharedPtr res) + { + SERVICE_IDLE_CHECK(); + + // Note: emulated truncate() can take a while + truncate_file(req->file_path, req->length); + res->success = wait_completion(LIST_TIMEOUT_MS * 5); + res->r_errno = r_errno; + } + + void mkdir_cb( + const mavros_msgs::srv::FileMakeDir::Request::SharedPtr req, + mavros_msgs::srv::FileMakeDir::Response::SharedPtr res) + { + SERVICE_IDLE_CHECK(); + + create_directory(req->dir_path); + res->success = wait_completion(OPEN_TIMEOUT_MS); + res->r_errno = r_errno; + } + + void rmdir_cb( + const mavros_msgs::srv::FileRemoveDir::Request::SharedPtr req, + mavros_msgs::srv::FileRemoveDir::Response::SharedPtr res) + { + SERVICE_IDLE_CHECK(); + + remove_directory(req->dir_path); + res->success = wait_completion(OPEN_TIMEOUT_MS); + res->r_errno = r_errno; + } + + void checksum_cb( + const mavros_msgs::srv::FileChecksum::Request::SharedPtr req, + mavros_msgs::srv::FileChecksum::Response::SharedPtr res) + { + SERVICE_IDLE_CHECK(); + + checksum_crc32_file(req->file_path); + res->success = wait_completion(LIST_TIMEOUT_MS); + res->crc32 = checksum_crc32; + res->r_errno = r_errno; + } #undef SERVICE_IDLE_CHECK - /** - * @brief Reset communication on both sides. - * @note This call break other calls, so use carefully. - */ - bool reset_cb(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &res) - { - send_reset(); - return true; - } + /** + * @brief Reset communication on both sides. + * @note This call break other calls, so use carefully. + */ + void reset_cb( + const std_srvs::srv::Empty::Request::SharedPtr req [[maybe_unused]], + std_srvs::srv::Empty::Response::SharedPtr res [[maybe_unused]]) + { + send_reset(); + session_file_map.clear(); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::FTPPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::FTPPlugin) diff --git a/mavros/src/plugins/geofence.cpp b/mavros/src/plugins/geofence.cpp index dbdf0d21e..a26257121 100644 --- a/mavros/src/plugins/geofence.cpp +++ b/mavros/src/plugins/geofence.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2021 Charlie Burge. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Geofence plugin * @file geofence.cpp @@ -6,239 +13,208 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2021 Charlie Burge. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include "mavros/mission_protocol_base.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT + -namespace mavros { -namespace std_plugins { /** * @brief Geofence manipulation plugin + * @plugin geofence */ -class GeofencePlugin : public plugin::MissionBase { +class GeofencePlugin : public plugin::MissionBase +{ public: - GeofencePlugin() : - MissionBase("GF"), - gf_nh("~geofence") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - MissionBase::initialize_with_nodehandle(&gf_nh); - - wp_state = WP::IDLE; - wp_type = plugin::WP_TYPE::FENCE; - - gf_nh.param("pull_after_gcs", do_pull_after_gcs, true); - gf_nh.param("use_mission_item_int", use_mission_item_int, false); - - gf_list_pub = gf_nh.advertise("waypoints", 2, true); - pull_srv = gf_nh.advertiseService("pull", &GeofencePlugin::pull_cb, this); - push_srv = gf_nh.advertiseService("push", &GeofencePlugin::push_cb, this); - clear_srv = gf_nh.advertiseService("clear", &GeofencePlugin::clear_cb, this); - - enable_connection_cb(); - enable_capabilities_cb(); - } - - Subscriptions get_subscriptions() override { - return { - make_handler(&GeofencePlugin::handle_mission_item), - make_handler(&GeofencePlugin::handle_mission_item_int), - make_handler(&GeofencePlugin::handle_mission_request), - make_handler(&GeofencePlugin::handle_mission_request_int), - make_handler(&GeofencePlugin::handle_mission_count), - make_handler(&GeofencePlugin::handle_mission_ack), - }; - } + explicit GeofencePlugin(plugin::UASPtr uas_) + : MissionBase(uas_, "geofence", plugin::MTYPE::FENCE, "GF", 25s) + { + enable_node_watch_parameters(); + + // NOTE(vooon): I'm not quite sure that this option would work with mavros router + node_declare_and_watch_parameter( + "pull_after_gcs", true, [&](const rclcpp::Parameter & p) { + do_pull_after_gcs = p.as_bool(); + }); + + node_declare_and_watch_parameter( + "use_mission_item_int", true, [&](const rclcpp::Parameter & p) { + use_mission_item_int = p.as_bool(); + }); + + auto gf_qos = rclcpp::QoS(10).transient_local(); + + gf_list_pub = node->create_publisher("~/fences", gf_qos); + + pull_srv = + node->create_service( + "~/pull", + std::bind(&GeofencePlugin::pull_cb, this, _1, _2)); + push_srv = + node->create_service( + "~/push", + std::bind(&GeofencePlugin::push_cb, this, _1, _2)); + clear_srv = + node->create_service( + "~/clear", + std::bind(&GeofencePlugin::clear_cb, this, _1, _2)); + + enable_connection_cb(); + enable_capabilities_cb(); + } private: - ros::NodeHandle gf_nh; - - ros::Publisher gf_list_pub; - ros::ServiceServer pull_srv; - ros::ServiceServer push_srv; - ros::ServiceServer clear_srv; - - /* -*- mid-level helpers -*- */ - - // Acts when capabilities of the fcu are changed - void capabilities_cb(UAS::MAV_CAP capabilities) override { - lock_guard lock(mutex); - if (m_uas->has_capability(UAS::MAV_CAP::MISSION_INT)) { - use_mission_item_int = true; - mission_item_int_support_confirmed = true; - ROS_INFO_NAMED(log_ns, "%s: Using MISSION_ITEM_INT", log_ns.c_str()); - } else { - use_mission_item_int = false; - mission_item_int_support_confirmed = false; - ROS_WARN_NAMED(log_ns, "%s: Falling back to MISSION_ITEM", log_ns.c_str()); - } - } - - // Act on first heartbeat from FCU - void connection_cb(bool connected) override - { - lock_guard lock(mutex); - if (connected) { - schedule_pull(BOOTUP_TIME_DT); - - if (gf_nh.hasParam("enable_partial_push")) { - gf_nh.getParam("enable_partial_push", enable_partial_push); - } - else { - enable_partial_push = m_uas->is_ardupilotmega(); - } - } - else { - schedule_timer.stop(); - } - } - - //! @brief publish the updated waypoint list after operation - void publish_waypoints() override - { - auto wpl = boost::make_shared(); - unique_lock lock(mutex); - - wpl->current_seq = wp_cur_active; - wpl->waypoints.clear(); - wpl->waypoints.reserve(waypoints.size()); - for (auto &it : waypoints) { - wpl->waypoints.push_back(it); - } - - lock.unlock(); - gf_list_pub.publish(wpl); - } - - /* -*- ROS callbacks -*- */ - - bool pull_cb(mavros_msgs::WaypointPull::Request &req, - mavros_msgs::WaypointPull::Response &res) - { - unique_lock lock(mutex); - - if (wp_state != WP::IDLE) - // Wrong initial state, other operation in progress? - return false; - - wp_state = WP::RXLIST; - wp_count = 0; - restart_timeout_timer(); - - lock.unlock(); - mission_request_list(); - res.success = wait_fetch_all(); - lock.lock(); - - res.wp_received = waypoints.size(); - go_idle(); // not nessessary, but prevents from blocking - return true; - } - - bool push_cb(mavros_msgs::WaypointPush::Request &req, - mavros_msgs::WaypointPush::Response &res) - { - unique_lock lock(mutex); - - if (wp_state != WP::IDLE) - // Wrong initial state, other operation in progress? - return false; - - if (req.start_index) { - // Partial Waypoint update - - if (!enable_partial_push) { - ROS_WARN_NAMED(log_ns, "%s: Partial Push not enabled. (Only supported on APM)", log_ns.c_str()); - res.success = false; - res.wp_transfered = 0; - return true; - } - - if (waypoints.size() < req.start_index + req.waypoints.size()) { - ROS_WARN_NAMED(log_ns, "%s: Partial push out of range rejected.", log_ns.c_str()); - res.success = false; - res.wp_transfered = 0; - return true; - } - - wp_state = WP::TXPARTIAL; - send_waypoints = waypoints; - - uint16_t seq = req.start_index; - for (auto &it : req.waypoints) { - send_waypoints[seq] = it; - seq++; - } - - wp_count = req.waypoints.size(); - wp_start_id = req.start_index; - wp_end_id = req.start_index + wp_count; - wp_cur_id = req.start_index; - restart_timeout_timer(); - - lock.unlock(); - mission_write_partial_list(wp_start_id, wp_end_id); - res.success = wait_push_all(); - lock.lock(); - - res.wp_transfered = wp_cur_id - wp_start_id + 1; - } - else { - // Full waypoint update - wp_state = WP::TXLIST; - - send_waypoints.clear(); - send_waypoints.reserve(req.waypoints.size()); - send_waypoints = req.waypoints; - - wp_count = send_waypoints.size(); - wp_end_id = wp_count; - wp_cur_id = 0; - restart_timeout_timer(); - - lock.unlock(); - mission_count(wp_count); - res.success = wait_push_all(); - lock.lock(); - - res.wp_transfered = wp_cur_id + 1; - } - - go_idle(); // same as in pull_cb - return true; - } - - bool clear_cb(mavros_msgs::WaypointClear::Request &req, - mavros_msgs::WaypointClear::Response &res) - { - unique_lock lock(mutex); - - if (wp_state != WP::IDLE) - return false; - - wp_state = WP::CLEAR; - restart_timeout_timer(); - - lock.unlock(); - mission_clear_all(); - res.success = wait_push_all(); - - lock.lock(); - go_idle(); // same as in pull_cb - return true; - } + rclcpp::Publisher::SharedPtr gf_list_pub; + + rclcpp::Service::SharedPtr pull_srv; + rclcpp::Service::SharedPtr push_srv; + rclcpp::Service::SharedPtr clear_srv; + + /* -*- mid-level helpers -*- */ + + // Acts when capabilities of the fcu are changed + void capabilities_cb(uas::MAV_CAP capabilities [[maybe_unused]]) override + { + lock_guard lock(mutex); + + if (uas->has_capability(uas::MAV_CAP::MISSION_INT)) { + use_mission_item_int = true; + mission_item_int_support_confirmed = true; + RCLCPP_INFO(get_logger(), "%s: Using MISSION_ITEM_INT", log_prefix); + } else { + use_mission_item_int = false; + mission_item_int_support_confirmed = false; + RCLCPP_WARN(get_logger(), "%s: Falling back to MISSION_ITEM", log_prefix); + } + } + + // Act on first heartbeat from FCU + void connection_cb(bool connected) override + { + lock_guard lock(mutex); + + if (connected) { + schedule_pull(BOOTUP_TIME); + } else if (schedule_timer) { + schedule_timer->cancel(); + } + } + + //! @brief publish the updated waypoint list after operation + void publish_waypoints() override + { + auto wpl = mavros_msgs::msg::WaypointList(); + unique_lock lock(mutex); + + wpl.current_seq = wp_cur_active; + wpl.waypoints.reserve(waypoints.size()); + for (auto & it : waypoints) { + wpl.waypoints.push_back(it); + } + + lock.unlock(); + gf_list_pub->publish(wpl); + } + + void publish_reached(const uint16_t seq [[maybe_unused]]) override + {} + + /* -*- ROS callbacks -*- */ + + void pull_cb( + const mavros_msgs::srv::WaypointPull::Request::SharedPtr req [[maybe_unused]], + mavros_msgs::srv::WaypointPull::Response::SharedPtr res) + { + unique_lock lock(mutex); + + if (wp_state != WP::IDLE) { + // Wrong initial state, other operation in progress? + return; + } + + wp_state = WP::RXLIST; + wp_count = 0; + restart_timeout_timer(); + + lock.unlock(); + mission_request_list(); + res->success = wait_fetch_all(); + lock.lock(); + + res->wp_received = waypoints.size(); + go_idle(); // not nessessary, but prevents from blocking + } + + void push_cb( + const mavros_msgs::srv::WaypointPush::Request::SharedPtr req, + mavros_msgs::srv::WaypointPush::Response::SharedPtr res) + { + unique_lock lock(mutex); + + if (wp_state != WP::IDLE) { + // Wrong initial state, other operation in progress? + return; + } + + if (req->start_index) { + // Partial update not supported for geofence points + RCLCPP_WARN(get_logger(), "%s: Partial update for fence points not supported", log_prefix); + res->success = false; + return; + } + + // Full waypoint update + wp_state = WP::TXLIST; + + send_waypoints.clear(); + send_waypoints.reserve(req->waypoints.size()); + for (auto & wp : req->waypoints) { + send_waypoints.emplace_back(wp); + } + + wp_count = send_waypoints.size(); + wp_end_id = wp_count; + wp_cur_id = 0; + restart_timeout_timer(); + + lock.unlock(); + mission_count(wp_count); + res->success = wait_push_all(); + lock.lock(); + + res->wp_transfered = wp_cur_id + 1; + go_idle(); // same as in pull_cb + } + + void clear_cb( + const mavros_msgs::srv::WaypointClear::Request::SharedPtr req [[maybe_unused]], + mavros_msgs::srv::WaypointClear::Response::SharedPtr res) + { + unique_lock lock(mutex); + + if (wp_state != WP::IDLE) { + return; + } + + wp_state = WP::CLEAR; + restart_timeout_timer(); + + lock.unlock(); + mission_clear_all(); + res->success = wait_push_all(); + + lock.lock(); + go_idle(); // same as in pull_cb + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::GeofencePlugin, mavros::plugin::PluginBase) \ No newline at end of file +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::GeofencePlugin) diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp index 57cce6298..b06865c84 100644 --- a/mavros/src/plugins/global_position.cpp +++ b/mavros/src/plugins/global_position.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2014,2017 Nuno Marques. + * Copyright 2015,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Global Position plugin * @file global_position.cpp @@ -7,509 +15,562 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014,2017 Nuno Marques. - * Copyright 2015,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ #include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace mavros { -namespace std_plugins { +#include + +#include +#include // NOLINT + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "std_msgs/msg/float64.hpp" +#include "std_msgs/msg/u_int32.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "sensor_msgs/msg/nav_sat_status.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geographic_msgs/msg/geo_point_stamped.hpp" +#include "mavros_msgs/msg/home_position.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT + /** * @brief Global position plugin. + * @plugin global_position * * Publishes global position. Conversion from GPS LLA to ECEF allows * publishing local position to TF and PoseWithCovarianceStamped. - * */ -class GlobalPositionPlugin : public plugin::PluginBase { +class GlobalPositionPlugin : public plugin::Plugin +{ public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - GlobalPositionPlugin() : PluginBase(), - gp_nh("~global_position"), - tf_send(false), - use_relative_alt(true), - is_map_init(false), - rot_cov(99999.0) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - // general params - gp_nh.param("frame_id", frame_id, "map"); - gp_nh.param("child_frame_id", child_frame_id, "base_link"); - gp_nh.param("rot_covariance", rot_cov, 99999.0); - gp_nh.param("gps_uere", gps_uere, 1.0); - gp_nh.param("use_relative_alt", use_relative_alt, true); - // tf subsection - gp_nh.param("tf/send", tf_send, false); - gp_nh.param("tf/frame_id", tf_frame_id, "map"); - gp_nh.param("tf/global_frame_id", tf_global_frame_id, "earth"); // The global_origin should be represented as "earth" coordinate frame (ECEF) (REP 105) - gp_nh.param("tf/child_frame_id", tf_child_frame_id, "base_link"); - - UAS_DIAG(m_uas).add("GPS", this, &GlobalPositionPlugin::gps_diag_run); - - // gps data - raw_fix_pub = gp_nh.advertise("raw/fix", 10); - raw_vel_pub = gp_nh.advertise("raw/gps_vel", 10); - raw_sat_pub = gp_nh.advertise("raw/satellites", 10); - - // fused global position - gp_fix_pub = gp_nh.advertise("global", 10); - gp_odom_pub = gp_nh.advertise("local", 10); - gp_rel_alt_pub = gp_nh.advertise("rel_alt", 10); - gp_hdg_pub = gp_nh.advertise("compass_hdg", 10); - - // global origin - gp_global_origin_pub = gp_nh.advertise("gp_origin", 10, true); - gp_set_global_origin_sub = gp_nh.subscribe("set_gp_origin", 10, &GlobalPositionPlugin::set_gp_origin_cb, this); - - // home position subscriber to set "map" origin - // TODO use UAS - hp_sub = gp_nh.subscribe("home", 10, &GlobalPositionPlugin::home_position_cb, this); - - // offset from local position to the global origin ("earth") - gp_global_offset_pub = gp_nh.advertise("gp_lp_offset", 10); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&GlobalPositionPlugin::handle_gps_raw_int), - // GPS_STATUS: there no corresponding ROS message, and it is not supported by APM - make_handler(&GlobalPositionPlugin::handle_global_position_int), - make_handler(&GlobalPositionPlugin::handle_gps_global_origin), - make_handler(&GlobalPositionPlugin::handle_lpned_system_global_offset) - }; - } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit GlobalPositionPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "global_position"), + tf_send(false), + use_relative_alt(true), + is_map_init(false), + rot_cov(99999.0) + { + enable_node_watch_parameters(); + + // general params + node_declare_and_watch_parameter( + "frame_id", "map", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + node_declare_and_watch_parameter( + "child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { + child_frame_id = p.as_string(); + }); + node_declare_and_watch_parameter( + "rot_covariance", 99999.0, [&](const rclcpp::Parameter & p) { + rot_cov = p.as_double(); + }); + node_declare_and_watch_parameter( + "gps_uere", 1.0, [&](const rclcpp::Parameter & p) { + gps_uere = p.as_double(); + }); + node_declare_and_watch_parameter( + "use_relative_alt", true, [&](const rclcpp::Parameter & p) { + use_relative_alt = p.as_bool(); + }); + + // tf subsection + node_declare_and_watch_parameter( + "tf.send", false, [&](const rclcpp::Parameter & p) { + tf_send = p.as_bool(); + }); + node_declare_and_watch_parameter( + "tf.frame_id", "map", [&](const rclcpp::Parameter & p) { + tf_frame_id = p.as_string(); + }); + // The global_origin should be represented as "earth" coordinate frame (ECEF) (REP 105) + node_declare_and_watch_parameter( + "tf.global_frame_id", "earth", [&](const rclcpp::Parameter & p) { + tf_global_frame_id = p.as_string(); + }); + node_declare_and_watch_parameter( + "tf.child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { + tf_child_frame_id = p.as_string(); + }); + + uas->diagnostic_updater.add("GPS", this, &GlobalPositionPlugin::gps_diag_run); + + auto sensor_qos = rclcpp::SensorDataQoS(); + + // gps data + raw_fix_pub = node->create_publisher("~/raw/fix", sensor_qos); + raw_vel_pub = node->create_publisher( + "~/raw/gps_vel", + sensor_qos); + raw_sat_pub = node->create_publisher("~/raw/satellites", sensor_qos); + + // fused global position + gp_fix_pub = node->create_publisher("~/global", sensor_qos); + gp_odom_pub = node->create_publisher("~/local", sensor_qos); + gp_rel_alt_pub = node->create_publisher("~/rel_alt", sensor_qos); + gp_hdg_pub = node->create_publisher("~/compass_hdg", sensor_qos); + + // global origin + gp_global_origin_pub = node->create_publisher( + "~/gp_origin", sensor_qos); + gp_set_global_origin_sub = + node->create_subscription( + "~/set_gp_origin", sensor_qos, + std::bind(&GlobalPositionPlugin::set_gp_origin_cb, this, _1)); + + // home position subscriber to set "map" origin + // TODO(vooon): use UAS + hp_sub = node->create_subscription( + "home_position/home", + sensor_qos, + std::bind(&GlobalPositionPlugin::home_position_cb, this, _1)); + + // offset from local position to the global origin ("earth") + gp_global_offset_pub = node->create_publisher( + "~/gp_lp_offset", + sensor_qos); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&GlobalPositionPlugin::handle_gps_raw_int), + // GPS_STATUS: there no corresponding ROS message, and it is not supported by APM + make_handler(&GlobalPositionPlugin::handle_global_position_int), + make_handler(&GlobalPositionPlugin::handle_gps_global_origin), + make_handler(&GlobalPositionPlugin::handle_lpned_system_global_offset) + }; + } private: - ros::NodeHandle gp_nh; - - ros::Publisher raw_fix_pub; - ros::Publisher raw_vel_pub; - ros::Publisher raw_sat_pub; - ros::Publisher gp_odom_pub; - ros::Publisher gp_fix_pub; - ros::Publisher gp_hdg_pub; - ros::Publisher gp_rel_alt_pub; - ros::Publisher gp_global_origin_pub; - ros::Publisher gp_global_offset_pub; - - ros::Subscriber gp_set_global_origin_sub; - ros::Subscriber hp_sub; - - std::string frame_id; //!< origin frame for topic headers - std::string child_frame_id; //!< body-fixed frame for topic headers - std::string tf_frame_id; //!< origin for TF - std::string tf_global_frame_id; //!< global origin for TF - std::string tf_child_frame_id; //!< frame for TF and Pose - - bool tf_send; - bool use_relative_alt; - bool is_map_init; - - double rot_cov; - double gps_uere; - - Eigen::Vector3d map_origin {}; //!< geodetic origin of map frame [lla] - Eigen::Vector3d ecef_origin {}; //!< geocentric origin of map frame [m] - Eigen::Vector3d local_ecef {}; //!< local ECEF coordinates on map frame [m] - - template - inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix) - { - fix->latitude = msg.lat / 1E7; // deg - fix->longitude = msg.lon / 1E7; // deg - fix->altitude = msg.alt / 1E3 + m_uas->geoid_to_ellipsoid_height(fix); // in meters - } - - inline void fill_unknown_cov(sensor_msgs::NavSatFix::Ptr fix) - { - fix->position_covariance.fill(0.0); - fix->position_covariance[0] = -1.0; - fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; - } - - /* -*- message handlers -*- */ - - void handle_gps_raw_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RAW_INT &raw_gps) - { - auto fix = boost::make_shared(); - - fix->header = m_uas->synchronized_header(child_frame_id, raw_gps.time_usec); - - fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; - if (raw_gps.fix_type > 2) - fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX; - else { - ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix"); - fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; - } - - fill_lla(raw_gps, fix); - - float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN; - float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN; - - ftf::EigenMapCovariance3d gps_cov(fix->position_covariance.data()); - - // With mavlink v2.0 use accuracies reported by sensor - if (msg->magic == MAVLINK_STX && - raw_gps.h_acc > 0 && raw_gps.v_acc > 0) { - gps_cov.diagonal() << std::pow(raw_gps.h_acc / 1E3, 2), std::pow(raw_gps.h_acc / 1E3, 2), std::pow(raw_gps.v_acc / 1E3, 2); - fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - } - // With mavlink v1.0 approximate accuracies by DOP - else if (!std::isnan(eph) && !std::isnan(epv)) { - gps_cov.diagonal() << std::pow(eph * gps_uere, 2), std::pow(eph * gps_uere, 2), std::pow(epv * gps_uere, 2); - fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED; - } - else { - fill_unknown_cov(fix); - } - - // store & publish - m_uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible); - raw_fix_pub.publish(fix); - - if (raw_gps.vel != UINT16_MAX && - raw_gps.cog != UINT16_MAX) { - double speed = raw_gps.vel / 1E2; // m/s - double course = angles::from_degrees(raw_gps.cog / 1E2); // rad - - auto vel = boost::make_shared(); - - vel->header.stamp = fix->header.stamp; - vel->header.frame_id = frame_id; - - // From nmea_navsat_driver - vel->twist.linear.x = speed * std::sin(course); - vel->twist.linear.y = speed * std::cos(course); - - raw_vel_pub.publish(vel); - } - - // publish satellite count - auto sat_cnt = boost::make_shared(); - sat_cnt->data = raw_gps.satellites_visible; - raw_sat_pub.publish(sat_cnt); - } - - void handle_gps_global_origin(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_GLOBAL_ORIGIN &glob_orig) - { - auto g_origin = boost::make_shared(); - // auto header = m_uas->synchronized_header(frame_id, glob_orig.time_boot_ms); #TODO: requires Mavlink msg update - - g_origin->header.frame_id = tf_global_frame_id; - g_origin->header.stamp = ros::Time::now(); - - g_origin->position.latitude = glob_orig.latitude / 1E7; - g_origin->position.longitude = glob_orig.longitude / 1E7; - g_origin->position.altitude = glob_orig.altitude / 1E3 + m_uas->geoid_to_ellipsoid_height(&g_origin->position); // convert height amsl to height above the ellipsoid - - try { - /** - * @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed) - * Note: "earth" frame, in ECEF, of the global origin - */ - GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), - GeographicLib::Constants::WGS84_f()); - - earth.Forward(g_origin->position.latitude, g_origin->position.longitude, g_origin->position.altitude, - g_origin->position.latitude, g_origin->position.longitude, g_origin->position.altitude); - - gp_global_origin_pub.publish(g_origin); - } - catch (const std::exception& e) { - ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl); - } - } - - /** @todo Handler for GLOBAL_POSITION_INT_COV */ - - void handle_global_position_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GLOBAL_POSITION_INT &gpos) - { - auto odom = boost::make_shared(); - auto fix = boost::make_shared(); - auto relative_alt = boost::make_shared(); - auto compass_heading = boost::make_shared(); - - auto header = m_uas->synchronized_header(child_frame_id, gpos.time_boot_ms); - - // Global position fix - fix->header = header; - - fill_lla(gpos, fix); - - // fill GPS status fields using GPS_RAW data - auto raw_fix = m_uas->get_gps_fix(); - if (raw_fix) { - fix->status.service = raw_fix->status.service; - fix->status.status = raw_fix->status.status; - fix->position_covariance = raw_fix->position_covariance; - fix->position_covariance_type = raw_fix->position_covariance_type; - } - else { - // no GPS_RAW_INT -> fix status unknown - fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; - fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; - - // we don't know covariance - fill_unknown_cov(fix); - } - - relative_alt->data = gpos.relative_alt / 1E3; // in meters - compass_heading->data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN; // in degrees - - /** - * @brief Global position odometry: - * - * X: spherical coordinate X-axis (meters) - * Y: spherical coordinate Y-axis (meters) - * Z: spherical coordinate Z-axis (meters) - * VX: latitude vel (m/s) - * VY: longitude vel (m/s) - * VZ: altitude vel (m/s) - * Angular rates: unknown - * Pose covariance: computed, with fixed diagonal - * Velocity covariance: unknown - */ - odom->header.stamp = header.stamp; - odom->header.frame_id = frame_id; - odom->child_frame_id = child_frame_id; - - // Linear velocity - tf::vectorEigenToMsg(Eigen::Vector3d(gpos.vy, gpos.vx, gpos.vz) / 1E2, - odom->twist.twist.linear); - - // Velocity covariance unknown - ftf::EigenMapCovariance6d vel_cov_out(odom->twist.covariance.data()); - vel_cov_out.fill(0.0); - vel_cov_out(0) = -1.0; - - // Current fix in ECEF - Eigen::Vector3d map_point; - - try { - /** - * @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed) - * - * Note: "ecef_origin" is the origin of "map" frame, in ECEF, and the local coordinates are - * in spherical coordinates, with the orientation in ENU (just like what is applied - * on Gazebo) - */ - GeographicLib::Geocentric map(GeographicLib::Constants::WGS84_a(), - GeographicLib::Constants::WGS84_f()); - - /** - * @brief Checks if the "map" origin is set. - * - If not, and the home position is also not received, it sets the current fix as the origin; - * - If the home position is received, it sets the "map" origin; - * - If the "map" origin is set, then it applies the rotations to the offset between the origin - * and the current local geocentric coordinates. - */ - // Current fix to ECEF - map.Forward(fix->latitude, fix->longitude, fix->altitude, - map_point.x(), map_point.y(), map_point.z()); - - // Set the current fix as the "map" origin if it's not set - if (!is_map_init && fix->status.status >= sensor_msgs::NavSatStatus::STATUS_FIX) { - map_origin.x() = fix->latitude; - map_origin.y() = fix->longitude; - map_origin.z() = fix->altitude; - - ecef_origin = map_point; // Local position is zero - is_map_init = true; - } - } - catch (const std::exception& e) { - ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl); - } - - // Compute the local coordinates in ECEF - local_ecef = map_point - ecef_origin; - // Compute the local coordinates in ENU - tf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef, map_origin), odom->pose.pose.position); - - /** - * @brief By default, we are using the relative altitude instead of the geocentric - * altitude, which is relative to the WGS-84 ellipsoid - */ - if (use_relative_alt) - odom->pose.pose.position.z = relative_alt->data; - - odom->pose.pose.orientation = m_uas->get_attitude_orientation_enu(); - - // Use ENU covariance to build XYZRPY covariance - ftf::EigenMapConstCovariance3d gps_cov(fix->position_covariance.data()); - ftf::EigenMapCovariance6d pos_cov_out(odom->pose.covariance.data()); - pos_cov_out.setZero(); - pos_cov_out.block<3, 3>(0, 0) = gps_cov; - pos_cov_out.block<3, 3>(3, 3).diagonal() << - rot_cov, - rot_cov, - rot_cov; - - // publish - gp_fix_pub.publish(fix); - gp_odom_pub.publish(odom); - gp_rel_alt_pub.publish(relative_alt); - gp_hdg_pub.publish(compass_heading); - - // TF - if (tf_send) { - geometry_msgs::TransformStamped transform; - - transform.header.stamp = odom->header.stamp; - transform.header.frame_id = tf_frame_id; - transform.child_frame_id = tf_child_frame_id; - - // setRotation() - transform.transform.rotation = odom->pose.pose.orientation; - - // setOrigin() - transform.transform.translation.x = odom->pose.pose.position.x; - transform.transform.translation.y = odom->pose.pose.position.y; - transform.transform.translation.z = odom->pose.pose.position.z; - - m_uas->tf2_broadcaster.sendTransform(transform); - } - } - - void handle_lpned_system_global_offset(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET &offset) - { - auto global_offset = boost::make_shared(); - global_offset->header = m_uas->synchronized_header(tf_global_frame_id, offset.time_boot_ms); - - auto enu_position = ftf::transform_frame_ned_enu(Eigen::Vector3d(offset.x, offset.y, offset.z)); - auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink( - ftf::transform_orientation_ned_enu( - ftf::quaternion_from_rpy(offset.roll, offset.pitch, offset.yaw))); - - tf::pointEigenToMsg(enu_position, global_offset->pose.position); - tf::quaternionEigenToMsg(enu_baselink_orientation, global_offset->pose.orientation); - - gp_global_offset_pub.publish(global_offset); - - // TF - if (tf_send) { - geometry_msgs::TransformStamped transform; - - transform.header.stamp = global_offset->header.stamp; - transform.header.frame_id = tf_global_frame_id; - transform.child_frame_id = tf_frame_id; - - // setRotation() - transform.transform.rotation = global_offset->pose.orientation; - - // setOrigin() - transform.transform.translation.x = global_offset->pose.position.x; - transform.transform.translation.y = global_offset->pose.position.y; - transform.transform.translation.z = global_offset->pose.position.z; - - m_uas->tf2_broadcaster.sendTransform(transform); - } - } - - /* -*- diagnostics -*- */ - void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) - { - int fix_type, satellites_visible; - float eph, epv; - - m_uas->get_gps_epts(eph, epv, fix_type, satellites_visible); - - if (satellites_visible <= 0) - stat.summary(2, "No satellites"); - else if (fix_type < 2) - stat.summary(1, "No fix"); - else if (fix_type == 2) - stat.summary(0, "2D fix"); - else if (fix_type >= 3) - stat.summary(0, "3D fix"); - - stat.addf("Satellites visible", "%zd", satellites_visible); - stat.addf("Fix type", "%d", fix_type); - - if (!std::isnan(eph)) - stat.addf("EPH (m)", "%.2f", eph); - else - stat.add("EPH (m)", "Unknown"); - - if (!std::isnan(epv)) - stat.addf("EPV (m)", "%.2f", epv); - else - stat.add("EPV (m)", "Unknown"); - } - - /* -*- callbacks -*- */ - - void home_position_cb(const mavros_msgs::HomePosition::ConstPtr &req) - { - map_origin.x() = req->geo.latitude; - map_origin.y() = req->geo.longitude; - map_origin.z() = req->geo.altitude; - - try { - /** - * @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed) - */ - GeographicLib::Geocentric map(GeographicLib::Constants::WGS84_a(), - GeographicLib::Constants::WGS84_f()); - - // map_origin to ECEF - map.Forward(map_origin.x(), map_origin.y(), map_origin.z(), - ecef_origin.x(), ecef_origin.y(), ecef_origin.z()); - } - catch (const std::exception& e) { - ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl); - } - - is_map_init = true; - } - - void set_gp_origin_cb(const geographic_msgs::GeoPointStamped::ConstPtr &req) - { - mavlink::common::msg::SET_GPS_GLOBAL_ORIGIN gpo = {}; - - Eigen::Vector3d global_position; - - gpo.target_system = m_uas->get_tgt_system(); - // gpo.time_boot_ms = stamp.toNSec() / 1000; #TODO: requires Mavlink msg update - - gpo.latitude = req->position.latitude * 1E7; - gpo.longitude = req->position.longitude * 1E7; - gpo.altitude = (req->position.altitude + m_uas->ellipsoid_to_geoid_height(&req->position)) * 1E3; - - UAS_FCU(m_uas)->send_message_ignore_drop(gpo); - } + rclcpp::Publisher::SharedPtr raw_fix_pub; + rclcpp::Publisher::SharedPtr raw_vel_pub; + rclcpp::Publisher::SharedPtr raw_sat_pub; + rclcpp::Publisher::SharedPtr gp_fix_pub; + rclcpp::Publisher::SharedPtr gp_odom_pub; + rclcpp::Publisher::SharedPtr gp_rel_alt_pub; + rclcpp::Publisher::SharedPtr gp_hdg_pub; + rclcpp::Publisher::SharedPtr gp_global_origin_pub; + rclcpp::Publisher::SharedPtr gp_global_offset_pub; + + rclcpp::Subscription::SharedPtr gp_set_global_origin_sub; + rclcpp::Subscription::SharedPtr hp_sub; + + std::string frame_id; //!< origin frame for topic headers + std::string child_frame_id; //!< body-fixed frame for topic headers + std::string tf_frame_id; //!< origin for TF + std::string tf_global_frame_id; //!< global origin for TF + std::string tf_child_frame_id; //!< frame for TF and Pose + + bool tf_send; + bool use_relative_alt; + bool is_map_init; + + double rot_cov; + double gps_uere; + + Eigen::Vector3d map_origin {}; //!< geodetic origin of map frame [lla] + Eigen::Vector3d ecef_origin {}; //!< geocentric origin of map frame [m] + Eigen::Vector3d local_ecef {}; //!< local ECEF coordinates on map frame [m] + + template + inline void fill_lla(const MsgT & msg, sensor_msgs::msg::NavSatFix & fix) + { + fix.latitude = msg.lat / 1E7; // deg + fix.longitude = msg.lon / 1E7; // deg + fix.altitude = msg.alt / 1E3 + uas->data.geoid_to_ellipsoid_height(fix); // in meters + } + + inline void fill_unknown_cov(sensor_msgs::msg::NavSatFix & fix) + { + fix.position_covariance.fill(0.0); + fix.position_covariance[0] = -1.0; + fix.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + } + + /* -*- message handlers -*- */ + + void handle_gps_raw_int( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::GPS_RAW_INT & raw_gps, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + using sensor_msgs::msg::NavSatFix; + using sensor_msgs::msg::NavSatStatus; + + auto fix = NavSatFix(); + + fix.header = uas->synchronized_header(child_frame_id, raw_gps.time_usec); + + fix.status.service = NavSatStatus::SERVICE_GPS; + if (raw_gps.fix_type > 2) { + fix.status.status = NavSatStatus::STATUS_FIX; + } else { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 30000, "GP: No GPS fix"); + fix.status.status = NavSatStatus::STATUS_NO_FIX; + } + + fill_lla(raw_gps, fix); + + float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN; + float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN; + + ftf::EigenMapCovariance3d gps_cov(fix.position_covariance.data()); + + if (msg->magic == MAVLINK_STX && + raw_gps.h_acc > 0 && raw_gps.v_acc > 0) + { + // With mavlink v2.0 use accuracies reported by sensor + + gps_cov.diagonal() << std::pow(raw_gps.h_acc / 1E3, 2), std::pow(raw_gps.h_acc / 1E3, 2), + std::pow(raw_gps.v_acc / 1E3, 2); + fix.position_covariance_type = NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + } else if (!std::isnan(eph) && !std::isnan(epv)) { + // With mavlink v1.0 approximate accuracies by DOP + + gps_cov.diagonal() << std::pow(eph * gps_uere, 2), std::pow(eph * gps_uere, 2), std::pow( + epv * gps_uere, 2); + fix.position_covariance_type = NavSatFix::COVARIANCE_TYPE_APPROXIMATED; + } else { + fill_unknown_cov(fix); + } + + // store & publish + uas->data.update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible); + raw_fix_pub->publish(fix); + + if (raw_gps.vel != UINT16_MAX && + raw_gps.cog != UINT16_MAX) + { + double speed = raw_gps.vel / 1E2; // m/s + double course = angles::from_degrees(raw_gps.cog / 1E2); // rad + + auto vel = geometry_msgs::msg::TwistStamped(); + + vel.header.stamp = fix.header.stamp; + vel.header.frame_id = frame_id; + + // From nmea_navsat_driver + vel.twist.linear.x = speed * std::sin(course); + vel.twist.linear.y = speed * std::cos(course); + + raw_vel_pub->publish(vel); + } + + // publish satellite count + auto sat_cnt = std_msgs::msg::UInt32(); + sat_cnt.data = raw_gps.satellites_visible; + raw_sat_pub->publish(sat_cnt); + } + + void handle_gps_global_origin( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::GPS_GLOBAL_ORIGIN & glob_orig, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto g_origin = geographic_msgs::msg::GeoPointStamped(); + + g_origin.header = uas->synchronized_header(tf_global_frame_id, glob_orig.time_usec); + g_origin.position.latitude = glob_orig.latitude / 1E7; + g_origin.position.longitude = glob_orig.longitude / 1E7; + g_origin.position.altitude = glob_orig.altitude / 1E3 + uas->data.geoid_to_ellipsoid_height( + g_origin.position); // convert height amsl to height above the ellipsoid + + try { + /** + * @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed) + * Note: "earth" frame, in ECEF, of the global origin + */ + GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f()); + + earth.Forward( + g_origin.position.latitude, g_origin.position.longitude, g_origin.position.altitude, + g_origin.position.latitude, g_origin.position.longitude, g_origin.position.altitude); + + gp_global_origin_pub->publish(g_origin); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(get_logger(), "GP: Caught exception: " << e.what()); + } + } + + /** @todo Handler for GLOBAL_POSITION_INT_COV */ + + void handle_global_position_int( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::GLOBAL_POSITION_INT & gpos, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto odom = nav_msgs::msg::Odometry(); + auto fix = sensor_msgs::msg::NavSatFix(); + auto relative_alt = std_msgs::msg::Float64(); + auto compass_heading = std_msgs::msg::Float64(); + + auto header = uas->synchronized_header(child_frame_id, gpos.time_boot_ms); + + // Global position fix + fix.header = header; + + fill_lla(gpos, fix); + + // fill GPS status fields using GPS_RAW data + auto raw_fix = uas->data.get_gps_fix(); + fix.status.service = raw_fix.status.service; + fix.status.status = raw_fix.status.status; + fix.position_covariance = raw_fix.position_covariance; + fix.position_covariance_type = raw_fix.position_covariance_type; + + relative_alt.data = gpos.relative_alt / 1E3; // in meters + compass_heading.data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN; // in degrees + + /** + * @brief Global position odometry: + * + * X: spherical coordinate X-axis (meters) + * Y: spherical coordinate Y-axis (meters) + * Z: spherical coordinate Z-axis (meters) + * VX: latitude vel (m/s) + * VY: longitude vel (m/s) + * VZ: altitude vel (m/s) + * Angular rates: unknown + * Pose covariance: computed, with fixed diagonal + * Velocity covariance: unknown + */ + odom.header.stamp = header.stamp; + odom.header.frame_id = frame_id; + odom.child_frame_id = child_frame_id; + + // Linear velocity + tf2::toMsg( + Eigen::Vector3d(gpos.vy, gpos.vx, gpos.vz) / 1E2, + odom.twist.twist.linear); + + // Velocity covariance unknown + ftf::EigenMapCovariance6d vel_cov_out(odom.twist.covariance.data()); + vel_cov_out.fill(0.0); + vel_cov_out(0) = -1.0; + + // Current fix in ECEF + Eigen::Vector3d map_point; + + try { + /** + * @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed) + * + * Note: "ecef_origin" is the origin of "map" frame, in ECEF, and the local coordinates are + * in spherical coordinates, with the orientation in ENU (just like what is applied + * on Gazebo) + */ + GeographicLib::Geocentric map(GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f()); + + /** + * @brief Checks if the "map" origin is set. + * - If not, and the home position is also not received, it sets the current fix as the origin; + * - If the home position is received, it sets the "map" origin; + * - If the "map" origin is set, then it applies the rotations to the offset between the origin + * and the current local geocentric coordinates. + */ + // Current fix to ECEF + map.Forward( + fix.latitude, fix.longitude, fix.altitude, + map_point.x(), map_point.y(), map_point.z()); + + // Set the current fix as the "map" origin if it's not set + if (!is_map_init && fix.status.status >= sensor_msgs::msg::NavSatStatus::STATUS_FIX) { + map_origin.x() = fix.latitude; + map_origin.y() = fix.longitude; + map_origin.z() = fix.altitude; + + ecef_origin = map_point; // Local position is zero + is_map_init = true; + } + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(get_logger(), "GP: Caught exception: " << e.what() ); + } + + // Compute the local coordinates in ECEF + local_ecef = map_point - ecef_origin; + // Compute the local coordinates in ENU + odom.pose.pose.position = tf2::toMsg( + ftf::transform_frame_ecef_enu( + local_ecef, + map_origin)); + + /** + * @brief By default, we are using the relative altitude instead of the geocentric + * altitude, which is relative to the WGS-84 ellipsoid + */ + if (use_relative_alt) { + odom.pose.pose.position.z = relative_alt.data; + } + + odom.pose.pose.orientation = uas->data.get_attitude_orientation_enu(); + + // Use ENU covariance to build XYZRPY covariance + ftf::EigenMapConstCovariance3d gps_cov(fix.position_covariance.data()); + ftf::EigenMapCovariance6d pos_cov_out(odom.pose.covariance.data()); + pos_cov_out.setZero(); + pos_cov_out.block<3, 3>(0, 0) = gps_cov; + pos_cov_out.block<3, 3>(3, 3).diagonal() << + rot_cov, + rot_cov, + rot_cov; + + // publish + gp_fix_pub->publish(fix); + gp_odom_pub->publish(odom); + gp_rel_alt_pub->publish(relative_alt); + gp_hdg_pub->publish(compass_heading); + + // TF + if (tf_send) { + geometry_msgs::msg::TransformStamped transform; + + transform.header.stamp = odom.header.stamp; + transform.header.frame_id = tf_frame_id; + transform.child_frame_id = tf_child_frame_id; + + // setRotation() + transform.transform.rotation = odom.pose.pose.orientation; + + // setOrigin() + transform.transform.translation.x = odom.pose.pose.position.x; + transform.transform.translation.y = odom.pose.pose.position.y; + transform.transform.translation.z = odom.pose.pose.position.z; + + uas->tf2_broadcaster.sendTransform(transform); + } + } + + void handle_lpned_system_global_offset( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET & offset, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto global_offset = geometry_msgs::msg::PoseStamped(); + global_offset.header = uas->synchronized_header(tf_global_frame_id, offset.time_boot_ms); + + auto enu_position = ftf::transform_frame_ned_enu(Eigen::Vector3d(offset.x, offset.y, offset.z)); + auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink( + ftf::transform_orientation_ned_enu( + ftf::quaternion_from_rpy(offset.roll, offset.pitch, offset.yaw))); + + global_offset.pose.position = tf2::toMsg(enu_position); + global_offset.pose.orientation = tf2::toMsg(enu_baselink_orientation); + + gp_global_offset_pub->publish(global_offset); + + // TF + if (tf_send) { + geometry_msgs::msg::TransformStamped transform; + + transform.header.stamp = global_offset.header.stamp; + transform.header.frame_id = tf_global_frame_id; + transform.child_frame_id = tf_frame_id; + + // setRotation() + transform.transform.rotation = global_offset.pose.orientation; + + // setOrigin() + transform.transform.translation.x = global_offset.pose.position.x; + transform.transform.translation.y = global_offset.pose.position.y; + transform.transform.translation.z = global_offset.pose.position.z; + + uas->tf2_broadcaster.sendTransform(transform); + } + } + + /* -*- diagnostics -*- */ + void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + int fix_type, satellites_visible; + float eph, epv; + + uas->data.get_gps_epts(eph, epv, fix_type, satellites_visible); + + if (satellites_visible <= 0) { + stat.summary(2, "No satellites"); + } else if (fix_type < 2) { + stat.summary(1, "No fix"); + } else if (fix_type == 2) { + stat.summary(0, "2D fix"); + } else if (fix_type >= 3) { + stat.summary(0, "3D fix"); + } + + stat.addf("Satellites visible", "%zd", satellites_visible); + stat.addf("Fix type", "%d", fix_type); + + if (!std::isnan(eph)) { + stat.addf("EPH (m)", "%.2f", eph); + } else { + stat.add("EPH (m)", "Unknown"); + } + + if (!std::isnan(epv)) { + stat.addf("EPV (m)", "%.2f", epv); + } else { + stat.add("EPV (m)", "Unknown"); + } + } + + /* -*- callbacks -*- */ + + void home_position_cb(const mavros_msgs::msg::HomePosition::SharedPtr req) + { + map_origin.x() = req->geo.latitude; + map_origin.y() = req->geo.longitude; + map_origin.z() = req->geo.altitude; + + try { + /** + * @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed) + */ + GeographicLib::Geocentric map(GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f()); + + // map_origin to ECEF + map.Forward( + map_origin.x(), map_origin.y(), map_origin.z(), + ecef_origin.x(), ecef_origin.y(), ecef_origin.z()); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(get_logger(), "GP: Caught exception: " << e.what()); + } + + is_map_init = true; + } + + void set_gp_origin_cb(const geographic_msgs::msg::GeoPointStamped::SharedPtr req) + { + mavlink::common::msg::SET_GPS_GLOBAL_ORIGIN gpo = {}; + + gpo.time_usec = get_time_usec(req->header.stamp); + gpo.target_system = uas->get_tgt_system(); + gpo.latitude = req->position.latitude * 1E7; + gpo.longitude = req->position.longitude * 1E7; + gpo.altitude = (req->position.altitude + + uas->data.ellipsoid_to_geoid_height(req->position)) * 1E3; + + uas->send_message(gpo); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::GlobalPositionPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::GlobalPositionPlugin) diff --git a/mavros/src/plugins/hil.cpp b/mavros/src/plugins/hil.cpp deleted file mode 100644 index 33ed3a82e..000000000 --- a/mavros/src/plugins/hil.cpp +++ /dev/null @@ -1,339 +0,0 @@ -/** - * @brief Hil plugin - * @file hil.cpp - * @author Mohamed Abdelkader - * @author Nuno Marques - * @author Pavel Vechersky - * @author Beat Küng - * - * @addtogroup plugin - * @{ - */ -/* - * Copyright 2016,2017 Mohamed Abdelkader, Nuno Marques, Pavel Vechersky, Beat Küng. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace mavros { -namespace std_plugins { -//! Tesla to Gauss coeff -static constexpr double TESLA_TO_GAUSS = 1.0e4; -//! Pascal to millBar coeff -static constexpr double PASCAL_TO_MILLIBAR = 1.0e-2; - -/** - * @brief Hil plugin - */ -class HilPlugin : public plugin::PluginBase { -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - HilPlugin() : PluginBase(), - hil_nh("~hil") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - hil_state_quaternion_sub = hil_nh.subscribe("state", 10, &HilPlugin::state_quat_cb, this); - hil_gps_sub = hil_nh.subscribe("gps", 10, &HilPlugin::gps_cb, this); - hil_sensor_sub = hil_nh.subscribe("imu_ned", 10, &HilPlugin::sensor_cb, this); - hil_flow_sub = hil_nh.subscribe("optical_flow", 10, &HilPlugin::optical_flow_cb, this); - hil_rcin_sub = hil_nh.subscribe("rc_inputs", 10, &HilPlugin::rcin_raw_cb, this); - - hil_controls_pub = hil_nh.advertise("controls", 10); - hil_actuator_controls_pub = hil_nh.advertise("actuator_controls", 10); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&HilPlugin::handle_hil_controls), - make_handler(&HilPlugin::handle_hil_actuator_controls), - }; - } - -private: - ros::NodeHandle hil_nh; - - ros::Publisher hil_controls_pub; - ros::Publisher hil_actuator_controls_pub; - - ros::Subscriber hil_state_quaternion_sub; - ros::Subscriber hil_gps_sub; - ros::Subscriber hil_sensor_sub; - ros::Subscriber hil_flow_sub; - ros::Subscriber hil_rcin_sub; - - Eigen::Quaterniond enu_orientation; - - /* -*- rx handlers -*- */ - - void handle_hil_controls(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIL_CONTROLS &hil_controls) { - auto hil_controls_msg = boost::make_shared(); - - hil_controls_msg->header.stamp = m_uas->synchronise_stamp(hil_controls.time_usec); - // [[[cog: - // for f in ( - // 'roll_ailerons', 'pitch_elevator', 'yaw_rudder', 'throttle', - // 'aux1', 'aux2', 'aux3', 'aux4', 'mode', 'nav_mode'): - // cog.outl("hil_controls_msg->%s = hil_controls.%s;" % (f, f)) - // ]]] - hil_controls_msg->roll_ailerons = hil_controls.roll_ailerons; - hil_controls_msg->pitch_elevator = hil_controls.pitch_elevator; - hil_controls_msg->yaw_rudder = hil_controls.yaw_rudder; - hil_controls_msg->throttle = hil_controls.throttle; - hil_controls_msg->aux1 = hil_controls.aux1; - hil_controls_msg->aux2 = hil_controls.aux2; - hil_controls_msg->aux3 = hil_controls.aux3; - hil_controls_msg->aux4 = hil_controls.aux4; - hil_controls_msg->mode = hil_controls.mode; - hil_controls_msg->nav_mode = hil_controls.nav_mode; - // [[[end]]] (checksum: a2c87ee8f36e7a32b08be5e0fe665b5a) - - hil_controls_pub.publish(hil_controls_msg); - } - - void handle_hil_actuator_controls(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIL_ACTUATOR_CONTROLS &hil_actuator_controls) { - auto hil_actuator_controls_msg = boost::make_shared(); - - hil_actuator_controls_msg->header.stamp = m_uas->synchronise_stamp(hil_actuator_controls.time_usec); - const auto &arr = hil_actuator_controls.controls; - std::copy(arr.cbegin(), arr.cend(), hil_actuator_controls_msg->controls.begin()); - hil_actuator_controls_msg->mode = hil_actuator_controls.mode; - hil_actuator_controls_msg->flags = hil_actuator_controls.flags; - - hil_actuator_controls_pub.publish(hil_actuator_controls_msg); - } - - /* -*- callbacks / low level send -*- */ - - /** - * @brief Send hil_state_quaternion to FCU. - * Message specification: @p https://mavlink.io/en/messages/common.html#HIL_STATE_QUATERNION - */ - void state_quat_cb(const mavros_msgs::HilStateQuaternion::ConstPtr &req) { - mavlink::common::msg::HIL_STATE_QUATERNION state_quat = {}; - - state_quat.time_usec = req->header.stamp.toNSec() / 1000; - auto q = ftf::transform_orientation_baselink_aircraft( - ftf::transform_orientation_enu_ned( - ftf::to_eigen(req->orientation))); - ftf::quaternion_to_mavlink(q, state_quat.attitude_quaternion); - state_quat.lat = req->geo.latitude * 1E7; - state_quat.lon = req->geo.longitude * 1E7; - // @warning geographic_msgs/GeoPoint.msg uses WGS 84 reference ellipsoid - // @TODO: Convert altitude to AMSL to be received by the FCU - // related to issue #529 - state_quat.alt = req->geo.altitude * 1E3; - state_quat.ind_airspeed = req->ind_airspeed * 1E2; - state_quat.true_airspeed = req->true_airspeed * 1E2; - // WRT world frame - auto ang_vel = ftf::transform_frame_enu_ned( - ftf::transform_frame_baselink_aircraft( - ftf::to_eigen(req->angular_velocity))); - auto lin_vel = ftf::transform_frame_enu_ned( - ftf::to_eigen(req->linear_velocity)) * 1E2; - // linear acceleration - WRT world frame - auto lin_acc = ftf::transform_frame_baselink_aircraft( - ftf::to_eigen(req->linear_acceleration)); - - // [[[cog: - // for a, b in zip(('rollspeed', 'pitchspeed', 'yawspeed'), "xyz"): - // cog.outl("state_quat.%s = ang_vel.%s();" % (a, b)) - // for f in "xyz": - // cog.outl("state_quat.v%s = lin_vel.%s();" % (f, f)) - // for f in "xyz": - // cog.outl("state_quat.%sacc = lin_acc.%s();" % (f, f)) - // ]]] - state_quat.rollspeed = ang_vel.x(); - state_quat.pitchspeed = ang_vel.y(); - state_quat.yawspeed = ang_vel.z(); - state_quat.vx = lin_vel.x(); - state_quat.vy = lin_vel.y(); - state_quat.vz = lin_vel.z(); - state_quat.xacc = lin_acc.x(); - state_quat.yacc = lin_acc.y(); - state_quat.zacc = lin_acc.z(); - // [[[end]]] (checksum: a29598b834ac1ec32ede01595aa5b3ac) - - UAS_FCU(m_uas)->send_message_ignore_drop(state_quat); - } - - /** - * @brief Send hil_gps to FCU. - * Message specification: @p https://mavlink.io/en/messages/common.html#HIL_GPS - */ - void gps_cb(const mavros_msgs::HilGPS::ConstPtr &req) { - mavlink::common::msg::HIL_GPS gps = {}; - - gps.time_usec = req->header.stamp.toNSec() / 1000; - gps.fix_type = req->fix_type; - gps.lat = req->geo.latitude * 1E7; - gps.lon = req->geo.longitude * 1E7; - // @warning geographic_msgs/GeoPoint.msg uses WGS 84 reference ellipsoid - // @TODO: Convert altitude to AMSL to be received by the FCU - // related to issue #529 - gps.alt = req->geo.altitude * 1E3; - // [[[cog: - // for f in ( - // 'eph', 'epv', 'vel', 'vn', 've', 'vd', 'cog'): - // cog.outl("gps.%s = req->%s * 1E2;" % (f, f)) - // ]]] - gps.eph = req->eph * 1E2; - gps.epv = req->epv * 1E2; - gps.vel = req->vel * 1E2; - gps.vn = req->vn * 1E2; - gps.ve = req->ve * 1E2; - gps.vd = req->vd * 1E2; - gps.cog = req->cog * 1E2; - // [[[end]]] (checksum: a283bcc78f496cead2e9f893200d825d) - gps.satellites_visible = req->satellites_visible; - - UAS_FCU(m_uas)->send_message_ignore_drop(gps); - } - - /** - * @brief Send hil_sensor to FCU. - * Message specification: @p https://mavlink.io/en/messages/common.html#HIL_SENSOR - */ - void sensor_cb(const mavros_msgs::HilSensor::ConstPtr &req) { - mavlink::common::msg::HIL_SENSOR sensor = {}; - - sensor.time_usec = req->header.stamp.toNSec() / 1000; - // WRT world frame - auto acc = ftf::transform_frame_baselink_aircraft( - ftf::to_eigen(req->acc)); - auto gyro = ftf::transform_frame_baselink_aircraft( - ftf::to_eigen(req->gyro)); - auto mag = ftf::transform_frame_baselink_aircraft( - ftf::to_eigen(req->mag) * TESLA_TO_GAUSS); - - // [[[cog: - // for a in ('acc', 'gyro', 'mag'): - // for b in "xyz": - // cog.outl("sensor.{b}{a} = {a}.{b}();".format(**locals())) - // for f in (('abs_pressure', 'PASCAL_TO_MILLIBAR'), - // ('diff_pressure', 'PASCAL_TO_MILLIBAR'), - // 'pressure_alt', 'temperature', 'fields_updated'): - // f1 = f if isinstance(f, str) else f[0] - // f2 = f if isinstance(f, str) else '{f[0]} * {f[1]}'.format(f=f) - // cog.outl("sensor.{f1} = req->{f2};".format(**locals())) - // ]]] - sensor.xacc = acc.x(); - sensor.yacc = acc.y(); - sensor.zacc = acc.z(); - sensor.xgyro = gyro.x(); - sensor.ygyro = gyro.y(); - sensor.zgyro = gyro.z(); - sensor.xmag = mag.x(); - sensor.ymag = mag.y(); - sensor.zmag = mag.z(); - sensor.abs_pressure = req->abs_pressure * PASCAL_TO_MILLIBAR; - sensor.diff_pressure = req->diff_pressure * PASCAL_TO_MILLIBAR; - sensor.pressure_alt = req->pressure_alt; - sensor.temperature = req->temperature; - sensor.fields_updated = req->fields_updated; - // [[[end]]] (checksum: 316bef821ad6fc33d9726a1c8e8c5404) - - UAS_FCU(m_uas)->send_message_ignore_drop(sensor); - } - - /** - * @brief Send simulated optical flow to FCU. - * Message specification: @p https://mavlink.io/en/messages/common.html#HIL_OPTICAL_FLOW - */ - void optical_flow_cb(const mavros_msgs::OpticalFlowRad::ConstPtr &req) { - mavlink::common::msg::HIL_OPTICAL_FLOW of = {}; - - auto int_xy = ftf::transform_frame_aircraft_baselink( - Eigen::Vector3d( - req->integrated_x, - req->integrated_y, - 0.0)); - auto int_gyro = ftf::transform_frame_aircraft_baselink( - Eigen::Vector3d( - req->integrated_xgyro, - req->integrated_ygyro, - req->integrated_zgyro)); - - of.time_usec = req->header.stamp.toNSec() / 1000; - of.sensor_id = INT8_MAX;//while we don't find a better way of handling it - of.integration_time_us = req->integration_time_us; - // [[[cog: - // for f in "xy": - // cog.outl("of.integrated_%s = int_xy.%s();" % (f, f)) - // for f in "xyz": - // cog.outl("of.integrated_%sgyro = int_gyro.%s();" % (f, f)) - // for f in ('time_delta_distance_us', 'distance', 'quality'): - // cog.outl("of.%s = req->%s;" % (f, f)) - // ]]] - of.integrated_x = int_xy.x(); - of.integrated_y = int_xy.y(); - of.integrated_xgyro = int_gyro.x(); - of.integrated_ygyro = int_gyro.y(); - of.integrated_zgyro = int_gyro.z(); - of.time_delta_distance_us = req->time_delta_distance_us; - of.distance = req->distance; - of.quality = req->quality; - // [[[end]]] (checksum: acbfae28f4f3bb8ca135423efaaa479e) - of.temperature = req->temperature * 100.0f; // in centi-degrees celsius - - UAS_FCU(m_uas)->send_message_ignore_drop(of); - } - - /** - * @brief Send simulated received RAW values of the RC channels to the FCU. - * Message specification: @p https://mavlink.io/en/messages/common.html#HIL_RC_INPUTS_RAW - */ - void rcin_raw_cb(const mavros_msgs::RCIn::ConstPtr &req) { - mavlink::common::msg::HIL_RC_INPUTS_RAW rcin {}; - - constexpr size_t MAX_CHANCNT = 12; - - std::array channels; - auto n = std::min(req->channels.size(), channels.size()); - std::copy(req->channels.begin(), req->channels.begin() + n, channels.begin()); - std::fill(channels.begin() + n, channels.end(), UINT16_MAX); - - rcin.time_usec = req->header.stamp.toNSec() / 100000; - // [[[cog: - // for i in range(1,13): - // cog.outl("rcin.chan%d_raw\t= channels[%2d];" % (i, i-1)) - // ]]] - rcin.chan1_raw = channels[ 0]; - rcin.chan2_raw = channels[ 1]; - rcin.chan3_raw = channels[ 2]; - rcin.chan4_raw = channels[ 3]; - rcin.chan5_raw = channels[ 4]; - rcin.chan6_raw = channels[ 5]; - rcin.chan7_raw = channels[ 6]; - rcin.chan8_raw = channels[ 7]; - rcin.chan9_raw = channels[ 8]; - rcin.chan10_raw = channels[ 9]; - rcin.chan11_raw = channels[10]; - rcin.chan12_raw = channels[11]; - // [[[end]]] (checksum: 8d6860789d596dc39e81b351c3a50fcd) - - UAS_FCU(m_uas)->send_message_ignore_drop(rcin); - } -}; -} // namespace std_plugins -} // namespace mavros - -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::HilPlugin, mavros::plugin::PluginBase) diff --git a/mavros/src/plugins/home_position.cpp b/mavros/src/plugins/home_position.cpp index d87229923..33f0e1b34 100644 --- a/mavros/src/plugins/home_position.cpp +++ b/mavros/src/plugins/home_position.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2017 Thomas Stastny, Nuno Marques. + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief HomePosition plugin * @file home_position.cpp @@ -7,175 +15,200 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2017 Thomas Stastny, Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include -#include -#include -#include +#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "std_srvs/srv/trigger.hpp" +#include "mavros_msgs/srv/command_long.hpp" +#include "mavros_msgs/msg/home_position.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT -namespace mavros { -namespace std_plugins { /** * @brief home position plugin. + * @plugin home_position * * Publishes home position. */ -class HomePositionPlugin : public plugin::PluginBase { +class HomePositionPlugin : public plugin::Plugin +{ public: - HomePositionPlugin() : - hp_nh("~home_position"), - REQUEST_POLL_TIME_DT(REQUEST_POLL_TIME_MS / 1000.0) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - hp_pub = hp_nh.advertise("home", 2, true); - hp_sub = hp_nh.subscribe("set", 10, &HomePositionPlugin::home_position_cb, this); - update_srv = hp_nh.advertiseService("req_update", &HomePositionPlugin::req_update_cb, this); - - poll_timer = hp_nh.createTimer(REQUEST_POLL_TIME_DT, &HomePositionPlugin::timeout_cb, this); - poll_timer.stop(); - enable_connection_cb(); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&HomePositionPlugin::handle_home_position), - }; - } + explicit HomePositionPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "home_position") + { + auto state_qos = rclcpp::QoS(10).transient_local(); + + hp_pub = node->create_publisher("~/home", state_qos); + hp_sub = + node->create_subscription( + "~/set", 10, + std::bind(&HomePositionPlugin::home_position_cb, this, _1)); + update_srv = + node->create_service( + "~/req_update", + std::bind(&HomePositionPlugin::req_update_cb, this, _1, _2)); + + + poll_timer = + node->create_wall_timer(REQUEST_POLL_TIME, std::bind(&HomePositionPlugin::timeout_cb, this)); + poll_timer->cancel(); + + enable_connection_cb(); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&HomePositionPlugin::handle_home_position), + }; + } private: - ros::NodeHandle hp_nh; - - ros::Publisher hp_pub; - ros::Subscriber hp_sub; - ros::ServiceServer update_srv; - - ros::Timer poll_timer; - - static constexpr int REQUEST_POLL_TIME_MS = 10000; //! position refresh poll interval - const ros::Duration REQUEST_POLL_TIME_DT; - - bool call_get_home_position(void) - { - using mavlink::common::MAV_CMD; - - bool ret = false; - - try { - ros::NodeHandle pnh("~"); - auto client = pnh.serviceClient("cmd/command"); - - mavros_msgs::CommandLong cmd{}; - - cmd.request.command = utils::enum_value(MAV_CMD::GET_HOME_POSITION); - - ret = client.call(cmd); - ret = cmd.response.success; - } - catch (ros::InvalidNameException &ex) { - ROS_ERROR_NAMED("home_position", "HP: %s", ex.what()); - } - - return ret; - } - - void handle_home_position(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HOME_POSITION &home_position) - { - poll_timer.stop(); - - auto hp = boost::make_shared(); - - auto pos = ftf::transform_frame_ned_enu(Eigen::Vector3d(home_position.x, home_position.y, home_position.z)); - auto q = ftf::transform_orientation_ned_enu(ftf::mavlink_to_quaternion(home_position.q)); - auto hp_approach_enu = ftf::transform_frame_ned_enu(Eigen::Vector3d(home_position.approach_x, home_position.approach_y, home_position.approach_z)); - - hp->header.stamp = ros::Time::now(); - hp->geo.latitude = home_position.latitude / 1E7; // deg - hp->geo.longitude = home_position.longitude / 1E7; // deg - hp->geo.altitude = home_position.altitude / 1E3 + m_uas->geoid_to_ellipsoid_height(&hp->geo); // in meters - tf::quaternionEigenToMsg(q, hp->orientation); - tf::pointEigenToMsg(pos, hp->position); - tf::vectorEigenToMsg(hp_approach_enu, hp->approach); - - ROS_DEBUG_NAMED("home_position", "HP: Home lat %f, long %f, alt %f", hp->geo.latitude, hp->geo.longitude, hp->geo.altitude); - hp_pub.publish(hp); - } - - void home_position_cb(const mavros_msgs::HomePosition::ConstPtr &req) - { - mavlink::common::msg::SET_HOME_POSITION hp {}; - - Eigen::Vector3d pos, approach; - Eigen::Quaterniond q; - - tf::pointMsgToEigen(req->position, pos); - pos = ftf::transform_frame_enu_ned(pos); - - tf::quaternionMsgToEigen(req->orientation, q); - q = ftf::transform_orientation_enu_ned(q); - - tf::vectorMsgToEigen(req->approach, approach); - approach = ftf::transform_frame_enu_ned(approach); - - hp.target_system = m_uas->get_tgt_system(); - ftf::quaternion_to_mavlink(q, hp.q); - - hp.altitude = req->geo.altitude * 1e3 + m_uas->ellipsoid_to_geoid_height(&req->geo); - // [[[cog: - // for f, m in (('latitude', '1e7'), ('longitude', '1e7')): - // cog.outl("hp.{f} = req->geo.{f} * {m};".format(**locals())) - // for a, b in (('', 'pos'), ('approach_', 'approach')): - // for f in "xyz": - // cog.outl("hp.{a}{f} = {b}.{f}();".format(**locals())) - // ]]] - hp.latitude = req->geo.latitude * 1e7; - hp.longitude = req->geo.longitude * 1e7; - hp.x = pos.x(); - hp.y = pos.y(); - hp.z = pos.z(); - hp.approach_x = approach.x(); - hp.approach_y = approach.y(); - hp.approach_z = approach.z(); - // [[[end]]] (checksum: 9c40c5b3ac06b3b82016b4f07a8e12b2) - - UAS_FCU(m_uas)->send_message_ignore_drop(hp); - } - - bool req_update_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - res.success = call_get_home_position(); - return true; - } - - void timeout_cb(const ros::TimerEvent &event) - { - ROS_INFO_NAMED("home_position", "HP: requesting home position"); - call_get_home_position(); - } - - void connection_cb(bool connected) override - { - if (connected) - poll_timer.start(); - else - poll_timer.stop(); - } + rclcpp::Publisher::SharedPtr hp_pub; + rclcpp::Subscription::SharedPtr hp_sub; + rclcpp::Service::SharedPtr update_srv; + + rclcpp::TimerBase::SharedPtr poll_timer; + + const std::chrono::nanoseconds REQUEST_POLL_TIME = 10s; + + bool call_get_home_position(void) + { + using mavlink::common::MAV_CMD; + + bool ret = false; + + try { + auto client = node->create_client("cmd/command"); + + auto cmdrq = std::make_shared(); + cmdrq->command = utils::enum_value(MAV_CMD::GET_HOME_POSITION); + + auto future = client->async_send_request(cmdrq); + auto response = future.get(); + ret = response->success; + } catch (std::exception & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "HP: %s" << ex.what()); + } + + return ret; + } + + void handle_home_position( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::HOME_POSITION & home_position, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + poll_timer->cancel(); + + auto hp = mavros_msgs::msg::HomePosition(); + + auto pos = + ftf::transform_frame_ned_enu( + Eigen::Vector3d( + home_position.x, home_position.y, + home_position.z)); + auto q = ftf::transform_orientation_ned_enu(ftf::mavlink_to_quaternion(home_position.q)); + auto hp_approach_enu = + ftf::transform_frame_ned_enu( + Eigen::Vector3d( + home_position.approach_x, + home_position.approach_y, home_position.approach_z)); + + hp.header.stamp = uas->synchronise_stamp(home_position.time_usec); + hp.geo.latitude = home_position.latitude / 1E7; // deg + hp.geo.longitude = home_position.longitude / 1E7; // deg + hp.geo.altitude = home_position.altitude / 1E3 + + uas->data.geoid_to_ellipsoid_height(hp.geo); // in meters + hp.orientation = tf2::toMsg(q); + hp.position = tf2::toMsg(pos); + tf2::toMsg(hp_approach_enu, hp.approach); + + RCLCPP_DEBUG( + get_logger(), "HP: Home lat %f, long %f, alt %f", hp.geo.latitude, + hp.geo.longitude, hp.geo.altitude); + + hp_pub->publish(hp); + } + + void home_position_cb(const mavros_msgs::msg::HomePosition::SharedPtr req) + { + mavlink::common::msg::SET_HOME_POSITION hp {}; + + Eigen::Vector3d pos, approach; + Eigen::Quaterniond q; + + tf2::fromMsg(req->position, pos); + pos = ftf::transform_frame_enu_ned(pos); + + tf2::fromMsg(req->orientation, q); + q = ftf::transform_orientation_enu_ned(q); + + tf2::fromMsg(req->approach, approach); + approach = ftf::transform_frame_enu_ned(approach); + + hp.target_system = uas->get_tgt_system(); + ftf::quaternion_to_mavlink(q, hp.q); + + hp.time_usec = get_time_usec(req->header.stamp); + hp.altitude = req->geo.altitude * 1e3 + uas->data.ellipsoid_to_geoid_height(req->geo); + // [[[cog: + // for f, m in (('latitude', '1e7'), ('longitude', '1e7')): + // cog.outl(f"hp.{f} = req->geo.{f} * {m};") + // for a, b in (('', 'pos'), ('approach_', 'approach')): + // for f in "xyz": + // cog.outl(f"hp.{a}{f} = {b}.{f}();") + // ]]] + hp.latitude = req->geo.latitude * 1e7; + hp.longitude = req->geo.longitude * 1e7; + hp.x = pos.x(); + hp.y = pos.y(); + hp.z = pos.z(); + hp.approach_x = approach.x(); + hp.approach_y = approach.y(); + hp.approach_z = approach.z(); + // [[[end]]] (checksum: 0865d5fcc1f7a15e36e177fc49dee70f) + + uas->send_message(hp); + } + + void req_update_cb( + const std_srvs::srv::Trigger::Request::SharedPtr req [[maybe_unused]], + std_srvs::srv::Trigger::Response::SharedPtr res) + { + res->success = call_get_home_position(); + } + + void timeout_cb() + { + RCLCPP_INFO(get_logger(), "HP: requesting home position"); + call_get_home_position(); + } + + void connection_cb(bool connected) override + { + if (connected) { + poll_timer->reset(); + } else { + poll_timer->cancel(); + } + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::HomePositionPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::HomePositionPlugin) diff --git a/mavros/src/plugins/imu.cpp b/mavros/src/plugins/imu.cpp index dbc657aab..d71057683 100644 --- a/mavros/src/plugins/imu.cpp +++ b/mavros/src/plugins/imu.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2013-2017,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief IMU and attitude data parser plugin * @file imu.cpp @@ -6,26 +13,29 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2013-2017 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ + +#include #include -#include -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -#include -#include -#include -#include -#include +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/magnetic_field.hpp" +#include "sensor_msgs/msg/temperature.hpp" +#include "sensor_msgs/msg/fluid_pressure.hpp" +#include "geometry_msgs/msg/vector3.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace std_plugins { //! Gauss to Tesla coeff static constexpr double GAUSS_TO_TESLA = 1.0e-4; //! millTesla to Tesla coeff @@ -42,532 +52,597 @@ static constexpr double MILLIBAR_TO_PASCAL = 1.0e2; static constexpr double RAD_TO_DEG = 180.0 / M_PI; -//! @brief IMU and attitude data publication plugin -class IMUPlugin : public plugin::PluginBase { +/** + * @brief IMU and attitude data publication plugin + * @plugin imu + */ +class IMUPlugin : public plugin::Plugin +{ public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - IMUPlugin() : PluginBase(), - imu_nh("~imu"), - has_hr_imu(false), - has_raw_imu(false), - has_scaled_imu(false), - has_att_quat(false), - received_linear_accel(false), - linear_accel_vec_flu(Eigen::Vector3d::Zero()), - linear_accel_vec_frd(Eigen::Vector3d::Zero()) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - double linear_stdev, angular_stdev, orientation_stdev, mag_stdev; - - /** - * @warning A rotation from the aircraft-frame to the base_link frame is applied. - * Additionally, it is reported the orientation of the vehicle to describe the - * transformation from the ENU frame to the base_link frame (ENU <-> base_link). - * THIS ORIENTATION IS NOT THE SAME AS THAT REPORTED BY THE FCU (NED <-> aircraft). - */ - imu_nh.param("frame_id", frame_id, "base_link"); - imu_nh.param("linear_acceleration_stdev", linear_stdev, 0.0003); // check default by MPU6000 spec - imu_nh.param("angular_velocity_stdev", angular_stdev, 0.02 * (M_PI / 180.0)); // check default by MPU6000 spec - imu_nh.param("orientation_stdev", orientation_stdev, 1.0); - imu_nh.param("magnetic_stdev", mag_stdev, 0.0); - - setup_covariance(linear_acceleration_cov, linear_stdev); - setup_covariance(angular_velocity_cov, angular_stdev); - setup_covariance(orientation_cov, orientation_stdev); - setup_covariance(magnetic_cov, mag_stdev); - setup_covariance(unk_orientation_cov, 0.0); - - imu_pub = imu_nh.advertise("data", 10); - magn_pub = imu_nh.advertise("mag", 10); - temp_imu_pub = imu_nh.advertise("temperature_imu", 10); - temp_baro_pub = imu_nh.advertise("temperature_baro", 10); - static_press_pub = imu_nh.advertise("static_pressure", 10); - diff_press_pub = imu_nh.advertise("diff_pressure", 10); - imu_raw_pub = imu_nh.advertise("data_raw", 10); - - // Reset has_* flags on connection change - enable_connection_cb(); - } - - Subscriptions get_subscriptions() override { - return { - make_handler(&IMUPlugin::handle_attitude), - make_handler(&IMUPlugin::handle_attitude_quaternion), - make_handler(&IMUPlugin::handle_highres_imu), - make_handler(&IMUPlugin::handle_raw_imu), - make_handler(&IMUPlugin::handle_scaled_imu), - make_handler(&IMUPlugin::handle_scaled_pressure), - }; - } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit IMUPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "imu"), + has_hr_imu(false), + has_raw_imu(false), + has_scaled_imu(false), + has_att_quat(false), + received_linear_accel(false), + linear_accel_vec_flu(Eigen::Vector3d::Zero()), + linear_accel_vec_frd(Eigen::Vector3d::Zero()) + { + enable_node_watch_parameters(); + + /** + * @warning A rotation from the aircraft-frame to the base_link frame is applied. + * Additionally, it is reported the orientation of the vehicle to describe the + * transformation from the ENU frame to the base_link frame (ENU <-> base_link). + * THIS ORIENTATION IS NOT THE SAME AS THAT REPORTED BY THE FCU (NED <-> aircraft). + */ + node_declare_and_watch_parameter( + "frame_id", "base_link", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + + node_declare_and_watch_parameter( + "linear_acceleration_stdev", 0.0003, [&](const rclcpp::Parameter & p) { + auto linear_stdev = p.as_double(); + setup_covariance(linear_acceleration_cov, linear_stdev); + }); + node_declare_and_watch_parameter( + "angular_velocity_stdev", 0.02 * (M_PI / 180.0), [&](const rclcpp::Parameter & p) { + auto angular_stdev = p.as_double(); + setup_covariance(angular_velocity_cov, angular_stdev); + }); + node_declare_and_watch_parameter( + "orientation_stdev", 1.0, [&](const rclcpp::Parameter & p) { + auto orientation_stdev = p.as_double(); + setup_covariance(orientation_cov, orientation_stdev); + }); + node_declare_and_watch_parameter( + "magnetic_stdev", 0.0, [&](const rclcpp::Parameter & p) { + auto mag_stdev = p.as_double(); + setup_covariance(magnetic_cov, mag_stdev); + }); + + setup_covariance(unk_orientation_cov, 0.0); + + auto sensor_qos = rclcpp::SensorDataQoS(); + + imu_pub = node->create_publisher("~/data", sensor_qos); + imu_raw_pub = node->create_publisher("~/data_raw", sensor_qos); + magn_pub = node->create_publisher("~/mag", sensor_qos); + temp_imu_pub = node->create_publisher( + "~/temperature_imu", + sensor_qos); + temp_baro_pub = node->create_publisher( + "~/temperature_baro", + sensor_qos); + static_press_pub = node->create_publisher( + "~/static_pressure", + sensor_qos); + diff_press_pub = node->create_publisher( + "~/diff_pressure", + sensor_qos); + + // Reset has_* flags on connection change + enable_connection_cb(); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&IMUPlugin::handle_attitude), + make_handler(&IMUPlugin::handle_attitude_quaternion), + make_handler(&IMUPlugin::handle_highres_imu), + make_handler(&IMUPlugin::handle_raw_imu), + make_handler(&IMUPlugin::handle_scaled_imu), + make_handler(&IMUPlugin::handle_scaled_pressure), + }; + } private: - ros::NodeHandle imu_nh; - std::string frame_id; - - ros::Publisher imu_pub; - ros::Publisher imu_raw_pub; - ros::Publisher magn_pub; - ros::Publisher temp_imu_pub; - ros::Publisher temp_baro_pub; - ros::Publisher static_press_pub; - ros::Publisher diff_press_pub; - - bool has_hr_imu; - bool has_raw_imu; - bool has_scaled_imu; - bool has_att_quat; - bool received_linear_accel; - Eigen::Vector3d linear_accel_vec_flu; - Eigen::Vector3d linear_accel_vec_frd; - ftf::Covariance3d linear_acceleration_cov; - ftf::Covariance3d angular_velocity_cov; - ftf::Covariance3d orientation_cov; - ftf::Covariance3d unk_orientation_cov; - ftf::Covariance3d magnetic_cov; - - /* -*- helpers -*- */ - - /** - * @brief Setup 3x3 covariance matrix - * @param cov Covariance matrix - * @param stdev Standard deviation - * @remarks Diagonal computed from the stdev - */ - void setup_covariance(ftf::Covariance3d &cov, double stdev) - { - ftf::EigenMapCovariance3d c(cov.data()); - - c.setZero(); - if (stdev) { - double sr = stdev * stdev; - c.diagonal() << sr, sr, sr; - } - else { - c(0,0) = -1.0; - } - } - - /** - * @brief Fill and publish IMU data message. - * @param time_boot_ms Message timestamp (not syncronized) - * @param orientation_enu Orientation in the base_link ENU frame - * @param orientation_ned Orientation in the aircraft NED frame - * @param gyro_flu Angular velocity/rate in the base_link Forward-Left-Up frame - * @param gyro_frd Angular velocity/rate in the aircraft Forward-Right-Down frame - */ - void publish_imu_data(uint32_t time_boot_ms, Eigen::Quaterniond &orientation_enu, - Eigen::Quaterniond &orientation_ned, Eigen::Vector3d &gyro_flu, Eigen::Vector3d &gyro_frd) - { - auto imu_ned_msg = boost::make_shared(); - auto imu_enu_msg = boost::make_shared(); - - // Fill message header - imu_enu_msg->header = m_uas->synchronized_header(frame_id, time_boot_ms); - imu_ned_msg->header = m_uas->synchronized_header("aircraft", time_boot_ms); - - // Convert from Eigen::Quaternond to geometry_msgs::Quaternion - tf::quaternionEigenToMsg(orientation_enu, imu_enu_msg->orientation); - tf::quaternionEigenToMsg(orientation_ned, imu_ned_msg->orientation); - - // Convert from Eigen::Vector3d to geometry_msgs::Vector3 - tf::vectorEigenToMsg(gyro_flu, imu_enu_msg->angular_velocity); - tf::vectorEigenToMsg(gyro_frd, imu_ned_msg->angular_velocity); - - // Eigen::Vector3d from HIGHRES_IMU or RAW_IMU, to geometry_msgs::Vector3 - tf::vectorEigenToMsg(linear_accel_vec_flu, imu_enu_msg->linear_acceleration); - tf::vectorEigenToMsg(linear_accel_vec_frd, imu_ned_msg->linear_acceleration); - - // Pass ENU msg covariances - imu_enu_msg->orientation_covariance = orientation_cov; - imu_enu_msg->angular_velocity_covariance = angular_velocity_cov; - imu_enu_msg->linear_acceleration_covariance = linear_acceleration_cov; - - // Pass NED msg covariances - imu_ned_msg->orientation_covariance = orientation_cov; - imu_ned_msg->angular_velocity_covariance = angular_velocity_cov; - imu_ned_msg->linear_acceleration_covariance = linear_acceleration_cov; - - if (!received_linear_accel) { - // Set element 0 of covariance matrix to -1 if no data received as per sensor_msgs/Imu defintion - imu_enu_msg->linear_acceleration_covariance[0] = -1; - imu_ned_msg->linear_acceleration_covariance[0] = -1; - } - - /** Store attitude in base_link ENU - * @snippet src/plugins/imu.cpp store_enu - */ - // [store_enu] - m_uas->update_attitude_imu_enu(imu_enu_msg); - // [store_enu] - - /** Store attitude in aircraft NED - * @snippet src/plugins/imu.cpp store_ned - */ - // [store_enu] - m_uas->update_attitude_imu_ned(imu_ned_msg); - // [store_ned] - - /** Publish only base_link ENU message - * @snippet src/plugins/imu.cpp pub_enu - */ - // [pub_enu] - imu_pub.publish(imu_enu_msg); - // [pub_enu] - } - - /** - * @brief Fill and publish IMU data_raw message; store linear acceleration for IMU data - * @param header Message frame_id and timestamp - * @param gyro_flu Orientation in the base_link Forward-Left-Up frame - * @param accel_flu Linear acceleration in the base_link Forward-Left-Up frame - * @param accel_frd Linear acceleration in the aircraft Forward-Right-Down frame - */ - void publish_imu_data_raw(std_msgs::Header &header, Eigen::Vector3d &gyro_flu, - Eigen::Vector3d &accel_flu, Eigen::Vector3d &accel_frd) - { - auto imu_msg = boost::make_shared(); - - // Fill message header - imu_msg->header = header; - - tf::vectorEigenToMsg(gyro_flu, imu_msg->angular_velocity); - tf::vectorEigenToMsg(accel_flu, imu_msg->linear_acceleration); - - // Save readings - linear_accel_vec_flu = accel_flu; - linear_accel_vec_frd = accel_frd; - received_linear_accel = true; - - imu_msg->orientation_covariance = unk_orientation_cov; - imu_msg->angular_velocity_covariance = angular_velocity_cov; - imu_msg->linear_acceleration_covariance = linear_acceleration_cov; - - // Publish message [ENU frame] - imu_raw_pub.publish(imu_msg); - } - - /** - * @brief Publish magnetic field data - * @param header Message frame_id and timestamp - * @param mag_field Magnetic field in the base_link ENU frame - */ - void publish_mag(std_msgs::Header &header, Eigen::Vector3d &mag_field) - { - auto magn_msg = boost::make_shared(); - - // Fill message header - magn_msg->header = header; - - tf::vectorEigenToMsg(mag_field, magn_msg->magnetic_field); - magn_msg->magnetic_field_covariance = magnetic_cov; - - // Publish message [ENU frame] - magn_pub.publish(magn_msg); - } - - /* -*- message handlers -*- */ - - /** - * @brief Handle ATTITUDE MAVlink message. - * Message specification: https://mavlink.io/en/messages/common.html#ATTITUDE - * @param msg Received Mavlink msg - * @param att ATTITUDE msg - */ - void handle_attitude(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE &att) - { - if (has_att_quat) - return; - - /** Orientation on the NED-aicraft frame: - * @snippet src/plugins/imu.cpp ned_aircraft_orient1 - */ - // [ned_aircraft_orient1] - auto ned_aircraft_orientation = ftf::quaternion_from_rpy(att.roll, att.pitch, att.yaw); - // [ned_aircraft_orient1] - - /** Angular velocity on the NED-aicraft frame: - * @snippet src/plugins/imu.cpp ned_ang_vel1 - */ - // [frd_ang_vel1] - auto gyro_frd = Eigen::Vector3d(att.rollspeed, att.pitchspeed, att.yawspeed); - // [frd_ang_vel1] - - /** The RPY describes the rotation: aircraft->NED. - * It is required to change this to aircraft->base_link: - * @snippet src/plugins/imu.cpp ned->baselink->enu - */ - // [ned->baselink->enu] - auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink( - ftf::transform_orientation_ned_enu(ned_aircraft_orientation)); - // [ned->baselink->enu] - - /** The angular velocity expressed in the aircraft frame. - * It is required to apply the static rotation to get it into the base_link frame: - * @snippet src/plugins/imu.cpp rotate_gyro - */ - // [rotate_gyro] - auto gyro_flu = ftf::transform_frame_aircraft_baselink(gyro_frd); - // [rotate_gyro] - - publish_imu_data(att.time_boot_ms, enu_baselink_orientation, ned_aircraft_orientation, gyro_flu, gyro_frd); - } - - /** - * @brief Handle ATTITUDE_QUATERNION MAVlink message. - * Message specification: https://mavlink.io/en/messages/common.html/#ATTITUDE_QUATERNION - * @param msg Received Mavlink msg - * @param att_q ATTITUDE_QUATERNION msg - */ - void handle_attitude_quaternion(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE_QUATERNION &att_q) - { - ROS_INFO_COND_NAMED(!has_att_quat, "imu", "IMU: Attitude quaternion IMU detected!"); - has_att_quat = true; - - /** Orientation on the NED-aicraft frame: - * @snippet src/plugins/imu.cpp ned_aircraft_orient2 - */ - // [ned_aircraft_orient2] - auto ned_aircraft_orientation = Eigen::Quaterniond(att_q.q1, att_q.q2, att_q.q3, att_q.q4); - // [ned_aircraft_orient2] - - /** Angular velocity on the NED-aicraft frame: - * @snippet src/plugins/imu.cpp ned_ang_vel2 - */ - // [frd_ang_vel2] - auto gyro_frd = Eigen::Vector3d(att_q.rollspeed, att_q.pitchspeed, att_q.yawspeed); - // [frd_ang_vel2] - - /** MAVLink quaternion exactly matches Eigen convention. - * The RPY describes the rotation: aircraft->NED. - * It is required to change this to aircraft->base_link: - * @snippet src/plugins/imu.cpp ned->baselink->enu - */ - auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink( - ftf::transform_orientation_ned_enu(ned_aircraft_orientation)); - - /** The angular velocity expressed in the aircraft frame. - * It is required to apply the static rotation to get it into the base_link frame: - * @snippet src/plugins/imu.cpp rotate_gyro - */ - auto gyro_flu = ftf::transform_frame_aircraft_baselink(gyro_frd); - - publish_imu_data(att_q.time_boot_ms, enu_baselink_orientation, ned_aircraft_orientation, gyro_flu, gyro_frd); - } - - /** - * @brief Handle HIGHRES_IMU MAVlink message. - * Message specification: https://mavlink.io/en/messages/common.html/#HIGHRES_IMU - * @param msg Received Mavlink msg - * @param imu_hr HIGHRES_IMU msg - */ - void handle_highres_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIGHRES_IMU &imu_hr) - { - ROS_INFO_COND_NAMED(!has_hr_imu, "imu", "IMU: High resolution IMU detected!"); - has_hr_imu = true; - - auto header = m_uas->synchronized_header(frame_id, imu_hr.time_usec); - /** @todo Make more paranoic check of HIGHRES_IMU.fields_updated - */ - - /** Check if accelerometer + gyroscope data are available. - * Data is expressed in aircraft frame it is required to rotate to the base_link frame: - * @snippet src/plugins/imu.cpp accel_available - */ - // [accel_available] - if (imu_hr.fields_updated & ((7 << 3) | (7 << 0))) { - auto gyro_flu = ftf::transform_frame_aircraft_baselink(Eigen::Vector3d(imu_hr.xgyro, imu_hr.ygyro, imu_hr.zgyro)); - - auto accel_frd = Eigen::Vector3d(imu_hr.xacc, imu_hr.yacc, imu_hr.zacc); - auto accel_flu = ftf::transform_frame_aircraft_baselink(accel_frd); - - publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd); - } - // [accel_available] - - /** Check if magnetometer data is available: - * @snippet src/plugins/imu.cpp mag_available - */ - // [mag_available] - if (imu_hr.fields_updated & (7 << 6)) { - auto mag_field = ftf::transform_frame_aircraft_baselink( - Eigen::Vector3d(imu_hr.xmag, imu_hr.ymag, imu_hr.zmag) * GAUSS_TO_TESLA); - - publish_mag(header, mag_field); - } - // [mag_available] - - /** Check if static pressure sensor data is available: - * @snippet src/plugins/imu.cpp static_pressure_available - */ - // [static_pressure_available] - if (imu_hr.fields_updated & (1 << 9)) { - auto static_pressure_msg = boost::make_shared(); - - static_pressure_msg->header = header; - static_pressure_msg->fluid_pressure = imu_hr.abs_pressure; - - static_press_pub.publish(static_pressure_msg); - } - // [static_pressure_available] - - /** Check if differential pressure sensor data is available: - * @snippet src/plugins/imu.cpp differential_pressure_available - */ - // [differential_pressure_available] - if (imu_hr.fields_updated & (1 << 10)) { - auto differential_pressure_msg = boost::make_shared(); - - differential_pressure_msg->header = header; - differential_pressure_msg->fluid_pressure = imu_hr.diff_pressure; - - diff_press_pub.publish(differential_pressure_msg); - } - // [differential_pressure_available] - - /** Check if temperature data is available: - * @snippet src/plugins/imu.cpp temperature_available - */ - // [temperature_available] - if (imu_hr.fields_updated & (1 << 12)) { - auto temp_msg = boost::make_shared(); - - temp_msg->header = header; - temp_msg->temperature = imu_hr.temperature; - - temp_imu_pub.publish(temp_msg); - } - // [temperature_available] - } - - /** - * @brief Handle RAW_IMU MAVlink message. - * Message specification: https://mavlink.io/en/messages/common.html/#RAW_IMU - * @param msg Received Mavlink msg - * @param imu_raw RAW_IMU msg - */ - void handle_raw_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RAW_IMU &imu_raw) - { - ROS_INFO_COND_NAMED(!has_raw_imu, "imu", "IMU: Raw IMU message used."); - has_raw_imu = true; - - if (has_hr_imu || has_scaled_imu) - return; - - auto imu_msg = boost::make_shared(); - auto header = m_uas->synchronized_header(frame_id, imu_raw.time_usec); - - /** @note APM send SCALED_IMU data as RAW_IMU - */ - auto gyro_flu = ftf::transform_frame_aircraft_baselink( - Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC); - auto accel_frd = Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc); - auto accel_flu = ftf::transform_frame_aircraft_baselink(accel_frd); - - if (m_uas->is_ardupilotmega()) { - accel_frd *= MILLIG_TO_MS2; - accel_flu *= MILLIG_TO_MS2; - } else if (m_uas->is_px4()) { - accel_frd *= MILLIMS2_TO_MS2; - accel_flu *= MILLIMS2_TO_MS2; - } - - publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd); - - if (!m_uas->is_ardupilotmega()) { - ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: linear acceleration on RAW_IMU known on APM only."); - ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: ~imu/data_raw stores unscaled raw acceleration report."); - linear_accel_vec_flu.setZero(); - linear_accel_vec_frd.setZero(); - } - - /** Magnetic field data: - * @snippet src/plugins/imu.cpp mag_field - */ - // [mag_field] - auto mag_field = ftf::transform_frame_aircraft_baselink( - Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA); - // [mag_field] - - publish_mag(header, mag_field); - } - - /** - * @brief Handle SCALED_IMU MAVlink message. - * Message specification: https://mavlink.io/en/messages/common.html/#SCALED_IMU - * @param msg Received Mavlink msg - * @param imu_raw SCALED_IMU msg - */ - void handle_scaled_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SCALED_IMU &imu_raw) - { - if (has_hr_imu) - return; - - ROS_INFO_COND_NAMED(!has_scaled_imu, "imu", "IMU: Scaled IMU message used."); - has_scaled_imu = true; - - auto imu_msg = boost::make_shared(); - auto header = m_uas->synchronized_header(frame_id, imu_raw.time_boot_ms); - - auto gyro_flu = ftf::transform_frame_aircraft_baselink( - Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC); - auto accel_frd = Eigen::Vector3d(Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc) * MILLIG_TO_MS2); - auto accel_flu = ftf::transform_frame_aircraft_baselink(accel_frd); - - publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd); - - /** Magnetic field data: - * @snippet src/plugins/imu.cpp mag_field - */ - auto mag_field = ftf::transform_frame_aircraft_baselink( - Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA); - - publish_mag(header, mag_field); - } - - /** - * @brief Handle SCALED_PRESSURE MAVlink message. - * Message specification: https://mavlink.io/en/messages/common.html/#SCALED_PRESSURE - * @param msg Received Mavlink msg - * @param press SCALED_PRESSURE msg - */ - void handle_scaled_pressure(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SCALED_PRESSURE &press) - { - if (has_hr_imu) - return; - - auto header = m_uas->synchronized_header(frame_id, press.time_boot_ms); - - auto temp_msg = boost::make_shared(); - temp_msg->header = header; - temp_msg->temperature = press.temperature / 100.0; - temp_baro_pub.publish(temp_msg); - - auto static_pressure_msg = boost::make_shared(); - static_pressure_msg->header = header; - static_pressure_msg->fluid_pressure = press.press_abs * 100.0; - static_press_pub.publish(static_pressure_msg); - - auto differential_pressure_msg = boost::make_shared(); - differential_pressure_msg->header = header; - differential_pressure_msg->fluid_pressure = press.press_diff * 100.0; - diff_press_pub.publish(differential_pressure_msg); - } - - // Checks for connection and overrides variable values - void connection_cb(bool connected) override - { - has_hr_imu = false; - has_scaled_imu = false; - has_att_quat = false; - } + std::string frame_id; + + rclcpp::Publisher::SharedPtr imu_pub; + rclcpp::Publisher::SharedPtr imu_raw_pub; + rclcpp::Publisher::SharedPtr magn_pub; + rclcpp::Publisher::SharedPtr temp_imu_pub; + rclcpp::Publisher::SharedPtr temp_baro_pub; + rclcpp::Publisher::SharedPtr static_press_pub; + rclcpp::Publisher::SharedPtr diff_press_pub; + + std::atomic has_hr_imu; + std::atomic has_raw_imu; + std::atomic has_scaled_imu; + std::atomic has_att_quat; + std::atomic received_linear_accel; + Eigen::Vector3d linear_accel_vec_flu; + Eigen::Vector3d linear_accel_vec_frd; + ftf::Covariance3d linear_acceleration_cov; + ftf::Covariance3d angular_velocity_cov; + ftf::Covariance3d orientation_cov; + ftf::Covariance3d unk_orientation_cov; + ftf::Covariance3d magnetic_cov; + + /* -*- helpers -*- */ + + /** + * @brief Setup 3x3 covariance matrix + * @param cov Covariance matrix + * @param stdev Standard deviation + * @remarks Diagonal computed from the stdev + */ + void setup_covariance(ftf::Covariance3d & cov, double stdev) + { + ftf::EigenMapCovariance3d c(cov.data()); + + c.setZero(); + if (stdev) { + double sr = stdev * stdev; + c.diagonal() << sr, sr, sr; + } else { + c(0, 0) = -1.0; + } + } + + /** + * @brief Fill and publish IMU data message. + * @param time_boot_ms Message timestamp (not syncronized) + * @param orientation_enu Orientation in the base_link ENU frame + * @param orientation_ned Orientation in the aircraft NED frame + * @param gyro_flu Angular velocity/rate in the base_link Forward-Left-Up frame + * @param gyro_frd Angular velocity/rate in the aircraft Forward-Right-Down frame + */ + void publish_imu_data( + uint32_t time_boot_ms, Eigen::Quaterniond & orientation_enu, + Eigen::Quaterniond & orientation_ned, Eigen::Vector3d & gyro_flu, Eigen::Vector3d & gyro_frd) + { + auto imu_ned_msg = sensor_msgs::msg::Imu(); + auto imu_enu_msg = sensor_msgs::msg::Imu(); + + // Fill message header + imu_enu_msg.header = uas->synchronized_header(frame_id, time_boot_ms); + imu_ned_msg.header = uas->synchronized_header("aircraft", time_boot_ms); + + // Convert from Eigen::Quaternond to geometry_msgs::Quaternion + imu_enu_msg.orientation = tf2::toMsg(orientation_enu); + imu_ned_msg.orientation = tf2::toMsg(orientation_ned); + + // Convert from Eigen::Vector3d to geometry_msgs::Vector3 + tf2::toMsg(gyro_flu, imu_enu_msg.angular_velocity); + tf2::toMsg(gyro_frd, imu_ned_msg.angular_velocity); + + // Eigen::Vector3d from HIGHRES_IMU or RAW_IMU, to geometry_msgs::Vector3 + tf2::toMsg(linear_accel_vec_flu, imu_enu_msg.linear_acceleration); + tf2::toMsg(linear_accel_vec_frd, imu_ned_msg.linear_acceleration); + + // Pass ENU msg covariances + imu_enu_msg.orientation_covariance = orientation_cov; + imu_enu_msg.angular_velocity_covariance = angular_velocity_cov; + imu_enu_msg.linear_acceleration_covariance = linear_acceleration_cov; + + // Pass NED msg covariances + imu_ned_msg.orientation_covariance = orientation_cov; + imu_ned_msg.angular_velocity_covariance = angular_velocity_cov; + imu_ned_msg.linear_acceleration_covariance = linear_acceleration_cov; + + if (!received_linear_accel) { + // Set element 0 of covariance matrix to -1 + // if no data received as per sensor_msgs/Imu defintion + imu_enu_msg.linear_acceleration_covariance[0] = -1; + imu_ned_msg.linear_acceleration_covariance[0] = -1; + } + + /** Store attitude in base_link ENU + * @snippet src/plugins/imu.cpp store_enu + */ + // [store_enu] + uas->data.update_attitude_imu_enu(imu_enu_msg); + // [store_enu] + + /** Store attitude in aircraft NED + * @snippet src/plugins/imu.cpp store_ned + */ + // [store_enu] + uas->data.update_attitude_imu_ned(imu_ned_msg); + // [store_ned] + + /** Publish only base_link ENU message + * @snippet src/plugins/imu.cpp pub_enu + */ + // [pub_enu] + imu_pub->publish(imu_enu_msg); + // [pub_enu] + } + + /** + * @brief Fill and publish IMU data_raw message; store linear acceleration for IMU data + * @param header Message frame_id and timestamp + * @param gyro_flu Orientation in the base_link Forward-Left-Up frame + * @param accel_flu Linear acceleration in the base_link Forward-Left-Up frame + * @param accel_frd Linear acceleration in the aircraft Forward-Right-Down frame + */ + void publish_imu_data_raw( + const std_msgs::msg::Header & header, const Eigen::Vector3d & gyro_flu, + const Eigen::Vector3d & accel_flu, const Eigen::Vector3d & accel_frd) + { + auto imu_msg = sensor_msgs::msg::Imu(); + + // Fill message header + imu_msg.header = header; + + tf2::toMsg(gyro_flu, imu_msg.angular_velocity); + tf2::toMsg(accel_flu, imu_msg.linear_acceleration); + + // Save readings + linear_accel_vec_flu = accel_flu; + linear_accel_vec_frd = accel_frd; + received_linear_accel = true; + + imu_msg.orientation_covariance = unk_orientation_cov; + imu_msg.angular_velocity_covariance = angular_velocity_cov; + imu_msg.linear_acceleration_covariance = linear_acceleration_cov; + + // Publish message [ENU frame] + imu_raw_pub->publish(imu_msg); + } + + /** + * @brief Publish magnetic field data + * @param header Message frame_id and timestamp + * @param mag_field Magnetic field in the base_link ENU frame + */ + void publish_mag(const std_msgs::msg::Header & header, const Eigen::Vector3d & mag_field) + { + auto magn_msg = sensor_msgs::msg::MagneticField(); + + // Fill message header + magn_msg.header = header; + + tf2::toMsg(mag_field, magn_msg.magnetic_field); + magn_msg.magnetic_field_covariance = magnetic_cov; + + // Publish message [ENU frame] + magn_pub->publish(magn_msg); + } + + /* -*- message handlers -*- */ + + /** + * @brief Handle ATTITUDE MAVlink message. + * Message specification: https://mavlink.io/en/messages/common.html#ATTITUDE + * @param msg Received Mavlink msg + * @param att ATTITUDE msg + */ + void handle_attitude( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::ATTITUDE & att, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + if (has_att_quat) { + return; + } + + /** Orientation on the NED-aicraft frame: + * @snippet src/plugins/imu.cpp ned_aircraft_orient1 + */ + // [ned_aircraft_orient1] + auto ned_aircraft_orientation = ftf::quaternion_from_rpy(att.roll, att.pitch, att.yaw); + // [ned_aircraft_orient1] + + /** Angular velocity on the NED-aicraft frame: + * @snippet src/plugins/imu.cpp ned_ang_vel1 + */ + // [frd_ang_vel1] + auto gyro_frd = Eigen::Vector3d(att.rollspeed, att.pitchspeed, att.yawspeed); + // [frd_ang_vel1] + + /** The RPY describes the rotation: aircraft->NED. + * It is required to change this to aircraft->base_link: + * @snippet src/plugins/imu.cpp ned->baselink->enu + */ + // [ned->baselink->enu] + auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink( + ftf::transform_orientation_ned_enu(ned_aircraft_orientation)); + // [ned->baselink->enu] + + /** The angular velocity expressed in the aircraft frame. + * It is required to apply the static rotation to get it into the base_link frame: + * @snippet src/plugins/imu.cpp rotate_gyro + */ + // [rotate_gyro] + auto gyro_flu = ftf::transform_frame_aircraft_baselink(gyro_frd); + // [rotate_gyro] + + publish_imu_data( + att.time_boot_ms, enu_baselink_orientation, ned_aircraft_orientation, gyro_flu, + gyro_frd); + } + + /** + * @brief Handle ATTITUDE_QUATERNION MAVlink message. + * Message specification: https://mavlink.io/en/messages/common.html/#ATTITUDE_QUATERNION + * @param msg Received Mavlink msg + * @param att_q ATTITUDE_QUATERNION msg + */ + void handle_attitude_quaternion( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::ATTITUDE_QUATERNION & att_q, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + RCLCPP_INFO_EXPRESSION( + get_logger(), !has_att_quat.exchange( + true), "IMU: Attitude quaternion IMU detected!"); + + /** Orientation on the NED-aicraft frame: + * @snippet src/plugins/imu.cpp ned_aircraft_orient2 + */ + // [ned_aircraft_orient2] + auto ned_aircraft_orientation = Eigen::Quaterniond(att_q.q1, att_q.q2, att_q.q3, att_q.q4); + // [ned_aircraft_orient2] + + /** Angular velocity on the NED-aicraft frame: + * @snippet src/plugins/imu.cpp ned_ang_vel2 + */ + // [frd_ang_vel2] + auto gyro_frd = Eigen::Vector3d(att_q.rollspeed, att_q.pitchspeed, att_q.yawspeed); + // [frd_ang_vel2] + + /** MAVLink quaternion exactly matches Eigen convention. + * The RPY describes the rotation: aircraft->NED. + * It is required to change this to aircraft->base_link: + * @snippet src/plugins/imu.cpp ned->baselink->enu + */ + auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink( + ftf::transform_orientation_ned_enu(ned_aircraft_orientation)); + + /** The angular velocity expressed in the aircraft frame. + * It is required to apply the static rotation to get it into the base_link frame: + * @snippet src/plugins/imu.cpp rotate_gyro + */ + auto gyro_flu = ftf::transform_frame_aircraft_baselink(gyro_frd); + + publish_imu_data( + att_q.time_boot_ms, enu_baselink_orientation, ned_aircraft_orientation, + gyro_flu, gyro_frd); + } + + /** + * @brief Handle HIGHRES_IMU MAVlink message. + * Message specification: https://mavlink.io/en/messages/common.html/#HIGHRES_IMU + * @param msg Received Mavlink msg + * @param imu_hr HIGHRES_IMU msg + */ + void handle_highres_imu( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::HIGHRES_IMU & imu_hr, plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + RCLCPP_INFO_EXPRESSION( + get_logger(), !has_hr_imu.exchange( + true), "IMU: High resolution IMU detected!"); + + auto header = uas->synchronized_header(frame_id, imu_hr.time_usec); + /** @todo Make more paranoic check of HIGHRES_IMU.fields_updated + */ + + /** Check if accelerometer + gyroscope data are available. + * Data is expressed in aircraft frame it is required to rotate to the base_link frame: + * @snippet src/plugins/imu.cpp accel_available + */ + // [accel_available] + if (imu_hr.fields_updated & ((7 << 3) | (7 << 0))) { + auto gyro_flu = + ftf::transform_frame_aircraft_baselink( + Eigen::Vector3d( + imu_hr.xgyro, imu_hr.ygyro, + imu_hr.zgyro)); + + auto accel_frd = Eigen::Vector3d(imu_hr.xacc, imu_hr.yacc, imu_hr.zacc); + auto accel_flu = ftf::transform_frame_aircraft_baselink(accel_frd); + + publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd); + } + // [accel_available] + + /** Check if magnetometer data is available: + * @snippet src/plugins/imu.cpp mag_available + */ + // [mag_available] + if (imu_hr.fields_updated & (7 << 6)) { + auto mag_field = ftf::transform_frame_aircraft_baselink( + Eigen::Vector3d(imu_hr.xmag, imu_hr.ymag, imu_hr.zmag) * GAUSS_TO_TESLA); + + publish_mag(header, mag_field); + } + // [mag_available] + + /** Check if static pressure sensor data is available: + * @snippet src/plugins/imu.cpp static_pressure_available + */ + // [static_pressure_available] + if (imu_hr.fields_updated & (1 << 9)) { + auto static_pressure_msg = sensor_msgs::msg::FluidPressure(); + + static_pressure_msg.header = header; + static_pressure_msg.fluid_pressure = imu_hr.abs_pressure; + + static_press_pub->publish(static_pressure_msg); + } + // [static_pressure_available] + + /** Check if differential pressure sensor data is available: + * @snippet src/plugins/imu.cpp differential_pressure_available + */ + // [differential_pressure_available] + if (imu_hr.fields_updated & (1 << 10)) { + auto differential_pressure_msg = sensor_msgs::msg::FluidPressure(); + + differential_pressure_msg.header = header; + differential_pressure_msg.fluid_pressure = imu_hr.diff_pressure; + + diff_press_pub->publish(differential_pressure_msg); + } + // [differential_pressure_available] + + /** Check if temperature data is available: + * @snippet src/plugins/imu.cpp temperature_available + */ + // [temperature_available] + if (imu_hr.fields_updated & (1 << 12)) { + auto temp_msg = sensor_msgs::msg::Temperature(); + + temp_msg.header = header; + temp_msg.temperature = imu_hr.temperature; + + temp_imu_pub->publish(temp_msg); + } + // [temperature_available] + } + + /** + * @brief Handle RAW_IMU MAVlink message. + * Message specification: https://mavlink.io/en/messages/common.html/#RAW_IMU + * @param msg Received Mavlink msg + * @param imu_raw RAW_IMU msg + */ + void handle_raw_imu( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::RAW_IMU & imu_raw, plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + RCLCPP_INFO_EXPRESSION(get_logger(), !has_raw_imu.exchange(true), "IMU: Raw IMU message used."); + + if (has_hr_imu || has_scaled_imu) { + return; + } + + auto imu_msg = sensor_msgs::msg::Imu(); + auto header = uas->synchronized_header(frame_id, imu_raw.time_usec); + + /** @note APM send SCALED_IMU data as RAW_IMU + */ + auto gyro_flu = ftf::transform_frame_aircraft_baselink( + Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC); + auto accel_frd = Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc); + auto accel_flu = ftf::transform_frame_aircraft_baselink(accel_frd); + + if (uas->is_ardupilotmega()) { + accel_frd *= MILLIG_TO_MS2; + accel_flu *= MILLIG_TO_MS2; + } else if (uas->is_px4()) { + accel_frd *= MILLIMS2_TO_MS2; + accel_flu *= MILLIMS2_TO_MS2; + } + + publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd); + + if (!uas->is_ardupilotmega()) { + RCLCPP_WARN_THROTTLE( + get_logger(), + *get_clock(), 60000, "IMU: linear acceleration on RAW_IMU known on APM only."); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 60000, + "IMU: ~imu/data_raw stores unscaled raw acceleration report."); + linear_accel_vec_flu.setZero(); + linear_accel_vec_frd.setZero(); + } + + /** Magnetic field data: + * @snippet src/plugins/imu.cpp mag_field + */ + // [mag_field] + auto mag_field = ftf::transform_frame_aircraft_baselink( + Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA); + // [mag_field] + + publish_mag(header, mag_field); + } + + /** + * @brief Handle SCALED_IMU MAVlink message. + * Message specification: https://mavlink.io/en/messages/common.html/#SCALED_IMU + * @param msg Received Mavlink msg + * @param imu_raw SCALED_IMU msg + */ + void handle_scaled_imu( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::SCALED_IMU & imu_raw, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + if (has_hr_imu) { + return; + } + + RCLCPP_INFO_EXPRESSION( + get_logger(), !has_scaled_imu.exchange( + true), "IMU: Scaled IMU message used."); + + auto header = uas->synchronized_header(frame_id, imu_raw.time_boot_ms); + + auto gyro_flu = ftf::transform_frame_aircraft_baselink( + Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC); + auto accel_frd = Eigen::Vector3d( + Eigen::Vector3d( + imu_raw.xacc, imu_raw.yacc, + imu_raw.zacc) * MILLIG_TO_MS2); + auto accel_flu = ftf::transform_frame_aircraft_baselink(accel_frd); + + publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd); + + /** Magnetic field data: + * @snippet src/plugins/imu.cpp mag_field + */ + auto mag_field = ftf::transform_frame_aircraft_baselink( + Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA); + + publish_mag(header, mag_field); + } + + /** + * @brief Handle SCALED_PRESSURE MAVlink message. + * Message specification: https://mavlink.io/en/messages/common.html/#SCALED_PRESSURE + * @param msg Received Mavlink msg + * @param press SCALED_PRESSURE msg + */ + void handle_scaled_pressure( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::SCALED_PRESSURE & press, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + if (has_hr_imu) { + return; + } + + auto header = uas->synchronized_header(frame_id, press.time_boot_ms); + + auto temp_msg = sensor_msgs::msg::Temperature(); + temp_msg.header = header; + temp_msg.temperature = press.temperature / 100.0; + temp_baro_pub->publish(temp_msg); + + auto static_pressure_msg = sensor_msgs::msg::FluidPressure(); + static_pressure_msg.header = header; + static_pressure_msg.fluid_pressure = press.press_abs * 100.0; + static_press_pub->publish(static_pressure_msg); + + auto differential_pressure_msg = sensor_msgs::msg::FluidPressure(); + differential_pressure_msg.header = header; + differential_pressure_msg.fluid_pressure = press.press_diff * 100.0; + diff_press_pub->publish(differential_pressure_msg); + } + + // Checks for connection and overrides variable values + void connection_cb([[maybe_unused]] bool connected) override + { + has_hr_imu = false; + has_raw_imu = false; + has_scaled_imu = false; + has_att_quat = false; + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::IMUPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::IMUPlugin) diff --git a/mavros/src/plugins/local_position.cpp b/mavros/src/plugins/local_position.cpp index e26cfed2d..03d58fa5e 100644 --- a/mavros/src/plugins/local_position.cpp +++ b/mavros/src/plugins/local_position.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2014,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief LocalPosition plugin * @file local_position.cpp @@ -8,243 +15,285 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include +#include -#include -#include -#include -#include -#include -#include +#include -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace std_plugins { /** * @brief Local position plugin. + * @plugin local_position + * * Publish local position to TF, PositionStamped, TwistStamped * and Odometry */ -class LocalPositionPlugin : public plugin::PluginBase { +class LocalPositionPlugin : public plugin::Plugin +{ public: - LocalPositionPlugin() : PluginBase(), - lp_nh("~local_position"), - tf_send(false), - has_local_position_ned(false), - has_local_position_ned_cov(false) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - // header frame_id. - // default to map (world-fixed,ENU as per REP-105). - lp_nh.param("frame_id", frame_id, "map"); - // Important tf subsection - // Report the transform from world to base_link here. - lp_nh.param("tf/send", tf_send, false); - lp_nh.param("tf/frame_id", tf_frame_id, "map"); - lp_nh.param("tf/child_frame_id", tf_child_frame_id, "base_link"); - - local_position = lp_nh.advertise("pose", 10); - local_position_cov = lp_nh.advertise("pose_cov", 10); - local_velocity_local = lp_nh.advertise("velocity_local", 10); - local_velocity_body = lp_nh.advertise("velocity_body", 10); - local_velocity_cov = lp_nh.advertise("velocity_body_cov", 10); - local_accel = lp_nh.advertise("accel", 10); - local_odom = lp_nh.advertise("odom",10); - } - - Subscriptions get_subscriptions() override { - return { - make_handler(&LocalPositionPlugin::handle_local_position_ned), - make_handler(&LocalPositionPlugin::handle_local_position_ned_cov) - }; - } + explicit LocalPositionPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "local_position"), + tf_send(false), + has_local_position_ned(false), + has_local_position_ned_cov(false) + { + enable_node_watch_parameters(); + + // header frame_id. + // default to map (world-fixed, ENU as per REP-105). + node_declare_and_watch_parameter( + "frame_id", "map", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + + // Important tf subsection + // Report the transform from world to base_link here. + node_declare_and_watch_parameter( + "tf.send", false, [&](const rclcpp::Parameter & p) { + tf_send = p.as_bool(); + }); + node_declare_and_watch_parameter( + "tf.frame_id", "map", [&](const rclcpp::Parameter & p) { + tf_frame_id = p.as_string(); + }); + node_declare_and_watch_parameter( + "tf.child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { + tf_child_frame_id = p.as_string(); + }); + + auto sensor_qos = rclcpp::SensorDataQoS(); + + local_position = node->create_publisher("~/pose", sensor_qos); + local_position_cov = node->create_publisher( + "~/pose_cov", sensor_qos); + local_velocity_local = node->create_publisher( + "~/velocity_local", sensor_qos); + local_velocity_body = node->create_publisher( + "~/velocity_body", + sensor_qos); + local_velocity_cov = node->create_publisher( + "~/velocity_body_cov", sensor_qos); + local_accel = node->create_publisher( + "~/accel", + sensor_qos); + local_odom = node->create_publisher("~/odom", sensor_qos); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&LocalPositionPlugin::handle_local_position_ned), + make_handler(&LocalPositionPlugin::handle_local_position_ned_cov) + }; + } private: - ros::NodeHandle lp_nh; - - ros::Publisher local_position; - ros::Publisher local_position_cov; - ros::Publisher local_velocity_local; - ros::Publisher local_velocity_body; - ros::Publisher local_velocity_cov; - ros::Publisher local_accel; - ros::Publisher local_odom; - - std::string frame_id; //!< frame for Pose - std::string tf_frame_id; //!< origin for TF - std::string tf_child_frame_id; //!< frame for TF - bool tf_send; - bool has_local_position_ned; - bool has_local_position_ned_cov; - - void publish_tf(boost::shared_ptr &odom) - { - if (tf_send) { - geometry_msgs::TransformStamped transform; - transform.header.stamp = odom->header.stamp; - transform.header.frame_id = tf_frame_id; - transform.child_frame_id = tf_child_frame_id; - transform.transform.translation.x = odom->pose.pose.position.x; - transform.transform.translation.y = odom->pose.pose.position.y; - transform.transform.translation.z = odom->pose.pose.position.z; - transform.transform.rotation = odom->pose.pose.orientation; - m_uas->tf2_broadcaster.sendTransform(transform); - } - } - - void handle_local_position_ned(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LOCAL_POSITION_NED &pos_ned) - { - has_local_position_ned = true; - - //--------------- Transform FCU position and Velocity Data ---------------// - auto enu_position = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z)); - auto enu_velocity = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.vx, pos_ned.vy, pos_ned.vz)); - - //--------------- Get Odom Information ---------------// - // Note this orientation describes baselink->ENU transform - auto enu_orientation_msg = m_uas->get_attitude_orientation_enu(); - auto baselink_angular_msg = m_uas->get_attitude_angular_velocity_enu(); - Eigen::Quaterniond enu_orientation; - tf::quaternionMsgToEigen(enu_orientation_msg, enu_orientation); - auto baselink_linear = ftf::transform_frame_enu_baselink(enu_velocity, enu_orientation.inverse()); - - auto odom = boost::make_shared(); - odom->header = m_uas->synchronized_header(frame_id, pos_ned.time_boot_ms); - odom->child_frame_id = tf_child_frame_id; - - tf::pointEigenToMsg(enu_position, odom->pose.pose.position); - odom->pose.pose.orientation = enu_orientation_msg; - tf::vectorEigenToMsg(baselink_linear, odom->twist.twist.linear); - odom->twist.twist.angular = baselink_angular_msg; - - // publish odom if we don't have LOCAL_POSITION_NED_COV - if (!has_local_position_ned_cov) { - local_odom.publish(odom); - } - - // publish pose always - auto pose = boost::make_shared(); - pose->header = odom->header; - pose->pose = odom->pose.pose; - local_position.publish(pose); - - // publish velocity always - // velocity in the body frame - auto twist_body = boost::make_shared(); - twist_body->header.stamp = odom->header.stamp; - twist_body->header.frame_id = tf_child_frame_id; - twist_body->twist.linear = odom->twist.twist.linear; - twist_body->twist.angular = baselink_angular_msg; - local_velocity_body.publish(twist_body); - - // velocity in the local frame - auto twist_local = boost::make_shared(); - twist_local->header.stamp = twist_body->header.stamp; - twist_local->header.frame_id = tf_child_frame_id; - tf::vectorEigenToMsg(enu_velocity, twist_local->twist.linear); - tf::vectorEigenToMsg(ftf::transform_frame_baselink_enu(ftf::to_eigen(baselink_angular_msg), enu_orientation), - twist_local->twist.angular); - - local_velocity_local.publish(twist_local); - - // publish tf - publish_tf(odom); - } - - void handle_local_position_ned_cov(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LOCAL_POSITION_NED_COV &pos_ned) - { - has_local_position_ned_cov = true; - - auto enu_position = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z)); - auto enu_velocity = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.vx, pos_ned.vy, pos_ned.vz)); - - auto enu_orientation_msg = m_uas->get_attitude_orientation_enu(); - auto baselink_angular_msg = m_uas->get_attitude_angular_velocity_enu(); - Eigen::Quaterniond enu_orientation; - tf::quaternionMsgToEigen(enu_orientation_msg, enu_orientation); - auto baselink_linear = ftf::transform_frame_enu_baselink(enu_velocity, enu_orientation.inverse()); - - auto odom = boost::make_shared(); - odom->header = m_uas->synchronized_header(frame_id, pos_ned.time_usec); - odom->child_frame_id = tf_child_frame_id; - - tf::pointEigenToMsg(enu_position, odom->pose.pose.position); - odom->pose.pose.orientation = enu_orientation_msg; - tf::vectorEigenToMsg(baselink_linear, odom->twist.twist.linear); - odom->twist.twist.angular = baselink_angular_msg; - - odom->pose.covariance[0] = pos_ned.covariance[0]; // x - odom->pose.covariance[7] = pos_ned.covariance[9]; // y - odom->pose.covariance[14] = pos_ned.covariance[17]; // z - - odom->twist.covariance[0] = pos_ned.covariance[24]; // vx - odom->twist.covariance[7] = pos_ned.covariance[30]; // vy - odom->twist.covariance[14] = pos_ned.covariance[35]; // vz - // TODO: orientation + angular velocity covariances from ATTITUDE_QUATERION_COV - - // publish odom always - local_odom.publish(odom); - - // publish pose_cov always - auto pose_cov = boost::make_shared(); - pose_cov->header = odom->header; - pose_cov->pose = odom->pose; - local_position_cov.publish(pose_cov); - - // publish velocity_cov always - auto twist_cov = boost::make_shared(); - twist_cov->header.stamp = odom->header.stamp; - twist_cov->header.frame_id = odom->child_frame_id; - twist_cov->twist = odom->twist; - local_velocity_cov.publish(twist_cov); - - // publish pose, velocity, tf if we don't have LOCAL_POSITION_NED - if (!has_local_position_ned) { - auto pose = boost::make_shared(); - pose->header = odom->header; - pose->pose = odom->pose.pose; - local_position.publish(pose); - - auto twist = boost::make_shared(); - twist->header.stamp = odom->header.stamp; - twist->header.frame_id = odom->child_frame_id; - twist->twist = odom->twist.twist; - local_velocity_body.publish(twist); - - // publish tf - publish_tf(odom); - } - - // publish accelerations - auto accel = boost::make_shared(); - accel->header = odom->header; - - auto enu_accel = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.ax, pos_ned.ay, pos_ned.az)); - tf::vectorEigenToMsg(enu_accel, accel->accel.accel.linear); - - accel->accel.covariance[0] = pos_ned.covariance[39]; // ax - accel->accel.covariance[7] = pos_ned.covariance[42]; // ay - accel->accel.covariance[14] = pos_ned.covariance[44]; // az - - local_accel.publish(accel); - } + rclcpp::Publisher::SharedPtr local_position; + rclcpp::Publisher::SharedPtr local_position_cov; + rclcpp::Publisher::SharedPtr local_velocity_local; + rclcpp::Publisher::SharedPtr local_velocity_body; + rclcpp::Publisher::SharedPtr local_velocity_cov; + rclcpp::Publisher::SharedPtr local_accel; + rclcpp::Publisher::SharedPtr local_odom; + + std::string frame_id; //!< frame for Pose + std::string tf_frame_id; //!< origin for TF + std::string tf_child_frame_id; //!< frame for TF + std::atomic tf_send; + std::atomic has_local_position_ned; + std::atomic has_local_position_ned_cov; + + void publish_tf(nav_msgs::msg::Odometry & odom) + { + if (tf_send) { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = odom.header.stamp; + transform.header.frame_id = tf_frame_id; + transform.child_frame_id = tf_child_frame_id; + transform.transform.translation.x = odom.pose.pose.position.x; + transform.transform.translation.y = odom.pose.pose.position.y; + transform.transform.translation.z = odom.pose.pose.position.z; + transform.transform.rotation = odom.pose.pose.orientation; + uas->tf2_broadcaster.sendTransform(transform); + } + } + + void handle_local_position_ned( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::LOCAL_POSITION_NED & pos_ned, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + has_local_position_ned = true; + + //--------------- Transform FCU position and Velocity Data ---------------// + auto enu_position = ftf::transform_frame_ned_enu( + Eigen::Vector3d( + pos_ned.x, pos_ned.y, + pos_ned.z)); + auto enu_velocity = + ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.vx, pos_ned.vy, pos_ned.vz)); + + //--------------- Get Odom Information ---------------// + // Note this orientation describes baselink->ENU transform + auto enu_orientation_msg = uas->data.get_attitude_orientation_enu(); + auto baselink_angular_msg = uas->data.get_attitude_angular_velocity_enu(); + Eigen::Quaterniond enu_orientation; tf2::fromMsg(enu_orientation_msg, enu_orientation); + auto baselink_linear = + ftf::transform_frame_enu_baselink(enu_velocity, enu_orientation.inverse()); + + auto odom = nav_msgs::msg::Odometry(); + odom.header = uas->synchronized_header(frame_id, pos_ned.time_boot_ms); + odom.child_frame_id = tf_child_frame_id; + + odom.pose.pose.position = tf2::toMsg(enu_position); + odom.pose.pose.orientation = enu_orientation_msg; + tf2::toMsg(baselink_linear, odom.twist.twist.linear); + odom.twist.twist.angular = baselink_angular_msg; + + // publish odom if we don't have LOCAL_POSITION_NED_COV + if (!has_local_position_ned_cov) { + local_odom->publish(odom); + } + + // publish pose always + auto pose = geometry_msgs::msg::PoseStamped(); + pose.header = odom.header; + pose.pose = odom.pose.pose; + local_position->publish(pose); + + // publish velocity always + // velocity in the body frame + auto twist_body = geometry_msgs::msg::TwistStamped(); + twist_body.header.stamp = odom.header.stamp; + twist_body.header.frame_id = tf_child_frame_id; + twist_body.twist.linear = odom.twist.twist.linear; + twist_body.twist.angular = baselink_angular_msg; + local_velocity_body->publish(twist_body); + + // velocity in the local frame + auto twist_local = geometry_msgs::msg::TwistStamped(); + twist_local.header.stamp = twist_body.header.stamp; + twist_local.header.frame_id = tf_child_frame_id; + tf2::toMsg(enu_velocity, twist_local.twist.linear); + tf2::toMsg( + ftf::transform_frame_baselink_enu(ftf::to_eigen(baselink_angular_msg), enu_orientation), + twist_local.twist.angular); + + local_velocity_local->publish(twist_local); + + // publish tf + publish_tf(odom); + } + + void handle_local_position_ned_cov( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::LOCAL_POSITION_NED_COV & pos_ned, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + has_local_position_ned_cov = true; + + auto enu_position = ftf::transform_frame_ned_enu( + Eigen::Vector3d( + pos_ned.x, pos_ned.y, + pos_ned.z)); + auto enu_velocity = + ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.vx, pos_ned.vy, pos_ned.vz)); + + auto enu_orientation_msg = uas->data.get_attitude_orientation_enu(); + auto baselink_angular_msg = uas->data.get_attitude_angular_velocity_enu(); + Eigen::Quaterniond enu_orientation; tf2::fromMsg(enu_orientation_msg, enu_orientation); + auto baselink_linear = + ftf::transform_frame_enu_baselink(enu_velocity, enu_orientation.inverse()); + + auto odom = nav_msgs::msg::Odometry(); + odom.header = uas->synchronized_header(frame_id, pos_ned.time_usec); + odom.child_frame_id = tf_child_frame_id; + + odom.pose.pose.position = tf2::toMsg(enu_position); + odom.pose.pose.orientation = enu_orientation_msg; + tf2::toMsg(baselink_linear, odom.twist.twist.linear); + odom.twist.twist.angular = baselink_angular_msg; + + odom.pose.covariance[0] = pos_ned.covariance[0]; // x + odom.pose.covariance[7] = pos_ned.covariance[9]; // y + odom.pose.covariance[14] = pos_ned.covariance[17]; // z + + odom.twist.covariance[0] = pos_ned.covariance[24]; // vx + odom.twist.covariance[7] = pos_ned.covariance[30]; // vy + odom.twist.covariance[14] = pos_ned.covariance[35]; // vz + // TODO(vooon): orientation + angular velocity covariances from ATTITUDE_QUATERION_COV + + // publish odom always + local_odom->publish(odom); + + // publish pose_cov always + auto pose_cov = geometry_msgs::msg::PoseWithCovarianceStamped(); + pose_cov.header = odom.header; + pose_cov.pose = odom.pose; + local_position_cov->publish(pose_cov); + + // publish velocity_cov always + auto twist_cov = geometry_msgs::msg::TwistWithCovarianceStamped(); + twist_cov.header.stamp = odom.header.stamp; + twist_cov.header.frame_id = odom.child_frame_id; + twist_cov.twist = odom.twist; + local_velocity_cov->publish(twist_cov); + + // publish pose, velocity, tf if we don't have LOCAL_POSITION_NED + if (!has_local_position_ned) { + auto pose = geometry_msgs::msg::PoseStamped(); + pose.header = odom.header; + pose.pose = odom.pose.pose; + local_position->publish(pose); + + auto twist = geometry_msgs::msg::TwistStamped(); + twist.header.stamp = odom.header.stamp; + twist.header.frame_id = odom.child_frame_id; + twist.twist = odom.twist.twist; + local_velocity_body->publish(twist); + + // publish tf + publish_tf(odom); + } + + // publish accelerations + auto accel = geometry_msgs::msg::AccelWithCovarianceStamped(); + accel.header = odom.header; + + auto enu_accel = ftf::transform_frame_ned_enu( + Eigen::Vector3d( + pos_ned.ax, pos_ned.ay, + pos_ned.az)); + tf2::toMsg(enu_accel, accel.accel.accel.linear); + + accel.accel.covariance[0] = pos_ned.covariance[39]; // ax + accel.accel.covariance[7] = pos_ned.covariance[42]; // ay + accel.accel.covariance[14] = pos_ned.covariance[44]; // az + + local_accel->publish(accel); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::LocalPositionPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::LocalPositionPlugin) diff --git a/mavros/src/plugins/manual_control.cpp b/mavros/src/plugins/manual_control.cpp index 60f0e1da5..8769faff9 100644 --- a/mavros/src/plugins/manual_control.cpp +++ b/mavros/src/plugins/manual_control.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2015 Matias Nitsche. + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief ManualControls plugin * @file manual_controls.cpp @@ -6,83 +14,85 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2015 Matias Nitsche. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include +#include +#include +#include -#include +#include + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace std_plugins { /** * @brief Manual Control plugin + * @plugin manual_control */ -class ManualControlPlugin : public plugin::PluginBase { +class ManualControlPlugin : public plugin::Plugin +{ public: - ManualControlPlugin() : PluginBase(), - manual_control_nh("~manual_control") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - control_pub = manual_control_nh.advertise("control", 10); - send_sub = manual_control_nh.subscribe("send", 1, &ManualControlPlugin::send_cb, this); - } - - Subscriptions get_subscriptions() override { - return { - make_handler(&ManualControlPlugin::handle_manual_control), - }; - } + explicit ManualControlPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "manual_control") + { + control_pub = node->create_publisher("~/control", 10); + send_sub = + node->create_subscription( + "~/send", 10, + std::bind(&ManualControlPlugin::send_cb, this, _1)); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&ManualControlPlugin::handle_manual_control), + }; + } private: - ros::NodeHandle manual_control_nh; - - ros::Publisher control_pub; - ros::Subscriber send_sub; - - /* -*- rx handlers -*- */ - - void handle_manual_control(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MANUAL_CONTROL &manual_control) - { - auto manual_control_msg = boost::make_shared(); - - manual_control_msg->header.stamp = ros::Time::now(); - manual_control_msg->x = (manual_control.x / 1000.0); - manual_control_msg->y = (manual_control.y / 1000.0); - manual_control_msg->z = (manual_control.z / 1000.0); - manual_control_msg->r = (manual_control.r / 1000.0); - manual_control_msg->buttons = manual_control.buttons; - - control_pub.publish(manual_control_msg); - } - - /* -*- callbacks -*- */ - - void send_cb(const mavros_msgs::ManualControl::ConstPtr req) - { - mavlink::common::msg::MANUAL_CONTROL msg = {}; - msg.target = m_uas->get_tgt_system(); - - msg.x = req->x; - msg.y = req->y; - msg.z = req->z; - msg.r = req->r; - msg.buttons = req->buttons; - - UAS_FCU(m_uas)->send_message_ignore_drop(msg); - } + rclcpp::Publisher::SharedPtr control_pub; + rclcpp::Subscription::SharedPtr send_sub; + + /* -*- rx handlers -*- */ + + void handle_manual_control( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::MANUAL_CONTROL & manual_control, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto manual_control_msg = mavros_msgs::msg::ManualControl(); + + manual_control_msg.header.stamp = node->now(); + manual_control_msg.x = (manual_control.x / 1000.0); + manual_control_msg.y = (manual_control.y / 1000.0); + manual_control_msg.z = (manual_control.z / 1000.0); + manual_control_msg.r = (manual_control.r / 1000.0); + manual_control_msg.buttons = manual_control.buttons; + + control_pub->publish(manual_control_msg); + } + + /* -*- callbacks -*- */ + + void send_cb(const mavros_msgs::msg::ManualControl::SharedPtr req) + { + mavlink::common::msg::MANUAL_CONTROL msg = {}; + msg.target = uas->get_tgt_system(); + msg.x = req->x; + msg.y = req->y; + msg.z = req->z; + msg.r = req->r; + msg.buttons = req->buttons; + + uas->send_message(msg); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::ManualControlPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::ManualControlPlugin) diff --git a/mavros/src/plugins/mission_protocol_base.cpp b/mavros/src/plugins/mission_protocol_base.cpp index b9fdcf7b3..e83c62477 100644 --- a/mavros/src/plugins/mission_protocol_base.cpp +++ b/mavros/src/plugins/mission_protocol_base.cpp @@ -1,357 +1,562 @@ -/** - * @brief Mission base plugin - * @file mission_protocol_base.cpp - * @author Vladimir Ermakov - * @author Charlie Burge - * - */ /* - * Copyright 2014,2015,2016,2017,2018 Vladimir Ermakov. + * Copyright 2014,2015,2016,2017,2018,2021 Vladimir Ermakov. * Copyright 2021 Charlie Burge. * * This file is part of the mavros package and subject to the license terms * in the top-level LICENSE file of the mavros repository. * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ +/** + * @brief Mission base plugin + * @file mission_protocol_base.cpp + * @author Vladimir Ermakov + * @author Charlie Burge + */ + +#include "mavros/mission_protocol_base.hpp" -#include +using namespace mavros; // NOLINT +using namespace mavros::plugin; // NOLINT -namespace mavros { -namespace plugin { -void MissionBase::handle_mission_item_int(const mavlink::mavlink_message_t *msg, WP_ITEM_INT &wpi) +std::ostream & mavros::plugin::operator<<(std::ostream & os, const MissionItem & mi) { - unique_lock lock(mutex); - - /* Only interested in the specific msg type */ - if (wpi.mission_type != enum_value(wp_type)) { - return; - } - /* receive item only in RX state */ - else if (wp_state == WP::RXWPINT) { - if (wpi.seq != wp_cur_id) { - ROS_WARN_NAMED(log_ns, "%s: Seq mismatch, dropping item (%d != %zu)", - log_ns.c_str(), wpi.seq, wp_cur_id); - return; - } - - ROS_INFO_STREAM_NAMED(log_ns, log_ns << ": item " << waypoint_to_string(wpi)); - - waypoints.push_back(mav_to_msg(wpi)); - if (++wp_cur_id < wp_count) { - restart_timeout_timer(); - mission_request_int(wp_cur_id); - } - else { - request_mission_done(); - mission_item_int_support_confirmed = true; - lock.unlock(); - publish_waypoints(); - } - } - else { - ROS_DEBUG_NAMED(log_ns, "%s: rejecting item, wrong state %d", log_ns.c_str(), enum_value(wp_state)); - if (do_pull_after_gcs && reschedule_pull) { - ROS_DEBUG_NAMED(log_ns, "%s: reschedule pull", log_ns.c_str()); - schedule_pull(WP_TIMEOUT_DT); - } - } + os << '#' << mi.seq << (mi.is_current ? '*' : ' ') << " F:" << +mi.frame << " C:" << + std::setw(3) << mi.command; + os << std::setprecision(7) << " p: " << mi.param1 << ' ' << mi.param2 << ' ' << mi.param3 << + ' ' << mi.param4; + os << std::setprecision(7) << " x: " << mi.x_lat << " y: " << mi.y_long << " z: " << mi.z_alt; + return os; } +void MissionBase::handle_mission_item( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + MISSION_ITEM & wpi, + MFilter filter [[maybe_unused]]) +{ + unique_lock lock(mutex); + + if (filter_message(wpi)) { + return; + } + + // receive item only in RX state + if (wp_state == WP::RXWP) { + if (sequence_mismatch(wpi)) { + return; + } + + auto it = waypoints.emplace(waypoints.end(), wpi); + RCLCPP_INFO_STREAM(get_logger(), log_prefix << ": item " << *it); + + if (++wp_cur_id < wp_count) { + restart_timeout_timer(); + mission_request(wp_cur_id); + } else { + request_mission_done(); + lock.unlock(); + publish_waypoints(); + } + } else { + RCLCPP_DEBUG( + get_logger(), "%s: rejecting item, wrong state %d", log_prefix, enum_value( + wp_state)); + if (do_pull_after_gcs && reschedule_pull) { + RCLCPP_DEBUG(get_logger(), "%s: reschedule pull", log_prefix); + schedule_pull(WP_TIMEOUT); + } + } +} -void MissionBase::handle_mission_item(const mavlink::mavlink_message_t *msg, WP_ITEM &wpi) +void MissionBase::handle_mission_item_int( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + MISSION_ITEM_INT & wpi, + MFilter filter [[maybe_unused]]) { - unique_lock lock(mutex); - - /* Only interested in the specific msg type */ - if (wpi.mission_type != enum_value(wp_type)) { - return; - } - /* receive item only in RX state */ - else if (wp_state == WP::RXWP) { - if (wpi.seq != wp_cur_id) { - ROS_WARN_NAMED(log_ns, "%s: Seq mismatch, dropping item (%d != %zu)", - log_ns.c_str(), wpi.seq, wp_cur_id); - return; - } - - ROS_INFO_STREAM_NAMED(log_ns, log_ns << ": item " << waypoint_to_string(wpi)); - - waypoints.push_back(mav_to_msg(wpi)); - if (++wp_cur_id < wp_count) { - restart_timeout_timer(); - mission_request(wp_cur_id); - } - else { - request_mission_done(); - lock.unlock(); - publish_waypoints(); - } - } - else { - ROS_DEBUG_NAMED(log_ns, "%s: rejecting item, wrong state %d", log_ns.c_str(), enum_value(wp_state)); - if (do_pull_after_gcs && reschedule_pull) { - ROS_DEBUG_NAMED(log_ns, "%s: reschedule pull", log_ns.c_str()); - schedule_pull(WP_TIMEOUT_DT); - } - } + unique_lock lock(mutex); + + if (filter_message(wpi)) { + return; + } + + // receive item only in RX state + if (wp_state == WP::RXWPINT) { + if (sequence_mismatch(wpi)) { + return; + } + + auto it = waypoints.emplace(waypoints.end(), wpi); + RCLCPP_INFO_STREAM(get_logger(), log_prefix << ": item " << *it); + + if (++wp_cur_id < wp_count) { + restart_timeout_timer(); + mission_request_int(wp_cur_id); + } else { + request_mission_done(); + mission_item_int_support_confirmed = true; + lock.unlock(); + publish_waypoints(); + } + } else { + RCLCPP_DEBUG( + get_logger(), "%s: rejecting item, wrong state %d", log_prefix, enum_value( + wp_state)); + if (do_pull_after_gcs && reschedule_pull) { + RCLCPP_DEBUG(get_logger(), "%s: reschedule pull", log_prefix); + schedule_pull(WP_TIMEOUT); + } + } } +void MissionBase::handle_mission_request( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::MISSION_REQUEST & mreq, + MFilter filter [[maybe_unused]]) +{ + lock_guard lock(mutex); + + if (filter_message(mreq)) { + return; + } + + if ( + (wp_state == WP::TXLIST && mreq.seq == 0) || + (wp_state == WP::TXPARTIAL && mreq.seq == wp_start_id) || (wp_state == WP::TXWP) || + (wp_state == WP::TXWPINT)) + { + if (sequence_mismatch(mreq)) { + return; + } + + restart_timeout_timer(); + if (mreq.seq < wp_end_id) { + RCLCPP_DEBUG( + get_logger(), "%s: FCU requested MISSION_ITEM waypoint %d", log_prefix, + mreq.seq); + wp_cur_id = mreq.seq; + if (use_mission_item_int) { + RCLCPP_DEBUG(get_logger(), "%s: Trying to send a MISSION_ITEM_INT instead", log_prefix); + wp_state = WP::TXWPINT; + send_waypoint(wp_cur_id); + } else { + wp_state = WP::TXWP; + send_waypoint(wp_cur_id); + } + } else { + RCLCPP_ERROR(get_logger(), "%s: FCU require seq out of range", log_prefix); + } + } else { + RCLCPP_DEBUG( + get_logger(), "%s: rejecting request, wrong state %d", log_prefix, + enum_value(wp_state)); + } +} -bool MissionBase::sequence_mismatch(const uint16_t &seq) +void MissionBase::handle_mission_request_int( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::MISSION_REQUEST_INT & mreq, + MFilter filter [[maybe_unused]]) { - if (seq != wp_cur_id && seq != wp_cur_id + 1) { - ROS_WARN_NAMED(log_ns, "%s: Seq mismatch, dropping request (%d != %zu)", - log_ns.c_str(), seq, wp_cur_id); - return true; - } else { - return false; - } + lock_guard lock(mutex); + + if (filter_message(mreq)) { + return; + } + + if ( + (wp_state == WP::TXLIST && mreq.seq == 0) || + (wp_state == WP::TXPARTIAL && mreq.seq == wp_start_id) || (wp_state == WP::TXWPINT)) + { + if (sequence_mismatch(mreq)) { + return; + } + + if (!use_mission_item_int) { + use_mission_item_int = true; + } + if (!mission_item_int_support_confirmed) { + mission_item_int_support_confirmed = true; + } + + restart_timeout_timer(); + if (mreq.seq < wp_end_id) { + RCLCPP_DEBUG( + get_logger(), "%s: FCU reqested MISSION_ITEM_INT waypoint %d", log_prefix, mreq.seq); + wp_state = WP::TXWPINT; + wp_cur_id = mreq.seq; + send_waypoint(wp_cur_id); + } else { + RCLCPP_ERROR(get_logger(), "%s: FCU require seq out of range", log_prefix); + } + } else { + RCLCPP_DEBUG( + get_logger(), "%s: rejecting request, wrong state %d", log_prefix, + enum_value(wp_state)); + } } +void MissionBase::handle_mission_count( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::MISSION_COUNT & mcnt, + MFilter filter [[maybe_unused]]) +{ + unique_lock lock(mutex); + + if (filter_message(mcnt)) { + return; + } + + if (wp_state == WP::RXLIST) { + // FCU report of MISSION_REQUEST_LIST + RCLCPP_DEBUG(get_logger(), "%s: count %d", log_prefix, mcnt.count); + + wp_count = mcnt.count; + wp_cur_id = 0; + + waypoints.clear(); + waypoints.reserve(wp_count); + + if (wp_count > 0) { + if (use_mission_item_int) { + wp_state = WP::RXWPINT; + restart_timeout_timer(); + mission_request_int(wp_cur_id); + } else { + wp_state = WP::RXWP; + restart_timeout_timer(); + mission_request(wp_cur_id); + } + } else { + request_mission_done(); + lock.unlock(); + publish_waypoints(); + } + } else { + RCLCPP_INFO(get_logger(), "%s: seems GCS requesting mission", log_prefix); + // schedule pull after GCS done + if (do_pull_after_gcs) { + RCLCPP_INFO(get_logger(), "%s: scheduling pull after GCS is done", log_prefix); + reschedule_pull = true; + schedule_pull(RESCHEDULE_TIME); + } + } +} -void MissionBase::handle_mission_request(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_REQUEST &mreq) +void MissionBase::handle_mission_ack( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::MISSION_ACK & mack, + MFilter filter [[maybe_unused]]) { - lock_guard lock(mutex); - - /* Only interested in the specific msg type */ - if (mreq.mission_type != enum_value(wp_type)) { - return; - } - else if ((wp_state == WP::TXLIST && mreq.seq == 0) || (wp_state == WP::TXPARTIAL && mreq.seq == wp_start_id) || (wp_state == WP::TXWP) || (wp_state == WP::TXWPINT)) { - if (sequence_mismatch(mreq.seq)) { - return; - } - - restart_timeout_timer(); - if (mreq.seq < wp_end_id) { - ROS_DEBUG_NAMED(log_ns, "%s: FCU requested MISSION_ITEM waypoint %d", log_ns.c_str(), mreq.seq); - wp_cur_id = mreq.seq; - if (use_mission_item_int) { - ROS_DEBUG_NAMED(log_ns, "%s: Trying to send a MISSION_ITEM_INT instead", log_ns.c_str()); - wp_state = WP::TXWPINT; - send_waypoint(wp_cur_id); - } else { - wp_state = WP::TXWP; - send_waypoint(wp_cur_id); - } - } - else - ROS_ERROR_NAMED(log_ns, "%s: FCU require seq out of range", log_ns.c_str()); - } - else - ROS_DEBUG_NAMED(log_ns, "%s: rejecting request, wrong state %d", log_ns.c_str(), enum_value(wp_state)); + unique_lock lock(mutex); + + auto ack_type = static_cast(mack.type); + + if (filter_message(mack)) { + return; + } + + auto is_tx_done = [&]() -> bool { + return + ( + wp_state == WP::TXLIST || + wp_state == WP::TXPARTIAL || + wp_state == WP::TXWP || + wp_state == WP::TXWPINT + ) && + (wp_cur_id == wp_end_id - 1) && + (ack_type == MRES::ACCEPTED) + ; + }; + + auto is_tx_seq_error = [&]() -> bool { + return + ( + wp_state == WP::TXWP || + wp_state == WP::TXWPINT + ) && + ack_type == MRES::INVALID_SEQUENCE + ; + }; + + auto is_tx_failed = [&]() -> bool { + return + wp_state == WP::TXLIST || + wp_state == WP::TXPARTIAL || + wp_state == WP::TXWP || + wp_state == WP::TXWPINT + ; + }; + + if (is_tx_done()) { + go_idle(); + waypoints = send_waypoints; + send_waypoints.clear(); + + if (wp_state == WP::TXWPINT) { + mission_item_int_support_confirmed = true; + } + + lock.unlock(); + list_sending.notify_all(); + publish_waypoints(); + + RCLCPP_INFO(get_logger(), "%s: mission sended", log_prefix); + } else if (is_tx_seq_error()) { + // Mission Ack: INVALID_SEQUENCE received during TXWP + // This happens when waypoint N was received by autopilot, + // but the request for waypoint N+1 failed. + // This causes seq mismatch, ignore and eventually the request for n+1 + // will get to us and seq will sync up. + RCLCPP_DEBUG(get_logger(), "%s: Received INVALID_SEQUENCE ack", log_prefix); + } else if (is_tx_failed()) { + go_idle(); + // use this flag for failure report + is_timedout = true; + lock.unlock(); + list_sending.notify_all(); + + RCLCPP_ERROR_STREAM( + get_logger(), log_prefix << ": upload failed: " << utils::to_string( + ack_type)); + } else if (wp_state == WP::CLEAR) { + go_idle(); + if (ack_type != MRES::ACCEPTED) { + is_timedout = true; + lock.unlock(); + RCLCPP_ERROR_STREAM( + get_logger(), + log_prefix << ": clear failed: " << utils::to_string(ack_type)); + } else { + waypoints.clear(); + lock.unlock(); + publish_waypoints(); + RCLCPP_INFO(get_logger(), "%s: mission cleared", log_prefix); + } + + list_sending.notify_all(); + } else { + RCLCPP_DEBUG(get_logger(), "%s: not planned ACK, type: %d", log_prefix, mack.type); + } } +void MissionBase::handle_mission_current( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::MISSION_CURRENT & mcur, + MFilter filter [[maybe_unused]]) +{ + unique_lock lock(mutex); + + // NOTE(vooon): this message does not have mission_type + // if (filter_message(mcur)) { + // return; + // } + + if (wp_state == WP::SET_CUR) { + /* MISSION_SET_CURRENT ACK */ + RCLCPP_DEBUG(get_logger(), "%s: set current #%d done", log_prefix, mcur.seq); + go_idle(); + wp_cur_active = mcur.seq; + set_current_waypoint(wp_cur_active); + + lock.unlock(); + list_sending.notify_all(); + publish_waypoints(); + } else if (wp_state == WP::IDLE && wp_cur_active != mcur.seq) { + /* update active */ + RCLCPP_DEBUG(get_logger(), "%s: update current #%d", log_prefix, mcur.seq); + wp_cur_active = mcur.seq; + set_current_waypoint(wp_cur_active); + + lock.unlock(); + publish_waypoints(); + } +} -void MissionBase::handle_mission_request_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_REQUEST_INT &mreq) +void MissionBase::handle_mission_item_reached( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::MISSION_ITEM_REACHED & mitr, + MFilter filter [[maybe_unused]]) { - lock_guard lock(mutex); - - /* Only interested in the specific msg type */ - if (mreq.mission_type != enum_value(wp_type)) { - return; - } - else if ((wp_state == WP::TXLIST && mreq.seq == 0) || (wp_state == WP::TXPARTIAL && mreq.seq == wp_start_id) || (wp_state == WP::TXWPINT)) { - if (sequence_mismatch(mreq.seq)) { - return; - } - - - if (!use_mission_item_int) { - use_mission_item_int = true; - } - if (!mission_item_int_support_confirmed) { - mission_item_int_support_confirmed = true; - } - - restart_timeout_timer(); - if (mreq.seq < wp_end_id) { - ROS_DEBUG_NAMED(log_ns, "%s: FCU reqested MISSION_ITEM_INT waypoint %d", log_ns.c_str(), mreq.seq); - wp_state = WP::TXWPINT; - wp_cur_id = mreq.seq; - send_waypoint(wp_cur_id); - } - else - ROS_ERROR_NAMED(log_ns, "%s: FCU require seq out of range", log_ns.c_str()); - } - else - ROS_DEBUG_NAMED(log_ns, "%s: rejecting request, wrong state %d", log_ns.c_str(), enum_value(wp_state)); + // NOTE(vooon): this message does not have mission_type + // if (filter_message(mitr)) { + // return; + // } + + // in QGC used as informational message + RCLCPP_INFO(get_logger(), "%s: reached #%d", log_prefix, mitr.seq); + publish_reached(mitr.seq); } +void MissionBase::timeout_cb() +{ + unique_lock lock(mutex); + + // run once + timeout_timer->cancel(); + + if (wp_retries > 0) { + wp_retries--; + RCLCPP_WARN(get_logger(), "%s: timeout, retries left %zu", log_prefix, wp_retries); + + switch (wp_state) { + case WP::RXLIST: + mission_request_list(); + break; + case WP::RXWP: + mission_request(wp_cur_id); + break; + case WP::RXWPINT: + mission_request(wp_cur_id); + break; + case WP::TXLIST: + mission_count(wp_count); + break; + case WP::TXPARTIAL: + mission_write_partial_list(wp_start_id, wp_end_id); + break; + case WP::TXWP: + send_waypoint(wp_cur_id); + break; + case WP::TXWPINT: + send_waypoint(wp_cur_id); + break; + case WP::CLEAR: + mission_clear_all(); + break; + case WP::SET_CUR: + mission_set_current(wp_set_active); + break; + + case WP::IDLE: + break; + } + + restart_timeout_timer_int(); + return; + } + + auto use_int = use_mission_item_int && !mission_item_int_support_confirmed; + + if (wp_state == WP::TXWPINT && use_int) { + RCLCPP_ERROR( + get_logger(), "%s: mission_item_int timed out, falling back to mission_item.", log_prefix); + use_mission_item_int = false; + + wp_state = WP::TXWP; + restart_timeout_timer(); + send_waypoint(wp_cur_id); + } else if (wp_state == WP::RXWPINT && use_int) { + RCLCPP_ERROR( + get_logger(), "%s: mission_item_int timed out, falling back to mission_item.", log_prefix); + use_mission_item_int = false; + + wp_state = WP::RXWP; + restart_timeout_timer(); + mission_request(wp_cur_id); + } else { + RCLCPP_ERROR(get_logger(), "%s: timed out.", log_prefix); + go_idle(); + is_timedout = true; + // prevent waiting cond var timeout + lock.unlock(); + list_receiving.notify_all(); + list_sending.notify_all(); + } +} -void MissionBase::handle_mission_count(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_COUNT &mcnt) +void MissionBase::mission_request(const uint16_t seq) { - unique_lock lock(mutex); - - /* Only interested in the specific msg type */ - if (mcnt.mission_type != enum_value(wp_type)) { - return; - } - else if (wp_state == WP::RXLIST) { - /* FCU report of MISSION_REQUEST_LIST */ - ROS_DEBUG_NAMED(log_ns, "%s: count %d", log_ns.c_str(), mcnt.count); - - wp_count = mcnt.count; - wp_cur_id = 0; - - waypoints.clear(); - waypoints.reserve(wp_count); - - if (wp_count > 0) { - if (use_mission_item_int) { - wp_state = WP::RXWPINT; - restart_timeout_timer(); - mission_request_int(wp_cur_id); - } else { - wp_state = WP::RXWP; - restart_timeout_timer(); - mission_request(wp_cur_id); - } - } - else { - request_mission_done(); - lock.unlock(); - publish_waypoints(); - } - } - else { - ROS_INFO_NAMED(log_ns, "%s: seems GCS requesting mission", log_ns.c_str()); - /* schedule pull after GCS done */ - if (do_pull_after_gcs) { - ROS_INFO_NAMED(log_ns, "%s: scheduling pull after GCS is done", log_ns.c_str()); - reschedule_pull = true; - schedule_pull(RESCHEDULE_DT); - } - } + RCLCPP_DEBUG(get_logger(), "%s:m: request #%u", log_prefix, seq); + + mavlink::common::msg::MISSION_REQUEST mrq {}; + uas->msg_set_target(mrq); + mrq.seq = seq; + mrq.mission_type = enum_value(mission_type); + + uas->send_message(mrq); } +void MissionBase::mission_request_int(const uint16_t seq) +{ + RCLCPP_DEBUG(get_logger(), "%s:m: request_int #%u", log_prefix, seq); -void MissionBase::handle_mission_ack(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_ACK &mack) + mavlink::common::msg::MISSION_REQUEST_INT mrq {}; + uas->msg_set_target(mrq); + mrq.seq = seq; + mrq.mission_type = enum_value(mission_type); + + uas->send_message(mrq); +} + +void MissionBase::mission_set_current(const uint16_t seq) +{ + RCLCPP_DEBUG(get_logger(), "%s:m: set current #%u", log_prefix, seq); + + mavlink::common::msg::MISSION_SET_CURRENT msc {}; + uas->msg_set_target(msc); + msc.seq = seq; + // msc.mission_type = enum_value(mission_type); + + uas->send_message(msc); +} + +void MissionBase::mission_request_list() +{ + RCLCPP_DEBUG(get_logger(), "%s:m: request list", log_prefix); + + mavlink::common::msg::MISSION_REQUEST_LIST mrl {}; + uas->msg_set_target(mrl); + mrl.mission_type = enum_value(mission_type); + + uas->send_message(mrl); +} + +void MissionBase::mission_count(const uint16_t cnt) { - unique_lock lock(mutex); - - auto ack_type = static_cast(mack.type); - - /* Only interested in the specific msg type */ - if (mack.mission_type != enum_value(wp_type)) { - return; - } - else if ((wp_state == WP::TXLIST || wp_state == WP::TXPARTIAL || wp_state == WP::TXWP || wp_state == WP::TXWPINT) - && (wp_cur_id == wp_end_id - 1) - && (ack_type == MRES::ACCEPTED)) { - go_idle(); - waypoints = send_waypoints; - send_waypoints.clear(); - if (wp_state == WP::TXWPINT) mission_item_int_support_confirmed = true; - lock.unlock(); - list_sending.notify_all(); - publish_waypoints(); - ROS_INFO_NAMED(log_ns, "%s: mission sended", log_ns.c_str()); - } - else if ((wp_state == WP::TXWP || wp_state == WP::TXWPINT) && ack_type == MRES::INVALID_SEQUENCE) { - // Mission Ack: INVALID_SEQUENCE received during TXWP - // This happens when waypoint N was received by autopilot, but the request for waypoint N+1 failed. - // This causes seq mismatch, ignore and eventually the request for n+1 will get to us and seq will sync up. - ROS_DEBUG_NAMED(log_ns, "%s: Received INVALID_SEQUENCE ack", log_ns.c_str()); - } - else if (wp_state == WP::TXLIST || wp_state == WP::TXPARTIAL || wp_state == WP::TXWP || wp_state == WP::TXWPINT) { - go_idle(); - /* use this flag for failure report */ - is_timedout = true; - lock.unlock(); - list_sending.notify_all(); - - ROS_ERROR_STREAM_NAMED(log_ns, log_ns << ": upload failed: " << utils::to_string(ack_type)); - } - else if (wp_state == WP::CLEAR) { - go_idle(); - if (ack_type != MRES::ACCEPTED) { - is_timedout = true; - lock.unlock(); - ROS_ERROR_STREAM_NAMED(log_ns, log_ns << ": clear failed: " << utils::to_string(ack_type)); - } - else { - waypoints.clear(); - lock.unlock(); - publish_waypoints(); - ROS_INFO_NAMED(log_ns, "%s: mission cleared", log_ns.c_str()); - } - - list_sending.notify_all(); - } - else - ROS_DEBUG_NAMED(log_ns, "%s: not planned ACK, type: %d", log_ns.c_str(), mack.type); + RCLCPP_DEBUG(get_logger(), "%s:m: count %u", log_prefix, cnt); + + mavlink::common::msg::MISSION_COUNT mcnt {}; + uas->msg_set_target(mcnt); + mcnt.count = cnt; + mcnt.mission_type = enum_value(mission_type); + + uas->send_message(mcnt); } +void MissionBase::mission_write_partial_list(const uint16_t start_index, const uint16_t end_index) +{ + RCLCPP_DEBUG( + get_logger(), "%s:m: write partial list %u - %u", log_prefix, + start_index, end_index); + + mavlink::common::msg::MISSION_WRITE_PARTIAL_LIST mwpl {}; + uas->msg_set_target(mwpl); + mwpl.start_index = start_index; + mwpl.end_index = end_index; + mwpl.mission_type = enum_value(mission_type); -void MissionBase::timeout_cb(const ros::TimerEvent &event) + uas->send_message(mwpl); +} + +void MissionBase::mission_clear_all() { - unique_lock lock(mutex); - if (wp_retries > 0) { - wp_retries--; - ROS_WARN_NAMED(log_ns, "%s: timeout, retries left %zu", log_ns.c_str(), wp_retries); - - switch (wp_state) { - case WP::RXLIST: - mission_request_list(); - break; - case WP::RXWP: - mission_request(wp_cur_id); - break; - case WP::RXWPINT: - mission_request(wp_cur_id); - break; - case WP::TXLIST: - mission_count(wp_count); - break; - case WP::TXPARTIAL: - mission_write_partial_list(wp_start_id, wp_end_id); - break; - case WP::TXWP: - send_waypoint(wp_cur_id); - break; - case WP::TXWPINT: - send_waypoint(wp_cur_id); - break; - case WP::CLEAR: - mission_clear_all(); - break; - case WP::SET_CUR: - mission_set_current(wp_set_active); - break; - - case WP::IDLE: - break; - } - - restart_timeout_timer_int(); - } - else { - if (wp_state == WP::TXWPINT && use_mission_item_int && !mission_item_int_support_confirmed) { - ROS_ERROR_NAMED(log_ns, "%s: mission_item_int timed out, falling back to mission_item.", log_ns.c_str()); - use_mission_item_int = false; - - wp_state = WP::TXWP; - restart_timeout_timer(); - send_waypoint(wp_cur_id); - } else if (wp_state == WP::RXWPINT && use_mission_item_int && !mission_item_int_support_confirmed) { - ROS_ERROR_NAMED(log_ns, "%s: mission_item_int timed out, falling back to mission_item.", log_ns.c_str()); - use_mission_item_int = false; - - wp_state = WP::RXWP; - restart_timeout_timer(); - mission_request(wp_cur_id); - } else { - ROS_ERROR_NAMED(log_ns, "%s: timed out.", log_ns.c_str()); - go_idle(); - is_timedout = true; - /* prevent waiting cond var timeout */ - lock.unlock(); - list_receiving.notify_all(); - list_sending.notify_all(); - } - } + RCLCPP_DEBUG(get_logger(), "%s:m: clear all", log_prefix); + + mavlink::common::msg::MISSION_CLEAR_ALL mclr {}; + uas->msg_set_target(mclr); + mclr.mission_type = enum_value(mission_type); + + uas->send_message(mclr); +} + +void MissionBase::mission_ack(const MRES type) +{ + RCLCPP_DEBUG(get_logger(), "%s:m: ACK %u", log_prefix, enum_value(type)); + + mavlink::common::msg::MISSION_ACK mack {}; + uas->msg_set_target(mack); + mack.type = enum_value(type); + mack.mission_type = enum_value(mission_type); + + uas->send_message(mack); } -} // namespace plugin -} // namespace mavros \ No newline at end of file diff --git a/mavros/src/plugins/nav_controller_output.cpp b/mavros/src/plugins/nav_controller_output.cpp index f3fad43a5..22aadc05f 100644 --- a/mavros/src/plugins/nav_controller_output.cpp +++ b/mavros/src/plugins/nav_controller_output.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2019 Karthik Desai + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief NavControllerOutput plugin * @file nav_controller_output.cpp @@ -6,70 +13,67 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2019 Karthik Desai - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include "mavros_msgs/NavControllerOutput.h" +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/nav_controller_output.hpp" namespace mavros { namespace std_plugins { +using namespace std::placeholders; // NOLINT + /** * @brief nav controller output plugin. + * @plugin nav_controller_output * * Publishes nav_controller_output message https://mavlink.io/en/messages/common.html#NAV_CONTROLLER_OUTPUT */ -class NavControllerOutputPlugin : public plugin::PluginBase +class NavControllerOutputPlugin : public plugin::Plugin { public: - NavControllerOutputPlugin() : PluginBase(), - nh("~") - { - } - - void initialize(UAS &uas_) - { - PluginBase::initialize(uas_); - nco_pub = nh.advertise("nav_controller_output", 10); - } + explicit NavControllerOutputPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "nav_controller_output") + { + nco_pub = node->create_publisher("~/output", 10); + } - Subscriptions get_subscriptions() - { - return { - make_handler(&NavControllerOutputPlugin::handle_nav_controller_output), - }; - } + Subscriptions get_subscriptions() + { + return { + make_handler(&NavControllerOutputPlugin::handle_nav_controller_output), + }; + } private: - ros::NodeHandle nh; + rclcpp::Publisher::SharedPtr nco_pub; - ros::Publisher nco_pub; + void handle_nav_controller_output( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::NAV_CONTROLLER_OUTPUT & nav_controller_output, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto nco_msg = mavros_msgs::msg::NavControllerOutput(); - void handle_nav_controller_output(const mavlink::mavlink_message_t *msg, mavlink::common::msg::NAV_CONTROLLER_OUTPUT &nav_controller_output) - { - auto nco_msg = boost::make_shared(); - nco_msg->header.stamp = ros::Time::now(); - nco_msg->nav_roll = nav_controller_output.nav_roll; - nco_msg->nav_pitch = nav_controller_output.nav_pitch; - nco_msg->nav_bearing = nav_controller_output.nav_bearing; - nco_msg->target_bearing = nav_controller_output.target_bearing; - nco_msg->wp_dist = nav_controller_output.wp_dist; - nco_msg->alt_error = nav_controller_output.alt_error; - nco_msg->aspd_error = nav_controller_output.aspd_error; - nco_msg->xtrack_error = nav_controller_output.xtrack_error; + nco_msg.header.stamp = node->now(); + nco_msg.nav_roll = nav_controller_output.nav_roll; + nco_msg.nav_pitch = nav_controller_output.nav_pitch; + nco_msg.nav_bearing = nav_controller_output.nav_bearing; + nco_msg.target_bearing = nav_controller_output.target_bearing; + nco_msg.wp_dist = nav_controller_output.wp_dist; + nco_msg.alt_error = nav_controller_output.alt_error; + nco_msg.aspd_error = nav_controller_output.aspd_error; + nco_msg.xtrack_error = nav_controller_output.xtrack_error; - nco_pub.publish(nco_msg); - } + nco_pub->publish(nco_msg); + } }; -} // namespace std_plugins -} // namespace mavros +} // namespace std_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::NavControllerOutputPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::NavControllerOutputPlugin) diff --git a/mavros/src/plugins/param.cpp b/mavros/src/plugins/param.cpp index 41c97ad1c..82696ff81 100644 --- a/mavros/src/plugins/param.cpp +++ b/mavros/src/plugins/param.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2014,2015,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Parameter plugin * @file param.cpp @@ -6,27 +13,46 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014,2015,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ #include #include -#include +#include +#include +#include +#include +#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/srv/param_pull.hpp" +#include "mavros_msgs/srv/param_set_v2.hpp" +#include "mavros_msgs/msg/param_event.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT +using utils::enum_value; -#include -#include -#include -#include -#include +// Copy from rclcpp/src/rclcpp/parameter_service_names.hpp +// They are not exposed to user's code. +namespace PSN +{ +static constexpr const char * get_parameters = "~/get_parameters"; +static constexpr const char * get_parameter_types = "~/get_parameter_types"; +static constexpr const char * set_parameters = "~/set_parameters"; +static constexpr const char * set_parameters_atomically = "~/set_parameters_atomically"; +static constexpr const char * describe_parameters = "~/describe_parameters"; +static constexpr const char * list_parameters = "~/list_parameters"; -namespace mavros { -namespace std_plugins { -using utils::enum_value; +static constexpr const char * events = "/parameter_events"; +} // namespace PSN /** * @brief Parameter storage @@ -44,902 +70,1078 @@ using utils::enum_value; * - real32 for float's * * So no reason to really use boost::any. - * But feel free to fire an issue if your AP do not like it. + * But feel free to fire an issue if your AP does not like it. */ -class Parameter { +class Parameter +{ public: - using MT = mavlink::common::MAV_PARAM_TYPE; - using PARAM_SET = mavlink::common::msg::PARAM_SET; - using XmlRpcValue = XmlRpc::XmlRpcValue; - - std::string param_id; - XmlRpcValue param_value; - uint16_t param_index; - uint16_t param_count; - - void set_value(mavlink::common::msg::PARAM_VALUE &pmsg) - { - mavlink::mavlink_param_union_t uv; - uv.param_float = pmsg.param_value; - - // #170 - copy union value to itermediate var - int int_tmp; - float float_tmp; - - switch (pmsg.param_type) { - // [[[cog: - // param_types = [ (s, 'float' if s == 'real32' else s) for s in ( - // 'int8', 'uint8', - // 'int16', 'uint16', - // 'int32', 'uint32', - // 'real32', - // )] - // unsupported_types = ('int64', 'uint64', 'real64') - // - // for a, b in param_types: - // btype = 'int' if 'int' in b else b - // cog.outl("case enum_value(MT::%s):" % a.upper()) - // cog.outl("\t%s_tmp = uv.param_%s;" % (btype, b)) - // cog.outl("\tparam_value = %s_tmp;" % btype) - // cog.outl("\tbreak;") - // ]]] - case enum_value(MT::INT8): - int_tmp = uv.param_int8; - param_value = int_tmp; - break; - case enum_value(MT::UINT8): - int_tmp = uv.param_uint8; - param_value = int_tmp; - break; - case enum_value(MT::INT16): - int_tmp = uv.param_int16; - param_value = int_tmp; - break; - case enum_value(MT::UINT16): - int_tmp = uv.param_uint16; - param_value = int_tmp; - break; - case enum_value(MT::INT32): - int_tmp = uv.param_int32; - param_value = int_tmp; - break; - case enum_value(MT::UINT32): - int_tmp = uv.param_uint32; - param_value = int_tmp; - break; - case enum_value(MT::REAL32): - float_tmp = uv.param_float; - param_value = float_tmp; - break; - // [[[end]]] (checksum: 5950e4ee032d4aa198b953f56909e129) - - default: - ROS_WARN_NAMED("param", "PM: Unsupported param %.16s (%u/%u) type: %u", - pmsg.param_id.data(), pmsg.param_index, pmsg.param_count, pmsg.param_type); - param_value = 0; - }; - } - - /** - * Variation of set_value with quirks for ArduPilotMega - */ - void set_value_apm_quirk(mavlink::common::msg::PARAM_VALUE &pmsg) - { - int32_t int_tmp; - float float_tmp; - - switch (pmsg.param_type) { - // [[[cog: - // for a, b in param_types: - // btype = 'int' if 'int' in b else b - // cog.outl("case enum_value(MT::%s):" % a.upper()) - // cog.outl("\t%s_tmp = pmsg.param_value;" % btype) - // cog.outl("\tparam_value = %s_tmp;" % btype) - // cog.outl("\tbreak;") - // ]]] - case enum_value(MT::INT8): - int_tmp = pmsg.param_value; - param_value = int_tmp; - break; - case enum_value(MT::UINT8): - int_tmp = pmsg.param_value; - param_value = int_tmp; - break; - case enum_value(MT::INT16): - int_tmp = pmsg.param_value; - param_value = int_tmp; - break; - case enum_value(MT::UINT16): - int_tmp = pmsg.param_value; - param_value = int_tmp; - break; - case enum_value(MT::INT32): - int_tmp = pmsg.param_value; - param_value = int_tmp; - break; - case enum_value(MT::UINT32): - int_tmp = pmsg.param_value; - param_value = int_tmp; - break; - case enum_value(MT::REAL32): - float_tmp = pmsg.param_value; - param_value = float_tmp; - break; - // [[[end]]] (checksum: c30ee34dd84213471690612ab49f1f73) - - default: - ROS_WARN_NAMED("param", "PM: Unsupported param %.16s (%u/%u) type: %u", - pmsg.param_id.data(), pmsg.param_index, pmsg.param_count, pmsg.param_type); - param_value = 0; - } - } - - //! Make PARAM_SET message. Set target ids manually! - PARAM_SET to_param_set() - { - // Note: XmlRpcValue does not have const cast operators. - // This method can't be const. - - mavlink::mavlink_param_union_t uv; - PARAM_SET ret{}; - - mavlink::set_string(ret.param_id, param_id); - - switch (param_value.getType()) { - // [[[cog: - // xmlrpc_types = ( - // ('TypeBoolean', 'uint8', 'bool'), - // ('TypeInt', 'int32', 'int32_t'), - // ('TypeDouble', 'real32', 'double'), - // ) - // - // for a, b, c in xmlrpc_types: - // uvb = 'float' if 'real32' == b else b - // cog.outl("case XmlRpcValue::%s:" % a) - // cog.outl("\tuv.param_%s = static_cast<%s>(param_value);" % (uvb, c)) - // cog.outl("\tret.param_type = enum_value(MT::%s);" % b.upper()) - // cog.outl("\tbreak;") - // ]]] - case XmlRpcValue::TypeBoolean: - uv.param_uint8 = static_cast(param_value); - ret.param_type = enum_value(MT::UINT8); - break; - case XmlRpcValue::TypeInt: - uv.param_int32 = static_cast(param_value); - ret.param_type = enum_value(MT::INT32); - break; - case XmlRpcValue::TypeDouble: - uv.param_float = static_cast(param_value); - ret.param_type = enum_value(MT::REAL32); - break; - // [[[end]]] (checksum: c414a3950fba234cbbe694a2576ae022) - - default: - ROS_WARN_NAMED("param", "PR: Unsupported XmlRpcValue type: %u", param_value.getType()); - } - - ret.param_value = uv.param_float; - return ret; - } - - //! Make PARAM_SET message. Set target ids manually! - PARAM_SET to_param_set_apm_qurk() - { - PARAM_SET ret{}; - - mavlink::set_string(ret.param_id, param_id); - - switch (param_value.getType()) { - // [[[cog: - // for a, b, c in xmlrpc_types: - // cog.outl("case XmlRpcValue::%s:" % a) - // cog.outl("\tret.param_value = static_cast<%s &>(param_value);" % c) - // cog.outl("\tret.param_type = enum_value(MT::%s);" % b.upper()) - // cog.outl("\tbreak;") - // ]]] - case XmlRpcValue::TypeBoolean: - ret.param_value = static_cast(param_value); - ret.param_type = enum_value(MT::UINT8); - break; - case XmlRpcValue::TypeInt: - ret.param_value = static_cast(param_value); - ret.param_type = enum_value(MT::INT32); - break; - case XmlRpcValue::TypeDouble: - ret.param_value = static_cast(param_value); - ret.param_type = enum_value(MT::REAL32); - break; - // [[[end]]] (checksum: 5b10c0e1f2e916f1c31313eaa5cc83e0) - - default: - ROS_WARN_NAMED("param", "PR: Unsupported XmlRpcValue type: %u", param_value.getType()); - } - - return ret; - } - - /** - * For get/set services - */ - int64_t to_integer() - { - switch (param_value.getType()) { - // [[[cog: - // for a, b, c in xmlrpc_types: - // if 'int' not in b: - // continue - // cog.outl("case XmlRpcValue::%s:\treturn static_cast<%s>(param_value);" % (a, c)) - // ]]] - case XmlRpcValue::TypeBoolean: return static_cast(param_value); - case XmlRpcValue::TypeInt: return static_cast(param_value); - // [[[end]]] (checksum: ce23a3bc04354d8cfcb82341beb83709) - - default: - return 0; - } - } - - double to_real() - { - if (param_value.getType() == XmlRpcValue::TypeDouble) - return static_cast(param_value); - else - return 0.0; - } - - // for debugging - std::string to_string() const - { - return utils::format("%s (%u/%u): %s", param_id.c_str(), param_index, param_count, param_value.toXml().c_str()); - } - - mavros_msgs::Param to_msg() - { - mavros_msgs::Param msg; - - // XXX(vooon): find better solution - msg.header.stamp = ros::Time::now(); - - msg.param_id = param_id; - msg.value.integer = to_integer(); - msg.value.real = to_real(); - msg.param_index = param_index; - msg.param_count = param_count; - - return msg; - } - - /** - * Exclude this parameters from ~param/push - */ - static bool check_exclude_param_id(std::string param_id) - { - return param_id == "SYSID_SW_MREV" || - param_id == "SYS_NUM_RESETS" || - param_id == "ARSPD_OFFSET" || - param_id == "GND_ABS_PRESS" || - param_id == "GND_ABS_PRESS2" || - param_id == "GND_ABS_PRESS3" || - param_id == "STAT_BOOTCNT" || - param_id == "STAT_FLTTIME" || - param_id == "STAT_RESET" || - param_id == "STAT_RUNTIME" || - param_id == "GND_TEMP" || - param_id == "CMD_TOTAL" || - param_id == "CMD_INDEX" || - param_id == "LOG_LASTFILE" || - param_id == "FENCE_TOTAL" || - param_id == "FORMAT_VERSION"; - } + using MT = mavlink::common::MAV_PARAM_TYPE; + using PARAM_SET = mavlink::common::msg::PARAM_SET; + + rclcpp::Time stamp; + std::string param_id; + rclcpp::ParameterValue param_value; + uint16_t param_index; + uint16_t param_count; + + explicit Parameter( + const std::string & param_id_, + const uint16_t param_index_ = 0, + const uint16_t param_count_ = 0, + const rclcpp::ParameterValue & param_value_ = rclcpp::ParameterValue()) + : stamp{}, + param_id(param_id_), + param_value(param_value_), + param_index(param_index_), + param_count(param_count_) + { + } + + explicit Parameter(const rclcpp::Parameter & param) + : Parameter(param.get_name(), 0, 0, param.get_parameter_value()) + { + } + + void set_value(mavlink::common::msg::PARAM_VALUE & pmsg) + { + mavlink::mavlink_param_union_t uv; + uv.param_float = pmsg.param_value; + + // #170 - copy union value to itermediate var + int int_tmp; + float float_tmp; + + switch (pmsg.param_type) { + // [[[cog: + // param_types = [ (s, 'float' if s == 'real32' else s) for s in ( + // 'int8', 'uint8', + // 'int16', 'uint16', + // 'int32', 'uint32', + // 'real32', + // )] + // unsupported_types = ('int64', 'uint64', 'real64') + // + // for a, b in param_types: + // btype = 'int' if 'int' in b else b + // cog.outl(f"case enum_value(MT::{a.upper()}):") + // cog.outl(f" {btype}_tmp = uv.param_{b};") + // cog.outl(f" param_value = rclcpp::ParameterValue({btype}_tmp);") + // cog.outl(f" break;") + // ]]] + case enum_value(MT::INT8): + int_tmp = uv.param_int8; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::UINT8): + int_tmp = uv.param_uint8; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::INT16): + int_tmp = uv.param_int16; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::UINT16): + int_tmp = uv.param_uint16; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::INT32): + int_tmp = uv.param_int32; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::UINT32): + int_tmp = uv.param_uint32; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::REAL32): + float_tmp = uv.param_float; + param_value = rclcpp::ParameterValue(float_tmp); + break; + // [[[end]]] (checksum: 9e08cfaf40fdb8644695057e6419a913) + + default: + RCLCPP_WARN( + get_logger(), "PR: Unsupported param %.16s (%u/%u) type: %u", + pmsg.param_id.data(), pmsg.param_index, pmsg.param_count, pmsg.param_type); + param_value = rclcpp::ParameterValue(); + } + } + + /** + * Variation of set_value with quirks for ArduPilotMega + */ + void set_value_apm_quirk(mavlink::common::msg::PARAM_VALUE & pmsg) + { + int32_t int_tmp; + float float_tmp; + + switch (pmsg.param_type) { + // [[[cog: + // for a, b in param_types: + // btype = 'int' if 'int' in b else b + // cog.outl(f"case enum_value(MT::{a.upper()}):") + // cog.outl(f" {btype}_tmp = pmsg.param_value;") + // cog.outl(f" param_value = rclcpp::ParameterValue({btype}_tmp);") + // cog.outl(f" break;") + // ]]] + case enum_value(MT::INT8): + int_tmp = pmsg.param_value; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::UINT8): + int_tmp = pmsg.param_value; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::INT16): + int_tmp = pmsg.param_value; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::UINT16): + int_tmp = pmsg.param_value; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::INT32): + int_tmp = pmsg.param_value; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::UINT32): + int_tmp = pmsg.param_value; + param_value = rclcpp::ParameterValue(int_tmp); + break; + case enum_value(MT::REAL32): + float_tmp = pmsg.param_value; + param_value = rclcpp::ParameterValue(float_tmp); + break; + // [[[end]]] (checksum: 5edecd0aeee6b2730b57904bc29aaff2) + + default: + RCLCPP_WARN( + get_logger(), + "PR: Unsupported param %.16s (%u/%u) type: %u", + pmsg.param_id.data(), pmsg.param_index, pmsg.param_count, pmsg.param_type); + param_value = rclcpp::ParameterValue(); + } + } + + //! Make PARAM_SET message. Set target ids manually! + PARAM_SET to_param_set() const + { + mavlink::mavlink_param_union_t uv; + PARAM_SET ret{}; + + mavlink::set_string(ret.param_id, param_id); + + switch (param_value.get_type()) { + // [[[cog: + // parameter_types = ( + // ('PARAMETER_BOOL', 'uint8', 'bool'), + // ('PARAMETER_INTEGER', 'int32', 'int32_t'), + // ('PARAMETER_DOUBLE', 'real32', 'double'), + // ) + // + // for a, b, c in parameter_types: + // uvb = 'float' if 'real32' == b else b + // cog.outl(f"case rclcpp::{a}:") + // cog.outl(f" uv.param_{uvb} = param_value.get<{c}>();") + // cog.outl(f" ret.param_type = enum_value(MT::{b.upper()});") + // cog.outl(f" break;") + // ]]] + case rclcpp::PARAMETER_BOOL: + uv.param_uint8 = param_value.get(); + ret.param_type = enum_value(MT::UINT8); + break; + case rclcpp::PARAMETER_INTEGER: + uv.param_int32 = param_value.get(); + ret.param_type = enum_value(MT::INT32); + break; + case rclcpp::PARAMETER_DOUBLE: + uv.param_float = param_value.get(); + ret.param_type = enum_value(MT::REAL32); + break; + // [[[end]]] (checksum: 5ee0c1c37bca338a29b07048f7212d7d) + + default: + RCLCPP_WARN_STREAM( + get_logger(), + "PR: Unsupported ParameterValue type: " << rclcpp::to_string(param_value.get_type())); + } + + ret.param_value = uv.param_float; + return ret; + } + + //! Make PARAM_SET message. Set target ids manually! + PARAM_SET to_param_set_apm_qurk() const + { + PARAM_SET ret{}; + + mavlink::set_string(ret.param_id, param_id); + + switch (param_value.get_type()) { + // [[[cog: + // for a, b, c in parameter_types: + // cog.outl(f"case rclcpp::{a}:") + // cog.outl(f" ret.param_value = param_value.get<{c}>();") + // cog.outl(f" ret.param_type = enum_value(MT::{b.upper()});") + // cog.outl(f" break;") + // ]]] + case rclcpp::PARAMETER_BOOL: + ret.param_value = param_value.get(); + ret.param_type = enum_value(MT::UINT8); + break; + case rclcpp::PARAMETER_INTEGER: + ret.param_value = param_value.get(); + ret.param_type = enum_value(MT::INT32); + break; + case rclcpp::PARAMETER_DOUBLE: + ret.param_value = param_value.get(); + ret.param_type = enum_value(MT::REAL32); + break; + // [[[end]]] (checksum: ba896d50b78e007407d92bd22aaceca8) + + default: + RCLCPP_WARN_STREAM( + get_logger(), + "PR: Unsupported ParameterValue type: " << rclcpp::to_string(param_value.get_type())); + } + + return ret; + } + + // for debugging + std::string to_string() const + { + auto pv = rclcpp::to_string(param_value); + return utils::format( + "%s (%u/%u): %s", + param_id.c_str(), param_index, param_count, pv.c_str()); + } + + mavros_msgs::msg::ParamEvent to_event_msg() const + { + mavros_msgs::msg::ParamEvent msg{}; + msg.header.stamp = stamp; + msg.param_id = param_id; + msg.value = param_value.to_value_msg(); + msg.param_index = param_index; + msg.param_count = param_count; + + return msg; + } + + rclcpp::Parameter to_rcl() const + { + return {param_id, param_value}; + } + + rcl_interfaces::msg::Parameter to_parameter_msg() const + { + return to_rcl().to_parameter_msg(); + } + + rcl_interfaces::msg::ParameterDescriptor to_descriptor() const + { + rcl_interfaces::msg::ParameterDescriptor msg{}; + msg.name = param_id; + msg.type = param_value.get_type(); + msg.read_only = check_exclude_param_id(param_id); +#ifndef USE_OLD_DECLARE_PARAMETER + msg.dynamic_typing = true; +#endif + + return msg; + } + + /** + * Exclude these parameters from ~param/push + */ + static bool check_exclude_param_id(const std::string & param_id) + { + static const std::set exclude_ids{ + "_HASH_CHECK", + "SYSID_SW_MREV", + "SYS_NUM_RESETS", + "ARSPD_OFFSET", + "GND_ABS_PRESS", + "GND_ABS_PRESS2", + "GND_ABS_PRESS3", + "STAT_BOOTCNT", + "STAT_FLTTIME", + "STAT_RESET", + "STAT_RUNTIME", + "GND_TEMP", + "CMD_TOTAL", + "CMD_INDEX", + "LOG_LASTFILE", + "MIS_TOTAL", + "FENCE_TOTAL", + "FORMAT_VERSION" + }; + + return exclude_ids.find(param_id) != exclude_ids.end(); + } + + inline rclcpp::Logger get_logger() const + { + return rclcpp::get_logger("mavros.param"); + } }; /** * @brief Parameter set transaction data */ -class ParamSetOpt { +class ParamSetOpt +{ public: - ParamSetOpt(Parameter &_p, size_t _rem) : - param(_p), - retries_remaining(_rem), - is_timedout(false) - { } - - Parameter param; - size_t retries_remaining; - bool is_timedout; - std::mutex cond_mutex; - std::condition_variable ack; + struct Result + { + bool success; + Parameter param; + }; + + ParamSetOpt(const Parameter & _p, size_t _rem) + : param(_p), + retries_remaining(_rem) + {} + + Parameter param; + std::atomic retries_remaining; + std::promise promise; }; /** * @brief Parameter manipulation plugin + * @plugin param */ -class ParamPlugin : public plugin::PluginBase { +class ParamPlugin : public plugin::Plugin +{ public: - ParamPlugin() : PluginBase(), - param_nh("~param"), - BOOTUP_TIME_DT(BOOTUP_TIME_MS / 1000.0), - LIST_TIMEOUT_DT(LIST_TIMEOUT_MS / 1000.0), - PARAM_TIMEOUT_DT(PARAM_TIMEOUT_MS / 1000.0), - RETRIES_COUNT(_RETRIES_COUNT), - param_count(-1), - param_state(PR::IDLE), - param_rx_retries(RETRIES_COUNT), - is_timedout(false) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - pull_srv = param_nh.advertiseService("pull", &ParamPlugin::pull_cb, this); - push_srv = param_nh.advertiseService("push", &ParamPlugin::push_cb, this); - set_srv = param_nh.advertiseService("set", &ParamPlugin::set_cb, this); - get_srv = param_nh.advertiseService("get", &ParamPlugin::get_cb, this); - - param_value_pub = param_nh.advertise("param_value", 100); - - shedule_timer = param_nh.createTimer(BOOTUP_TIME_DT, &ParamPlugin::shedule_cb, this, true); - shedule_timer.stop(); - timeout_timer = param_nh.createTimer(PARAM_TIMEOUT_DT, &ParamPlugin::timeout_cb, this, true); - timeout_timer.stop(); - enable_connection_cb(); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&ParamPlugin::handle_param_value), - }; - } + explicit ParamPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "param", rclcpp::NodeOptions().start_parameter_services( + false).start_parameter_event_publisher(false)), + BOOTUP_TIME(10s), + LIST_TIMEOUT(30s), + PARAM_TIMEOUT(1s), + RETRIES_COUNT(3), + param_count(-1), + param_state(PR::IDLE), + param_rx_retries(RETRIES_COUNT), + is_timedout(false) + { + auto event_qos = rclcpp::ParameterEventsQoS(); + auto qos = rclcpp::ParametersQoS().get_rmw_qos_profile(); + + param_event_pub = node->create_publisher("~/event", event_qos); + std_event_pub = node->create_publisher( + PSN::events, + event_qos); + + // Custom parameter services + pull_srv = + node->create_service( + "~/pull", + std::bind(&ParamPlugin::pull_cb, this, _1, _2), qos); + set_srv = + node->create_service( + "~/set", + std::bind(&ParamPlugin::set_cb, this, _1, _2), qos); + + // Standard parameter services + get_parameters_srv = node->create_service( + PSN::get_parameters, + std::bind(&ParamPlugin::get_parameters_cb, this, _1, _2), qos); + get_parameter_types_srv = node->create_service( + PSN::get_parameter_types, + std::bind(&ParamPlugin::get_parameter_types_cb, this, _1, _2), qos); + set_parameters_srv = node->create_service( + PSN::set_parameters, + std::bind(&ParamPlugin::set_parameters_cb, this, _1, _2), qos); + set_parameters_atomically_srv = + node->create_service( + PSN::set_parameters_atomically, + std::bind(&ParamPlugin::set_parameters_atomically_cb, this, _1, _2), qos); + describe_parameters_srv = node->create_service( + PSN::describe_parameters, + std::bind(&ParamPlugin::describe_parameters_cb, this, _1, _2), qos); + list_parameters_srv = node->create_service( + PSN::list_parameters, + std::bind(&ParamPlugin::list_parameters_cb, this, _1, _2), qos); + + schedule_timer = + node->create_wall_timer(BOOTUP_TIME, std::bind(&ParamPlugin::schedule_cb, this)); + schedule_timer->cancel(); + + timeout_timer = + node->create_wall_timer(PARAM_TIMEOUT, std::bind(&ParamPlugin::timeout_cb, this)); + timeout_timer->cancel(); + + enable_connection_cb(); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&ParamPlugin::handle_param_value), + }; + } private: - using lock_guard = std::lock_guard; - using unique_lock = std::unique_lock; - - std::recursive_mutex mutex; - ros::NodeHandle param_nh; - - ros::ServiceServer pull_srv; - ros::ServiceServer push_srv; - ros::ServiceServer set_srv; - ros::ServiceServer get_srv; - - ros::Publisher param_value_pub; - - ros::Timer shedule_timer; //!< for startup shedule fetch - ros::Timer timeout_timer; //!< for timeout resend - - static constexpr int BOOTUP_TIME_MS = 10000; //!< APM boot time - static constexpr int PARAM_TIMEOUT_MS = 1000; //!< Param wait time - static constexpr int LIST_TIMEOUT_MS = 30000; //!< Receive all time - static constexpr int _RETRIES_COUNT = 3; - - const ros::Duration BOOTUP_TIME_DT; - const ros::Duration LIST_TIMEOUT_DT; - const ros::Duration PARAM_TIMEOUT_DT; - const int RETRIES_COUNT; - - std::unordered_map parameters; - std::list parameters_missing_idx; - std::unordered_map> set_parameters; - ssize_t param_count; - enum class PR { - IDLE, - RXLIST, - RXPARAM, - RXPARAM_TIMEDOUT, - TXPARAM - }; - PR param_state; - - size_t param_rx_retries; - bool is_timedout; - std::mutex list_cond_mutex; - std::condition_variable list_receiving; - - /* -*- message handlers -*- */ - - void handle_param_value(const mavlink::mavlink_message_t *msg, mavlink::common::msg::PARAM_VALUE &pmsg) - { - lock_guard lock(mutex); - - auto param_id = mavlink::to_string(pmsg.param_id); - - // search - auto param_it = parameters.find(param_id); - if (param_it != parameters.end()) { - // parameter exists - auto &p = param_it->second; - - if (m_uas->is_ardupilotmega()) - p.set_value_apm_quirk(pmsg); - else - p.set_value(pmsg); - - // check that ack required - auto set_it = set_parameters.find(param_id); - if (set_it != set_parameters.end()) { - set_it->second->ack.notify_all(); - } - - param_value_pub.publish(p.to_msg()); - - ROS_WARN_STREAM_COND_NAMED( - ((p.param_index != pmsg.param_index && - pmsg.param_index != UINT16_MAX) || - p.param_count != pmsg.param_count), - "param", - "PR: Param " << p.to_string() << " different index: " << pmsg.param_index << "/" << pmsg.param_count); - ROS_DEBUG_STREAM_NAMED("param", "PR: Update param " << p.to_string()); - } - else { - // insert new element - Parameter p{}; - p.param_id = param_id; - p.param_index = pmsg.param_index; - p.param_count = pmsg.param_count; - - if (m_uas->is_ardupilotmega()) - p.set_value_apm_quirk(pmsg); - else - p.set_value(pmsg); - - parameters[param_id] = p; - - param_value_pub.publish(p.to_msg()); - - ROS_DEBUG_STREAM_NAMED("param", "PR: New param " << p.to_string()); - } - - if (param_state == PR::RXLIST || param_state == PR::RXPARAM || param_state == PR::RXPARAM_TIMEDOUT) { - - // we received first param. setup list timeout - if (param_state == PR::RXLIST) { - param_count = pmsg.param_count; - param_state = PR::RXPARAM; - - parameters_missing_idx.clear(); - if (param_count != UINT16_MAX) { - ROS_DEBUG_NAMED("param", "PR: waiting %zu parameters", param_count); - // declare that all parameters are missing - for (uint16_t idx = 0; idx < param_count; idx++) - parameters_missing_idx.push_back(idx); - } - else - ROS_WARN_NAMED("param", "PR: FCU does not know index for first element! " - "Param list may be truncated."); - } - - // trying to avoid endless rerequest loop - // Issue #276 - bool it_is_first_requested = parameters_missing_idx.front() == pmsg.param_index; - - // remove idx for that message - parameters_missing_idx.remove(pmsg.param_index); - - // in receiving mode we use param_rx_retries for LIST and PARAM - if (it_is_first_requested) { - ROS_DEBUG_NAMED("param", "PR: got a value of a requested param idx=%u, " - "resetting retries count", pmsg.param_index); - param_rx_retries = RETRIES_COUNT; - } else if (param_state == PR::RXPARAM_TIMEDOUT) { - ROS_INFO_NAMED("param", "PR: got an unsolicited param value idx=%u, " - "not resetting retries count %zu", pmsg.param_index, param_rx_retries); - } - - restart_timeout_timer(); - - /* index starting from 0, receivig done */ - if (parameters_missing_idx.empty()) { - ssize_t missed = param_count - parameters.size(); - ROS_INFO_COND_NAMED(missed == 0, "param", "PR: parameters list received"); - ROS_WARN_COND_NAMED(missed > 0, "param", - "PR: parameters list received, but %zd parametars are missed", - missed); - go_idle(); - list_receiving.notify_all(); - } else if (param_state == PR::RXPARAM_TIMEDOUT) { - uint16_t first_miss_idx = parameters_missing_idx.front(); - ROS_DEBUG_NAMED("param", "PR: requesting next timed out parameter idx=%u", first_miss_idx); - param_request_read("", first_miss_idx); - } - } - } - - /* -*- low-level send function -*- */ - - void param_request_list() - { - ROS_DEBUG_NAMED("param", "PR:m: request list"); - - mavlink::common::msg::PARAM_REQUEST_LIST rql{}; - m_uas->msg_set_target(rql); - - UAS_FCU(m_uas)->send_message_ignore_drop(rql); - } - - void param_request_read(std::string id, int16_t index = -1) - { - ROS_ASSERT(index >= -1); - - ROS_DEBUG_NAMED("param", "PR:m: request '%s', idx %d", id.c_str(), index); - - mavlink::common::msg::PARAM_REQUEST_READ rqr{}; - m_uas->msg_set_target(rqr); - rqr.param_index = index; - - if (index != -1) { - mavlink::set_string(rqr.param_id, id); - } - - UAS_FCU(m_uas)->send_message_ignore_drop(rqr); - } - - void param_set(Parameter ¶m) - { - ROS_DEBUG_STREAM_NAMED("param", "PR:m: set param " << param.to_string()); - - // GCC 4.8 can't type out lambda return - auto ps = ([this, ¶m]() -> mavlink::common::msg::PARAM_SET { - if (m_uas->is_ardupilotmega()) - return param.to_param_set_apm_qurk(); - else - return param.to_param_set(); - })(); - - m_uas->msg_set_target(ps); - - UAS_FCU(m_uas)->send_message_ignore_drop(ps); - } - - /* -*- mid-level functions -*- */ - - void connection_cb(bool connected) override - { - lock_guard lock(mutex); - if (connected) { - shedule_pull(BOOTUP_TIME_DT); - } - else { - shedule_timer.stop(); - } - } - - void shedule_pull(const ros::Duration &dt) - { - shedule_timer.stop(); - shedule_timer.setPeriod(dt); - shedule_timer.start(); - } - - void shedule_cb(const ros::TimerEvent &event) - { - lock_guard lock(mutex); - if (param_state != PR::IDLE) { - // try later - ROS_DEBUG_NAMED("param", "PR: busy, reshedule pull"); - shedule_pull(BOOTUP_TIME_DT); - } - - ROS_DEBUG_NAMED("param", "PR: start sheduled pull"); - param_state = PR::RXLIST; - param_rx_retries = RETRIES_COUNT; - parameters.clear(); - - restart_timeout_timer(); - param_request_list(); - } - - void timeout_cb(const ros::TimerEvent &event) - { - lock_guard lock(mutex); - if (param_state == PR::RXLIST && param_rx_retries > 0) { - param_rx_retries--; - ROS_WARN_NAMED("param", "PR: request list timeout, retries left %zu", param_rx_retries); - - restart_timeout_timer(); - param_request_list(); - } - else if (param_state == PR::RXPARAM || param_state == PR::RXPARAM_TIMEDOUT) { - if (parameters_missing_idx.empty()) { - ROS_WARN_NAMED("param", "PR: missing list is clear, but we in RXPARAM state, " - "maybe last rerequest fails. Params missed: %zd", - param_count - parameters.size()); - go_idle(); - list_receiving.notify_all(); - return; - } - - param_state = PR::RXPARAM_TIMEDOUT; - uint16_t first_miss_idx = parameters_missing_idx.front(); - if (param_rx_retries > 0) { - param_rx_retries--; - ROS_WARN_NAMED("param", "PR: request param #%u timeout, retries left %zu, and %zu params still missing", - first_miss_idx, param_rx_retries, parameters_missing_idx.size()); - restart_timeout_timer(); - param_request_read("", first_miss_idx); - } - else { - ROS_ERROR_NAMED("param", "PR: request param #%u completely missing.", first_miss_idx); - parameters_missing_idx.pop_front(); - restart_timeout_timer(); - if (!parameters_missing_idx.empty()) { - param_rx_retries = RETRIES_COUNT; - first_miss_idx = parameters_missing_idx.front(); - - ROS_WARN_NAMED("param", "PR: %zu params still missing, trying to request next: #%u", - parameters_missing_idx.size(), first_miss_idx); - param_request_read("", first_miss_idx); - } - } - } - else if (param_state == PR::TXPARAM) { - auto it = set_parameters.begin(); - if (it == set_parameters.end()) { - ROS_DEBUG_NAMED("param", "PR: send list empty, but state TXPARAM"); - go_idle(); - return; - } - - if (it->second->retries_remaining > 0) { - it->second->retries_remaining--; - ROS_WARN_NAMED("param", "PR: Resend param set for %s, retries left %zu", - it->second->param.param_id.c_str(), - it->second->retries_remaining); - restart_timeout_timer(); - param_set(it->second->param); - } - else { - ROS_ERROR_NAMED("param", "PR: Param set for %s timed out.", - it->second->param.param_id.c_str()); - it->second->is_timedout = true; - it->second->ack.notify_all(); - } - } - else { - ROS_DEBUG_NAMED("param", "PR: timeout in IDLE!"); - } - } - - void restart_timeout_timer() - { - is_timedout = false; - timeout_timer.stop(); - timeout_timer.start(); - } - - void go_idle() - { - param_state = PR::IDLE; - timeout_timer.stop(); - } - - bool wait_fetch_all() - { - std::unique_lock lock(list_cond_mutex); - - return list_receiving.wait_for(lock, std::chrono::nanoseconds(LIST_TIMEOUT_DT.toNSec())) - == std::cv_status::no_timeout - && !is_timedout; - } - - bool wait_param_set_ack_for(std::shared_ptr opt) - { - std::unique_lock lock(opt->cond_mutex); - - return opt->ack.wait_for(lock, std::chrono::nanoseconds(PARAM_TIMEOUT_DT.toNSec()) * (RETRIES_COUNT + 2)) - == std::cv_status::no_timeout - && !opt->is_timedout; - } - - bool send_param_set_and_wait(Parameter ¶m) - { - unique_lock lock(mutex); - - // add to waiting list - auto opt = std::make_shared(param, RETRIES_COUNT); - set_parameters[param.param_id] = opt; - - param_state = PR::TXPARAM; - restart_timeout_timer(); - param_set(param); - - lock.unlock(); - bool is_not_timeout = wait_param_set_ack_for(opt); - lock.lock(); - - // free opt data - set_parameters.erase(param.param_id); - - go_idle(); - return is_not_timeout; - } - - //! Set ROS param only if name is good - bool rosparam_set_allowed(const Parameter &p) - { - if (m_uas->is_px4() && p.param_id == "_HASH_CHECK") { - auto v = p.param_value; // const XmlRpcValue can't cast - ROS_INFO_NAMED("param", "PR: PX4 parameter _HASH_CHECK ignored: 0x%8x", static_cast(v)); - return false; - } - - param_nh.setParam(p.param_id, p.param_value); - return true; - } - - /* -*- ROS callbacks -*- */ - - /** - * @brief fetches all parameters from device - * @service ~param/pull - */ - bool pull_cb(mavros_msgs::ParamPull::Request &req, - mavros_msgs::ParamPull::Response &res) - { - unique_lock lock(mutex); - - if ((param_state == PR::IDLE && parameters.empty()) - || req.force_pull) { - if (!req.force_pull) - ROS_DEBUG_NAMED("param", "PR: start pull"); - else - ROS_INFO_NAMED("param", "PR: start force pull"); - - param_state = PR::RXLIST; - param_rx_retries = RETRIES_COUNT; - parameters.clear(); - - shedule_timer.stop(); - restart_timeout_timer(); - param_request_list(); - - lock.unlock(); - res.success = wait_fetch_all(); - } - else if (param_state == PR::RXLIST || param_state == PR::RXPARAM || param_state == PR::RXPARAM_TIMEDOUT) { - lock.unlock(); - res.success = wait_fetch_all(); - } - else { - lock.unlock(); - res.success = true; - } - - lock.lock(); - res.param_received = parameters.size(); - - for (auto p : parameters) { - lock.unlock(); - rosparam_set_allowed(p.second); - lock.lock(); - } - - return true; - } - - /** - * @brief push all parameter value to device - * @service ~param/push - */ - bool push_cb(mavros_msgs::ParamPush::Request &req, - mavros_msgs::ParamPush::Response &res) - { - XmlRpc::XmlRpcValue param_dict; - if (!param_nh.getParam("", param_dict)) - return true; - - ROS_ASSERT(param_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct); - - int tx_count = 0; - for (auto ¶m : param_dict) { - if (Parameter::check_exclude_param_id(param.first)) { - ROS_DEBUG_STREAM_NAMED("param", "PR: Exclude param: " << param.first); - continue; - } - - unique_lock lock(mutex); - auto param_it = parameters.find(param.first); - if (param_it != parameters.end()) { - // copy current state of Parameter - auto to_send = param_it->second; - - // Update XmlRpcValue - to_send.param_value = param.second; - - lock.unlock(); - bool set_res = send_param_set_and_wait(to_send); - lock.lock(); - - if (set_res) - tx_count++; - } - else { - ROS_WARN_STREAM_NAMED("param", "PR: Unknown rosparam: " << param.first); - } - } - - res.success = true; - res.param_transfered = tx_count; - - return true; - } - - /** - * @brief sets parameter value - * @service ~param/set - */ - bool set_cb(mavros_msgs::ParamSet::Request &req, - mavros_msgs::ParamSet::Response &res) - { - unique_lock lock(mutex); - - if (param_state == PR::RXLIST || param_state == PR::RXPARAM || param_state == PR::RXPARAM_TIMEDOUT) { - ROS_ERROR_NAMED("param", "PR: receiving not complete"); - return false; - } - - auto param_it = parameters.find(req.param_id); - if (param_it != parameters.end()) { - auto to_send = param_it->second; - - // according to ParamValue description - if (req.value.integer != 0) - to_send.param_value = static_cast(req.value.integer); - else if (req.value.real != 0.0) - to_send.param_value = req.value.real; - else if (param_it->second.param_value.getType() == XmlRpc::XmlRpcValue::Type::TypeDouble) - to_send.param_value = req.value.real; - else - to_send.param_value = 0; - - lock.unlock(); - res.success = send_param_set_and_wait(to_send); - lock.lock(); - - res.value.integer = param_it->second.to_integer(); - res.value.real = param_it->second.to_real(); - - lock.unlock(); - rosparam_set_allowed(param_it->second); - } - else { - ROS_ERROR_STREAM_NAMED("param", "PR: Unknown parameter to set: " << req.param_id); - res.success = false; - } - - return true; - } - - /** - * @brief get parameter - * @service ~param/get - */ - bool get_cb(mavros_msgs::ParamGet::Request &req, - mavros_msgs::ParamGet::Response &res) - { - lock_guard lock(mutex); - - auto param_it = parameters.find(req.param_id); - if (param_it != parameters.end()) { - res.success = true; - - res.value.integer = param_it->second.to_integer(); - res.value.real = param_it->second.to_real(); - } - else { - ROS_ERROR_STREAM_NAMED("param", "PR: Unknown parameter to get: " << req.param_id); - res.success = false; - } - - return true; - } + using lock_guard = std::lock_guard; + using unique_lock = std::unique_lock; + + std::recursive_mutex mutex; + + rclcpp::Service::SharedPtr pull_srv; + // rclcpp::Service::SharedPtr push_srv; + rclcpp::Service::SharedPtr set_srv; + // rclcpp::Service::SharedPtr get_srv; + + // NOTE(vooon): override standard ROS2 parameter services + rclcpp::Service::SharedPtr get_parameters_srv; + rclcpp::Service::SharedPtr get_parameter_types_srv; + rclcpp::Service::SharedPtr set_parameters_srv; + rclcpp::Service::SharedPtr + set_parameters_atomically_srv; + rclcpp::Service::SharedPtr describe_parameters_srv; + rclcpp::Service::SharedPtr list_parameters_srv; + + rclcpp::Publisher::SharedPtr param_event_pub; + rclcpp::Publisher::SharedPtr std_event_pub; + + rclcpp::TimerBase::SharedPtr schedule_timer; //!< for startup schedule fetch + rclcpp::TimerBase::SharedPtr timeout_timer; //!< for timeout resend + + const std::chrono::nanoseconds BOOTUP_TIME; + const std::chrono::nanoseconds LIST_TIMEOUT; + const std::chrono::nanoseconds PARAM_TIMEOUT; + const int RETRIES_COUNT; + + enum class PR + { + IDLE, + RXLIST, + RXPARAM, + RXPARAM_TIMEDOUT, + TXPARAM + }; + + std::unordered_map parameters; + std::list parameters_missing_idx; + std::unordered_map> set_parameters; + ssize_t param_count; + PR param_state; + + size_t param_rx_retries; + bool is_timedout; + std::mutex list_cond_mutex; + std::condition_variable list_receiving; + + /* -*- message handlers -*- */ + + void handle_param_value( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::PARAM_VALUE & pmsg, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + lock_guard lock(mutex); + + auto lg = get_logger(); + auto param_id = mavlink::to_string(pmsg.param_id); + + auto update_parameter = [this, &pmsg](Parameter & p, bool is_new) { + p.stamp = node->now(); + if (uas->is_ardupilotmega()) { + p.set_value_apm_quirk(pmsg); + } else { + p.set_value(pmsg); + } + + param_event_pub->publish(p.to_event_msg()); + { + rcl_interfaces::msg::ParameterEvent evt{}; + evt.stamp = p.stamp; + evt.node = node->get_fully_qualified_name(); + if (is_new) { + evt.new_parameters.push_back(p.to_parameter_msg()); + } else { + evt.changed_parameters.push_back(p.to_parameter_msg()); + } + + std_event_pub->publish(evt); + } + + // check that ack required + auto set_it = set_parameters.find(p.param_id); + if (set_it != set_parameters.end()) { + set_it->second->promise.set_value({true, p}); + } + + RCLCPP_WARN_STREAM_EXPRESSION( + get_logger(), ((p.param_index != pmsg.param_index && + pmsg.param_index != UINT16_MAX) || + p.param_count != pmsg.param_count), + "PR: Param " << p.to_string() << " different index: " << pmsg.param_index << "/" << + pmsg.param_count); + }; + + // search + auto param_it = parameters.find(param_id); + if (param_it != parameters.end()) { + // parameter exists + auto & p = param_it->second; + + update_parameter(p, false); + RCLCPP_DEBUG_STREAM(lg, "PR: Update param " << p.to_string()); + + } else { + // insert new element + auto pp = + parameters.emplace(param_id, Parameter(param_id, pmsg.param_index, pmsg.param_count)); + auto & p = pp.first->second; + + update_parameter(p, true); + RCLCPP_DEBUG_STREAM(lg, "PR: New param " << p.to_string()); + } + + if (param_state == PR::RXLIST || param_state == PR::RXPARAM || + param_state == PR::RXPARAM_TIMEDOUT) + { + // we received first param. setup list timeout + if (param_state == PR::RXLIST) { + param_count = pmsg.param_count; + param_state = PR::RXPARAM; + + parameters_missing_idx.clear(); + if (param_count != UINT16_MAX) { + RCLCPP_DEBUG(lg, "PR: waiting %zu parameters", param_count); + // declare that all parameters are missing + for (uint16_t idx = 0; idx < param_count; idx++) { + parameters_missing_idx.push_back(idx); + } + } else { + RCLCPP_WARN( + lg, "PR: FCU does not know index for first element! " + "Param list may be truncated."); + } + } + + // trying to avoid endless rerequest loop + // Issue #276 + bool it_is_first_requested = parameters_missing_idx.front() == pmsg.param_index; + + // remove idx for that message + parameters_missing_idx.remove(pmsg.param_index); + + // in receiving mode we use param_rx_retries for LIST and PARAM + if (it_is_first_requested) { + RCLCPP_DEBUG( + lg, "PR: got a value of a requested param idx=%u, " + "resetting retries count", pmsg.param_index); + param_rx_retries = RETRIES_COUNT; + } else if (param_state == PR::RXPARAM_TIMEDOUT) { + RCLCPP_INFO( + lg, "PR: got an unsolicited param value idx=%u, " + "not resetting retries count %zu", pmsg.param_index, param_rx_retries); + } + + restart_timeout_timer(); + + /* index starting from 0, receivig done */ + if (parameters_missing_idx.empty()) { + ssize_t missed = param_count - parameters.size(); + RCLCPP_INFO_EXPRESSION(lg, missed == 0, "PR: parameters list received"); + RCLCPP_WARN_EXPRESSION( + lg, + missed > 0, "PR: parameters list received, but %zd parametars are missed", + missed); + go_idle(); + list_receiving.notify_all(); + + } else if (param_state == PR::RXPARAM_TIMEDOUT) { + uint16_t first_miss_idx = parameters_missing_idx.front(); + RCLCPP_DEBUG(lg, "PR: requesting next timed out parameter idx=%u", first_miss_idx); + param_request_read("", first_miss_idx); + } + } + } + + /* -*- low-level send function -*- */ + + void param_request_list() + { + RCLCPP_DEBUG(get_logger(), "PR:m: request list"); + + mavlink::common::msg::PARAM_REQUEST_LIST rql{}; + uas->msg_set_target(rql); + + uas->send_message(rql); + } + + void param_request_read(const std::string & id, int16_t index = -1) + { + rcpputils::require_true(index >= -1); + + RCLCPP_DEBUG(get_logger(), "PR:m: request '%s', idx %d", id.c_str(), index); + + mavlink::common::msg::PARAM_REQUEST_READ rqr{}; + uas->msg_set_target(rqr); + rqr.param_index = index; + + if (index != -1) { + mavlink::set_string(rqr.param_id, id); + } + + uas->send_message(rqr); + } + + void param_set(const Parameter & param) + { + RCLCPP_DEBUG_STREAM(get_logger(), "PR:m: set param " << param.to_string()); + + // GCC 4.8 can't type out lambda return + auto ps = ([this, ¶m]() -> mavlink::common::msg::PARAM_SET { + if (uas->is_ardupilotmega()) { + return param.to_param_set_apm_qurk(); + } else { + return param.to_param_set(); + } + })(); + + uas->msg_set_target(ps); + + uas->send_message(ps); + } + + /* -*- mid-level functions -*- */ + + void clear_all_parameters() + { + rcl_interfaces::msg::ParameterEvent evt{}; + evt.stamp = node->now(); + evt.node = node->get_fully_qualified_name(); + + for (const auto & p : parameters) { + evt.deleted_parameters.push_back(p.second.to_parameter_msg()); + } + + parameters.clear(); + std_event_pub->publish(evt); + } + + void connection_cb(bool connected) override + { + lock_guard lock(mutex); + + if (connected) { + schedule_pull(); + } else { + schedule_timer->cancel(); + clear_all_parameters(); + } + } + + void schedule_pull() + { + schedule_timer->reset(); + } + + void schedule_cb() + { + lock_guard lock(mutex); + schedule_timer->cancel(); + + if (param_state != PR::IDLE) { + // try later + RCLCPP_DEBUG(get_logger(), "PR: busy, reschedule pull"); + schedule_pull(); + } + + RCLCPP_DEBUG(get_logger(), "PR: start scheduled pull"); + param_state = PR::RXLIST; + param_rx_retries = RETRIES_COUNT; + clear_all_parameters(); + + restart_timeout_timer(); + param_request_list(); + } + + void timeout_cb() + { + lock_guard lock(mutex); + timeout_timer->cancel(); + + auto lg = get_logger(); + + if (param_state == PR::RXLIST && param_rx_retries > 0) { + param_rx_retries--; + RCLCPP_WARN(lg, "PR: request list timeout, retries left %zu", param_rx_retries); + + restart_timeout_timer(); + param_request_list(); + + } else if (param_state == PR::RXPARAM || param_state == PR::RXPARAM_TIMEDOUT) { + if (parameters_missing_idx.empty()) { + RCLCPP_WARN( + lg, "PR: missing list is clear, but we in RXPARAM state, " + "maybe last rerequest fails. Params missed: %zd", + param_count - parameters.size()); + go_idle(); + list_receiving.notify_all(); + return; + } + + param_state = PR::RXPARAM_TIMEDOUT; + uint16_t first_miss_idx = parameters_missing_idx.front(); + if (param_rx_retries > 0) { + param_rx_retries--; + RCLCPP_WARN( + lg, "PR: request param #%u timeout, retries left %zu, and %zu params still missing", + first_miss_idx, param_rx_retries, parameters_missing_idx.size()); + restart_timeout_timer(); + param_request_read("", first_miss_idx); + + } else { + RCLCPP_ERROR(lg, "PR: request param #%u completely missing.", first_miss_idx); + parameters_missing_idx.pop_front(); + restart_timeout_timer(); + if (!parameters_missing_idx.empty()) { + param_rx_retries = RETRIES_COUNT; + first_miss_idx = parameters_missing_idx.front(); + + RCLCPP_WARN( + lg, "PR: %zu params still missing, trying to request next: #%u", + parameters_missing_idx.size(), first_miss_idx); + param_request_read("", first_miss_idx); + } + } + + } else if (param_state == PR::TXPARAM) { + auto it = set_parameters.begin(); + if (it == set_parameters.end()) { + RCLCPP_DEBUG(lg, "PR: send list empty, but state TXPARAM"); + go_idle(); + return; + } + + if (it->second->retries_remaining > 0) { + it->second->retries_remaining--; + RCLCPP_WARN( + lg, "PR: Resend param set for %s, retries left %zu", + it->second->param.param_id.c_str(), + it->second->retries_remaining.load()); + restart_timeout_timer(); + param_set(it->second->param); + + } else { + RCLCPP_ERROR( + lg, "PR: Param set for %s timed out.", + it->second->param.param_id.c_str()); + it->second->promise.set_value({false, it->second->param}); + } + + } else { + RCLCPP_DEBUG(lg, "PR: timeout in IDLE!"); + } + } + + void restart_timeout_timer() + { + is_timedout = false; + timeout_timer->reset(); + } + + void go_idle() + { + param_state = PR::IDLE; + timeout_timer->cancel(); + } + + bool wait_fetch_all() + { + std::unique_lock lock(list_cond_mutex); + + return list_receiving.wait_for(lock, LIST_TIMEOUT) == std::cv_status::no_timeout && + !is_timedout; + } + + ParamSetOpt::Result wait_param_set_ack_for(const std::shared_ptr opt) + { + auto future = opt->promise.get_future(); + + auto wres = future.wait_for(PARAM_TIMEOUT * (RETRIES_COUNT + 2)); + if (wres != std::future_status::ready) { + return {false, opt->param}; + } + + return future.get(); + } + + ParamSetOpt::Result send_param_set_and_wait(const Parameter & param) + { + unique_lock lock(mutex); + + // add to waiting list + auto opt = std::make_shared(param, RETRIES_COUNT); + set_parameters[param.param_id] = opt; + + param_state = PR::TXPARAM; + restart_timeout_timer(); + param_set(param); + + lock.unlock(); + auto res = wait_param_set_ack_for(opt); + lock.lock(); + + // free opt data + set_parameters.erase(param.param_id); + + go_idle(); + return res; + } + + //! Find and copy parameter + // Would return new empty instance for unknown id's + Parameter copy_parameter(const std::string & param_id) + { + unique_lock lock(mutex); + + auto it = parameters.find(param_id); + if (it == parameters.end()) { + return Parameter(param_id); + } + + return it->second; + } + + /* -*- ROS callbacks -*- */ + + /** + * @brief fetches all parameters from device + * @service ~param/pull + */ + void pull_cb( + const mavros_msgs::srv::ParamPull::Request::SharedPtr req, + mavros_msgs::srv::ParamPull::Response::SharedPtr res) + { + unique_lock lock(mutex); + + auto is_in_progress = [&]() -> bool { + return + param_state == PR::RXLIST || + param_state == PR::RXPARAM || + param_state == PR::RXPARAM_TIMEDOUT + ; + }; + + if ((param_state == PR::IDLE && parameters.empty()) || + req->force_pull) + { + if (!req->force_pull) { + RCLCPP_DEBUG(get_logger(), "PR: start pull"); + } else { + RCLCPP_INFO(get_logger(), "PR: start force pull"); + } + + param_state = PR::RXLIST; + param_rx_retries = RETRIES_COUNT; + clear_all_parameters(); + + schedule_timer->cancel(); + restart_timeout_timer(); + param_request_list(); + + lock.unlock(); + res->success = wait_fetch_all(); + + } else if (is_in_progress()) { + lock.unlock(); + res->success = wait_fetch_all(); + + } else { + lock.unlock(); + res->success = true; + } + + lock.lock(); + res->param_received = parameters.size(); + } + + /** + * @brief sets parameter value + * @service ~param/set + */ + void set_cb( + const mavros_msgs::srv::ParamSetV2::Request::SharedPtr req, + mavros_msgs::srv::ParamSetV2::Response::SharedPtr res) + { + { + unique_lock lock(mutex); + + if (param_state == PR::RXLIST || param_state == PR::RXPARAM || + param_state == PR::RXPARAM_TIMEDOUT) + { + RCLCPP_ERROR(get_logger(), "PR: receiving not complete"); + // throw std::runtime_error("receiving in progress"); + res->success = false; + return; + } + lock.unlock(); + } + + if (Parameter::check_exclude_param_id(req->param_id) && !req->force_set) { + RCLCPP_INFO_STREAM(get_logger(), "PR: parameter set excluded: " << req->param_id); + res->success = false; + return; + } + + auto to_send = copy_parameter(req->param_id); + if (to_send.param_value.get_type() == rclcpp::PARAMETER_NOT_SET && !req->force_set) { + RCLCPP_ERROR_STREAM(get_logger(), "PR: Unknown parameter to set: " << req->param_id); + res->success = false; + return; + } + + to_send.param_value = rclcpp::ParameterValue(req->value); + auto sres = send_param_set_and_wait(to_send); + res->success = sres.success; + res->value = sres.param.param_value.to_value_msg(); + } + + /** + * @brief Get parameters (std) + */ + void get_parameters_cb( + const rcl_interfaces::srv::GetParameters::Request::SharedPtr req, + rcl_interfaces::srv::GetParameters::Response::SharedPtr res) + { + unique_lock lock(mutex); + + for (auto name : req->names) { + auto it = parameters.find(name); + if (it == parameters.end()) { + RCLCPP_WARN_STREAM(get_logger(), "PR: Failed to get parameter type: " << name); + // return PARAMETER_NOT_SET + res->values.emplace_back(); + continue; + } + + res->values.push_back(it->second.param_value.to_value_msg()); + } + } + + /** + * @brief Get parameter types (std) + */ + void get_parameter_types_cb( + const rcl_interfaces::srv::GetParameterTypes::Request::SharedPtr req, + rcl_interfaces::srv::GetParameterTypes::Response::SharedPtr res) + { + unique_lock lock(mutex); + + for (auto name : req->names) { + auto it = parameters.find(name); + if (it == parameters.end()) { + RCLCPP_WARN_STREAM(get_logger(), "PR: Failed to get parameter type: " << name); + res->types.emplace_back(rclcpp::PARAMETER_NOT_SET); + continue; + } + + res->types.emplace_back(it->second.param_value.get_type()); + } + } + + /** + * @brief Set parameters (std) + */ + void set_parameters_cb( + const rcl_interfaces::srv::SetParameters::Request::SharedPtr req, + rcl_interfaces::srv::SetParameters::Response::SharedPtr res) + { + for (const auto & p : req->parameters) { + rcl_interfaces::msg::SetParametersResult result; + if (Parameter::check_exclude_param_id(p.name)) { + RCLCPP_WARN_STREAM(get_logger(), "PR: parameter set excluded: " << p.name); + + result.successful = false; + result.reason = "Parameter excluded. " + "Use ~/set with force if you really need to send that value"; + res->results.push_back(result); + continue; + } + + auto to_send = copy_parameter(p.name); + if (to_send.param_value.get_type() == rclcpp::PARAMETER_NOT_SET) { + RCLCPP_ERROR_STREAM(get_logger(), "PR: Unknown parameter to set: " << p.name); + + result.successful = false; + result.reason = "Undeclared parameter"; + res->results.push_back(result); + continue; + } + + to_send.param_value = rclcpp::ParameterValue(p.value); + + // NOTE(vooon): It is possible to fill-in send list at once. + // But then it's hard to wait for each result. + auto sres = send_param_set_and_wait(to_send); + result.successful = sres.success; + res->results.push_back(result); + } + } + + /** + * @brief Get parameters atomically (std) + */ + void set_parameters_atomically_cb( + const rcl_interfaces::srv::SetParametersAtomically::Request::SharedPtr req [[maybe_unused]], + rcl_interfaces::srv::SetParametersAtomically::Response::SharedPtr res) + { + RCLCPP_ERROR(get_logger(), "PR: Client calls unsupported ~/set_parameters_atomically"); + res->result.successful = false; + res->result.reason = "device do not support atomic set"; + } + + /** + * @brief Describe parameters (std) + */ + void describe_parameters_cb( + const rcl_interfaces::srv::DescribeParameters::Request::SharedPtr req, + rcl_interfaces::srv::DescribeParameters::Response::SharedPtr res) + { + unique_lock lock(mutex); + + for (auto name : req->names) { + auto it = parameters.find(name); + if (it == parameters.end()) { + RCLCPP_WARN_STREAM( + get_logger(), "PR: Failed to describe parameters: " << name); + res->descriptors.emplace_back(); + continue; + } + + res->descriptors.push_back(it->second.to_descriptor()); + } + } + + /** + * @brief List parameters (std) + */ + void list_parameters_cb( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr req [[maybe_unused]], + rcl_interfaces::srv::ListParameters::Response::SharedPtr res) + { + unique_lock lock(mutex); + + // NOTE(vooon): all fcu parameters are "flat", so no need to check req prefixes or depth + for (auto p : parameters) { + res->result.names.emplace_back(p.first); + } + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::ParamPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::ParamPlugin) diff --git a/mavros/src/plugins/rallypoint.cpp b/mavros/src/plugins/rallypoint.cpp index d67504be4..08c85a963 100644 --- a/mavros/src/plugins/rallypoint.cpp +++ b/mavros/src/plugins/rallypoint.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2021 Charlie Burge. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Rallypoint plugin * @file rallypoint.cpp @@ -6,239 +13,207 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2021 Charlie Burge. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include "mavros/mission_protocol_base.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT -namespace mavros { -namespace std_plugins { /** * @brief Rallypoint manipulation plugin + * @plugin rallypoint */ -class RallypointPlugin : public plugin::MissionBase { +class RallypointPlugin : public plugin::MissionBase +{ public: - RallypointPlugin() : - MissionBase("RP"), - rp_nh("~rallypoint") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - MissionBase::initialize_with_nodehandle(&rp_nh); - - wp_state = WP::IDLE; - wp_type = plugin::WP_TYPE::RALLY; - - rp_nh.param("pull_after_gcs", do_pull_after_gcs, true); - rp_nh.param("use_mission_item_int", use_mission_item_int, false); - - rp_list_pub = rp_nh.advertise("waypoints", 2, true); - pull_srv = rp_nh.advertiseService("pull", &RallypointPlugin::pull_cb, this); - push_srv = rp_nh.advertiseService("push", &RallypointPlugin::push_cb, this); - clear_srv = rp_nh.advertiseService("clear", &RallypointPlugin::clear_cb, this); - - enable_connection_cb(); - enable_capabilities_cb(); - } - - Subscriptions get_subscriptions() override { - return { - make_handler(&RallypointPlugin::handle_mission_item), - make_handler(&RallypointPlugin::handle_mission_item_int), - make_handler(&RallypointPlugin::handle_mission_request), - make_handler(&RallypointPlugin::handle_mission_request_int), - make_handler(&RallypointPlugin::handle_mission_count), - make_handler(&RallypointPlugin::handle_mission_ack), - }; - } + explicit RallypointPlugin(plugin::UASPtr uas_) + : MissionBase(uas_, "rallypoint", plugin::MTYPE::RALLY, "RP", 20s) + { + enable_node_watch_parameters(); + + // NOTE(vooon): I'm not quite sure that this option would work with mavros router + node_declare_and_watch_parameter( + "pull_after_gcs", true, [&](const rclcpp::Parameter & p) { + do_pull_after_gcs = p.as_bool(); + }); + + node_declare_and_watch_parameter( + "use_mission_item_int", true, [&](const rclcpp::Parameter & p) { + use_mission_item_int = p.as_bool(); + }); + + auto rp_qos = rclcpp::QoS(10).transient_local(); + + rp_list_pub = node->create_publisher("~/rallypoints", rp_qos); + + pull_srv = + node->create_service( + "~/pull", + std::bind(&RallypointPlugin::pull_cb, this, _1, _2)); + push_srv = + node->create_service( + "~/push", + std::bind(&RallypointPlugin::push_cb, this, _1, _2)); + clear_srv = + node->create_service( + "~/clear", + std::bind(&RallypointPlugin::clear_cb, this, _1, _2)); + + enable_connection_cb(); + enable_capabilities_cb(); + } private: - ros::NodeHandle rp_nh; - - ros::Publisher rp_list_pub; - ros::ServiceServer pull_srv; - ros::ServiceServer push_srv; - ros::ServiceServer clear_srv; - - /* -*- mid-level helpers -*- */ - - // Acts when capabilities of the fcu are changed - void capabilities_cb(UAS::MAV_CAP capabilities) override { - lock_guard lock(mutex); - if (m_uas->has_capability(UAS::MAV_CAP::MISSION_INT)) { - use_mission_item_int = true; - mission_item_int_support_confirmed = true; - ROS_INFO_NAMED(log_ns, "%s: Using MISSION_ITEM_INT", log_ns.c_str()); - } else { - use_mission_item_int = false; - mission_item_int_support_confirmed = false; - ROS_WARN_NAMED(log_ns, "%s: Falling back to MISSION_ITEM", log_ns.c_str()); - } - } - - // Act on first heartbeat from FCU - void connection_cb(bool connected) override - { - lock_guard lock(mutex); - if (connected) { - schedule_pull(BOOTUP_TIME_DT); - - if (rp_nh.hasParam("enable_partial_push")) { - rp_nh.getParam("enable_partial_push", enable_partial_push); - } - else { - enable_partial_push = m_uas->is_ardupilotmega(); - } - } - else { - schedule_timer.stop(); - } - } - - //! @brief publish the updated waypoint list after operation - void publish_waypoints() override - { - auto wpl = boost::make_shared(); - unique_lock lock(mutex); - - wpl->current_seq = wp_cur_active; - wpl->waypoints.clear(); - wpl->waypoints.reserve(waypoints.size()); - for (auto &it : waypoints) { - wpl->waypoints.push_back(it); - } - - lock.unlock(); - rp_list_pub.publish(wpl); - } - - /* -*- ROS callbacks -*- */ - - bool pull_cb(mavros_msgs::WaypointPull::Request &req, - mavros_msgs::WaypointPull::Response &res) - { - unique_lock lock(mutex); - - if (wp_state != WP::IDLE) - // Wrong initial state, other operation in progress? - return false; - - wp_state = WP::RXLIST; - wp_count = 0; - restart_timeout_timer(); - - lock.unlock(); - mission_request_list(); - res.success = wait_fetch_all(); - lock.lock(); - - res.wp_received = waypoints.size(); - go_idle(); // not nessessary, but prevents from blocking - return true; - } - - bool push_cb(mavros_msgs::WaypointPush::Request &req, - mavros_msgs::WaypointPush::Response &res) - { - unique_lock lock(mutex); - - if (wp_state != WP::IDLE) - // Wrong initial state, other operation in progress? - return false; - - if (req.start_index) { - // Partial Waypoint update - - if (!enable_partial_push) { - ROS_WARN_NAMED(log_ns, "%s: Partial Push not enabled. (Only supported on APM)", log_ns.c_str()); - res.success = false; - res.wp_transfered = 0; - return true; - } - - if (waypoints.size() < req.start_index + req.waypoints.size()) { - ROS_WARN_NAMED(log_ns, "%s: Partial push out of range rejected.", log_ns.c_str()); - res.success = false; - res.wp_transfered = 0; - return true; - } - - wp_state = WP::TXPARTIAL; - send_waypoints = waypoints; - - uint16_t seq = req.start_index; - for (auto &it : req.waypoints) { - send_waypoints[seq] = it; - seq++; - } - - wp_count = req.waypoints.size(); - wp_start_id = req.start_index; - wp_end_id = req.start_index + wp_count; - wp_cur_id = req.start_index; - restart_timeout_timer(); - - lock.unlock(); - mission_write_partial_list(wp_start_id, wp_end_id); - res.success = wait_push_all(); - lock.lock(); - - res.wp_transfered = wp_cur_id - wp_start_id + 1; - } - else { - // Full waypoint update - wp_state = WP::TXLIST; - - send_waypoints.clear(); - send_waypoints.reserve(req.waypoints.size()); - send_waypoints = req.waypoints; - - wp_count = send_waypoints.size(); - wp_end_id = wp_count; - wp_cur_id = 0; - restart_timeout_timer(); - - lock.unlock(); - mission_count(wp_count); - res.success = wait_push_all(); - lock.lock(); - - res.wp_transfered = wp_cur_id + 1; - } - - go_idle(); // same as in pull_cb - return true; - } - - bool clear_cb(mavros_msgs::WaypointClear::Request &req, - mavros_msgs::WaypointClear::Response &res) - { - unique_lock lock(mutex); - - if (wp_state != WP::IDLE) - return false; - - wp_state = WP::CLEAR; - restart_timeout_timer(); - - lock.unlock(); - mission_clear_all(); - res.success = wait_push_all(); - - lock.lock(); - go_idle(); // same as in pull_cb - return true; - } + rclcpp::Publisher::SharedPtr rp_list_pub; + + rclcpp::Service::SharedPtr pull_srv; + rclcpp::Service::SharedPtr push_srv; + rclcpp::Service::SharedPtr clear_srv; + + /* -*- mid-level helpers -*- */ + + // Acts when capabilities of the fcu are changed + void capabilities_cb(uas::MAV_CAP capabilities [[maybe_unused]]) override + { + lock_guard lock(mutex); + + if (uas->has_capability(uas::MAV_CAP::MISSION_INT)) { + use_mission_item_int = true; + mission_item_int_support_confirmed = true; + RCLCPP_INFO(get_logger(), "%s: Using MISSION_ITEM_INT", log_prefix); + } else { + use_mission_item_int = false; + mission_item_int_support_confirmed = false; + RCLCPP_WARN(get_logger(), "%s: Falling back to MISSION_ITEM", log_prefix); + } + } + + // Act on first heartbeat from FCU + void connection_cb(bool connected) override + { + lock_guard lock(mutex); + + if (connected) { + schedule_pull(BOOTUP_TIME); + } else if (schedule_timer) { + schedule_timer->cancel(); + } + } + + //! @brief publish the updated waypoint list after operation + void publish_waypoints() override + { + auto wpl = mavros_msgs::msg::WaypointList(); + unique_lock lock(mutex); + + wpl.current_seq = wp_cur_active; + wpl.waypoints.reserve(waypoints.size()); + for (auto & it : waypoints) { + wpl.waypoints.push_back(it); + } + + lock.unlock(); + rp_list_pub->publish(wpl); + } + + void publish_reached(const uint16_t seq [[maybe_unused]]) override + {} + + /* -*- ROS callbacks -*- */ + + void pull_cb( + const mavros_msgs::srv::WaypointPull::Request::SharedPtr req [[maybe_unused]], + mavros_msgs::srv::WaypointPull::Response::SharedPtr res) + { + unique_lock lock(mutex); + + if (wp_state != WP::IDLE) { + // Wrong initial state, other operation in progress? + return; + } + + wp_state = WP::RXLIST; + wp_count = 0; + restart_timeout_timer(); + + lock.unlock(); + mission_request_list(); + res->success = wait_fetch_all(); + lock.lock(); + + res->wp_received = waypoints.size(); + go_idle(); // not nessessary, but prevents from blocking + } + + void push_cb( + const mavros_msgs::srv::WaypointPush::Request::SharedPtr req, + mavros_msgs::srv::WaypointPush::Response::SharedPtr res) + { + unique_lock lock(mutex); + + if (wp_state != WP::IDLE) { + // Wrong initial state, other operation in progress? + return; + } + + if (req->start_index) { + // Partial update not supported for rallypoints + RCLCPP_WARN(get_logger(), "%s: Partial update for rallypoints not supported", log_prefix); + res->success = false; + return; + } + + // Full waypoint update + wp_state = WP::TXLIST; + + send_waypoints.clear(); + send_waypoints.reserve(req->waypoints.size()); + for (auto & wp : req->waypoints) { + send_waypoints.emplace_back(wp); + } + + wp_count = send_waypoints.size(); + wp_end_id = wp_count; + wp_cur_id = 0; + restart_timeout_timer(); + + lock.unlock(); + mission_count(wp_count); + res->success = wait_push_all(); + lock.lock(); + + res->wp_transfered = wp_cur_id + 1; + go_idle(); // same as in pull_cb + } + + void clear_cb( + const mavros_msgs::srv::WaypointClear::Request::SharedPtr req [[maybe_unused]], + mavros_msgs::srv::WaypointClear::Response::SharedPtr res) + { + unique_lock lock(mutex); + + if (wp_state != WP::IDLE) { + return; + } + + wp_state = WP::CLEAR; + restart_timeout_timer(); + + lock.unlock(); + mission_clear_all(); + res->success = wait_push_all(); + + lock.lock(); + go_idle(); // same as in pull_cb + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::RallypointPlugin, mavros::plugin::PluginBase) \ No newline at end of file +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::RallypointPlugin) diff --git a/mavros/src/plugins/rc_io.cpp b/mavros/src/plugins/rc_io.cpp index c62d76cd7..3f3e3ac6b 100644 --- a/mavros/src/plugins/rc_io.cpp +++ b/mavros/src/plugins/rc_io.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2014,2015,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief RC IO plugin * @file rc_io.cpp @@ -6,263 +13,282 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014,2015,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include -#include -#include -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/rc_in.hpp" +#include "mavros_msgs/msg/rc_out.hpp" +#include "mavros_msgs/msg/override_rc_in.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace std_plugins { /** * @brief RC IO plugin + * @plugin rc_io */ -class RCIOPlugin : public plugin::PluginBase { +class RCIOPlugin : public plugin::Plugin +{ public: - RCIOPlugin() : PluginBase(), - rc_nh("~rc"), - raw_rc_in(0), - raw_rc_out(0), - has_rc_channels_msg(false) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - rc_in_pub = rc_nh.advertise("in", 10); - rc_out_pub = rc_nh.advertise("out", 10); - override_sub = rc_nh.subscribe("override", 10, &RCIOPlugin::override_cb, this); - - enable_connection_cb(); - }; - - Subscriptions get_subscriptions() override { - return { - make_handler(&RCIOPlugin::handle_rc_channels_raw), - make_handler(&RCIOPlugin::handle_rc_channels), - make_handler(&RCIOPlugin::handle_servo_output_raw), - }; - } + explicit RCIOPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "rc"), + raw_rc_in(0), + raw_rc_out(0), + has_rc_channels_msg(false) + { + rc_in_pub = node->create_publisher("~/in", 10); + rc_out_pub = node->create_publisher("~/out", 10); + override_sub = node->create_subscription( + "~/override", 10, std::bind( + &RCIOPlugin::override_cb, this, + _1)); + + enable_connection_cb(); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&RCIOPlugin::handle_rc_channels_raw), + make_handler(&RCIOPlugin::handle_rc_channels), + make_handler(&RCIOPlugin::handle_servo_output_raw), + }; + } private: - using lock_guard = std::lock_guard; - std::mutex mutex; - ros::NodeHandle rc_nh; - - std::vector raw_rc_in; - std::vector raw_rc_out; - std::atomic has_rc_channels_msg; - - ros::Publisher rc_in_pub; - ros::Publisher rc_out_pub; - ros::Subscriber override_sub; - - /* -*- rx handlers -*- */ - - void handle_rc_channels_raw(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RC_CHANNELS_RAW &port) - { - /* if we receive RC_CHANNELS, drop RC_CHANNELS_RAW */ - if (has_rc_channels_msg) - return; - - lock_guard lock(mutex); - - size_t offset = port.port * 8; - if (raw_rc_in.size() < offset + 8) - raw_rc_in.resize(offset + 8); - - // [[[cog: - // import cog - // for i in range(1, 9): - // cog.outl("raw_rc_in[offset + %d] = port.chan%d_raw;" % (i - 1, i)) - // ]]] - raw_rc_in[offset + 0] = port.chan1_raw; - raw_rc_in[offset + 1] = port.chan2_raw; - raw_rc_in[offset + 2] = port.chan3_raw; - raw_rc_in[offset + 3] = port.chan4_raw; - raw_rc_in[offset + 4] = port.chan5_raw; - raw_rc_in[offset + 5] = port.chan6_raw; - raw_rc_in[offset + 6] = port.chan7_raw; - raw_rc_in[offset + 7] = port.chan8_raw; - // [[[end]]] (checksum: fcb14b1ddfff9ce7dd02f5bd03825cff) - - auto rcin_msg = boost::make_shared(); - - rcin_msg->header.stamp = m_uas->synchronise_stamp(port.time_boot_ms); - rcin_msg->rssi = port.rssi; - rcin_msg->channels = raw_rc_in; - - rc_in_pub.publish(rcin_msg); - } - - void handle_rc_channels(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RC_CHANNELS &channels) - { - constexpr size_t MAX_CHANCNT = 18; - lock_guard lock(mutex); - - ROS_INFO_COND_NAMED(!has_rc_channels_msg, "rc", "RC_CHANNELS message detected!"); - has_rc_channels_msg = true; - - if (channels.chancount > MAX_CHANCNT) { - ROS_WARN_THROTTLE_NAMED(60, "rc", - "FCU receives %u RC channels, but RC_CHANNELS can store %zu", - channels.chancount, MAX_CHANCNT); - - channels.chancount = MAX_CHANCNT; - } - - raw_rc_in.resize(channels.chancount); - - // switch works as start point selector. - switch (channels.chancount) { - // [[[cog: - // for i in range(18, 0, -1): - // cog.outl("case %2d: raw_rc_in[%2d] = channels.chan%d_raw;" % (i, i - 1, i)) - // ]]] - case 18: raw_rc_in[17] = channels.chan18_raw; - case 17: raw_rc_in[16] = channels.chan17_raw; - case 16: raw_rc_in[15] = channels.chan16_raw; - case 15: raw_rc_in[14] = channels.chan15_raw; - case 14: raw_rc_in[13] = channels.chan14_raw; - case 13: raw_rc_in[12] = channels.chan13_raw; - case 12: raw_rc_in[11] = channels.chan12_raw; - case 11: raw_rc_in[10] = channels.chan11_raw; - case 10: raw_rc_in[ 9] = channels.chan10_raw; - case 9: raw_rc_in[ 8] = channels.chan9_raw; - case 8: raw_rc_in[ 7] = channels.chan8_raw; - case 7: raw_rc_in[ 6] = channels.chan7_raw; - case 6: raw_rc_in[ 5] = channels.chan6_raw; - case 5: raw_rc_in[ 4] = channels.chan5_raw; - case 4: raw_rc_in[ 3] = channels.chan4_raw; - case 3: raw_rc_in[ 2] = channels.chan3_raw; - case 2: raw_rc_in[ 1] = channels.chan2_raw; - case 1: raw_rc_in[ 0] = channels.chan1_raw; - // [[[end]]] (checksum: 56e9ab5407bd2c864abde230a6cf3fed) - case 0: break; - } - - auto rcin_msg = boost::make_shared(); - - rcin_msg->header.stamp = m_uas->synchronise_stamp(channels.time_boot_ms); - rcin_msg->rssi = channels.rssi; - rcin_msg->channels = raw_rc_in; - - rc_in_pub.publish(rcin_msg); - } - - void handle_servo_output_raw(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SERVO_OUTPUT_RAW &port) - { - lock_guard lock(mutex); - - uint8_t num_channels; - - // If using Mavlink protocol v2, number of available servo channels is 16; - // otherwise, 8 - if (msg->magic == MAVLINK_STX) - num_channels = 16; - else - num_channels = 8; - - size_t offset = port.port * num_channels; - if (raw_rc_out.size() < offset + num_channels) - raw_rc_out.resize(offset + num_channels); - - // [[[cog: - // for i in range(1, 9): - // cog.outl("raw_rc_out[offset + %d] = port.servo%d_raw;" % (i - 1, i)) - // ]]] - raw_rc_out[offset + 0] = port.servo1_raw; - raw_rc_out[offset + 1] = port.servo2_raw; - raw_rc_out[offset + 2] = port.servo3_raw; - raw_rc_out[offset + 3] = port.servo4_raw; - raw_rc_out[offset + 4] = port.servo5_raw; - raw_rc_out[offset + 5] = port.servo6_raw; - raw_rc_out[offset + 6] = port.servo7_raw; - raw_rc_out[offset + 7] = port.servo8_raw; - // [[[end]]] (checksum: 946d524fe9fbaa3e52fbdf8a905fbf0f) - if (msg->magic == MAVLINK_STX) { - // [[[cog: - // for i in range(9, 17): - // cog.outl("raw_rc_out[offset + %d] = port.servo%d_raw;" % (i - 1, i)) - // ]]] - raw_rc_out[offset + 8] = port.servo9_raw; - raw_rc_out[offset + 9] = port.servo10_raw; - raw_rc_out[offset + 10] = port.servo11_raw; - raw_rc_out[offset + 11] = port.servo12_raw; - raw_rc_out[offset + 12] = port.servo13_raw; - raw_rc_out[offset + 13] = port.servo14_raw; - raw_rc_out[offset + 14] = port.servo15_raw; - raw_rc_out[offset + 15] = port.servo16_raw; - // [[[end]]] (checksum: 60a386cba6faa126ee7dfe1b22f50398) - } - - auto rcout_msg = boost::make_shared(); - - // XXX: Why time_usec is 32 bit? We should test that. - uint64_t time_usec = port.time_usec; - - rcout_msg->header.stamp = m_uas->synchronise_stamp(time_usec); - rcout_msg->channels = raw_rc_out; - - rc_out_pub.publish(rcout_msg); - } - - /* -*- callbacks -*- */ - - void connection_cb(bool connected) override - { - lock_guard lock(mutex); - raw_rc_in.clear(); - raw_rc_out.clear(); - has_rc_channels_msg = false; - } - - void override_cb(const mavros_msgs::OverrideRCIn::ConstPtr req) - { - if (!m_uas->is_ardupilotmega() && !m_uas->is_px4()) - ROS_WARN_THROTTLE_NAMED(30, "rc", "RC override not supported by this FCU!"); - - mavlink::common::msg::RC_CHANNELS_OVERRIDE ovr = {}; - ovr.target_system = m_uas->get_tgt_system(); - ovr.target_component = m_uas->get_tgt_component(); - - // [[[cog: - // for i in range(1, 19): - // cog.outl("ovr.chan%d_raw = req->channels[%d];" % (i, i - 1)) - // ]]] - ovr.chan1_raw = req->channels[0]; - ovr.chan2_raw = req->channels[1]; - ovr.chan3_raw = req->channels[2]; - ovr.chan4_raw = req->channels[3]; - ovr.chan5_raw = req->channels[4]; - ovr.chan6_raw = req->channels[5]; - ovr.chan7_raw = req->channels[6]; - ovr.chan8_raw = req->channels[7]; - ovr.chan9_raw = req->channels[8]; - ovr.chan10_raw = req->channels[9]; - ovr.chan11_raw = req->channels[10]; - ovr.chan12_raw = req->channels[11]; - ovr.chan13_raw = req->channels[12]; - ovr.chan14_raw = req->channels[13]; - ovr.chan15_raw = req->channels[14]; - ovr.chan16_raw = req->channels[15]; - ovr.chan17_raw = req->channels[16]; - ovr.chan18_raw = req->channels[17]; - // [[[end]]] (checksum: 3c9f7f1ce77a49651345daab995ea70e) - UAS_FCU(m_uas)->send_message_ignore_drop(ovr); - } + using lock_guard = std::lock_guard; + std::mutex mutex; + + std::vector raw_rc_in; + std::vector raw_rc_out; + std::atomic has_rc_channels_msg; + + rclcpp::Publisher::SharedPtr rc_in_pub; + rclcpp::Publisher::SharedPtr rc_out_pub; + rclcpp::Subscription::SharedPtr override_sub; + + /* -*- rx handlers -*- */ + + void handle_rc_channels_raw( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::RC_CHANNELS_RAW & port, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + /* if we receive RC_CHANNELS, drop RC_CHANNELS_RAW */ + if (has_rc_channels_msg) { + return; + } + + lock_guard lock(mutex); + + size_t offset = port.port * 8; + if (raw_rc_in.size() < offset + 8) { + raw_rc_in.resize(offset + 8); + } + + // [[[cog: + // for i in range(1, 9): + // cog.outl(f"raw_rc_in[offset + {i - 1}] = port.chan{i}_raw;") + // ]]] + raw_rc_in[offset + 0] = port.chan1_raw; + raw_rc_in[offset + 1] = port.chan2_raw; + raw_rc_in[offset + 2] = port.chan3_raw; + raw_rc_in[offset + 3] = port.chan4_raw; + raw_rc_in[offset + 4] = port.chan5_raw; + raw_rc_in[offset + 5] = port.chan6_raw; + raw_rc_in[offset + 6] = port.chan7_raw; + raw_rc_in[offset + 7] = port.chan8_raw; + // [[[end]]] (checksum: 7ae5a061d1f05239433e9a78b4b1887a) + + auto rcin_msg = mavros_msgs::msg::RCIn(); + rcin_msg.header.stamp = uas->synchronise_stamp(port.time_boot_ms); + rcin_msg.rssi = port.rssi; + rcin_msg.channels = raw_rc_in; + + rc_in_pub->publish(rcin_msg); + } + + void handle_rc_channels( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::RC_CHANNELS & channels, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + constexpr size_t MAX_CHANCNT = 18; + lock_guard lock(mutex); + + RCLCPP_INFO_EXPRESSION( + get_logger(), !has_rc_channels_msg.exchange( + true), "RC_CHANNELS message detected!"); + + if (channels.chancount > MAX_CHANCNT) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 60000, + "FCU receives %u RC channels, but RC_CHANNELS can store %zu", + channels.chancount, MAX_CHANCNT); + + channels.chancount = MAX_CHANCNT; + } + + raw_rc_in.resize(channels.chancount); + + // switch works as start point selector. + switch (channels.chancount) { + // [[[cog: + // for i in range(18, 0, -1): + // cog.outl(f"case {i}: raw_rc_in[{i - 1}] = channels.chan{i}_raw; [[fallthrough]];") + // ]]] + case 18: raw_rc_in[17] = channels.chan18_raw; [[fallthrough]]; + case 17: raw_rc_in[16] = channels.chan17_raw; [[fallthrough]]; + case 16: raw_rc_in[15] = channels.chan16_raw; [[fallthrough]]; + case 15: raw_rc_in[14] = channels.chan15_raw; [[fallthrough]]; + case 14: raw_rc_in[13] = channels.chan14_raw; [[fallthrough]]; + case 13: raw_rc_in[12] = channels.chan13_raw; [[fallthrough]]; + case 12: raw_rc_in[11] = channels.chan12_raw; [[fallthrough]]; + case 11: raw_rc_in[10] = channels.chan11_raw; [[fallthrough]]; + case 10: raw_rc_in[9] = channels.chan10_raw; [[fallthrough]]; + case 9: raw_rc_in[8] = channels.chan9_raw; [[fallthrough]]; + case 8: raw_rc_in[7] = channels.chan8_raw; [[fallthrough]]; + case 7: raw_rc_in[6] = channels.chan7_raw; [[fallthrough]]; + case 6: raw_rc_in[5] = channels.chan6_raw; [[fallthrough]]; + case 5: raw_rc_in[4] = channels.chan5_raw; [[fallthrough]]; + case 4: raw_rc_in[3] = channels.chan4_raw; [[fallthrough]]; + case 3: raw_rc_in[2] = channels.chan3_raw; [[fallthrough]]; + case 2: raw_rc_in[1] = channels.chan2_raw; [[fallthrough]]; + case 1: raw_rc_in[0] = channels.chan1_raw; [[fallthrough]]; + // [[[end]]] (checksum: d88127d1dce2f6b42244d905e5686cb3) + case 0: break; + } + + auto rcin_msg = mavros_msgs::msg::RCIn(); + + rcin_msg.header.stamp = uas->synchronise_stamp(channels.time_boot_ms); + rcin_msg.rssi = channels.rssi; + rcin_msg.channels = raw_rc_in; + + rc_in_pub->publish(rcin_msg); + } + + void handle_servo_output_raw( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::SERVO_OUTPUT_RAW & port, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + lock_guard lock(mutex); + + uint8_t num_channels; + + // If using Mavlink protocol v2, number of available servo channels is 16; + // otherwise, 8 + if (msg->magic == MAVLINK_STX) { + num_channels = 16; + } else { + num_channels = 8; + } + + size_t offset = port.port * num_channels; + if (raw_rc_out.size() < offset + num_channels) { + raw_rc_out.resize(offset + num_channels); + } + + // [[[cog: + // for i in range(1, 9): + // cog.outl(f"raw_rc_out[offset + {i - 1}] = port.servo{i}_raw;") + // ]]] + raw_rc_out[offset + 0] = port.servo1_raw; + raw_rc_out[offset + 1] = port.servo2_raw; + raw_rc_out[offset + 2] = port.servo3_raw; + raw_rc_out[offset + 3] = port.servo4_raw; + raw_rc_out[offset + 4] = port.servo5_raw; + raw_rc_out[offset + 5] = port.servo6_raw; + raw_rc_out[offset + 6] = port.servo7_raw; + raw_rc_out[offset + 7] = port.servo8_raw; + // [[[end]]] (checksum: 46b3fd22ff05ec22fed73852907b4f45) + if (msg->magic == MAVLINK_STX) { + // [[[cog: + // for i in range(9, 17): + // cog.outl(f"raw_rc_out[offset + {i - 1}] = port.servo{i}_raw;") + // ]]] + raw_rc_out[offset + 8] = port.servo9_raw; + raw_rc_out[offset + 9] = port.servo10_raw; + raw_rc_out[offset + 10] = port.servo11_raw; + raw_rc_out[offset + 11] = port.servo12_raw; + raw_rc_out[offset + 12] = port.servo13_raw; + raw_rc_out[offset + 13] = port.servo14_raw; + raw_rc_out[offset + 14] = port.servo15_raw; + raw_rc_out[offset + 15] = port.servo16_raw; + // [[[end]]] (checksum: c008714176d3c498f792098d65557830) + } + + auto rcout_msg = mavros_msgs::msg::RCOut(); + + // XXX: Why time_usec is 32 bit? We should test that. + uint64_t time_usec = port.time_usec; + + rcout_msg.header.stamp = uas->synchronise_stamp(time_usec); + rcout_msg.channels = raw_rc_out; + + rc_out_pub->publish(rcout_msg); + } + + /* -*- callbacks -*- */ + + void connection_cb([[maybe_unused]] bool connected) override + { + lock_guard lock(mutex); + raw_rc_in.clear(); + raw_rc_out.clear(); + has_rc_channels_msg = false; + } + + void override_cb(const mavros_msgs::msg::OverrideRCIn::SharedPtr req) + { + if (!uas->is_ardupilotmega() && !uas->is_px4()) { + RCLCPP_WARN_THROTTLE( + get_logger(), + *get_clock(), 30000, "RC override not supported by this FCU!"); + } + + mavlink::common::msg::RC_CHANNELS_OVERRIDE ovr = {}; + uas->msg_set_target(ovr); + + // [[[cog: + // for i in range(1, 19): + // cog.outl(f"ovr.chan{i}_raw = req->channels[{i - 1}];") + // ]]] + ovr.chan1_raw = req->channels[0]; + ovr.chan2_raw = req->channels[1]; + ovr.chan3_raw = req->channels[2]; + ovr.chan4_raw = req->channels[3]; + ovr.chan5_raw = req->channels[4]; + ovr.chan6_raw = req->channels[5]; + ovr.chan7_raw = req->channels[6]; + ovr.chan8_raw = req->channels[7]; + ovr.chan9_raw = req->channels[8]; + ovr.chan10_raw = req->channels[9]; + ovr.chan11_raw = req->channels[10]; + ovr.chan12_raw = req->channels[11]; + ovr.chan13_raw = req->channels[12]; + ovr.chan14_raw = req->channels[13]; + ovr.chan15_raw = req->channels[14]; + ovr.chan16_raw = req->channels[15]; + ovr.chan17_raw = req->channels[16]; + ovr.chan18_raw = req->channels[17]; + // [[[end]]] (checksum: f878252945b6d1ca47c416b4f3aec145) + + uas->send_message(ovr); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::RCIOPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::RCIOPlugin) diff --git a/mavros/src/plugins/safety_area.cpp b/mavros/src/plugins/safety_area.cpp deleted file mode 100644 index e83bf0945..000000000 --- a/mavros/src/plugins/safety_area.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/** - * @brief SafetyArea plugin - * @file safety_area.cpp - * @author Nuno Marques - * @author Vladimir Ermakov - * - * @addtogroup plugin - * @{ - */ -/* - * Copyright 2014 Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include - -#include - -namespace mavros { -namespace std_plugins { -/** - * @brief Safety allopwed area plugin - * - * Send safety area to FCU controller. - */ -class SafetyAreaPlugin : public plugin::PluginBase { -public: - SafetyAreaPlugin() : PluginBase(), - safety_nh("~safety_area") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - safety_nh.param("frame_id", frame_id, "safety_area"); - - bool manual_def = false; - double p1x, p1y, p1z, - p2x, p2y, p2z; - - if (safety_nh.getParam("p1/x", p1x) && - safety_nh.getParam("p1/y", p1y) && - safety_nh.getParam("p1/z", p1z)) { - manual_def = true; - ROS_DEBUG_NAMED("safetyarea", "SA: Manual set: P1(%f %f %f)", - p1x, p1y, p1z); - } - - if (manual_def && - safety_nh.getParam("p2/x", p2x) && - safety_nh.getParam("p2/y", p2y) && - safety_nh.getParam("p2/z", p2z)) { - manual_def = true; - ROS_DEBUG_NAMED("safetyarea", "SA: Manual set: P2(%f %f %f)", - p2x, p2y, p2z); - } - else - manual_def = false; - - if (manual_def) - send_safety_set_allowed_area( - Eigen::Vector3d(p1x, p1y, p1z), - Eigen::Vector3d(p2x, p2y, p2z)); - - safetyarea_sub = safety_nh.subscribe("set", 10, &SafetyAreaPlugin::safetyarea_cb, this); - safetyarea_pub = safety_nh.advertise("get",10); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&SafetyAreaPlugin::handle_safety_allowed_area) - }; - } - -private: - ros::NodeHandle safety_nh; - - std::string frame_id; - - ros::Subscriber safetyarea_sub; - ros::Publisher safetyarea_pub; - - /* -*- rx handlers -*- */ - void handle_safety_allowed_area(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SAFETY_ALLOWED_AREA &saa) - { - auto saa_msg = boost::make_shared(); - - Eigen::Vector3d p1(saa.p1x, saa.p1y, saa.p1z); - Eigen::Vector3d p2(saa.p2x, saa.p2y, saa.p2z); - - p1 = ftf::transform_frame_ned_enu(p1); - p2 = ftf::transform_frame_ned_enu(p2); - - saa_msg->header.stamp = ros::Time::now(); - saa_msg->header.frame_id = frame_id; - //saa_msg->header.frame_id = utils::to_string(saa.frame); @TODO: after #311 merged this will work - - saa_msg->polygon.points.resize(2); - saa_msg->polygon.points[0].x = p1.x(); - saa_msg->polygon.points[0].y = p1.y(); - saa_msg->polygon.points[0].z = p1.z(); - saa_msg->polygon.points[1].x = p2.x(); - saa_msg->polygon.points[1].y = p2.y(); - saa_msg->polygon.points[1].z = p2.z(); - - safetyarea_pub.publish(saa_msg); - } - - /* -*- mid-level helpers -*- */ - - /** - * @brief Send a safety zone (volume), which is defined by two corners of a cube, - * to the FCU. - * - * @note ENU frame. - */ - void send_safety_set_allowed_area(Eigen::Vector3d p1, Eigen::Vector3d p2) - { - ROS_INFO_STREAM_NAMED("safetyarea", "SA: Set safety area: P1 " << p1 << " P2 " << p2); - - p1 = ftf::transform_frame_enu_ned(p1); - p2 = ftf::transform_frame_enu_ned(p2); - - mavlink::common::msg::SAFETY_SET_ALLOWED_AREA s = {}; - m_uas->msg_set_target(s); - - // TODO: use enum from lib - s.frame = utils::enum_value(mavlink::common::MAV_FRAME::LOCAL_NED); - - // [[[cog: - // for p in range(1, 3): - // for v in ('x', 'y', 'z'): - // cog.outl("s.p%d%s = p%d.%s();" % (p, v, p, v)) - // ]]] - s.p1x = p1.x(); - s.p1y = p1.y(); - s.p1z = p1.z(); - s.p2x = p2.x(); - s.p2y = p2.y(); - s.p2z = p2.z(); - // [[[end]]] (checksum: c996a362f338fcc6b714c8be583c3be0) - - UAS_FCU(m_uas)->send_message_ignore_drop(s); - } - - /* -*- callbacks -*- */ - - void safetyarea_cb(const geometry_msgs::PolygonStamped::ConstPtr &req) - { - if (req->polygon.points.size() != 2) { - ROS_ERROR_NAMED("safetyarea", "SA: Polygon should contain only two points"); - return; - } - - // eigen_conversions do not have convertor for Point32 - // [[[cog: - // for p in range(2): - // cog.outl("Eigen::Vector3d p%d(%s);" % (p + 1, ', '.join([ - // 'req->polygon.points[%d].%s' % (p, v) for v in ('x', 'y', 'z') - // ]))) - // ]]] - Eigen::Vector3d p1(req->polygon.points[0].x, req->polygon.points[0].y, req->polygon.points[0].z); - Eigen::Vector3d p2(req->polygon.points[1].x, req->polygon.points[1].y, req->polygon.points[1].z); - // [[[end]]] (checksum: c3681d584e02f7d91d6b3b48f87b1771) - - send_safety_set_allowed_area(p1, p2); - } -}; -} // namespace std_plugins -} // namespace mavros - -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SafetyAreaPlugin, mavros::plugin::PluginBase) diff --git a/mavros/src/plugins/setpoint_accel.cpp b/mavros/src/plugins/setpoint_accel.cpp index ac711094c..6d270a542 100644 --- a/mavros/src/plugins/setpoint_accel.cpp +++ b/mavros/src/plugins/setpoint_accel.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2014 Nuno Marques. + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief SetpointAcceleration plugin * @file setpoint_accel.cpp @@ -7,99 +15,104 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014 Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include -#include -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" +#include "mavros/setpoint_mixin.hpp" -#include +#include "geometry_msgs/msg/vector3_stamped.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace std_plugins { /** * @brief Setpoint acceleration/force plugin + * @plugin setpoint_accel * * Send setpoint accelerations/forces to FCU controller. */ -class SetpointAccelerationPlugin : public plugin::PluginBase, - private plugin::SetPositionTargetLocalNEDMixin { +class SetpointAccelerationPlugin : public plugin::Plugin, + private plugin::SetPositionTargetLocalNEDMixin +{ public: - SetpointAccelerationPlugin() : PluginBase(), - sp_nh("~setpoint_accel"), - send_force(false) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); + explicit SetpointAccelerationPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "setpoint_accel") + { + node->declare_parameter("send_force", false); - sp_nh.param("send_force", send_force, false); + auto sensor_qos = rclcpp::SensorDataQoS(); - accel_sub = sp_nh.subscribe("accel", 10, &SetpointAccelerationPlugin::accel_cb, this); - } + accel_sub = node->create_subscription( + "~/accel", sensor_qos, std::bind( + &SetpointAccelerationPlugin::accel_cb, this, + _1)); + } - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - friend class SetPositionTargetLocalNEDMixin; - ros::NodeHandle sp_nh; - - ros::Subscriber accel_sub; - - bool send_force; - - /* -*- mid-level helpers -*- */ - - /** - * @brief Send acceleration/force to FCU acceleration controller. - * - * @warning Send only AFX AFY AFZ. ENU frame. - */ - void send_setpoint_acceleration(const ros::Time &stamp, Eigen::Vector3d &accel_enu) - { - using mavlink::common::MAV_FRAME; - - /* Documentation start from bit 1 instead 0. - * Ignore position and velocity vectors, yaw and yaw rate - */ - uint16_t ignore_all_except_a_xyz = (3 << 10) | (7 << 3) | (7 << 0); - - if (send_force) - ignore_all_except_a_xyz |= (1 << 9); - - auto accel = ftf::transform_frame_enu_ned(accel_enu); - - set_position_target_local_ned(stamp.toNSec() / 1000000, - utils::enum_value(MAV_FRAME::LOCAL_NED), - ignore_all_except_a_xyz, - Eigen::Vector3d::Zero(), - Eigen::Vector3d::Zero(), - accel, - 0.0, 0.0); - } - - /* -*- callbacks -*- */ - - void accel_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req) { - Eigen::Vector3d accel_enu; - - tf::vectorMsgToEigen(req->vector, accel_enu); - send_setpoint_acceleration(req->header.stamp, accel_enu); - } + friend class plugin::SetPositionTargetLocalNEDMixin; + + rclcpp::Subscription::SharedPtr accel_sub; + + /* -*- mid-level helpers -*- */ + + /** + * @brief Send acceleration/force to FCU acceleration controller. + * + * @warning Send only AFX AFY AFZ. ENU frame. + */ + void send_setpoint_acceleration(const rclcpp::Time & stamp, const Eigen::Vector3d & accel_enu) + { + using mavlink::common::MAV_FRAME; + + bool send_force; + node->get_parameter("send_force", send_force); + + /* Documentation start from bit 1 instead 0. + * Ignore position and velocity vectors, yaw and yaw rate + */ + uint16_t ignore_all_except_a_xyz = (3 << 10) | (7 << 3) | (7 << 0); + + if (send_force) { + ignore_all_except_a_xyz |= (1 << 9); + } + + auto accel = ftf::transform_frame_enu_ned(accel_enu); + + set_position_target_local_ned( + get_time_boot_ms(stamp), + utils::enum_value(MAV_FRAME::LOCAL_NED), + ignore_all_except_a_xyz, + Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero(), + accel, + 0.0, 0.0); + } + + /* -*- callbacks -*- */ + + void accel_cb(const geometry_msgs::msg::Vector3Stamped::SharedPtr req) + { + Eigen::Vector3d accel_enu; + + tf2::fromMsg(req->vector, accel_enu); + send_setpoint_acceleration(req->header.stamp, accel_enu); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SetpointAccelerationPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::SetpointAccelerationPlugin) diff --git a/mavros/src/plugins/setpoint_attitude.cpp b/mavros/src/plugins/setpoint_attitude.cpp index dd300192d..62b9cde9c 100644 --- a/mavros/src/plugins/setpoint_attitude.cpp +++ b/mavros/src/plugins/setpoint_attitude.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2014 Nuno Marques. + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief SetpointAttitude plugin * @file setpoint_attitude.cpp @@ -7,217 +15,213 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014 Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include -#include -#include +#include #include #include #include -#include -#include -#include +#include -namespace mavros { -namespace std_plugins { +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" +#include "mavros/setpoint_mixin.hpp" -using SyncPoseThrustPolicy = message_filters::sync_policies::ApproximateTime; -using SyncTwistThrustPolicy = message_filters::sync_policies::ApproximateTime; +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "mavros_msgs/msg/thrust.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT + +using SyncPoseThrustPolicy = message_filters::sync_policies::ApproximateTime< + geometry_msgs::msg::PoseStamped, mavros_msgs::msg::Thrust>; +using SyncTwistThrustPolicy = message_filters::sync_policies::ApproximateTime< + geometry_msgs::msg::TwistStamped, mavros_msgs::msg::Thrust>; using SyncPoseThrust = message_filters::Synchronizer; using SyncTwistThrust = message_filters::Synchronizer; /** * @brief Setpoint attitude plugin + * @plugin setpoint_attitude * * Send setpoint attitude/orientation/thrust to FCU controller. */ -class SetpointAttitudePlugin : public plugin::PluginBase, - private plugin::SetAttitudeTargetMixin, - private plugin::TF2ListenerMixin { +class SetpointAttitudePlugin : public plugin::Plugin, + private plugin::SetAttitudeTargetMixin +{ public: - SetpointAttitudePlugin() : PluginBase(), - sp_nh("~setpoint_attitude"), - tf_listen(false), - tf_rate(50.0), - use_quaternion(false), - reverse_thrust(false) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - // main params - sp_nh.param("use_quaternion", use_quaternion, false); - sp_nh.param("reverse_thrust", reverse_thrust, false); - // tf params - sp_nh.param("tf/listen", tf_listen, false); - sp_nh.param("tf/frame_id", tf_frame_id, "map"); - sp_nh.param("tf/child_frame_id", tf_child_frame_id, "target_attitude"); - sp_nh.param("tf/rate_limit", tf_rate, 50.0); - - // thrust msg subscriber to sync - th_sub.subscribe(sp_nh, "thrust", 1); - - if (tf_listen) { - ROS_INFO_STREAM_NAMED("attitude", - "Listen to desired attitude transform " - << tf_frame_id << " -> " << tf_child_frame_id); - - tf2_start("AttitudeSpTFSync", th_sub, &SetpointAttitudePlugin::transform_cb); - } - else if (use_quaternion) { - /** - * @brief Use message_filters to sync attitude and thrust msg coming from different topics - */ - pose_sub.subscribe(sp_nh, "attitude", 1); - - /** - * @brief Matches messages, even if they have different time stamps, - * by using an adaptative algorithm - */ - sync_pose.reset(new SyncPoseThrust(SyncPoseThrustPolicy(10), pose_sub, th_sub)); - sync_pose->registerCallback(boost::bind(&SetpointAttitudePlugin::attitude_pose_cb, this, _1, _2)); - } - else { - twist_sub.subscribe(sp_nh, "cmd_vel", 1); - sync_twist.reset(new SyncTwistThrust(SyncTwistThrustPolicy(10), twist_sub, th_sub)); - sync_twist->registerCallback(boost::bind(&SetpointAttitudePlugin::attitude_twist_cb, this, _1, _2)); - } - } - - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + explicit SetpointAttitudePlugin(plugin::UASPtr uas_) + : Plugin(uas_, "setpoint_attitude"), + reverse_thrust(false) + { + enable_node_watch_parameters(); + + auto qos = rclcpp::QoS(10); + + node_declare_and_watch_parameter( + "reverse_thrust", false, [&](const rclcpp::Parameter & p) { + reverse_thrust = p.as_bool(); + }); + + node_declare_and_watch_parameter( + "use_quaternion", false, [&](const rclcpp::Parameter & p) { + auto use_quaternion = p.as_bool(); + + pose_sub.unsubscribe(); + twist_sub.unsubscribe(); + sync_pose.reset(); + sync_twist.reset(); + + if (use_quaternion) { + /** + * @brief Use message_filters to sync attitude and thrust msg coming from different topics + */ + pose_sub.subscribe(node, "~/attitude", qos.get_rmw_qos_profile()); + + sync_pose = std::make_unique(SyncPoseThrustPolicy(10), pose_sub, th_sub); + sync_pose->registerCallback(&SetpointAttitudePlugin::attitude_pose_cb, this); + + } else { + twist_sub.subscribe(node, "~/cmd_vel", qos.get_rmw_qos_profile()); + + sync_twist = + std::make_unique(SyncTwistThrustPolicy(10), twist_sub, th_sub); + sync_twist->registerCallback(&SetpointAttitudePlugin::attitude_twist_cb, this); + } + }); + + // thrust msg subscriber to sync + th_sub.subscribe(node, "~/thrust", qos.get_rmw_qos_profile()); + } + + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - friend class SetAttitudeTargetMixin; - friend class TF2ListenerMixin; - ros::NodeHandle sp_nh; - - message_filters::Subscriber th_sub; - message_filters::Subscriber pose_sub; - message_filters::Subscriber twist_sub; - - std::unique_ptr sync_pose; - std::unique_ptr sync_twist; - - std::string tf_frame_id; - std::string tf_child_frame_id; - - bool tf_listen; - double tf_rate; - - bool use_quaternion; - - bool reverse_thrust; - float normalized_thrust; - - /** - * @brief Function to verify if the thrust values are normalized; - * considers also the reversed trust values - */ - inline bool is_normalized(float thrust){ - if (reverse_thrust) { - if (thrust < -1.0) { - ROS_WARN_NAMED("attitude", "Not normalized reversed thrust! Thd(%f) < Min(%f)", thrust, -1.0); - return false; - } - } - else { - if (thrust < 0.0) { - ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) < Min(%f)", thrust, 0.0); - return false; - } - } - - if (thrust > 1.0) { - ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) > Max(%f)", thrust, 1.0); - return false; - } - return true; - } - - /* -*- mid-level helpers -*- */ - - /** - * @brief Send attitude setpoint and thrust to FCU attitude controller - */ - void send_attitude_quaternion(const ros::Time &stamp, const Eigen::Affine3d &tr, const float thrust) - { - /** - * @note RPY, also bits numbering started from 1 in docs - */ - const uint8_t ignore_all_except_q_and_thrust = (7 << 0); - - auto q = ftf::transform_orientation_enu_ned( - ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())) - ); - - set_attitude_target(stamp.toNSec() / 1000000, - ignore_all_except_q_and_thrust, - q, - Eigen::Vector3d::Zero(), - thrust); - } - - /** - * @brief Send angular velocity setpoint and thrust to FCU attitude controller - */ - void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel, const float thrust) - { - /** - * @note Q, also bits noumbering started from 1 in docs - */ - const uint8_t ignore_all_except_rpy = (1 << 7); - - auto av = ftf::transform_frame_ned_enu(ang_vel); - - set_attitude_target(stamp.toNSec() / 1000000, - ignore_all_except_rpy, - Eigen::Quaterniond::Identity(), - av, - thrust); - } - - /* -*- callbacks -*- */ - - void transform_cb(const geometry_msgs::TransformStamped &transform, const mavros_msgs::Thrust::ConstPtr &thrust_msg) { - Eigen::Affine3d tr; - tf::transformMsgToEigen(transform.transform, tr); - - send_attitude_quaternion(transform.header.stamp, tr, thrust_msg->thrust); - } - - void attitude_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &pose_msg, const mavros_msgs::Thrust::ConstPtr &thrust_msg) { - Eigen::Affine3d tr; - tf::poseMsgToEigen(pose_msg->pose, tr); - - if (is_normalized(thrust_msg->thrust)) - send_attitude_quaternion(pose_msg->header.stamp, tr, thrust_msg->thrust); - } - - void attitude_twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req, const mavros_msgs::Thrust::ConstPtr &thrust_msg) { - Eigen::Vector3d ang_vel; - tf::vectorMsgToEigen(req->twist.angular, ang_vel); - - if (is_normalized(thrust_msg->thrust)) - send_attitude_ang_velocity(req->header.stamp, ang_vel, thrust_msg->thrust); - } - + friend class plugin::SetAttitudeTargetMixin; + + message_filters::Subscriber th_sub; + message_filters::Subscriber pose_sub; + message_filters::Subscriber twist_sub; + + std::unique_ptr sync_pose; + std::unique_ptr sync_twist; + + bool reverse_thrust; + float normalized_thrust; + + /** + * @brief Function to verify if the thrust values are normalized; + * considers also the reversed trust values + */ + inline bool is_normalized(float thrust) + { + auto lg = get_logger(); + + if (reverse_thrust) { + if (thrust < -1.0) { + RCLCPP_WARN(lg, "Not normalized reversed thrust! Thd(%f) < Min(%f)", thrust, -1.0); + return false; + } + } else { + if (thrust < 0.0) { + RCLCPP_WARN(lg, "Not normalized thrust! Thd(%f) < Min(%f)", thrust, 0.0); + return false; + } + } + + if (thrust > 1.0) { + RCLCPP_WARN(lg, "Not normalized thrust! Thd(%f) > Max(%f)", thrust, 1.0); + return false; + } + return true; + } + + /* -*- mid-level helpers -*- */ + + /** + * @brief Send attitude setpoint and thrust to FCU attitude controller + */ + void send_attitude_quaternion( + const rclcpp::Time & stamp, const Eigen::Affine3d & tr, + const float thrust) + { + /** + * @note RPY, also bits numbering started from 1 in docs + */ + const uint8_t ignore_all_except_q_and_thrust = (7 << 0); + + auto q = ftf::transform_orientation_enu_ned( + ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())) + ); + + set_attitude_target( + get_time_boot_ms(stamp), + ignore_all_except_q_and_thrust, + q, + Eigen::Vector3d::Zero(), + thrust); + } + + /** + * @brief Send angular velocity setpoint and thrust to FCU attitude controller + */ + void send_attitude_ang_velocity( + const rclcpp::Time & stamp, const Eigen::Vector3d & ang_vel, + const float thrust) + { + /** + * @note Q, also bits noumbering started from 1 in docs + */ + const uint8_t ignore_all_except_rpy = (1 << 7); + + auto av = ftf::transform_frame_ned_enu(ang_vel); + + set_attitude_target( + get_time_boot_ms(stamp), + ignore_all_except_rpy, + Eigen::Quaterniond::Identity(), + av, + thrust); + } + + /* -*- callbacks -*- */ + + void attitude_pose_cb( + const geometry_msgs::msg::PoseStamped::SharedPtr pose_msg, + const mavros_msgs::msg::Thrust::SharedPtr thrust_msg) + { + Eigen::Affine3d tr; + tf2::fromMsg(pose_msg->pose, tr); + + if (is_normalized(thrust_msg->thrust)) { + send_attitude_quaternion(pose_msg->header.stamp, tr, thrust_msg->thrust); + } + } + + void attitude_twist_cb( + const geometry_msgs::msg::TwistStamped::SharedPtr req, + const mavros_msgs::msg::Thrust::SharedPtr thrust_msg) + { + Eigen::Vector3d ang_vel; + tf2::fromMsg(req->twist.angular, ang_vel); + + if (is_normalized(thrust_msg->thrust)) { + send_attitude_ang_velocity(req->header.stamp, ang_vel, thrust_msg->thrust); + } + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SetpointAttitudePlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::SetpointAttitudePlugin) diff --git a/mavros/src/plugins/setpoint_position.cpp b/mavros/src/plugins/setpoint_position.cpp index c91f81c66..00f9b0ee0 100644 --- a/mavros/src/plugins/setpoint_position.cpp +++ b/mavros/src/plugins/setpoint_position.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2014,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief SetpointPosition plugin * @file setpoint_position.cpp @@ -6,294 +13,279 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include -#include +#include +#include -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" +#include "mavros/setpoint_mixin.hpp" -#include -#include +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geographic_msgs/msg/geo_pose_stamped.hpp" -#include -namespace mavros { -namespace std_plugins { +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT using mavlink::common::MAV_FRAME; + /** * @brief Setpoint position plugin + * @plugin setpoint_position * * Send setpoint positions to FCU controller. */ -class SetpointPositionPlugin : public plugin::PluginBase, - private plugin::SetPositionTargetLocalNEDMixin, - private plugin::SetPositionTargetGlobalIntMixin, - private plugin::TF2ListenerMixin { +class SetpointPositionPlugin : public plugin::Plugin, + private plugin::SetPositionTargetLocalNEDMixin, + private plugin::SetPositionTargetGlobalIntMixin +{ public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - SetpointPositionPlugin() : PluginBase(), - sp_nh("~setpoint_position"), - spg_nh("~"), - tf_listen(false), - tf_rate(50.0) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - // tf params - sp_nh.param("tf/listen", tf_listen, false); - sp_nh.param("tf/frame_id", tf_frame_id, "map"); - sp_nh.param("tf/child_frame_id", tf_child_frame_id, "target_position"); - sp_nh.param("tf/rate_limit", tf_rate, 50.0); - - if (tf_listen) { - ROS_INFO_STREAM_NAMED("setpoint", "Listen to position setpoint transform " - << tf_frame_id << " -> " << tf_child_frame_id); - tf2_start("PositionSpTF", &SetpointPositionPlugin::transform_cb); - } - else { - setpoint_sub = sp_nh.subscribe("local", 10, &SetpointPositionPlugin::setpoint_cb, this); - // Subscriber for goal gps - setpointg_sub = sp_nh.subscribe("global", 10, &SetpointPositionPlugin::setpointg_cb, this); - // Subscriber for goal gps but will convert it to local coordinates - setpointg2l_sub = sp_nh.subscribe("global_to_local", 10, &SetpointPositionPlugin::setpointg2l_cb, this); - // subscriber for current gps state, mavros/global_position/global. - gps_sub = spg_nh.subscribe("global_position/global", 10, &SetpointPositionPlugin::gps_cb, this); - // Subscribe for current local ENU pose. - local_sub = spg_nh.subscribe("local_position/pose", 10, &SetpointPositionPlugin::local_cb, this); - } - mav_frame_srv = sp_nh.advertiseService("mav_frame", &SetpointPositionPlugin::set_mav_frame_cb, this); - - // mav_frame - std::string mav_frame_str; - if (!sp_nh.getParam("mav_frame", mav_frame_str)) { - mav_frame = MAV_FRAME::LOCAL_NED; - } else { - mav_frame = utils::mav_frame_from_str(mav_frame_str); - } - } - - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit SetpointPositionPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "setpoint_position") + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "mav_frame", "LOCAL_NED", [&](const rclcpp::Parameter & p) { + auto mav_frame_str = p.as_string(); + auto new_mav_frame = utils::mav_frame_from_str(mav_frame_str); + + if (new_mav_frame == MAV_FRAME::LOCAL_NED && mav_frame_str != "LOCAL_NED") { + throw rclcpp::exceptions::InvalidParameterValueException( + utils::format( + "unknown MAV_FRAME: %s", + mav_frame_str.c_str())); + } + mav_frame = new_mav_frame; + }); + + auto sensor_qos = rclcpp::SensorDataQoS(); + + setpoint_sub = node->create_subscription( + "~/local", sensor_qos, std::bind( + &SetpointPositionPlugin::setpoint_cb, this, + _1)); + setpointg_sub = node->create_subscription( + "~/global", + sensor_qos, std::bind( + &SetpointPositionPlugin::setpointg_cb, this, + _1)); + setpointg2l_sub = node->create_subscription( + "~/global_to_local", sensor_qos, + std::bind(&SetpointPositionPlugin::setpointg2l_cb, this, _1)); + + gps_sub = node->create_subscription( + "global_position/global", + sensor_qos, + std::bind(&SetpointPositionPlugin::gps_cb, this, _1)); + local_sub = node->create_subscription( + "local_position/pose", + sensor_qos, + std::bind(&SetpointPositionPlugin::local_cb, this, _1)); + } + + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - friend class SetPositionTargetLocalNEDMixin; - friend class SetPositionTargetGlobalIntMixin; - friend class TF2ListenerMixin; - - ros::NodeHandle sp_nh; - ros::NodeHandle spg_nh; //!< to get local position and gps coord which are not under sp_h() - ros::Subscriber setpoint_sub; - ros::Subscriber setpointg_sub; //!< Global setpoint - ros::Subscriber setpointg2l_sub;//!< Global setpoint converted to local setpoint - ros::Subscriber gps_sub; //!< current GPS - ros::Subscriber local_sub; //!< current local ENU - ros::ServiceServer mav_frame_srv; - - /* Stores current gps state. */ - //sensor_msgs::NavSatFix current_gps_msg; - Eigen::Vector3d current_gps; //!< geodetic coordinates LLA - Eigen::Vector3d current_local_pos; //!< Current local position in ENU - uint32_t old_gps_stamp = 0; //!< old time gps time stamp in [ms], to check if new gps msg is received - - std::string tf_frame_id; - std::string tf_child_frame_id; - - bool tf_listen; - double tf_rate; - - MAV_FRAME mav_frame; - - /* -*- mid-level helpers -*- */ - - /** - * @brief Send setpoint to FCU position controller. - * - * @warning Send only XYZ, Yaw. ENU frame. - */ - void send_position_target(const ros::Time &stamp, const Eigen::Affine3d &tr) - { - using mavlink::common::MAV_FRAME; - - /* Documentation start from bit 1 instead 0; - * Ignore velocity and accel vectors, yaw rate. - * - * In past versions on PX4 there been bug described in #273. - * If you got similar issue please try update firmware first. - */ - const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3); - - auto p = [&] () { - if (static_cast(mav_frame) == MAV_FRAME::BODY_NED || static_cast(mav_frame) == MAV_FRAME::BODY_OFFSET_NED) { - return ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(tr.translation())); - } else { - return ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation())); - } - } (); - - auto q = [&] () { - if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) { - return ftf::transform_orientation_absolute_frame_aircraft_baselink(Eigen::Quaterniond(tr.rotation())); - } else { - return ftf::transform_orientation_enu_ned( - ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))); - } - } (); - - set_position_target_local_ned(stamp.toNSec() / 1000000, - utils::enum_value(mav_frame), - ignore_all_except_xyz_y, - p, - Eigen::Vector3d::Zero(), - Eigen::Vector3d::Zero(), - ftf::quaternion_get_yaw(q), 0.0); - } - - /* -*- callbacks -*- */ - - /* common TF listener moved to mixin */ - void transform_cb(const geometry_msgs::TransformStamped &transform) - { - Eigen::Affine3d tr; - // TODO: later, when tf2 5.12 will be released need to revisit and replace this to - // tf2::convert() - tf::transformMsgToEigen(transform.transform, tr); - - send_position_target(transform.header.stamp, tr); - } - - void setpoint_cb(const geometry_msgs::PoseStamped::ConstPtr &req) - { - Eigen::Affine3d tr; - tf::poseMsgToEigen(req->pose, tr); - - send_position_target(req->header.stamp, tr); - } - - /** - * Gets setpoint position setpoint and send SET_POSITION_TARGET_GLOBAL_INT - */ - void setpointg_cb(const geographic_msgs::GeoPoseStamped::ConstPtr &req) - { - using mavlink::common::POSITION_TARGET_TYPEMASK; - - uint16_t type_mask = uint16_t(POSITION_TARGET_TYPEMASK::VX_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::VY_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::VZ_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::AX_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::AY_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::AZ_IGNORE); - - Eigen::Quaterniond attitude; - tf::quaternionMsgToEigen(req->pose.orientation, attitude); - Eigen::Quaterniond q = ftf::transform_orientation_enu_ned( - ftf::transform_orientation_baselink_aircraft(attitude)); - - set_position_target_global_int( - req->header.stamp.toNSec() / 1000000, - uint8_t(MAV_FRAME::GLOBAL_INT), - type_mask, - req->pose.position.latitude * 1e7, - req->pose.position.longitude * 1e7, - req->pose.position.altitude, - Eigen::Vector3d::Zero(), - Eigen::Vector3d::Zero(), - ftf::quaternion_get_yaw(q), - 0); - } - - /** - * Gets gps setpoint, converts it to local ENU, and sends it to FCU - */ - void setpointg2l_cb(const geographic_msgs::GeoPoseStamped::ConstPtr &req) - { - /** - * The idea is to convert the change in LLA(goal_gps-current_gps) to change in ENU - * 1- convert current/goal gps points to current/goal ECEF points - * 2- claculate offset in ECEF frame - * 3- converts ECEF offset to ENU offset given current gps LLA - * 4- adds ENU offset to current local ENU to that will be sent to FCU - */ - - GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); - - Eigen::Vector3d goal_gps(req->pose.position.latitude, req->pose.position.longitude, req->pose.position.altitude); - - // current gps -> curent ECEF - Eigen::Vector3d current_ecef; - earth.Forward(current_gps.x(), current_gps.y(), current_gps.z(), - current_ecef.x(), current_ecef.y(), current_ecef.z()); - - // goal gps -> goal ECEF - Eigen::Vector3d goal_ecef; - earth.Forward(goal_gps.x(), goal_gps.y(), goal_gps.z(), - goal_ecef.x(), goal_ecef.y(), goal_ecef.z()); - - // get ENU offset from ECEF offset - Eigen::Vector3d ecef_offset = goal_ecef - current_ecef; - Eigen::Vector3d enu_offset = ftf::transform_frame_ecef_enu(ecef_offset, current_gps); - - // prepare yaw angle - Eigen::Affine3d sp; // holds position setpoint - Eigen::Quaterniond q; // holds desired yaw - - tf::quaternionMsgToEigen(req->pose.orientation, q); - - // set position setpoint - sp.translation() = current_local_pos + enu_offset; - // set desired orientation - sp.linear() = q.toRotationMatrix(); - - // Only send if current gps is updated, to avoid divergence - if ((req->header.stamp.toNSec() / 1000000) > old_gps_stamp) { - old_gps_stamp = req->header.stamp.toNSec() / 1000000; - send_position_target(req->header.stamp, sp); - } - else { - ROS_WARN_THROTTLE_NAMED(10, "spgp", "SPG: sp not sent."); - } - } - - /** - * Current GPS coordinates - */ - void gps_cb(const sensor_msgs::NavSatFix::ConstPtr &msg) - { - current_gps = {msg->latitude, msg->longitude, msg->altitude}; - } - - /** - * current local position in ENU - */ - void local_cb(const geometry_msgs::PoseStamped::ConstPtr &msg) - { - current_local_pos = ftf::to_eigen(msg->pose.position); - } - - bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res) - { - mav_frame = static_cast(req.mav_frame); - const std::string mav_frame_str = utils::to_string(mav_frame); - sp_nh.setParam("mav_frame", mav_frame_str); - res.success = true; - return true; - } + friend class plugin::SetPositionTargetLocalNEDMixin; + friend class plugin::SetPositionTargetGlobalIntMixin; + + rclcpp::Subscription::SharedPtr setpoint_sub; + //! Global setpoint + rclcpp::Subscription::SharedPtr setpointg_sub; + //! Global setpoint converted to local setpoint + rclcpp::Subscription::SharedPtr setpointg2l_sub; + //! current GPS + rclcpp::Subscription::SharedPtr gps_sub; + //! current local ENU + rclcpp::Subscription::SharedPtr local_sub; + + /* Stores current gps state. */ + // sensor_msgs::NavSatFix current_gps_msg; + + //! geodetic coordinates LLA + Eigen::Vector3d current_gps; + //! Current local position in ENU + Eigen::Vector3d current_local_pos; + //! old time gps time stamp in [ms], to check if new gps msg is received + uint32_t old_gps_stamp = 0; + + MAV_FRAME mav_frame; + + /* -*- mid-level helpers -*- */ + + /** + * @brief Send setpoint to FCU position controller. + * + * @warning Send only XYZ, Yaw. ENU frame. + */ + void send_position_target(const rclcpp::Time & stamp, const Eigen::Affine3d & tr) + { + using mavlink::common::MAV_FRAME; + + /* Documentation start from bit 1 instead 0; + * Ignore velocity and accel vectors, yaw rate. + * + * In past versions on PX4 there been bug described in #273. + * If you got similar issue please try update firmware first. + */ + const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3); + + auto p = [&]() { + if (mav_frame == MAV_FRAME::BODY_NED || + mav_frame == MAV_FRAME::BODY_OFFSET_NED) + { + return ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(tr.translation())); + } else { + return ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation())); + } + } (); + + auto q = [&]() { + if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) { + return ftf::transform_orientation_absolute_frame_aircraft_baselink( + Eigen::Quaterniond(tr.rotation())); + } else { + return ftf::transform_orientation_enu_ned( + ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))); + } + } (); + + set_position_target_local_ned( + get_time_boot_ms(stamp), + utils::enum_value(mav_frame), + ignore_all_except_xyz_y, + p, + Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero(), + ftf::quaternion_get_yaw(q), 0.0); + } + + /* -*- callbacks -*- */ + + void setpoint_cb(const geometry_msgs::msg::PoseStamped::SharedPtr req) + { + Eigen::Affine3d tr; + tf2::fromMsg(req->pose, tr); + + send_position_target(req->header.stamp, tr); + } + + /** + * Gets setpoint position setpoint and send SET_POSITION_TARGET_GLOBAL_INT + */ + void setpointg_cb(const geographic_msgs::msg::GeoPoseStamped::SharedPtr req) + { + using mavlink::common::POSITION_TARGET_TYPEMASK; + + uint16_t type_mask = uint16_t(POSITION_TARGET_TYPEMASK::VX_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::VY_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::VZ_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::AX_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::AY_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::AZ_IGNORE); + + Eigen::Quaterniond attitude = ftf::to_eigen(req->pose.orientation); + Eigen::Quaterniond q = ftf::transform_orientation_enu_ned( + ftf::transform_orientation_baselink_aircraft(attitude)); + + set_position_target_global_int( + get_time_boot_ms(req->header.stamp), + uint8_t(MAV_FRAME::GLOBAL_INT), + type_mask, + req->pose.position.latitude * 1e7, + req->pose.position.longitude * 1e7, + req->pose.position.altitude, + Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero(), + ftf::quaternion_get_yaw(q), + 0); + } + + /** + * Gets gps setpoint, converts it to local ENU, and sends it to FCU + */ + void setpointg2l_cb(const geographic_msgs::msg::GeoPoseStamped::SharedPtr req) + { + /** + * The idea is to convert the change in LLA(goal_gps-current_gps) to change in ENU + * 1. convert current/goal gps points to current/goal ECEF points + * 2. claculate offset in ECEF frame + * 3. converts ECEF offset to ENU offset given current gps LLA + * 4. adds ENU offset to current local ENU to that will be sent to FCU + */ + + GeographicLib::Geocentric earth( + GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); + + Eigen::Vector3d goal_gps(req->pose.position.latitude, req->pose.position.longitude, + req->pose.position.altitude); + + // current gps -> curent ECEF + Eigen::Vector3d current_ecef; + earth.Forward( + current_gps.x(), current_gps.y(), current_gps.z(), + current_ecef.x(), current_ecef.y(), current_ecef.z()); + + // goal gps -> goal ECEF + Eigen::Vector3d goal_ecef; + earth.Forward( + goal_gps.x(), goal_gps.y(), goal_gps.z(), + goal_ecef.x(), goal_ecef.y(), goal_ecef.z()); + + // get ENU offset from ECEF offset + Eigen::Vector3d ecef_offset = goal_ecef - current_ecef; + Eigen::Vector3d enu_offset = ftf::transform_frame_ecef_enu(ecef_offset, current_gps); + + // prepare yaw angle + Eigen::Affine3d sp; // holds position setpoint + Eigen::Quaterniond q; // holds desired yaw + + tf2::fromMsg(req->pose.orientation, q); + + // set position setpoint + sp.translation() = current_local_pos + enu_offset; + // set desired orientation + sp.linear() = q.toRotationMatrix(); + + // Only send if current gps is updated, to avoid divergence + if (get_time_boot_ms(req->header.stamp) > old_gps_stamp) { + old_gps_stamp = get_time_boot_ms(req->header.stamp); + send_position_target(req->header.stamp, sp); + } else { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 10000, "SPG: sp not sent."); + } + } + + /** + * Current GPS coordinates + */ + void gps_cb(const sensor_msgs::msg::NavSatFix::SharedPtr msg) + { + current_gps = {msg->latitude, msg->longitude, msg->altitude}; + } + + /** + * current local position in ENU + */ + void local_cb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { + current_local_pos = ftf::to_eigen(msg->pose.position); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SetpointPositionPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::SetpointPositionPlugin) diff --git a/mavros/src/plugins/setpoint_raw.cpp b/mavros/src/plugins/setpoint_raw.cpp index 6ecfa20a7..653c180c7 100644 --- a/mavros/src/plugins/setpoint_raw.cpp +++ b/mavros/src/plugins/setpoint_raw.cpp @@ -14,270 +14,320 @@ * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ -#include -#include -#include +#include -#include -#include -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" +#include "mavros/setpoint_mixin.hpp" + +#include "mavros_msgs/msg/attitude_target.hpp" +#include "mavros_msgs/msg/position_target.hpp" +#include "mavros_msgs/msg/global_position_target.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace std_plugins { /** * @brief Setpoint RAW plugin + * @plugin setpoint_raw * * Send position setpoints and publish current state (return loop). * User can decide what set of filed needed for operation via IGNORE bits. */ -class SetpointRawPlugin : public plugin::PluginBase, - private plugin::SetPositionTargetLocalNEDMixin, - private plugin::SetPositionTargetGlobalIntMixin, - private plugin::SetAttitudeTargetMixin { +class SetpointRawPlugin : public plugin::Plugin, + private plugin::SetPositionTargetLocalNEDMixin, + private plugin::SetPositionTargetGlobalIntMixin, + private plugin::SetAttitudeTargetMixin +{ public: - SetpointRawPlugin() : PluginBase(), - sp_nh("~setpoint_raw") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - bool tf_listen; - - local_sub = sp_nh.subscribe("local", 10, &SetpointRawPlugin::local_cb, this); - global_sub = sp_nh.subscribe("global", 10, &SetpointRawPlugin::global_cb, this); - attitude_sub = sp_nh.subscribe("attitude", 10, &SetpointRawPlugin::attitude_cb, this); - target_local_pub = sp_nh.advertise("target_local", 10); - target_global_pub = sp_nh.advertise("target_global", 10); - target_attitude_pub = sp_nh.advertise("target_attitude", 10); - - // Set Thrust scaling in px4_config.yaml, setpoint_raw block. - if (!sp_nh.getParam("thrust_scaling", thrust_scaling)) - { - ROS_WARN_THROTTLE_NAMED(5, "setpoint_raw", "thrust_scaling parameter is unset. Attitude (and angular rate/thrust) setpoints will be ignored."); - thrust_scaling = -1.0; - } - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&SetpointRawPlugin::handle_position_target_local_ned), - make_handler(&SetpointRawPlugin::handle_position_target_global_int), - make_handler(&SetpointRawPlugin::handle_attitude_target), - }; - } + explicit SetpointRawPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "setpoint_raw"), + thrust_scaling(1.0) + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "thrust_scaling", NAN, [&](const rclcpp::Parameter & p) { + thrust_scaling = p.as_double(); + }); + + auto sensor_qos = rclcpp::SensorDataQoS(); + + local_sub = node->create_subscription( + "~/local", sensor_qos, std::bind( + &SetpointRawPlugin::local_cb, this, + _1)); + global_sub = node->create_subscription( + "~/global", + sensor_qos, std::bind( + &SetpointRawPlugin::global_cb, this, + _1)); + attitude_sub = node->create_subscription( + "~/attitude", + sensor_qos, std::bind( + &SetpointRawPlugin::attitude_cb, this, _1)); + + target_local_pub = node->create_publisher( + "~/target_local", + sensor_qos); + target_global_pub = node->create_publisher( + "~/target_global", sensor_qos); + target_attitude_pub = node->create_publisher( + "~/target_attitude", sensor_qos); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&SetpointRawPlugin::handle_position_target_local_ned), + make_handler(&SetpointRawPlugin::handle_position_target_global_int), + make_handler(&SetpointRawPlugin::handle_attitude_target), + }; + } private: - friend class SetPositionTargetLocalNEDMixin; - friend class SetPositionTargetGlobalIntMixin; - friend class SetAttitudeTargetMixin; - ros::NodeHandle sp_nh; - - ros::Subscriber local_sub, global_sub, attitude_sub; - ros::Publisher target_local_pub, target_global_pub, target_attitude_pub; - - double thrust_scaling; - - /* -*- message handlers -*- */ - void handle_position_target_local_ned(const mavlink::mavlink_message_t *msg, mavlink::common::msg::POSITION_TARGET_LOCAL_NED &tgt) - { - // Transform desired position,velocities,and accels from ENU to NED frame - auto position = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.x, tgt.y, tgt.z)); - auto velocity = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.vx, tgt.vy, tgt.vz)); - auto af = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.afx, tgt.afy, tgt.afz)); - float yaw = ftf::quaternion_get_yaw( - ftf::transform_orientation_aircraft_baselink( - ftf::transform_orientation_ned_enu( - ftf::quaternion_from_rpy(0.0, 0.0, tgt.yaw)))); - Eigen::Vector3d ang_vel_ned(0.0, 0.0, tgt.yaw_rate); - auto ang_vel_enu = ftf::transform_frame_ned_enu(ang_vel_ned); - float yaw_rate = ang_vel_enu.z(); - - auto target = boost::make_shared(); - - target->header.stamp = m_uas->synchronise_stamp(tgt.time_boot_ms); - target->coordinate_frame = tgt.coordinate_frame; - target->type_mask = tgt.type_mask; - tf::pointEigenToMsg(position, target->position); - tf::vectorEigenToMsg(velocity, target->velocity); - tf::vectorEigenToMsg(af, target->acceleration_or_force); - target->yaw = yaw; - target->yaw_rate = yaw_rate; - - target_local_pub.publish(target); - } - - void handle_position_target_global_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::POSITION_TARGET_GLOBAL_INT &tgt) - { - // Transform desired velocities from ENU to NED frame - auto velocity = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.vx, tgt.vy, tgt.vz)); - auto af = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.afx, tgt.afy, tgt.afz)); - float yaw = ftf::quaternion_get_yaw( - ftf::transform_orientation_aircraft_baselink( - ftf::transform_orientation_ned_enu( - ftf::quaternion_from_rpy(0.0, 0.0, tgt.yaw)))); - Eigen::Vector3d ang_vel_ned(0.0, 0.0, tgt.yaw_rate); - auto ang_vel_enu = ftf::transform_frame_ned_enu(ang_vel_ned); - float yaw_rate = ang_vel_enu.z(); - - auto target = boost::make_shared(); - - target->header.stamp = m_uas->synchronise_stamp(tgt.time_boot_ms); - target->coordinate_frame = tgt.coordinate_frame; - target->type_mask = tgt.type_mask; - target->latitude = tgt.lat_int / 1e7; - target->longitude = tgt.lon_int / 1e7; - target->altitude = tgt.alt; - tf::vectorEigenToMsg(velocity, target->velocity); - tf::vectorEigenToMsg(af, target->acceleration_or_force); - target->yaw = yaw; - target->yaw_rate = yaw_rate; - - target_global_pub.publish(target); - } - - void handle_attitude_target(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE_TARGET &tgt) - { - // Transform orientation from baselink -> ENU - // to aircraft -> NED - auto orientation = ftf::transform_orientation_ned_enu( - ftf::transform_orientation_baselink_aircraft( - ftf::mavlink_to_quaternion(tgt.q))); - - auto body_rate = ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(tgt.body_roll_rate, tgt.body_pitch_rate, tgt.body_yaw_rate)); - - auto target = boost::make_shared(); - - target->header.stamp = m_uas->synchronise_stamp(tgt.time_boot_ms); - target->type_mask = tgt.type_mask; - tf::quaternionEigenToMsg(orientation, target->orientation); - tf::vectorEigenToMsg(body_rate, target->body_rate); - target->thrust = tgt.thrust; - - target_attitude_pub.publish(target); - } - - /* -*- callbacks -*- */ - - void local_cb(const mavros_msgs::PositionTarget::ConstPtr &req) - { - Eigen::Vector3d position, velocity, af; - float yaw, yaw_rate; - - tf::pointMsgToEigen(req->position, position); - tf::vectorMsgToEigen(req->velocity, velocity); - tf::vectorMsgToEigen(req->acceleration_or_force, af); - - // Transform frame ENU->NED - if (req->coordinate_frame == mavros_msgs::PositionTarget::FRAME_BODY_NED || req->coordinate_frame == mavros_msgs::PositionTarget::FRAME_BODY_OFFSET_NED) { - position = ftf::transform_frame_baselink_aircraft(position); - velocity = ftf::transform_frame_baselink_aircraft(velocity); - af = ftf::transform_frame_baselink_aircraft(af); - yaw = ftf::quaternion_get_yaw( - ftf::transform_orientation_absolute_frame_aircraft_baselink( - ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))); - } else { - position = ftf::transform_frame_enu_ned(position); - velocity = ftf::transform_frame_enu_ned(velocity); - af = ftf::transform_frame_enu_ned(af); - yaw = ftf::quaternion_get_yaw( - ftf::transform_orientation_aircraft_baselink( - ftf::transform_orientation_ned_enu( - ftf::quaternion_from_rpy(0.0, 0.0, req->yaw)))); - } - - Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate); - auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu); - yaw_rate = ang_vel_ned.z(); - - set_position_target_local_ned( - req->header.stamp.toNSec() / 1000000, - req->coordinate_frame, - req->type_mask, - position, - velocity, - af, - yaw, yaw_rate); - } - - void global_cb(const mavros_msgs::GlobalPositionTarget::ConstPtr &req) - { - Eigen::Vector3d velocity, af; - float yaw, yaw_rate; - - tf::vectorMsgToEigen(req->velocity, velocity); - tf::vectorMsgToEigen(req->acceleration_or_force, af); - - // Transform frame ENU->NED - velocity = ftf::transform_frame_enu_ned(velocity); - af = ftf::transform_frame_enu_ned(af); - yaw = ftf::quaternion_get_yaw( - ftf::transform_orientation_aircraft_baselink( - ftf::transform_orientation_ned_enu( - ftf::quaternion_from_rpy(0.0, 0.0, req->yaw)))); - Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate); - auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu); - yaw_rate = ang_vel_ned.z(); - - set_position_target_global_int( - req->header.stamp.toNSec() / 1000000, - req->coordinate_frame, - req->type_mask, - req->latitude * 1e7, - req->longitude * 1e7, - req->altitude, - velocity, - af, - yaw, yaw_rate); - } - - void attitude_cb(const mavros_msgs::AttitudeTarget::ConstPtr &req) - { - Eigen::Quaterniond desired_orientation; - Eigen::Vector3d baselink_angular_rate; - Eigen::Vector3d body_rate; - double thrust; - - // ignore thrust is false by default, unless no thrust scaling is set or thrust is zero - auto ignore_thrust = req->thrust != 0.0 && thrust_scaling < 0.0; - - if (ignore_thrust) { - // I believe it's safer without sending zero thrust, but actually ignoring the actuation. - ROS_FATAL_THROTTLE_NAMED(5, "setpoint_raw", "Recieved thrust, but ignore_thrust is true: " - "the most likely cause of this is a failure to specify the thrust_scaling parameters " - "on px4/apm_config.yaml. Actuation will be ignored."); - return; - } else { - if (thrust_scaling == 0.0) { - ROS_WARN_THROTTLE_NAMED(5, "setpoint_raw", "thrust_scaling parameter is set to zero."); - } - thrust = std::min(1.0, std::max(0.0, req->thrust * thrust_scaling)); - } - - // Take care of attitude setpoint - desired_orientation = ftf::to_eigen(req->orientation); - - // Transform desired orientation to represent aircraft->NED, - // MAVROS operates on orientation of base_link->ENU - auto ned_desired_orientation = ftf::transform_orientation_enu_ned( - ftf::transform_orientation_baselink_aircraft(desired_orientation)); - - body_rate = ftf::transform_frame_baselink_aircraft( - ftf::to_eigen(req->body_rate)); - - set_attitude_target( - req->header.stamp.toNSec() / 1000000, - req->type_mask, - ned_desired_orientation, - body_rate, - thrust); - - } + friend class plugin::SetPositionTargetLocalNEDMixin; + friend class plugin::SetPositionTargetGlobalIntMixin; + friend class plugin::SetAttitudeTargetMixin; + + rclcpp::Subscription::SharedPtr local_sub; + rclcpp::Subscription::SharedPtr global_sub; + rclcpp::Subscription::SharedPtr attitude_sub; + + rclcpp::Publisher::SharedPtr target_local_pub; + rclcpp::Publisher::SharedPtr target_global_pub; + rclcpp::Publisher::SharedPtr target_attitude_pub; + + double thrust_scaling; + + /* -*- message handlers -*- */ + void handle_position_target_local_ned( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::POSITION_TARGET_LOCAL_NED & tgt, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // Transform desired position,velocities,and accels from ENU to NED frame + auto position = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.x, tgt.y, tgt.z)); + auto velocity = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.vx, tgt.vy, tgt.vz)); + auto af = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.afx, tgt.afy, tgt.afz)); + float yaw = ftf::quaternion_get_yaw( + ftf::transform_orientation_aircraft_baselink( + ftf::transform_orientation_ned_enu( + ftf::quaternion_from_rpy(0.0, 0.0, tgt.yaw)))); + Eigen::Vector3d ang_vel_ned(0.0, 0.0, tgt.yaw_rate); + auto ang_vel_enu = ftf::transform_frame_ned_enu(ang_vel_ned); + float yaw_rate = ang_vel_enu.z(); + + auto target = mavros_msgs::msg::PositionTarget(); + target.header.stamp = uas->synchronise_stamp(tgt.time_boot_ms); + target.coordinate_frame = tgt.coordinate_frame; + target.type_mask = tgt.type_mask; + target.position = tf2::toMsg(position); + tf2::toMsg(velocity, target.velocity); + tf2::toMsg(af, target.acceleration_or_force); + target.yaw = yaw; + target.yaw_rate = yaw_rate; + + target_local_pub->publish(target); + } + + void handle_position_target_global_int( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::POSITION_TARGET_GLOBAL_INT & tgt, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // Transform desired velocities from ENU to NED frame + auto velocity = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.vx, tgt.vy, tgt.vz)); + auto af = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.afx, tgt.afy, tgt.afz)); + float yaw = ftf::quaternion_get_yaw( + ftf::transform_orientation_aircraft_baselink( + ftf::transform_orientation_ned_enu( + ftf::quaternion_from_rpy(0.0, 0.0, tgt.yaw)))); + Eigen::Vector3d ang_vel_ned(0.0, 0.0, tgt.yaw_rate); + auto ang_vel_enu = ftf::transform_frame_ned_enu(ang_vel_ned); + float yaw_rate = ang_vel_enu.z(); + + auto target = mavros_msgs::msg::GlobalPositionTarget(); + target.header.stamp = uas->synchronise_stamp(tgt.time_boot_ms); + target.coordinate_frame = tgt.coordinate_frame; + target.type_mask = tgt.type_mask; + target.latitude = tgt.lat_int / 1e7; + target.longitude = tgt.lon_int / 1e7; + target.altitude = tgt.alt; + tf2::toMsg(velocity, target.velocity); + tf2::toMsg(af, target.acceleration_or_force); + target.yaw = yaw; + target.yaw_rate = yaw_rate; + + target_global_pub->publish(target); + } + + void handle_attitude_target( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::ATTITUDE_TARGET & tgt, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // Transform orientation from baselink -> ENU + // to aircraft -> NED + auto orientation = ftf::transform_orientation_ned_enu( + ftf::transform_orientation_baselink_aircraft( + ftf::mavlink_to_quaternion(tgt.q))); + + auto body_rate = + ftf::transform_frame_baselink_aircraft( + Eigen::Vector3d( + tgt.body_roll_rate, + tgt.body_pitch_rate, tgt.body_yaw_rate)); + + auto target = mavros_msgs::msg::AttitudeTarget(); + target.header.stamp = uas->synchronise_stamp(tgt.time_boot_ms); + target.type_mask = tgt.type_mask; + target.orientation = tf2::toMsg(orientation); + tf2::toMsg(body_rate, target.body_rate); + target.thrust = tgt.thrust; + + target_attitude_pub->publish(target); + } + + /* -*- callbacks -*- */ + + void local_cb(const mavros_msgs::msg::PositionTarget::SharedPtr req) + { + using mavros_msgs::msg::PositionTarget; + + Eigen::Vector3d position, velocity, af; + float yaw, yaw_rate; + + tf2::fromMsg(req->position, position); + tf2::fromMsg(req->velocity, velocity); + tf2::fromMsg(req->acceleration_or_force, af); + + // Transform frame ENU->NED + if (req->coordinate_frame == PositionTarget::FRAME_BODY_NED || + req->coordinate_frame == PositionTarget::FRAME_BODY_OFFSET_NED) + { + position = ftf::transform_frame_baselink_aircraft(position); + velocity = ftf::transform_frame_baselink_aircraft(velocity); + af = ftf::transform_frame_baselink_aircraft(af); + yaw = ftf::quaternion_get_yaw( + ftf::transform_orientation_absolute_frame_aircraft_baselink( + ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))); + } else { + position = ftf::transform_frame_enu_ned(position); + velocity = ftf::transform_frame_enu_ned(velocity); + af = ftf::transform_frame_enu_ned(af); + yaw = ftf::quaternion_get_yaw( + ftf::transform_orientation_aircraft_baselink( + ftf::transform_orientation_ned_enu( + ftf::quaternion_from_rpy(0.0, 0.0, req->yaw)))); + } + + Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate); + auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu); + yaw_rate = ang_vel_ned.z(); + + set_position_target_local_ned( + get_time_boot_ms( + req->header.stamp), + req->coordinate_frame, + req->type_mask, + position, + velocity, + af, + yaw, yaw_rate); + } + + void global_cb(const mavros_msgs::msg::GlobalPositionTarget::SharedPtr req) + { + Eigen::Vector3d velocity, af; + float yaw, yaw_rate; + + tf2::fromMsg(req->velocity, velocity); + tf2::fromMsg(req->acceleration_or_force, af); + + // Transform frame ENU->NED + velocity = ftf::transform_frame_enu_ned(velocity); + af = ftf::transform_frame_enu_ned(af); + yaw = ftf::quaternion_get_yaw( + ftf::transform_orientation_aircraft_baselink( + ftf::transform_orientation_ned_enu( + ftf::quaternion_from_rpy(0.0, 0.0, req->yaw)))); + Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate); + auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu); + yaw_rate = ang_vel_ned.z(); + + set_position_target_global_int( + get_time_boot_ms(req->header.stamp), + req->coordinate_frame, + req->type_mask, + req->latitude * 1e7, + req->longitude * 1e7, + req->altitude, + velocity, + af, + yaw, yaw_rate); + } + + void attitude_cb(const mavros_msgs::msg::AttitudeTarget::SharedPtr req) + { + Eigen::Quaterniond desired_orientation; + Eigen::Vector3d baselink_angular_rate; + Eigen::Vector3d body_rate; + double thrust; + + // Set Thrust scaling in px4_config.yaml, setpoint_raw block. + // ignore thrust is false by default, unless no thrust scaling is set or thrust is zero + auto ignore_thrust = req->thrust != 0.0 && std::isnan(thrust_scaling); + + if (ignore_thrust) { + // I believe it's safer without sending zero thrust, but actually ignoring the actuation. + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000, + "Recieved thrust, but ignore_thrust is true: " + "the most likely cause of this is a failure to specify the thrust_scaling parameters " + "on px4/apm_config.yaml. Actuation will be ignored."); + return; + } else { + if (thrust_scaling == 0.0) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "thrust_scaling parameter is set to zero."); + } + if (std::isnan(thrust_scaling)) { + thrust_scaling = 1.0; + } + thrust = std::min(1.0, std::max(0.0, req->thrust * thrust_scaling)); + + // Take care of attitude setpoint + desired_orientation = ftf::to_eigen(req->orientation); + + // Transform desired orientation to represent aircraft->NED, + // MAVROS operates on orientation of base_link->ENU + auto ned_desired_orientation = ftf::transform_orientation_enu_ned( + ftf::transform_orientation_baselink_aircraft(desired_orientation)); + + body_rate = ftf::transform_frame_baselink_aircraft( + ftf::to_eigen(req->body_rate)); + + set_attitude_target( + get_time_boot_ms(req->header.stamp), + req->type_mask, + ned_desired_orientation, + body_rate, + thrust); + } + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SetpointRawPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::SetpointRawPlugin) diff --git a/mavros/src/plugins/setpoint_trajectory.cpp b/mavros/src/plugins/setpoint_trajectory.cpp index 091f25c16..95c5386e2 100644 --- a/mavros/src/plugins/setpoint_trajectory.cpp +++ b/mavros/src/plugins/setpoint_trajectory.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2019 Jaeyoung Lim. + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief SetpointTRAJECTORY plugin * @file setpoint_trajectory.cpp @@ -6,229 +14,261 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2019 Jaeyoung Lim. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace mavros { -namespace std_plugins { +#include + +#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" +#include "mavros/setpoint_mixin.hpp" + +#include "nav_msgs/msg/path.hpp" +#include "mavros_msgs/msg/position_target.hpp" +#include "trajectory_msgs/msg/multi_dof_joint_trajectory.hpp" +#include "trajectory_msgs/msg/multi_dof_joint_trajectory_point.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT using mavlink::common::MAV_FRAME; /** * @brief Setpoint TRAJECTORY plugin + * @plugin setpoint_trajectory * * Receive trajectory setpoints and send setpoint_raw setpoints along the trajectory. */ -class SetpointTrajectoryPlugin : public plugin::PluginBase, - private plugin::SetPositionTargetLocalNEDMixin { +class SetpointTrajectoryPlugin : public plugin::Plugin, + private plugin::SetPositionTargetLocalNEDMixin +{ public: - SetpointTrajectoryPlugin() : PluginBase(), - sp_nh("~setpoint_trajectory") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - sp_nh.param("frame_id", frame_id, "map"); - - local_sub = sp_nh.subscribe("local", 10, &SetpointTrajectoryPlugin::local_cb, this); - desired_pub = sp_nh.advertise("desired", 10); - - trajectory_reset_srv = sp_nh.advertiseService("reset", &SetpointTrajectoryPlugin::reset_cb, this); - mav_frame_srv = sp_nh.advertiseService("mav_frame", &SetpointTrajectoryPlugin::set_mav_frame_cb, this); - - sp_timer = sp_nh.createTimer(ros::Duration(0.01), &SetpointTrajectoryPlugin::reference_cb, this, true); - - // mav_frame - std::string mav_frame_str; - if (!sp_nh.getParam("mav_frame", mav_frame_str)) { - mav_frame = MAV_FRAME::LOCAL_NED; - } else { - mav_frame = utils::mav_frame_from_str(mav_frame_str); - } - } - - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + explicit SetpointTrajectoryPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "setpoint_trajectory") + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "frame_id", "map", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + + node_declare_and_watch_parameter( + "mav_frame", "LOCAL_NED", [&](const rclcpp::Parameter & p) { + auto mav_frame_str = p.as_string(); + auto new_mav_frame = utils::mav_frame_from_str(mav_frame_str); + + if (new_mav_frame == MAV_FRAME::LOCAL_NED && mav_frame_str != "LOCAL_NED") { + throw rclcpp::exceptions::InvalidParameterValueException( + utils::format( + "unknown MAV_FRAME: %s", + mav_frame_str.c_str())); + } + mav_frame = new_mav_frame; + }); + + auto sensor_qos = rclcpp::SensorDataQoS(); + + local_sub = node->create_subscription( + "~/local", + sensor_qos, std::bind( + &SetpointTrajectoryPlugin::local_cb, this, + _1)); + desired_pub = node->create_publisher("~/desired", sensor_qos); + + trajectory_reset_srv = + node->create_service( + "~/reset", + std::bind(&SetpointTrajectoryPlugin::reset_cb, this, _1, _2)); + + reset_timer(10ms); + } + + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - friend class SetPositionTargetLocalNEDMixin; - using lock_guard = std::lock_guard; - std::mutex mutex; - - ros::NodeHandle sp_nh; - - ros::Timer sp_timer; - - ros::Subscriber local_sub; - ros::Publisher desired_pub; - - ros::ServiceServer trajectory_reset_srv; - ros::ServiceServer mav_frame_srv; - - trajectory_msgs::MultiDOFJointTrajectory::ConstPtr trajectory_target_msg; - std::vector::const_iterator setpoint_target; - std::vector::const_iterator next_setpoint_target; - - std::string frame_id; - MAV_FRAME mav_frame; - ftf::StaticTF transform; - - void reset_timer(ros::Duration duration){ - sp_timer.stop(); - sp_timer.setPeriod(duration); - sp_timer.start(); - } - - void publish_path(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr &req){ - nav_msgs::Path msg; - - msg.header.stamp = ros::Time::now(); - msg.header.frame_id = frame_id; - for (const auto &p : req->points) { - if (p.transforms.empty()) - continue; - - geometry_msgs::PoseStamped pose_msg; - pose_msg.pose.position.x = p.transforms[0].translation.x; - pose_msg.pose.position.y = p.transforms[0].translation.y; - pose_msg.pose.position.z = p.transforms[0].translation.z; - pose_msg.pose.orientation = p.transforms[0].rotation; - msg.poses.emplace_back(pose_msg); - } - desired_pub.publish(msg); - } - - /* -*- callbacks -*- */ - - void local_cb(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr &req) - { - lock_guard lock(mutex); - - if(static_cast(mav_frame) == MAV_FRAME::BODY_NED || static_cast(mav_frame) == MAV_FRAME::BODY_OFFSET_NED){ - transform = ftf::StaticTF::BASELINK_TO_AIRCRAFT; - } else { - transform = ftf::StaticTF::ENU_TO_NED; - } - - trajectory_target_msg = req; - - // Read first duration of the setpoint and set the timer - setpoint_target = req->points.cbegin(); - reset_timer(setpoint_target->time_from_start); - publish_path(req); - } - - void reference_cb(const ros::TimerEvent &event) - { - using mavlink::common::POSITION_TARGET_TYPEMASK; - lock_guard lock(mutex); - - if(!trajectory_target_msg) - return; - - Eigen::Vector3d position, velocity, af; - Eigen::Quaterniond attitude = Eigen::Quaterniond::Identity(); - float yaw = 0.f; - float yaw_rate = 0.f; - uint16_t type_mask = 0; - if(!setpoint_target->transforms.empty()){ - position = ftf::detail::transform_static_frame(ftf::to_eigen(setpoint_target->transforms[0].translation), transform); - attitude = ftf::detail::transform_orientation(ftf::to_eigen(setpoint_target->transforms[0].rotation), transform); - - } else { - type_mask = type_mask | uint16_t(POSITION_TARGET_TYPEMASK::X_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::Y_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::Z_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::YAW_IGNORE); - - } - - if(!setpoint_target->velocities.empty()){ - velocity = ftf::detail::transform_static_frame(ftf::to_eigen(setpoint_target->velocities[0].linear), transform); - yaw_rate = setpoint_target->velocities[0].angular.z; - - } else { - type_mask = type_mask | uint16_t(POSITION_TARGET_TYPEMASK::VX_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::VY_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::VZ_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::YAW_RATE_IGNORE); - - } - - if(!setpoint_target->accelerations.empty()){ - af = ftf::detail::transform_static_frame(ftf::to_eigen(setpoint_target->accelerations[0].linear), transform); - - } else { - type_mask = type_mask | uint16_t(POSITION_TARGET_TYPEMASK::AX_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::AY_IGNORE) - | uint16_t(POSITION_TARGET_TYPEMASK::AZ_IGNORE); - - } - - set_position_target_local_ned( - ros::Time::now().toNSec() / 1000000, - utils::enum_value(mav_frame), - type_mask, - position, - velocity, - af, - ftf::quaternion_get_yaw(attitude), - yaw_rate); - - next_setpoint_target = setpoint_target + 1; - if (next_setpoint_target != trajectory_target_msg->points.cend()) { - reset_timer(setpoint_target->time_from_start); - setpoint_target = next_setpoint_target; - } else { - trajectory_target_msg.reset(); - } - - return; - } - - bool reset_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - if(trajectory_target_msg){ - trajectory_target_msg.reset(); - res.success = true; - } else { - res.success = false; - res.message = "Trajectory reset denied: Empty trajectory"; - } - - return true; - } - - bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res) - { - mav_frame = static_cast(req.mav_frame); - const std::string mav_frame_str = utils::to_string(mav_frame); - sp_nh.setParam("mav_frame", mav_frame_str); - res.success = true; - return true; - } + friend class plugin::SetPositionTargetLocalNEDMixin; + using lock_guard = std::lock_guard; + using V_Point = std::vector; + + std::mutex mutex; + + rclcpp::Subscription::SharedPtr local_sub; + rclcpp::Publisher::SharedPtr desired_pub; + rclcpp::Service::SharedPtr trajectory_reset_srv; + rclcpp::TimerBase::SharedPtr sp_timer; + + trajectory_msgs::msg::MultiDOFJointTrajectory::SharedPtr trajectory_target_msg; + + V_Point::const_iterator setpoint_target; + V_Point::const_iterator next_setpoint_target; + + std::string frame_id; + MAV_FRAME mav_frame; + ftf::StaticTF transform; + + void reset_timer(const builtin_interfaces::msg::Duration & dur) + { + reset_timer(rclcpp::Duration(dur).to_chrono()); + } + + void reset_timer(const std::chrono::nanoseconds dur) + { + if (sp_timer) { + sp_timer->cancel(); + } + + sp_timer = + node->create_wall_timer(dur, std::bind(&SetpointTrajectoryPlugin::reference_cb, this)); + } + + void publish_path(const trajectory_msgs::msg::MultiDOFJointTrajectory::SharedPtr req) + { + nav_msgs::msg::Path msg; + + msg.header.stamp = node->now(); + msg.header.frame_id = frame_id; + for (const auto & p : req->points) { + if (p.transforms.empty()) { + continue; + } + + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.pose.position.x = p.transforms[0].translation.x; + pose_msg.pose.position.y = p.transforms[0].translation.y; + pose_msg.pose.position.z = p.transforms[0].translation.z; + pose_msg.pose.orientation = p.transforms[0].rotation; + msg.poses.emplace_back(pose_msg); + } + desired_pub->publish(msg); + } + + /* -*- callbacks -*- */ + + void local_cb(const trajectory_msgs::msg::MultiDOFJointTrajectory::SharedPtr req) + { + lock_guard lock(mutex); + + if (mav_frame == MAV_FRAME::BODY_NED || + mav_frame == MAV_FRAME::BODY_OFFSET_NED) + { + transform = ftf::StaticTF::BASELINK_TO_AIRCRAFT; + } else { + transform = ftf::StaticTF::ENU_TO_NED; + } + + trajectory_target_msg = req; + + // Read first duration of the setpoint and set the timer + setpoint_target = req->points.cbegin(); + reset_timer(setpoint_target->time_from_start); + publish_path(req); + } + + void reference_cb() + { + using mavlink::common::POSITION_TARGET_TYPEMASK; + lock_guard lock(mutex); + + if (!trajectory_target_msg) { + return; + } + + Eigen::Vector3d position, velocity, af; + Eigen::Quaterniond attitude = Eigen::Quaterniond::Identity(); + float yaw_rate = 0; + uint16_t type_mask = 0; + + if (!setpoint_target->transforms.empty()) { + position = + ftf::detail::transform_static_frame( + ftf::to_eigen( + setpoint_target->transforms[0].translation), transform); + attitude = + ftf::detail::transform_orientation( + ftf::to_eigen( + setpoint_target->transforms[0].rotation), transform); + + } else { + type_mask = type_mask | uint16_t(POSITION_TARGET_TYPEMASK::X_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::Y_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::Z_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::YAW_IGNORE); + } + + if (!setpoint_target->velocities.empty()) { + velocity = + ftf::detail::transform_static_frame( + ftf::to_eigen( + setpoint_target->velocities[0].linear), transform); + yaw_rate = setpoint_target->velocities[0].angular.z; + + } else { + type_mask = type_mask | uint16_t(POSITION_TARGET_TYPEMASK::VX_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::VY_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::VZ_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::YAW_RATE_IGNORE); + } + + if (!setpoint_target->accelerations.empty()) { + af = + ftf::detail::transform_static_frame( + ftf::to_eigen( + setpoint_target->accelerations[0].linear), transform); + + } else { + type_mask = type_mask | uint16_t(POSITION_TARGET_TYPEMASK::AX_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::AY_IGNORE) | + uint16_t(POSITION_TARGET_TYPEMASK::AZ_IGNORE); + } + + set_position_target_local_ned( + get_time_boot_ms(), + utils::enum_value(mav_frame), + type_mask, + position, + velocity, + af, + ftf::quaternion_get_yaw(attitude), + yaw_rate); + + next_setpoint_target = setpoint_target + 1; + if (next_setpoint_target != trajectory_target_msg->points.cend()) { + reset_timer(setpoint_target->time_from_start); + setpoint_target = next_setpoint_target; + } else { + trajectory_target_msg.reset(); + } + } + + void reset_cb( + std_srvs::srv::Trigger::Request::SharedPtr req [[maybe_unused]], + std_srvs::srv::Trigger::Response::SharedPtr res [[maybe_unused]]) + { + lock_guard lock(mutex); + + if (trajectory_target_msg) { + trajectory_target_msg.reset(); + res->success = true; + } else { + res->success = false; + res->message = "Trajectory reset denied: Empty trajectory"; + } + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SetpointTrajectoryPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::SetpointTrajectoryPlugin) diff --git a/mavros/src/plugins/setpoint_velocity.cpp b/mavros/src/plugins/setpoint_velocity.cpp index fb7eeede0..850b43ecb 100644 --- a/mavros/src/plugins/setpoint_velocity.cpp +++ b/mavros/src/plugins/setpoint_velocity.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2014 Nuno Marques. + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief SetpointVelocity plugin * @file setpoint_velocity.cpp @@ -7,139 +15,147 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014 Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include -#include +#include -#include -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" +#include "mavros/setpoint_mixin.hpp" -#include +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" -namespace mavros { -namespace std_plugins { +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT using mavlink::common::MAV_FRAME; + /** * @brief Setpoint velocity plugin + * @plugin setpoint_velocity * * Send setpoint velocities to FCU controller. */ -class SetpointVelocityPlugin : public plugin::PluginBase, - private plugin::SetPositionTargetLocalNEDMixin { +class SetpointVelocityPlugin : public plugin::Plugin, + private plugin::SetPositionTargetLocalNEDMixin +{ public: - SetpointVelocityPlugin() : PluginBase(), - sp_nh("~setpoint_velocity") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - //cmd_vel usually is the topic used for velocity control in many controllers / planners - vel_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointVelocityPlugin::vel_cb, this); - vel_unstamped_sub = sp_nh.subscribe("cmd_vel_unstamped", 10, &SetpointVelocityPlugin::vel_unstamped_cb, this); - mav_frame_srv = sp_nh.advertiseService("mav_frame", &SetpointVelocityPlugin::set_mav_frame_cb, this); - - // mav_frame - std::string mav_frame_str; - if (!sp_nh.getParam("mav_frame", mav_frame_str)) { - mav_frame = MAV_FRAME::LOCAL_NED; - } else { - mav_frame = utils::mav_frame_from_str(mav_frame_str); - } - } - - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + explicit SetpointVelocityPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "setpoint_velocity") + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "mav_frame", "LOCAL_NED", [&](const rclcpp::Parameter & p) { + auto mav_frame_str = p.as_string(); + auto new_mav_frame = utils::mav_frame_from_str(mav_frame_str); + + if (new_mav_frame == MAV_FRAME::LOCAL_NED && mav_frame_str != "LOCAL_NED") { + throw rclcpp::exceptions::InvalidParameterValueException( + utils::format( + "unknown MAV_FRAME: %s", + mav_frame_str.c_str())); + } + mav_frame = new_mav_frame; + }); + + auto sensor_qos = rclcpp::SensorDataQoS(); + + // cmd_vel usually is the topic used for velocity control in many controllers / planners + vel_sub = node->create_subscription( + "~/cmd_vel", sensor_qos, std::bind( + &SetpointVelocityPlugin::vel_cb, this, + _1)); + vel_unstamped_sub = node->create_subscription( + "~/cmd_vel_unstamped", + sensor_qos, std::bind( + &SetpointVelocityPlugin::vel_unstamped_cb, this, _1)); + } + + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - friend class SetPositionTargetLocalNEDMixin; - ros::NodeHandle sp_nh; - - ros::Subscriber vel_sub; - ros::Subscriber vel_unstamped_sub; - ros::ServiceServer mav_frame_srv; - - MAV_FRAME mav_frame; - - /* -*- mid-level helpers -*- */ - - /** - * @brief Send velocity to FCU velocity controller - * - * @warning Send only VX VY VZ. ENU frame. - */ - void send_setpoint_velocity(const ros::Time &stamp, Eigen::Vector3d &vel_enu, double yaw_rate) - { - /** - * Documentation start from bit 1 instead 0; - * Ignore position and accel vectors, yaw. - */ - uint16_t ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0); - auto vel = [&] () { - if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) { - return ftf::transform_frame_baselink_aircraft(vel_enu); - } else { - return ftf::transform_frame_enu_ned(vel_enu); - } - } (); - - auto yr = [&] () { - if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) { - return ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(0.0, 0.0, yaw_rate)); - } else { - return ftf::transform_frame_ned_enu(Eigen::Vector3d(0.0, 0.0, yaw_rate)); - } - } (); - - set_position_target_local_ned(stamp.toNSec() / 1000000, - utils::enum_value(mav_frame), - ignore_all_except_v_xyz_yr, - Eigen::Vector3d::Zero(), - vel, - Eigen::Vector3d::Zero(), - 0.0, yr.z()); - } - - /* -*- callbacks -*- */ - - void vel_cb(const geometry_msgs::TwistStamped::ConstPtr &req) { - Eigen::Vector3d vel_enu; - - tf::vectorMsgToEigen(req->twist.linear, vel_enu); - send_setpoint_velocity(req->header.stamp, vel_enu, - req->twist.angular.z); - } - - void vel_unstamped_cb(const geometry_msgs::Twist::ConstPtr &req) { - Eigen::Vector3d vel_enu; - - tf::vectorMsgToEigen(req->linear, vel_enu); - send_setpoint_velocity(ros::Time::now(), vel_enu, - req->angular.z); - } - - bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res) - { - mav_frame = static_cast(req.mav_frame); - const std::string mav_frame_str = utils::to_string(mav_frame); - sp_nh.setParam("mav_frame", mav_frame_str); - res.success = true; - return true; - } + friend class plugin::SetPositionTargetLocalNEDMixin; + + rclcpp::Subscription::SharedPtr vel_sub; + rclcpp::Subscription::SharedPtr vel_unstamped_sub; + + MAV_FRAME mav_frame; + + /* -*- mid-level helpers -*- */ + + /** + * @brief Send velocity to FCU velocity controller + * + * @warning Send only VX VY VZ. ENU frame. + */ + void send_setpoint_velocity( + const rclcpp::Time & stamp, const Eigen::Vector3d & vel_enu, + const double yaw_rate) + { + /** + * Documentation start from bit 1 instead 0; + * Ignore position and accel vectors, yaw. + */ + uint16_t ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0); + auto vel = [&]() { + if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) { + return ftf::transform_frame_baselink_aircraft(vel_enu); + } else { + return ftf::transform_frame_enu_ned(vel_enu); + } + } (); + + auto yr = [&]() { + if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) { + return ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(0.0, 0.0, yaw_rate)); + } else { + return ftf::transform_frame_ned_enu(Eigen::Vector3d(0.0, 0.0, yaw_rate)); + } + } (); + + set_position_target_local_ned( + get_time_boot_ms(stamp), + utils::enum_value(mav_frame), + ignore_all_except_v_xyz_yr, + Eigen::Vector3d::Zero(), + vel, + Eigen::Vector3d::Zero(), + 0.0, yr.z()); + } + + /* -*- callbacks -*- */ + + void vel_cb(const geometry_msgs::msg::TwistStamped::SharedPtr req) + { + Eigen::Vector3d vel_enu; + + tf2::fromMsg(req->twist.linear, vel_enu); + send_setpoint_velocity( + req->header.stamp, vel_enu, + req->twist.angular.z); + } + + void vel_unstamped_cb(const geometry_msgs::msg::Twist::SharedPtr req) + { + Eigen::Vector3d vel_enu; + + tf2::fromMsg(req->linear, vel_enu); + send_setpoint_velocity( + node->now(), vel_enu, + req->angular.z); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SetpointVelocityPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::SetpointVelocityPlugin) diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp index bfa5f116d..e2d46a692 100644 --- a/mavros/src/plugins/sys_status.cpp +++ b/mavros/src/plugins/sys_status.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2013,2014,2015,2016,2021 Vladimir Ermakov. + * Copyright 2022 Dr.-Ing. Amilcar do Carmo Lucas, IAV GmbH. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief System Status plugin * @file sys_status.cpp @@ -6,43 +14,44 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2013,2014,2015,2016 Vladimir Ermakov. - * Copyright 2022 Dr.-Ing. Amilcar do Carmo Lucas, IAV GmbH. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#ifdef HAVE_SENSOR_MSGS_BATTERYSTATE_MSG -#include -using BatteryMsg = sensor_msgs::BatteryState; -#else -#include -using BatteryMsg = mavros_msgs::BatteryStatus; -#endif - -namespace mavros { -namespace std_plugins { +#include +#include +#include +#include +#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/state.hpp" +#include "mavros_msgs/msg/estimator_status.hpp" +#include "mavros_msgs/msg/extended_state.hpp" +#include "mavros_msgs/srv/stream_rate.hpp" +#include "mavros_msgs/srv/set_mode.hpp" +#include "mavros_msgs/srv/command_long.hpp" +#include "mavros_msgs/msg/status_text.hpp" +#include "mavros_msgs/msg/vehicle_info.hpp" +#include "mavros_msgs/srv/vehicle_info_get.hpp" +#include "mavros_msgs/srv/message_interval.hpp" +#include "sensor_msgs/msg/battery_state.hpp" + +namespace mavros +{ +namespace std_plugins +{ using mavlink::minimal::MAV_TYPE; using mavlink::minimal::MAV_AUTOPILOT; using mavlink::minimal::MAV_STATE; using utils::enum_value; +using BatteryMsg = sensor_msgs::msg::BatteryState; +using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT #define MAX_NR_BATTERY_STATUS 10 @@ -54,96 +63,95 @@ using utils::enum_value; class HeartbeatStatus : public diagnostic_updater::DiagnosticTask { public: - HeartbeatStatus(const std::string &name, size_t win_size) : - diagnostic_updater::DiagnosticTask(name), - times_(win_size), - seq_nums_(win_size), - window_size_(win_size), - min_freq_(0.2), - max_freq_(100), - tolerance_(0.1), - autopilot(MAV_AUTOPILOT::GENERIC), - type(MAV_TYPE::GENERIC), - system_status(MAV_STATE::UNINIT) - { - clear(); - } - - void clear() - { - std::lock_guard lock(mutex); - ros::Time curtime = ros::Time::now(); - count_ = 0; - - for (size_t i = 0; i < window_size_; i++) { - times_[i] = curtime; - seq_nums_[i] = count_; - } - - hist_indx_ = 0; - } - - void tick(uint8_t type_, uint8_t autopilot_, - std::string &mode_, uint8_t system_status_) - { - std::lock_guard lock(mutex); - count_++; - - type = static_cast(type_); - autopilot = static_cast(autopilot_); - mode = mode_; - system_status = static_cast(system_status_); - } - - void run(diagnostic_updater::DiagnosticStatusWrapper &stat) - { - std::lock_guard lock(mutex); - - ros::Time curtime = ros::Time::now(); - int curseq = count_; - int events = curseq - seq_nums_[hist_indx_]; - double window = (curtime - times_[hist_indx_]).toSec(); - double freq = events / window; - seq_nums_[hist_indx_] = curseq; - times_[hist_indx_] = curtime; - hist_indx_ = (hist_indx_ + 1) % window_size_; - - if (events == 0) { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No events recorded."); - } - else if (freq < min_freq_ * (1 - tolerance_)) { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Frequency too low."); - } - else if (freq > max_freq_ * (1 + tolerance_)) { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Frequency too high."); - } - else { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Normal"); - } - - stat.addf("Heartbeats since startup", "%d", count_); - stat.addf("Frequency (Hz)", "%f", freq); - stat.add("Vehicle type", utils::to_string(type)); - stat.add("Autopilot type", utils::to_string(autopilot)); - stat.add("Mode", mode); - stat.add("System status", utils::to_string(system_status)); - } + HeartbeatStatus(const std::string & name, size_t win_size) + : diagnostic_updater::DiagnosticTask(name), + times_(win_size), + seq_nums_(win_size), + window_size_(win_size), + min_freq_(0.2), + max_freq_(100), + tolerance_(0.1), + autopilot(MAV_AUTOPILOT::GENERIC), + type(MAV_TYPE::GENERIC), + system_status(MAV_STATE::UNINIT) + { + clear(); + } + + void clear() + { + std::lock_guard lock(mutex); + rclcpp::Time curtime = clock.now(); + count_ = 0; + + for (size_t i = 0; i < window_size_; i++) { + times_[i] = curtime; + seq_nums_[i] = count_; + } + + hist_indx_ = 0; + } + + void tick( + uint8_t type_, uint8_t autopilot_, + std::string & mode_, uint8_t system_status_) + { + std::lock_guard lock(mutex); + count_++; + + type = static_cast(type_); + autopilot = static_cast(autopilot_); + mode = mode_; + system_status = static_cast(system_status_); + } + + void run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + std::lock_guard lock(mutex); + + rclcpp::Time curtime = clock.now(); + int curseq = count_; + int events = curseq - seq_nums_[hist_indx_]; + double window = (curtime - times_[hist_indx_]).seconds(); + double freq = events / window; + seq_nums_[hist_indx_] = curseq; + times_[hist_indx_] = curtime; + hist_indx_ = (hist_indx_ + 1) % window_size_; + + if (events == 0) { + stat.summary(DiagnosticStatus::ERROR, "No events recorded."); + } else if (freq < min_freq_ * (1 - tolerance_)) { + stat.summary(DiagnosticStatus::WARN, "Frequency too low."); + } else if (freq > max_freq_ * (1 + tolerance_)) { + stat.summary(DiagnosticStatus::WARN, "Frequency too high."); + } else { + stat.summary(DiagnosticStatus::OK, "Normal"); + } + + stat.addf("Heartbeats since startup", "%d", count_); + stat.addf("Frequency (Hz)", "%f", freq); + stat.add("Vehicle type", utils::to_string(type)); + stat.add("Autopilot type", utils::to_string(autopilot)); + stat.add("Mode", mode); + stat.add("System status", utils::to_string(system_status)); + } private: - int count_; - std::vector times_; - std::vector seq_nums_; - int hist_indx_; - std::mutex mutex; - const size_t window_size_; - const double min_freq_; - const double max_freq_; - const double tolerance_; - - MAV_AUTOPILOT autopilot; - MAV_TYPE type; - std::string mode; - MAV_STATE system_status; + rclcpp::Clock clock; + int count_; + std::vector times_; + std::vector seq_nums_; + int hist_indx_; + std::mutex mutex; + const size_t window_size_; + const double min_freq_; + const double max_freq_; + const double tolerance_; + + MAV_AUTOPILOT autopilot; + MAV_TYPE type; + std::string mode; + MAV_STATE system_status; }; @@ -153,131 +161,110 @@ class HeartbeatStatus : public diagnostic_updater::DiagnosticTask class SystemStatusDiag : public diagnostic_updater::DiagnosticTask { public: - SystemStatusDiag(const std::string &name) : - diagnostic_updater::DiagnosticTask(name), - last_st {} - { } - - void set(mavlink::common::msg::SYS_STATUS &st) - { - std::lock_guard lock(mutex); - last_st = st; - } - - void run(diagnostic_updater::DiagnosticStatusWrapper &stat) { - std::lock_guard lock(mutex); - - if ((last_st.onboard_control_sensors_health & last_st.onboard_control_sensors_enabled) - != last_st.onboard_control_sensors_enabled) - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Sensor health"); - else - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Normal"); - - stat.addf("Sensor present", "0x%08X", last_st.onboard_control_sensors_present); - stat.addf("Sensor enabled", "0x%08X", last_st.onboard_control_sensors_enabled); - stat.addf("Sensor health", "0x%08X", last_st.onboard_control_sensors_health); - - using STS = mavlink::common::MAV_SYS_STATUS_SENSOR; - - // [[[cog: - // import pymavlink.dialects.v20.common as common - // ename = 'MAV_SYS_STATUS_SENSOR' - // ename_pfx2 = 'MAV_SYS_STATUS_' - // - // enum = sorted(common.enums[ename].items()) - // enum.pop() # -> remove ENUM_END - // - // for k, e in enum: - // desc = e.description.split(' ', 1)[1] if e.description.startswith('0x') else e.description - // sts = e.name - // - // if sts.startswith(ename + '_'): - // sts = sts[len(ename) + 1:] - // if sts.startswith(ename_pfx2): - // sts = sts[len(ename_pfx2):] - // if sts[0].isdigit(): - // sts = 'SENSOR_' + sts - // - // cog.outl(f"""\ - // if (last_st.onboard_control_sensors_enabled & enum_value(STS::{sts})) - // \tstat.add("{desc.strip()}", (last_st.onboard_control_sensors_health & enum_value(STS::{sts})) ? "Ok" : "Fail");""") - // ]]] - if (last_st.onboard_control_sensors_enabled & enum_value(STS::SENSOR_3D_GYRO)) - stat.add("3D gyro", (last_st.onboard_control_sensors_health & enum_value(STS::SENSOR_3D_GYRO)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::SENSOR_3D_ACCEL)) - stat.add("3D accelerometer", (last_st.onboard_control_sensors_health & enum_value(STS::SENSOR_3D_ACCEL)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::SENSOR_3D_MAG)) - stat.add("3D magnetometer", (last_st.onboard_control_sensors_health & enum_value(STS::SENSOR_3D_MAG)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::ABSOLUTE_PRESSURE)) - stat.add("absolute pressure", (last_st.onboard_control_sensors_health & enum_value(STS::ABSOLUTE_PRESSURE)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::DIFFERENTIAL_PRESSURE)) - stat.add("differential pressure", (last_st.onboard_control_sensors_health & enum_value(STS::DIFFERENTIAL_PRESSURE)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::GPS)) - stat.add("GPS", (last_st.onboard_control_sensors_health & enum_value(STS::GPS)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::OPTICAL_FLOW)) - stat.add("optical flow", (last_st.onboard_control_sensors_health & enum_value(STS::OPTICAL_FLOW)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::VISION_POSITION)) - stat.add("computer vision position", (last_st.onboard_control_sensors_health & enum_value(STS::VISION_POSITION)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::LASER_POSITION)) - stat.add("laser based position", (last_st.onboard_control_sensors_health & enum_value(STS::LASER_POSITION)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::EXTERNAL_GROUND_TRUTH)) - stat.add("external ground truth (Vicon or Leica)", (last_st.onboard_control_sensors_health & enum_value(STS::EXTERNAL_GROUND_TRUTH)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::ANGULAR_RATE_CONTROL)) - stat.add("3D angular rate control", (last_st.onboard_control_sensors_health & enum_value(STS::ANGULAR_RATE_CONTROL)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::ATTITUDE_STABILIZATION)) - stat.add("attitude stabilization", (last_st.onboard_control_sensors_health & enum_value(STS::ATTITUDE_STABILIZATION)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::YAW_POSITION)) - stat.add("yaw position", (last_st.onboard_control_sensors_health & enum_value(STS::YAW_POSITION)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::Z_ALTITUDE_CONTROL)) - stat.add("z/altitude control", (last_st.onboard_control_sensors_health & enum_value(STS::Z_ALTITUDE_CONTROL)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::XY_POSITION_CONTROL)) - stat.add("x/y position control", (last_st.onboard_control_sensors_health & enum_value(STS::XY_POSITION_CONTROL)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::MOTOR_OUTPUTS)) - stat.add("motor outputs / control", (last_st.onboard_control_sensors_health & enum_value(STS::MOTOR_OUTPUTS)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::RC_RECEIVER)) - stat.add("rc receiver", (last_st.onboard_control_sensors_health & enum_value(STS::RC_RECEIVER)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::SENSOR_3D_GYRO2)) - stat.add("2nd 3D gyro", (last_st.onboard_control_sensors_health & enum_value(STS::SENSOR_3D_GYRO2)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::SENSOR_3D_ACCEL2)) - stat.add("2nd 3D accelerometer", (last_st.onboard_control_sensors_health & enum_value(STS::SENSOR_3D_ACCEL2)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::SENSOR_3D_MAG2)) - stat.add("2nd 3D magnetometer", (last_st.onboard_control_sensors_health & enum_value(STS::SENSOR_3D_MAG2)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::GEOFENCE)) - stat.add("geofence", (last_st.onboard_control_sensors_health & enum_value(STS::GEOFENCE)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::AHRS)) - stat.add("AHRS subsystem health", (last_st.onboard_control_sensors_health & enum_value(STS::AHRS)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::TERRAIN)) - stat.add("Terrain subsystem health", (last_st.onboard_control_sensors_health & enum_value(STS::TERRAIN)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::REVERSE_MOTOR)) - stat.add("Motors are reversed", (last_st.onboard_control_sensors_health & enum_value(STS::REVERSE_MOTOR)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::LOGGING)) - stat.add("Logging", (last_st.onboard_control_sensors_health & enum_value(STS::LOGGING)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::BATTERY)) - stat.add("Battery", (last_st.onboard_control_sensors_health & enum_value(STS::BATTERY)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::PROXIMITY)) - stat.add("Proximity", (last_st.onboard_control_sensors_health & enum_value(STS::PROXIMITY)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::SATCOM)) - stat.add("Satellite Communication", (last_st.onboard_control_sensors_health & enum_value(STS::SATCOM)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::PREARM_CHECK)) - stat.add("pre-arm check status. Always healthy when armed", (last_st.onboard_control_sensors_health & enum_value(STS::PREARM_CHECK)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::OBSTACLE_AVOIDANCE)) - stat.add("Avoidance/collision prevention", (last_st.onboard_control_sensors_health & enum_value(STS::OBSTACLE_AVOIDANCE)) ? "Ok" : "Fail"); - if (last_st.onboard_control_sensors_enabled & enum_value(STS::PROPULSION)) - stat.add("propulsion (actuator, esc, motor or propellor)", (last_st.onboard_control_sensors_health & enum_value(STS::PROPULSION)) ? "Ok" : "Fail"); - // [[[end]]] (checksum: 24471e5532db5c99f411475509d41f72) - - stat.addf("CPU Load (%)", "%.1f", last_st.load / 10.0); - stat.addf("Drop rate (%)", "%.1f", last_st.drop_rate_comm / 10.0); - stat.addf("Errors comm", "%d", last_st.errors_comm); - stat.addf("Errors count #1", "%d", last_st.errors_count1); - stat.addf("Errors count #2", "%d", last_st.errors_count2); - stat.addf("Errors count #3", "%d", last_st.errors_count3); - stat.addf("Errors count #4", "%d", last_st.errors_count4); - } + explicit SystemStatusDiag(const std::string & name) + : diagnostic_updater::DiagnosticTask(name), + last_st{} + {} + + void set(mavlink::common::msg::SYS_STATUS & st) + { + std::lock_guard lock(mutex); + last_st = st; + } + + void run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + std::lock_guard lock(mutex); + + if ((last_st.onboard_control_sensors_health & last_st.onboard_control_sensors_enabled) != + last_st.onboard_control_sensors_enabled) + { + stat.summary(DiagnosticStatus::ERROR, "Sensor health"); + } else { + stat.summary(DiagnosticStatus::OK, "Normal"); + } + + stat.addf("Sensor present", "0x%08X", last_st.onboard_control_sensors_present); + stat.addf("Sensor enabled", "0x%08X", last_st.onboard_control_sensors_enabled); + stat.addf("Sensor health", "0x%08X", last_st.onboard_control_sensors_health); + + using STS = mavlink::common::MAV_SYS_STATUS_SENSOR; + + auto check_flag = [&](const std::string & name, STS flag) { + if (last_st.onboard_control_sensors_enabled & enum_value(flag)) { + stat.add( + name, (last_st.onboard_control_sensors_health & enum_value(flag)) ? "Ok" : "Fail"); + } + }; + + // [[[cog: + // import pymavlink.dialects.v20.common as common + // ename = 'MAV_SYS_STATUS_SENSOR' + // ename_pfx2 = 'MAV_SYS_STATUS_' + // + // enum = sorted(common.enums[ename].items()) + // enum.pop() # -> remove ENUM_END + // + // for k, e in enum: + // desc = (e.description.split(' ', 1)[1] + // if e.description.startswith('0x') + // else e.description) + // sts = e.name + // + // if sts.startswith(ename + '_'): + // sts = sts[len(ename) + 1:] + // if sts.startswith(ename_pfx2): + // sts = sts[len(ename_pfx2):] + // if sts[0].isdigit(): + // sts = 'SENSOR_' + sts + // + // cog.outl(f"""check_flag("{desc.strip()}", STS::{sts});""") + // ]]] + check_flag("3D gyro", STS::SENSOR_3D_GYRO); + check_flag("3D accelerometer", STS::SENSOR_3D_ACCEL); + check_flag("3D magnetometer", STS::SENSOR_3D_MAG); + check_flag("absolute pressure", STS::ABSOLUTE_PRESSURE); + check_flag("differential pressure", STS::DIFFERENTIAL_PRESSURE); + check_flag("GPS", STS::GPS); + check_flag("optical flow", STS::OPTICAL_FLOW); + check_flag("computer vision position", STS::VISION_POSITION); + check_flag("laser based position", STS::LASER_POSITION); + check_flag("external ground truth (Vicon or Leica)", STS::EXTERNAL_GROUND_TRUTH); + check_flag("3D angular rate control", STS::ANGULAR_RATE_CONTROL); + check_flag("attitude stabilization", STS::ATTITUDE_STABILIZATION); + check_flag("yaw position", STS::YAW_POSITION); + check_flag("z/altitude control", STS::Z_ALTITUDE_CONTROL); + check_flag("x/y position control", STS::XY_POSITION_CONTROL); + check_flag("motor outputs / control", STS::MOTOR_OUTPUTS); + check_flag("rc receiver", STS::RC_RECEIVER); + check_flag("2nd 3D gyro", STS::SENSOR_3D_GYRO2); + check_flag("2nd 3D accelerometer", STS::SENSOR_3D_ACCEL2); + check_flag("2nd 3D magnetometer", STS::SENSOR_3D_MAG2); + check_flag("geofence", STS::GEOFENCE); + check_flag("AHRS subsystem health", STS::AHRS); + check_flag("Terrain subsystem health", STS::TERRAIN); + check_flag("Motors are reversed", STS::REVERSE_MOTOR); + check_flag("Logging", STS::LOGGING); + check_flag("Battery", STS::BATTERY); + check_flag("Proximity", STS::PROXIMITY); + check_flag("Satellite Communication", STS::SATCOM); + check_flag("pre-arm check status. Always healthy when armed", STS::PREARM_CHECK); + check_flag("Avoidance/collision prevention", STS::OBSTACLE_AVOIDANCE); + check_flag("propulsion (actuator, esc, motor or propellor)", STS::PROPULSION); + // [[[end]]] (checksum: 435a149e38737aac78b4be94b670a6dd) + + stat.addf("CPU Load (%)", "%.1f", last_st.load / 10.0); + stat.addf("Drop rate (%)", "%.1f", last_st.drop_rate_comm / 10.0); + stat.addf("Errors comm", "%d", last_st.errors_comm); + stat.addf("Errors count #1", "%d", last_st.errors_count1); + stat.addf("Errors count #2", "%d", last_st.errors_count2); + stat.addf("Errors count #3", "%d", last_st.errors_count3); + stat.addf("Errors count #4", "%d", last_st.errors_count4); + } private: - std::mutex mutex; - mavlink::common::msg::SYS_STATUS last_st; + std::mutex mutex; + mavlink::common::msg::SYS_STATUS last_st; }; @@ -287,83 +274,86 @@ class SystemStatusDiag : public diagnostic_updater::DiagnosticTask class BatteryStatusDiag : public diagnostic_updater::DiagnosticTask { public: - explicit BatteryStatusDiag(const std::string &name) : - diagnostic_updater::DiagnosticTask(name), - voltage(-1.0f), - current(0.0f), - remaining(0.0f), - min_voltage(6.0f) - { } - - // Move constructor, required to dynamically create an array of instances of this class - // because it contains an unique mutex object - BatteryStatusDiag(BatteryStatusDiag&& other) noexcept : - diagnostic_updater::DiagnosticTask(""), - voltage(-1.0f), - current(0.0f), - remaining(0.0f), - min_voltage(6.0f) - { - *this = std::move(other); - } - - // Move assignment operator, required to dynamically create an array of instances of this class - // because it contains an unique mutex object - BatteryStatusDiag& operator=(BatteryStatusDiag&& other) noexcept { - if (this != &other) - { - *this = std::move(other); - } - return *this; - } - - void set_min_voltage(float volt) { - std::lock_guard lock(mutex); - min_voltage = volt; - } - - void set(float volt, float curr, float rem) { - std::lock_guard lock(mutex); - voltage = volt; - current = curr; - remaining = rem; - } - - void setcell_v(const std::vector voltages) { - std::lock_guard lock(mutex); - cell_voltage = voltages; - } - - void run(diagnostic_updater::DiagnosticStatusWrapper &stat) - { - std::lock_guard lock(mutex); - - if (voltage < 0.0f) - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No data"); - else if (voltage < min_voltage) - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Low voltage"); - else - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Normal"); - - stat.addf("Voltage", "%.2f", voltage); - stat.addf("Current", "%.1f", current); - stat.addf("Remaining", "%.1f", remaining * 100.0f); - const int nr_cells = cell_voltage.size(); - if (nr_cells > 1) - { - for (int i = 1; i <= nr_cells ; ++i) { - stat.addf(utils::format("Cell %u", i), "%.2f", cell_voltage[i-1]); - } - } - } + explicit BatteryStatusDiag(const std::string & name) + : diagnostic_updater::DiagnosticTask(name), + voltage(-1.0f), + current(0.0f), + remaining(0.0f), + min_voltage(6.0f) + {} + + // Move constructor, required to dynamically create an array of instances of this class + // because it contains an unique mutex object + BatteryStatusDiag(BatteryStatusDiag && other) noexcept + : diagnostic_updater::DiagnosticTask(""), + voltage(-1.0f), + current(0.0f), + remaining(0.0f), + min_voltage(6.0f) + { + *this = std::move(other); + } + + // Move assignment operator, required to dynamically create an array of instances of this class + // because it contains an unique mutex object + BatteryStatusDiag & operator=(BatteryStatusDiag && other) noexcept + { + if (this != &other) { + *this = std::move(other); + } + return *this; + } + + void set_min_voltage(float volt) + { + std::lock_guard lock(mutex); + min_voltage = volt; + } + + void set(float volt, float curr, float rem) + { + std::lock_guard lock(mutex); + voltage = volt; + current = curr; + remaining = rem; + } + + void setcell_v(const std::vector voltages) + { + std::lock_guard lock(mutex); + cell_voltage = voltages; + } + + void run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + std::lock_guard lock(mutex); + + if (voltage < 0.0f) { + stat.summary(DiagnosticStatus::ERROR, "No data"); + } else if (voltage < min_voltage) { + stat.summary(DiagnosticStatus::WARN, "Low voltage"); + } else { + stat.summary(DiagnosticStatus::OK, "Normal"); + } + + stat.addf("Voltage", "%.2f", voltage); + stat.addf("Current", "%.1f", current); + stat.addf("Remaining", "%.1f", remaining * 100.0f); + const int nr_cells = cell_voltage.size(); + if (nr_cells > 1) { + for (int i = 1; i <= nr_cells; ++i) { + stat.addf(utils::format("Cell %u", i), "%.2f", cell_voltage[i - 1]); + } + } + } private: - std::mutex mutex; - float voltage; - float current; - float remaining; - float min_voltage; - std::vector cell_voltage; + std::mutex mutex; + float voltage; + float current; + float remaining; + float min_voltage; + std::vector cell_voltage; }; @@ -373,49 +363,53 @@ class BatteryStatusDiag : public diagnostic_updater::DiagnosticTask class MemInfo : public diagnostic_updater::DiagnosticTask { public: - MemInfo(const std::string &name) : - diagnostic_updater::DiagnosticTask(name), - freemem(UINT32_MAX), - brkval(0), - last_rcd(0) - { } - - void set(uint32_t f, uint16_t b) { - freemem = f; - brkval = b; - last_rcd = ros::Time::now().toNSec(); - } - - void run(diagnostic_updater::DiagnosticStatusWrapper &stat) - { - // access atomic variables just once - size_t freemem_ = freemem; - uint16_t brkval_ = brkval; - ros::Time last_rcd_; - last_rcd_.fromNSec(last_rcd.load()); - constexpr int timeout = 10.0; // seconds - - // summarize the results - if (last_rcd_.isZero()) { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Not initialised"); - } else if (ros::Time::now().toSec() - last_rcd_.toSec() > timeout) { - stat.summary(diagnostic_msgs::DiagnosticStatus::STALE, "Not received for more than " + std::to_string(timeout) + "s"); - } else { - if (freemem == UINT32_MAX) - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No data"); - else if (freemem < 200) - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Low mem"); - else - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Normal"); - } - stat.addf("Free memory (B)", "%zd", freemem_); - stat.addf("Heap top", "0x%04X", brkval_); - } + explicit MemInfo(const std::string & name) + : diagnostic_updater::DiagnosticTask(name), + freemem(UINT32_MAX), + brkval(0), + last_rcd(0) + {} + + void set(uint32_t f, uint16_t b) + { + freemem = f; + brkval = b; + last_rcd = clock.now().nanoseconds(); + } + + void run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + // access atomic variables just once + size_t freemem_ = freemem; + uint16_t brkval_ = brkval; + rclcpp::Time last_rcd_(last_rcd.load()); + const rclcpp::Duration timeout(10s); // seconds + + // summarize the results + if (last_rcd_.nanoseconds() == 0) { + stat.summary(DiagnosticStatus::ERROR, "Not initialised"); + } else if (clock.now() - last_rcd_ > timeout) { + stat.summary( + DiagnosticStatus::STALE, + "Not received for more than " + std::to_string(timeout.seconds())); + } else { + if (freemem == UINT32_MAX) { + stat.summary(DiagnosticStatus::ERROR, "No data"); + } else if (freemem < 200) { + stat.summary(DiagnosticStatus::WARN, "Low mem"); + } else { + stat.summary(DiagnosticStatus::OK, "Normal"); + } + } + stat.addf("Free memory (B)", "%zd", freemem_); + stat.addf("Heap top", "0x%04X", brkval_); + } private: - std::atomic freemem; - std::atomic brkval; - std::atomic last_rcd; + rclcpp::Clock clock; + std::atomic freemem; + std::atomic brkval; + std::atomic last_rcd; }; @@ -425,879 +419,1015 @@ class MemInfo : public diagnostic_updater::DiagnosticTask class HwStatus : public diagnostic_updater::DiagnosticTask { public: - HwStatus(const std::string &name) : - diagnostic_updater::DiagnosticTask(name), - vcc(-1.0), - i2cerr(0), - i2cerr_last(0), - last_rcd(0) - { } - - void set(uint16_t v, uint8_t e) { - std::lock_guard lock(mutex); - vcc = v * 0.001f; - i2cerr = e; - last_rcd = ros::Time::now(); - } - - void run(diagnostic_updater::DiagnosticStatusWrapper &stat) - { - std::lock_guard lock(mutex); - constexpr int timeout = 10.0; // seconds - if (last_rcd.isZero()) { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Not initialised"); - } else if (ros::Time::now().toSec() - last_rcd.toSec() > timeout) { - stat.summary(diagnostic_msgs::DiagnosticStatus::STALE, "Not received for more than " + std::to_string(timeout) + "s"); - } else { - if (vcc < 0) - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No data"); - else if (vcc < 4.5) - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Low voltage"); - else if (i2cerr != i2cerr_last) { - i2cerr_last = i2cerr; - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "New I2C error"); - } - else - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Normal"); - } - stat.addf("Core voltage", "%f", vcc); - stat.addf("I2C errors", "%zu", i2cerr); - } + explicit HwStatus(const std::string & name) + : diagnostic_updater::DiagnosticTask(name), + vcc(-1.0), + i2cerr(0), + i2cerr_last(0), + last_rcd(0) + {} + + void set(uint16_t v, uint8_t e) + { + std::lock_guard lock(mutex); + vcc = v * 0.001f; + i2cerr = e; + last_rcd = clock.now(); + } + + void run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + std::lock_guard lock(mutex); + const rclcpp::Duration timeout(10s); // seconds + if (last_rcd.nanoseconds() == 0) { + stat.summary(DiagnosticStatus::ERROR, "Not initialised"); + } else if (clock.now() - last_rcd > timeout) { + stat.summary( + DiagnosticStatus::STALE, "Not received for more than " + std::to_string( + timeout.seconds())); + } else { + if (vcc < 0) { + stat.summary(DiagnosticStatus::ERROR, "No data"); + } else if (vcc < 4.5) { + stat.summary(DiagnosticStatus::WARN, "Low voltage"); + } else if (i2cerr != i2cerr_last) { + i2cerr_last = i2cerr; + stat.summary(DiagnosticStatus::WARN, "New I2C error"); + } else { + stat.summary(DiagnosticStatus::OK, "Normal"); + } + } + stat.addf("Core voltage", "%f", vcc); + stat.addf("I2C errors", "%zu", i2cerr); + } private: - std::mutex mutex; - float vcc; - size_t i2cerr; - size_t i2cerr_last; - ros::Time last_rcd; + rclcpp::Clock clock; + std::mutex mutex; + float vcc; + size_t i2cerr; + size_t i2cerr_last; + rclcpp::Time last_rcd; }; /** * @brief System status plugin. + * @plugin sys_status * * Required by all plugins. */ -class SystemStatusPlugin : public plugin::PluginBase +class SystemStatusPlugin : public plugin::Plugin { public: - SystemStatusPlugin() : PluginBase(), - nh("~"), - hb_diag("Heartbeat", 10), - mem_diag("APM Memory"), - hwst_diag("APM Hardware"), - sys_diag("System"), - conn_heartbeat_mav_type(MAV_TYPE::ONBOARD_CONTROLLER), - version_retries(RETRIES_COUNT), - disable_diag(false), - has_battery_status0(false) - { - batt_diag.reserve(MAX_NR_BATTERY_STATUS); - batt_diag.emplace_back("Battery"); - for (int i = 2; i <= MAX_NR_BATTERY_STATUS ; ++i) { - batt_diag.emplace_back(utils::format("Battery %u", i)); - } - } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - ros::WallDuration conn_heartbeat; - - double conn_timeout_d; - double conn_heartbeat_d; - std::vector min_voltage; - std::string conn_heartbeat_mav_type_str; - - nh.param("conn/timeout", conn_timeout_d, 10.0); - nh.param("sys/min_voltage", min_voltage, {10.0}); - nh.param("sys/disable_diag", disable_diag, false); - - // heartbeat rate parameter - if (nh.getParam("conn/heartbeat_rate", conn_heartbeat_d) && conn_heartbeat_d != 0.0) { - conn_heartbeat = ros::WallDuration(ros::Rate(conn_heartbeat_d)); - } - - // heartbeat mav type parameter - if (nh.getParam("conn/heartbeat_mav_type", conn_heartbeat_mav_type_str)) { - conn_heartbeat_mav_type = utils::mav_type_from_str(conn_heartbeat_mav_type_str); - } - - // heartbeat diag always enabled - UAS_DIAG(m_uas).add(hb_diag); - if (!disable_diag) { - UAS_DIAG(m_uas).add(sys_diag); - for (size_t i = 0; i < MAX_NR_BATTERY_STATUS && i < min_voltage.size(); ++i) { - batt_diag[i].set_min_voltage(min_voltage[i]); - UAS_DIAG(m_uas).add(batt_diag[i]); - } - } - - - // one-shot timeout timer - timeout_timer = nh.createWallTimer(ros::WallDuration(conn_timeout_d), - &SystemStatusPlugin::timeout_cb, this, true); - //timeout_timer.start(); - - if (!conn_heartbeat.isZero()) { - heartbeat_timer = nh.createWallTimer(conn_heartbeat, - &SystemStatusPlugin::heartbeat_cb, this); - //heartbeat_timer.start(); - } - - // version request timer - autopilot_version_timer = nh.createWallTimer(ros::WallDuration(1.0), - &SystemStatusPlugin::autopilot_version_cb, this); - autopilot_version_timer.stop(); - - state_pub = nh.advertise("state", 10, true); - extended_state_pub = nh.advertise("extended_state", 10); - batt_pub = nh.advertise("battery", 10); - estimator_status_pub = nh.advertise("estimator_status", 10); - statustext_pub = nh.advertise("statustext/recv", 10); - statustext_sub = nh.subscribe("statustext/send", 10, &SystemStatusPlugin::statustext_cb, this); - rate_srv = nh.advertiseService("set_stream_rate", &SystemStatusPlugin::set_rate_cb, this); - mode_srv = nh.advertiseService("set_mode", &SystemStatusPlugin::set_mode_cb, this); - vehicle_info_get_srv = nh.advertiseService("vehicle_info_get", &SystemStatusPlugin::vehicle_info_get_cb, this); - message_interval_srv = nh.advertiseService("set_message_interval", &SystemStatusPlugin::set_message_interval_cb, this); - - // init state topic - publish_disconnection(); - enable_connection_cb(); - } - - Subscriptions get_subscriptions() override { - return { - make_handler(&SystemStatusPlugin::handle_heartbeat), - make_handler(&SystemStatusPlugin::handle_sys_status), - make_handler(&SystemStatusPlugin::handle_statustext), - make_handler(&SystemStatusPlugin::handle_meminfo), - make_handler(&SystemStatusPlugin::handle_hwstatus), - make_handler(&SystemStatusPlugin::handle_autopilot_version), - make_handler(&SystemStatusPlugin::handle_extended_sys_state), - make_handler(&SystemStatusPlugin::handle_battery_status), - make_handler(&SystemStatusPlugin::handle_estimator_status), - }; - } + explicit SystemStatusPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "sys"), + hb_diag("Heartbeat", 10), + mem_diag("APM Memory"), + hwst_diag("APM Hardware"), + sys_diag("System"), + conn_heartbeat_mav_type(MAV_TYPE::ONBOARD_CONTROLLER), + version_retries(RETRIES_COUNT), + disable_diag(false), + has_battery_status0(false) + { + batt_diag.reserve(MAX_NR_BATTERY_STATUS); + batt_diag.emplace_back("Battery"); + for (size_t i = 2; i <= MAX_NR_BATTERY_STATUS; ++i) { + batt_diag.emplace_back(utils::format("Battery %u", i)); + } -private: - ros::NodeHandle nh; - - HeartbeatStatus hb_diag; - MemInfo mem_diag; - HwStatus hwst_diag; - SystemStatusDiag sys_diag; - std::vector batt_diag; - ros::WallTimer timeout_timer; - ros::WallTimer heartbeat_timer; - ros::WallTimer autopilot_version_timer; - - ros::Publisher state_pub; - ros::Publisher extended_state_pub; - ros::Publisher batt_pub; - ros::Publisher estimator_status_pub; - ros::Publisher statustext_pub; - ros::Subscriber statustext_sub; - ros::ServiceServer rate_srv; - ros::ServiceServer mode_srv; - ros::ServiceServer vehicle_info_get_srv; - ros::ServiceServer message_interval_srv; - - MAV_TYPE conn_heartbeat_mav_type; - static constexpr int RETRIES_COUNT = 6; - int version_retries; - bool disable_diag; - bool has_battery_status0; - - using M_VehicleInfo = std::unordered_map; - M_VehicleInfo vehicles; - - /* -*- mid-level helpers -*- */ - - // Get vehicle key for the unordered map containing all vehicles - inline uint16_t get_vehicle_key(uint8_t sysid,uint8_t compid) { - return sysid << 8 | compid; - } - - // Find or create vehicle info - inline M_VehicleInfo::iterator find_or_create_vehicle_info(uint8_t sysid, uint8_t compid) { - auto key = get_vehicle_key(sysid, compid); - M_VehicleInfo::iterator ret = vehicles.find(key); - - if (ret == vehicles.end()) { - // Not found - mavros_msgs::VehicleInfo v; - v.sysid = sysid; - v.compid = compid; - v.available_info = 0; - - auto res = vehicles.emplace(key, v); //-> pair - ret = res.first; - } - - ROS_ASSERT(ret != vehicles.end()); - return ret; - } - - /** - * Sent STATUSTEXT message to rosout - * - * @param[in] severity Levels defined in common.xml - */ - void process_statustext_normal(uint8_t severity, std::string &text) - { - using mavlink::common::MAV_SEVERITY; - - switch (severity) { - // [[[cog: - // for l1, l2 in ( - // (('EMERGENCY', 'ALERT', 'CRITICAL', 'ERROR'), 'ERROR'), - // (('WARNING', 'NOTICE'), 'WARN'), - // (('INFO', ), 'INFO'), - // (('DEBUG', ), 'DEBUG') - // ): - // for v in l1: - // cog.outl("case enum_value(MAV_SEVERITY::%s):" % v) - // cog.outl("\tROS_%s_STREAM_NAMED(\"fcu\", \"FCU: \" << text);" % l2) - // cog.outl("\tbreak;") - // ]]] - case enum_value(MAV_SEVERITY::EMERGENCY): - case enum_value(MAV_SEVERITY::ALERT): - case enum_value(MAV_SEVERITY::CRITICAL): - case enum_value(MAV_SEVERITY::ERROR): - ROS_ERROR_STREAM_NAMED("fcu", "FCU: " << text); - break; - case enum_value(MAV_SEVERITY::WARNING): - case enum_value(MAV_SEVERITY::NOTICE): - ROS_WARN_STREAM_NAMED("fcu", "FCU: " << text); - break; - case enum_value(MAV_SEVERITY::INFO): - ROS_INFO_STREAM_NAMED("fcu", "FCU: " << text); - break; - case enum_value(MAV_SEVERITY::DEBUG): - ROS_DEBUG_STREAM_NAMED("fcu", "FCU: " << text); - break; - // [[[end]]] (checksum: 315aa363b5ecb4dda66cc8e1e3d3aa48) - default: - ROS_WARN_STREAM_NAMED("fcu", "FCU: UNK(" << +severity << "): " << text); - break; - }; - } - - static std::string custom_version_to_hex_string(std::array &array) - { - // should be little-endian - uint64_t b; - memcpy(&b, array.data(), sizeof(uint64_t)); - b = le64toh(b); - - return utils::format("%016llx", b); - } - - void process_autopilot_version_normal(mavlink::common::msg::AUTOPILOT_VERSION &apv, uint8_t sysid, uint8_t compid) - { - char prefix[16]; - std::snprintf(prefix, sizeof(prefix), "VER: %d.%d", sysid, compid); - - ROS_INFO_NAMED("sys", "%s: Capabilities 0x%016llx", prefix, (long long int)apv.capabilities); - ROS_INFO_NAMED("sys", "%s: Flight software: %08x (%s)", - prefix, - apv.flight_sw_version, - custom_version_to_hex_string(apv.flight_custom_version).c_str()); - ROS_INFO_NAMED("sys", "%s: Middleware software: %08x (%s)", - prefix, - apv.middleware_sw_version, - custom_version_to_hex_string(apv.middleware_custom_version).c_str()); - ROS_INFO_NAMED("sys", "%s: OS software: %08x (%s)", - prefix, - apv.os_sw_version, - custom_version_to_hex_string(apv.os_custom_version).c_str()); - ROS_INFO_NAMED("sys", "%s: Board hardware: %08x", prefix, apv.board_version); - ROS_INFO_NAMED("sys", "%s: VID/PID: %04x:%04x", prefix, apv.vendor_id, apv.product_id); - ROS_INFO_NAMED("sys", "%s: UID: %016llx", prefix, (long long int)apv.uid); - } - - void process_autopilot_version_apm_quirk(mavlink::common::msg::AUTOPILOT_VERSION &apv, uint8_t sysid, uint8_t compid) - { - char prefix[16]; - std::snprintf(prefix, sizeof(prefix), "VER: %d.%d", sysid, compid); - - // Note based on current APM's impl. - // APM uses custom version array[8] as a string - ROS_INFO_NAMED("sys", "%s: Capabilities 0x%016llx", prefix, (long long int)apv.capabilities); - ROS_INFO_NAMED("sys", "%s: Flight software: %08x (%*s)", - prefix, - apv.flight_sw_version, - 8, apv.flight_custom_version.data()); - ROS_INFO_NAMED("sys", "%s: Middleware software: %08x (%*s)", - prefix, - apv.middleware_sw_version, - 8, apv.middleware_custom_version.data()); - ROS_INFO_NAMED("sys", "%s: OS software: %08x (%*s)", - prefix, - apv.os_sw_version, - 8, apv.os_custom_version.data()); - ROS_INFO_NAMED("sys", "%s: Board hardware: %08x", prefix, apv.board_version); - ROS_INFO_NAMED("sys", "%s: VID/PID: %04x:%04x", prefix, apv.vendor_id, apv.product_id); - ROS_INFO_NAMED("sys", "%s: UID: %016llx", prefix, (long long int)apv.uid); - } - - void publish_disconnection() { - auto state_msg = boost::make_shared(); - state_msg->header.stamp = ros::Time::now(); - state_msg->connected = false; - state_msg->armed = false; - state_msg->guided = false; - state_msg->mode = ""; - state_msg->system_status = enum_value(MAV_STATE::UNINIT); - - state_pub.publish(state_msg); - } - - /* -*- message handlers -*- */ - - void handle_heartbeat(const mavlink::mavlink_message_t *msg, mavlink::minimal::msg::HEARTBEAT &hb) - { - using mavlink::minimal::MAV_MODE_FLAG; - - // Store generic info of all heartbeats seen - auto it = find_or_create_vehicle_info(msg->sysid, msg->compid); - - auto vehicle_mode = m_uas->str_mode_v10(hb.base_mode, hb.custom_mode); - auto stamp = ros::Time::now(); - - // Update vehicle data - it->second.header.stamp = stamp; - it->second.available_info |= mavros_msgs::VehicleInfo::HAVE_INFO_HEARTBEAT; - it->second.autopilot = hb.autopilot; - it->second.type = hb.type; - it->second.system_status = hb.system_status; - it->second.base_mode = hb.base_mode; - it->second.custom_mode = hb.custom_mode; - it->second.mode = vehicle_mode; - - if (!(hb.base_mode & enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED))) { - it->second.mode_id = hb.base_mode; - } else { - it->second.mode_id = hb.custom_mode; - } - - // Continue from here only if vehicle is my target - if (!m_uas->is_my_target(msg->sysid, msg->compid)) { - ROS_DEBUG_NAMED("sys", "HEARTBEAT from [%d, %d] dropped.", msg->sysid, msg->compid); - return; - } - - // update context && setup connection timeout - m_uas->update_heartbeat(hb.type, hb.autopilot, hb.base_mode); - m_uas->update_connection_status(true); - timeout_timer.stop(); - timeout_timer.start(); - - // build state message after updating uas - auto state_msg = boost::make_shared(); - state_msg->header.stamp = stamp; - state_msg->connected = true; - state_msg->armed = !!(hb.base_mode & enum_value(MAV_MODE_FLAG::SAFETY_ARMED)); - state_msg->guided = !!(hb.base_mode & enum_value(MAV_MODE_FLAG::GUIDED_ENABLED)); - state_msg->manual_input = !!(hb.base_mode & enum_value(MAV_MODE_FLAG::MANUAL_INPUT_ENABLED)); - state_msg->mode = vehicle_mode; - state_msg->system_status = hb.system_status; - - state_pub.publish(state_msg); - hb_diag.tick(hb.type, hb.autopilot, state_msg->mode, hb.system_status); - } - - void handle_extended_sys_state(const mavlink::mavlink_message_t *msg, mavlink::common::msg::EXTENDED_SYS_STATE &state) - { - auto state_msg = boost::make_shared(); - state_msg->header.stamp = ros::Time::now(); - state_msg->vtol_state = state.vtol_state; - state_msg->landed_state = state.landed_state; - - extended_state_pub.publish(state_msg); - } - - void handle_sys_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYS_STATUS &stat) - { - using MC = mavlink::minimal::MAV_COMPONENT; - if (static_cast(msg->compid) == MC::COMP_ID_GIMBAL) { - return; - } - - float volt = stat.voltage_battery * 0.001f; // mV - float curr = stat.current_battery * 0.01f; // 10 mA or -1 - float rem = stat.battery_remaining * 0.01f; // or -1 - - sys_diag.set(stat); - - if (has_battery_status0) - return; - - batt_diag[0].set(volt, curr, rem); - auto batt_msg = boost::make_shared(); - batt_msg->header.stamp = ros::Time::now(); - -#ifdef HAVE_SENSOR_MSGS_BATTERYSTATE_MSG - batt_msg->voltage = volt; - batt_msg->current = -curr; - batt_msg->charge = NAN; - batt_msg->capacity = NAN; - batt_msg->design_capacity = NAN; - batt_msg->percentage = rem; - batt_msg->power_supply_status = BatteryMsg::POWER_SUPPLY_STATUS_DISCHARGING; - batt_msg->power_supply_health = BatteryMsg::POWER_SUPPLY_HEALTH_UNKNOWN; - batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_UNKNOWN; - batt_msg->present = true; - batt_msg->cell_voltage.clear(); // not necessary. Cell count and Voltage unknown. - batt_msg->location = ""; - batt_msg->serial_number = ""; -#else // mavros_msgs::BatteryStatus - batt_msg->voltage = volt; - batt_msg->current = curr; - batt_msg->remaining = rem; -#endif - - batt_pub.publish(batt_msg); - } - - void handle_statustext(const mavlink::mavlink_message_t *msg, mavlink::common::msg::STATUSTEXT &textm) - { - auto text = mavlink::to_string(textm.text); - process_statustext_normal(textm.severity, text); - - auto st_msg = boost::make_shared(); - st_msg->header.stamp = ros::Time::now(); - st_msg->severity = textm.severity; - st_msg->text = text; - statustext_pub.publish(st_msg); - } - - void handle_meminfo(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::MEMINFO &mem) - { - mem_diag.set(std::max(static_cast(mem.freemem), mem.freemem32), mem.brkval); - } - - void handle_hwstatus(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::HWSTATUS &hwst) - { - hwst_diag.set(hwst.Vcc, hwst.I2Cerr); - } - - void handle_autopilot_version(const mavlink::mavlink_message_t *msg, mavlink::common::msg::AUTOPILOT_VERSION &apv) - { - // we want to store only FCU caps - if (m_uas->is_my_target(msg->sysid, msg->compid)) { - autopilot_version_timer.stop(); - m_uas->update_capabilities(true, apv.capabilities); - } - - // but print all version responses - if (m_uas->is_ardupilotmega()) - process_autopilot_version_apm_quirk(apv, msg->sysid, msg->compid); - else - process_autopilot_version_normal(apv, msg->sysid, msg->compid); - - // Store generic info of all autopilot seen - auto it = find_or_create_vehicle_info(msg->sysid, msg->compid); - - // Update vehicle data - it->second.header.stamp = ros::Time::now(); - it->second.available_info |= mavros_msgs::VehicleInfo::HAVE_INFO_AUTOPILOT_VERSION; - it->second.capabilities = apv.capabilities; - it->second.flight_sw_version = apv.flight_sw_version; - it->second.middleware_sw_version = apv.middleware_sw_version; - it->second.os_sw_version = apv.os_sw_version; - it->second.board_version = apv.board_version; - it->second.flight_custom_version = custom_version_to_hex_string(apv.flight_custom_version); - it->second.vendor_id = apv.vendor_id; - it->second.product_id = apv.product_id; - it->second.uid = apv.uid; - } - - void handle_battery_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::BATTERY_STATUS &bs) - { -#ifdef HAVE_SENSOR_MSGS_BATTERYSTATE_MSG - using BT = mavlink::common::MAV_BATTERY_TYPE; - auto batt_msg = boost::make_shared(); - batt_msg->header.stamp = ros::Time::now(); - - batt_msg->current = bs.current_battery==-1?NAN:-(bs.current_battery * 0.01f); // 10 mA - batt_msg->charge = NAN; - batt_msg->capacity = NAN; - batt_msg->design_capacity = NAN; - batt_msg->percentage = bs.battery_remaining * 0.01f; - batt_msg->power_supply_status = BatteryMsg::POWER_SUPPLY_STATUS_DISCHARGING; - batt_msg->power_supply_health = BatteryMsg::POWER_SUPPLY_HEALTH_UNKNOWN; - - switch (bs.type) { - // [[[cog: - // for f in ( - // 'LIPO', - // 'LIFE', - // 'LION', - // 'NIMH', - // 'UNKNOWN'): - // cog.outl("case enum_value(BT::%s):" % f) - // if f == 'UNKNOWN': - // cog.outl("default:") - // cog.outl("\tbatt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_%s;" % f) - // cog.outl("\tbreak;") - // ]]] - case enum_value(BT::LIPO): - batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_LIPO; - break; - case enum_value(BT::LIFE): - batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_LIFE; - break; - case enum_value(BT::LION): - batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_LION; - break; - case enum_value(BT::NIMH): - batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_NIMH; - break; - case enum_value(BT::UNKNOWN): - default: - batt_msg->power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_UNKNOWN; - break; - // [[[end]]] (checksum: 2bf14a81b3027f14ba1dd9b4c086a41d) - } - - batt_msg->present = true; - - batt_msg->cell_voltage.clear(); - batt_msg->cell_voltage.reserve(bs.voltages.size() + bs.voltages_ext.size()); - float cell_voltage; - float voltage_acc = 0.0f; - float total_voltage = 0.0f; - constexpr float coalesce_voltage = (UINT16_MAX-1) * 0.001f; // 65,534V cell voltage means that the next element in the array must be added to this one - for (auto v : bs.voltages) { - if (v == UINT16_MAX) - break; - - if (v == UINT16_MAX-1) // cell voltage is above 65,534V - { - voltage_acc += coalesce_voltage; - continue; // add to the next array element to get the correct voltage - } - cell_voltage = voltage_acc + (v * 0.001f); // 1 mV - voltage_acc = 0.0f; - batt_msg->cell_voltage.push_back(cell_voltage); - total_voltage += cell_voltage; - } - for (auto v : bs.voltages_ext) { - if (v == UINT16_MAX || v == 0) // this one is different from the for loop above to support mavlink2 message truncation - break; - - if (v == UINT16_MAX-1) // cell voltage is above 65,534V - { - voltage_acc += coalesce_voltage; - continue; // add to the next array element to get the correct voltage - } - cell_voltage = voltage_acc + (v * 0.001f); // 1 mV - voltage_acc = 0.0f; - batt_msg->cell_voltage.push_back(cell_voltage); - total_voltage += cell_voltage; - } - batt_msg->voltage = total_voltage; - - batt_msg->location = utils::format("id%u", bs.id); - batt_msg->serial_number = ""; - batt_pub.publish(batt_msg); - - if (bs.id == 0) { - has_battery_status0 = true; - } - - if (!disable_diag && bs.id >= 0 && bs.id < MAX_NR_BATTERY_STATUS) { - batt_diag[bs.id].set(total_voltage, batt_msg->current, batt_msg->percentage); - batt_diag[bs.id].setcell_v(batt_msg->cell_voltage); - } -#endif - } - - void handle_estimator_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ESTIMATOR_STATUS &status) - { - using ESF = mavlink::common::ESTIMATOR_STATUS_FLAGS; - - auto est_status_msg = boost::make_shared(); - est_status_msg->header.stamp = ros::Time::now(); - - // [[[cog: - // import pymavlink.dialects.v20.common as common - // ename = 'ESTIMATOR_STATUS_FLAGS' - // ename_pfx2 = 'ESTIMATOR_' - // - // enum = sorted(common.enums[ename].items()) - // enum.pop() # -> remove ENUM_END - // - // for k, e in enum: - // desc = e.description.split(' ', 1)[1] if e.description.startswith('0x') else e.description - // esf = e.name - // - // if esf.startswith(ename + '_'): - // esf = esf[len(ename) + 1:] - // if esf.startswith(ename_pfx2): - // esf = esf[len(ename_pfx2):] - // if esf[0].isdigit(): - // esf = 'SENSOR_' + esf - // cog.outl("est_status_msg->%s_status_flag = !!(status.flags & enum_value(ESF::%s));" % (esf.lower(), esf)) - // ]]] - est_status_msg->attitude_status_flag = !!(status.flags & enum_value(ESF::ATTITUDE)); - est_status_msg->velocity_horiz_status_flag = !!(status.flags & enum_value(ESF::VELOCITY_HORIZ)); - est_status_msg->velocity_vert_status_flag = !!(status.flags & enum_value(ESF::VELOCITY_VERT)); - est_status_msg->pos_horiz_rel_status_flag = !!(status.flags & enum_value(ESF::POS_HORIZ_REL)); - est_status_msg->pos_horiz_abs_status_flag = !!(status.flags & enum_value(ESF::POS_HORIZ_ABS)); - est_status_msg->pos_vert_abs_status_flag = !!(status.flags & enum_value(ESF::POS_VERT_ABS)); - est_status_msg->pos_vert_agl_status_flag = !!(status.flags & enum_value(ESF::POS_VERT_AGL)); - est_status_msg->const_pos_mode_status_flag = !!(status.flags & enum_value(ESF::CONST_POS_MODE)); - est_status_msg->pred_pos_horiz_rel_status_flag = !!(status.flags & enum_value(ESF::PRED_POS_HORIZ_REL)); - est_status_msg->pred_pos_horiz_abs_status_flag = !!(status.flags & enum_value(ESF::PRED_POS_HORIZ_ABS)); - est_status_msg->gps_glitch_status_flag = !!(status.flags & enum_value(ESF::GPS_GLITCH)); - est_status_msg->accel_error_status_flag = !!(status.flags & enum_value(ESF::ACCEL_ERROR)); - // [[[end]]] (checksum: da59238f4d4337aeb395f7205db08237) - - estimator_status_pub.publish(est_status_msg); - } - - /* -*- timer callbacks -*- */ - - void timeout_cb(const ros::WallTimerEvent &event) - { - m_uas->update_connection_status(false); - } - - void heartbeat_cb(const ros::WallTimerEvent &event) - { - using mavlink::common::MAV_MODE; - - mavlink::minimal::msg::HEARTBEAT hb {}; - - hb.type = enum_value(conn_heartbeat_mav_type); //! @todo patch PX4 so it can also handle this type as datalink - hb.autopilot = enum_value(MAV_AUTOPILOT::INVALID); - hb.base_mode = enum_value(MAV_MODE::MANUAL_ARMED); - hb.custom_mode = 0; - hb.system_status = enum_value(MAV_STATE::ACTIVE); - - UAS_FCU(m_uas)->send_message_ignore_drop(hb); - } - - void autopilot_version_cb(const ros::WallTimerEvent &event) - { - using mavlink::common::MAV_CMD; - - bool ret = false; - - // Request from all first 3 times, then fallback to unicast - bool do_broadcast = version_retries > RETRIES_COUNT / 2; - - try { - auto client = nh.serviceClient("cmd/command"); - - mavros_msgs::CommandLong cmd{}; - - cmd.request.broadcast = do_broadcast; - cmd.request.command = enum_value(MAV_CMD::REQUEST_AUTOPILOT_CAPABILITIES); - cmd.request.confirmation = false; - cmd.request.param1 = 1.0; - - ROS_DEBUG_NAMED("sys", "VER: Sending %s request.", - (do_broadcast) ? "broadcast" : "unicast"); - ret = client.call(cmd); - } - catch (ros::InvalidNameException &ex) { - ROS_ERROR_NAMED("sys", "VER: %s", ex.what()); - } - - ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!"); - - if (version_retries > 0) { - version_retries--; - ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys", - "VER: %s request timeout, retries left %d", - (do_broadcast) ? "broadcast" : "unicast", - version_retries); - } - else { - m_uas->update_capabilities(false); - autopilot_version_timer.stop(); - ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, " - "switched to default capabilities"); - } - } - - void connection_cb(bool connected) override - { - has_battery_status0 = false; - - // if connection changes, start delayed version request - version_retries = RETRIES_COUNT; - if (connected) - autopilot_version_timer.start(); - else - autopilot_version_timer.stop(); - - // add/remove APM diag tasks - if (connected && !disable_diag && m_uas->is_ardupilotmega()) { - UAS_DIAG(m_uas).add(mem_diag); - UAS_DIAG(m_uas).add(hwst_diag); - } - else { - UAS_DIAG(m_uas).removeByName(mem_diag.getName()); - UAS_DIAG(m_uas).removeByName(hwst_diag.getName()); - } - - if (!connected) { - // publish connection change - publish_disconnection(); - - // Clear known vehicles - vehicles.clear(); - } - } - - /* -*- subscription callbacks -*- */ - - void statustext_cb(const mavros_msgs::StatusText::ConstPtr &req) { - mavlink::common::msg::STATUSTEXT statustext {}; - statustext.severity = req->severity; - - // Limit the length of the string by null-terminating at the 50-th character - ROS_WARN_COND_NAMED(req->text.length() >= statustext.text.size(), "sys", - "Status text too long: truncating..."); - mavlink::set_string_z(statustext.text, req->text); - - UAS_FCU(m_uas)->send_message_ignore_drop(statustext); - } - - /* -*- ros callbacks -*- */ - - bool set_rate_cb(mavros_msgs::StreamRate::Request &req, - mavros_msgs::StreamRate::Response &res) - { - mavlink::common::msg::REQUEST_DATA_STREAM rq = {}; - - rq.target_system = m_uas->get_tgt_system(); - rq.target_component = m_uas->get_tgt_component(); - rq.req_stream_id = req.stream_id; - rq.req_message_rate = req.message_rate; - rq.start_stop = (req.on_off) ? 1 : 0; - - UAS_FCU(m_uas)->send_message_ignore_drop(rq); - return true; - } - - bool set_mode_cb(mavros_msgs::SetMode::Request &req, - mavros_msgs::SetMode::Response &res) - { - using mavlink::minimal::MAV_MODE_FLAG; - - uint8_t base_mode = req.base_mode; - uint32_t custom_mode = 0; - - if (req.custom_mode != "") { - if (!m_uas->cmode_from_str(req.custom_mode, custom_mode)) { - res.mode_sent = false; - return true; - } - - /** - * @note That call may trigger unexpected arming change because - * base_mode arming flag state based on previous HEARTBEAT - * message value. - */ - base_mode |= (m_uas->get_armed()) ? enum_value(MAV_MODE_FLAG::SAFETY_ARMED) : 0; - base_mode |= (m_uas->get_hil_state()) ? enum_value(MAV_MODE_FLAG::HIL_ENABLED) : 0; - base_mode |= enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED); - } - - mavlink::common::msg::SET_MODE sm = {}; - sm.target_system = m_uas->get_tgt_system(); - sm.base_mode = base_mode; - sm.custom_mode = custom_mode; - - UAS_FCU(m_uas)->send_message_ignore_drop(sm); - res.mode_sent = true; - return true; - } - - bool vehicle_info_get_cb(mavros_msgs::VehicleInfoGet::Request &req, - mavros_msgs::VehicleInfoGet::Response &res) - { - if (req.get_all) { - // Send all vehicles - for (const auto &got : vehicles) { - res.vehicles.emplace_back(got.second); - } - - res.success = true; - return res.success; - } - - uint8_t req_sysid = req.sysid; - uint8_t req_compid = req.compid; - - if (req.sysid == 0 && req.compid == 0) { - // use target - req_sysid = m_uas->get_tgt_system(); - req_compid = m_uas->get_tgt_component(); - } - - uint16_t key = get_vehicle_key(req_sysid, req_compid); - auto it = vehicles.find(key); - - if (it == vehicles.end()) { - // Vehicle not found - res.success = false; - return res.success; - } - - res.vehicles.emplace_back(it->second); - res.success = true; - return res.success; - } - - bool set_message_interval_cb(mavros_msgs::MessageInterval::Request &req, - mavros_msgs::MessageInterval::Response &res) - { - using mavlink::common::MAV_CMD; - - try { - auto client = nh.serviceClient("cmd/command"); - - // calculate interval - float interval_us; - if (req.message_rate < 0) { - interval_us = -1.0f; - } else if (req.message_rate == 0) { - interval_us = 0.0f; - } else { - interval_us = 1000000.0f / req.message_rate; - } - - mavros_msgs::CommandLong cmd{}; - - cmd.request.broadcast = false; - cmd.request.command = enum_value(MAV_CMD::SET_MESSAGE_INTERVAL); - cmd.request.confirmation = false; - cmd.request.param1 = req.message_id; - cmd.request.param2 = interval_us; - - ROS_DEBUG_NAMED("sys", "SetMessageInterval: Request msgid %u at %f hz", - req.message_id, req.message_rate); - res.success = client.call(cmd); + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "conn_timeout", 10.0, [&](const rclcpp::Parameter & p) { + auto conn_timeout = rclcpp::Duration::from_seconds(p.as_double()); + + timeout_timer = + node->create_wall_timer( + conn_timeout.to_chrono(), + std::bind(&SystemStatusPlugin::timeout_cb, this)); + }); + + node_declare_and_watch_parameter( + "min_voltage", std::vector({10.0}), [&](const rclcpp::Parameter & p) { + min_voltage = p.as_double_array(); + for (size_t i = 0; i < batt_diag.size() && i < min_voltage.size(); ++i) { + batt_diag[i].set_min_voltage(min_voltage[i]); } - catch (ros::InvalidNameException &ex) { - ROS_ERROR_NAMED("sys", "SetMessageInterval: %s", ex.what()); + }); + + node_declare_and_watch_parameter( + "disable_diag", false, [&](const rclcpp::Parameter & p) { + disable_diag = p.as_bool(); + + if (!disable_diag) { + uas->diagnostic_updater.add(sys_diag); + for (size_t i = 0; i < batt_diag.size() && i < min_voltage.size(); ++i) { + uas->diagnostic_updater.add(batt_diag[i]); + } + + } else { + uas->diagnostic_updater.removeByName(sys_diag.getName()); + uas->diagnostic_updater.removeByName(mem_diag.getName()); + uas->diagnostic_updater.removeByName(hwst_diag.getName()); + for (auto & d : batt_diag) { + uas->diagnostic_updater.removeByName(d.getName()); + } } + }); + + node_declare_and_watch_parameter( + "heartbeat_mav_type", utils::enum_to_name( + conn_heartbeat_mav_type), [&](const rclcpp::Parameter & p) { + conn_heartbeat_mav_type = utils::mav_type_from_str(p.as_string()); + }); + + node_declare_and_watch_parameter( + "heartbeat_rate", 1.0, [&](const rclcpp::Parameter & p) { + auto rate_d = p.as_double(); + + if (rate_d == 0) { + if (heartbeat_timer) { + heartbeat_timer->cancel(); + heartbeat_timer.reset(); + } + } else { + rclcpp::WallRate rate(rate_d); + + heartbeat_timer = + node->create_wall_timer( + rate.period(), std::bind( + &SystemStatusPlugin::heartbeat_cb, + this)); + } + }); + + auto state_qos = rclcpp::QoS(10).transient_local(); + auto sensor_qos = rclcpp::SensorDataQoS(); + + state_pub = node->create_publisher( + "state", state_qos); + extended_state_pub = node->create_publisher( + "extended_state", state_qos); + estimator_status_pub = node->create_publisher( + "estimator_status", state_qos); + batt_pub = node->create_publisher("battery", sensor_qos); + + statustext_pub = node->create_publisher( + "statustext/recv", sensor_qos); + statustext_sub = node->create_subscription( + "statustext/send", sensor_qos, + std::bind(&SystemStatusPlugin::statustext_cb, this, _1)); + + srv_cg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + mode_srv = node->create_service( + "set_mode", + std::bind( + &SystemStatusPlugin::set_mode_cb, this, _1, + _2), rmw_qos_profile_services_default, srv_cg); + stream_rate_srv = node->create_service( + "set_stream_rate", + std::bind( + &SystemStatusPlugin::set_rate_cb, this, _1, + _2), rmw_qos_profile_services_default, srv_cg); + message_interval_srv = node->create_service( + "set_message_interval", + std::bind( + &SystemStatusPlugin::set_message_interval_cb, this, _1, + _2), rmw_qos_profile_services_default, srv_cg); + vehicle_info_get_srv = node->create_service( + "vehicle_info_get", std::bind( + &SystemStatusPlugin::vehicle_info_get_cb, this, _1, + _2), rmw_qos_profile_services_default, srv_cg); + + uas->diagnostic_updater.add(hb_diag); + + autopilot_version_timer = node->create_wall_timer( + 1s, std::bind(&SystemStatusPlugin::autopilot_version_cb, this), + srv_cg); + + // init state topic + publish_disconnection(); + enable_connection_cb(); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&SystemStatusPlugin::handle_heartbeat), + make_handler(&SystemStatusPlugin::handle_sys_status), + make_handler(&SystemStatusPlugin::handle_statustext), + make_handler(&SystemStatusPlugin::handle_meminfo), + make_handler(&SystemStatusPlugin::handle_hwstatus), + make_handler(&SystemStatusPlugin::handle_autopilot_version), + make_handler(&SystemStatusPlugin::handle_extended_sys_state), + make_handler(&SystemStatusPlugin::handle_battery_status), + make_handler(&SystemStatusPlugin::handle_estimator_status), + }; + } + +private: + HeartbeatStatus hb_diag; + MemInfo mem_diag; + HwStatus hwst_diag; + SystemStatusDiag sys_diag; + std::vector batt_diag; + + rclcpp::TimerBase::SharedPtr timeout_timer; + rclcpp::TimerBase::SharedPtr heartbeat_timer; + rclcpp::TimerBase::SharedPtr autopilot_version_timer; + + rclcpp::Publisher::SharedPtr state_pub; + rclcpp::Publisher::SharedPtr extended_state_pub; + rclcpp::Publisher::SharedPtr estimator_status_pub; + rclcpp::Publisher::SharedPtr batt_pub; + + rclcpp::Publisher::SharedPtr statustext_pub; + rclcpp::Subscription::SharedPtr statustext_sub; + + rclcpp::CallbackGroup::SharedPtr srv_cg; + rclcpp::Service::SharedPtr stream_rate_srv; + rclcpp::Service::SharedPtr message_interval_srv; + rclcpp::Service::SharedPtr mode_srv; + rclcpp::Service::SharedPtr vehicle_info_get_srv; + + MAV_TYPE conn_heartbeat_mav_type; + static constexpr int RETRIES_COUNT = 6; + int version_retries; + bool disable_diag; + bool has_battery_status0; + float battery_voltage; + std::vector min_voltage; + + using M_VehicleInfo = std::unordered_map; + M_VehicleInfo vehicles; + + /* -*- mid-level helpers -*- */ + + // Get vehicle key for the unordered map containing all vehicles + inline uint16_t get_vehicle_key(uint8_t sysid, uint8_t compid) + { + return sysid << 8 | compid; + } + + // Find or create vehicle info + inline M_VehicleInfo::iterator find_or_create_vehicle_info(uint8_t sysid, uint8_t compid) + { + auto key = get_vehicle_key(sysid, compid); + M_VehicleInfo::iterator ret = vehicles.find(key); + + if (ret == vehicles.end()) { + // Not found + mavros_msgs::msg::VehicleInfo v; + v.sysid = sysid; + v.compid = compid; + v.available_info = 0; + + auto res = vehicles.emplace(key, v); //-> pair + ret = res.first; + } + + rcpputils::assert_true(ret != vehicles.end()); + return ret; + } + + /** + * Sent STATUSTEXT message to rosout + * + * @param[in] severity Levels defined in common.xml + */ + void process_statustext_normal(uint8_t severity, const std::string & text) + { + using mavlink::common::MAV_SEVERITY; + + switch (severity) { + // [[[cog: + // for l1, l2 in ( + // (('EMERGENCY', 'ALERT', 'CRITICAL', 'ERROR'), 'ERROR'), + // (('WARNING', 'NOTICE'), 'WARN'), + // (('INFO', ), 'INFO'), + // (('DEBUG', ), 'DEBUG') + // ): + // for v in l1: + // cog.outl(f"case enum_value(MAV_SEVERITY::{v}):") + // cog.outl(f" RCLCPP_{l2}_STREAM(node->get_logger(), \"FCU: \" << text);") + // cog.outl(f" break;") + // ]]] + case enum_value(MAV_SEVERITY::EMERGENCY): + case enum_value(MAV_SEVERITY::ALERT): + case enum_value(MAV_SEVERITY::CRITICAL): + case enum_value(MAV_SEVERITY::ERROR): + RCLCPP_ERROR_STREAM(node->get_logger(), "FCU: " << text); + break; + case enum_value(MAV_SEVERITY::WARNING): + case enum_value(MAV_SEVERITY::NOTICE): + RCLCPP_WARN_STREAM(node->get_logger(), "FCU: " << text); + break; + case enum_value(MAV_SEVERITY::INFO): + RCLCPP_INFO_STREAM(node->get_logger(), "FCU: " << text); + break; + case enum_value(MAV_SEVERITY::DEBUG): + RCLCPP_DEBUG_STREAM(node->get_logger(), "FCU: " << text); + break; + // [[[end]]] (checksum: d05760afbeece46673c8f73f89b63f3d) + default: + RCLCPP_WARN_STREAM(node->get_logger(), "FCU: UNK(" << +severity << "): " << text); + break; + } + } + + static std::string custom_version_to_hex_string(const std::array & array) + { + // should be little-endian + uint64_t b; + memcpy(&b, array.data(), sizeof(uint64_t)); + b = le64toh(b); + + return utils::format("%016llx", b); + } + + void process_autopilot_version_normal( + mavlink::common::msg::AUTOPILOT_VERSION & apv, + uint8_t sysid, uint8_t compid) + { + char prefix[16]; + std::snprintf(prefix, sizeof(prefix), "VER: %d.%d", sysid, compid); + + auto lg = node->get_logger(); + auto log_info = [&lg, &prefix] < typename ... Args > (const std::string & fmt, Args ... args) { + RCLCPP_INFO(lg, fmt.c_str(), prefix, args ...); + }; // NOLINT + + log_info("%s: Capabilities 0x%016llx", apv.capabilities); + log_info( + "%s: Flight software: %08x (%s)", + apv.flight_sw_version, custom_version_to_hex_string(apv.flight_custom_version).c_str()); + log_info( + "%s: Middleware software: %08x (%s)", + apv.middleware_sw_version, + custom_version_to_hex_string(apv.middleware_custom_version).c_str()); + log_info( + "%s: OS software: %08x (%s)", + apv.os_sw_version, custom_version_to_hex_string(apv.os_custom_version).c_str()); + log_info("%s: Board hardware: %08x", apv.board_version); + log_info("%s: VID/PID: %04x:%04x", apv.vendor_id, apv.product_id); + log_info("%s: UID: %016llx", apv.uid); + } + + void process_autopilot_version_apm_quirk( + mavlink::common::msg::AUTOPILOT_VERSION & apv, + uint8_t sysid, uint8_t compid) + { + char prefix[16]; + std::snprintf(prefix, sizeof(prefix), "VER: %d.%d", sysid, compid); + + auto lg = node->get_logger(); + auto log_info = [&lg, &prefix] < typename ... Args > (const std::string & fmt, Args ... args) { + RCLCPP_INFO(lg, fmt.c_str(), prefix, args ...); + }; // NOLINT + + // Note based on current APM's impl. + // APM uses custom version array[8] as a string + log_info("%s: Capabilities 0x%016llx", apv.capabilities); + log_info( + "%s: Flight software: %08x (%*s)", + apv.flight_sw_version, 8, apv.flight_custom_version.data()); + log_info( + "%s: Middleware software: %08x (%*s)", + apv.middleware_sw_version, 8, apv.middleware_custom_version.data()); + log_info( + "%s: OS software: %08x (%*s)", + apv.os_sw_version, 8, apv.os_custom_version.data()); + log_info("%s: Board hardware: %08x", apv.board_version); + log_info("%s: VID/PID: %04x:%04x", apv.vendor_id, apv.product_id); + log_info("%s: UID: %016llx", apv.uid); + } + + void publish_disconnection() + { + auto state_msg = mavros_msgs::msg::State(); + state_msg.header.stamp = node->now(); + state_msg.connected = false; + state_msg.armed = false; + state_msg.guided = false; + state_msg.mode = ""; + state_msg.system_status = enum_value(MAV_STATE::UNINIT); + + state_pub->publish(state_msg); + } + + /* -*- message handlers -*- */ + + void handle_heartbeat( + const mavlink::mavlink_message_t * msg, + mavlink::minimal::msg::HEARTBEAT & hb, plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + using mavlink::minimal::MAV_MODE_FLAG; + + // XXX(vooon): i assume that UAS not interested in HBs from non-target system. + + // Store generic info of all heartbeats seen + auto it = find_or_create_vehicle_info(msg->sysid, msg->compid); + + auto vehicle_mode = uas->str_mode_v10(hb.base_mode, hb.custom_mode); + auto stamp = node->now(); + + // Update vehicle data + it->second.header.stamp = stamp; + it->second.available_info |= mavros_msgs::msg::VehicleInfo::HAVE_INFO_HEARTBEAT; + it->second.autopilot = hb.autopilot; + it->second.type = hb.type; + it->second.system_status = hb.system_status; + it->second.base_mode = hb.base_mode; + it->second.custom_mode = hb.custom_mode; + it->second.mode = vehicle_mode; + + if (!(hb.base_mode & enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED))) { + it->second.mode_id = hb.base_mode; + } else { + it->second.mode_id = hb.custom_mode; + } + + // Continue from here only if vehicle is my target + if (!uas->is_my_target(msg->sysid, msg->compid)) { + RCLCPP_DEBUG(node->get_logger(), "HEARTBEAT from %d.%d dropped.", msg->sysid, msg->compid); + return; + } + + // update context && setup connection timeout + uas->update_heartbeat(hb.type, hb.autopilot, hb.base_mode); + uas->update_connection_status(true); + timeout_timer->reset(); + + // build state message after updating uas + auto state_msg = mavros_msgs::msg::State(); + state_msg.header.stamp = stamp; + state_msg.connected = true; + state_msg.armed = !!(hb.base_mode & enum_value(MAV_MODE_FLAG::SAFETY_ARMED)); + state_msg.guided = !!(hb.base_mode & enum_value(MAV_MODE_FLAG::GUIDED_ENABLED)); + state_msg.manual_input = !!(hb.base_mode & enum_value(MAV_MODE_FLAG::MANUAL_INPUT_ENABLED)); + state_msg.mode = vehicle_mode; + state_msg.system_status = hb.system_status; + + state_pub->publish(state_msg); + hb_diag.tick(hb.type, hb.autopilot, state_msg.mode, hb.system_status); + } + + void handle_extended_sys_state( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::EXTENDED_SYS_STATE & state, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto state_msg = mavros_msgs::msg::ExtendedState(); + state_msg.header.stamp = node->now(); + state_msg.vtol_state = state.vtol_state; + state_msg.landed_state = state.landed_state; + + extended_state_pub->publish(state_msg); + } + + void handle_sys_status( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::SYS_STATUS & stat, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + using MC = mavlink::minimal::MAV_COMPONENT; + if (static_cast(msg->compid) == MC::COMP_ID_GIMBAL) { + return; + } + + float volt = stat.voltage_battery / 1000.0f; // mV + float curr = stat.current_battery / 100.0f; // 10 mA or -1 + float rem = stat.battery_remaining / 100.0f; // or -1 + + battery_voltage = volt; + sys_diag.set(stat); + + if (has_battery_status0) { + return; + } + + batt_diag[0].set(volt, curr, rem); + + auto batt_msg = BatteryMsg(); + batt_msg.header.stamp = node->now(); + batt_msg.voltage = volt; + batt_msg.current = -curr; + batt_msg.charge = NAN; + batt_msg.capacity = NAN; + batt_msg.design_capacity = NAN; + batt_msg.percentage = rem; + batt_msg.power_supply_status = BatteryMsg::POWER_SUPPLY_STATUS_DISCHARGING; + batt_msg.power_supply_health = BatteryMsg::POWER_SUPPLY_HEALTH_UNKNOWN; + batt_msg.power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_UNKNOWN; + batt_msg.present = true; + batt_msg.cell_voltage.clear(); // not necessary. Cell count and Voltage unknown. + batt_msg.location = ""; + batt_msg.serial_number = ""; + + batt_pub->publish(batt_msg); + } + + void handle_statustext( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::STATUSTEXT & textm, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto text = mavlink::to_string(textm.text); + process_statustext_normal(textm.severity, text); + + auto st_msg = mavros_msgs::msg::StatusText(); + st_msg.header.stamp = node->now(); + st_msg.severity = textm.severity; + st_msg.text = text; + + statustext_pub->publish(st_msg); + } + + void handle_meminfo( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::ardupilotmega::msg::MEMINFO & mem, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + mem_diag.set(std::max(static_cast(mem.freemem), mem.freemem32), mem.brkval); + } + + void handle_hwstatus( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::ardupilotmega::msg::HWSTATUS & hwst, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + hwst_diag.set(hwst.Vcc, hwst.I2Cerr); + } + + void handle_autopilot_version( + const mavlink::mavlink_message_t * msg, + mavlink::common::msg::AUTOPILOT_VERSION & apv, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // XXX(vooon): i assume that UAS no longer interested in other systems + // so will recv versions only from target system's components + + // we want to store only FCU caps + if (uas->is_my_target(msg->sysid, msg->compid)) { + autopilot_version_timer->cancel(); + uas->update_capabilities(true, apv.capabilities); + } + + // but print all version responses + if (uas->is_ardupilotmega()) { + process_autopilot_version_apm_quirk(apv, msg->sysid, msg->compid); + } else { + process_autopilot_version_normal(apv, msg->sysid, msg->compid); + } + + // Store generic info of all autopilot seen + auto it = find_or_create_vehicle_info(msg->sysid, msg->compid); + + // Update vehicle data + it->second.header.stamp = node->now(); + it->second.available_info |= mavros_msgs::msg::VehicleInfo::HAVE_INFO_AUTOPILOT_VERSION; + it->second.capabilities = apv.capabilities; + it->second.flight_sw_version = apv.flight_sw_version; + it->second.middleware_sw_version = apv.middleware_sw_version; + it->second.os_sw_version = apv.os_sw_version; + it->second.board_version = apv.board_version; + it->second.flight_custom_version = custom_version_to_hex_string(apv.flight_custom_version); + it->second.vendor_id = apv.vendor_id; + it->second.product_id = apv.product_id; + it->second.uid = apv.uid; + } + + void handle_battery_status( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::BATTERY_STATUS & bs, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // PX4. + using BT = mavlink::common::MAV_BATTERY_TYPE; + + auto batt_msg = BatteryMsg(); + batt_msg.header.stamp = node->now(); + batt_msg.voltage = battery_voltage; + batt_msg.current = -(bs.current_battery / 100.0f); // 10 mA + batt_msg.charge = NAN; + batt_msg.capacity = NAN; + batt_msg.design_capacity = NAN; + batt_msg.percentage = bs.battery_remaining / 100.0f; + batt_msg.power_supply_status = BatteryMsg::POWER_SUPPLY_STATUS_DISCHARGING; + batt_msg.power_supply_health = BatteryMsg::POWER_SUPPLY_HEALTH_UNKNOWN; + + switch (bs.type) { + // [[[cog: + // for f in ( + // 'LIPO', + // 'LIFE', + // 'LION', + // 'NIMH', + // 'UNKNOWN'): + // cog.outl(f"case enum_value(BT::{f}):") + // if f == 'UNKNOWN': + // cog.outl("default:") + // cog.outl( + // f" batt_msg.power_supply_technology = " + // f"BatteryMsg::POWER_SUPPLY_TECHNOLOGY_{f};") + // cog.outl(f" break;") + // ]]] + case enum_value(BT::LIPO): + batt_msg.power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_LIPO; + break; + case enum_value(BT::LIFE): + batt_msg.power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_LIFE; + break; + case enum_value(BT::LION): + batt_msg.power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_LION; + break; + case enum_value(BT::NIMH): + batt_msg.power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_NIMH; + break; + case enum_value(BT::UNKNOWN): + default: + batt_msg.power_supply_technology = BatteryMsg::POWER_SUPPLY_TECHNOLOGY_UNKNOWN; + break; + // [[[end]]] (checksum: 9ff6279a1b2c35e23e0b2e09e915c5ca) + } + + batt_msg.present = true; + + batt_msg.cell_voltage.clear(); + batt_msg.cell_voltage.reserve(bs.voltages.size() + bs.voltages_ext.size()); + float cell_voltage; + float voltage_acc = 0.0f; + float total_voltage = 0.0f; + // 65,534V cell voltage means that the next element in the array must be added to this one + constexpr float coalesce_voltage = (UINT16_MAX - 1) * 0.001f; + for (auto v : bs.voltages) { + if (v == UINT16_MAX) { + break; + } + + if (v == UINT16_MAX - 1) { // cell voltage is above 65,534V + voltage_acc += coalesce_voltage; + continue; // add to the next array element to get the correct voltage + } + + cell_voltage = voltage_acc + (v * 0.001f); // 1 mV + voltage_acc = 0.0f; + batt_msg.cell_voltage.push_back(cell_voltage); + total_voltage += cell_voltage; + } + for (auto v : bs.voltages_ext) { + if (v == UINT16_MAX || v == 0) { + // this one is different from the for loop above to support mavlink2 message truncation + break; + } + + if (v == UINT16_MAX - 1) { + // cell voltage is above 65,534V + // add to the next array element to get the correct voltage + voltage_acc += coalesce_voltage; + continue; + } + + cell_voltage = voltage_acc + (v * 0.001f); // 1 mV + voltage_acc = 0.0f; + batt_msg.cell_voltage.push_back(cell_voltage); + total_voltage += cell_voltage; + } + batt_msg.voltage = total_voltage; + + batt_msg.location = utils::format("id%u", bs.id); + batt_msg.serial_number = ""; + + batt_pub->publish(batt_msg); + - ROS_ERROR_COND_NAMED(!res.success, "sys", "SetMessageInterval: command plugin service call failed!"); + if (bs.id == 0) { + has_battery_status0 = true; + } + + if (!disable_diag && bs.id >= 0 && bs.id < MAX_NR_BATTERY_STATUS) { + batt_diag[bs.id].set(total_voltage, batt_msg.current, batt_msg.percentage); + batt_diag[bs.id].setcell_v(batt_msg.cell_voltage); + } + } + + void handle_estimator_status( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::ESTIMATOR_STATUS & status, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + using ESF = mavlink::common::ESTIMATOR_STATUS_FLAGS; + + auto est_status_msg = mavros_msgs::msg::EstimatorStatus(); + est_status_msg.header.stamp = node->now(); + + auto check_flag = [status](ESF flag) -> bool { + return !!(status.flags & enum_value(flag)); + }; + + // [[[cog: + // import pymavlink.dialects.v20.common as common + // ename = 'ESTIMATOR_STATUS_FLAGS' + // ename_pfx2 = 'ESTIMATOR_' + // + // enum = sorted(common.enums[ename].items()) + // enum.pop() # -> remove ENUM_END + // + // for k, e in enum: + // esf = e.name + // + // if esf.startswith(ename + '_'): + // esf = esf[len(ename) + 1:] + // if esf.startswith(ename_pfx2): + // esf = esf[len(ename_pfx2):] + // if esf[0].isdigit(): + // esf = 'SENSOR_' + esf + // cog.outl(f"est_status_msg.{esf.lower()}_status_flag = check_flag(ESF::{esf});") + // ]]] + est_status_msg.attitude_status_flag = check_flag(ESF::ATTITUDE); + est_status_msg.velocity_horiz_status_flag = check_flag(ESF::VELOCITY_HORIZ); + est_status_msg.velocity_vert_status_flag = check_flag(ESF::VELOCITY_VERT); + est_status_msg.pos_horiz_rel_status_flag = check_flag(ESF::POS_HORIZ_REL); + est_status_msg.pos_horiz_abs_status_flag = check_flag(ESF::POS_HORIZ_ABS); + est_status_msg.pos_vert_abs_status_flag = check_flag(ESF::POS_VERT_ABS); + est_status_msg.pos_vert_agl_status_flag = check_flag(ESF::POS_VERT_AGL); + est_status_msg.const_pos_mode_status_flag = check_flag(ESF::CONST_POS_MODE); + est_status_msg.pred_pos_horiz_rel_status_flag = check_flag(ESF::PRED_POS_HORIZ_REL); + est_status_msg.pred_pos_horiz_abs_status_flag = check_flag(ESF::PRED_POS_HORIZ_ABS); + est_status_msg.gps_glitch_status_flag = check_flag(ESF::GPS_GLITCH); + est_status_msg.accel_error_status_flag = check_flag(ESF::ACCEL_ERROR); + // [[[end]]] (checksum: fc30da81f9490dede61a58e82c8a2d53) + + estimator_status_pub->publish(est_status_msg); + } + + /* -*- timer callbacks -*- */ + + void timeout_cb() + { + uas->update_connection_status(false); + } + + void heartbeat_cb() + { + using mavlink::common::MAV_MODE; + + mavlink::minimal::msg::HEARTBEAT hb {}; + hb.type = enum_value(conn_heartbeat_mav_type); + hb.autopilot = enum_value(MAV_AUTOPILOT::INVALID); + hb.base_mode = enum_value(MAV_MODE::MANUAL_ARMED); + hb.custom_mode = 0; + hb.system_status = enum_value(MAV_STATE::ACTIVE); + + uas->send_message(hb); + } + + void autopilot_version_cb() + { + using mavlink::common::MAV_CMD; + + auto lg = get_logger(); + bool ret = false; + + // Request from all first 3 times, then fallback to unicast + bool do_broadcast = version_retries > RETRIES_COUNT / 2; + + try { + auto client = node->create_client("cmd/command"); + + auto cmdrq = std::make_shared(); + cmdrq->broadcast = do_broadcast; + cmdrq->command = enum_value(MAV_CMD::REQUEST_AUTOPILOT_CAPABILITIES); + cmdrq->confirmation = false; + cmdrq->param1 = 1.0; + + RCLCPP_DEBUG( + lg, "VER: Sending %s request.", + (do_broadcast) ? "broadcast" : "unicast"); + + auto future = client->async_send_request(cmdrq); + // NOTE(vooon): temporary hack from @Michel1968 + // See: https://github.com/mavlink/mavros/issues/1588#issuecomment-1027699924 + const auto future_status = future.wait_for(1s); + if (future_status == std::future_status::ready) { + auto response = future.get(); + ret = response->success; + } else { + RCLCPP_ERROR(lg, "VER: autopilot version service timeout"); + } + } catch (std::exception & ex) { + RCLCPP_ERROR_STREAM(lg, "VER: " << ex.what()); + } + + RCLCPP_ERROR_EXPRESSION(lg, !ret, "VER: command plugin service call failed!"); + + if (version_retries > 0) { + version_retries--; + RCLCPP_WARN_EXPRESSION( + lg, version_retries != RETRIES_COUNT - 1, + "VER: %s request timeout, retries left %d", + (do_broadcast) ? "broadcast" : "unicast", + version_retries); + } else { + uas->update_capabilities(false); + autopilot_version_timer->cancel(); + RCLCPP_WARN( + lg, + "VER: your FCU don't support AUTOPILOT_VERSION, " + "switched to default capabilities"); + } + } + + void connection_cb(bool connected) override + { + has_battery_status0 = false; + + // if connection changes, start delayed version request + version_retries = RETRIES_COUNT; + if (connected) { + autopilot_version_timer->reset(); + } else { + autopilot_version_timer->cancel(); + } + + // add/remove APM diag tasks + if (connected && disable_diag && uas->is_ardupilotmega()) { + uas->diagnostic_updater.add(mem_diag); + uas->diagnostic_updater.add(hwst_diag); + } else { + uas->diagnostic_updater.removeByName(mem_diag.getName()); + uas->diagnostic_updater.removeByName(hwst_diag.getName()); + } + + if (!connected) { + // publish connection change + publish_disconnection(); + + // Clear known vehicles + vehicles.clear(); + } + } + + /* -*- subscription callbacks -*- */ + + void statustext_cb(const mavros_msgs::msg::StatusText::SharedPtr req) + { + mavlink::common::msg::STATUSTEXT statustext {}; + statustext.severity = req->severity; + mavlink::set_string_z(statustext.text, req->text); + + // Limit the length of the string by null-terminating at the 50-th character + RCLCPP_WARN_EXPRESSION( + node->get_logger(), + req->text.length() >= statustext.text.size(), + "Status text too long: truncating..."); + + uas->send_message(statustext); + } + + /* -*- ros callbacks -*- */ + + void set_rate_cb( + const mavros_msgs::srv::StreamRate::Request::SharedPtr req, + mavros_msgs::srv::StreamRate::Response::SharedPtr res [[maybe_unused]]) + { + mavlink::common::msg::REQUEST_DATA_STREAM rq = {}; + + uas->msg_set_target(rq); + rq.req_stream_id = req->stream_id; + rq.req_message_rate = req->message_rate; + rq.start_stop = (req->on_off) ? 1 : 0; + + uas->send_message(rq); + } + + void set_message_interval_cb( + const mavros_msgs::srv::MessageInterval::Request::SharedPtr req, + mavros_msgs::srv::MessageInterval::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + + auto lg = get_logger(); + + try { + auto client = node->create_client("cmd/command"); + + // calculate interval + float interval_us; + if (req->message_rate < 0) { + interval_us = -1.0f; + } else if (req->message_rate == 0) { + interval_us = 0.0f; + } else { + interval_us = 1000000.0f / req->message_rate; + } + + auto cmdrq = std::make_shared(); + cmdrq->broadcast = false; + cmdrq->command = enum_value(MAV_CMD::SET_MESSAGE_INTERVAL); + cmdrq->confirmation = false; + cmdrq->param1 = req->message_id; + cmdrq->param2 = interval_us; + + RCLCPP_DEBUG( + lg, + "SYS: Request msgid %u at %f hz", + req->message_id, req->message_rate); + + auto future = client->async_send_request(cmdrq); + // NOTE(vooon): same hack as for VER + const auto future_status = future.wait_for(1s); + if (future_status == std::future_status::ready) { + auto response = future.get(); + res->success = response->success; + } else { + RCLCPP_ERROR(lg, "SYS: set_message_interval service timeout"); + } + } catch (std::exception & ex) { + RCLCPP_ERROR_STREAM(lg, "SYS: " << ex.what()); + } + + RCLCPP_ERROR_EXPRESSION( + lg, !res->success, + "SYS: command plugin service call failed!"); + } + + void set_mode_cb( + const mavros_msgs::srv::SetMode::Request::SharedPtr req, + mavros_msgs::srv::SetMode::Response::SharedPtr res) + { + using mavlink::minimal::MAV_MODE_FLAG; + + uint8_t base_mode = req->base_mode; + uint32_t custom_mode = 0; + + if (req->custom_mode != "") { + if (!uas->cmode_from_str(req->custom_mode, custom_mode)) { + res->mode_sent = false; + // XXX(vooon): throw? + return; + } + + /** + * @note That call may trigger unexpected arming change because + * base_mode arming flag state based on previous HEARTBEAT + * message value. + */ + base_mode |= (uas->get_armed()) ? enum_value(MAV_MODE_FLAG::SAFETY_ARMED) : 0; + base_mode |= (uas->get_hil_state()) ? enum_value(MAV_MODE_FLAG::HIL_ENABLED) : 0; + base_mode |= enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED); + } - return res.success; + mavlink::common::msg::SET_MODE sm = {}; + sm.target_system = uas->get_tgt_system(); + sm.base_mode = base_mode; + sm.custom_mode = custom_mode; + + uas->send_message(sm); + res->mode_sent = true; + } + + void vehicle_info_get_cb( + const mavros_msgs::srv::VehicleInfoGet::Request::SharedPtr req, + mavros_msgs::srv::VehicleInfoGet::Response::SharedPtr res) + { + if (req->get_all) { + // Send all vehicles + for (const auto & got : vehicles) { + res->vehicles.emplace_back(got.second); + } + + res->success = true; + return; } + + uint8_t req_sysid = req->sysid; + uint8_t req_compid = req->compid; + + if (req->sysid == 0 && req->compid == 0) { + // use target + req_sysid = uas->get_tgt_system(); + req_compid = uas->get_tgt_component(); + } + + uint16_t key = get_vehicle_key(req_sysid, req_compid); + auto it = vehicles.find(key); + if (it == vehicles.end()) { + // Vehicle not found + res->success = false; + return; + } + + res->vehicles.emplace_back(it->second); + res->success = true; + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SystemStatusPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::SystemStatusPlugin) diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index d9fc37b85..b76bc9690 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -7,22 +7,31 @@ * @{ */ /* - * Copyright 2014,2015,2016,2017 Vladimir Ermakov, M.H.Kabir. + * Copyright 2014,2015,2016,2017,2021 Vladimir Ermakov, M.H.Kabir. * * This file is part of the mavros package and subject to the license terms * in the top-level LICENSE file of the mavros repository. * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ -#include +#include +#include -#include -#include -#include -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "sensor_msgs/msg/time_reference.hpp" +#include "mavros_msgs/msg/timesync_status.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT -namespace mavros { -namespace std_plugins { /** * Time syncronization status publisher * @@ -31,475 +40,512 @@ namespace std_plugins { class TimeSyncStatus : public diagnostic_updater::DiagnosticTask { public: - TimeSyncStatus(const std::string &name, size_t win_size) : - diagnostic_updater::DiagnosticTask(name), - times_(win_size), - seq_nums_(win_size), - window_size_(win_size), - min_freq_(0.01), - max_freq_(10), - tolerance_(0.1), - last_rtt(0), - rtt_sum(0), - last_remote_ts(0), - offset(0) - { - clear(); - } - - void clear() - { - std::lock_guard lock(mutex); - - ros::Time curtime = ros::Time::now(); - count_ = 0; - rtt_sum = 0; - - for (size_t i = 0; i < window_size_; i++) - { - times_[i] = curtime; - seq_nums_[i] = count_; - } - - hist_indx_ = 0; - } - - void tick(int64_t rtt_ns, uint64_t remote_timestamp_ns, int64_t time_offset_ns) - { - std::lock_guard lock(mutex); - - count_++; - last_rtt = rtt_ns; - rtt_sum += rtt_ns; - last_remote_ts = remote_timestamp_ns; - offset = time_offset_ns; - } - - void set_timestamp(uint64_t remote_timestamp_ns) - { - std::lock_guard lock(mutex); - last_remote_ts = remote_timestamp_ns; - } - - void run(diagnostic_updater::DiagnosticStatusWrapper &stat) - { - std::lock_guard lock(mutex); - - ros::Time curtime = ros::Time::now(); - int curseq = count_; - int events = curseq - seq_nums_[hist_indx_]; - double window = (curtime - times_[hist_indx_]).toSec(); - double freq = events / window; - seq_nums_[hist_indx_] = curseq; - times_[hist_indx_] = curtime; - hist_indx_ = (hist_indx_ + 1) % window_size_; - - if (events == 0) { - stat.summary(2, "No events recorded."); - } - else if (freq < min_freq_ * (1 - tolerance_)) { - stat.summary(1, "Frequency too low."); - } - else if (freq > max_freq_ * (1 + tolerance_)) { - stat.summary(1, "Frequency too high."); - } - else { - stat.summary(0, "Normal"); - } - - stat.addf("Timesyncs since startup", "%d", count_); - stat.addf("Frequency (Hz)", "%f", freq); - stat.addf("Last RTT (ms)", "%0.6f", last_rtt / 1e6); - stat.addf("Mean RTT (ms)", "%0.6f", (count_) ? rtt_sum / count_ / 1e6 : 0.0); - stat.addf("Last remote time (s)", "%0.9f", last_remote_ts / 1e9); - stat.addf("Estimated time offset (s)", "%0.9f", offset / 1e9); - } + TimeSyncStatus(const std::string & name, size_t win_size) + : diagnostic_updater::DiagnosticTask(name), + times_(win_size), + seq_nums_(win_size), + window_size_(win_size), + min_freq_(0.01), + max_freq_(10), + tolerance_(0.1), + last_rtt(0), + rtt_sum(0), + last_remote_ts(0), + offset(0) + { + clear(); + } + + void clear() + { + std::lock_guard lock(mutex); + + auto curtime = clock.now(); + count_ = 0; + rtt_sum = 0; + + for (size_t i = 0; i < window_size_; i++) { + times_[i] = curtime; + seq_nums_[i] = count_; + } + + hist_indx_ = 0; + } + + void tick(int64_t rtt_ns, uint64_t remote_timestamp_ns, int64_t time_offset_ns) + { + std::lock_guard lock(mutex); + + count_++; + last_rtt = rtt_ns; + rtt_sum += rtt_ns; + last_remote_ts = remote_timestamp_ns; + offset = time_offset_ns; + } + + void set_timestamp(uint64_t remote_timestamp_ns) + { + std::lock_guard lock(mutex); + last_remote_ts = remote_timestamp_ns; + } + + void run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + std::lock_guard lock(mutex); + + auto curtime = clock.now(); + int curseq = count_; + int events = curseq - seq_nums_[hist_indx_]; + double window = (curtime - times_[hist_indx_]).seconds(); + double freq = events / window; + seq_nums_[hist_indx_] = curseq; + times_[hist_indx_] = curtime; + hist_indx_ = (hist_indx_ + 1) % window_size_; + + if (events == 0) { + stat.summary(2, "No events recorded."); + } else if (freq < min_freq_ * (1 - tolerance_)) { + stat.summary(1, "Frequency too low."); + } else if (freq > max_freq_ * (1 + tolerance_)) { + stat.summary(1, "Frequency too high."); + } else { + stat.summary(0, "Normal"); + } + + stat.addf("Timesyncs since startup", "%d", count_); + stat.addf("Frequency (Hz)", "%f", freq); + stat.addf("Last RTT (ms)", "%0.6f", last_rtt / 1e6); + stat.addf("Mean RTT (ms)", "%0.6f", (count_) ? rtt_sum / count_ / 1e6 : 0.0); + stat.addf("Last remote time (s)", "%0.9f", last_remote_ts / 1e9); + stat.addf("Estimated time offset (s)", "%0.9f", offset / 1e9); + } private: - int count_; - std::vector times_; - std::vector seq_nums_; - int hist_indx_; - std::mutex mutex; - const size_t window_size_; - const double min_freq_; - const double max_freq_; - const double tolerance_; - int64_t last_rtt; - int64_t rtt_sum; - uint64_t last_remote_ts; - int64_t offset; + rclcpp::Clock clock; + int count_; + std::vector times_; + std::vector seq_nums_; + int hist_indx_; + std::mutex mutex; + const size_t window_size_; + const double min_freq_; + const double max_freq_; + const double tolerance_; + int64_t last_rtt; + int64_t rtt_sum; + uint64_t last_remote_ts; + int64_t offset; }; /** * @brief System time plugin + * @plugin sys_time */ -class SystemTimePlugin : public plugin::PluginBase { +class SystemTimePlugin : public plugin::Plugin +{ public: - SystemTimePlugin() : PluginBase(), - nh("~"), - dt_diag("Time Sync", 10), - time_offset(0.0), - time_skew(0.0), - sequence(0), - filter_alpha(0), - filter_beta(0), - high_rtt_count(0), - high_deviation_count(0) - { } - - using TSM = UAS::timesync_mode; - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - double conn_system_time_d; - double conn_timesync_d; - std::string ts_mode_str; - - ros::WallDuration conn_system_time; - ros::WallDuration conn_timesync; - - if (nh.getParam("conn/system_time_rate", conn_system_time_d) && conn_system_time_d != 0.0) { - conn_system_time = ros::WallDuration(ros::Rate(conn_system_time_d)); - } - - if (nh.getParam("conn/timesync_rate", conn_timesync_d) && conn_timesync_d != 0.0) { - conn_timesync = ros::WallDuration(ros::Rate(conn_timesync_d)); - } - - nh.param("time/time_ref_source", time_ref_source, "fcu"); - nh.param("time/timesync_mode", ts_mode_str, "MAVLINK"); - - // Filter gains - // - // Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead - // to a smoother estimate, but track time drift more slowly, introducing a bias - // in the estimate. Larger values will cause low-amplitude oscillations. - // - // Beta : Used to smooth the clock skew estimate. Smaller values will lead to a - // tighter estimation of the skew (derivative), but will negatively affect how fast the - // filter reacts to clock skewing (e.g cause by temperature changes to the oscillator). - // Larger values will cause large-amplitude oscillations. - nh.param("time/timesync_alpha_initial", filter_alpha_initial, 0.05f); - nh.param("time/timesync_beta_initial", filter_beta_initial, 0.05f); - nh.param("time/timesync_alpha_final", filter_alpha_final, 0.003f); - nh.param("time/timesync_beta_final", filter_beta_final, 0.003f); - filter_alpha = filter_alpha_initial; - filter_beta = filter_beta_initial; - - // Filter gain scheduling - // - // The filter interpolates between the initial and final gains while the number of - // exhanged timesync packets is less than convergence_window. A lower value will - // allow the timesync to converge faster, but with potentially less accurate initial - // offset and skew estimates. - nh.param("time/convergence_window", convergence_window, 500); - - // Outlier rejection and filter reset - // - // Samples with round-trip time higher than max_rtt_sample are not used to update the filter. - // More than max_consecutive_high_rtt number of such events in a row will throw a warning - // but not reset the filter. - // Samples whose calculated clock offset is more than max_deviation_sample off from the current - // estimate are not used to update the filter. More than max_consecutive_high_deviation number - // of such events in a row will reset the filter. This usually happens only due to a time jump - // on the remote system. - nh.param("time/max_rtt_sample", max_rtt_sample, 10); // in ms - nh.param("time/max_deviation_sample", max_deviation_sample, 100); // in ms - nh.param("time/max_consecutive_high_rtt", max_cons_high_rtt, 5); - nh.param("time/max_consecutive_high_deviation", max_cons_high_deviation, 5); - - // Set timesync mode - auto ts_mode = utils::timesync_mode_from_str(ts_mode_str); - m_uas->set_timesync_mode(ts_mode); - ROS_INFO_STREAM_NAMED("time", "TM: Timesync mode: " << utils::to_string(ts_mode)); - - nh.param("time/publish_sim_time", publish_sim_time, false); - if (publish_sim_time) { - sim_time_pub = nh.advertise("/clock", 10); - ROS_INFO_STREAM_NAMED("time", "TM: Publishing sim time"); - } else { - ROS_INFO_STREAM_NAMED("time", "TM: Not publishing sim time"); - } - time_ref_pub = nh.advertise("time_reference", 10); - - timesync_status_pub = nh.advertise("timesync_status", 10); - - // timer for sending system time messages - if (!conn_system_time.isZero()) { - sys_time_timer = nh.createWallTimer(conn_system_time, - &SystemTimePlugin::sys_time_cb, this); - sys_time_timer.start(); - } - - // timer for sending timesync messages - if (!conn_timesync.isZero() && !(ts_mode == TSM::NONE || ts_mode == TSM::PASSTHROUGH)) { - // enable timesync diag only if that feature enabled - UAS_DIAG(m_uas).add(dt_diag); - - timesync_timer = nh.createWallTimer(conn_timesync, - &SystemTimePlugin::timesync_cb, this); - timesync_timer.start(); - } - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&SystemTimePlugin::handle_system_time), - make_handler(&SystemTimePlugin::handle_timesync), - }; - } + using TSM = uas::timesync_mode; + + explicit SystemTimePlugin(plugin::UASPtr uas_) + : Plugin(uas_, "time"), + dt_diag("Time Sync", 10), + time_offset(0.0), + time_skew(0.0), + sequence(0), + filter_alpha(0), + filter_beta(0), + high_rtt_count(0), + high_deviation_count(0) + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "time_ref_source", "fcu", [&](const rclcpp::Parameter & p) { + time_ref_source = p.as_string(); + }); + + node_declare_and_watch_parameter( + "timesync_mode", "MAVLINK", [&](const rclcpp::Parameter & p) { + auto ts_mode = utils::timesync_mode_from_str(p.as_string()); + uas->set_timesync_mode(ts_mode); + RCLCPP_INFO_STREAM(get_logger(), "TM: Timesync mode: " << utils::to_string(ts_mode)); + }); + + node_declare_and_watch_parameter( + "system_time_rate", 0.0, [&](const rclcpp::Parameter & p) { + auto rate_d = p.as_double(); + + if (rate_d == 0) { + if (sys_time_timer) { + sys_time_timer->cancel(); + sys_time_timer.reset(); + } + } else { + rclcpp::WallRate rate(rate_d); + + sys_time_timer = + node->create_wall_timer( + rate.period(), + std::bind(&SystemTimePlugin::sys_time_cb, this)); + } + }); + + node_declare_and_watch_parameter( + "timesync_rate", 0.0, [&](const rclcpp::Parameter & p) { + auto rate_d = p.as_double(); + + if (rate_d == 0) { + if (timesync_timer) { + timesync_timer->cancel(); + timesync_timer.reset(); + uas->diagnostic_updater.removeByName(dt_diag.getName()); + } + } else { + rclcpp::WallRate rate(rate_d); + + timesync_timer = + node->create_wall_timer( + rate.period(), + std::bind(&SystemTimePlugin::timesync_cb, this)); + + uas->diagnostic_updater.add(dt_diag); + } + }); + + // Filter gains + // + // Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead + // to a smoother estimate, but track time drift more slowly, introducing a bias + // in the estimate. Larger values will cause low-amplitude oscillations. + // + // Beta : Used to smooth the clock skew estimate. Smaller values will lead to a + // tighter estimation of the skew (derivative), but will negatively affect how fast the + // filter reacts to clock skewing (e.g cause by temperature changes to the oscillator). + // Larger values will cause large-amplitude oscillations. + node_declare_and_watch_parameter( + "timesync_alpha_initial", 0.05, [&](const rclcpp::Parameter & p) { + filter_alpha_initial = p.as_double(); + reset_filter(); + }); + node_declare_and_watch_parameter( + "timesync_beta_initial", 0.05, [&](const rclcpp::Parameter & p) { + filter_beta_initial = p.as_double(); + reset_filter(); + }); + node_declare_and_watch_parameter( + "timesync_alpha_final", 0.003, [&](const rclcpp::Parameter & p) { + filter_alpha_final = p.as_double(); + reset_filter(); + }); + node_declare_and_watch_parameter( + "timesync_beta_final", 0.003, [&](const rclcpp::Parameter & p) { + filter_beta_final = p.as_double(); + reset_filter(); + }); + + // Filter gain scheduling + // + // The filter interpolates between the initial and final gains while the number of + // exhanged timesync packets is less than convergence_window. A lower value will + // allow the timesync to converge faster, but with potentially less accurate initial + // offset and skew estimates. + node_declare_and_watch_parameter( + "convergence_window", 500, [&](const rclcpp::Parameter & p) { + convergence_window = p.as_int(); + }); + + // Outlier rejection and filter reset + // + // Samples with round-trip time higher than max_rtt_sample are not used to update the filter. + // More than max_consecutive_high_rtt number of such events in a row will throw a warning + // but not reset the filter. + // Samples whose calculated clock offset is more than max_deviation_sample off from the current + // estimate are not used to update the filter. More than max_consecutive_high_deviation number + // of such events in a row will reset the filter. This usually happens only due to a time jump + // on the remote system. + node_declare_and_watch_parameter( + "max_rtt_sample", 10, [&](const rclcpp::Parameter & p) { + max_rtt_sample = p.as_int(); // in ms + }); + node_declare_and_watch_parameter( + "max_deviation_sample", 10, [&](const rclcpp::Parameter & p) { + max_deviation_sample = p.as_int(); // in ms + }); + node_declare_and_watch_parameter( + "max_consecutive_high_rtt", 10, [&](const rclcpp::Parameter & p) { + max_cons_high_rtt = p.as_int(); + }); + node_declare_and_watch_parameter( + "max_consecutive_high_deviation", 10, [&](const rclcpp::Parameter & p) { + max_cons_high_deviation = p.as_int(); + }); + + auto sensor_qos = rclcpp::SensorDataQoS(); + + time_ref_pub = node->create_publisher( + "time_reference", + sensor_qos); + timesync_status_pub = node->create_publisher( + "timesync_status", sensor_qos); + + reset_filter(); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&SystemTimePlugin::handle_system_time), + make_handler(&SystemTimePlugin::handle_timesync), + }; + } private: - ros::NodeHandle nh; - ros::Publisher sim_time_pub; - ros::Publisher time_ref_pub; - ros::Publisher timesync_status_pub; - - ros::WallTimer sys_time_timer; - ros::WallTimer timesync_timer; - - TimeSyncStatus dt_diag; - - std::string time_ref_source; - - // Estimated statistics - double time_offset; - double time_skew; - - // Filter parameters - uint32_t sequence; - double filter_alpha; - double filter_beta; - - // Filter settings - float filter_alpha_initial; - float filter_beta_initial; - float filter_alpha_final; - float filter_beta_final; - int convergence_window; - - // Outlier rejection - int max_rtt_sample; - int max_deviation_sample; - int max_cons_high_rtt; - int max_cons_high_deviation; - int high_rtt_count; - int high_deviation_count; - - bool publish_sim_time; - - void handle_system_time(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYSTEM_TIME &mtime) - { - // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 - const bool fcu_time_valid = mtime.time_unix_usec > 1234567890ULL * 1000000; - - if (fcu_time_valid) { - // continious publish for ntpd - auto time_unix = boost::make_shared(); - ros::Time time_ref( - mtime.time_unix_usec / 1000000, // t_sec - (mtime.time_unix_usec % 1000000) * 1000); // t_nsec - - time_unix->header.stamp = ros::Time::now(); - time_unix->time_ref = time_ref; - time_unix->source = time_ref_source; - - time_ref_pub.publish(time_unix); - if(publish_sim_time) { - auto clock = boost::make_shared(); - clock->clock = time_ref; - sim_time_pub.publish(clock); - } - } - else { - ROS_WARN_THROTTLE_NAMED(60, "time", "TM: Wrong FCU time."); - } - } - - void handle_timesync(const mavlink::mavlink_message_t *msg, mavlink::common::msg::TIMESYNC &tsync) - { - uint64_t now_ns = ros::Time::now().toNSec(); - - if (tsync.tc1 == 0) { - send_timesync_msg(now_ns, tsync.ts1); - return; - } - else if (tsync.tc1 > 0) { - // Time offset between this system and the remote system is calculated assuming RTT for - // the timesync packet is roughly equal both ways. - add_timesync_observation((tsync.ts1 + now_ns - tsync.tc1 * 2) / 2, tsync.ts1, tsync.tc1); - } - } - - void sys_time_cb(const ros::WallTimerEvent &event) - { - // For filesystem only - uint64_t time_unix_usec = ros::Time::now().toNSec() / 1000; // nano -> micro - - mavlink::common::msg::SYSTEM_TIME mtime {}; - mtime.time_unix_usec = time_unix_usec; - - UAS_FCU(m_uas)->send_message_ignore_drop(mtime); - } - - void timesync_cb(const ros::WallTimerEvent &event) - { - auto ts_mode = m_uas->get_timesync_mode(); - if (ts_mode == TSM::MAVLINK) { - send_timesync_msg(0, ros::Time::now().toNSec()); - } else if (ts_mode == TSM::ONBOARD) { - // Calculate offset between CLOCK_REALTIME (ros::WallTime) and CLOCK_MONOTONIC - uint64_t realtime_now_ns = ros::Time::now().toNSec(); - uint64_t monotonic_now_ns = get_monotonic_now(); - - add_timesync_observation(realtime_now_ns - monotonic_now_ns, realtime_now_ns, monotonic_now_ns); - } - } - - void send_timesync_msg(uint64_t tc1, uint64_t ts1) - { - mavlink::common::msg::TIMESYNC tsync {}; - tsync.tc1 = tc1; - tsync.ts1 = ts1; - - UAS_FCU(m_uas)->send_message_ignore_drop(tsync); - } - - void add_timesync_observation(int64_t offset_ns, uint64_t local_time_ns, uint64_t remote_time_ns) - { - uint64_t now_ns = ros::Time::now().toNSec(); - - // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system - uint64_t rtt_ns = now_ns - local_time_ns; - - // Calculate the difference of this sample from the current estimate - uint64_t deviation = llabs(int64_t(time_offset) - offset_ns); - - if (rtt_ns < max_rtt_sample * 1000000ULL) { // Only use samples with low RTT - if (sync_converged() && (deviation > max_deviation_sample * 1000000ULL)) { - // Increment the counter if we have a good estimate and are getting samples far from the estimate - high_deviation_count++; - - // We reset the filter if we received consecutive samples which violate our present estimate. - // This is most likely due to a time jump on the offboard system. - if (high_deviation_count > max_cons_high_deviation) { - ROS_ERROR_NAMED("time", "TM : Time jump detected. Resetting time synchroniser."); - - // Reset the filter - reset_filter(); - - // Reset diagnostics - dt_diag.clear(); - dt_diag.set_timestamp(remote_time_ns); - } - } else { - // Filter gain scheduling - if (!sync_converged()) { - // Interpolate with a sigmoid function - float progress = float(sequence) / convergence_window; - float p = 1.0f - expf(0.5f * (1.0f - 1.0f / (1.0f - progress))); - filter_alpha = p * filter_alpha_final + (1.0f - p) * filter_alpha_initial; - filter_beta = p * filter_beta_final + (1.0f - p) * filter_beta_initial; - } else { - filter_alpha = filter_alpha_final; - filter_beta = filter_beta_final; - } - - // Perform filter update - add_sample(offset_ns); - - // Save time offset for other components to use - m_uas->set_time_offset(sync_converged() ? time_offset : 0); - - // Increment sequence counter after filter update - sequence++; - - // Reset high deviation count after filter update - high_deviation_count = 0; - - // Reset high RTT count after filter update - high_rtt_count = 0; - } - } else { - // Increment counter if round trip time is too high for accurate timesync - high_rtt_count++; - - if (high_rtt_count > max_cons_high_rtt) { - // Issue a warning to the user if the RTT is constantly high - ROS_WARN_NAMED("time", "TM : RTT too high for timesync: %0.2f ms.", rtt_ns / 1000000.0); - - // Reset counter - high_rtt_count = 0; - } - } - - // Publish timesync status - auto timesync_status = boost::make_shared(); - - timesync_status->header.stamp = ros::Time::now(); - timesync_status->remote_timestamp_ns = remote_time_ns; - timesync_status->observed_offset_ns = offset_ns; - timesync_status->estimated_offset_ns = time_offset; - timesync_status->round_trip_time_ms = float(rtt_ns / 1000000.0); - - timesync_status_pub.publish(timesync_status); - - // Update diagnostics - dt_diag.tick(rtt_ns, remote_time_ns, time_offset); - } - - void add_sample(int64_t offset_ns) - { - /* Online exponential smoothing filter. The derivative of the estimate is also - * estimated in order to produce an estimate without steady state lag: - * https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing - */ - - double time_offset_prev = time_offset; - - if (sequence == 0) { // First offset sample - time_offset = offset_ns; - } else { - // Update the clock offset estimate - time_offset = filter_alpha * offset_ns + (1.0 - filter_alpha) * (time_offset + time_skew); - - // Update the clock skew estimate - time_skew = filter_beta * (time_offset - time_offset_prev) + (1.0 - filter_beta) * time_skew; - } - } - - void reset_filter() - { - // Do a full reset of all statistics and parameters - sequence = 0; - time_offset = 0.0; - time_skew = 0.0; - filter_alpha = filter_alpha_initial; - filter_beta = filter_beta_initial; - high_deviation_count = 0; - high_rtt_count = 0; - } - - inline bool sync_converged() - { - return sequence >= convergence_window; - } - - uint64_t get_monotonic_now(void) - { - struct timespec spec; - clock_gettime(CLOCK_MONOTONIC, &spec); - - return spec.tv_sec * 1000000000ULL + spec.tv_nsec; - } + rclcpp::Publisher::SharedPtr time_ref_pub; + rclcpp::Publisher::SharedPtr timesync_status_pub; + + rclcpp::TimerBase::SharedPtr sys_time_timer; + rclcpp::TimerBase::SharedPtr timesync_timer; + + TimeSyncStatus dt_diag; + + std::string time_ref_source; + + // Estimated statistics + double time_offset; + double time_skew; + + // Filter parameters + uint32_t sequence; + double filter_alpha; + double filter_beta; + + // Filter settings + float filter_alpha_initial; + float filter_beta_initial; + float filter_alpha_final; + float filter_beta_final; + int convergence_window; + + // Outlier rejection + int max_rtt_sample; + int max_deviation_sample; + int max_cons_high_rtt; + int max_cons_high_deviation; + int high_rtt_count; + int high_deviation_count; + + void handle_system_time( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::SYSTEM_TIME & mtime, plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 + const bool fcu_time_valid = mtime.time_unix_usec > 1234567890ULL * 1000000; + + if (fcu_time_valid) { + // continious publish for ntpd + auto time_unix = sensor_msgs::msg::TimeReference(); + rclcpp::Time time_ref( + mtime.time_unix_usec / 1000000, // t_sec + (mtime.time_unix_usec % 1000000) * 1000); // t_nsec + + time_unix.header.stamp = node->now(); + time_unix.time_ref = time_ref; + time_unix.source = time_ref_source; + + time_ref_pub->publish(time_unix); + } else { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 60000, "TM: Wrong FCU time."); + } + } + + void handle_timesync( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::TIMESYNC & tsync, plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + uint64_t now_ns = node->now().nanoseconds(); + + if (tsync.tc1 == 0) { + send_timesync_msg(now_ns, tsync.ts1); + return; + } else if (tsync.tc1 > 0) { + // Time offset between this system and the remote system is calculated assuming RTT for + // the timesync packet is roughly equal both ways. + add_timesync_observation((tsync.ts1 + now_ns - tsync.tc1 * 2) / 2, tsync.ts1, tsync.tc1); + } + } + + void sys_time_cb() + { + // For filesystem only + uint64_t time_unix_usec = node->now().nanoseconds() / 1000; // nano -> micro + + mavlink::common::msg::SYSTEM_TIME mtime {}; + mtime.time_unix_usec = time_unix_usec; + + uas->send_message(mtime); + } + + void timesync_cb() + { + auto ts_mode = uas->get_timesync_mode(); + if (ts_mode == TSM::NONE || ts_mode == TSM::PASSTHROUGH) { + // NOTE(vooon): nothing to do. keep timer running for possible mode change + } else if (ts_mode == TSM::MAVLINK) { + send_timesync_msg(0, node->now().nanoseconds()); + } else if (ts_mode == TSM::ONBOARD) { + // Calculate offset between CLOCK_REALTIME (ros::WallTime) and CLOCK_MONOTONIC + uint64_t realtime_now_ns = node->now().nanoseconds(); + uint64_t monotonic_now_ns = get_monotonic_now(); + + add_timesync_observation( + realtime_now_ns - monotonic_now_ns, realtime_now_ns, + monotonic_now_ns); + } + } + + void send_timesync_msg(uint64_t tc1, uint64_t ts1) + { + mavlink::common::msg::TIMESYNC tsync {}; + tsync.tc1 = tc1; + tsync.ts1 = ts1; + + uas->send_message(tsync); + } + + void add_timesync_observation(int64_t offset_ns, uint64_t local_time_ns, uint64_t remote_time_ns) + { + uint64_t now_ns = node->now().nanoseconds(); + + // Calculate the round trip time (RTT) it took the timesync + // packet to bounce back to us from remote system + uint64_t rtt_ns = now_ns - local_time_ns; + + // Calculate the difference of this sample from the current estimate + uint64_t deviation = llabs(int64_t(time_offset) - offset_ns); + + if (rtt_ns < max_rtt_sample * 1000000ULL) { // Only use samples with low RTT + if (sync_converged() && (deviation > max_deviation_sample * 1000000ULL)) { + // Increment the counter if we have a good estimate and are + // getting samples far from the estimate + high_deviation_count++; + + // We reset the filter if we received consecutive samples + // which violate our present estimate. + // This is most likely due to a time jump on the offboard system. + if (high_deviation_count > max_cons_high_deviation) { + RCLCPP_ERROR(get_logger(), "TM: Time jump detected. Resetting time synchroniser."); + + // Reset the filter + reset_filter(); + + // Reset diagnostics + dt_diag.clear(); + dt_diag.set_timestamp(remote_time_ns); + } + } else { + // Filter gain scheduling + if (!sync_converged()) { + // Interpolate with a sigmoid function + float progress = static_cast(sequence) / convergence_window; + float p = 1.0f - expf(0.5f * (1.0f - 1.0f / (1.0f - progress))); + filter_alpha = p * filter_alpha_final + (1.0f - p) * filter_alpha_initial; + filter_beta = p * filter_beta_final + (1.0f - p) * filter_beta_initial; + } else { + filter_alpha = filter_alpha_final; + filter_beta = filter_beta_final; + } + + // Perform filter update + add_sample(offset_ns); + + // Save time offset for other components to use + uas->set_time_offset(sync_converged() ? time_offset : 0); + + // Increment sequence counter after filter update + sequence++; + + // Reset high deviation count after filter update + high_deviation_count = 0; + + // Reset high RTT count after filter update + high_rtt_count = 0; + } + } else { + // Increment counter if round trip time is too high for accurate timesync + high_rtt_count++; + + if (high_rtt_count > max_cons_high_rtt) { + // Issue a warning to the user if the RTT is constantly high + RCLCPP_WARN(get_logger(), "TM: RTT too high for timesync: %0.2f ms.", rtt_ns / 1000000.0); + + // Reset counter + high_rtt_count = 0; + } + } + + // Publish timesync status + auto timesync_status = mavros_msgs::msg::TimesyncStatus(); + timesync_status.header.stamp = node->now(); + timesync_status.remote_timestamp_ns = remote_time_ns; + timesync_status.observed_offset_ns = offset_ns; + timesync_status.estimated_offset_ns = time_offset; + timesync_status.round_trip_time_ms = static_cast(rtt_ns / 1000000.0); + + timesync_status_pub->publish(timesync_status); + + // Update diagnostics + dt_diag.tick(rtt_ns, remote_time_ns, time_offset); + } + + void add_sample(int64_t offset_ns) + { + /* Online exponential smoothing filter. The derivative of the estimate is also + * estimated in order to produce an estimate without steady state lag: + * https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing + */ + + double time_offset_prev = time_offset; + + if (sequence == 0) { // First offset sample + time_offset = offset_ns; + } else { + // Update the clock offset estimate + time_offset = filter_alpha * offset_ns + (1.0 - filter_alpha) * (time_offset + time_skew); + + // Update the clock skew estimate + time_skew = filter_beta * (time_offset - time_offset_prev) + (1.0 - filter_beta) * time_skew; + } + } + + void reset_filter() + { + // Do a full reset of all statistics and parameters + sequence = 0; + time_offset = 0.0; + time_skew = 0.0; + filter_alpha = filter_alpha_initial; + filter_beta = filter_beta_initial; + high_deviation_count = 0; + high_rtt_count = 0; + } + + inline bool sync_converged() + { + return sequence >= uint32_t(convergence_window); + } + + uint64_t get_monotonic_now(void) + { + struct timespec spec; + clock_gettime(CLOCK_MONOTONIC, &spec); + + return spec.tv_sec * 1000000000ULL + spec.tv_nsec; + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SystemTimePlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::SystemTimePlugin) diff --git a/mavros/src/plugins/vfr_hud.cpp b/mavros/src/plugins/vfr_hud.cpp deleted file mode 100644 index 89352193c..000000000 --- a/mavros/src/plugins/vfr_hud.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/** - * @brief VFR HUD plugin - * @file vfr_hud.cpp - * @author Vladimir Ermakov - * - * @addtogroup plugin - * @{ - */ -/* - * Copyright 2014,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include -#include - -#include - -namespace mavros { -namespace std_plugins { -/** - * @brief VFR HUD plugin. - */ -class VfrHudPlugin : public plugin::PluginBase { -public: - VfrHudPlugin() : PluginBase(), - nh("~") - { } - - /** - * Plugin initializer. Constructor should not do this. - */ - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - vfr_pub = nh.advertise("vfr_hud", 10); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&VfrHudPlugin::handle_vfr_hud), - }; - } - -private: - ros::NodeHandle nh; - - ros::Publisher vfr_pub; - - void handle_vfr_hud(const mavlink::mavlink_message_t *msg, mavlink::common::msg::VFR_HUD &vfr_hud) - { - auto vmsg = boost::make_shared(); - vmsg->header.stamp = ros::Time::now(); - vmsg->airspeed = vfr_hud.airspeed; - vmsg->groundspeed = vfr_hud.groundspeed; - vmsg->heading = vfr_hud.heading; - vmsg->throttle = vfr_hud.throttle / 100.0; // comes in 0..100 range - vmsg->altitude = vfr_hud.alt; - vmsg->climb = vfr_hud.climb; - - vfr_pub.publish(vmsg); - } -}; -} // namespace std_plugins -} // namespace mavros - -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::VfrHudPlugin, mavros::plugin::PluginBase) diff --git a/mavros/src/plugins/waypoint.cpp b/mavros/src/plugins/waypoint.cpp index 1f17d86f5..f117c6a11 100644 --- a/mavros/src/plugins/waypoint.cpp +++ b/mavros/src/plugins/waypoint.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2014,2015,2016,2017,2018,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Waypoint plugin * @file waypoint.cpp @@ -6,325 +13,318 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014,2015,2016,2017,2018 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include -#include -#include -#include +#include "mavros/mission_protocol_base.hpp" +#include "mavros_msgs/msg/waypoint_list.hpp" +#include "mavros_msgs/msg/waypoint_reached.hpp" +#include "mavros_msgs/srv/waypoint_set_current.hpp" +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT -namespace mavros { -namespace std_plugins { /** * @brief Mission manupulation plugin + * @plugin waypoint */ -class WaypointPlugin : public plugin::MissionBase { +class WaypointPlugin : public plugin::MissionBase +{ public: - WaypointPlugin() : - MissionBase("WP"), - wp_nh("~mission") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - MissionBase::initialize_with_nodehandle(&wp_nh); - - wp_state = WP::IDLE; - wp_type = plugin::WP_TYPE::MISSION; - - wp_nh.param("pull_after_gcs", do_pull_after_gcs, true); - wp_nh.param("use_mission_item_int", use_mission_item_int, false); - - wp_list_pub = wp_nh.advertise("waypoints", 2, true); - pull_srv = wp_nh.advertiseService("pull", &WaypointPlugin::pull_cb, this); - push_srv = wp_nh.advertiseService("push", &WaypointPlugin::push_cb, this); - clear_srv = wp_nh.advertiseService("clear", &WaypointPlugin::clear_cb, this); - - wp_reached_pub = wp_nh.advertise("reached", 10, true); - set_cur_srv = wp_nh.advertiseService("set_current", &WaypointPlugin::set_cur_cb, this); - - enable_connection_cb(); - enable_capabilities_cb(); - } - - Subscriptions get_subscriptions() override { - return { - make_handler(&WaypointPlugin::handle_mission_item), - make_handler(&WaypointPlugin::handle_mission_item_int), - make_handler(&WaypointPlugin::handle_mission_request), - make_handler(&WaypointPlugin::handle_mission_request_int), - make_handler(&WaypointPlugin::handle_mission_count), - make_handler(&WaypointPlugin::handle_mission_ack), - make_handler(&WaypointPlugin::handle_mission_current), - make_handler(&WaypointPlugin::handle_mission_item_reached), - }; - } + explicit WaypointPlugin(plugin::UASPtr uas_) + : MissionBase(uas_, "mission"), + enable_partial_push_auto(true) + { + rcl_interfaces::msg::ParameterDescriptor desc_pp{}; +#ifndef USE_OLD_DECLARE_PARAMETER + desc_pp.dynamic_typing = true; +#endif + + enable_node_watch_parameters(); + + // NOTE(vooon): I'm not quite sure that this option would work with mavros router + node_declare_and_watch_parameter( + "pull_after_gcs", true, [&](const rclcpp::Parameter & p) { + do_pull_after_gcs = p.as_bool(); + }); + + node_declare_and_watch_parameter( + "use_mission_item_int", true, [&](const rclcpp::Parameter & p) { + use_mission_item_int = p.as_bool(); + }); + + node_declare_and_watch_parameter( + "enable_partial_push", 2, [&](const rclcpp::Parameter & p) { + RCLCPP_DEBUG_STREAM(get_logger(), log_prefix << ": enable_partial_push = " << p); + + if (p.get_type() == rclcpp::PARAMETER_INTEGER) { + auto v = p.as_int(); + + enable_partial_push_auto = v >= 2; + if (enable_partial_push_auto) { + enable_partial_push = detect_partial_push(); + } else { + enable_partial_push = v != 0; + } + } + + if (p.get_type() == rclcpp::PARAMETER_BOOL) { + enable_partial_push = p.as_bool(); + } + }, desc_pp); + + auto wp_qos = rclcpp::QoS(10).transient_local(); + + wp_list_pub = node->create_publisher("~/waypoints", wp_qos); + wp_reached_pub = node->create_publisher("~/reached", wp_qos); + + pull_srv = + node->create_service( + "~/pull", + std::bind(&WaypointPlugin::pull_cb, this, _1, _2)); + push_srv = + node->create_service( + "~/push", + std::bind(&WaypointPlugin::push_cb, this, _1, _2)); + clear_srv = + node->create_service( + "~/clear", + std::bind(&WaypointPlugin::clear_cb, this, _1, _2)); + set_cur_srv = node->create_service( + "~/set_current", std::bind( + &WaypointPlugin::set_cur_cb, this, _1, _2)); + + enable_connection_cb(); + enable_capabilities_cb(); + } private: - ros::NodeHandle wp_nh; - - ros::Publisher wp_list_pub; - ros::ServiceServer pull_srv; - ros::ServiceServer push_srv; - ros::ServiceServer clear_srv; - - ros::Publisher wp_reached_pub; - ros::ServiceServer set_cur_srv; - - /* -*- rx handlers -*- */ - - /** - * @brief handle MISSION_CURRENT mavlink msg - * This confirms a SET_CUR action - * @param msg Received Mavlink msg - * @param mcur MISSION_CURRENT from msg - */ - void handle_mission_current(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_CURRENT &mcur) - { - unique_lock lock(mutex); - - if (wp_state == WP::SET_CUR) { - /* MISSION_SET_CURRENT ACK */ - ROS_DEBUG_NAMED(log_ns, "%s: set current #%d done", log_ns.c_str(), mcur.seq); - go_idle(); - wp_cur_active = mcur.seq; - set_current_waypoint(wp_cur_active); - - lock.unlock(); - list_sending.notify_all(); - publish_waypoints(); - } - else if (wp_state == WP::IDLE && wp_cur_active != mcur.seq) { - /* update active */ - ROS_DEBUG_NAMED(log_ns, "%s: update current #%d", log_ns.c_str(), mcur.seq); - wp_cur_active = mcur.seq; - set_current_waypoint(wp_cur_active); - - lock.unlock(); - publish_waypoints(); - } - } - - /** - * @brief handle MISSION_ITEM_REACHED mavlink msg - * @param msg Received Mavlink msg - * @param mitr MISSION_ITEM_REACHED from msg - */ - void handle_mission_item_reached(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_ITEM_REACHED &mitr) - { - /* in QGC used as informational message */ - ROS_INFO_NAMED(log_ns, "%s: reached #%d", log_ns.c_str(), mitr.seq); - - auto wpr = boost::make_shared(); - - wpr->header.stamp = ros::Time::now(); - wpr->wp_seq = mitr.seq; - - wp_reached_pub.publish(wpr); - } - - /* -*- mid-level helpers -*- */ - - // Acts when capabilities of the fcu are changed - void capabilities_cb(UAS::MAV_CAP capabilities) override { - lock_guard lock(mutex); - if (m_uas->has_capability(UAS::MAV_CAP::MISSION_INT)) { - use_mission_item_int = true; - mission_item_int_support_confirmed = true; - ROS_INFO_NAMED(log_ns, "%s: Using MISSION_ITEM_INT", log_ns.c_str()); - } else { - use_mission_item_int = false; - mission_item_int_support_confirmed = false; - ROS_WARN_NAMED(log_ns, "%s: Falling back to MISSION_ITEM", log_ns.c_str()); - } - } - - // Act on first heartbeat from FCU - void connection_cb(bool connected) override - { - lock_guard lock(mutex); - if (connected) { - schedule_pull(BOOTUP_TIME_DT); - - if (wp_nh.hasParam("enable_partial_push")) { - wp_nh.getParam("enable_partial_push", enable_partial_push); - } - else { - enable_partial_push = m_uas->is_ardupilotmega(); - } - } - else { - schedule_timer.stop(); - } - } - - //! @brief publish the updated waypoint list after operation - void publish_waypoints() override - { - auto wpl = boost::make_shared(); - unique_lock lock(mutex); - - wpl->current_seq = wp_cur_active; - wpl->waypoints.clear(); - wpl->waypoints.reserve(waypoints.size()); - for (auto &it : waypoints) { - wpl->waypoints.push_back(it); - } - - lock.unlock(); - wp_list_pub.publish(wpl); - } - - /* -*- ROS callbacks -*- */ - - bool pull_cb(mavros_msgs::WaypointPull::Request &req, - mavros_msgs::WaypointPull::Response &res) - { - unique_lock lock(mutex); - - if (wp_state != WP::IDLE) - // Wrong initial state, other operation in progress? - return false; - - wp_state = WP::RXLIST; - wp_count = 0; - restart_timeout_timer(); - - lock.unlock(); - mission_request_list(); - res.success = wait_fetch_all(); - lock.lock(); - - res.wp_received = waypoints.size(); - go_idle(); // not nessessary, but prevents from blocking - return true; - } - - bool push_cb(mavros_msgs::WaypointPush::Request &req, - mavros_msgs::WaypointPush::Response &res) - { - unique_lock lock(mutex); - - if (wp_state != WP::IDLE) - // Wrong initial state, other operation in progress? - return false; - - if (req.start_index) { - // Partial Waypoint update - - if (!enable_partial_push) { - ROS_WARN_NAMED(log_ns, "%s: Partial Push not enabled. (Only supported on APM)", log_ns.c_str()); - res.success = false; - res.wp_transfered = 0; - return true; - } - - if (waypoints.size() < req.start_index + req.waypoints.size()) { - ROS_WARN_NAMED(log_ns, "%s: Partial push out of range rejected.", log_ns.c_str()); - res.success = false; - res.wp_transfered = 0; - return true; - } - - wp_state = WP::TXPARTIAL; - send_waypoints = waypoints; - - uint16_t seq = req.start_index; - for (auto &it : req.waypoints) { - send_waypoints[seq] = it; - seq++; - } - - wp_count = req.waypoints.size(); - wp_start_id = req.start_index; - wp_end_id = req.start_index + wp_count; - wp_cur_id = req.start_index; - restart_timeout_timer(); - - lock.unlock(); - mission_write_partial_list(wp_start_id, wp_end_id); - res.success = wait_push_all(); - lock.lock(); - - res.wp_transfered = wp_cur_id - wp_start_id + 1; - } - else { - // Full waypoint update - wp_state = WP::TXLIST; - - send_waypoints.clear(); - send_waypoints.reserve(req.waypoints.size()); - send_waypoints = req.waypoints; - - wp_count = send_waypoints.size(); - wp_end_id = wp_count; - wp_cur_id = 0; - restart_timeout_timer(); - - lock.unlock(); - mission_count(wp_count); - res.success = wait_push_all(); - lock.lock(); - - res.wp_transfered = wp_cur_id + 1; - } - - go_idle(); // same as in pull_cb - return true; - } - - bool clear_cb(mavros_msgs::WaypointClear::Request &req, - mavros_msgs::WaypointClear::Response &res) - { - unique_lock lock(mutex); - - if (wp_state != WP::IDLE) - return false; - - wp_state = WP::CLEAR; - restart_timeout_timer(); - - lock.unlock(); - mission_clear_all(); - res.success = wait_push_all(); - - lock.lock(); - go_idle(); // same as in pull_cb - return true; - } - - bool set_cur_cb(mavros_msgs::WaypointSetCurrent::Request &req, - mavros_msgs::WaypointSetCurrent::Response &res) - { - unique_lock lock(mutex); - - if (wp_state != WP::IDLE) - return false; - - wp_state = WP::SET_CUR; - wp_set_active = req.wp_seq; - restart_timeout_timer(); - - lock.unlock(); - mission_set_current(wp_set_active); - res.success = wait_push_all(); - - lock.lock(); - go_idle(); // same as in pull_cb - return true; - } + rclcpp::Publisher::SharedPtr wp_list_pub; + rclcpp::Publisher::SharedPtr wp_reached_pub; + + rclcpp::Service::SharedPtr pull_srv; + rclcpp::Service::SharedPtr push_srv; + rclcpp::Service::SharedPtr clear_srv; + rclcpp::Service::SharedPtr set_cur_srv; + + bool enable_partial_push_auto; + + /* -*- mid-level helpers -*- */ + + // Acts when capabilities of the fcu are changed + void capabilities_cb(uas::MAV_CAP capabilities [[maybe_unused]]) override + { + lock_guard lock(mutex); + + if (uas->has_capability(uas::MAV_CAP::MISSION_INT)) { + use_mission_item_int = true; + mission_item_int_support_confirmed = true; + RCLCPP_INFO(get_logger(), "%s: Using MISSION_ITEM_INT", log_prefix); + } else { + use_mission_item_int = false; + mission_item_int_support_confirmed = false; + RCLCPP_WARN(get_logger(), "%s: Falling back to MISSION_ITEM", log_prefix); + } + } + + // Act on first heartbeat from FCU + void connection_cb(bool connected) override + { + lock_guard lock(mutex); + + if (connected) { + schedule_pull(BOOTUP_TIME); + + if (enable_partial_push_auto) { + enable_partial_push = detect_partial_push(); + + RCLCPP_INFO_STREAM( + get_logger(), log_prefix << ": detected enable_partial_push: " << enable_partial_push); + } + } else if (schedule_timer) { + schedule_timer->cancel(); + } + } + + void publish_waypoints() override + { + auto wpl = mavros_msgs::msg::WaypointList(); + unique_lock lock(mutex); + + wpl.current_seq = wp_cur_active; + wpl.waypoints.reserve(waypoints.size()); + for (auto & it : waypoints) { + wpl.waypoints.push_back(it); + } + + lock.unlock(); + wp_list_pub->publish(wpl); + } + + void publish_reached(const uint16_t seq) override + { + auto wr = mavros_msgs::msg::WaypointReached(); + + wr.header.stamp = node->now(); + wr.wp_seq = seq; + + wp_reached_pub->publish(wr); + } + + bool detect_partial_push() + { + return uas->is_ardupilotmega(); + } + + /* -*- ROS callbacks -*- */ + + void pull_cb( + const mavros_msgs::srv::WaypointPull::Request::SharedPtr req [[maybe_unused]], + mavros_msgs::srv::WaypointPull::Response::SharedPtr res) + { + unique_lock lock(mutex); + + if (wp_state != WP::IDLE) { + // Wrong initial state, other operation in progress? + return; + } + + wp_state = WP::RXLIST; + wp_count = 0; + restart_timeout_timer(); + + lock.unlock(); + mission_request_list(); + res->success = wait_fetch_all(); + lock.lock(); + + res->wp_received = waypoints.size(); + go_idle(); // not nessessary, but prevents from blocking + } + + void push_cb( + const mavros_msgs::srv::WaypointPush::Request::SharedPtr req, + mavros_msgs::srv::WaypointPush::Response::SharedPtr res) + { + unique_lock lock(mutex); + + if (wp_state != WP::IDLE) { + // Wrong initial state, other operation in progress? + return; + } + + if (req->start_index) { + // Partial Waypoint update + + if (!enable_partial_push) { + RCLCPP_WARN( + get_logger(), "%s: Partial Push not enabled. (Only supported on APM)", log_prefix); + res->success = false; + res->wp_transfered = 0; + return; + } + + if (waypoints.size() < req->start_index + req->waypoints.size()) { + RCLCPP_WARN(get_logger(), "%s: Partial push out of range rejected.", log_prefix); + res->success = false; + res->wp_transfered = 0; + return; + } + + wp_state = WP::TXPARTIAL; + send_waypoints = waypoints; + + uint16_t seq = req->start_index; + for (auto & it : req->waypoints) { + send_waypoints[seq++] = it; + } + + wp_count = req->waypoints.size(); + wp_start_id = req->start_index; + wp_end_id = req->start_index + wp_count; + wp_cur_id = req->start_index; + restart_timeout_timer(); + + lock.unlock(); + mission_write_partial_list(wp_start_id, wp_end_id); + res->success = wait_push_all(); + lock.lock(); + + res->wp_transfered = wp_cur_id - wp_start_id + 1; + } else { + // Full waypoint update + wp_state = WP::TXLIST; + + send_waypoints.clear(); + send_waypoints.reserve(req->waypoints.size()); + for (auto & wp : req->waypoints) { + send_waypoints.emplace_back(wp); + } + + wp_count = send_waypoints.size(); + wp_end_id = wp_count; + wp_cur_id = 0; + restart_timeout_timer(); + + lock.unlock(); + mission_count(wp_count); + res->success = wait_push_all(); + lock.lock(); + + res->wp_transfered = wp_cur_id + 1; + } + + go_idle(); // same as in pull_cb + } + + void clear_cb( + const mavros_msgs::srv::WaypointClear::Request::SharedPtr req [[maybe_unused]], + mavros_msgs::srv::WaypointClear::Response::SharedPtr res) + { + unique_lock lock(mutex); + + if (wp_state != WP::IDLE) { + return; + } + + wp_state = WP::CLEAR; + restart_timeout_timer(); + + lock.unlock(); + mission_clear_all(); + res->success = wait_push_all(); + + lock.lock(); + go_idle(); // same as in pull_cb + } + + void set_cur_cb( + const mavros_msgs::srv::WaypointSetCurrent::Request::SharedPtr req, + mavros_msgs::srv::WaypointSetCurrent::Response::SharedPtr res) + { + unique_lock lock(mutex); + + if (wp_state != WP::IDLE) { + return; + } + + wp_state = WP::SET_CUR; + wp_set_active = req->wp_seq; + restart_timeout_timer(); + + lock.unlock(); + mission_set_current(wp_set_active); + res->success = wait_push_all(); + + lock.lock(); + go_idle(); // same as in pull_cb + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::WaypointPlugin, mavros::plugin::PluginBase) \ No newline at end of file +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::WaypointPlugin) diff --git a/mavros/src/plugins/wind_estimation.cpp b/mavros/src/plugins/wind_estimation.cpp index d4b5c2306..4d414f4b0 100644 --- a/mavros/src/plugins/wind_estimation.cpp +++ b/mavros/src/plugins/wind_estimation.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2018 Thomas Stastny + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Wind estimation plugin * @file wind_estimation.cpp @@ -6,99 +14,102 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2018 Thomas Stastny - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include #include -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -#include +#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" + +namespace mavros +{ +namespace std_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace std_plugins { /** * @brief Wind estimation plugin. + * @plugin wind_estimation */ -class WindEstimationPlugin : public plugin::PluginBase { +class WindEstimationPlugin : public plugin::Plugin +{ public: - WindEstimationPlugin() : PluginBase(), - nh("~") - { } - - /** - * Plugin initializer. Constructor should not do this. - */ - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - wind_pub = nh.advertise("wind_estimation", 10); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&WindEstimationPlugin::handle_apm_wind), - make_handler(&WindEstimationPlugin::handle_px4_wind), - }; - } + explicit WindEstimationPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "wind") + { + auto sensor_qos = rclcpp::SensorDataQoS(); + + wind_pub = node->create_publisher( + "wind_estimation", sensor_qos); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&WindEstimationPlugin::handle_apm_wind), + make_handler(&WindEstimationPlugin::handle_px4_wind), + }; + } private: - ros::NodeHandle nh; - - ros::Publisher wind_pub; - - /** - * Handle APM specific wind estimation message - */ - void handle_apm_wind(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::WIND &wind) - { - const double speed = wind.speed; - const double course = angles::from_degrees(wind.direction) + M_PI; // direction "from" -> direction "to" - - auto twist_cov = boost::make_shared(); - twist_cov->header.stamp = ros::Time::now(); - twist_cov->twist.twist.linear.x = speed * std::sin(course); // E - twist_cov->twist.twist.linear.y = speed * std::cos(course); // N - twist_cov->twist.twist.linear.z = -wind.speed_z;// D -> U - - // covariance matrix unknown in APM msg - ftf::EigenMapCovariance6d cov_map(twist_cov->twist.covariance.data()); - cov_map.setZero(); - cov_map(0, 0) = -1.0; - - wind_pub.publish(twist_cov); - } - - /** - * Handle PX4 specific wind estimation message - */ - void handle_px4_wind(const mavlink::mavlink_message_t *msg, mavlink::common::msg::WIND_COV &wind) - { - auto twist_cov = boost::make_shared(); - twist_cov->header.stamp = m_uas->synchronise_stamp(wind.time_usec); - - tf::vectorEigenToMsg(ftf::transform_frame_ned_enu(Eigen::Vector3d(wind.wind_x, wind.wind_y, wind.wind_z)), - twist_cov->twist.twist.linear); - - // fill available covariance elements - ftf::EigenMapCovariance6d cov_map(twist_cov->twist.covariance.data()); - cov_map.setZero(); - cov_map(0, 0) = wind.var_horiz; // NOTE: this is a summed covariance for both x and y horizontal wind components - cov_map(2, 2) = wind.var_vert; - - wind_pub.publish(twist_cov); - } + rclcpp::Publisher::SharedPtr wind_pub; + + /** + * Handle APM specific wind estimation message + */ + void handle_apm_wind( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::ardupilotmega::msg::WIND & wind, plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + const double speed = wind.speed; + // direction "from" -> direction "to" + const double course = angles::from_degrees(wind.direction) + M_PI; + + auto twist_cov = geometry_msgs::msg::TwistWithCovarianceStamped(); + twist_cov.header.stamp = node->now(); + twist_cov.twist.twist.linear.x = speed * std::sin(course); // E + twist_cov.twist.twist.linear.y = speed * std::cos(course); // N + twist_cov.twist.twist.linear.z = -wind.speed_z; // D -> U + + // covariance matrix unknown in APM msg + ftf::EigenMapCovariance6d cov_map(twist_cov.twist.covariance.data()); + cov_map.setZero(); + cov_map(0, 0) = -1.0; + + wind_pub->publish(twist_cov); + } + + /** + * Handle PX4 specific wind estimation message + */ + void handle_px4_wind( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::WIND_COV & wind, plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto twist_cov = geometry_msgs::msg::TwistWithCovarianceStamped(); + twist_cov.header.stamp = uas->synchronise_stamp(wind.time_usec); + + tf2::toMsg( + ftf::transform_frame_ned_enu(Eigen::Vector3d(wind.wind_x, wind.wind_y, wind.wind_z)), + twist_cov.twist.twist.linear); + + // fill available covariance elements + ftf::EigenMapCovariance6d cov_map(twist_cov.twist.covariance.data()); + cov_map.setZero(); + // NOTE: this is a summed covariance for both x and y horizontal wind components + cov_map(0, 0) = wind.var_horiz; + cov_map(2, 2) = wind.var_vert; + + wind_pub->publish(twist_cov); + } }; -} // namespace std_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::WindEstimationPlugin, mavros::plugin::PluginBase) +} // namespace std_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::std_plugins::WindEstimationPlugin) diff --git a/mavros/test/mavros_py/__init__.py b/mavros/test/mavros_py/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mavros/test/mavros_py/test_mission.py b/mavros/test/mavros_py/test_mission.py new file mode 100644 index 000000000..0a32e4b2c --- /dev/null +++ b/mavros/test/mavros_py/test_mission.py @@ -0,0 +1,41 @@ +# -*- coding: utf-8 -*- + +import pathlib + +import pytest + +from mavros.mission import QGroundControlWPL + +# from mavros.mission import QGroundControlPlan + + +@pytest.mark.parametrize( + "file_class,file_name,expected_mission_len,expected_fence_len,expected_rally_len", + [ + ( + QGroundControlWPL, + "CMAC-circuit.txt", + 8, + 0, + 0, + ), + # ( # XXX TODO(vooon): implement me! + # QGroundControlPlan, + # "simple.plan", + # 0, + # 0, + # 0, + # ), + ], +) +def test_PlanFile_load( + file_class, file_name, expected_mission_len, expected_fence_len, expected_rally_len +): + file_path = pathlib.Path(__file__).parent / "testdata" / file_name + + with file_path.open() as file_: + pf = file_class().load(file_) + + assert expected_mission_len == len(pf.mission or []) + assert expected_fence_len == len(pf.fence or []) + assert expected_rally_len == len(pf.rally or []) diff --git a/mavros/test/mavros_py/test_param.py b/mavros/test/mavros_py/test_param.py new file mode 100644 index 000000000..2e660d18b --- /dev/null +++ b/mavros/test/mavros_py/test_param.py @@ -0,0 +1,175 @@ +# -*- coding: utf-8 -*- + +import datetime +import io +import pathlib +from unittest.mock import MagicMock, patch + +import pytest + +from mavros.param import ( + MavProxyParam, + MissionPlannerParam, + ParamDict, + Parameter, + QGroundControlParam, +) + + +def test_ParamDict_get(): + pm = ParamDict() + pm._pm = MagicMock() + + tv1 = Parameter("TEST1", value=1) + tv2 = Parameter("TEST2", value=2.0) + + pm.setdefault("TEST1", tv1) + pm.setdefault("TEST2", tv2.value) + + assert pm["TEST1"].value == 1 + assert pm.TEST2.value == 2.0 + + with pytest.raises(KeyError): + pm["NO_KEY"] + + with pytest.raises(AttributeError): + pm.NO_ATTR + + +def test_ParamDict_set(): + pm = ParamDict() + pm._pm = MagicMock() + pm._pm._node = MagicMock() + pm._pm.cli_set_parameters = MagicMock(return_value=None) + + tv1 = Parameter("TEST1", value=1) + tv2 = Parameter("TEST2", value=2.0) + + with patch("mavros.utils.call_set_parameters", MagicMock(return_value={})) as csp: + pm["TEST1"] = tv1 + + csp.assert_called_once_with( + node=pm._pm._node, client=pm._pm.cli_set_parameters, parameters=[tv1] + ) + + with patch("mavros.utils.call_set_parameters", MagicMock(return_value={})) as csp: + pm.TEST2 = tv2 + + csp.assert_called_once_with( + node=pm._pm._node, client=pm._pm.cli_set_parameters, parameters=[tv2] + ) + + with patch("mavros.utils.call_set_parameters", MagicMock(return_value={})) as csp: + + pm.TEST_B = True + pm.TEST_I = 3 + pm.TEST_F = 4.0 + + csp.assert_called() + assert pm.TEST_B.value is True + assert pm.TEST_I.value == 3 + assert pm.TEST_F.value == 4.0 + + with patch("mavros.utils.call_set_parameters", MagicMock(return_value={})) as csp: + + pm.setdefault("TEST_D", 5) + + csp.assert_not_called() + assert pm.TEST_D.value == 5 + + +def test_ParamDict_del(): + pm = ParamDict() + pm._pm = MagicMock() + + pm.setdefault("TEST1", 1) + pm.setdefault("TEST2", 2.0) + + del pm["TEST1"] + del pm.TEST2 + + with pytest.raises(KeyError): + del pm["NO_KEY"] + + with pytest.raises(AttributeError): + del pm.NO_ATTR + + +@pytest.mark.parametrize( + "file_class,file_name,expected_len", + [ + ( + MavProxyParam, + "mavproxy.parm", + 1296, + ), + ( + MissionPlannerParam, + "missionplanner.parm", + 353, + ), + ( + QGroundControlParam, + "qgroundcontrol.params", + 1296, + ), + ], +) +def test_ParamFile_load(file_class, file_name, expected_len): + file_path = pathlib.Path(__file__).parent / "testdata" / file_name + + with file_path.open() as file_: + pf = file_class().load(file_) + + assert expected_len == len(pf.parameters) + + +SAVE_PARAMS = { + "TEST_I": Parameter("TEST_I", value=100), + "TEST_F": Parameter("TEST_F", value=1e3), +} + +SAVE_STAMP = datetime.datetime.now() + + +@pytest.mark.parametrize( + "file_class,expected_output", + [ + ( + MavProxyParam, + f"""\ +#NOTE: {SAVE_STAMP.strftime("%d.%m.%Y %T")}\r\n\ +TEST_I 100\r\n\ +TEST_F 1000.0\r\n\ +""", + ), + ( + MissionPlannerParam, + f"""\ +#NOTE: {SAVE_STAMP.strftime("%d.%m.%Y %T")}\r\n\ +TEST_I,100\r\n\ +TEST_F,1000.0\r\n\ +""", + ), + ( + QGroundControlParam, + f"""\ +# NOTE: {SAVE_STAMP.strftime("%d.%m.%Y %T")}\n\ +# Onboard parameters saved by mavparam for (2.1)\n\ +# MAV ID\tCOMPONENT ID\tPARAM NAME\tVALUE\t(TYPE)\n\ +2\t1\tTEST_I\t100\t6\n\ +2\t1\tTEST_F\t1000.0\t9\n\ +""", + ), + ], +) +def test_ParamFile_save(file_class, expected_output): + + pf = file_class() + pf.parameters = SAVE_PARAMS + pf.tgt_system = 2 + + out = io.StringIO() + pf.save(out) + + assert expected_output == out.getvalue() diff --git a/mavros/test/mavros_py/testdata/CMAC-circuit.txt b/mavros/test/mavros_py/testdata/CMAC-circuit.txt new file mode 100644 index 000000000..e411cd488 --- /dev/null +++ b/mavros/test/mavros_py/testdata/CMAC-circuit.txt @@ -0,0 +1,9 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.362938 149.165085 650.000000 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.359833 149.164703 41.029999 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359585 149.161392 100.000000 1 +3 1 3 16 0.000000 0.000000 0.000000 0.000000 -35.366463 149.162231 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366131 149.164581 100.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359272 149.163757 100.000000 1 +6 0 3 177 2.000000 -1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359272 149.163757 100.000000 1 diff --git a/mavros/test/mavros_py/testdata/mavproxy.parm b/mavros/test/mavros_py/testdata/mavproxy.parm new file mode 100644 index 000000000..7e4aacb02 --- /dev/null +++ b/mavros/test/mavros_py/testdata/mavproxy.parm @@ -0,0 +1,1296 @@ +ACRO_BAL_PITCH 1.000000 +ACRO_BAL_ROLL 1.000000 +ACRO_OPTIONS 0 +ACRO_RP_EXPO 0.300000 +ACRO_RP_P 4.500000 +ACRO_THR_MID 0.000000 +ACRO_TRAINER 2 +ACRO_YAW_P 4.500000 +ACRO_Y_EXPO 0.000000 +ADSB_TYPE 0 +AHRS_COMP_BETA 0.100000 +AHRS_CUSTOM_PIT 0.000000 +AHRS_CUSTOM_ROLL 0.000000 +AHRS_CUSTOM_YAW 0.000000 +AHRS_EKF_TYPE 3 +AHRS_GPS_GAIN 1.000000 +AHRS_GPS_MINSATS 6 +AHRS_GPS_USE 1 +AHRS_ORIENTATION 0 +AHRS_RP_P 0.200000 +AHRS_TRIM_X 0.000000 +AHRS_TRIM_Y 0.000000 +AHRS_TRIM_Z 0.000000 +AHRS_WIND_MAX 0 +AHRS_YAW_P 0.200000 +ANGLE_MAX 3000 +ARMING_ACCTHRESH 0.750000 +ARMING_CHECK 1 +ARMING_MIS_ITEMS 0 +ARMING_RUDDER 2 +ATC_ACCEL_P_MAX 110000.000000 +ATC_ACCEL_R_MAX 110000.000000 +ATC_ACCEL_Y_MAX 27000.000000 +ATC_ANGLE_BOOST 1 +ATC_ANG_LIM_TC 1.000000 +ATC_ANG_PIT_P 4.500000 +ATC_ANG_RLL_P 4.500000 +ATC_ANG_YAW_P 4.500000 +ATC_INPUT_TC 0.150000 +ATC_RATE_FF_ENAB 1 +ATC_RATE_P_MAX 0.000000 +ATC_RATE_R_MAX 0.000000 +ATC_RATE_Y_MAX 0.000000 +ATC_RAT_PIT_D 0.003600 +ATC_RAT_PIT_FF 0.000000 +ATC_RAT_PIT_FLTD 20.000000 +ATC_RAT_PIT_FLTE 0.000000 +ATC_RAT_PIT_FLTT 20.000000 +ATC_RAT_PIT_I 0.135000 +ATC_RAT_PIT_IMAX 0.500000 +ATC_RAT_PIT_P 0.135000 +ATC_RAT_PIT_SMAX 0.000000 +ATC_RAT_RLL_D 0.003600 +ATC_RAT_RLL_FF 0.000000 +ATC_RAT_RLL_FLTD 20.000000 +ATC_RAT_RLL_FLTE 0.000000 +ATC_RAT_RLL_FLTT 20.000000 +ATC_RAT_RLL_I 0.135000 +ATC_RAT_RLL_IMAX 0.500000 +ATC_RAT_RLL_P 0.135000 +ATC_RAT_RLL_SMAX 0.000000 +ATC_RAT_YAW_D 0.000000 +ATC_RAT_YAW_FF 0.000000 +ATC_RAT_YAW_FLTD 0.000000 +ATC_RAT_YAW_FLTE 2.500000 +ATC_RAT_YAW_FLTT 20.000000 +ATC_RAT_YAW_I 0.018000 +ATC_RAT_YAW_IMAX 0.500000 +ATC_RAT_YAW_P 0.180000 +ATC_RAT_YAW_SMAX 0.000000 +ATC_SLEW_YAW 6000.000000 +ATC_THR_MIX_MAN 0.100000 +ATC_THR_MIX_MAX 0.500000 +ATC_THR_MIX_MIN 0.100000 +AUTOTUNE_AGGR 0.100000 +AUTOTUNE_AXES 7 +AUTOTUNE_MIN_D 0.001000 +AUTO_OPTIONS 0 +AVD_ENABLE 0 +AVOID_ACCEL_MAX 3.000000 +AVOID_ALT_MIN 0.000000 +AVOID_ANGLE_MAX 1000 +AVOID_BACKUP_SPD 0.750000 +AVOID_BEHAVE 0 +AVOID_DIST_MAX 5.000000 +AVOID_ENABLE 3 +AVOID_MARGIN 2.000000 +BARO1_DEVID 65540 +BARO1_GND_PRESS 94502.796875 +BARO1_WCF_ENABLE 0 +BARO2_DEVID 65796 +BARO2_GND_PRESS 94503.234375 +BARO2_WCF_ENABLE 0 +BARO3_DEVID 0 +BARO3_GND_PRESS 0.000000 +BARO3_WCF_ENABLE 0 +BARO_ALT_OFFSET 0.000000 +BARO_EXT_BUS -1 +BARO_FLTR_RNG 0 +BARO_GND_TEMP 0.000000 +BARO_PRIMARY 0 +BARO_PROBE_EXT 0 +BATT2_MONITOR 0 +BATT3_MONITOR 0 +BATT4_MONITOR 0 +BATT5_MONITOR 0 +BATT6_MONITOR 0 +BATT7_MONITOR 0 +BATT8_MONITOR 0 +BATT9_MONITOR 0 +BATT_AMP_OFFSET 0.000000 +BATT_AMP_PERVLT 17.000000 +BATT_ARM_MAH 0 +BATT_ARM_VOLT 0.000000 +BATT_BUS 0 +BATT_CAPACITY 0 +BATT_CRT_MAH 0.000000 +BATT_CRT_VOLT 0.000000 +BATT_CURR_PIN 12 +BATT_FS_CRT_ACT 0 +BATT_FS_LOW_ACT 0 +BATT_FS_VOLTSRC 0 +BATT_LOW_MAH 0.000000 +BATT_LOW_TIMER 10 +BATT_LOW_VOLT 10.500000 +BATT_MONITOR 4 +BATT_OPTIONS 0 +BATT_SERIAL_NUM -1 +BATT_VOLT_MULT 10.100000 +BATT_VOLT_PIN 13 +BCN_ALT 0.000000 +BCN_LATITUDE 0.000000 +BCN_LONGITUDE 0.000000 +BCN_ORIENT_YAW 0 +BCN_TYPE 0 +BRD_ALT_CONFIG 0 +BRD_BOOT_DELAY 0 +BRD_OPTIONS 0 +BRD_PWM_COUNT 8 +BRD_RTC_TYPES 1 +BRD_RTC_TZ_MIN 0 +BRD_SAFETYOPTION 3 +BRD_SERIAL_NUM 0 +BRD_VBUS_MIN 4.300000 +BRD_VSERVO_MIN 0.000000 +BTN_ENABLE 0 +CAM_AUTO_ONLY 0 +CAM_DURATION 10 +CAM_FEEDBACK_PIN -1 +CAM_FEEDBACK_POL 1 +CAM_MAX_ROLL 0 +CAM_MIN_INTERVAL 0 +CAM_RC_TYPE 0 +CAM_RELAY_ON 1 +CAM_SERVO_OFF 1100 +CAM_SERVO_ON 1300 +CAM_TRIGG_DIST 0.000000 +CAM_TRIGG_TYPE 0 +CAM_TYPE 0 +CAN_D1_PROTOCOL 1 +CAN_D2_PROTOCOL 1 +CAN_LOGLEVEL 0 +CAN_P1_DRIVER 0 +CAN_P2_DRIVER 0 +CAN_SLCAN_CPORT 0 +CAN_SLCAN_SDELAY 1 +CAN_SLCAN_SERNUM -1 +CAN_SLCAN_TIMOUT 0 +CHUTE_ENABLED 0 +CIRCLE_OPTIONS 1 +CIRCLE_RADIUS 1000.000000 +CIRCLE_RATE 20.000000 +COMPASS_AUTODEC 1 +COMPASS_AUTO_ROT 2 +COMPASS_CAL_FIT 16.000000 +COMPASS_CUS_PIT 0.000000 +COMPASS_CUS_ROLL 0.000000 +COMPASS_CUS_YAW 0.000000 +COMPASS_DEC 0.000000 +COMPASS_DEV_ID 97539 +COMPASS_DEV_ID2 131874 +COMPASS_DEV_ID3 263178 +COMPASS_DEV_ID4 97283 +COMPASS_DEV_ID5 97795 +COMPASS_DEV_ID6 98051 +COMPASS_DEV_ID7 983044 +COMPASS_DEV_ID8 0 +COMPASS_DIA2_X 1.000000 +COMPASS_DIA2_Y 1.000000 +COMPASS_DIA2_Z 1.000000 +COMPASS_DIA3_X 1.000000 +COMPASS_DIA3_Y 1.000000 +COMPASS_DIA3_Z 1.000000 +COMPASS_DIA_X 1.000000 +COMPASS_DIA_Y 1.000000 +COMPASS_DIA_Z 1.000000 +COMPASS_ENABLE 1 +COMPASS_EXTERN2 0 +COMPASS_EXTERN3 0 +COMPASS_EXTERNAL 1 +COMPASS_FLTR_RNG 0 +COMPASS_LEARN 0 +COMPASS_MOT2_X 0.000000 +COMPASS_MOT2_Y 0.000000 +COMPASS_MOT2_Z 0.000000 +COMPASS_MOT3_X 0.000000 +COMPASS_MOT3_Y 0.000000 +COMPASS_MOT3_Z 0.000000 +COMPASS_MOTCT 0 +COMPASS_MOT_X 0.000000 +COMPASS_MOT_Y 0.000000 +COMPASS_MOT_Z 0.000000 +COMPASS_ODI2_X 0.000000 +COMPASS_ODI2_Y 0.000000 +COMPASS_ODI2_Z 0.000000 +COMPASS_ODI3_X 0.000000 +COMPASS_ODI3_Y 0.000000 +COMPASS_ODI3_Z 0.000000 +COMPASS_ODI_X 0.000000 +COMPASS_ODI_Y 0.000000 +COMPASS_ODI_Z 0.000000 +COMPASS_OFFS_MAX 1800 +COMPASS_OFS2_X 5.000000 +COMPASS_OFS2_Y 13.000000 +COMPASS_OFS2_Z -18.000000 +COMPASS_OFS3_X 5.000000 +COMPASS_OFS3_Y 13.000000 +COMPASS_OFS3_Z -18.000000 +COMPASS_OFS_X 5.000000 +COMPASS_OFS_Y 13.000000 +COMPASS_OFS_Z -18.000000 +COMPASS_OPTIONS 0 +COMPASS_ORIENT 0 +COMPASS_ORIENT2 0 +COMPASS_ORIENT3 0 +COMPASS_PMOT_EN 0 +COMPASS_PRIO1_ID 97539 +COMPASS_PRIO2_ID 131874 +COMPASS_PRIO3_ID 263178 +COMPASS_SCALE 1.000000 +COMPASS_SCALE2 1.000000 +COMPASS_SCALE3 1.000000 +COMPASS_TYPEMASK 0 +COMPASS_USE 1 +COMPASS_USE2 1 +COMPASS_USE3 1 +DEV_OPTIONS 0 +DISARM_DELAY 10 +EAHRS_TYPE 0 +EK2_ENABLE 0 +EK3_ABIAS_P_NSE 0.003000 +EK3_ACC_BIAS_LIM 1.000000 +EK3_ACC_P_NSE 0.350000 +EK3_AFFINITY 0 +EK3_ALT_M_NSE 2.000000 +EK3_BCN_DELAY 50 +EK3_BCN_I_GTE 500 +EK3_BCN_M_NSE 1.000000 +EK3_BETA_MASK 0 +EK3_CHECK_SCALE 100 +EK3_DRAG_BCOEF_X 0.000000 +EK3_DRAG_BCOEF_Y 0.000000 +EK3_DRAG_MCOEF 0.000000 +EK3_DRAG_M_NSE 0.500000 +EK3_EAS_I_GATE 400 +EK3_EAS_M_NSE 1.400000 +EK3_ENABLE 1 +EK3_ERR_THRESH 0.200000 +EK3_FLOW_DELAY 10 +EK3_FLOW_I_GATE 300 +EK3_FLOW_M_NSE 0.250000 +EK3_FLOW_USE 1 +EK3_GBIAS_P_NSE 0.001000 +EK3_GLITCH_RAD 25 +EK3_GPS_CHECK 31 +EK3_GSF_DELAY 1000 +EK3_GSF_RST_MAX 2 +EK3_GSF_RUN_MASK 3 +EK3_GSF_USE_MASK 3 +EK3_GYRO_P_NSE 0.015000 +EK3_HGT_DELAY 60 +EK3_HGT_I_GATE 500 +EK3_HRT_FILT 2.000000 +EK3_IMU_MASK 3 +EK3_MAGB_P_NSE 0.000100 +EK3_MAGE_P_NSE 0.001000 +EK3_MAG_CAL 3 +EK3_MAG_EF_LIM 50 +EK3_MAG_I_GATE 300 +EK3_MAG_MASK 0 +EK3_MAG_M_NSE 0.050000 +EK3_MAX_FLOW 2.500000 +EK3_NOAID_M_NSE 10.000000 +EK3_OGNM_TEST_SF 2.000000 +EK3_OGN_HGT_MASK 0 +EK3_POSNE_M_NSE 0.500000 +EK3_POS_I_GATE 500 +EK3_RNG_I_GATE 500 +EK3_RNG_M_NSE 0.500000 +EK3_RNG_USE_HGT -1 +EK3_RNG_USE_SPD 2.000000 +EK3_SRC1_POSXY 3 +EK3_SRC1_POSZ 1 +EK3_SRC1_VELXY 3 +EK3_SRC1_VELZ 3 +EK3_SRC1_YAW 1 +EK3_SRC2_POSXY 0 +EK3_SRC2_POSZ 1 +EK3_SRC2_VELXY 0 +EK3_SRC2_VELZ 0 +EK3_SRC2_YAW 0 +EK3_SRC3_POSXY 0 +EK3_SRC3_POSZ 1 +EK3_SRC3_VELXY 0 +EK3_SRC3_VELZ 0 +EK3_SRC3_YAW 0 +EK3_SRC_OPTIONS 1 +EK3_TAU_OUTPUT 25 +EK3_TERR_GRAD 0.100000 +EK3_VELD_M_NSE 0.500000 +EK3_VELNE_M_NSE 0.300000 +EK3_VEL_I_GATE 500 +EK3_VIS_VERR_MAX 0.900000 +EK3_VIS_VERR_MIN 0.100000 +EK3_WENC_VERR 0.100000 +EK3_WIND_PSCALE 0.500000 +EK3_WIND_P_NSE 0.200000 +EK3_YAW_I_GATE 300 +EK3_YAW_M_NSE 0.500000 +ESC_CALIBRATION 0 +FENCE_ACTION 1 +FENCE_ALT_MAX 100.000000 +FENCE_ALT_MIN -10.000000 +FENCE_ENABLE 0 +FENCE_MARGIN 2.000000 +FENCE_RADIUS 150.000000 +FENCE_TOTAL 0 +FENCE_TYPE 7 +FFT_ENABLE 0 +FHLD_BRAKE_RATE 8 +FHLD_FILT_HZ 5.000000 +FHLD_FLOW_MAX 0.600000 +FHLD_QUAL_MIN 10 +FHLD_XY_FILT_HZ 5.000000 +FHLD_XY_I 0.300000 +FHLD_XY_IMAX 3000.000000 +FHLD_XY_P 0.200000 +FLIGHT_OPTIONS 0 +FLOW_ADDR 0 +FLOW_FXSCALER 0 +FLOW_FYSCALER 0 +FLOW_ORIENT_YAW 0 +FLOW_POS_X 0.000000 +FLOW_POS_Y 0.000000 +FLOW_POS_Z 0.000000 +FLOW_TYPE 0 +FLTMODE1 7 +FLTMODE2 9 +FLTMODE3 6 +FLTMODE4 3 +FLTMODE5 5 +FLTMODE6 0 +FLTMODE_CH 5 +FOLL_ENABLE 0 +FORMAT_VERSION 120 +FRAME_CLASS 1 +FRAME_TYPE 0 +FRSKY_DNLINK1_ID 20 +FRSKY_DNLINK2_ID 7 +FRSKY_UPLINK_ID 13 +FS_CRASH_CHECK 1 +FS_EKF_ACTION 1 +FS_EKF_THRESH 0.800000 +FS_GCS_ENABLE 0 +FS_GCS_TIMEOUT 5.000000 +FS_OPTIONS 16 +FS_THR_ENABLE 1 +FS_THR_VALUE 975 +FS_VIBE_ENABLE 1 +GCS_PID_MASK 0 +GEN_TYPE 0 +GND_EFFECT_COMP 1 +GPS1_CAN_OVRIDE 0 +GPS2_CAN_OVRIDE 0 +GPS_AUTO_CONFIG 1 +GPS_AUTO_SWITCH 1 +GPS_BLEND_MASK 5 +GPS_BLEND_TC 10.000000 +GPS_CAN_NODEID1 0 +GPS_CAN_NODEID2 0 +GPS_COM_PORT 1 +GPS_COM_PORT2 1 +GPS_DELAY_MS 0 +GPS_DELAY_MS2 0 +GPS_DRV_OPTIONS 0 +GPS_GNSS_MODE 0 +GPS_GNSS_MODE2 0 +GPS_HDOP_GOOD 140 +GPS_INJECT_TO 127 +GPS_MB1_TYPE 0 +GPS_MB2_TYPE 0 +GPS_MIN_DGPS 100 +GPS_MIN_ELEV -100 +GPS_NAVFILTER 8 +GPS_POS1_X 0.000000 +GPS_POS1_Y 0.000000 +GPS_POS1_Z 0.000000 +GPS_POS2_X 0.000000 +GPS_POS2_Y 0.000000 +GPS_POS2_Z 0.000000 +GPS_PRIMARY 0 +GPS_RATE_MS 200 +GPS_RATE_MS2 200 +GPS_RAW_DATA 0 +GPS_SAVE_CFG 2 +GPS_SBAS_MODE 2 +GPS_SBP_LOGMASK -256 +GPS_TYPE 1 +GPS_TYPE2 0 +GRIP_ENABLE 0 +GUID_OPTIONS 0 +INITIAL_MODE 0 +INS_ACC1_CALTEMP -300.000000 +INS_ACC2OFFS_X 0.001000 +INS_ACC2OFFS_Y 0.001000 +INS_ACC2OFFS_Z 0.001000 +INS_ACC2SCAL_X 1.001000 +INS_ACC2SCAL_Y 1.001000 +INS_ACC2SCAL_Z 1.001000 +INS_ACC2_CALTEMP -300.000000 +INS_ACC2_ID 2753036 +INS_ACC3OFFS_X 0.000000 +INS_ACC3OFFS_Y 0.000000 +INS_ACC3OFFS_Z 0.000000 +INS_ACC3SCAL_X 1.000000 +INS_ACC3SCAL_Y 1.000000 +INS_ACC3SCAL_Z 1.000000 +INS_ACC3_CALTEMP -300.000000 +INS_ACC3_ID 0 +INS_ACCEL_FILTER 20 +INS_ACCOFFS_X 0.001000 +INS_ACCOFFS_Y 0.001000 +INS_ACCOFFS_Z 0.001000 +INS_ACCSCAL_X 1.001000 +INS_ACCSCAL_Y 1.001000 +INS_ACCSCAL_Z 1.001000 +INS_ACC_BODYFIX 2 +INS_ACC_ID 2753028 +INS_ENABLE_MASK 127 +INS_FAST_SAMPLE 1 +INS_GYR1_CALTEMP -300.000000 +INS_GYR2OFFS_X 0.000000 +INS_GYR2OFFS_Y 0.000000 +INS_GYR2OFFS_Z 0.000000 +INS_GYR2_CALTEMP -300.000000 +INS_GYR2_ID 2752780 +INS_GYR3OFFS_X 0.000000 +INS_GYR3OFFS_Y 0.000000 +INS_GYR3OFFS_Z 0.000000 +INS_GYR3_CALTEMP -300.000000 +INS_GYR3_ID 0 +INS_GYROFFS_X 0.000000 +INS_GYROFFS_Y 0.000000 +INS_GYROFFS_Z 0.000000 +INS_GYRO_FILTER 20 +INS_GYRO_RATE 0 +INS_GYR_CAL 1 +INS_GYR_ID 2752772 +INS_HNTCH_ENABLE 0 +INS_LOG_BAT_CNT 1024 +INS_LOG_BAT_LGCT 32 +INS_LOG_BAT_LGIN 20 +INS_LOG_BAT_MASK 0 +INS_LOG_BAT_OPT 0 +INS_NOTCH_ENABLE 0 +INS_POS1_X 0.000000 +INS_POS1_Y 0.000000 +INS_POS1_Z 0.000000 +INS_POS2_X 0.000000 +INS_POS2_Y 0.000000 +INS_POS2_Z 0.000000 +INS_POS3_X 0.000000 +INS_POS3_Y 0.000000 +INS_POS3_Z 0.000000 +INS_STILL_THRESH 2.500000 +INS_TCAL1_ENABLE 0 +INS_TCAL2_ENABLE 0 +INS_TCAL3_ENABLE 0 +INS_TCAL_OPTIONS 0 +INS_TRIM_OPTION 1 +INS_USE 1 +INS_USE2 1 +INS_USE3 1 +LAND_ALT_LOW 1000 +LAND_REPOSITION 1 +LAND_SPEED 50 +LAND_SPEED_HIGH 0 +LGR_DEPLOY_ALT 0 +LGR_DEPLOY_PIN -1 +LGR_DEPLOY_POL 0 +LGR_OPTIONS 3 +LGR_RETRACT_ALT 0 +LGR_STARTUP 0 +LGR_WOW_PIN 8 +LGR_WOW_POL 1 +LOG_BACKEND_TYPE 1 +LOG_BITMASK 176126 +LOG_DISARMED 0 +LOG_FILE_BUFSIZE 200 +LOG_FILE_DSRMROT 0 +LOG_FILE_MB_FREE 500 +LOG_FILE_TIMEOUT 5 +LOG_MAV_BUFSIZE 8 +LOG_REPLAY 0 +LOIT_ACC_MAX 500.000000 +LOIT_ANG_MAX 0.000000 +LOIT_BRK_ACCEL 250.000000 +LOIT_BRK_DELAY 1.000000 +LOIT_BRK_JERK 500.000000 +LOIT_SPEED 1250.000000 +MIS_OPTIONS 0 +MIS_RESTART 0 +MIS_TOTAL 0 +MNT_TYPE 0 +MOT_BAT_CURR_MAX 0.000000 +MOT_BAT_CURR_TC 5.000000 +MOT_BAT_IDX 0 +MOT_BAT_VOLT_MAX 12.800000 +MOT_BAT_VOLT_MIN 9.600000 +MOT_BOOST_SCALE 0.000000 +MOT_HOVER_LEARN 2 +MOT_PWM_MAX 2000 +MOT_PWM_MIN 1000 +MOT_PWM_TYPE 0 +MOT_SAFE_DISARM 0 +MOT_SAFE_TIME 1.000000 +MOT_SLEW_DN_TIME 0.000000 +MOT_SLEW_UP_TIME 0.000000 +MOT_SPIN_ARM 0.100000 +MOT_SPIN_MAX 0.950000 +MOT_SPIN_MIN 0.150000 +MOT_SPOOL_TIME 0.500000 +MOT_THST_EXPO 0.650000 +MOT_THST_HOVER 0.390000 +MOT_YAW_HEADROOM 200 +MSP_OPTIONS 0 +MSP_OSD_NCELLS 0 +NTF_BUZZ_ENABLE 1 +NTF_BUZZ_ON_LVL 1 +NTF_BUZZ_PIN 0 +NTF_BUZZ_VOLUME 100 +NTF_DISPLAY_TYPE 0 +NTF_LED_BRIGHT 3 +NTF_LED_LEN 1 +NTF_LED_OVERRIDE 0 +NTF_LED_TYPES 199 +NTF_OREO_THEME 0 +OA_TYPE 0 +OSD_TYPE 0 +PHLD_BRAKE_ANGLE 3000 +PHLD_BRAKE_RATE 8 +PILOT_ACCEL_Z 250 +PILOT_SPEED_DN 0 +PILOT_SPEED_UP 250 +PILOT_THR_BHV 0 +PILOT_THR_FILT 0.000000 +PILOT_TKOFF_ALT 0.000000 +PLND_ENABLED 0 +PRX_FILT 0.250000 +PRX_IGN_ANG1 0 +PRX_IGN_ANG2 0 +PRX_IGN_ANG3 0 +PRX_IGN_ANG4 0 +PRX_IGN_ANG5 0 +PRX_IGN_ANG6 0 +PRX_IGN_GND 0 +PRX_IGN_WID1 0 +PRX_IGN_WID2 0 +PRX_IGN_WID3 0 +PRX_IGN_WID4 0 +PRX_IGN_WID5 0 +PRX_IGN_WID6 0 +PRX_LOG_RAW 0 +PRX_ORIENT 0 +PRX_TYPE 0 +PRX_YAW_CORR 0 +PSC_ACCZ_D 0.000000 +PSC_ACCZ_FF 0.000000 +PSC_ACCZ_FLTD 0.000000 +PSC_ACCZ_FLTE 20.000000 +PSC_ACCZ_FLTT 0.000000 +PSC_ACCZ_I 1.000000 +PSC_ACCZ_IMAX 800.000000 +PSC_ACCZ_P 0.500000 +PSC_ACCZ_SMAX 0.000000 +PSC_ACC_XY_FILT 2.000000 +PSC_ANGLE_MAX 0.000000 +PSC_POSXY_P 1.000000 +PSC_POSZ_P 1.000000 +PSC_VELXY_D 0.500000 +PSC_VELXY_D_FILT 5.000000 +PSC_VELXY_FF 0.000000 +PSC_VELXY_FILT 5.000000 +PSC_VELXY_I 1.000000 +PSC_VELXY_IMAX 1000.000000 +PSC_VELXY_P 2.000000 +PSC_VELZ_D 0.000000 +PSC_VELZ_FF 0.000000 +PSC_VELZ_FLTD 5.000000 +PSC_VELZ_FLTE 5.000000 +PSC_VELZ_I 0.000000 +PSC_VELZ_IMAX 1000.000000 +PSC_VELZ_P 5.000000 +RALLY_INCL_HOME 1 +RALLY_LIMIT_KM 0.300000 +RALLY_TOTAL 0 +RC10_DZ 0 +RC10_MAX 1900 +RC10_MIN 1100 +RC10_OPTION 0 +RC10_REVERSED 0 +RC10_TRIM 1500 +RC11_DZ 0 +RC11_MAX 1900 +RC11_MIN 1100 +RC11_OPTION 0 +RC11_REVERSED 0 +RC11_TRIM 1500 +RC12_DZ 0 +RC12_MAX 1900 +RC12_MIN 1100 +RC12_OPTION 0 +RC12_REVERSED 0 +RC12_TRIM 1500 +RC13_DZ 0 +RC13_MAX 1900 +RC13_MIN 1100 +RC13_OPTION 0 +RC13_REVERSED 0 +RC13_TRIM 1500 +RC14_DZ 0 +RC14_MAX 1900 +RC14_MIN 1100 +RC14_OPTION 0 +RC14_REVERSED 0 +RC14_TRIM 1500 +RC15_DZ 0 +RC15_MAX 1900 +RC15_MIN 1100 +RC15_OPTION 0 +RC15_REVERSED 0 +RC15_TRIM 1500 +RC16_DZ 0 +RC16_MAX 1900 +RC16_MIN 1100 +RC16_OPTION 0 +RC16_REVERSED 0 +RC16_TRIM 1500 +RC1_DZ 20 +RC1_MAX 2000 +RC1_MIN 1000 +RC1_OPTION 0 +RC1_REVERSED 0 +RC1_TRIM 1500 +RC2_DZ 20 +RC2_MAX 2000 +RC2_MIN 1000 +RC2_OPTION 0 +RC2_REVERSED 0 +RC2_TRIM 1500 +RC3_DZ 30 +RC3_MAX 2000 +RC3_MIN 1000 +RC3_OPTION 0 +RC3_REVERSED 0 +RC3_TRIM 1500 +RC4_DZ 20 +RC4_MAX 2000 +RC4_MIN 1000 +RC4_OPTION 0 +RC4_REVERSED 0 +RC4_TRIM 1500 +RC5_DZ 0 +RC5_MAX 2000 +RC5_MIN 1000 +RC5_OPTION 0 +RC5_REVERSED 0 +RC5_TRIM 1500 +RC6_DZ 0 +RC6_MAX 2000 +RC6_MIN 1000 +RC6_OPTION 0 +RC6_REVERSED 0 +RC6_TRIM 1500 +RC7_DZ 0 +RC7_MAX 2000 +RC7_MIN 1000 +RC7_OPTION 7 +RC7_REVERSED 0 +RC7_TRIM 1500 +RC8_DZ 0 +RC8_MAX 2000 +RC8_MIN 1000 +RC8_OPTION 0 +RC8_REVERSED 0 +RC8_TRIM 1500 +RC9_DZ 0 +RC9_MAX 1900 +RC9_MIN 1100 +RC9_OPTION 0 +RC9_REVERSED 0 +RC9_TRIM 1500 +RCMAP_PITCH 2 +RCMAP_ROLL 1 +RCMAP_THROTTLE 3 +RCMAP_YAW 4 +RC_OPTIONS 32 +RC_OVERRIDE_TIME 3.000000 +RC_PROTOCOLS 1 +RC_SPEED 490 +RELAY_DEFAULT 0 +RELAY_PIN 13 +RELAY_PIN2 -1 +RELAY_PIN3 -1 +RELAY_PIN4 -1 +RELAY_PIN5 -1 +RELAY_PIN6 -1 +RNGFND1_TYPE 0 +RNGFND2_TYPE 0 +RNGFND3_TYPE 0 +RNGFND4_TYPE 0 +RNGFND5_TYPE 0 +RNGFND6_TYPE 0 +RNGFND7_TYPE 0 +RNGFND8_TYPE 0 +RNGFND9_TYPE 0 +RNGFNDA_TYPE 0 +RNGFND_GAIN 0.800000 +RPM2_PIN -1 +RPM2_SCALING 1.000000 +RPM2_TYPE 0 +RPM_MAX 100000.000000 +RPM_MIN 10.000000 +RPM_MIN_QUAL 0.500000 +RPM_PIN 54 +RPM_SCALING 1.000000 +RPM_TYPE 0 +RSSI_TYPE 0 +RTL_ALT 1500 +RTL_ALT_FINAL 0 +RTL_ALT_TYPE 0 +RTL_CLIMB_MIN 0 +RTL_CONE_SLOPE 3.000000 +RTL_LOIT_TIME 5000 +RTL_OPTIONS 0 +RTL_SPEED 0 +SCHED_DEBUG 0 +SCHED_LOOP_RATE 400 +SCHED_OPTIONS 0 +SCR_ENABLE 0 +SERIAL0_BAUD 115 +SERIAL0_PROTOCOL 2 +SERIAL1_BAUD 57 +SERIAL1_OPTIONS 0 +SERIAL1_PROTOCOL 2 +SERIAL2_BAUD 57 +SERIAL2_OPTIONS 0 +SERIAL2_PROTOCOL 2 +SERIAL3_BAUD 38 +SERIAL3_OPTIONS 0 +SERIAL3_PROTOCOL 5 +SERIAL4_BAUD 38 +SERIAL4_OPTIONS 0 +SERIAL4_PROTOCOL 5 +SERIAL5_BAUD 57 +SERIAL5_OPTIONS 0 +SERIAL5_PROTOCOL -1 +SERIAL6_BAUD 57 +SERIAL6_OPTIONS 0 +SERIAL6_PROTOCOL -1 +SERIAL7_BAUD 57 +SERIAL7_OPTIONS 0 +SERIAL7_PROTOCOL -1 +SERIAL_PASS1 0 +SERIAL_PASS2 -1 +SERIAL_PASSTIMO 15 +SERVO10_FUNCTION 0 +SERVO10_MAX 1900 +SERVO10_MIN 1100 +SERVO10_REVERSED 0 +SERVO10_TRIM 1500 +SERVO11_FUNCTION 0 +SERVO11_MAX 1900 +SERVO11_MIN 1100 +SERVO11_REVERSED 0 +SERVO11_TRIM 1500 +SERVO12_FUNCTION 0 +SERVO12_MAX 1900 +SERVO12_MIN 1100 +SERVO12_REVERSED 0 +SERVO12_TRIM 1500 +SERVO13_FUNCTION 0 +SERVO13_MAX 1900 +SERVO13_MIN 1100 +SERVO13_REVERSED 0 +SERVO13_TRIM 1500 +SERVO14_FUNCTION 0 +SERVO14_MAX 1900 +SERVO14_MIN 1100 +SERVO14_REVERSED 0 +SERVO14_TRIM 1500 +SERVO15_FUNCTION 0 +SERVO15_MAX 1900 +SERVO15_MIN 1100 +SERVO15_REVERSED 0 +SERVO15_TRIM 1500 +SERVO16_FUNCTION 0 +SERVO16_MAX 1900 +SERVO16_MIN 1100 +SERVO16_REVERSED 0 +SERVO16_TRIM 1500 +SERVO1_FUNCTION 33 +SERVO1_MAX 1900 +SERVO1_MIN 1100 +SERVO1_REVERSED 0 +SERVO1_TRIM 1500 +SERVO2_FUNCTION 34 +SERVO2_MAX 1900 +SERVO2_MIN 1100 +SERVO2_REVERSED 0 +SERVO2_TRIM 1500 +SERVO3_FUNCTION 35 +SERVO3_MAX 1900 +SERVO3_MIN 1100 +SERVO3_REVERSED 0 +SERVO3_TRIM 1500 +SERVO4_FUNCTION 36 +SERVO4_MAX 1900 +SERVO4_MIN 1100 +SERVO4_REVERSED 0 +SERVO4_TRIM 1500 +SERVO5_FUNCTION 0 +SERVO5_MAX 1900 +SERVO5_MIN 1100 +SERVO5_REVERSED 0 +SERVO5_TRIM 1500 +SERVO6_FUNCTION 0 +SERVO6_MAX 1900 +SERVO6_MIN 1100 +SERVO6_REVERSED 0 +SERVO6_TRIM 1500 +SERVO7_FUNCTION 0 +SERVO7_MAX 1900 +SERVO7_MIN 1100 +SERVO7_REVERSED 0 +SERVO7_TRIM 1500 +SERVO8_FUNCTION 0 +SERVO8_MAX 1900 +SERVO8_MIN 1100 +SERVO8_REVERSED 0 +SERVO8_TRIM 1500 +SERVO9_FUNCTION 0 +SERVO9_MAX 1900 +SERVO9_MIN 1100 +SERVO9_REVERSED 0 +SERVO9_TRIM 1500 +SERVO_DSHOT_RATE 0 +SERVO_RATE 50 +SERVO_ROB_POSMAX 4095 +SERVO_ROB_POSMIN 0 +SERVO_SBUS_RATE 50 +SERVO_VOLZ_MASK 0 +SID_AXIS 0 +SIMPLE 0 +SIM_ACC1_BIAS_X 0.000000 +SIM_ACC1_BIAS_Y 0.000000 +SIM_ACC1_BIAS_Z 0.000000 +SIM_ACC1_RND 0.000000 +SIM_ACC1_SCAL_X 0.000000 +SIM_ACC1_SCAL_Y 0.000000 +SIM_ACC1_SCAL_Z 0.000000 +SIM_ACC2_BIAS_X 0.000000 +SIM_ACC2_BIAS_Y 0.000000 +SIM_ACC2_BIAS_Z 0.000000 +SIM_ACC2_RND 0.000000 +SIM_ACC2_SCAL_X 0.000000 +SIM_ACC2_SCAL_Y 0.000000 +SIM_ACC2_SCAL_Z 0.000000 +SIM_ACC3_BIAS_X 0.000000 +SIM_ACC3_BIAS_Y 0.000000 +SIM_ACC3_BIAS_Z 0.000000 +SIM_ACC3_RND 0.000000 +SIM_ACC3_SCAL_X 0.000000 +SIM_ACC3_SCAL_Y 0.000000 +SIM_ACC3_SCAL_Z 0.000000 +SIM_ACCEL1_FAIL 0.000000 +SIM_ACCEL2_FAIL 0.000000 +SIM_ACCEL3_FAIL 0.000000 +SIM_ACC_FAIL_MSK 0 +SIM_ACC_TRIM_X 0.000000 +SIM_ACC_TRIM_Y 0.000000 +SIM_ACC_TRIM_Z 0.000000 +SIM_ADSB_ALT 1000.000000 +SIM_ADSB_COUNT -1 +SIM_ADSB_RADIUS 10000.000000 +SIM_ADSB_TX 0 +SIM_ARSPD2_FAIL 0.000000 +SIM_ARSPD2_FAILP 0.000000 +SIM_ARSPD2_OFS 2013.000000 +SIM_ARSPD2_PITOT 0.000000 +SIM_ARSPD2_RND 2.000000 +SIM_ARSPD_FAIL 0.000000 +SIM_ARSPD_FAILP 0.000000 +SIM_ARSPD_OFS 2013.000000 +SIM_ARSPD_PITOT 0.000000 +SIM_ARSPD_RND 2.000000 +SIM_ARSPD_SIGN 0 +SIM_BAR2_DELAY 0 +SIM_BAR2_DISABLE 0 +SIM_BAR2_DRIFT 0.000000 +SIM_BAR2_FREEZE 0 +SIM_BAR2_GLITCH 0.000000 +SIM_BAR2_RND 0.200000 +SIM_BAR2_WCF_BAK 0.000000 +SIM_BAR2_WCF_FWD 0.000000 +SIM_BAR2_WCF_LFT 0.000000 +SIM_BAR2_WCF_RGT 0.000000 +SIM_BAR3_DELAY 0 +SIM_BAR3_DISABLE 0 +SIM_BAR3_DRIFT 0.000000 +SIM_BAR3_FREEZE 0 +SIM_BAR3_GLITCH 0.000000 +SIM_BAR3_RND 0.200000 +SIM_BAR3_WCF_BAK 0.000000 +SIM_BAR3_WCF_FWD 0.000000 +SIM_BAR3_WCF_LFT 0.000000 +SIM_BAR3_WCF_RGT 0.000000 +SIM_BARO_COUNT 2 +SIM_BARO_DELAY 0 +SIM_BARO_DISABLE 0 +SIM_BARO_DRIFT 0.000000 +SIM_BARO_FREEZE 0 +SIM_BARO_GLITCH 0.000000 +SIM_BARO_RND 0.000000 +SIM_BARO_WCF_BAK 0.000000 +SIM_BARO_WCF_FWD 0.000000 +SIM_BARO_WCF_LFT 0.000000 +SIM_BARO_WCF_RGT 0.000000 +SIM_BATT_CAP_AH 0.000000 +SIM_BATT_VOLTAGE 12.600000 +SIM_BAUDLIMIT_EN 0 +SIM_BZ_ENABLE 0 +SIM_BZ_PIN 0 +SIM_DRIFT_SPEED 0.050000 +SIM_DRIFT_TIME 5.000000 +SIM_EFI_TYPE 0 +SIM_ENGINE_FAIL 0 +SIM_ENGINE_MUL 1.000000 +SIM_FLOAT_EXCEPT 1 +SIM_FLOW_DELAY 0 +SIM_FLOW_ENABLE 0 +SIM_FLOW_POS_X 0.000000 +SIM_FLOW_POS_Y 0.000000 +SIM_FLOW_POS_Z 0.000000 +SIM_FLOW_RATE 10 +SIM_FLOW_RND 0.050000 +SIM_GND_BEHAV -1 +SIM_GPS2_ACC 0.300000 +SIM_GPS2_ALT_OFS 0 +SIM_GPS2_BYTELOS 0.000000 +SIM_GPS2_DELAY 1 +SIM_GPS2_DISABLE 1 +SIM_GPS2_DRFTALT 0.000000 +SIM_GPS2_GLTCH_X 0.000000 +SIM_GPS2_GLTCH_Y 0.000000 +SIM_GPS2_GLTCH_Z 0.000000 +SIM_GPS2_HDG 0 +SIM_GPS2_HZ 5 +SIM_GPS2_LCKTIME 0 +SIM_GPS2_NOISE 0.000000 +SIM_GPS2_NUMSATS 10 +SIM_GPS2_POS_X 0.000000 +SIM_GPS2_POS_Y 0.000000 +SIM_GPS2_POS_Z 0.000000 +SIM_GPS2_TYPE 1 +SIM_GPS2_VERR_X 0.000000 +SIM_GPS2_VERR_Y 0.000000 +SIM_GPS2_VERR_Z 0.000000 +SIM_GPS_ACC 0.300000 +SIM_GPS_ALT_OFS 0 +SIM_GPS_BYTELOSS 0.000000 +SIM_GPS_DELAY 1 +SIM_GPS_DISABLE 0 +SIM_GPS_DRIFTALT 0.000000 +SIM_GPS_GLITCH_X 0.000000 +SIM_GPS_GLITCH_Y 0.000000 +SIM_GPS_GLITCH_Z 0.000000 +SIM_GPS_HDG 0 +SIM_GPS_HZ 5 +SIM_GPS_LOCKTIME 0 +SIM_GPS_NOISE 0.000000 +SIM_GPS_NUMSATS 10 +SIM_GPS_POS_X 0.000000 +SIM_GPS_POS_Y 0.000000 +SIM_GPS_POS_Z 0.000000 +SIM_GPS_TYPE 1 +SIM_GPS_VERR_X 0.000000 +SIM_GPS_VERR_Y 0.000000 +SIM_GPS_VERR_Z 0.000000 +SIM_GRPE_ENABLE 0 +SIM_GRPE_PIN -1 +SIM_GRPS_ENABLE 0 +SIM_GRPS_GRAB 2000 +SIM_GRPS_PIN -1 +SIM_GRPS_RELEASE 1000 +SIM_GRPS_REVERSE 0 +SIM_GYR1_RND 0.000000 +SIM_GYR1_SCALE_X 0.000000 +SIM_GYR1_SCALE_Y 0.000000 +SIM_GYR1_SCALE_Z 0.000000 +SIM_GYR2_RND 0.000000 +SIM_GYR2_SCALE_X 0.000000 +SIM_GYR2_SCALE_Y 0.000000 +SIM_GYR2_SCALE_Z 0.000000 +SIM_GYR3_RND 0.000000 +SIM_GYR3_SCALE_X 0.000000 +SIM_GYR3_SCALE_Y 0.000000 +SIM_GYR3_SCALE_Z 0.000000 +SIM_GYR_FAIL_MSK 0 +SIM_IE24_ENABLE 0 +SIM_IE24_ERROR 0 +SIM_IE24_STATE -1 +SIM_IMUT1_ENABLE 0 +SIM_IMUT2_ENABLE 0 +SIM_IMUT3_ENABLE 0 +SIM_IMUT_END 45.000000 +SIM_IMUT_FIXED 0.000000 +SIM_IMUT_START 25.000000 +SIM_IMUT_TCONST 300.000000 +SIM_IMU_COUNT 2 +SIM_IMU_POS_X 0.000000 +SIM_IMU_POS_Y 0.000000 +SIM_IMU_POS_Z 0.000000 +SIM_INS_THR_MIN 0.100000 +SIM_LED_LAYOUT 0 +SIM_LOOP_DELAY 0 +SIM_MAG1_DEVID 97539 +SIM_MAG1_FAIL 0 +SIM_MAG1_SCALING 1.000000 +SIM_MAG2_DEVID 131874 +SIM_MAG2_DIA_X 0.000000 +SIM_MAG2_DIA_Y 0.000000 +SIM_MAG2_DIA_Z 0.000000 +SIM_MAG2_FAIL 0 +SIM_MAG2_ODI_X 0.000000 +SIM_MAG2_ODI_Y 0.000000 +SIM_MAG2_ODI_Z 0.000000 +SIM_MAG2_OFS_X 5.000000 +SIM_MAG2_OFS_Y 13.000000 +SIM_MAG2_OFS_Z -18.000000 +SIM_MAG2_ORIENT 0 +SIM_MAG2_SCALING 1.000000 +SIM_MAG3_DEVID 263178 +SIM_MAG3_DIA_X 0.000000 +SIM_MAG3_DIA_Y 0.000000 +SIM_MAG3_DIA_Z 0.000000 +SIM_MAG3_FAIL 0 +SIM_MAG3_ODI_X 0.000000 +SIM_MAG3_ODI_Y 0.000000 +SIM_MAG3_ODI_Z 0.000000 +SIM_MAG3_OFS_X 5.000000 +SIM_MAG3_OFS_Y 13.000000 +SIM_MAG3_OFS_Z -18.000000 +SIM_MAG3_ORIENT 0 +SIM_MAG3_SCALING 1.000000 +SIM_MAG4_DEVID 97283 +SIM_MAG5_DEVID 97795 +SIM_MAG6_DEVID 98051 +SIM_MAG7_DEVID 0 +SIM_MAG8_DEVID 0 +SIM_MAG_ALY_HGT 1.000000 +SIM_MAG_ALY_X 0.000000 +SIM_MAG_ALY_Y 0.000000 +SIM_MAG_ALY_Z 0.000000 +SIM_MAG_DELAY 0 +SIM_MAG_DIA_X 0.000000 +SIM_MAG_DIA_Y 0.000000 +SIM_MAG_DIA_Z 0.000000 +SIM_MAG_MOT_X 0.000000 +SIM_MAG_MOT_Y 0.000000 +SIM_MAG_MOT_Z 0.000000 +SIM_MAG_ODI_X 0.000000 +SIM_MAG_ODI_Y 0.000000 +SIM_MAG_ODI_Z 0.000000 +SIM_MAG_OFS_X 5.000000 +SIM_MAG_OFS_Y 13.000000 +SIM_MAG_OFS_Z -18.000000 +SIM_MAG_ORIENT 0 +SIM_MAG_RND 0.000000 +SIM_ODOM_ENABLE 0 +SIM_OPOS_ALT 584.000000 +SIM_OPOS_HDG 353.000000 +SIM_OPOS_LAT -35.363262 +SIM_OPOS_LNG 149.165237 +SIM_PARA_ENABLE 0 +SIM_PARA_PIN -1 +SIM_PIN_MASK 0 +SIM_PLD_ALT_LMT 15.000000 +SIM_PLD_DIST_LMT 10.000000 +SIM_PLD_ENABLE 0 +SIM_PLD_HEIGHT 0.000000 +SIM_PLD_LAT 0.000000 +SIM_PLD_LON 0.000000 +SIM_PLD_RATE 100 +SIM_PLD_TYPE 0 +SIM_PLD_YAW 0 +SIM_RATE_HZ 1200 +SIM_RC_CHANCOUNT 16 +SIM_RC_FAIL 0 +SIM_RICH_CTRL -1 +SIM_RICH_ENABLE 0 +SIM_SAFETY_STATE 0 +SIM_SAIL_TYPE 0 +SIM_SERVO_SPEED 0.140000 +SIM_SHIP_DSIZE 10.000000 +SIM_SHIP_ENABLE 0 +SIM_SHIP_PSIZE 1000.000000 +SIM_SHIP_SPEED 3.000000 +SIM_SHIP_SYSID 17 +SIM_SHOVE_TIME 0 +SIM_SHOVE_X 0.000000 +SIM_SHOVE_Y 0.000000 +SIM_SHOVE_Z 0.000000 +SIM_SONAR_GLITCH 0.000000 +SIM_SONAR_POS_X 0.000000 +SIM_SONAR_POS_Y 0.000000 +SIM_SONAR_POS_Z 0.000000 +SIM_SONAR_RND 0.000000 +SIM_SONAR_SCALE 12.121200 +SIM_SPEEDUP 1.000000 +SIM_SPR_ENABLE 0 +SIM_SPR_PUMP -1 +SIM_SPR_SPIN -1 +SIM_TA_ENABLE 1 +SIM_TEMP_BFACTOR 0.000000 +SIM_TEMP_FLIGHT 35.000000 +SIM_TEMP_START 25.000000 +SIM_TEMP_TCONST 30.000000 +SIM_TERRAIN 1 +SIM_THML_SCENARI 0 +SIM_TIDE_DIR 0.000000 +SIM_TIDE_SPEED 0.000000 +SIM_TWIST_TIME 0 +SIM_TWIST_X 0.000000 +SIM_TWIST_Y 0.000000 +SIM_TWIST_Z 0.000000 +SIM_VIB_FREQ_X 0.000000 +SIM_VIB_FREQ_Y 0.000000 +SIM_VIB_FREQ_Z 0.000000 +SIM_VIB_MOT_MAX 0.000000 +SIM_VIB_MOT_MULT 1.000000 +SIM_VICON_FAIL 0 +SIM_VICON_GLIT_X 0.000000 +SIM_VICON_GLIT_Y 0.000000 +SIM_VICON_GLIT_Z 0.000000 +SIM_VICON_POS_X 0.000000 +SIM_VICON_POS_Y 0.000000 +SIM_VICON_POS_Z 0.000000 +SIM_VICON_TMASK 3 +SIM_VICON_VGLI_X 0.000000 +SIM_VICON_VGLI_Y 0.000000 +SIM_VICON_VGLI_Z 0.000000 +SIM_VICON_YAW 0 +SIM_VICON_YAWERR 0 +SIM_WAVE_AMP 0.500000 +SIM_WAVE_DIR 0.000000 +SIM_WAVE_ENABLE 0 +SIM_WAVE_LENGTH 10.000000 +SIM_WAVE_SPEED 0.500000 +SIM_WIND_DELAY 0 +SIM_WIND_DIR 180.000000 +SIM_WIND_DIR_Z 0.000000 +SIM_WIND_SPD 0.000000 +SIM_WIND_T 0 +SIM_WIND_TURB 0.000000 +SIM_WIND_T_ALT 60.000000 +SIM_WIND_T_COEF 0.010000 +SIM_WOW_PIN -1 +SPRAY_ENABLE 0 +SR0_ADSB 4 +SR0_EXTRA1 4 +SR0_EXTRA2 4 +SR0_EXTRA3 4 +SR0_EXT_STAT 4 +SR0_PARAMS 0 +SR0_POSITION 4 +SR0_RAW_CTRL 4 +SR0_RAW_SENS 4 +SR0_RC_CHAN 4 +SR1_ADSB 0 +SR1_EXTRA1 0 +SR1_EXTRA2 0 +SR1_EXTRA3 0 +SR1_EXT_STAT 0 +SR1_PARAMS 0 +SR1_POSITION 0 +SR1_RAW_CTRL 0 +SR1_RAW_SENS 0 +SR1_RC_CHAN 0 +SR2_ADSB 0 +SR2_EXTRA1 0 +SR2_EXTRA2 0 +SR2_EXTRA3 0 +SR2_EXT_STAT 0 +SR2_PARAMS 0 +SR2_POSITION 0 +SR2_RAW_CTRL 0 +SR2_RAW_SENS 0 +SR2_RC_CHAN 0 +SR3_ADSB 0 +SR3_EXTRA1 0 +SR3_EXTRA2 0 +SR3_EXTRA3 0 +SR3_EXT_STAT 0 +SR3_PARAMS 0 +SR3_POSITION 0 +SR3_RAW_CTRL 0 +SR3_RAW_SENS 0 +SR3_RC_CHAN 0 +SR4_ADSB 0 +SR4_EXTRA1 0 +SR4_EXTRA2 0 +SR4_EXTRA3 0 +SR4_EXT_STAT 0 +SR4_PARAMS 0 +SR4_POSITION 0 +SR4_RAW_CTRL 0 +SR4_RAW_SENS 0 +SR4_RC_CHAN 0 +SR5_ADSB 0 +SR5_EXTRA1 0 +SR5_EXTRA2 0 +SR5_EXTRA3 0 +SR5_EXT_STAT 0 +SR5_PARAMS 0 +SR5_POSITION 0 +SR5_RAW_CTRL 0 +SR5_RAW_SENS 0 +SR5_RC_CHAN 0 +SR6_ADSB 0 +SR6_EXTRA1 0 +SR6_EXTRA2 0 +SR6_EXTRA3 0 +SR6_EXT_STAT 0 +SR6_PARAMS 0 +SR6_POSITION 0 +SR6_RAW_CTRL 0 +SR6_RAW_SENS 0 +SR6_RC_CHAN 0 +SRTL_ACCURACY 2.000000 +SRTL_OPTIONS 0 +SRTL_POINTS 300 +STAT_BOOTCNT 1 +STAT_FLTTIME 0 +STAT_RESET 0 +STAT_RUNTIME 0 +SUPER_SIMPLE 0 +SYSID_ENFORCE 0 +SYSID_MYGCS 255 +SYSID_THISMAV 1 +TCAL_ENABLED 0 +TELEM_DELAY 0 +TERRAIN_ENABLE 1 +TERRAIN_OPTIONS 0 +TERRAIN_SPACING 100 +THROW_MOT_START 0 +THROW_NEXTMODE 18 +THROW_TYPE 0 +THR_DZ 100 +TUNE 0 +TUNE_MAX 0.000000 +TUNE_MIN 0.000000 +VISO_TYPE 0 +VTX_ENABLE 0 +WINCH_TYPE 0 +WPNAV_ACCEL 250.000000 +WPNAV_ACCEL_Z 100.000000 +WPNAV_JERK 1.000000 +WPNAV_RADIUS 200.000000 +WPNAV_RFND_USE 1 +WPNAV_SPEED 1000.000000 +WPNAV_SPEED_DN 150.000000 +WPNAV_SPEED_UP 250.000000 +WP_NAVALT_MIN 0.000000 +WP_YAW_BEHAVIOR 2 +ZIGZ_AUTO_ENABLE 0 diff --git a/mavros/test/mavros_py/testdata/missionplanner.parm b/mavros/test/mavros_py/testdata/missionplanner.parm new file mode 100644 index 000000000..26d68c47b --- /dev/null +++ b/mavros/test/mavros_py/testdata/missionplanner.parm @@ -0,0 +1,354 @@ +#NOTE: 22.09.2014 17:33:53 +ACRO_LOCKING,0 +ACRO_PITCH_RATE,180 +ACRO_ROLL_RATE,180 +AHRS_COMP_BETA,0.10000000149011612 +AHRS_GPS_GAIN,1.0 +AHRS_GPS_MINSATS,6 +AHRS_GPS_USE,1 +AHRS_ORIENTATION,0 +AHRS_RP_P,0.20000000298023224 +AHRS_TRIM_X,0.058273520320653915 +AHRS_TRIM_Y,-0.017083074897527695 +AHRS_TRIM_Z,0.0 +AHRS_WIND_MAX,0 +AHRS_YAW_P,0.20000000298023224 +ALT_CTRL_ALG,0 +ALT_HOLD_FBWCM,0 +ALT_HOLD_RTL,8000 +ALT_MIX,1.0 +ALT_OFFSET,0 +ARMING_CHECK,0 +ARMING_DIS_RUD,0 +ARMING_REQUIRE,0 +ARSPD_AUTOCAL,1 +ARSPD_ENABLE,1 +ARSPD_FBW_MAX,22 +ARSPD_FBW_MIN,9 +ARSPD_OFFSET,2128.38037109375 +ARSPD_PIN,0 +ARSPD_RATIO,2.201099157333374 +ARSPD_TUBE_ORDER,2 +ARSPD_USE,1 +AUTOTUNE_LEVEL,5 +AUTO_FBW_STEER,0 +BATT_AMP_OFFSET,0.0 +BATT_AMP_PERVOLT,18.001800537109375 +BATT_CAPACITY,2200 +BATT_CURR_PIN,12 +BATT_MONITOR,4 +BATT_VOLT2_MULT,1.0 +BATT_VOLT2_PIN,-1 +BATT_VOLT_MULT,10.100000381469727 +BATT_VOLT_PIN,13 +CAM_DURATION,10 +CAM_SERVO_OFF,1100 +CAM_SERVO_ON,1300 +CAM_TRIGG_DIST,0.0 +CAM_TRIGG_TYPE,0 +COMPASS_AUTODEC,1 +COMPASS_DEC,0.0 +COMPASS_EXTERNAL,1 +COMPASS_LEARN,0 +COMPASS_MOTCT,0 +COMPASS_MOT_X,0.0 +COMPASS_MOT_Y,0.0 +COMPASS_MOT_Z,0.0 +COMPASS_OFS_X,-2.8186113834381104 +COMPASS_OFS_Y,29.56283187866211 +COMPASS_OFS_Z,-153.5463104248047 +COMPASS_ORIENT,8 +COMPASS_USE,1 +ELEVON_CH1_REV,0 +ELEVON_CH2_REV,0 +ELEVON_MIXING,0 +ELEVON_OUTPUT,0 +ELEVON_REVERSE,0 +FBWA_TDRAG_CHAN,0 +FBWB_CLIMB_RATE,2 +FBWB_ELEV_REV,0 +FENCE_ACTION,0 +FENCE_AUTOENABLE,0 +FENCE_CHANNEL,0 +FENCE_MAXALT,300 +FENCE_MINALT,30 +FENCE_RETALT,0 +FENCE_RET_RALLY,0 +FENCE_TOTAL,6 +FLAPERON_OUTPUT,0 +FLAP_1_PERCNT,0 +FLAP_1_SPEED,0 +FLAP_2_PERCNT,0 +FLAP_2_SPEED,0 +FLAP_IN_CHANNEL,8 +FLAP_SLEWRATE,75 +FLTMODE1,11 +FLTMODE2,6 +FLTMODE3,5 +FLTMODE4,10 +FLTMODE5,12 +FLTMODE6,0 +FLTMODE_CH,5 +FORMAT_VERSION,13 +FS_BATT_MAH,0.0 +FS_BATT_VOLTAGE,0.0 +FS_GCS_ENABL,0 +FS_LONG_ACTN,1 +FS_LONG_TIMEOUT,20.0 +FS_SHORT_ACTN,1 +FS_SHORT_TIMEOUT,1.5 +GLIDE_SLOPE_MIN,15 +GND_ABS_PRESS,99563.359375 +GND_ALT_OFFSET,0 +GND_TEMP,26.19577407836914 +GPS_MIN_ELEV,-100 +GPS_NAVFILTER,8 +GPS_SBAS_MODE,2 +GPS_TYPE,2 +GROUND_STEER_ALT,0.0 +GROUND_STEER_DPS,90 +INS_ACCOFFS_X,0.17904429137706757 +INS_ACCOFFS_Y,0.2191154956817627 +INS_ACCOFFS_Z,0.6167079210281372 +INS_ACCSCAL_X,0.9940382838249207 +INS_ACCSCAL_Y,0.9934437870979309 +INS_ACCSCAL_Z,0.9752721786499023 +INS_GYROFFS_X,0.0013595402706414461 +INS_GYROFFS_Y,-0.026158515363931656 +INS_GYROFFS_Z,0.02016429416835308 +INS_MPU6K_FILTER,0 +INS_PRODUCT_ID,88 +INVERTEDFLT_CH,0 +KFF_RDDRMIX,0.5 +KFF_THR2PTCH,0.0 +LAND_FLAP_PERCNT,0 +LAND_FLARE_ALT,5.0 +LAND_FLARE_SEC,3.0 +LAND_PITCH_CD,25 +LEVEL_ROLL_LIMIT,5 +LIM_PITCH_MAX,3500 +LIM_PITCH_MIN,-3000 +LIM_ROLL_CD,5000 +LOG_BITMASK,5190 +MAG_ENABLE,1 +MIN_GNDSPD_CM,0 +MIS_RESTART,0 +MIS_TOTAL,5 +MIXING_GAIN,0.5 +MNT_ANGMAX_PAN,4500 +MNT_ANGMAX_ROL,4500 +MNT_ANGMAX_TIL,4500 +MNT_ANGMIN_PAN,-4500 +MNT_ANGMIN_ROL,-4500 +MNT_ANGMIN_TIL,-4500 +MNT_CONTROL_X,0.0 +MNT_CONTROL_Y,0.0 +MNT_CONTROL_Z,0.0 +MNT_JSTICK_SPD,0 +MNT_MODE,0 +MNT_NEUTRAL_X,0.0 +MNT_NEUTRAL_Y,0.0 +MNT_NEUTRAL_Z,0.0 +MNT_RC_IN_PAN,0 +MNT_RC_IN_ROLL,0 +MNT_RC_IN_TILT,0 +MNT_RETRACT_X,0.0 +MNT_RETRACT_Y,0.0 +MNT_RETRACT_Z,0.0 +MNT_STAB_PAN,0 +MNT_STAB_ROLL,0 +MNT_STAB_TILT,0 +NAVL1_DAMPING,0.75 +NAVL1_PERIOD,25.0 +NAV_CONTROLLER,1 +PTCH2SRV_D,0.12879998981952667 +PTCH2SRV_I,0.14000000059604645 +PTCH2SRV_IMAX,3500 +PTCH2SRV_P,1.8399999141693115 +PTCH2SRV_RLL,1.2000000476837158 +PTCH2SRV_RMAX_DN,60 +PTCH2SRV_RMAX_UP,60 +PTCH2SRV_TCONST,0.5 +RALLY_LIMIT_KM,1.0 +RALLY_TOTAL,1 +RC10_DZ,0 +RC10_FUNCTION,0 +RC10_MAX,1900 +RC10_MIN,1100 +RC10_REV,1 +RC10_TRIM,1500 +RC11_DZ,0 +RC11_FUNCTION,0 +RC11_MAX,1900 +RC11_MIN,1100 +RC11_REV,1 +RC11_TRIM,1500 +RC1_DZ,30 +RC1_MAX,2017 +RC1_MIN,992 +RC1_REV,-1 +RC1_TRIM,1495 +RC2_DZ,30 +RC2_MAX,2017 +RC2_MIN,992 +RC2_REV,-1 +RC2_TRIM,1502 +RC3_DZ,30 +RC3_MAX,2017 +RC3_MIN,992 +RC3_REV,1 +RC3_TRIM,1003 +RC4_DZ,30 +RC4_MAX,2017 +RC4_MIN,992 +RC4_REV,-1 +RC4_TRIM,1509 +RC5_DZ,0 +RC5_FUNCTION,0 +RC5_MAX,1966 +RC5_MIN,1100 +RC5_REV,1 +RC5_TRIM,1966 +RC6_DZ,0 +RC6_FUNCTION,0 +RC6_MAX,2017 +RC6_MIN,992 +RC6_REV,1 +RC6_TRIM,1495 +RC7_DZ,0 +RC7_FUNCTION,2 +RC7_MAX,1660 +RC7_MIN,1146 +RC7_REV,-1 +RC7_TRIM,1659 +RC8_DZ,0 +RC8_FUNCTION,2 +RC8_MAX,1828 +RC8_MIN,1237 +RC8_REV,1 +RC8_TRIM,1238 +RCMAP_PITCH,2 +RCMAP_ROLL,1 +RCMAP_THROTTLE,3 +RCMAP_YAW,4 +RELAY_DEFAULT,0 +RELAY_PIN,13 +RELAY_PIN2,-1 +RELAY_PIN3,-1 +RELAY_PIN4,-1 +RLL2SRV_D,0.03320127725601196 +RLL2SRV_I,0.031118083745241165 +RLL2SRV_IMAX,4000 +RLL2SRV_P,0.4743039608001709 +RLL2SRV_RMAX,60 +RLL2SRV_TCONST,0.5 +RNGFND2_FUNCTION,0 +RNGFND2_MAX_CM,700 +RNGFND2_MIN_CM,20 +RNGFND2_OFFSET,0.0 +RNGFND2_PIN,-1 +RNGFND2_RMETRIC,1 +RNGFND2_SCALING,3.0 +RNGFND2_SETTLE_M,0 +RNGFND2_STOP_PIN,-1 +RNGFND2_TYPE,0 +RNGFND_FUNCTION,0 +RNGFND_LANDING,0 +RNGFND_MAX_CM,700 +RNGFND_MIN_CM,20 +RNGFND_OFFSET,0.0 +RNGFND_PIN,-1 +RNGFND_RMETRIC,1 +RNGFND_SCALING,3.0 +RNGFND_SETTLE_MS,0 +RNGFND_STOP_PIN,-1 +RNGFND_TYPE,0 +RSSI_PIN,1 +RSSI_RANGE,3.299999952316284 +RST_MISSION_CH,0 +RST_SWITCH_CH,0 +SCALING_SPEED,15.0 +SCHED_DEBUG,0 +SERIAL0_BAUD,115 +SERIAL1_BAUD,57 +SKIP_GYRO_CAL,0 +SR0_EXTRA1,10 +SR0_EXTRA2,10 +SR0_EXTRA3,2 +SR0_EXT_STAT,2 +SR0_PARAMS,10 +SR0_POSITION,3 +SR0_RAW_CTRL,2 +SR0_RAW_SENS,2 +SR0_RC_CHAN,2 +SR1_EXTRA1,2 +SR1_EXTRA2,2 +SR1_EXTRA3,2 +SR1_EXT_STAT,2 +SR1_PARAMS,10 +SR1_POSITION,2 +SR1_RAW_CTRL,2 +SR1_RAW_SENS,2 +SR1_RC_CHAN,2 +STAB_PITCH_DOWN,2.0 +STEER2SRV_D,0.004999999888241291 +STEER2SRV_I,0.20000000298023224 +STEER2SRV_IMAX,1500 +STEER2SRV_MINSPD,1.0 +STEER2SRV_P,1.7999999523162842 +STEER2SRV_TCONST,0.75 +STICK_MIXING,1 +SYSID_MYGCS,255 +SYSID_SW_TYPE,0 +SYSID_THISMAV,1 +SYS_NUM_RESETS,117 +TECS_CLMB_MAX,5.0 +TECS_HGT_OMEGA,3.0 +TECS_INTEG_GAIN,0.10000000149011612 +TECS_LAND_ARSPD,-1.0 +TECS_LAND_SINK,0.25 +TECS_LAND_SPDWGT,1.0 +TECS_LAND_TCONST,2.0 +TECS_LAND_THR,-1.0 +TECS_PITCH_MAX,0 +TECS_PITCH_MIN,0 +TECS_PTCH_DAMP,0.0 +TECS_RLL2THR,10.0 +TECS_SINK_MAX,5.0 +TECS_SINK_MIN,2.0 +TECS_SPDWEIGHT,1.0 +TECS_SPD_OMEGA,2.0 +TECS_THR_DAMP,0.5 +TECS_TIME_CONST,5.0 +TECS_VERT_ACC,7.0 +TELEM_DELAY,0 +THROTTLE_NUDGE,1 +THR_FAILSAFE,1 +THR_FS_VALUE,995 +THR_MAX,80 +THR_MIN,0 +THR_PASS_STAB,0 +THR_SLEWRATE,60 +THR_SUPP_MAN,0 +TKOFF_FLAP_PCNT,0 +TKOFF_ROTATE_SPD,0.0 +TKOFF_TDRAG_ELEV,0 +TKOFF_TDRAG_SPD1,0.0 +TKOFF_THR_DELAY,1 +TKOFF_THR_MAX,100 +TKOFF_THR_MINACC,3.0 +TKOFF_THR_MINSPD,1.0 +TKOFF_THR_SLEW,0 +TRIM_ARSPD_CM,1500 +TRIM_AUTO,0 +TRIM_PITCH_CD,0 +TRIM_THROTTLE,60 +VTAIL_OUTPUT,0 +WP_LOITER_RAD,60 +WP_MAX_RADIUS,50 +WP_RADIUS,46 +YAW2SRV_DAMP,0.0 +YAW2SRV_IMAX,1500 +YAW2SRV_INT,0.0 +YAW2SRV_RLL,1.0 +YAW2SRV_SLIP,0.0 diff --git a/mavros/test/mavros_py/testdata/qgroundcontrol.params b/mavros/test/mavros_py/testdata/qgroundcontrol.params new file mode 100644 index 000000000..0af380634 --- /dev/null +++ b/mavros/test/mavros_py/testdata/qgroundcontrol.params @@ -0,0 +1,1304 @@ +# Onboard parameters for Vehicle 1 +# +# Stack: ArduPilot +# Vehicle: Multi-Rotor +# Version: 4.1.0 dev +# Git Revision: e00d40a3 +# +# Vehicle-Id Component-Id Name Value Type +1 1 ACRO_BAL_PITCH 1.000000000000000000 9 +1 1 ACRO_BAL_ROLL 1.000000000000000000 9 +1 1 ACRO_OPTIONS 0 2 +1 1 ACRO_RP_EXPO 0.300000011920928955 9 +1 1 ACRO_RP_P 4.500000000000000000 9 +1 1 ACRO_THR_MID 0.000000000000000000 9 +1 1 ACRO_TRAINER 2 2 +1 1 ACRO_YAW_P 4.500000000000000000 9 +1 1 ACRO_Y_EXPO 0.000000000000000000 9 +1 1 ADSB_TYPE 0 2 +1 1 AHRS_COMP_BETA 0.100000001490116119 9 +1 1 AHRS_CUSTOM_PIT 0.000000000000000000 9 +1 1 AHRS_CUSTOM_ROLL 0.000000000000000000 9 +1 1 AHRS_CUSTOM_YAW 0.000000000000000000 9 +1 1 AHRS_EKF_TYPE 3 2 +1 1 AHRS_GPS_GAIN 1.000000000000000000 9 +1 1 AHRS_GPS_MINSATS 6 2 +1 1 AHRS_GPS_USE 1 2 +1 1 AHRS_ORIENTATION 0 2 +1 1 AHRS_RP_P 0.200000002980232239 9 +1 1 AHRS_TRIM_X 0.000000000000000000 9 +1 1 AHRS_TRIM_Y 0.000000000000000000 9 +1 1 AHRS_TRIM_Z 0.000000000000000000 9 +1 1 AHRS_WIND_MAX 0 2 +1 1 AHRS_YAW_P 0.200000002980232239 9 +1 1 ANGLE_MAX 3000 4 +1 1 ARMING_ACCTHRESH 0.750000000000000000 9 +1 1 ARMING_CHECK 1 6 +1 1 ARMING_MIS_ITEMS 0 6 +1 1 ARMING_RUDDER 2 2 +1 1 ATC_ACCEL_P_MAX 110000.000000000000000000 9 +1 1 ATC_ACCEL_R_MAX 110000.000000000000000000 9 +1 1 ATC_ACCEL_Y_MAX 27000.000000000000000000 9 +1 1 ATC_ANGLE_BOOST 1 2 +1 1 ATC_ANG_LIM_TC 1.000000000000000000 9 +1 1 ATC_ANG_PIT_P 4.500000000000000000 9 +1 1 ATC_ANG_RLL_P 4.500000000000000000 9 +1 1 ATC_ANG_YAW_P 4.500000000000000000 9 +1 1 ATC_INPUT_TC 0.150000005960464478 9 +1 1 ATC_RATE_FF_ENAB 1 2 +1 1 ATC_RATE_P_MAX 0.000000000000000000 9 +1 1 ATC_RATE_R_MAX 0.000000000000000000 9 +1 1 ATC_RATE_Y_MAX 0.000000000000000000 9 +1 1 ATC_RAT_PIT_D 0.003599999938160181 9 +1 1 ATC_RAT_PIT_FF 0.000000000000000000 9 +1 1 ATC_RAT_PIT_FLTD 20.000000000000000000 9 +1 1 ATC_RAT_PIT_FLTE 0.000000000000000000 9 +1 1 ATC_RAT_PIT_FLTT 20.000000000000000000 9 +1 1 ATC_RAT_PIT_I 0.135000005364418030 9 +1 1 ATC_RAT_PIT_IMAX 0.500000000000000000 9 +1 1 ATC_RAT_PIT_P 0.135000005364418030 9 +1 1 ATC_RAT_PIT_SMAX 0.000000000000000000 9 +1 1 ATC_RAT_RLL_D 0.003599999938160181 9 +1 1 ATC_RAT_RLL_FF 0.000000000000000000 9 +1 1 ATC_RAT_RLL_FLTD 20.000000000000000000 9 +1 1 ATC_RAT_RLL_FLTE 0.000000000000000000 9 +1 1 ATC_RAT_RLL_FLTT 20.000000000000000000 9 +1 1 ATC_RAT_RLL_I 0.135000005364418030 9 +1 1 ATC_RAT_RLL_IMAX 0.500000000000000000 9 +1 1 ATC_RAT_RLL_P 0.135000005364418030 9 +1 1 ATC_RAT_RLL_SMAX 0.000000000000000000 9 +1 1 ATC_RAT_YAW_D 0.000000000000000000 9 +1 1 ATC_RAT_YAW_FF 0.000000000000000000 9 +1 1 ATC_RAT_YAW_FLTD 0.000000000000000000 9 +1 1 ATC_RAT_YAW_FLTE 2.500000000000000000 9 +1 1 ATC_RAT_YAW_FLTT 20.000000000000000000 9 +1 1 ATC_RAT_YAW_I 0.017999999225139618 9 +1 1 ATC_RAT_YAW_IMAX 0.500000000000000000 9 +1 1 ATC_RAT_YAW_P 0.180000007152557373 9 +1 1 ATC_RAT_YAW_SMAX 0.000000000000000000 9 +1 1 ATC_SLEW_YAW 6000.000000000000000000 9 +1 1 ATC_THR_MIX_MAN 0.100000001490116119 9 +1 1 ATC_THR_MIX_MAX 0.500000000000000000 9 +1 1 ATC_THR_MIX_MIN 0.100000001490116119 9 +1 1 AUTOTUNE_AGGR 0.100000001490116119 9 +1 1 AUTOTUNE_AXES 7 2 +1 1 AUTOTUNE_MIN_D 0.001000000047497451 9 +1 1 AUTO_OPTIONS 0 6 +1 1 AVD_ENABLE 0 2 +1 1 AVOID_ACCEL_MAX 3.000000000000000000 9 +1 1 AVOID_ALT_MIN 0.000000000000000000 9 +1 1 AVOID_ANGLE_MAX 1000 4 +1 1 AVOID_BACKUP_SPD 0.750000000000000000 9 +1 1 AVOID_BEHAVE 0 2 +1 1 AVOID_DIST_MAX 5.000000000000000000 9 +1 1 AVOID_ENABLE 3 2 +1 1 AVOID_MARGIN 2.000000000000000000 9 +1 1 BARO1_DEVID 65540 6 +1 1 BARO1_GND_PRESS 94502.796875000000000000 9 +1 1 BARO1_WCF_ENABLE 0 2 +1 1 BARO2_DEVID 65796 6 +1 1 BARO2_GND_PRESS 94503.234375000000000000 9 +1 1 BARO2_WCF_ENABLE 0 2 +1 1 BARO3_DEVID 0 6 +1 1 BARO3_GND_PRESS 0.000000000000000000 9 +1 1 BARO3_WCF_ENABLE 0 2 +1 1 BARO_ALT_OFFSET 0.000000000000000000 9 +1 1 BARO_EXT_BUS -1 2 +1 1 BARO_FLTR_RNG 0 2 +1 1 BARO_GND_TEMP 0.000000000000000000 9 +1 1 BARO_PRIMARY 0 2 +1 1 BARO_PROBE_EXT 0 6 +1 1 BATT2_MONITOR 0 2 +1 1 BATT3_MONITOR 0 2 +1 1 BATT4_MONITOR 0 2 +1 1 BATT5_MONITOR 0 2 +1 1 BATT6_MONITOR 0 2 +1 1 BATT7_MONITOR 0 2 +1 1 BATT8_MONITOR 0 2 +1 1 BATT9_MONITOR 0 2 +1 1 BATT_AMP_OFFSET 0.000000000000000000 9 +1 1 BATT_AMP_PERVLT 17.000000000000000000 9 +1 1 BATT_ARM_MAH 0 6 +1 1 BATT_ARM_VOLT 0.000000000000000000 9 +1 1 BATT_BUS 0 2 +1 1 BATT_CAPACITY 0 6 +1 1 BATT_CRT_MAH 0.000000000000000000 9 +1 1 BATT_CRT_VOLT 0.000000000000000000 9 +1 1 BATT_CURR_PIN 12 2 +1 1 BATT_FS_CRT_ACT 0 2 +1 1 BATT_FS_LOW_ACT 0 2 +1 1 BATT_FS_VOLTSRC 0 2 +1 1 BATT_LOW_MAH 0.000000000000000000 9 +1 1 BATT_LOW_TIMER 10 2 +1 1 BATT_LOW_VOLT 10.500000000000000000 9 +1 1 BATT_MONITOR 4 2 +1 1 BATT_OPTIONS 0 6 +1 1 BATT_SERIAL_NUM -1 6 +1 1 BATT_VOLT_MULT 10.100000381469726563 9 +1 1 BATT_VOLT_PIN 13 2 +1 1 BCN_ALT 0.000000000000000000 9 +1 1 BCN_LATITUDE 0.000000000000000000 9 +1 1 BCN_LONGITUDE 0.000000000000000000 9 +1 1 BCN_ORIENT_YAW 0 4 +1 1 BCN_TYPE 0 2 +1 1 BRD_ALT_CONFIG 0 2 +1 1 BRD_BOOT_DELAY 0 4 +1 1 BRD_OPTIONS 0 6 +1 1 BRD_PWM_COUNT 8 2 +1 1 BRD_RTC_TYPES 1 2 +1 1 BRD_RTC_TZ_MIN 0 4 +1 1 BRD_SAFETYOPTION 3 4 +1 1 BRD_SERIAL_NUM 0 4 +1 1 BRD_VBUS_MIN 4.300000190734863281 9 +1 1 BRD_VSERVO_MIN 0.000000000000000000 9 +1 1 BTN_ENABLE 0 2 +1 1 CAM_AUTO_ONLY 0 2 +1 1 CAM_DURATION 10 2 +1 1 CAM_FEEDBACK_PIN -1 2 +1 1 CAM_FEEDBACK_POL 1 2 +1 1 CAM_MAX_ROLL 0 4 +1 1 CAM_MIN_INTERVAL 0 4 +1 1 CAM_RC_TYPE 0 2 +1 1 CAM_RELAY_ON 1 2 +1 1 CAM_SERVO_OFF 1100 4 +1 1 CAM_SERVO_ON 1300 4 +1 1 CAM_TRIGG_DIST 0.000000000000000000 9 +1 1 CAM_TRIGG_TYPE 0 2 +1 1 CAM_TYPE 0 2 +1 1 CAN_D1_PROTOCOL 1 2 +1 1 CAN_D2_PROTOCOL 1 2 +1 1 CAN_LOGLEVEL 0 2 +1 1 CAN_P1_DRIVER 0 2 +1 1 CAN_P2_DRIVER 0 2 +1 1 CAN_SLCAN_CPORT 0 2 +1 1 CAN_SLCAN_SDELAY 1 2 +1 1 CAN_SLCAN_SERNUM -1 2 +1 1 CAN_SLCAN_TIMOUT 0 2 +1 1 CHUTE_ENABLED 0 2 +1 1 CIRCLE_OPTIONS 1 4 +1 1 CIRCLE_RADIUS 1000.000000000000000000 9 +1 1 CIRCLE_RATE 20.000000000000000000 9 +1 1 COMPASS_AUTODEC 1 2 +1 1 COMPASS_AUTO_ROT 2 2 +1 1 COMPASS_CAL_FIT 16.000000000000000000 9 +1 1 COMPASS_CUS_PIT 0.000000000000000000 9 +1 1 COMPASS_CUS_ROLL 0.000000000000000000 9 +1 1 COMPASS_CUS_YAW 0.000000000000000000 9 +1 1 COMPASS_DEC 0.207633301615715027 9 +1 1 COMPASS_DEV_ID 97539 6 +1 1 COMPASS_DEV_ID2 131874 6 +1 1 COMPASS_DEV_ID3 263178 6 +1 1 COMPASS_DEV_ID4 97283 6 +1 1 COMPASS_DEV_ID5 97795 6 +1 1 COMPASS_DEV_ID6 98051 6 +1 1 COMPASS_DEV_ID7 983044 6 +1 1 COMPASS_DEV_ID8 0 6 +1 1 COMPASS_DIA2_X 1.000000000000000000 9 +1 1 COMPASS_DIA2_Y 1.000000000000000000 9 +1 1 COMPASS_DIA2_Z 1.000000000000000000 9 +1 1 COMPASS_DIA3_X 1.000000000000000000 9 +1 1 COMPASS_DIA3_Y 1.000000000000000000 9 +1 1 COMPASS_DIA3_Z 1.000000000000000000 9 +1 1 COMPASS_DIA_X 1.000000000000000000 9 +1 1 COMPASS_DIA_Y 1.000000000000000000 9 +1 1 COMPASS_DIA_Z 1.000000000000000000 9 +1 1 COMPASS_ENABLE 1 2 +1 1 COMPASS_EXTERN2 0 2 +1 1 COMPASS_EXTERN3 0 2 +1 1 COMPASS_EXTERNAL 1 2 +1 1 COMPASS_FLTR_RNG 0 2 +1 1 COMPASS_LEARN 0 2 +1 1 COMPASS_MOT2_X 0.000000000000000000 9 +1 1 COMPASS_MOT2_Y 0.000000000000000000 9 +1 1 COMPASS_MOT2_Z 0.000000000000000000 9 +1 1 COMPASS_MOT3_X 0.000000000000000000 9 +1 1 COMPASS_MOT3_Y 0.000000000000000000 9 +1 1 COMPASS_MOT3_Z 0.000000000000000000 9 +1 1 COMPASS_MOTCT 0 2 +1 1 COMPASS_MOT_X 0.000000000000000000 9 +1 1 COMPASS_MOT_Y 0.000000000000000000 9 +1 1 COMPASS_MOT_Z 0.000000000000000000 9 +1 1 COMPASS_ODI2_X 0.000000000000000000 9 +1 1 COMPASS_ODI2_Y 0.000000000000000000 9 +1 1 COMPASS_ODI2_Z 0.000000000000000000 9 +1 1 COMPASS_ODI3_X 0.000000000000000000 9 +1 1 COMPASS_ODI3_Y 0.000000000000000000 9 +1 1 COMPASS_ODI3_Z 0.000000000000000000 9 +1 1 COMPASS_ODI_X 0.000000000000000000 9 +1 1 COMPASS_ODI_Y 0.000000000000000000 9 +1 1 COMPASS_ODI_Z 0.000000000000000000 9 +1 1 COMPASS_OFFS_MAX 1800 4 +1 1 COMPASS_OFS2_X 5.000000000000000000 9 +1 1 COMPASS_OFS2_Y 13.000000000000000000 9 +1 1 COMPASS_OFS2_Z -18.000000000000000000 9 +1 1 COMPASS_OFS3_X 5.000000000000000000 9 +1 1 COMPASS_OFS3_Y 13.000000000000000000 9 +1 1 COMPASS_OFS3_Z -18.000000000000000000 9 +1 1 COMPASS_OFS_X 5.000000000000000000 9 +1 1 COMPASS_OFS_Y 13.000000000000000000 9 +1 1 COMPASS_OFS_Z -18.000000000000000000 9 +1 1 COMPASS_OPTIONS 0 4 +1 1 COMPASS_ORIENT 0 2 +1 1 COMPASS_ORIENT2 0 2 +1 1 COMPASS_ORIENT3 0 2 +1 1 COMPASS_PMOT_EN 0 2 +1 1 COMPASS_PRIO1_ID 97539 6 +1 1 COMPASS_PRIO2_ID 131874 6 +1 1 COMPASS_PRIO3_ID 263178 6 +1 1 COMPASS_SCALE 1.000000000000000000 9 +1 1 COMPASS_SCALE2 1.000000000000000000 9 +1 1 COMPASS_SCALE3 1.000000000000000000 9 +1 1 COMPASS_TYPEMASK 0 6 +1 1 COMPASS_USE 1 2 +1 1 COMPASS_USE2 1 2 +1 1 COMPASS_USE3 1 2 +1 1 DEV_OPTIONS 0 6 +1 1 DISARM_DELAY 10 2 +1 1 EAHRS_TYPE 0 2 +1 1 EK2_ENABLE 0 2 +1 1 EK3_ABIAS_P_NSE 0.003000000026077032 9 +1 1 EK3_ACC_BIAS_LIM 1.000000000000000000 9 +1 1 EK3_ACC_P_NSE 0.349999994039535522 9 +1 1 EK3_AFFINITY 0 6 +1 1 EK3_ALT_M_NSE 2.000000000000000000 9 +1 1 EK3_BCN_DELAY 50 2 +1 1 EK3_BCN_I_GTE 500 4 +1 1 EK3_BCN_M_NSE 1.000000000000000000 9 +1 1 EK3_BETA_MASK 0 2 +1 1 EK3_CHECK_SCALE 100 4 +1 1 EK3_DRAG_BCOEF_X 0.000000000000000000 9 +1 1 EK3_DRAG_BCOEF_Y 0.000000000000000000 9 +1 1 EK3_DRAG_MCOEF 0.000000000000000000 9 +1 1 EK3_DRAG_M_NSE 0.500000000000000000 9 +1 1 EK3_EAS_I_GATE 400 4 +1 1 EK3_EAS_M_NSE 1.399999976158142090 9 +1 1 EK3_ENABLE 1 2 +1 1 EK3_ERR_THRESH 0.200000002980232239 9 +1 1 EK3_FLOW_DELAY 10 2 +1 1 EK3_FLOW_I_GATE 300 4 +1 1 EK3_FLOW_M_NSE 0.250000000000000000 9 +1 1 EK3_FLOW_USE 1 2 +1 1 EK3_GBIAS_P_NSE 0.001000000047497451 9 +1 1 EK3_GLITCH_RAD 25 2 +1 1 EK3_GPS_CHECK 31 2 +1 1 EK3_GSF_DELAY 1000 4 +1 1 EK3_GSF_RST_MAX 2 2 +1 1 EK3_GSF_RUN_MASK 3 2 +1 1 EK3_GSF_USE_MASK 3 2 +1 1 EK3_GYRO_P_NSE 0.014999999664723873 9 +1 1 EK3_HGT_DELAY 60 4 +1 1 EK3_HGT_I_GATE 500 4 +1 1 EK3_HRT_FILT 2.000000000000000000 9 +1 1 EK3_IMU_MASK 3 2 +1 1 EK3_MAGB_P_NSE 0.000099999997473788 9 +1 1 EK3_MAGE_P_NSE 0.001000000047497451 9 +1 1 EK3_MAG_CAL 3 2 +1 1 EK3_MAG_EF_LIM 50 4 +1 1 EK3_MAG_I_GATE 300 4 +1 1 EK3_MAG_MASK 0 2 +1 1 EK3_MAG_M_NSE 0.050000000745058060 9 +1 1 EK3_MAX_FLOW 2.500000000000000000 9 +1 1 EK3_NOAID_M_NSE 10.000000000000000000 9 +1 1 EK3_OGNM_TEST_SF 2.000000000000000000 9 +1 1 EK3_OGN_HGT_MASK 0 2 +1 1 EK3_POSNE_M_NSE 0.500000000000000000 9 +1 1 EK3_POS_I_GATE 500 4 +1 1 EK3_RNG_I_GATE 500 4 +1 1 EK3_RNG_M_NSE 0.500000000000000000 9 +1 1 EK3_RNG_USE_HGT -1 2 +1 1 EK3_RNG_USE_SPD 2.000000000000000000 9 +1 1 EK3_SRC1_POSXY 3 2 +1 1 EK3_SRC1_POSZ 1 2 +1 1 EK3_SRC1_VELXY 3 2 +1 1 EK3_SRC1_VELZ 3 2 +1 1 EK3_SRC1_YAW 1 2 +1 1 EK3_SRC2_POSXY 0 2 +1 1 EK3_SRC2_POSZ 1 2 +1 1 EK3_SRC2_VELXY 0 2 +1 1 EK3_SRC2_VELZ 0 2 +1 1 EK3_SRC2_YAW 0 2 +1 1 EK3_SRC3_POSXY 0 2 +1 1 EK3_SRC3_POSZ 1 2 +1 1 EK3_SRC3_VELXY 0 2 +1 1 EK3_SRC3_VELZ 0 2 +1 1 EK3_SRC3_YAW 0 2 +1 1 EK3_SRC_OPTIONS 1 4 +1 1 EK3_TAU_OUTPUT 25 2 +1 1 EK3_TERR_GRAD 0.100000001490116119 9 +1 1 EK3_VELD_M_NSE 0.500000000000000000 9 +1 1 EK3_VELNE_M_NSE 0.300000011920928955 9 +1 1 EK3_VEL_I_GATE 500 4 +1 1 EK3_VIS_VERR_MAX 0.899999976158142090 9 +1 1 EK3_VIS_VERR_MIN 0.100000001490116119 9 +1 1 EK3_WENC_VERR 0.100000001490116119 9 +1 1 EK3_WIND_PSCALE 0.500000000000000000 9 +1 1 EK3_WIND_P_NSE 0.200000002980232239 9 +1 1 EK3_YAW_I_GATE 300 4 +1 1 EK3_YAW_M_NSE 0.500000000000000000 9 +1 1 ESC_CALIBRATION 0 2 +1 1 FENCE_ACTION 1 2 +1 1 FENCE_ALT_MAX 100.000000000000000000 9 +1 1 FENCE_ALT_MIN -10.000000000000000000 9 +1 1 FENCE_ENABLE 0 2 +1 1 FENCE_MARGIN 2.000000000000000000 9 +1 1 FENCE_RADIUS 150.000000000000000000 9 +1 1 FENCE_TOTAL 0 2 +1 1 FENCE_TYPE 7 2 +1 1 FFT_ENABLE 0 2 +1 1 FHLD_BRAKE_RATE 8 2 +1 1 FHLD_FILT_HZ 5.000000000000000000 9 +1 1 FHLD_FLOW_MAX 0.600000023841857910 9 +1 1 FHLD_QUAL_MIN 10 2 +1 1 FHLD_XY_FILT_HZ 5.000000000000000000 9 +1 1 FHLD_XY_I 0.300000011920928955 9 +1 1 FHLD_XY_IMAX 3000.000000000000000000 9 +1 1 FHLD_XY_P 0.200000002980232239 9 +1 1 FLIGHT_OPTIONS 0 6 +1 1 FLOW_ADDR 0 2 +1 1 FLOW_FXSCALER 0 4 +1 1 FLOW_FYSCALER 0 4 +1 1 FLOW_ORIENT_YAW 0 4 +1 1 FLOW_POS_X 0.000000000000000000 9 +1 1 FLOW_POS_Y 0.000000000000000000 9 +1 1 FLOW_POS_Z 0.000000000000000000 9 +1 1 FLOW_TYPE 0 2 +1 1 FLTMODE1 7 2 +1 1 FLTMODE2 9 2 +1 1 FLTMODE3 6 2 +1 1 FLTMODE4 3 2 +1 1 FLTMODE5 5 2 +1 1 FLTMODE6 0 2 +1 1 FLTMODE_CH 5 2 +1 1 FOLL_ENABLE 0 2 +1 1 FORMAT_VERSION 120 4 +1 1 FRAME_CLASS 1 2 +1 1 FRAME_TYPE 0 2 +1 1 FRSKY_DNLINK1_ID 20 2 +1 1 FRSKY_DNLINK2_ID 7 2 +1 1 FRSKY_UPLINK_ID 13 2 +1 1 FS_CRASH_CHECK 1 2 +1 1 FS_EKF_ACTION 1 2 +1 1 FS_EKF_THRESH 0.800000011920928955 9 +1 1 FS_GCS_ENABLE 0 2 +1 1 FS_GCS_TIMEOUT 5.000000000000000000 9 +1 1 FS_OPTIONS 16 6 +1 1 FS_THR_ENABLE 1 2 +1 1 FS_THR_VALUE 975 4 +1 1 FS_VIBE_ENABLE 1 2 +1 1 GCS_PID_MASK 0 4 +1 1 GEN_TYPE 0 2 +1 1 GND_EFFECT_COMP 1 2 +1 1 GPS1_CAN_OVRIDE 0 6 +1 1 GPS2_CAN_OVRIDE 0 6 +1 1 GPS_AUTO_CONFIG 1 2 +1 1 GPS_AUTO_SWITCH 1 2 +1 1 GPS_BLEND_MASK 5 2 +1 1 GPS_BLEND_TC 10.000000000000000000 9 +1 1 GPS_CAN_NODEID1 0 6 +1 1 GPS_CAN_NODEID2 0 6 +1 1 GPS_COM_PORT 1 2 +1 1 GPS_COM_PORT2 1 2 +1 1 GPS_DELAY_MS 0 4 +1 1 GPS_DELAY_MS2 0 4 +1 1 GPS_DRV_OPTIONS 0 4 +1 1 GPS_GNSS_MODE 0 2 +1 1 GPS_GNSS_MODE2 0 2 +1 1 GPS_HDOP_GOOD 140 4 +1 1 GPS_INJECT_TO 127 2 +1 1 GPS_MB1_TYPE 0 2 +1 1 GPS_MB2_TYPE 0 2 +1 1 GPS_MIN_DGPS 100 2 +1 1 GPS_MIN_ELEV -100 2 +1 1 GPS_NAVFILTER 8 2 +1 1 GPS_POS1_X 0.000000000000000000 9 +1 1 GPS_POS1_Y 0.000000000000000000 9 +1 1 GPS_POS1_Z 0.000000000000000000 9 +1 1 GPS_POS2_X 0.000000000000000000 9 +1 1 GPS_POS2_Y 0.000000000000000000 9 +1 1 GPS_POS2_Z 0.000000000000000000 9 +1 1 GPS_PRIMARY 0 2 +1 1 GPS_RATE_MS 200 4 +1 1 GPS_RATE_MS2 200 4 +1 1 GPS_RAW_DATA 0 2 +1 1 GPS_SAVE_CFG 2 2 +1 1 GPS_SBAS_MODE 2 2 +1 1 GPS_SBP_LOGMASK -256 4 +1 1 GPS_TYPE 1 2 +1 1 GPS_TYPE2 0 2 +1 1 GRIP_ENABLE 0 2 +1 1 GUID_OPTIONS 0 6 +1 1 INITIAL_MODE 0 2 +1 1 INS_ACC1_CALTEMP -300.000000000000000000 9 +1 1 INS_ACC2OFFS_X 0.001000000047497451 9 +1 1 INS_ACC2OFFS_Y 0.001000000047497451 9 +1 1 INS_ACC2OFFS_Z 0.001000000047497451 9 +1 1 INS_ACC2SCAL_X 1.001000046730041504 9 +1 1 INS_ACC2SCAL_Y 1.001000046730041504 9 +1 1 INS_ACC2SCAL_Z 1.001000046730041504 9 +1 1 INS_ACC2_CALTEMP -300.000000000000000000 9 +1 1 INS_ACC2_ID 2753036 6 +1 1 INS_ACC3OFFS_X 0.000000000000000000 9 +1 1 INS_ACC3OFFS_Y 0.000000000000000000 9 +1 1 INS_ACC3OFFS_Z 0.000000000000000000 9 +1 1 INS_ACC3SCAL_X 1.000000000000000000 9 +1 1 INS_ACC3SCAL_Y 1.000000000000000000 9 +1 1 INS_ACC3SCAL_Z 1.000000000000000000 9 +1 1 INS_ACC3_CALTEMP -300.000000000000000000 9 +1 1 INS_ACC3_ID 0 6 +1 1 INS_ACCEL_FILTER 20 4 +1 1 INS_ACCOFFS_X 0.001000000047497451 9 +1 1 INS_ACCOFFS_Y 0.001000000047497451 9 +1 1 INS_ACCOFFS_Z 0.001000000047497451 9 +1 1 INS_ACCSCAL_X 1.001000046730041504 9 +1 1 INS_ACCSCAL_Y 1.001000046730041504 9 +1 1 INS_ACCSCAL_Z 1.001000046730041504 9 +1 1 INS_ACC_BODYFIX 2 2 +1 1 INS_ACC_ID 2753028 6 +1 1 INS_ENABLE_MASK 127 2 +1 1 INS_FAST_SAMPLE 1 2 +1 1 INS_GYR1_CALTEMP 25.018318176269531250 9 +1 1 INS_GYR2OFFS_X 0.000038567155570490 9 +1 1 INS_GYR2OFFS_Y 0.000007562429345853 9 +1 1 INS_GYR2OFFS_Z 0.000013632265108754 9 +1 1 INS_GYR2_CALTEMP 25.018318176269531250 9 +1 1 INS_GYR2_ID 2752780 6 +1 1 INS_GYR3OFFS_X 0.000000000000000000 9 +1 1 INS_GYR3OFFS_Y 0.000000000000000000 9 +1 1 INS_GYR3OFFS_Z 0.000000000000000000 9 +1 1 INS_GYR3_CALTEMP -300.000000000000000000 9 +1 1 INS_GYR3_ID 0 6 +1 1 INS_GYROFFS_X 0.000031752835639054 9 +1 1 INS_GYROFFS_Y 0.000035681368899532 9 +1 1 INS_GYROFFS_Z 0.000018077400454786 9 +1 1 INS_GYRO_FILTER 20 4 +1 1 INS_GYRO_RATE 0 2 +1 1 INS_GYR_CAL 1 2 +1 1 INS_GYR_ID 2752772 6 +1 1 INS_HNTCH_ENABLE 0 2 +1 1 INS_LOG_BAT_CNT 1024 4 +1 1 INS_LOG_BAT_LGCT 32 4 +1 1 INS_LOG_BAT_LGIN 20 2 +1 1 INS_LOG_BAT_MASK 0 2 +1 1 INS_LOG_BAT_OPT 0 2 +1 1 INS_NOTCH_ENABLE 0 2 +1 1 INS_POS1_X 0.000000000000000000 9 +1 1 INS_POS1_Y 0.000000000000000000 9 +1 1 INS_POS1_Z 0.000000000000000000 9 +1 1 INS_POS2_X 0.000000000000000000 9 +1 1 INS_POS2_Y 0.000000000000000000 9 +1 1 INS_POS2_Z 0.000000000000000000 9 +1 1 INS_POS3_X 0.000000000000000000 9 +1 1 INS_POS3_Y 0.000000000000000000 9 +1 1 INS_POS3_Z 0.000000000000000000 9 +1 1 INS_STILL_THRESH 2.500000000000000000 9 +1 1 INS_TCAL1_ENABLE 0 2 +1 1 INS_TCAL2_ENABLE 0 2 +1 1 INS_TCAL3_ENABLE 0 2 +1 1 INS_TCAL_OPTIONS 0 6 +1 1 INS_TRIM_OPTION 1 2 +1 1 INS_USE 1 2 +1 1 INS_USE2 1 2 +1 1 INS_USE3 1 2 +1 1 LAND_ALT_LOW 1000 4 +1 1 LAND_REPOSITION 1 2 +1 1 LAND_SPEED 50 4 +1 1 LAND_SPEED_HIGH 0 4 +1 1 LGR_DEPLOY_ALT 0 4 +1 1 LGR_DEPLOY_PIN -1 2 +1 1 LGR_DEPLOY_POL 0 2 +1 1 LGR_OPTIONS 3 4 +1 1 LGR_RETRACT_ALT 0 4 +1 1 LGR_STARTUP 0 2 +1 1 LGR_WOW_PIN 8 2 +1 1 LGR_WOW_POL 1 2 +1 1 LOG_BACKEND_TYPE 1 2 +1 1 LOG_BITMASK 176126 6 +1 1 LOG_DISARMED 0 2 +1 1 LOG_FILE_BUFSIZE 200 4 +1 1 LOG_FILE_DSRMROT 0 2 +1 1 LOG_FILE_MB_FREE 500 4 +1 1 LOG_FILE_TIMEOUT 5 4 +1 1 LOG_MAV_BUFSIZE 8 2 +1 1 LOG_REPLAY 0 2 +1 1 LOIT_ACC_MAX 500.000000000000000000 9 +1 1 LOIT_ANG_MAX 0.000000000000000000 9 +1 1 LOIT_BRK_ACCEL 250.000000000000000000 9 +1 1 LOIT_BRK_DELAY 1.000000000000000000 9 +1 1 LOIT_BRK_JERK 500.000000000000000000 9 +1 1 LOIT_SPEED 1250.000000000000000000 9 +1 1 MIS_OPTIONS 0 4 +1 1 MIS_RESTART 0 2 +1 1 MIS_TOTAL 0 4 +1 1 MNT_TYPE 0 2 +1 1 MOT_BAT_CURR_MAX 0.000000000000000000 9 +1 1 MOT_BAT_CURR_TC 5.000000000000000000 9 +1 1 MOT_BAT_IDX 0 2 +1 1 MOT_BAT_VOLT_MAX 12.800000190734863281 9 +1 1 MOT_BAT_VOLT_MIN 9.600000381469726563 9 +1 1 MOT_BOOST_SCALE 0.000000000000000000 9 +1 1 MOT_HOVER_LEARN 2 2 +1 1 MOT_PWM_MAX 2000 4 +1 1 MOT_PWM_MIN 1000 4 +1 1 MOT_PWM_TYPE 0 2 +1 1 MOT_SAFE_DISARM 0 2 +1 1 MOT_SAFE_TIME 1.000000000000000000 9 +1 1 MOT_SLEW_DN_TIME 0.000000000000000000 9 +1 1 MOT_SLEW_UP_TIME 0.000000000000000000 9 +1 1 MOT_SPIN_ARM 0.100000001490116119 9 +1 1 MOT_SPIN_MAX 0.949999988079071045 9 +1 1 MOT_SPIN_MIN 0.150000005960464478 9 +1 1 MOT_SPOOL_TIME 0.500000000000000000 9 +1 1 MOT_THST_EXPO 0.649999976158142090 9 +1 1 MOT_THST_HOVER 0.389999985694885254 9 +1 1 MOT_YAW_HEADROOM 200 4 +1 1 MSP_OPTIONS 0 2 +1 1 MSP_OSD_NCELLS 0 2 +1 1 NTF_BUZZ_ENABLE 1 2 +1 1 NTF_BUZZ_ON_LVL 1 2 +1 1 NTF_BUZZ_PIN 0 2 +1 1 NTF_BUZZ_VOLUME 100 2 +1 1 NTF_DISPLAY_TYPE 0 2 +1 1 NTF_LED_BRIGHT 3 2 +1 1 NTF_LED_LEN 1 2 +1 1 NTF_LED_OVERRIDE 0 2 +1 1 NTF_LED_TYPES 199 6 +1 1 NTF_OREO_THEME 0 2 +1 1 OA_TYPE 0 2 +1 1 OSD_TYPE 0 2 +1 1 PHLD_BRAKE_ANGLE 3000 4 +1 1 PHLD_BRAKE_RATE 8 4 +1 1 PILOT_ACCEL_Z 250 4 +1 1 PILOT_SPEED_DN 0 4 +1 1 PILOT_SPEED_UP 250 4 +1 1 PILOT_THR_BHV 0 4 +1 1 PILOT_THR_FILT 0.000000000000000000 9 +1 1 PILOT_TKOFF_ALT 0.000000000000000000 9 +1 1 PLND_ENABLED 0 2 +1 1 PRX_FILT 0.250000000000000000 9 +1 1 PRX_IGN_ANG1 0 4 +1 1 PRX_IGN_ANG2 0 4 +1 1 PRX_IGN_ANG3 0 4 +1 1 PRX_IGN_ANG4 0 4 +1 1 PRX_IGN_ANG5 0 4 +1 1 PRX_IGN_ANG6 0 4 +1 1 PRX_IGN_GND 0 2 +1 1 PRX_IGN_WID1 0 2 +1 1 PRX_IGN_WID2 0 2 +1 1 PRX_IGN_WID3 0 2 +1 1 PRX_IGN_WID4 0 2 +1 1 PRX_IGN_WID5 0 2 +1 1 PRX_IGN_WID6 0 2 +1 1 PRX_LOG_RAW 0 2 +1 1 PRX_ORIENT 0 2 +1 1 PRX_TYPE 0 2 +1 1 PRX_YAW_CORR 0 4 +1 1 PSC_ACCZ_D 0.000000000000000000 9 +1 1 PSC_ACCZ_FF 0.000000000000000000 9 +1 1 PSC_ACCZ_FLTD 0.000000000000000000 9 +1 1 PSC_ACCZ_FLTE 20.000000000000000000 9 +1 1 PSC_ACCZ_FLTT 0.000000000000000000 9 +1 1 PSC_ACCZ_I 1.000000000000000000 9 +1 1 PSC_ACCZ_IMAX 800.000000000000000000 9 +1 1 PSC_ACCZ_P 0.500000000000000000 9 +1 1 PSC_ACCZ_SMAX 0.000000000000000000 9 +1 1 PSC_ACC_XY_FILT 2.000000000000000000 9 +1 1 PSC_ANGLE_MAX 0.000000000000000000 9 +1 1 PSC_POSXY_P 1.000000000000000000 9 +1 1 PSC_POSZ_P 1.000000000000000000 9 +1 1 PSC_VELXY_D 0.500000000000000000 9 +1 1 PSC_VELXY_D_FILT 5.000000000000000000 9 +1 1 PSC_VELXY_FF 0.000000000000000000 9 +1 1 PSC_VELXY_FILT 5.000000000000000000 9 +1 1 PSC_VELXY_I 1.000000000000000000 9 +1 1 PSC_VELXY_IMAX 1000.000000000000000000 9 +1 1 PSC_VELXY_P 2.000000000000000000 9 +1 1 PSC_VELZ_D 0.000000000000000000 9 +1 1 PSC_VELZ_FF 0.000000000000000000 9 +1 1 PSC_VELZ_FLTD 5.000000000000000000 9 +1 1 PSC_VELZ_FLTE 5.000000000000000000 9 +1 1 PSC_VELZ_I 0.000000000000000000 9 +1 1 PSC_VELZ_IMAX 1000.000000000000000000 9 +1 1 PSC_VELZ_P 5.000000000000000000 9 +1 1 RALLY_INCL_HOME 1 2 +1 1 RALLY_LIMIT_KM 0.300000011920928955 9 +1 1 RALLY_TOTAL 0 2 +1 1 RC10_DZ 0 4 +1 1 RC10_MAX 1900 4 +1 1 RC10_MIN 1100 4 +1 1 RC10_OPTION 0 4 +1 1 RC10_REVERSED 0 2 +1 1 RC10_TRIM 1500 4 +1 1 RC11_DZ 0 4 +1 1 RC11_MAX 1900 4 +1 1 RC11_MIN 1100 4 +1 1 RC11_OPTION 0 4 +1 1 RC11_REVERSED 0 2 +1 1 RC11_TRIM 1500 4 +1 1 RC12_DZ 0 4 +1 1 RC12_MAX 1900 4 +1 1 RC12_MIN 1100 4 +1 1 RC12_OPTION 0 4 +1 1 RC12_REVERSED 0 2 +1 1 RC12_TRIM 1500 4 +1 1 RC13_DZ 0 4 +1 1 RC13_MAX 1900 4 +1 1 RC13_MIN 1100 4 +1 1 RC13_OPTION 0 4 +1 1 RC13_REVERSED 0 2 +1 1 RC13_TRIM 1500 4 +1 1 RC14_DZ 0 4 +1 1 RC14_MAX 1900 4 +1 1 RC14_MIN 1100 4 +1 1 RC14_OPTION 0 4 +1 1 RC14_REVERSED 0 2 +1 1 RC14_TRIM 1500 4 +1 1 RC15_DZ 0 4 +1 1 RC15_MAX 1900 4 +1 1 RC15_MIN 1100 4 +1 1 RC15_OPTION 0 4 +1 1 RC15_REVERSED 0 2 +1 1 RC15_TRIM 1500 4 +1 1 RC16_DZ 0 4 +1 1 RC16_MAX 1900 4 +1 1 RC16_MIN 1100 4 +1 1 RC16_OPTION 0 4 +1 1 RC16_REVERSED 0 2 +1 1 RC16_TRIM 1500 4 +1 1 RC1_DZ 20 4 +1 1 RC1_MAX 2000 4 +1 1 RC1_MIN 1000 4 +1 1 RC1_OPTION 0 4 +1 1 RC1_REVERSED 0 2 +1 1 RC1_TRIM 1500 4 +1 1 RC2_DZ 20 4 +1 1 RC2_MAX 2000 4 +1 1 RC2_MIN 1000 4 +1 1 RC2_OPTION 0 4 +1 1 RC2_REVERSED 0 2 +1 1 RC2_TRIM 1500 4 +1 1 RC3_DZ 30 4 +1 1 RC3_MAX 2000 4 +1 1 RC3_MIN 1000 4 +1 1 RC3_OPTION 0 4 +1 1 RC3_REVERSED 0 2 +1 1 RC3_TRIM 1500 4 +1 1 RC4_DZ 20 4 +1 1 RC4_MAX 2000 4 +1 1 RC4_MIN 1000 4 +1 1 RC4_OPTION 0 4 +1 1 RC4_REVERSED 0 2 +1 1 RC4_TRIM 1500 4 +1 1 RC5_DZ 0 4 +1 1 RC5_MAX 2000 4 +1 1 RC5_MIN 1000 4 +1 1 RC5_OPTION 0 4 +1 1 RC5_REVERSED 0 2 +1 1 RC5_TRIM 1500 4 +1 1 RC6_DZ 0 4 +1 1 RC6_MAX 2000 4 +1 1 RC6_MIN 1000 4 +1 1 RC6_OPTION 0 4 +1 1 RC6_REVERSED 0 2 +1 1 RC6_TRIM 1500 4 +1 1 RC7_DZ 0 4 +1 1 RC7_MAX 2000 4 +1 1 RC7_MIN 1000 4 +1 1 RC7_OPTION 7 4 +1 1 RC7_REVERSED 0 2 +1 1 RC7_TRIM 1500 4 +1 1 RC8_DZ 0 4 +1 1 RC8_MAX 2000 4 +1 1 RC8_MIN 1000 4 +1 1 RC8_OPTION 0 4 +1 1 RC8_REVERSED 0 2 +1 1 RC8_TRIM 1500 4 +1 1 RC9_DZ 0 4 +1 1 RC9_MAX 1900 4 +1 1 RC9_MIN 1100 4 +1 1 RC9_OPTION 0 4 +1 1 RC9_REVERSED 0 2 +1 1 RC9_TRIM 1500 4 +1 1 RCMAP_PITCH 2 2 +1 1 RCMAP_ROLL 1 2 +1 1 RCMAP_THROTTLE 3 2 +1 1 RCMAP_YAW 4 2 +1 1 RC_OPTIONS 32 6 +1 1 RC_OVERRIDE_TIME 3.000000000000000000 9 +1 1 RC_PROTOCOLS 1 6 +1 1 RC_SPEED 490 4 +1 1 RELAY_DEFAULT 0 2 +1 1 RELAY_PIN 13 2 +1 1 RELAY_PIN2 -1 2 +1 1 RELAY_PIN3 -1 2 +1 1 RELAY_PIN4 -1 2 +1 1 RELAY_PIN5 -1 2 +1 1 RELAY_PIN6 -1 2 +1 1 RNGFND1_TYPE 0 2 +1 1 RNGFND2_TYPE 0 2 +1 1 RNGFND3_TYPE 0 2 +1 1 RNGFND4_TYPE 0 2 +1 1 RNGFND5_TYPE 0 2 +1 1 RNGFND6_TYPE 0 2 +1 1 RNGFND7_TYPE 0 2 +1 1 RNGFND8_TYPE 0 2 +1 1 RNGFND9_TYPE 0 2 +1 1 RNGFNDA_TYPE 0 2 +1 1 RNGFND_GAIN 0.800000011920928955 9 +1 1 RPM2_PIN -1 2 +1 1 RPM2_SCALING 1.000000000000000000 9 +1 1 RPM2_TYPE 0 2 +1 1 RPM_MAX 100000.000000000000000000 9 +1 1 RPM_MIN 10.000000000000000000 9 +1 1 RPM_MIN_QUAL 0.500000000000000000 9 +1 1 RPM_PIN 54 2 +1 1 RPM_SCALING 1.000000000000000000 9 +1 1 RPM_TYPE 0 2 +1 1 RSSI_TYPE 0 2 +1 1 RTL_ALT 1500 4 +1 1 RTL_ALT_FINAL 0 4 +1 1 RTL_ALT_TYPE 0 2 +1 1 RTL_CLIMB_MIN 0 4 +1 1 RTL_CONE_SLOPE 3.000000000000000000 9 +1 1 RTL_LOIT_TIME 5000 6 +1 1 RTL_OPTIONS 0 6 +1 1 RTL_SPEED 0 4 +1 1 SCHED_DEBUG 0 2 +1 1 SCHED_LOOP_RATE 400 4 +1 1 SCHED_OPTIONS 0 2 +1 1 SCR_ENABLE 0 2 +1 1 SERIAL0_BAUD 115 6 +1 1 SERIAL0_PROTOCOL 2 2 +1 1 SERIAL1_BAUD 57 6 +1 1 SERIAL1_OPTIONS 0 4 +1 1 SERIAL1_PROTOCOL 2 2 +1 1 SERIAL2_BAUD 57 6 +1 1 SERIAL2_OPTIONS 0 4 +1 1 SERIAL2_PROTOCOL 2 2 +1 1 SERIAL3_BAUD 38 6 +1 1 SERIAL3_OPTIONS 0 4 +1 1 SERIAL3_PROTOCOL 5 2 +1 1 SERIAL4_BAUD 38 6 +1 1 SERIAL4_OPTIONS 0 4 +1 1 SERIAL4_PROTOCOL 5 2 +1 1 SERIAL5_BAUD 57 6 +1 1 SERIAL5_OPTIONS 0 4 +1 1 SERIAL5_PROTOCOL -1 2 +1 1 SERIAL6_BAUD 57 6 +1 1 SERIAL6_OPTIONS 0 4 +1 1 SERIAL6_PROTOCOL -1 2 +1 1 SERIAL7_BAUD 57 6 +1 1 SERIAL7_OPTIONS 0 4 +1 1 SERIAL7_PROTOCOL -1 2 +1 1 SERIAL_PASS1 0 2 +1 1 SERIAL_PASS2 -1 2 +1 1 SERIAL_PASSTIMO 15 2 +1 1 SERVO10_FUNCTION 0 4 +1 1 SERVO10_MAX 1900 4 +1 1 SERVO10_MIN 1100 4 +1 1 SERVO10_REVERSED 0 2 +1 1 SERVO10_TRIM 1500 4 +1 1 SERVO11_FUNCTION 0 4 +1 1 SERVO11_MAX 1900 4 +1 1 SERVO11_MIN 1100 4 +1 1 SERVO11_REVERSED 0 2 +1 1 SERVO11_TRIM 1500 4 +1 1 SERVO12_FUNCTION 0 4 +1 1 SERVO12_MAX 1900 4 +1 1 SERVO12_MIN 1100 4 +1 1 SERVO12_REVERSED 0 2 +1 1 SERVO12_TRIM 1500 4 +1 1 SERVO13_FUNCTION 0 4 +1 1 SERVO13_MAX 1900 4 +1 1 SERVO13_MIN 1100 4 +1 1 SERVO13_REVERSED 0 2 +1 1 SERVO13_TRIM 1500 4 +1 1 SERVO14_FUNCTION 0 4 +1 1 SERVO14_MAX 1900 4 +1 1 SERVO14_MIN 1100 4 +1 1 SERVO14_REVERSED 0 2 +1 1 SERVO14_TRIM 1500 4 +1 1 SERVO15_FUNCTION 0 4 +1 1 SERVO15_MAX 1900 4 +1 1 SERVO15_MIN 1100 4 +1 1 SERVO15_REVERSED 0 2 +1 1 SERVO15_TRIM 1500 4 +1 1 SERVO16_FUNCTION 0 4 +1 1 SERVO16_MAX 1900 4 +1 1 SERVO16_MIN 1100 4 +1 1 SERVO16_REVERSED 0 2 +1 1 SERVO16_TRIM 1500 4 +1 1 SERVO1_FUNCTION 33 4 +1 1 SERVO1_MAX 1900 4 +1 1 SERVO1_MIN 1100 4 +1 1 SERVO1_REVERSED 0 2 +1 1 SERVO1_TRIM 1500 4 +1 1 SERVO2_FUNCTION 34 4 +1 1 SERVO2_MAX 1900 4 +1 1 SERVO2_MIN 1100 4 +1 1 SERVO2_REVERSED 0 2 +1 1 SERVO2_TRIM 1500 4 +1 1 SERVO3_FUNCTION 35 4 +1 1 SERVO3_MAX 1900 4 +1 1 SERVO3_MIN 1100 4 +1 1 SERVO3_REVERSED 0 2 +1 1 SERVO3_TRIM 1500 4 +1 1 SERVO4_FUNCTION 36 4 +1 1 SERVO4_MAX 1900 4 +1 1 SERVO4_MIN 1100 4 +1 1 SERVO4_REVERSED 0 2 +1 1 SERVO4_TRIM 1500 4 +1 1 SERVO5_FUNCTION 0 4 +1 1 SERVO5_MAX 1900 4 +1 1 SERVO5_MIN 1100 4 +1 1 SERVO5_REVERSED 0 2 +1 1 SERVO5_TRIM 1500 4 +1 1 SERVO6_FUNCTION 0 4 +1 1 SERVO6_MAX 1900 4 +1 1 SERVO6_MIN 1100 4 +1 1 SERVO6_REVERSED 0 2 +1 1 SERVO6_TRIM 1500 4 +1 1 SERVO7_FUNCTION 0 4 +1 1 SERVO7_MAX 1900 4 +1 1 SERVO7_MIN 1100 4 +1 1 SERVO7_REVERSED 0 2 +1 1 SERVO7_TRIM 1500 4 +1 1 SERVO8_FUNCTION 0 4 +1 1 SERVO8_MAX 1900 4 +1 1 SERVO8_MIN 1100 4 +1 1 SERVO8_REVERSED 0 2 +1 1 SERVO8_TRIM 1500 4 +1 1 SERVO9_FUNCTION 0 4 +1 1 SERVO9_MAX 1900 4 +1 1 SERVO9_MIN 1100 4 +1 1 SERVO9_REVERSED 0 2 +1 1 SERVO9_TRIM 1500 4 +1 1 SERVO_DSHOT_RATE 0 2 +1 1 SERVO_RATE 50 4 +1 1 SERVO_ROB_POSMAX 4095 6 +1 1 SERVO_ROB_POSMIN 0 6 +1 1 SERVO_SBUS_RATE 50 4 +1 1 SERVO_VOLZ_MASK 0 6 +1 1 SID_AXIS 0 2 +1 1 SIMPLE 0 2 +1 1 SIM_ACC1_BIAS_X 0.000000000000000000 9 +1 1 SIM_ACC1_BIAS_Y 0.000000000000000000 9 +1 1 SIM_ACC1_BIAS_Z 0.000000000000000000 9 +1 1 SIM_ACC1_RND 0.000000000000000000 9 +1 1 SIM_ACC1_SCAL_X 0.000000000000000000 9 +1 1 SIM_ACC1_SCAL_Y 0.000000000000000000 9 +1 1 SIM_ACC1_SCAL_Z 0.000000000000000000 9 +1 1 SIM_ACC2_BIAS_X 0.000000000000000000 9 +1 1 SIM_ACC2_BIAS_Y 0.000000000000000000 9 +1 1 SIM_ACC2_BIAS_Z 0.000000000000000000 9 +1 1 SIM_ACC2_RND 0.000000000000000000 9 +1 1 SIM_ACC2_SCAL_X 0.000000000000000000 9 +1 1 SIM_ACC2_SCAL_Y 0.000000000000000000 9 +1 1 SIM_ACC2_SCAL_Z 0.000000000000000000 9 +1 1 SIM_ACC3_BIAS_X 0.000000000000000000 9 +1 1 SIM_ACC3_BIAS_Y 0.000000000000000000 9 +1 1 SIM_ACC3_BIAS_Z 0.000000000000000000 9 +1 1 SIM_ACC3_RND 0.000000000000000000 9 +1 1 SIM_ACC3_SCAL_X 0.000000000000000000 9 +1 1 SIM_ACC3_SCAL_Y 0.000000000000000000 9 +1 1 SIM_ACC3_SCAL_Z 0.000000000000000000 9 +1 1 SIM_ACCEL1_FAIL 0.000000000000000000 9 +1 1 SIM_ACCEL2_FAIL 0.000000000000000000 9 +1 1 SIM_ACCEL3_FAIL 0.000000000000000000 9 +1 1 SIM_ACC_FAIL_MSK 0 2 +1 1 SIM_ACC_TRIM_X 0.000000000000000000 9 +1 1 SIM_ACC_TRIM_Y 0.000000000000000000 9 +1 1 SIM_ACC_TRIM_Z 0.000000000000000000 9 +1 1 SIM_ADSB_ALT 1000.000000000000000000 9 +1 1 SIM_ADSB_COUNT -1 4 +1 1 SIM_ADSB_RADIUS 10000.000000000000000000 9 +1 1 SIM_ADSB_TX 0 2 +1 1 SIM_ARSPD2_FAIL 0.000000000000000000 9 +1 1 SIM_ARSPD2_FAILP 0.000000000000000000 9 +1 1 SIM_ARSPD2_OFS 2013.000000000000000000 9 +1 1 SIM_ARSPD2_PITOT 0.000000000000000000 9 +1 1 SIM_ARSPD2_RND 2.000000000000000000 9 +1 1 SIM_ARSPD_FAIL 0.000000000000000000 9 +1 1 SIM_ARSPD_FAILP 0.000000000000000000 9 +1 1 SIM_ARSPD_OFS 2013.000000000000000000 9 +1 1 SIM_ARSPD_PITOT 0.000000000000000000 9 +1 1 SIM_ARSPD_RND 2.000000000000000000 9 +1 1 SIM_ARSPD_SIGN 0 2 +1 1 SIM_BAR2_DELAY 0 4 +1 1 SIM_BAR2_DISABLE 0 2 +1 1 SIM_BAR2_DRIFT 0.000000000000000000 9 +1 1 SIM_BAR2_FREEZE 0 2 +1 1 SIM_BAR2_GLITCH 0.000000000000000000 9 +1 1 SIM_BAR2_RND 0.200000002980232239 9 +1 1 SIM_BAR2_WCF_BAK 0.000000000000000000 9 +1 1 SIM_BAR2_WCF_FWD 0.000000000000000000 9 +1 1 SIM_BAR2_WCF_LFT 0.000000000000000000 9 +1 1 SIM_BAR2_WCF_RGT 0.000000000000000000 9 +1 1 SIM_BAR3_DELAY 0 4 +1 1 SIM_BAR3_DISABLE 0 2 +1 1 SIM_BAR3_DRIFT 0.000000000000000000 9 +1 1 SIM_BAR3_FREEZE 0 2 +1 1 SIM_BAR3_GLITCH 0.000000000000000000 9 +1 1 SIM_BAR3_RND 0.200000002980232239 9 +1 1 SIM_BAR3_WCF_BAK 0.000000000000000000 9 +1 1 SIM_BAR3_WCF_FWD 0.000000000000000000 9 +1 1 SIM_BAR3_WCF_LFT 0.000000000000000000 9 +1 1 SIM_BAR3_WCF_RGT 0.000000000000000000 9 +1 1 SIM_BARO_COUNT 2 2 +1 1 SIM_BARO_DELAY 0 4 +1 1 SIM_BARO_DISABLE 0 2 +1 1 SIM_BARO_DRIFT 0.000000000000000000 9 +1 1 SIM_BARO_FREEZE 0 2 +1 1 SIM_BARO_GLITCH 0.000000000000000000 9 +1 1 SIM_BARO_RND 0.000000000000000000 9 +1 1 SIM_BARO_WCF_BAK 0.000000000000000000 9 +1 1 SIM_BARO_WCF_FWD 0.000000000000000000 9 +1 1 SIM_BARO_WCF_LFT 0.000000000000000000 9 +1 1 SIM_BARO_WCF_RGT 0.000000000000000000 9 +1 1 SIM_BATT_CAP_AH 0.000000000000000000 9 +1 1 SIM_BATT_VOLTAGE 12.600000381469726563 9 +1 1 SIM_BAUDLIMIT_EN 0 2 +1 1 SIM_BZ_ENABLE 0 2 +1 1 SIM_BZ_PIN 0 2 +1 1 SIM_DRIFT_SPEED 0.050000000745058060 9 +1 1 SIM_DRIFT_TIME 5.000000000000000000 9 +1 1 SIM_EFI_TYPE 0 2 +1 1 SIM_ENGINE_FAIL 0 2 +1 1 SIM_ENGINE_MUL 1.000000000000000000 9 +1 1 SIM_FLOAT_EXCEPT 1 2 +1 1 SIM_FLOW_DELAY 0 2 +1 1 SIM_FLOW_ENABLE 0 2 +1 1 SIM_FLOW_POS_X 0.000000000000000000 9 +1 1 SIM_FLOW_POS_Y 0.000000000000000000 9 +1 1 SIM_FLOW_POS_Z 0.000000000000000000 9 +1 1 SIM_FLOW_RATE 10 4 +1 1 SIM_FLOW_RND 0.050000000745058060 9 +1 1 SIM_GND_BEHAV -1 2 +1 1 SIM_GPS2_ACC 0.300000011920928955 9 +1 1 SIM_GPS2_ALT_OFS 0 4 +1 1 SIM_GPS2_BYTELOS 0.000000000000000000 9 +1 1 SIM_GPS2_DELAY 1 2 +1 1 SIM_GPS2_DISABLE 1 2 +1 1 SIM_GPS2_DRFTALT 0.000000000000000000 9 +1 1 SIM_GPS2_GLTCH_X 0.000000000000000000 9 +1 1 SIM_GPS2_GLTCH_Y 0.000000000000000000 9 +1 1 SIM_GPS2_GLTCH_Z 0.000000000000000000 9 +1 1 SIM_GPS2_HDG 0 2 +1 1 SIM_GPS2_HZ 5 2 +1 1 SIM_GPS2_LCKTIME 0 4 +1 1 SIM_GPS2_NOISE 0.000000000000000000 9 +1 1 SIM_GPS2_NUMSATS 10 2 +1 1 SIM_GPS2_POS_X 0.000000000000000000 9 +1 1 SIM_GPS2_POS_Y 0.000000000000000000 9 +1 1 SIM_GPS2_POS_Z 0.000000000000000000 9 +1 1 SIM_GPS2_TYPE 1 2 +1 1 SIM_GPS2_VERR_X 0.000000000000000000 9 +1 1 SIM_GPS2_VERR_Y 0.000000000000000000 9 +1 1 SIM_GPS2_VERR_Z 0.000000000000000000 9 +1 1 SIM_GPS_ACC 0.300000011920928955 9 +1 1 SIM_GPS_ALT_OFS 0 4 +1 1 SIM_GPS_BYTELOSS 0.000000000000000000 9 +1 1 SIM_GPS_DELAY 1 2 +1 1 SIM_GPS_DISABLE 0 2 +1 1 SIM_GPS_DRIFTALT 0.000000000000000000 9 +1 1 SIM_GPS_GLITCH_X 0.000000000000000000 9 +1 1 SIM_GPS_GLITCH_Y 0.000000000000000000 9 +1 1 SIM_GPS_GLITCH_Z 0.000000000000000000 9 +1 1 SIM_GPS_HDG 0 2 +1 1 SIM_GPS_HZ 5 2 +1 1 SIM_GPS_LOCKTIME 0 4 +1 1 SIM_GPS_NOISE 0.000000000000000000 9 +1 1 SIM_GPS_NUMSATS 10 2 +1 1 SIM_GPS_POS_X 0.000000000000000000 9 +1 1 SIM_GPS_POS_Y 0.000000000000000000 9 +1 1 SIM_GPS_POS_Z 0.000000000000000000 9 +1 1 SIM_GPS_TYPE 1 2 +1 1 SIM_GPS_VERR_X 0.000000000000000000 9 +1 1 SIM_GPS_VERR_Y 0.000000000000000000 9 +1 1 SIM_GPS_VERR_Z 0.000000000000000000 9 +1 1 SIM_GRPE_ENABLE 0 2 +1 1 SIM_GRPE_PIN -1 2 +1 1 SIM_GRPS_ENABLE 0 2 +1 1 SIM_GRPS_GRAB 2000 4 +1 1 SIM_GRPS_PIN -1 2 +1 1 SIM_GRPS_RELEASE 1000 4 +1 1 SIM_GRPS_REVERSE 0 2 +1 1 SIM_GYR1_RND 0.000000000000000000 9 +1 1 SIM_GYR1_SCALE_X 0.000000000000000000 9 +1 1 SIM_GYR1_SCALE_Y 0.000000000000000000 9 +1 1 SIM_GYR1_SCALE_Z 0.000000000000000000 9 +1 1 SIM_GYR2_RND 0.000000000000000000 9 +1 1 SIM_GYR2_SCALE_X 0.000000000000000000 9 +1 1 SIM_GYR2_SCALE_Y 0.000000000000000000 9 +1 1 SIM_GYR2_SCALE_Z 0.000000000000000000 9 +1 1 SIM_GYR3_RND 0.000000000000000000 9 +1 1 SIM_GYR3_SCALE_X 0.000000000000000000 9 +1 1 SIM_GYR3_SCALE_Y 0.000000000000000000 9 +1 1 SIM_GYR3_SCALE_Z 0.000000000000000000 9 +1 1 SIM_GYR_FAIL_MSK 0 2 +1 1 SIM_IE24_ENABLE 0 2 +1 1 SIM_IE24_ERROR 0 6 +1 1 SIM_IE24_STATE -1 2 +1 1 SIM_IMUT1_ENABLE 0 2 +1 1 SIM_IMUT2_ENABLE 0 2 +1 1 SIM_IMUT3_ENABLE 0 2 +1 1 SIM_IMUT_END 45.000000000000000000 9 +1 1 SIM_IMUT_FIXED 0.000000000000000000 9 +1 1 SIM_IMUT_START 25.000000000000000000 9 +1 1 SIM_IMUT_TCONST 300.000000000000000000 9 +1 1 SIM_IMU_COUNT 2 2 +1 1 SIM_IMU_POS_X 0.000000000000000000 9 +1 1 SIM_IMU_POS_Y 0.000000000000000000 9 +1 1 SIM_IMU_POS_Z 0.000000000000000000 9 +1 1 SIM_INS_THR_MIN 0.100000001490116119 9 +1 1 SIM_LED_LAYOUT 0 2 +1 1 SIM_LOOP_DELAY 0 6 +1 1 SIM_MAG1_DEVID 97539 6 +1 1 SIM_MAG1_FAIL 0 2 +1 1 SIM_MAG1_SCALING 1.000000000000000000 9 +1 1 SIM_MAG2_DEVID 131874 6 +1 1 SIM_MAG2_DIA_X 0.000000000000000000 9 +1 1 SIM_MAG2_DIA_Y 0.000000000000000000 9 +1 1 SIM_MAG2_DIA_Z 0.000000000000000000 9 +1 1 SIM_MAG2_FAIL 0 2 +1 1 SIM_MAG2_ODI_X 0.000000000000000000 9 +1 1 SIM_MAG2_ODI_Y 0.000000000000000000 9 +1 1 SIM_MAG2_ODI_Z 0.000000000000000000 9 +1 1 SIM_MAG2_OFS_X 5.000000000000000000 9 +1 1 SIM_MAG2_OFS_Y 13.000000000000000000 9 +1 1 SIM_MAG2_OFS_Z -18.000000000000000000 9 +1 1 SIM_MAG2_ORIENT 0 2 +1 1 SIM_MAG2_SCALING 1.000000000000000000 9 +1 1 SIM_MAG3_DEVID 263178 6 +1 1 SIM_MAG3_DIA_X 0.000000000000000000 9 +1 1 SIM_MAG3_DIA_Y 0.000000000000000000 9 +1 1 SIM_MAG3_DIA_Z 0.000000000000000000 9 +1 1 SIM_MAG3_FAIL 0 2 +1 1 SIM_MAG3_ODI_X 0.000000000000000000 9 +1 1 SIM_MAG3_ODI_Y 0.000000000000000000 9 +1 1 SIM_MAG3_ODI_Z 0.000000000000000000 9 +1 1 SIM_MAG3_OFS_X 5.000000000000000000 9 +1 1 SIM_MAG3_OFS_Y 13.000000000000000000 9 +1 1 SIM_MAG3_OFS_Z -18.000000000000000000 9 +1 1 SIM_MAG3_ORIENT 0 2 +1 1 SIM_MAG3_SCALING 1.000000000000000000 9 +1 1 SIM_MAG4_DEVID 97283 6 +1 1 SIM_MAG5_DEVID 97795 6 +1 1 SIM_MAG6_DEVID 98051 6 +1 1 SIM_MAG7_DEVID 0 6 +1 1 SIM_MAG8_DEVID 0 6 +1 1 SIM_MAG_ALY_HGT 1.000000000000000000 9 +1 1 SIM_MAG_ALY_X 0.000000000000000000 9 +1 1 SIM_MAG_ALY_Y 0.000000000000000000 9 +1 1 SIM_MAG_ALY_Z 0.000000000000000000 9 +1 1 SIM_MAG_DELAY 0 4 +1 1 SIM_MAG_DIA_X 0.000000000000000000 9 +1 1 SIM_MAG_DIA_Y 0.000000000000000000 9 +1 1 SIM_MAG_DIA_Z 0.000000000000000000 9 +1 1 SIM_MAG_MOT_X 0.000000000000000000 9 +1 1 SIM_MAG_MOT_Y 0.000000000000000000 9 +1 1 SIM_MAG_MOT_Z 0.000000000000000000 9 +1 1 SIM_MAG_ODI_X 0.000000000000000000 9 +1 1 SIM_MAG_ODI_Y 0.000000000000000000 9 +1 1 SIM_MAG_ODI_Z 0.000000000000000000 9 +1 1 SIM_MAG_OFS_X 5.000000000000000000 9 +1 1 SIM_MAG_OFS_Y 13.000000000000000000 9 +1 1 SIM_MAG_OFS_Z -18.000000000000000000 9 +1 1 SIM_MAG_ORIENT 0 2 +1 1 SIM_MAG_RND 0.000000000000000000 9 +1 1 SIM_ODOM_ENABLE 0 2 +1 1 SIM_OPOS_ALT 584.000000000000000000 9 +1 1 SIM_OPOS_HDG 353.000000000000000000 9 +1 1 SIM_OPOS_LAT -35.363262176513671875 9 +1 1 SIM_OPOS_LNG 149.165237426757812500 9 +1 1 SIM_PARA_ENABLE 0 2 +1 1 SIM_PARA_PIN -1 2 +1 1 SIM_PIN_MASK 0 4 +1 1 SIM_PLD_ALT_LMT 15.000000000000000000 9 +1 1 SIM_PLD_DIST_LMT 10.000000000000000000 9 +1 1 SIM_PLD_ENABLE 0 2 +1 1 SIM_PLD_HEIGHT 0.000000000000000000 9 +1 1 SIM_PLD_LAT 0.000000000000000000 9 +1 1 SIM_PLD_LON 0.000000000000000000 9 +1 1 SIM_PLD_RATE 100 6 +1 1 SIM_PLD_TYPE 0 2 +1 1 SIM_PLD_YAW 0 4 +1 1 SIM_RATE_HZ 1200 4 +1 1 SIM_RC_CHANCOUNT 16 2 +1 1 SIM_RC_FAIL 0 2 +1 1 SIM_RICH_CTRL -1 2 +1 1 SIM_RICH_ENABLE 0 2 +1 1 SIM_SAFETY_STATE 0 2 +1 1 SIM_SAIL_TYPE 0 2 +1 1 SIM_SERVO_SPEED 0.140000000596046448 9 +1 1 SIM_SHIP_DSIZE 10.000000000000000000 9 +1 1 SIM_SHIP_ENABLE 0 2 +1 1 SIM_SHIP_PSIZE 1000.000000000000000000 9 +1 1 SIM_SHIP_SPEED 3.000000000000000000 9 +1 1 SIM_SHIP_SYSID 17 2 +1 1 SIM_SHOVE_TIME 0 6 +1 1 SIM_SHOVE_X 0.000000000000000000 9 +1 1 SIM_SHOVE_Y 0.000000000000000000 9 +1 1 SIM_SHOVE_Z 0.000000000000000000 9 +1 1 SIM_SONAR_GLITCH 0.000000000000000000 9 +1 1 SIM_SONAR_POS_X 0.000000000000000000 9 +1 1 SIM_SONAR_POS_Y 0.000000000000000000 9 +1 1 SIM_SONAR_POS_Z 0.000000000000000000 9 +1 1 SIM_SONAR_RND 0.000000000000000000 9 +1 1 SIM_SONAR_SCALE 12.121199607849121094 9 +1 1 SIM_SPEEDUP 1.000000000000000000 9 +1 1 SIM_SPR_ENABLE 0 2 +1 1 SIM_SPR_PUMP -1 2 +1 1 SIM_SPR_SPIN -1 2 +1 1 SIM_TA_ENABLE 1 2 +1 1 SIM_TEMP_BFACTOR 0.000000000000000000 9 +1 1 SIM_TEMP_FLIGHT 35.000000000000000000 9 +1 1 SIM_TEMP_START 25.000000000000000000 9 +1 1 SIM_TEMP_TCONST 30.000000000000000000 9 +1 1 SIM_TERRAIN 1 2 +1 1 SIM_THML_SCENARI 0 2 +1 1 SIM_TIDE_DIR 0.000000000000000000 9 +1 1 SIM_TIDE_SPEED 0.000000000000000000 9 +1 1 SIM_TWIST_TIME 0 6 +1 1 SIM_TWIST_X 0.000000000000000000 9 +1 1 SIM_TWIST_Y 0.000000000000000000 9 +1 1 SIM_TWIST_Z 0.000000000000000000 9 +1 1 SIM_VIB_FREQ_X 0.000000000000000000 9 +1 1 SIM_VIB_FREQ_Y 0.000000000000000000 9 +1 1 SIM_VIB_FREQ_Z 0.000000000000000000 9 +1 1 SIM_VIB_MOT_MAX 0.000000000000000000 9 +1 1 SIM_VIB_MOT_MULT 1.000000000000000000 9 +1 1 SIM_VICON_FAIL 0 2 +1 1 SIM_VICON_GLIT_X 0.000000000000000000 9 +1 1 SIM_VICON_GLIT_Y 0.000000000000000000 9 +1 1 SIM_VICON_GLIT_Z 0.000000000000000000 9 +1 1 SIM_VICON_POS_X 0.000000000000000000 9 +1 1 SIM_VICON_POS_Y 0.000000000000000000 9 +1 1 SIM_VICON_POS_Z 0.000000000000000000 9 +1 1 SIM_VICON_TMASK 3 2 +1 1 SIM_VICON_VGLI_X 0.000000000000000000 9 +1 1 SIM_VICON_VGLI_Y 0.000000000000000000 9 +1 1 SIM_VICON_VGLI_Z 0.000000000000000000 9 +1 1 SIM_VICON_YAW 0 4 +1 1 SIM_VICON_YAWERR 0 4 +1 1 SIM_WAVE_AMP 0.500000000000000000 9 +1 1 SIM_WAVE_DIR 0.000000000000000000 9 +1 1 SIM_WAVE_ENABLE 0 2 +1 1 SIM_WAVE_LENGTH 10.000000000000000000 9 +1 1 SIM_WAVE_SPEED 0.500000000000000000 9 +1 1 SIM_WIND_DELAY 0 4 +1 1 SIM_WIND_DIR 180.000000000000000000 9 +1 1 SIM_WIND_DIR_Z 0.000000000000000000 9 +1 1 SIM_WIND_SPD 0.000000000000000000 9 +1 1 SIM_WIND_T 0 2 +1 1 SIM_WIND_TURB 0.000000000000000000 9 +1 1 SIM_WIND_T_ALT 60.000000000000000000 9 +1 1 SIM_WIND_T_COEF 0.009999999776482582 9 +1 1 SIM_WOW_PIN -1 2 +1 1 SPRAY_ENABLE 0 2 +1 1 SR0_ADSB 4 4 +1 1 SR0_EXTRA1 4 4 +1 1 SR0_EXTRA2 4 4 +1 1 SR0_EXTRA3 4 4 +1 1 SR0_EXT_STAT 4 4 +1 1 SR0_PARAMS 0 4 +1 1 SR0_POSITION 4 4 +1 1 SR0_RAW_CTRL 4 4 +1 1 SR0_RAW_SENS 4 4 +1 1 SR0_RC_CHAN 4 4 +1 1 SR1_ADSB 0 4 +1 1 SR1_EXTRA1 0 4 +1 1 SR1_EXTRA2 0 4 +1 1 SR1_EXTRA3 0 4 +1 1 SR1_EXT_STAT 0 4 +1 1 SR1_PARAMS 0 4 +1 1 SR1_POSITION 0 4 +1 1 SR1_RAW_CTRL 0 4 +1 1 SR1_RAW_SENS 0 4 +1 1 SR1_RC_CHAN 0 4 +1 1 SR2_ADSB 0 4 +1 1 SR2_EXTRA1 0 4 +1 1 SR2_EXTRA2 0 4 +1 1 SR2_EXTRA3 0 4 +1 1 SR2_EXT_STAT 0 4 +1 1 SR2_PARAMS 0 4 +1 1 SR2_POSITION 0 4 +1 1 SR2_RAW_CTRL 0 4 +1 1 SR2_RAW_SENS 0 4 +1 1 SR2_RC_CHAN 0 4 +1 1 SR3_ADSB 0 4 +1 1 SR3_EXTRA1 0 4 +1 1 SR3_EXTRA2 0 4 +1 1 SR3_EXTRA3 0 4 +1 1 SR3_EXT_STAT 0 4 +1 1 SR3_PARAMS 0 4 +1 1 SR3_POSITION 0 4 +1 1 SR3_RAW_CTRL 0 4 +1 1 SR3_RAW_SENS 0 4 +1 1 SR3_RC_CHAN 0 4 +1 1 SR4_ADSB 0 4 +1 1 SR4_EXTRA1 0 4 +1 1 SR4_EXTRA2 0 4 +1 1 SR4_EXTRA3 0 4 +1 1 SR4_EXT_STAT 0 4 +1 1 SR4_PARAMS 0 4 +1 1 SR4_POSITION 0 4 +1 1 SR4_RAW_CTRL 0 4 +1 1 SR4_RAW_SENS 0 4 +1 1 SR4_RC_CHAN 0 4 +1 1 SR5_ADSB 0 4 +1 1 SR5_EXTRA1 0 4 +1 1 SR5_EXTRA2 0 4 +1 1 SR5_EXTRA3 0 4 +1 1 SR5_EXT_STAT 0 4 +1 1 SR5_PARAMS 0 4 +1 1 SR5_POSITION 0 4 +1 1 SR5_RAW_CTRL 0 4 +1 1 SR5_RAW_SENS 0 4 +1 1 SR5_RC_CHAN 0 4 +1 1 SR6_ADSB 0 4 +1 1 SR6_EXTRA1 0 4 +1 1 SR6_EXTRA2 0 4 +1 1 SR6_EXTRA3 0 4 +1 1 SR6_EXT_STAT 0 4 +1 1 SR6_PARAMS 0 4 +1 1 SR6_POSITION 0 4 +1 1 SR6_RAW_CTRL 0 4 +1 1 SR6_RAW_SENS 0 4 +1 1 SR6_RC_CHAN 0 4 +1 1 SRTL_ACCURACY 2.000000000000000000 9 +1 1 SRTL_OPTIONS 0 6 +1 1 SRTL_POINTS 300 4 +1 1 STAT_BOOTCNT 1 4 +1 1 STAT_FLTTIME 0 6 +1 1 STAT_RESET 168462400 6 +1 1 STAT_RUNTIME 47989 6 +1 1 SUPER_SIMPLE 0 2 +1 1 SYSID_ENFORCE 0 2 +1 1 SYSID_MYGCS 255 4 +1 1 SYSID_THISMAV 1 4 +1 1 TCAL_ENABLED 0 2 +1 1 TELEM_DELAY 0 2 +1 1 TERRAIN_ENABLE 1 2 +1 1 TERRAIN_OPTIONS 0 4 +1 1 TERRAIN_SPACING 100 4 +1 1 THROW_MOT_START 0 2 +1 1 THROW_NEXTMODE 18 2 +1 1 THROW_TYPE 0 2 +1 1 THR_DZ 100 4 +1 1 TUNE 0 2 +1 1 TUNE_MAX 0.000000000000000000 9 +1 1 TUNE_MIN 0.000000000000000000 9 +1 1 VISO_TYPE 0 2 +1 1 VTX_ENABLE 0 2 +1 1 WINCH_TYPE 0 2 +1 1 WPNAV_ACCEL 250.000000000000000000 9 +1 1 WPNAV_ACCEL_Z 100.000000000000000000 9 +1 1 WPNAV_JERK 1.000000000000000000 9 +1 1 WPNAV_RADIUS 200.000000000000000000 9 +1 1 WPNAV_RFND_USE 1 2 +1 1 WPNAV_SPEED 1000.000000000000000000 9 +1 1 WPNAV_SPEED_DN 150.000000000000000000 9 +1 1 WPNAV_SPEED_UP 250.000000000000000000 9 +1 1 WP_NAVALT_MIN 0.000000000000000000 9 +1 1 WP_YAW_BEHAVIOR 2 2 +1 1 ZIGZ_AUTO_ENABLE 0 2 diff --git a/mavros/test/mavros_py/testdata/simple.plan b/mavros/test/mavros_py/testdata/simple.plan new file mode 100644 index 000000000..de88e50e1 --- /dev/null +++ b/mavros/test/mavros_py/testdata/simple.plan @@ -0,0 +1,271 @@ +{ + "fileType": "Plan", + "geoFence": { + "breachReturn": [ + -35.36185308085855, + 149.1639402537287, + 50 + ], + "circles": [ + { + "circle": { + "center": [ + -35.36218563059447, + 149.16461502741134 + ], + "radius": 463.7075269143206 + }, + "inclusion": true, + "version": 1 + } + ], + "polygons": [ + { + "inclusion": true, + "polygon": [ + [ + -35.35577972719051, + 149.15033147518085 + ], + [ + -35.35499865210675, + 149.1715783983837 + ], + [ + -35.365094075579435, + 149.1694684777504 + ], + [ + -35.360859582008764, + 149.1497179521595 + ] + ], + "version": 1 + } + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 15, + "firmwareType": 3, + "hoverSpeed": 5, + "items": [ + { + "AMSLAltAboveTerrain": null, + "Altitude": 50, + "AltitudeMode": 1, + "autoContinue": true, + "command": 22, + "doJumpId": 1, + "frame": 3, + "params": [ + 15, + 0, + 0, + null, + 0, + 0, + 50 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 50, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 2, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + -35.36001036, + 149.16537679, + 50 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 50, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + -35.35789911, + 149.16762139, + 50 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 50, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + -35.35652005, + 149.16443406, + 50 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 50, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + -35.35671532, + 149.15541078, + 50 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 50, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + -35.35804555, + 149.15351036, + 50 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 50, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + -35.35946119, + 149.15657797, + 50 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 50, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + -35.35963204, + 149.16123177, + 50 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 50, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 9, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + -35.36130393, + 149.16235407, + 50 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 20, + "doJumpId": 10, + "frame": 2, + "params": [ + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + -35.3632621, + 149.1652374, + 585 + ], + "vehicleType": 2, + "version": 2 + }, + "rallyPoints": { + "points": [ + [ + -35.36270731, + 149.16440413, + 0 + ], + [ + -35.35825302, + 149.1672024, + 0 + ], + [ + -35.35969306, + 149.15249281, + 0 + ] + ], + "version": 2 + }, + "version": 1 +} diff --git a/mavros/test/test_frame_conversions.cpp b/mavros/test/test_frame_conversions.cpp index 838527428..e48ce9c2e 100644 --- a/mavros/test/test_frame_conversions.cpp +++ b/mavros/test/test_frame_conversions.cpp @@ -1,211 +1,218 @@ +// +// mavros +// Copyright 2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// + /** * Test libmavros frame conversion utilities */ #include -#include -#include +#include "rclcpp/rclcpp.hpp" +#include "mavros/frame_tf.hpp" -using namespace mavros; +using namespace mavros; // NOLINT static const double epsilon = 1e-9; static const double epsilon_f = 1e-6; // gMock has ability to define array matcher, but there problems with that. // so trying good old for loop -#define EXPECT_QUATERNION(q1, q2, epsilon) \ - EXPECT_NEAR(q1.w(), q2.w(), epsilon); \ - EXPECT_NEAR(q1.x(), q2.x(), epsilon); \ - EXPECT_NEAR(q1.y(), q2.y(), epsilon); \ - EXPECT_NEAR(q1.z(), q2.z(), epsilon) +#define EXPECT_QUATERNION(q1, q2, epsilon) \ + EXPECT_NEAR(q1.w(), q2.w(), epsilon); \ + EXPECT_NEAR(q1.x(), q2.x(), epsilon); \ + EXPECT_NEAR(q1.y(), q2.y(), epsilon); \ + EXPECT_NEAR(q1.z(), q2.z(), epsilon) /* -*- test general transform function -*- */ TEST(FRAME_TF, transform_static_frame__aircraft_to_baselink_123) { - Eigen::Vector3d input(1, 2, 3); - Eigen::Vector3d expected(1, -2, -3); + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d expected(1, -2, -3); - auto out = ftf::detail::transform_static_frame(input, ftf::StaticTF::AIRCRAFT_TO_BASELINK); + auto out = ftf::detail::transform_static_frame(input, ftf::StaticTF::AIRCRAFT_TO_BASELINK); - EXPECT_NEAR(expected.x(), out.x(), epsilon); - EXPECT_NEAR(expected.y(), out.y(), epsilon); - EXPECT_NEAR(expected.z(), out.z(), epsilon); + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); } TEST(FRAME_TF, transform_static_frame__baselink_to_aircraft_123) { - Eigen::Vector3d input(1, 2, 3); - Eigen::Vector3d expected(1, -2, -3); + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d expected(1, -2, -3); - auto out = ftf::detail::transform_static_frame(input, ftf::StaticTF::BASELINK_TO_AIRCRAFT); + auto out = ftf::detail::transform_static_frame(input, ftf::StaticTF::BASELINK_TO_AIRCRAFT); - EXPECT_NEAR(expected.x(), out.x(), epsilon); - EXPECT_NEAR(expected.y(), out.y(), epsilon); - EXPECT_NEAR(expected.z(), out.z(), epsilon); + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); } TEST(FRAME_TF, transform_static_frame__enu_to_ned_123) { - Eigen::Vector3d input(1, 2, 3); - Eigen::Vector3d expected(2, 1, -3); + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d expected(2, 1, -3); - auto out = ftf::detail::transform_static_frame(input, ftf::StaticTF::ENU_TO_NED); + auto out = ftf::detail::transform_static_frame(input, ftf::StaticTF::ENU_TO_NED); - EXPECT_NEAR(expected.x(), out.x(), epsilon); - EXPECT_NEAR(expected.y(), out.y(), epsilon); - EXPECT_NEAR(expected.z(), out.z(), epsilon); + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); } TEST(FRAME_TF, transform_static_frame__ned_to_enu_123) { - Eigen::Vector3d input(1, 2, 3); - Eigen::Vector3d expected(2, 1, -3); + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d expected(2, 1, -3); - auto out = ftf::detail::transform_static_frame(input, ftf::StaticTF::NED_TO_ENU); + auto out = ftf::detail::transform_static_frame(input, ftf::StaticTF::NED_TO_ENU); - EXPECT_NEAR(expected.x(), out.x(), epsilon); - EXPECT_NEAR(expected.y(), out.y(), epsilon); - EXPECT_NEAR(expected.z(), out.z(), epsilon); + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); } TEST(FRAME_TF, transform_static_frame__ecef_to_enu_123_00) { - Eigen::Vector3d input(1, 2, 3); - Eigen::Vector3d map_origin(0, 0, 0); - Eigen::Vector3d expected(2, 3, 1); + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d map_origin(0, 0, 0); + Eigen::Vector3d expected(2, 3, 1); - auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticEcefTF::ECEF_TO_ENU); + auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticEcefTF::ECEF_TO_ENU); - EXPECT_NEAR(expected.x(), out.x(), epsilon); - EXPECT_NEAR(expected.y(), out.y(), epsilon); - EXPECT_NEAR(expected.z(), out.z(), epsilon); + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); } TEST(FRAME_TF, transform_static_frame__enu_to_ecef_123_00) { - Eigen::Vector3d input(1, 2, 3); - Eigen::Vector3d map_origin(0, 0, 0); - Eigen::Vector3d expected(3, 1, 2); + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d map_origin(0, 0, 0); + Eigen::Vector3d expected(3, 1, 2); - auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticEcefTF::ENU_TO_ECEF); + auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticEcefTF::ENU_TO_ECEF); - EXPECT_NEAR(expected.x(), out.x(), epsilon); - EXPECT_NEAR(expected.y(), out.y(), epsilon); - EXPECT_NEAR(expected.z(), out.z(), epsilon); + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); } TEST(FRAME_TF, transform_static_frame__ecef_to_enu_123_4030) { - Eigen::Vector3d input(1, 2, 3); - Eigen::Vector3d map_origin(40, 30, 0); - Eigen::Vector3d expected(1.23205080756887, 1.09867532044397, 3.35782122034753); + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d map_origin(40, 30, 0); + Eigen::Vector3d expected(1.23205080756887, 1.09867532044397, 3.35782122034753); - auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticEcefTF::ECEF_TO_ENU); + auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticEcefTF::ECEF_TO_ENU); - EXPECT_NEAR(expected.x(), out.x(), epsilon); - EXPECT_NEAR(expected.y(), out.y(), epsilon); - EXPECT_NEAR(expected.z(), out.z(), epsilon); + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); } TEST(FRAME_TF, transform_static_frame__enu_to_ecef_123_4030) { - Eigen::Vector3d input(1, 2, 3); - Eigen::Vector3d map_origin(40, 30, 0); - Eigen::Vector3d expected(0.3769010460539777, 1.37230445877637, 3.46045171529757); + Eigen::Vector3d input(1, 2, 3); + Eigen::Vector3d map_origin(40, 30, 0); + Eigen::Vector3d expected(0.3769010460539777, 1.37230445877637, 3.46045171529757); - auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticEcefTF::ENU_TO_ECEF); + auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticEcefTF::ENU_TO_ECEF); - EXPECT_NEAR(expected.x(), out.x(), epsilon); - EXPECT_NEAR(expected.y(), out.y(), epsilon); - EXPECT_NEAR(expected.z(), out.z(), epsilon); + EXPECT_NEAR(expected.x(), out.x(), epsilon); + EXPECT_NEAR(expected.y(), out.y(), epsilon); + EXPECT_NEAR(expected.z(), out.z(), epsilon); } TEST(FRAME_TF, quaternion_transforms__ned_to_ned_123) { - auto input_aircraft_ned_orient = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); - auto aircraft_enu_orient = ftf::transform_orientation_ned_enu(input_aircraft_ned_orient); - auto baselink_enu_orient = ftf::transform_orientation_aircraft_baselink(aircraft_enu_orient); - aircraft_enu_orient = ftf::transform_orientation_baselink_aircraft(baselink_enu_orient); - auto output_aircraft_ned = ftf::transform_orientation_enu_ned(aircraft_enu_orient); + auto input_aircraft_ned_orient = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); + auto aircraft_enu_orient = ftf::transform_orientation_ned_enu(input_aircraft_ned_orient); + auto baselink_enu_orient = ftf::transform_orientation_aircraft_baselink(aircraft_enu_orient); + aircraft_enu_orient = ftf::transform_orientation_baselink_aircraft(baselink_enu_orient); + auto output_aircraft_ned = ftf::transform_orientation_enu_ned(aircraft_enu_orient); - EXPECT_QUATERNION(input_aircraft_ned_orient, output_aircraft_ned, epsilon); + EXPECT_QUATERNION(input_aircraft_ned_orient, output_aircraft_ned, epsilon); } #if 0 // not implemented TEST(FRAME_TF, transform_static_frame__quaterniond_123) { - auto input = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); - auto expected = ftf::quaternion_from_rpy(1.0, -2.0, -3.0); + auto input = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); + auto expected = ftf::quaternion_from_rpy(1.0, -2.0, -3.0); - ftf::TRANSFORM_TYPE enu_sensor = ftf::PLATFORM_TO_ENU; - auto out = ftf::transform_frame(input,enu_sensor); + ftf::TRANSFORM_TYPE enu_sensor = ftf::PLATFORM_TO_ENU; + auto out = ftf::transform_frame(input, enu_sensor); - EXPECT_QUATERNION(expected, out, epsilon); + EXPECT_QUATERNION(expected, out, epsilon); } TEST(FRAME_TF, transform_frame__covariance3x3) { - ftf::Covariance3d input = {{ - 1.0, 2.0, 3.0, - 4.0, 5.0, 6.0, - 7.0, 8.0, 9.0 - }}; - - /* Calculated as: - * | 1 0 0 | - * input * | 0 -1 0 | - * | 0 0 -1 | - */ - ftf::Covariance3d expected = {{ - 1.0, -2.0, -3.0, - 4.0, -5.0, -6.0, - 7.0, -8.0, -9.0 - }}; - - auto out = ftf::transform_frame(input,enu_sensor); - - for (size_t idx = 0; idx < expected.size(); idx++) { - SCOPED_TRACE(idx); - EXPECT_NEAR(expected[idx], out[idx], epsilon); - } + ftf::Covariance3d input = {{ + 1.0, 2.0, 3.0, + 4.0, 5.0, 6.0, + 7.0, 8.0, 9.0 + }}; + + /* Calculated as: + * | 1 0 0 | + * input * | 0 -1 0 | + * | 0 0 -1 | + */ + ftf::Covariance3d expected = {{ + 1.0, -2.0, -3.0, + 4.0, -5.0, -6.0, + 7.0, -8.0, -9.0 + }}; + + auto out = ftf::transform_frame(input, enu_sensor); + + for (size_t idx = 0; idx < expected.size(); idx++) { + SCOPED_TRACE(idx); + EXPECT_NEAR(expected[idx], out[idx], epsilon); + } } -TEST(FRAME_TF, transform_frame__covariance6x6) +TEST(FRAME_TF, transform_frame__covariance6x6) { - ftf::Covariance6d input = {{ - 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, - 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, - 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, - 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, - 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, - 31.0, 32.0, 33.0, 34.0, 35.0, 36.0 - }}; - - ftf::Covariance6d expected = {{ - 1.0, -2.0, -3.0, 4.0, -5.0, -6.0, - 7.0, -8.0, -9.0, 10.0, -11.0, -12.0, - 13.0, -14.0, -15.0, 16.0, -17.0, -18.0, - 19.0, -20.0, -21.0, 22.0, -23.0, -24.0, - 25.0, -26.0, -27.0, 28.0, -29.0, -30.0, - 31.0, -32.0, -33.0, 34.0, -35.0, -36.0 - }}; - - auto out = ftf::transform_frame(input); - - for (size_t idx = 0; idx < expected.size(); idx++) { - SCOPED_TRACE(idx); - EXPECT_NEAR(expected[idx], out[idx], epsilon); - } + ftf::Covariance6d input = {{ + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, + 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, + 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, + 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, + 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, + 31.0, 32.0, 33.0, 34.0, 35.0, 36.0 + }}; + + ftf::Covariance6d expected = {{ + 1.0, -2.0, -3.0, 4.0, -5.0, -6.0, + 7.0, -8.0, -9.0, 10.0, -11.0, -12.0, + 13.0, -14.0, -15.0, 16.0, -17.0, -18.0, + 19.0, -20.0, -21.0, 22.0, -23.0, -24.0, + 25.0, -26.0, -27.0, 28.0, -29.0, -30.0, + 31.0, -32.0, -33.0, 34.0, -35.0, -36.0 + }}; + + auto out = ftf::transform_frame(input); + + for (size_t idx = 0; idx < expected.size(); idx++) { + SCOPED_TRACE(idx); + EXPECT_NEAR(expected[idx], out[idx], epsilon); + } } #endif - -int main(int argc, char **argv) +int main(int argc, char ** argv) { - //ros::init(argc, argv, "mavros_test", ros::init_options::AnonymousName); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/mavros/test/test_quaternion_utils.cpp b/mavros/test/test_quaternion_utils.cpp index e8deb4175..0316b368e 100644 --- a/mavros/test/test_quaternion_utils.cpp +++ b/mavros/test/test_quaternion_utils.cpp @@ -1,16 +1,25 @@ +// +// mavros +// Copyright 2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// + /** * Test libmavros quaternion utilities */ #include -#include -#include +#include +#include #include #include -using namespace mavros; +using namespace mavros; // NOLINT static const double epsilon = 1e-9; static const double epsilon_f = 1e-6; @@ -18,152 +27,153 @@ static const double deg_to_rad = M_PI / 180.0; // gMock has ability to define array matcher, but there problems with that. // so trying good old for loop -#define ASSERT_QUATERNION(q1, q2, epsilon) \ - ASSERT_NEAR(q1.w(), q2.w(), epsilon); \ - ASSERT_NEAR(q1.x(), q2.x(), epsilon); \ - ASSERT_NEAR(q1.y(), q2.y(), epsilon); \ - ASSERT_NEAR(q1.z(), q2.z(), epsilon) +#define ASSERT_QUATERNION(q1, q2, epsilon) \ + ASSERT_NEAR(q1.w(), q2.w(), epsilon); \ + ASSERT_NEAR(q1.x(), q2.x(), epsilon); \ + ASSERT_NEAR(q1.y(), q2.y(), epsilon); \ + ASSERT_NEAR(q1.z(), q2.z(), epsilon) -#define EXPECT_QUATERNION(q1, q2, epsilon) \ - EXPECT_NEAR(q1.w(), q2.w(), epsilon); \ - EXPECT_NEAR(q1.x(), q2.x(), epsilon); \ - EXPECT_NEAR(q1.y(), q2.y(), epsilon); \ - EXPECT_NEAR(q1.z(), q2.z(), epsilon) +#define EXPECT_QUATERNION(q1, q2, epsilon) \ + EXPECT_NEAR(q1.w(), q2.w(), epsilon); \ + EXPECT_NEAR(q1.x(), q2.x(), epsilon); \ + EXPECT_NEAR(q1.y(), q2.y(), epsilon); \ + EXPECT_NEAR(q1.z(), q2.z(), epsilon) /* -*- quaternion_from_rpy / getYaw -*- */ TEST(FRAME_TF, quaternion_from_rpy__check_compatibility) { - auto eigen_q = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); - auto eigen_neg_q = ftf::quaternion_from_rpy(-1.0, -2.0, -3.0); - tf2::Quaternion bt_q; bt_q.setRPY(1.0, 2.0, 3.0); - tf2::Quaternion bt_neg_q; bt_neg_q.setRPY(-1.0, -2.0, -3.0); + auto eigen_q = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); + auto eigen_neg_q = ftf::quaternion_from_rpy(-1.0, -2.0, -3.0); + tf2::Quaternion bt_q; bt_q.setRPY(1.0, 2.0, 3.0); + tf2::Quaternion bt_neg_q; bt_neg_q.setRPY(-1.0, -2.0, -3.0); - EXPECT_QUATERNION(bt_q, eigen_q, epsilon); - EXPECT_QUATERNION(bt_neg_q, eigen_neg_q, epsilon); + EXPECT_QUATERNION(bt_q, eigen_q, epsilon); + EXPECT_QUATERNION(bt_neg_q, eigen_neg_q, epsilon); } TEST(FRAME_TF, quaternion_from_rpy__paranoic_check) { - auto q1 = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); - auto q2 = ftf::quaternion_from_rpy(Eigen::Vector3d(1.0, 2.0, 3.0)); + auto q1 = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); + auto q2 = ftf::quaternion_from_rpy(Eigen::Vector3d(1.0, 2.0, 3.0)); - EXPECT_QUATERNION(q1, q2, epsilon); + EXPECT_QUATERNION(q1, q2, epsilon); } TEST(FRAME_TF, quaternion_to_rpy__123) { - // this test only works on positive rpy: 0..pi - auto q = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); - auto rpy = ftf::quaternion_to_rpy(q); + // this test only works on positive rpy: 0..pi + auto q = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); + auto rpy = ftf::quaternion_to_rpy(q); - EXPECT_NEAR(1.0, rpy.x(), epsilon); - EXPECT_NEAR(2.0, rpy.y(), epsilon); - EXPECT_NEAR(3.0, rpy.z(), epsilon); + EXPECT_NEAR(1.0, rpy.x(), epsilon); + EXPECT_NEAR(2.0, rpy.y(), epsilon); + EXPECT_NEAR(3.0, rpy.z(), epsilon); } TEST(FRAME_TF, quaternion_to_rpy__pm_pi) { - // this test try large count of different angles - - // in degrees - const ssize_t min = -180; - const ssize_t max = 180; - const ssize_t step = 15; + // this test try large count of different angles - const auto test_orientation = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); + // in degrees + const ssize_t min = -180; + const ssize_t max = 180; + const ssize_t step = 15; - for (ssize_t roll = min; roll <= max; roll += step) { - for (ssize_t pitch = min; pitch <= max; pitch += step) { - for (ssize_t yaw = min; yaw <= max; yaw += step) { + const auto test_orientation = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); - Eigen::Vector3d expected_deg(roll, pitch, yaw); - Eigen::Vector3d expected = expected_deg * deg_to_rad; + for (ssize_t roll = min; roll <= max; roll += step) { + for (ssize_t pitch = min; pitch <= max; pitch += step) { + for (ssize_t yaw = min; yaw <= max; yaw += step) { + Eigen::Vector3d expected_deg(roll, pitch, yaw); + Eigen::Vector3d expected = expected_deg * deg_to_rad; - std::stringstream ss; + std::stringstream ss; - ss << "DEG(" << expected_deg.x() << ", " << expected_deg.y() << ", " << expected_deg.z() << ") "; - ss << "RAD(" << expected.x() << ", " << expected.y() << ", " << expected.z() << ")"; + ss << "DEG(" << expected_deg.x() << ", " << expected_deg.y() << ", " << expected_deg.z() << + ") "; + ss << "RAD(" << expected.x() << ", " << expected.y() << ", " << expected.z() << ")"; - SCOPED_TRACE(ss.str()); + SCOPED_TRACE(ss.str()); - // rpy->q->rpy->q - auto q1 = ftf::quaternion_from_rpy(expected); - auto rpy = ftf::quaternion_to_rpy(q1); - auto q2 = ftf::quaternion_from_rpy(rpy); + // rpy->q->rpy->q + auto q1 = ftf::quaternion_from_rpy(expected); + auto rpy = ftf::quaternion_to_rpy(q1); + auto q2 = ftf::quaternion_from_rpy(rpy); - // direct assumption is failed at ranges outside 0..pi - //EXPECT_NEAR(expected.x(), rpy.x(), epsilon); - //EXPECT_NEAR(expected.y(), rpy.y(), epsilon); - //EXPECT_NEAR(expected.z(), rpy.z(), epsilon); + // direct assumption is failed at ranges outside 0..pi + // EXPECT_NEAR(expected.x(), rpy.x(), epsilon); + // EXPECT_NEAR(expected.y(), rpy.y(), epsilon); + // EXPECT_NEAR(expected.z(), rpy.z(), epsilon); - // at -pi..0 we got complimentary q2 to q - //EXPECT_QUATERNION(q1, q2, epsilon); + // at -pi..0 we got complimentary q2 to q + // EXPECT_QUATERNION(q1, q2, epsilon); - // instead of direct comparision we rotate other quaternion and then compare results - auto tq1 = q1 * test_orientation * q1.inverse(); - auto tq2 = q2 * test_orientation * q2.inverse(); + // instead of direct comparision we rotate other quaternion and then compare results + auto tq1 = q1 * test_orientation * q1.inverse(); + auto tq2 = q2 * test_orientation * q2.inverse(); - EXPECT_QUATERNION(tq1, tq2, epsilon); - } - } - } + EXPECT_QUATERNION(tq1, tq2, epsilon); + } + } + } } // ftf::quaternion_to_rpy() is not compatible with tf2::Matrix3x3(q).getRPY() TEST(FRAME_TF, quaternion_get_yaw__123) { - // with pich >= pi/2 got incorrect result. - // so 1, 2, 3 rad (57, 115, 172 deg) replaced with 60, 89, 172 deg - auto q = ftf::quaternion_from_rpy(60.0 * deg_to_rad, 89.0 * deg_to_rad, 3.0); + // with pich >= pi/2 got incorrect result. + // so 1, 2, 3 rad (57, 115, 172 deg) replaced with 60, 89, 172 deg + auto q = ftf::quaternion_from_rpy(60.0 * deg_to_rad, 89.0 * deg_to_rad, 3.0); - EXPECT_NEAR(3.0, ftf::quaternion_get_yaw(q), epsilon); + EXPECT_NEAR(3.0, ftf::quaternion_get_yaw(q), epsilon); } TEST(FRAME_TF, quaternion_get_yaw__pm_pi) { - // in degrees - const ssize_t min = -180; - const ssize_t max = 180; - const ssize_t step = 1; + // in degrees + const ssize_t min = -180; + const ssize_t max = 180; + const ssize_t step = 1; - for (ssize_t yaw = min; yaw <= max; yaw += step) { - Eigen::Vector3d expected_deg(1.0, 2.0, yaw); - Eigen::Vector3d expected = expected_deg * deg_to_rad; + for (ssize_t yaw = min; yaw <= max; yaw += step) { + Eigen::Vector3d expected_deg(1.0, 2.0, yaw); + Eigen::Vector3d expected = expected_deg * deg_to_rad; - std::stringstream ss; + std::stringstream ss; - ss << "DEG(" << expected_deg.x() << ", " << expected_deg.y() << ", " << expected_deg.z() << ") "; - ss << "RAD(" << expected.x() << ", " << expected.y() << ", " << expected.z() << ")"; + ss << "DEG(" << expected_deg.x() << ", " << expected_deg.y() << ", " << expected_deg.z() << + ") "; + ss << "RAD(" << expected.x() << ", " << expected.y() << ", " << expected.z() << ")"; - SCOPED_TRACE(ss.str()); + SCOPED_TRACE(ss.str()); - auto q1 = ftf::quaternion_from_rpy(expected); - double q1_yaw = ftf::quaternion_get_yaw(q1); + auto q1 = ftf::quaternion_from_rpy(expected); + double q1_yaw = ftf::quaternion_get_yaw(q1); - EXPECT_NEAR(expected.z(), q1_yaw, epsilon); - } + EXPECT_NEAR(expected.z(), q1_yaw, epsilon); + } } /* -*- mavlink util -*- */ TEST(FRAME_TF, quaternion_to_mavlink__123) { - auto eigen_q = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); - std::array mavlink_q; + auto eigen_q = ftf::quaternion_from_rpy(1.0, 2.0, 3.0); + std::array mavlink_q; - ftf::quaternion_to_mavlink(eigen_q, mavlink_q); + ftf::quaternion_to_mavlink(eigen_q, mavlink_q); - EXPECT_NEAR(mavlink_q[0], eigen_q.w(), epsilon_f); - EXPECT_NEAR(mavlink_q[1], eigen_q.x(), epsilon_f); - EXPECT_NEAR(mavlink_q[2], eigen_q.y(), epsilon_f); - EXPECT_NEAR(mavlink_q[3], eigen_q.z(), epsilon_f); + EXPECT_NEAR(mavlink_q[0], eigen_q.w(), epsilon_f); + EXPECT_NEAR(mavlink_q[1], eigen_q.x(), epsilon_f); + EXPECT_NEAR(mavlink_q[2], eigen_q.y(), epsilon_f); + EXPECT_NEAR(mavlink_q[3], eigen_q.z(), epsilon_f); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/mavros/test/test_router.cpp b/mavros/test/test_router.cpp new file mode 100644 index 000000000..335908e2d --- /dev/null +++ b/mavros/test/test_router.cpp @@ -0,0 +1,474 @@ +// +// mavros +// Copyright 2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// + +/** + * Test mavros router node + */ + + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/executors.hpp" +#include "mavconn/interface.hpp" +#include "mavros/mavros_router.hpp" + +using ::testing::_; + +using namespace mavros::router; // NOLINT + +using namespace mavconn; // NOLINT +using mavlink_message_t = mavlink::mavlink_message_t; +using msgid_t = mavlink::msgid_t; + +namespace mavlink +{ +namespace common +{ +using namespace mavlink::minimal; // NOLINT +namespace msg +{ +using namespace mavlink::minimal::msg; // NOLINT +} +} +} // namespace mavlink + +using LT = Endpoint::Type; + +class MockEndpoint : public Endpoint +{ +public: + using SharedPtr = std::shared_ptr; + + MOCK_METHOD0(is_open, bool()); + MOCK_METHOD0(open, std::pair()); + MOCK_METHOD0(close, void()); + + MOCK_METHOD3( + send_message, void(const mavlink_message_t * msg, + const Framing framing, id_t src_id)); + MOCK_METHOD2( + recv_message, void(const mavlink_message_t * msg, + const Framing framing)); + MOCK_METHOD1(diag_run, void(diagnostic_updater::DiagnosticStatusWrapper &)); +}; + +namespace mavros +{ +namespace router +{ + +class TestRouter : public ::testing::Test +{ +public: + TestRouter() + { + // std::cout << "init" << std::endl; + rclcpp::init(0, nullptr); + } + + ~TestRouter() + { + // NOTE(vooon): required to remove any remaining Nodes + // std::cout << "kill" << std::endl; + rclcpp::shutdown(); + } + + Router::SharedPtr create_node() + { + auto router = std::make_shared("test_mavros_router"); + + auto make_and_add_mock_endpoint = + [router](id_t id, const std::string & url, LT type, + std::set remotes) { + auto ep = std::make_shared(); + + ep->parent = router; + ep->id = id; + ep->link_type = type; + ep->url = url; + ep->remote_addrs = remotes; + + router->endpoints[id] = ep; + + // XXX(vooon): another strange thing: mock reports about not freed mock objects. + // let's silence it for now. + testing::Mock::AllowLeak(&(*ep)); + }; + + router->id_counter = 1000; + make_and_add_mock_endpoint(1000, "mock://fcu1", LT::fcu, {0x0000, 0x0100, 0x0101}); + make_and_add_mock_endpoint(1001, "mock://fcu2", LT::fcu, {0x0000, 0x0200, 0x0201}); + make_and_add_mock_endpoint(1002, "/uas1", LT::uas, {0x0000, 0x0100, 0x01BF}); + make_and_add_mock_endpoint(1003, "/uas2", LT::uas, {0x0000, 0x0200, 0x02BF}); + make_and_add_mock_endpoint(1004, "mock://gcs1", LT::gcs, {0x0000, 0xFF00, 0xFFBE}); + make_and_add_mock_endpoint(1005, "mock://gcs2", LT::gcs, {0x0000, 0xFF00, 0xFFBD}); + + return router; + } + + MockEndpoint::SharedPtr getep(Router::SharedPtr router, id_t id) + { + auto ep = router->endpoints[id]; + return std::static_pointer_cast(ep); + } + + std::unordered_map & get_endpoints(Router::SharedPtr router) + { + return router->endpoints; + } + + mavlink_message_t convert_message(const mavlink::Message & msg, const addr_t source = 0x0101) + { + mavlink_message_t ret; + mavlink::MsgMap map(ret); + ret.sysid = source >> 8; + ret.compid = source & 0xFF; + msg.serialize(map); + + // std::cout << "in msg: " << msg.to_yaml() << std::endl; + // std::cout << "msg len: " << +ret.len << std::endl; + // for (size_t i = 0; i < ret.len; i++) { + // std::cout << + // utils::format("byte: %2zu: %02x", i, (_MAV_PAYLOAD(&ret)[i] & 0xff)) << std::endl; + // } + + return ret; + } + + mavlink::common::msg::HEARTBEAT make_heartbeat() + { + using mavlink::common::MAV_AUTOPILOT; + using mavlink::common::MAV_MODE; + using mavlink::common::MAV_STATE; + using mavlink::common::MAV_TYPE; + + mavlink::common::msg::HEARTBEAT hb = {}; + hb.type = static_cast(MAV_TYPE::ONBOARD_CONTROLLER); + hb.autopilot = static_cast(MAV_AUTOPILOT::INVALID); + hb.base_mode = static_cast(MAV_MODE::MANUAL_ARMED); + hb.custom_mode = 0; + hb.system_status = static_cast(MAV_STATE::ACTIVE); + + return hb; + } + + inline size_t get_stat_msg_routed(Router::SharedPtr router) + { + return router->stat_msg_routed.load(); + } + + inline size_t get_stat_msg_sent(Router::SharedPtr router) + { + return router->stat_msg_sent.load(); + } + + inline size_t get_stat_msg_dropped(Router::SharedPtr router) + { + return router->stat_msg_dropped.load(); + } +}; + +TEST_F(TestRouter, set_parameter) +{ + auto router = std::make_shared("test_mavros_router"); + + router->set_parameters( + std::vector{ + rclcpp::Parameter("fcu_urls", std::vector{"udp://:45001@", "tcp-l://:57601"}), + rclcpp::Parameter("gcs_urls", std::vector{"udp://:45002@", "tcp-l://:57602"}), + rclcpp::Parameter("uas_urls", std::vector{"/uas1", "/uas2"}) + }); + + auto expected_values = std::vector>{ + {"tcp-l://:57601", LT::fcu}, // NOTE(vooon): std::set change order of tcp and udp + {"udp://:45001@", LT::fcu}, + {"tcp-l://:57602", LT::gcs}, + {"udp://:45002@", LT::gcs}, + {"/uas1", LT::uas}, + {"/uas2", LT::uas}, + }; + + auto endpoints = get_endpoints(router); + + EXPECT_EQ(size_t(6), endpoints.size()); + for (auto & kv : endpoints) { + auto & ep = kv.second; + auto tc = expected_values.at(ep->id - 1000); + + EXPECT_EQ(tc.first, ep->url); + EXPECT_EQ(tc.second, ep->link_type); + } + + // XXX NOTE(vooon): i do not see destructor call! + // std::cout << "ref cnt: " << router.use_count() << std::endl; + // router.reset(); + // std::cout << "ref cnt: " << router.use_count() << std::endl; +} + +#define DEFINE_EPS() \ + auto fcu1 = getep(router, 1000); \ + auto fcu2 = getep(router, 1001); \ + auto uas1 = getep(router, 1002); \ + auto uas2 = getep(router, 1003); \ + auto gcs1 = getep(router, 1004); \ + auto gcs2 = getep(router, 1005); + +#define VERIFY_EPS() \ + testing::Mock::VerifyAndClearExpectations(&(*fcu1)); \ + testing::Mock::VerifyAndClearExpectations(&(*fcu2)); \ + testing::Mock::VerifyAndClearExpectations(&(*uas1)); \ + testing::Mock::VerifyAndClearExpectations(&(*uas2)); \ + testing::Mock::VerifyAndClearExpectations(&(*gcs1)); \ + testing::Mock::VerifyAndClearExpectations(&(*gcs2)); + +TEST_F(TestRouter, route_fcu_broadcast) +{ + auto router = this->create_node(); + + auto hb = make_heartbeat(); + auto hbmsg = convert_message(hb, 0x0101); + auto fr = Framing::ok; + + DEFINE_EPS(); + + EXPECT_CALL(*fcu1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*fcu2, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*uas1, send_message(_, fr, _)); + EXPECT_CALL(*uas2, send_message(_, fr, _)); + EXPECT_CALL(*gcs1, send_message(_, fr, _)); + EXPECT_CALL(*gcs2, send_message(_, fr, _)); + + router->route_message(fcu1, &hbmsg, fr); + + VERIFY_EPS(); +} + +TEST_F(TestRouter, route_uas_broadcast) +{ + auto router = this->create_node(); + + auto hb = make_heartbeat(); + auto hbmsg = convert_message(hb, 0x01BF); + auto fr = Framing::ok; + + DEFINE_EPS(); + + EXPECT_CALL(*fcu1, send_message(_, fr, _)); + EXPECT_CALL(*fcu2, send_message(_, fr, _)); + EXPECT_CALL(*uas1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*uas2, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*gcs1, send_message(_, fr, _)); + EXPECT_CALL(*gcs2, send_message(_, fr, _)); + + router->route_message(uas1, &hbmsg, fr); + + VERIFY_EPS(); +} + +TEST_F(TestRouter, route_gcs_broadcast) +{ + auto router = this->create_node(); + + auto hb = make_heartbeat(); + auto hbmsg = convert_message(hb, 0xFFBE); + auto fr = Framing::ok; + + DEFINE_EPS(); + + EXPECT_CALL(*fcu1, send_message(_, fr, _)); + EXPECT_CALL(*fcu2, send_message(_, fr, _)); + EXPECT_CALL(*uas1, send_message(_, fr, _)); + EXPECT_CALL(*uas2, send_message(_, fr, _)); + EXPECT_CALL(*gcs1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*gcs2, send_message(_, fr, _)).Times(0); + + router->route_message(gcs1, &hbmsg, fr); + + VERIFY_EPS(); +} + +TEST_F(TestRouter, route_targeted_one_system) +{ + using MF = mavlink::common::MAV_MODE_FLAG; + using utils::enum_value; + + auto router = this->create_node(); + + auto set_mode = mavlink::common::msg::SET_MODE(); + set_mode.target_system = 0x01; + set_mode.base_mode = enum_value(MF::SAFETY_ARMED) | enum_value(MF::TEST_ENABLED); + set_mode.custom_mode = 4; + + auto smmsg = convert_message(set_mode, 0x0101); + auto fr = Framing::ok; + + DEFINE_EPS(); + + EXPECT_CALL(*fcu1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*fcu2, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*uas1, send_message(_, fr, _)).Times(1); + EXPECT_CALL(*uas2, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*gcs1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*gcs2, send_message(_, fr, _)).Times(0); + + router->route_message(fcu1, &smmsg, fr); + + VERIFY_EPS(); +} + +TEST_F(TestRouter, route_targeted_two_system) +{ + using MF = mavlink::common::MAV_MODE_FLAG; + using utils::enum_value; + + auto router = this->create_node(); + + auto set_mode = mavlink::common::msg::SET_MODE(); + set_mode.target_system = 0xFF; + set_mode.base_mode = enum_value(MF::SAFETY_ARMED) | enum_value(MF::TEST_ENABLED); + set_mode.custom_mode = 4; + + auto smmsg = convert_message(set_mode, 0x01BF); + auto fr = Framing::ok; + + DEFINE_EPS(); + + EXPECT_CALL(*fcu1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*fcu2, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*uas1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*uas2, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*gcs1, send_message(_, fr, _)).Times(1); + EXPECT_CALL(*gcs2, send_message(_, fr, _)).Times(1); + + router->route_message(uas1, &smmsg, fr); + + VERIFY_EPS(); +} + +TEST_F(TestRouter, route_targeted_system_component) +{ + auto router = this->create_node(); + + auto ping = mavlink::common::msg::PING(); + ping.target_system = 0x02; + ping.target_component = 0xBF; + ping.seq = 1234; + ping.time_usec = 123456789000000ULL; + + auto pmsg = convert_message(ping, 0x0101); + auto fr = Framing::ok; + + DEFINE_EPS(); + + EXPECT_CALL(*fcu1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*fcu2, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*uas1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*uas2, send_message(_, fr, _)).Times(1); + EXPECT_CALL(*gcs1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*gcs2, send_message(_, fr, _)).Times(0); + + router->route_message(fcu1, &pmsg, fr); + + VERIFY_EPS(); +} + +TEST_F(TestRouter, endpoint_recv_message) +{ + auto router = std::make_shared("test_mavros_router"); + + router->set_parameters( + std::vector{ + rclcpp::Parameter("uas_urls", std::vector{"/uas1"}) + }); + + auto uas1 = getep(router, 1000); + + uas1->stale_addrs.emplace(0x0100); + uas1->stale_addrs.emplace(0x01BF); + + ASSERT_NE(uas1->stale_addrs.end(), uas1->stale_addrs.find(0x0100)); + ASSERT_NE(uas1->stale_addrs.end(), uas1->stale_addrs.find(0x01BF)); + ASSERT_NE(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x0000)); + ASSERT_EQ(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x0100)); + ASSERT_EQ(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x01BF)); + + auto hb = make_heartbeat(); + auto hbmsg = convert_message(hb, 0x01BF); + auto fr = Framing::ok; + + uas1->recv_message(&hbmsg, fr); + + ASSERT_EQ(uas1->stale_addrs.end(), uas1->stale_addrs.find(0x0100)); + ASSERT_EQ(uas1->stale_addrs.end(), uas1->stale_addrs.find(0x01BF)); + ASSERT_NE(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x0000)); + ASSERT_NE(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x0100)); + ASSERT_NE(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x01BF)); + + ASSERT_EQ(size_t(1), get_stat_msg_routed(router)); + ASSERT_EQ(size_t(0), get_stat_msg_sent(router)); + ASSERT_EQ(size_t(1), get_stat_msg_dropped(router)); +} + +#if 0 // TODO(vooon): +TEST_F(TestRouter, uas_recv_message) +{ + auto router = this->create_node(); + + auto ping = mavlink::common::msg::PING(); + ping.target_system = 0x02; + ping.target_component = 0x00; + ping.seq = 1234; + ping.time_usec = 123456789000000ULL; + + auto pmsg = convert_message(ping, 0x0101); + auto fr = Framing::ok; + + mavros_msgs::msg::Mavlink rmsg{}; + mavros_msgs::mavlink::convert(pmsg, rmsg, utils::enum_value(fr)); + + DEFINE_EPS(); + + EXPECT_CALL(*fcu1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*fcu2, send_message(_, fr, _)).Times(1); + EXPECT_CALL(*uas1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*uas2, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*gcs1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*gcs2, send_message(_, fr, _)).Times(0); + + EXPECT_CALL(*uas1, recv_message(_, fr)); + + rclcpp::executors::MultiThreadedExecutor exec; + exec.add_node(router); + + auto pub = router->create_publisher("/uas2/mavlink_sink", 1); + pub->publish(rmsg); + + exec.spin(); + + VERIFY_EPS(); +} +#endif + +} // namespace router +} // namespace mavros + +int main(int argc, char ** argv) +{ + // rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/mavros/test/test_sensor_orientation.cpp b/mavros/test/test_sensor_orientation.cpp index 388971d35..c6842d923 100644 --- a/mavros/test/test_sensor_orientation.cpp +++ b/mavros/test/test_sensor_orientation.cpp @@ -1,3 +1,11 @@ +// mavros +// Copyright 2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// + /** * Test libmavros sensor orientation utilities * @@ -6,118 +14,122 @@ #include -#include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" +#include "mavros/utils.hpp" +#include "mavros/frame_tf.hpp" +#include "mavconn/mavlink_dialect.hpp" -using namespace mavros; +using namespace mavros; // NOLINT using mavros::utils::enum_value; using SO = mavlink::common::MAV_SENSOR_ORIENTATION; static const double epsilon = 1e-9; #define EXPECT_QUATERNION(exp, res) \ - EXPECT_NEAR(exp.w(), res.w(), epsilon); \ - EXPECT_NEAR(exp.x(), res.x(), epsilon); \ - EXPECT_NEAR(exp.y(), res.y(), epsilon); \ - EXPECT_NEAR(exp.z(), res.z(), epsilon) + EXPECT_NEAR(exp.w(), res.w(), epsilon); \ + EXPECT_NEAR(exp.x(), res.x(), epsilon); \ + EXPECT_NEAR(exp.y(), res.y(), epsilon); \ + EXPECT_NEAR(exp.z(), res.z(), epsilon) TEST(UTILS, sensor_orientation_matching__none) { - auto expected = ftf::quaternion_from_rpy(0.0, 0.0, 0.0); - auto out = utils::sensor_orientation_matching(SO::ROTATION_NONE); + auto expected = ftf::quaternion_from_rpy(0.0, 0.0, 0.0); + auto out = utils::sensor_orientation_matching(SO::ROTATION_NONE); - EXPECT_QUATERNION(expected, out); + EXPECT_QUATERNION(expected, out); } TEST(UTILS, sensor_orientation_matching__roll_180) { - auto expected = ftf::quaternion_from_rpy(M_PI, 0.0, 0.0); - auto out = utils::sensor_orientation_matching(SO::ROTATION_ROLL_180); + auto expected = ftf::quaternion_from_rpy(M_PI, 0.0, 0.0); + auto out = utils::sensor_orientation_matching(SO::ROTATION_ROLL_180); - EXPECT_QUATERNION(expected, out); + EXPECT_QUATERNION(expected, out); } TEST(UTILS, sensor_orientation_matching__roll_180_yaw_90) { - auto expected = ftf::quaternion_from_rpy(M_PI, 0.0, M_PI/2); - auto out = utils::sensor_orientation_matching(SO::ROTATION_ROLL_180_YAW_90); + auto expected = ftf::quaternion_from_rpy(M_PI, 0.0, M_PI / 2); + auto out = utils::sensor_orientation_matching(SO::ROTATION_ROLL_180_YAW_90); - EXPECT_QUATERNION(expected, out); + EXPECT_QUATERNION(expected, out); } TEST(UTILS, sensor_orientation_matching__custom) { - auto expected = ftf::quaternion_from_rpy(0.0, 0.0, 0.0); - auto out = utils::sensor_orientation_matching(SO::ROTATION_CUSTOM); + auto expected = ftf::quaternion_from_rpy(0.0, 0.0, 0.0); + auto out = utils::sensor_orientation_matching(SO::ROTATION_CUSTOM); - EXPECT_QUATERNION(expected, out); + EXPECT_QUATERNION(expected, out); } TEST(UTILS, to_string__none) { - EXPECT_EQ("NONE", utils::to_string(SO::ROTATION_NONE)); + EXPECT_EQ("NONE", utils::to_string(SO::ROTATION_NONE)); } TEST(UTILS, to_string__roll_180) { - EXPECT_EQ("ROLL_180", utils::to_string(SO::ROTATION_ROLL_180)); + EXPECT_EQ("ROLL_180", utils::to_string(SO::ROTATION_ROLL_180)); } TEST(UTILS, to_string__roll_180_yaw_90) { - EXPECT_EQ("ROLL_180_YAW_90", utils::to_string(SO::ROTATION_ROLL_180_YAW_90)); + EXPECT_EQ("ROLL_180_YAW_90", utils::to_string(SO::ROTATION_ROLL_180_YAW_90)); } TEST(UTILS, to_string__custom) { - EXPECT_EQ("CUSTOM", utils::to_string(SO::ROTATION_CUSTOM)); + EXPECT_EQ("CUSTOM", utils::to_string(SO::ROTATION_CUSTOM)); } TEST(UTILS, sensor_orientation_from_str__none) { - EXPECT_EQ(enum_value(SO::ROTATION_NONE), utils::sensor_orientation_from_str("NONE")); + EXPECT_EQ(enum_value(SO::ROTATION_NONE), utils::sensor_orientation_from_str("NONE")); } TEST(UTILS, sensor_orientation_from_str__unknown) { - EXPECT_LT(utils::sensor_orientation_from_str("completely wrong identifier"), 0); + EXPECT_LT(utils::sensor_orientation_from_str("completely wrong identifier"), 0); } TEST(UTILS, sensor_orientation_from_str__number) { - EXPECT_EQ(enum_value(SO::ROTATION_ROLL_270), utils::sensor_orientation_from_str("20")); + EXPECT_EQ(enum_value(SO::ROTATION_ROLL_270), utils::sensor_orientation_from_str("20")); } TEST(UTILS, sensor_orientation_from_str__wrong_number) { - // 123 >> 38 (max) - EXPECT_LT(utils::sensor_orientation_from_str("123"), 0); + // 123 >> 38 (max) + EXPECT_LT(utils::sensor_orientation_from_str("123"), 0); } TEST(UTILS, sensor_orientation_from_str__roll_180) { - EXPECT_EQ(enum_value(SO::ROTATION_ROLL_180), utils::sensor_orientation_from_str("ROLL_180")); + EXPECT_EQ(enum_value(SO::ROTATION_ROLL_180), utils::sensor_orientation_from_str("ROLL_180")); } TEST(UTILS, sensor_orientation_from_str__roll_180_yaw_90) { - EXPECT_EQ(enum_value(SO::ROTATION_ROLL_180_YAW_90), utils::sensor_orientation_from_str("ROLL_180_YAW_90")); + EXPECT_EQ( + enum_value(SO::ROTATION_ROLL_180_YAW_90), + utils::sensor_orientation_from_str("ROLL_180_YAW_90")); } TEST(UTILS, sensor_orientation_from_str__last_element_roll_90_yaw_270) { - EXPECT_EQ(enum_value(SO::ROTATION_ROLL_90_YAW_270), utils::sensor_orientation_from_str("ROLL_90_YAW_270")); + EXPECT_EQ( + enum_value(SO::ROTATION_ROLL_90_YAW_270), + utils::sensor_orientation_from_str("ROLL_90_YAW_270")); } TEST(UTILS, sensor_orientation_from_str__custom) { - EXPECT_EQ(enum_value(SO::ROTATION_CUSTOM), utils::sensor_orientation_from_str("CUSTOM")); + EXPECT_EQ(enum_value(SO::ROTATION_CUSTOM), utils::sensor_orientation_from_str("CUSTOM")); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/mavros/test/test_uas.cpp b/mavros/test/test_uas.cpp new file mode 100644 index 000000000..f207f5eba --- /dev/null +++ b/mavros/test/test_uas.cpp @@ -0,0 +1,263 @@ +// +// mavros +// Copyright 2021 Vladimir Ermakov, All rights reserved. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// + +/** + * Test mavros uas node + */ + +#include +#include + +#include +#include + +#include "rclcpp/executors.hpp" +#include "mavconn/interface.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin_filter.hpp" + +using ::testing::_; +using ::testing::Return; + +using namespace mavros; // NOLINT +using namespace mavros::uas; // NOLINT +using namespace mavros::plugin; // NOLINT + +using namespace mavconn; // NOLINT +using mavlink_message_t = mavlink::mavlink_message_t; +using msgid_t = mavlink::msgid_t; + +namespace mavlink +{ +namespace common +{ +using namespace mavlink::minimal; // NOLINT +namespace msg +{ +using namespace mavlink::minimal::msg; // NOLINT +} +} +} // namespace mavlink + +class MockUAS : public UAS +{ +public: + using SharedPtr = std::shared_ptr; + + explicit MockUAS(const std::string name_) + : UAS(name_) {} + + MOCK_METHOD1(create_plugin_instance, plugin::Plugin::SharedPtr(const std::string & pl_name)); +}; + +class MockPlugin : public plugin::Plugin +{ +public: + using SharedPtr = std::shared_ptr; + + explicit MockPlugin(UAS::SharedPtr uas_) + : Plugin(uas_) {} + + MOCK_METHOD0(get_subscriptions, plugin::Plugin::Subscriptions(void)); + + inline plugin::Plugin::SharedPtr getptr() + { + return std::static_pointer_cast(shared_from_this()); + } + + inline Subscriptions allsubs() + { + return { + make_handler(mavlink::common::msg::STATUSTEXT::MSG_ID, &MockPlugin::handle_statustext_raw), + make_handler(&MockPlugin::handle_heartbeat_anyok), + make_handler(&MockPlugin::handle_heartbeat_systemandok), + make_handler(&MockPlugin::handle_heartbeat_componentandok) + }; + } + + inline Subscriptions rawsubs() + { + return { + make_handler(mavlink::common::msg::STATUSTEXT::MSG_ID, &MockPlugin::handle_statustext_raw), + }; + } + + MOCK_METHOD2(handle_statustext_raw, void(const mavlink_message_t * msg, const Framing framing)); + MOCK_METHOD3( + handle_heartbeat_anyok, + void(const mavlink_message_t * msg, mavlink::common::msg::HEARTBEAT & hb, + filter::AnyOk filter)); + MOCK_METHOD3( + handle_heartbeat_systemandok, + void(const mavlink_message_t * msg, mavlink::common::msg::HEARTBEAT & hb, + filter::SystemAndOk filter)); + MOCK_METHOD3( + handle_heartbeat_componentandok, + void(const mavlink_message_t * msg, mavlink::common::msg::HEARTBEAT & hb, + filter::ComponentAndOk filter)); +}; + +namespace mavros +{ +namespace uas +{ + +class TestUAS : public ::testing::Test +{ +public: + TestUAS() + { + // std::cout << "init" << std::endl; + rclcpp::init(0, nullptr); + } + + ~TestUAS() + { + // NOTE(vooon): required to remove any remaining Nodes + // std::cout << "kill" << std::endl; + rclcpp::shutdown(); + } + + MockUAS::SharedPtr create_node() + { + auto uas = std::make_shared("test_mavros_uas"); + uas->startup_delay_timer->cancel(); + return uas; + } + + void set_lists(MockUAS::SharedPtr p, UAS::StrV denylist, UAS::StrV allowlist) + { + p->plugin_denylist = denylist; + p->plugin_allowlist = allowlist; + } + + bool is_plugin_allowed(MockUAS::SharedPtr p, const std::string pl_name) + { + return p->is_plugin_allowed(pl_name); + } + + void add_plugin(MockUAS::SharedPtr p, const std::string & pl_name) + { + p->add_plugin(pl_name); + } + + void plugin_route(MockUAS::SharedPtr p, const mavlink_message_t * msg, const Framing framing) + { + p->plugin_route(msg, framing); + } + + mavlink_message_t convert_message(const mavlink::Message & msg, const uint16_t source = 0x0101) + { + mavlink_message_t ret; + mavlink::MsgMap map(ret); + ret.sysid = source >> 8; + ret.compid = source & 0xFF; + msg.serialize(map); + + return ret; + } + + mavlink::common::msg::HEARTBEAT make_heartbeat() + { + using mavlink::common::MAV_AUTOPILOT; + using mavlink::common::MAV_MODE; + using mavlink::common::MAV_STATE; + using mavlink::common::MAV_TYPE; + + mavlink::common::msg::HEARTBEAT hb = {}; + hb.type = static_cast(MAV_TYPE::ONBOARD_CONTROLLER); + hb.autopilot = static_cast(MAV_AUTOPILOT::INVALID); + hb.base_mode = static_cast(MAV_MODE::MANUAL_ARMED); + hb.custom_mode = 0; + hb.system_status = static_cast(MAV_STATE::ACTIVE); + + return hb; + } +}; + +TEST_F(TestUAS, is_plugin_allowed) +{ + auto uas = create_node(); + + // by default all allowed + set_lists(uas, {}, {}); + EXPECT_EQ(true, is_plugin_allowed(uas, "test1")); + EXPECT_EQ(true, is_plugin_allowed(uas, "test2")); + + // check denylist + set_lists(uas, {"test1", "prefix*", "*suffix"}, {}); + EXPECT_EQ(false, is_plugin_allowed(uas, "test1")); + EXPECT_EQ(true, is_plugin_allowed(uas, "test2")); + EXPECT_EQ(false, is_plugin_allowed(uas, "prefix_test")); + EXPECT_EQ(false, is_plugin_allowed(uas, "test_suffix")); + + // check allowlist + set_lists(uas, {"*"}, {"test1", "prefix*", "*suffix"}); + EXPECT_EQ(true, is_plugin_allowed(uas, "test1")); + EXPECT_EQ(false, is_plugin_allowed(uas, "test2")); + EXPECT_EQ(true, is_plugin_allowed(uas, "prefix_test")); + EXPECT_EQ(true, is_plugin_allowed(uas, "test_suffix")); +} + +TEST_F(TestUAS, add_plugin__route_message__filter) +{ + auto uas = create_node(); + auto plugin1 = std::make_shared(uas); + auto subs1 = plugin1->allsubs(); + auto plugin2 = std::make_shared(uas); + auto subs2 = plugin2->rawsubs(); + + // XXX(vooon): silence leak warnings: they work badly with shared_ptr + testing::Mock::AllowLeak(&(*uas)); + testing::Mock::AllowLeak(&(*plugin1)); + testing::Mock::AllowLeak(&(*plugin2)); + + mavlink::common::msg::STATUSTEXT stxt{}; + auto m_stxt = convert_message(stxt, 0x0000); + + auto hb = make_heartbeat(); + auto m_hb11 = convert_message(hb, 0x0101); + auto m_hb12 = convert_message(hb, 0x0102); + auto m_hb21 = convert_message(hb, 0x0201); + + EXPECT_CALL(*uas, create_plugin_instance("test1")).WillRepeatedly(Return(plugin1)); + EXPECT_CALL(*uas, create_plugin_instance("test2")).WillRepeatedly(Return(plugin2)); + EXPECT_CALL(*plugin1, get_subscriptions()).WillRepeatedly(Return(subs1)); + EXPECT_CALL(*plugin2, get_subscriptions()).WillRepeatedly(Return(subs2)); + + add_plugin(uas, "test1"); + add_plugin(uas, "test2"); + + EXPECT_CALL(*plugin1, handle_statustext_raw).Times(2); + EXPECT_CALL(*plugin2, handle_statustext_raw).Times(2); + EXPECT_CALL(*plugin1, handle_heartbeat_anyok).Times(3); + EXPECT_CALL(*plugin1, handle_heartbeat_systemandok).Times(2); + EXPECT_CALL(*plugin1, handle_heartbeat_componentandok).Times(1); + + plugin_route(uas, &m_stxt, Framing::ok); + plugin_route(uas, &m_stxt, Framing::bad_crc); + plugin_route(uas, &m_hb21, Framing::ok); // AnyOk + plugin_route(uas, &m_hb12, Framing::ok); // AnyOk, SystemAndOk + plugin_route(uas, &m_hb11, Framing::ok); // AnyOk, SystemAndOk, ComponentAndOk + plugin_route(uas, &m_hb11, Framing::bad_crc); // none + + testing::Mock::VerifyAndClearExpectations(&(*uas)); + testing::Mock::VerifyAndClearExpectations(&(*plugin1)); + testing::Mock::VerifyAndClearExpectations(&(*plugin2)); +} + +} // namespace uas +} // namespace mavros + +int main(int argc, char ** argv) +{ + // rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/mavros/tools/cogall.sh b/mavros/tools/cogall.sh index cdb82d1de..ae08c266d 100755 --- a/mavros/tools/cogall.sh +++ b/mavros/tools/cogall.sh @@ -2,4 +2,4 @@ set -x -fgrep '[[[cog' --exclude 'cogall.sh' --exclude '*.md' --exclude '*.swp' -lr | cog.py -cr @- +fgrep '[[[cog' --exclude 'cogall.sh' --exclude '*.md' --exclude '*.swp' -lr | python3 -m cogapp -cr @- diff --git a/mavros/tools/uncrustify-cpp.cfg b/mavros/tools/uncrustify-cpp.cfg deleted file mode 100644 index 4f1c17b04..000000000 --- a/mavros/tools/uncrustify-cpp.cfg +++ /dev/null @@ -1,125 +0,0 @@ -tok_split_gte=false -utf8_byte=false -utf8_force=true -indent_cmt_with_tabs=true -indent_align_string=false -indent_braces=false -indent_braces_no_func=false -indent_braces_no_class=false -indent_braces_no_struct=false -indent_brace_parent=false -indent_namespace=false -indent_extern=false -indent_class=true -indent_class_colon=false -indent_else_if=false -indent_var_def_cont=false -indent_func_call_param=true -indent_func_def_param=false -indent_func_proto_param=false -indent_func_class_param=false -indent_func_ctor_var_param=false -indent_template_param=false -indent_func_param_double=false -indent_relative_single_line_comments=false -indent_col1_comment=false -indent_access_spec_body=false -indent_paren_nl=false -indent_comma_paren=false -indent_bool_paren=false -indent_first_bool_expr=false -indent_square_nl=false -indent_preserve_sql=false -indent_align_assign=true -sp_balance_nested_parens=false -align_keep_tabs=true -align_with_tabs=true -align_on_tabstop=false -align_number_right=true -align_func_params=false -align_same_func_call_params=false -align_var_def_colon=false -align_var_def_attribute=false -align_var_def_inline=false -align_right_cmt_mix=false -align_on_operator=false -align_mix_var_proto=false -align_single_line_func=false -align_single_line_brace=false -align_nl_cont=false -align_left_shift=true -align_oc_decl_colon=false -nl_collapse_empty_body=true -nl_assign_leave_one_liners=false -nl_class_leave_one_liners=false -nl_enum_leave_one_liners=false -nl_getset_leave_one_liners=false -nl_func_leave_one_liners=false -nl_if_leave_one_liners=false -nl_multi_line_cond=false -nl_multi_line_define=false -nl_before_case=false -nl_after_case=false -nl_after_return=false -nl_after_semicolon=false -nl_after_brace_open=false -nl_after_brace_open_cmt=false -nl_after_vbrace_open=false -nl_after_vbrace_open_empty=false -nl_after_brace_close=false -nl_after_vbrace_close=false -nl_define_macro=false -nl_squeeze_ifdef=false -nl_ds_struct_enum_cmt=false -nl_ds_struct_enum_close_brace=false -nl_create_if_one_liner=false -nl_create_for_one_liner=false -nl_create_while_one_liner=false -ls_for_split_full=false -ls_func_split_full=false -nl_after_multiline_comment=false -eat_blanks_after_open_brace=true -eat_blanks_before_close_brace=true -mod_full_brace_if_chain=false -mod_pawn_semicolon=false -mod_full_paren_if_bool=false -mod_remove_extra_semicolon=false -mod_sort_import=false -mod_sort_using=false -mod_sort_include=false -mod_move_case_break=false -mod_remove_empty_return=false -cmt_indent_multi=true -cmt_c_group=false -cmt_c_nl_start=false -cmt_c_nl_end=false -cmt_cpp_group=false -cmt_cpp_nl_start=false -cmt_cpp_nl_end=false -cmt_cpp_to_c=false -cmt_star_cont=false -cmt_multi_check_last=true -cmt_insert_before_preproc=false -pp_indent_at_level=false -pp_region_indent_code=false -pp_if_indent_code=false -pp_define_at_level=false -input_tab_size=8 -output_tab_size=8 -indent_columns=8 -indent_continue=8 -nl_max=3 -utf8_bom=remove -indent_with_tabs=2 -sp_arith=add -sp_assign=add -sp_bool=add -sp_compare=add -sp_after_type=add -sp_before_sparen=add -sp_else_brace=add -sp_brace_else=add -sp_catch_brace=add -sp_try_brace=add -sp_cond_colon=add -sp_cond_question=ignore diff --git a/mavros_cog.py b/mavros_cog.py index 0adb7b0e0..bf96fbbaf 100644 --- a/mavros_cog.py +++ b/mavros_cog.py @@ -3,7 +3,171 @@ Install: - ln -s $PWD/mavros_cog.py $HOME/.local/lib/python2.7/site-packages/ + ln -s $PWD/mavros_cog.py $HOME/.local/lib/python3.8/site-packages/ ''' -import cog +try: + import cog +except Exception: + _cog_present = False +else: + _cog_present = True + +import pathlib +import re +import typing +import xml.etree.ElementTree as ET # nosemgrep +from xml.dom import minidom # nosemgrep + +import attr +from comment_parser import comment_parser + +CPP_MIME = 'text/x-c++' +REGISTER_PLUGIN_RE = re.compile( + r'MAVROS_PLUGIN_REGISTER\((?P[a-zA-Z0-9_\:]+)\)') +PLUGIN_NAME_RE = re.compile(r'@plugin\ (?P[a-z0-9_]+)') +PLUGIN_BRIEF_RE = re.compile(r'^@brief\ (?P.+)$') + +plugins_without_macro = { + "mavros/src/plugins/mission_protocol_base.cpp" +} + +if _cog_present: + + def dbg(s): + cog.msg(s) +else: + + def dbg(s): + print(s) + + +class NoPluginRegister(Exception): + """ + That Exception returned if plugin registration not found + """ + pass + + +@attr.s(auto_attribs=True) +class PluginInfo: + """ + Info parsed from plugin source file + """ + path: pathlib.Path + klass: str + name: str + description: str + is_example: bool + + @property + def factory_klass(self) -> str: + return f"mavros::plugin::PluginFactoryTemplate<{self.klass}>" + + @property + def as_xml(self) -> ET.Element: + ret = ET.Element('class', + name=self.name, + type=self.factory_klass, + base_class_type='mavros::plugin::PluginFactory') + desc = ET.SubElement(ret, 'description') + desc.text = self.description + + if not self.is_example: + return ret + else: + return ET.Comment(et_to_str(ret)) + + @property + def sort_key(self): + return (not self.is_example, self.name) + + @classmethod + def parse_file(cls, path: pathlib.Path) -> 'PluginInfo': + with path.open('r') as fd: + source = fd.read() + + m = REGISTER_PLUGIN_RE.search(source) + if m is None: + raise NoPluginRegister( + f"plugin registration macro not found in file: {path}") + + klass = m.group('klass') + comments = comment_parser.extract_comments_from_str(source, CPP_MIME) + + # assume that only one plugin is allowed per file + comment = next(c.text() for c in comments if '@plugin' in c.text()) + + # clean comment from * + def remove_prefix(line: str) -> str: + line = line.strip() + if line.startswith('*'): + line = line[1:] + return line.strip() + + comment = '\n'.join( + remove_prefix(line) for line in comment.splitlines()).strip() + + m = PLUGIN_NAME_RE.search(comment) + name = m.group('name') + + is_example = '@example_plugin' in comment + + # TODO(vooon): reformat comment for description + + return cls( + path=path, + klass=klass, + name=name, + description=comment, + is_example=is_example, + ) + + +def load_all_plugin_infos(dir: pathlib.Path) -> typing.Iterator[PluginInfo]: + for fl in dir.glob('*.cpp'): + try: + if str(fl) not in plugins_without_macro: + yield PluginInfo.parse_file(fl) + except NoPluginRegister as ex: + dbg(f"skipping file: {ex}") + except Exception as ex: + dbg(f"failed to load file {fl}: {ex}") + + +def et_to_str(root: ET.Element) -> str: + # XXX(vooon): pretty print + xml_ = ET.tostring(root) + xml_ = minidom.parseString(xml_).toprettyxml(indent=' ') + return '\n'.join(xml_.splitlines()[1:]) # remove + + +def cwd() -> pathlib.Path: + if _cog_present: + return pathlib.Path(cog.inFile).parent + else: + return pathlib.Path('.') + + +def outl(s: str): + if _cog_present: + cog.outl(s) + else: + print(s) + + +def outl_plugins_xml(dir: str, lib_path: str): + plugins = sorted(load_all_plugin_infos(cwd() / dir), + key=lambda p: p.sort_key) + + root = ET.Element("library", path=lib_path) + for pl in plugins: + root.append(pl.as_xml) + + xml_ = et_to_str(root) + outl(xml_) + + +def outl_glob_files(dir: str, glob: str = "*.cpp"): + for f in sorted((cwd() / dir).glob(glob)): + outl(str(f.relative_to(cwd()))) diff --git a/mavros_extras/CHANGELOG.rst b/mavros_extras/CHANGELOG.rst index b66df9f25..3fdfae5ce 100644 --- a/mavros_extras/CHANGELOG.rst +++ b/mavros_extras/CHANGELOG.rst @@ -2,12 +2,111 @@ Changelog for package mavros_extras ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.16.0 (2023-05-05) -------------------- -* Merge pull request `#1817 `_ from lucasw/pluginlib_hpp - use hpp instead of deprecated .h pluginlib headers -* use hpp instead of deprecated .h pluginlib headers -* Contributors: Lucas Walter, Vladimir Ermakov +2.5.0 (2023-05-05) +------------------ + +2.4.0 (2022-12-30) +------------------ +* extras: uncrustify +* extras: fix build, 2 +* extras: fix build +* extras: fix cog +* Merge branch 'master' into ros2 + * master: + 1.15.0 + update changelog + ci: update actions + Implement debug float array handler + mavros_extras: Fix a sequence point warning + mavros_extras: Fix a comparison that shouldn't be bitwise + mavros: Fix some warnings + mavros_extras: Fix buggy check for lat/lon ignored + libmavconn: fix MAVLink v1.0 output selection +* 1.15.0 +* update changelog +* Merge pull request `#1811 `_ from scoutdi/debug-float-array + Implement debug float array handler +* Implement debug float array handler + Co-authored-by: Morten Fyhn Amundsen +* Merge pull request `#1807 `_ from scoutdi/fix-bitwise-comparison + mavros_extras: Fix a comparison that shouldn't be bitwise +* Merge pull request `#1808 `_ from scoutdi/fix-sequence-point-warning + mavros_extras: Fix a sequence point warning +* mavros_extras: Fix a sequence point warning +* mavros_extras: Fix a comparison that shouldn't be bitwise +* Merge pull request `#1805 `_ from scoutdi/fix-latlon-check + mavros_extras: Fix buggy check for lat/lon ignored +* mavros_extras: Fix buggy check for lat/lon ignored +* Contributors: Morten Fyhn Amundsen, Sverre Velten Rothmund, Vladimir Ermakov + +2.3.0 (2022-09-24) +------------------ +* extras: fix linter errors +* extras: fix toMsg +* extras: fix build +* extras: port guided_target +* mavros: remove custom find script, re-generate +* Merge branch 'master' into ros2 + * master: + 1.14.0 + update changelog + scripts: waypoint and param files are text, not binary + libmavconn: fix MAVLink v1.0 output selection + plugins: add guided_target to accept offboard position targets + add cmake module path for geographiclib on debian based systems + use already installed FindGeographicLib.cmake +* 1.14.0 +* update changelog +* Merge pull request `#1780 `_ from snktshrma/master + guided_target: accept position-target-global-int messages +* plugins: add guided_target to accept offboard position targets + Update guided_target.cpp + Update guided_target.cpp + Update mavros_plugins.xml + Update CMakeLists.txt + Added offboard_position.cpp + Update apm_config.yaml + Update offboard_position.cpp + Update offboard_position.cpp + Rename offboard_position.cpp to guided_target.cpp + Update CMakeLists.txt + Update mavros_plugins.xml + Update apm_config.yaml + Update guided_target.cpp +* Contributors: Sanket Sharma, Vladimir Ermakov + +2.2.0 (2022-06-27) +------------------ +* extras: fix build +* extras: fix build +* Merge branch 'master' into ros2 + * master: + mount_control.cpp: detect MOUNT_ORIENTATION stale messages + ESCTelemetryItem.msg: correct RPM units + apm_config.yaml: add mount configuration + sys_status.cpp fix free memory for values > 64KiB + uncrustify cellular_status.cpp + Add CellularStatus plugin and message + *_config.yaml: document usage of multiple batteries diagnostics + sys_status.cpp: fix compilation + sys_status.cpp: support diagnostics on up-to 10 batteries + sys_status.cpp: do not use harcoded constants + sys_status.cpp: Timeout on MEMINFO and HWSTATUS mavlink messages and publish on the diagnostics + sys_status.cpp: fix enabling of mem_diag and hwst_diag + sys_status.cpp: Do not use battery1 voltage as voltage for all other batteries (bugfix). + sys_status.cpp: ignore sys_status mavlink messages from gimbals + mount_control.cpp: use mount_nh for params to keep similarities with other plugins set diag settings before add() + sys_status.cpp: remove deprecated BATTERY2 mavlink message support + Mount control plugin: add configurable diagnostics + Bugfix: increment_f had no value asigned when input LaserScan was bigger than obstacle.distances.size() + Bugfix: wrong interpolation when the reduction ratio (scale_factor) is not integer. + Disable startup_px4_usb_quirk in px4_config.yaml +* cmake: style fix +* cmake: downgrade to C++17 as 20 breaks something in rclcpp +* cmake: hide -std=c++2a +* Merge pull request `#1744 `_ from amilcarlucas/pr_gimbal_diagnostics_fixes + mount_control.cpp: detect MOUNT_ORIENTATION stale messages +* extras: fix cog re to extract plugin name 1.15.0 (2022-12-30) ------------------- @@ -50,6 +149,10 @@ Changelog for package mavros_extras correct MountConfigure response success correct constructor initialization order some gimbals send negated/inverted angle measurements, correct that to obey the MAVLink frame convention using run-time parameters +* Merge pull request `#1735 `_ from clydemcqueen/fix_1734 + Fix crash in vision_pose plugin +* Remove unrelated log message +* Initialize last_transform_stamp with RCL_ROS_TIME, fixes `#1734 `_ * Merge pull request `#1727 `_ from BV-OpenSource/pr-cellular-status Pr cellular status * uncrustify cellular_status.cpp @@ -63,7 +166,304 @@ Changelog for package mavros_extras Fix obstacle distance * Bugfix: increment_f had no value asigned when input LaserScan was bigger than obstacle.distances.size() * Bugfix: wrong interpolation when the reduction ratio (scale_factor) is not integer. -* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Rui Mendes, Sanket Sharma, Vladimir Ermakov, oroel +* Contributors: Clyde McQueen, Dr.-Ing. Amilcar do Carmo Lucas, Rui Mendes, Vladimir Ermakov, oroel + +2.1.1 (2022-03-02) +------------------ +* plugins: Fix misprint + Fix `#1709 `_ +* Contributors: Vladimir Ermakov + +2.1.0 (2022-02-02) +------------------ +* plugins: fix topic names to use prefix for namespaced ones +* plugins: fix topic names to use prefix for namespaced ones +* ci: fix several lint warnings +* extras: terrain: fix copy-paste artifact +* extras: port terrain plugin +* Merge branch 'master' into ros2 + * master: + 1.13.0 + update changelog + py-lib: fix compatibility with py3 for Noetic + re-generate all coglets + test: add checks for ROTATION_CUSTOM + lib: Fix rotation search for CUSTOM + Removed CamelCase for class members. Publish to "report" + More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages + Fixed callback name to match `handle\_{MESSAGE_NAME.lower()}` convention + Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html + Fixed topic names to match more closely what other plugins use. Fixed a typo. + Add plugin for reporting terrain height estimate from FCU + 1.12.2 + update changelog + Set time/publish_sim_time to false by default + plugin: setpoint_raw: move getParam to initializer + extras: trajectory: backport `#1667 `_ +* 1.13.0 +* update changelog +* Merge pull request `#1677 `_ from AndersonRayner/add_terrain + Add plugin for reporting terrain height estimate from the FCU +* Removed CamelCase for class members. Publish to "report" +* More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages +* Fixed callback name to match `handle\_{MESSAGE_NAME.lower()}` convention +* Fixed topic names to match more closely what other plugins use. Fixed a typo. +* Add plugin for reporting terrain height estimate from FCU +* 1.12.2 +* update changelog +* Merge pull request `#1675 `_ from BOB4Drone/ros2 + fix bof +* Update mag_calibration_status.cpp +* fix code style + fix code style +* Update mag_calibration_status.cpp +* fix misprint and use size() + fix misprint and use size() +* fix bof + fix `#1668 `_ +* extras: trajectory: backport `#1667 `_ +* extras: trajectory: make linter happy after `#1667 `_ +* Merge pull request `#1667 `_ from BOB4Drone/ros2 + fix bof +* fix bof +* Merge branch 'master' into ros2 + * master: + 1.12.1 + update changelog + mavconn: fix connection issue introduced by `#1658 `_ + mavros_extras: Fix some warnings + mavros: Fix some warnings +* 1.12.1 +* update changelog +* Merge pull request `#1660 `_ from scoutdi/fix-warnings + Fix warnings +* mavros_extras: Fix some warnings +* extras: fix parameter name +* extras: fix topic names +* Contributors: BOB4Drone, Morten Fyhn Amundsen, Vladimir Ermakov, matt + +2.0.5 (2021-11-28) +------------------ +* extras: make cpplint happy +* extras: fix most of build errors of SSP +* extras: servo_state_publisher ported. almost... +* extras: start porting servo_state_publisher +* extras: make cpplint happy +* extras: fix some linter errors. + Do you know how to make me mad? Just let ament_uncrustify and + ament_cpplint require opposite requirements! +* fix some build warnings; drop old copter vis +* Merge branch 'master' into ros2 + * master: + 1.12.0 + update changelog + Fix multiple bugs + lib: fix mission frame debug print + extras: distance_sensor: revert back to zero quaternion +* 1.12.0 +* update changelog +* extras: fix some more lint warns +* plugin: fix some compile warnings +* cmake: require C++20 to build all modules +* extras: port distance_sensor plugin +* extras: fix camera plugin +* extras: port camera plugin +* lib: ignore MAVPACKED-related warnings from mavlink +* extras: distance_sensor: revert back to zero quaternion + Fix `#1653 `_ +* msgs: update conversion header +* Merge branch 'master' into ros2 + * master: + 1.11.1 + update changelog + lib: fix build +* 1.11.1 +* update changelog +* Merge branch 'master' into ros2 + * master: + 1.11.0 + update changelog + lib: fix ftf warnings + msgs: use pragmas to ignore unaligned pointer warnings + extras: landing_target: fix misprint + msgs: fix convert const + plugin: setpoint_raw: fix misprint + msgs: try to hide 'unaligned pointer' warning + plugin: sys: fix compillation error + plugin: initialize quaternions with identity + plugin: sys: Use wall timers for connection management + Use meters for relative altitude + distance_sensor: Initialize sensor orientation quaternion to zero + Address review comments + Add camera plugin for interfacing with mavlink camera protocol +* 1.11.0 +* update changelog +* extras: landing_target: fix misprint +* plugin: initialize quaternions with identity + Eigen::Quaternion[d|f] () does not initialize with zeroes or identity. + So we must initialize with identity vector objects that can be left + unassigned. + Related to `#1652 `_ +* Merge pull request `#1651 `_ from Jaeyoung-Lim/pr-image-capture-plugin + Add camera plugin for interfacing with mavlink camera protocol +* Merge pull request `#1652 `_ from scoutdi/avoid-uninit-orientation + distance_sensor: Initialize sensor orientation quaternion to zero +* Use meters for relative altitude +* distance_sensor: Initialize sensor orientation quaternion to zero + Without this, you'll get random garbage data for the quaternion field + of the DISTANCE_SENSOR MAVLink messages sent to the autopilot. + The quaternion field should be set to zero when unused, according to the + MAVLink message's field description. +* Address review comments +* Add camera plugin for interfacing with mavlink camera protocol + Add camera image captured message for handling camera trigger information +* extras: port fake_gps +* extras: port tunnel +* extras: update metadata +* extras: port hil +* extras: fix odom +* extras: port odom +* extras: port px4flow +* extras: fix some linter warnings +* extras: fix some linter warnings +* extras: fix some linter warnings +* extras: fix some linter warnings +* extras: port wheel_odometry (partially) +* extras: port vision_speed +* extras: port vibration +* extras: port vfr_hud +* extras: port trajectory +* extras: port rangefinder +* extras: port onboard computer status, play_tune +* extras: fix linter warnings +* extras: port obstacle_distance +* extras: update metadata xml +* extras: port mount_control +* extras: fix build for Foxy +* extras: port mocap +* extras: port mag cal status +* extras: port log_transfer +* extras: fix rtcm seq +* extras: port gps_rtk +* extras: port gps_input +* extras: fixing some linter warnings +* extras: fixing some linter warnings +* Contributors: Jaeyoung-Lim, Morten Fyhn Amundsen, Vladimir Ermakov + +2.0.4 (2021-11-04) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.10.0 + prepare release +* 1.10.0 +* prepare release +* extras: remove safety_area as outdated +* extras: port esc_telemetry +* extras: port esc_status plugin +* extras: porting gps_status +* Merge branch 'master' into ros2 + * master: (25 commits) + Remove reference + Catch std::length_error in send_message + Show ENOTCONN error instead of crash + Tunnel: Check for invalid payload length + Tunnel.msg: Generate enum with cog + mavros_extras: Create tunnel plugin + mavros_msgs: Add Tunnel message + MountControl.msg: fix copy-paste + sys_time.cpp: typo + sys_time: publish /clock for simulation times + 1.9.0 + update changelog + Spelling corrections + Changed OverrideRCIn to 18 channels + This adds functionality to erase all logs on the SD card via mavlink + publish BATTERY2 message as /mavros/battery2 topic + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 + Added NAV_CONTROLLER_OUTPUT Plugin + Added GPS_INPUT plugin + Update esc_status plugin with datatype change on MAVLink. + ... +* Merge pull request `#1625 `_ from scoutdi/tunnel-plugin + Plugin for TUNNEL messages +* Tunnel: Check for invalid payload length +* mavros_extras: Create tunnel plugin +* Merge pull request `#1605 `_ from Peter010103/ros2 + mavros_extras: Ported vision_pose_estimate plugin for ROS2 +* 1.9.0 +* update changelog +* Merge pull request `#1621 `_ from amilcarlucas/pr/mount-control-spelling + Spelling corrections +* Spelling corrections +* Merge pull request `#1615 `_ from amilcarlucas/pr/erase-logs + This adds functionality to erase all logs on the SD card via mavlink +* Merge pull request `#1618 `_ from amilcarlucas/pr/GPS_INPUT-plugin + Added GPS_INPUT plugin +* This adds functionality to erase all logs on the SD card via mavlink +* Added GPS_INPUT plugin +* Merge pull request `#1606 `_ from BV-OpenSource/master + Add Mount angles message for communications with ardupilotmega. +* Merge branch 'master' into master +* Update esc_status plugin with datatype change on MAVLink. + ESC_INFO MAVLink message was updated to have negative temperates and also at a different resolution. This commit updates those changes on this side. +* Convert status data from cdeg to rad. +* Publish quaternion information with Mount Status mavlink message. +* Add missing subscription. +* extras: port cam_imu_sync +* extras: re-generate cog +* extras: port debug_value +* Remove Mount_Status plugin. Add Status data to Mount_Control plugin. Remove Mount_Status message. +* extras: fix build, add UAS::send_massage(msg, compid) +* extras: port companion_process_status +* msgs: re-generate file lists +* style: apply ament_uncrustify --reformat +* Merge branch 'master' into ros2 + * master: + extras: esc_telemetry: fix build + extras: fix esc_telemetry centi-volt/amp conversion + extras: uncrustify all plugins + plugins: reformat xml + extras: reformat plugins xml + extras: fix apm esc_telemetry + msgs: fix types for apm's esc telemetry + actually allocate memory for the telemetry information + fixed some compile errors + added esc_telemetry plugin + Reset calibration flag when re-calibrating. Prevent wrong data output. + Exclude changes to launch files. + Delete debug files. + Apply uncrustify changes. + Set progress array to global to prevent erasing data. + Move Compass calibration report to extras. Rewrite code based on instructions. + Remove extra message from CMakeLists. + Add message and service definition. + Add compass calibration feedback status. Add service to call the 'Next' button in calibrations. +* extras: esc_telemetry: fix build +* extras: fix esc_telemetry centi-volt/amp conversion +* extras: uncrustify all plugins +* extras: reformat plugins xml +* extras: fix apm esc_telemetry +* actually allocate memory for the telemetry information +* fixed some compile errors +* added esc_telemetry plugin +* Add Mount angles message for communications with ardupilotmega. +* Added subscriber callback function for ROS2 +* Added initialise function in vision_pose_estimate +* Boilerplate vision_pose_estimate plugin +* extras: landing_target: disable tf listener, it segfaults +* extras: regenerate plugins xml, ament_uncrustify +* mavros_extras: improve landing_target logging +* mavros_extras: ported landing_target plugin to ros2 +* Reset calibration flag when re-calibrating. Prevent wrong data output. +* Delete debug files. +* Apply uncrustify changes. +* Set progress array to global to prevent erasing data. +* Move Compass calibration report to extras. Rewrite code based on instructions. +* extras: port 3dr radio +* extras: add urdf package +* extras: adsb: begin porting to ros2 +* Contributors: Abhijith Thottumadayil Jagadeesh, André Filipe, David Jablonski, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Morten Fyhn Amundsen, Peter010103, Ricardo Marques, Russell, Vladimir Ermakov 1.13.0 (2022-01-13) ------------------- diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt index 0b19ac443..d8d1e568d 100644 --- a/mavros_extras/CMakeLists.txt +++ b/mavros_extras/CMakeLists.txt @@ -1,76 +1,98 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(mavros_extras) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - mavros - mavros_msgs - roscpp - sensor_msgs - std_msgs - tf - tf2_eigen - urdf - visualization_msgs -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) - -find_package(Eigen3) -if(NOT EIGEN3_FOUND) - # Fallback to cmake_modules - find_package(cmake_modules REQUIRED) - find_package(Eigen REQUIRED) - set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) - set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) +# Default to C++20 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++2a") endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # we dont use add_compile_options with pedantic in message packages + # because the Python C extensions dont comply with it + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wcomment") + +# Allow GNU extensions (-std=gnu++20) +set(CMAKE_C_EXTENSIONS ON) +set(CMAKE_CXX_EXTENSIONS ON) + +find_package(ament_cmake REQUIRED) + +# find mavros dependencies +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rcpputils REQUIRED) +find_package(message_filters REQUIRED) + +find_package(mavlink REQUIRED) +find_package(mavros REQUIRED) +find_package(libmavconn REQUIRED) + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +# find_package(yaml_cpp REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) + +## Find GeographicLib +# Append to CMAKE_MODULE_PATH since debian/ubuntu installs +# FindGeographicLib.cmake in a nonstand location +set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib") +find_package(GeographicLib REQUIRED) + +find_package(angles REQUIRED) +find_package(eigen_stl_containers REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(urdf REQUIRED) + +find_package(diagnostic_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) + +find_package(geographic_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) -include(EnableCXX11) -include(MavrosMavlink) - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - - - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES - CATKIN_DEPENDS geometry_msgs mavros mavros_msgs roscpp sensor_msgs std_msgs visualization_msgs - DEPENDS Boost +include_directories( + $ + $ ) -########### -## Build ## -########### - include_directories( - ${catkin_INCLUDE_DIRS} ${mavlink_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GeographicLib_INCLUDE_DIRS} ) -add_library(mavros_extras +if(rclcpp_VERSION VERSION_LESS 9.0.0) + add_definitions( + -DUSE_OLD_DECLARE_PARAMETER + ) +endif() + + +# [[[cog: +# import mavros_cog +# ]]] +# [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) + +add_library(mavros_extras_plugins SHARED + # [[[cog: + # mavros_cog.outl_glob_files('src/plugins') + # ]]] + src/plugins/3dr_radio.cpp src/plugins/adsb.cpp src/plugins/cam_imu_sync.cpp src/plugins/camera.cpp src/plugins/cellular_status.cpp src/plugins/companion_process_status.cpp - src/plugins/onboard_computer_status.cpp src/plugins/debug_value.cpp src/plugins/distance_sensor.cpp src/plugins/esc_status.cpp @@ -79,6 +101,8 @@ add_library(mavros_extras src/plugins/gps_input.cpp src/plugins/gps_rtk.cpp src/plugins/gps_status.cpp + src/plugins/guided_target.cpp + src/plugins/hil.cpp src/plugins/landing_target.cpp src/plugins/log_transfer.cpp src/plugins/mag_calibration_status.cpp @@ -86,90 +110,97 @@ add_library(mavros_extras src/plugins/mount_control.cpp src/plugins/obstacle_distance.cpp src/plugins/odom.cpp + src/plugins/onboard_computer_status.cpp src/plugins/play_tune.cpp src/plugins/px4flow.cpp src/plugins/rangefinder.cpp src/plugins/terrain.cpp src/plugins/trajectory.cpp src/plugins/tunnel.cpp + src/plugins/vfr_hud.cpp src/plugins/vibration.cpp src/plugins/vision_pose_estimate.cpp src/plugins/vision_speed_estimate.cpp src/plugins/wheel_odometry.cpp - src/plugins/guided_target.cpp -) -add_dependencies(mavros_extras - ${catkin_EXPORTED_TARGETS} + # [[[end]]] (checksum: a6dc36e6871deb96f56d61e2a1a38f37) ) -target_link_libraries(mavros_extras - ${mavros_LIBRARIES} - ${catkin_LIBRARIES} +ament_target_dependencies(mavros_extras_plugins + angles + geometry_msgs + geographic_msgs + mavros + mavros_msgs + std_msgs + std_srvs + sensor_msgs + pluginlib + nav_msgs + trajectory_msgs + rclcpp + rclcpp_components + rcpputils + libmavconn + diagnostic_updater + tf2_ros + tf2_eigen + message_filters + Eigen3 + yaml_cpp_vendor ) - -## Declare a cpp executable -add_executable(visualization - src/visualization.cpp +pluginlib_export_plugin_description_file(mavros mavros_plugins.xml) + +add_library(mavros_extras SHARED + # [[[cog: + # mavros_cog.outl_glob_files('src/lib') + # ]]] + src/lib/servo_state_publisher.cpp + # [[[end]]] (checksum: a3ce43c71c567f697861bcbcd0f25aa3) ) -target_link_libraries(visualization - ${catkin_LIBRARIES} +ament_target_dependencies(mavros_extras + rclcpp + rclcpp_components + std_msgs + sensor_msgs + mavros_msgs + #console_bridge + yaml_cpp_vendor + urdf ) +rclcpp_components_register_node(mavros_extras PLUGIN "mavros::extras::ServoStatePublisher" EXECUTABLE servo_state_publisher) -add_executable(servo_state_publisher - src/servo_state_publisher.cpp +install(TARGETS mavros_extras mavros_extras_plugins + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -target_link_libraries(servo_state_publisher - ${catkin_LIBRARIES} -) - -############# -## Install ## -############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -file(GLOB SCRIPTS ${PROJECT_SOURCE_DIR}/scripts/*) -install(PROGRAMS - ${SCRIPTS} - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) -## Mark executables and/or libraries for installation -install(TARGETS mavros_extras visualization servo_state_publisher - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} ) -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) -## Mark other files for installation (e.g. launch and bag files, etc.) -install(FILES - mavros_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) + find_package(ament_lint_auto REQUIRED) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) + # NOTE(vooon): Does not support our custom triple-license, tiered to make it to work. + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_mavros_extras.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() + ament_lint_auto_find_test_dependencies() +endif() -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +#ament_export_dependencies(console_bridge) +ament_export_include_directories(include) +ament_export_libraries(mavros_extras) +ament_export_dependencies(eigen3_cmake_module) +ament_export_dependencies(Eigen3) +#ament_export_targets(mavros_node) +ament_package() # vim: ts=2 sw=2 et: diff --git a/mavros_extras/include/mavros_extras/servo_state_publisher.hpp b/mavros_extras/include/mavros_extras/servo_state_publisher.hpp new file mode 100644 index 000000000..9c62db1d0 --- /dev/null +++ b/mavros_extras/include/mavros_extras/servo_state_publisher.hpp @@ -0,0 +1,145 @@ +/* + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief Publish servo states as JointState message + * @file + * @author Vladimir Ermakov + */ + +#pragma once + +#ifndef MAVROS_EXTRAS__SERVO_STATE_PUBLISHER_HPP_ +#define MAVROS_EXTRAS__SERVO_STATE_PUBLISHER_HPP_ + +#include +#include + +#include +#include +#include +#include +#include // NOLINT + +#include "rclcpp/macros.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/string.hpp" +#include "mavros_msgs/msg/rc_out.hpp" +#include "sensor_msgs/msg/joint_state.hpp" + +namespace mavros +{ +namespace extras +{ + +/** + * ServoDescription captures configuration for one joint. + */ +class ServoDescription +{ +public: + std::string joint_name; + float joint_lower; + float joint_upper; + + size_t rc_channel; + + uint16_t rc_min; + uint16_t rc_max; + uint16_t rc_trim; + uint16_t rc_dz; + bool rc_rev; + + explicit ServoDescription(std::string joint_name_ = {}) + : joint_name(joint_name_), + joint_lower(-M_PI / 4), + joint_upper(M_PI / 4), + rc_channel(0), + rc_min(1000), + rc_max(2000), + rc_trim(1500), + rc_dz(0), + rc_rev(false) + {} + + ServoDescription(urdf::Model & model, std::string joint_name_, YAML::Node config); + + /** + * Normalization code taken from PX4 Firmware + * src/modules/sensors/sensors.cpp Sensors::rc_poll() line 1966 + */ + inline float normalize(uint16_t pwm) + { + // 1) fix bounds + pwm = std::max(pwm, rc_min); + pwm = std::min(pwm, rc_max); + + // 2) scale around mid point + float chan; + if (pwm > (rc_trim + rc_dz)) { + chan = (pwm - rc_trim - rc_dz) / static_cast(rc_max - rc_trim - rc_dz); + } else if (pwm < (rc_trim - rc_dz)) { + chan = (pwm - rc_trim + rc_dz) / static_cast(rc_trim - rc_min - rc_dz); + } else { + chan = 0.0; + } + + if (rc_rev) { + chan *= -1; + } + + if (!std::isfinite(chan)) { + chan = 0.0; + } + + return chan; + } + + inline float calculate_position(uint16_t pwm) + { + float channel = normalize(pwm); + + // not sure should i differently map -1..0 and 0..1 + // for now there arduino map() (explicit) + float position = (channel + 1.0) * (joint_upper - joint_lower) / (1.0 + 1.0) + joint_lower; + + return position; + } +}; + +/** + * ServoStatePublisher class implements servo_state_publisher node + * + * That node translates RC Servo outputs to URDF Joint states + */ +class ServoStatePublisher : public rclcpp::Node +{ +public: + explicit ServoStatePublisher(const std::string & node_name = "servo_state_publisher") + : ServoStatePublisher(rclcpp::NodeOptions(), node_name) {} + + explicit ServoStatePublisher( + const rclcpp::NodeOptions & options, + const std::string & node_name = "servo_state_publisher"); + +private: + rclcpp::Subscription::SharedPtr robot_description_sub; + rclcpp::Subscription::SharedPtr rc_out_sub; + rclcpp::Publisher::SharedPtr joint_states_pub; + + std::shared_mutex mutex; + std::list servos; + + void robot_description_cb(const std_msgs::msg::String::SharedPtr msg); + void rc_out_cb(const mavros_msgs::msg::RCOut::SharedPtr msg); +}; + +} // namespace extras +} // namespace mavros + +#endif // MAVROS_EXTRAS__SERVO_STATE_PUBLISHER_HPP_ diff --git a/mavros_extras/launch/f710_joy.yaml b/mavros_extras/launch/f710_joy.yaml deleted file mode 100644 index 1e243e346..000000000 --- a/mavros_extras/launch/f710_joy.yaml +++ /dev/null @@ -1,29 +0,0 @@ -# Joystic configuration for Logitech F710 gamepad -# TODO: check scale, button map -axes_map: - roll: 3 - pitch: 4 - yaw: 0 - throttle: 1 -axes_scale: - roll: 1.0 - pitch: 1.0 - yaw: 1.0 - throttle: 1.0 -button_map: - takeoff: 0 - land: 1 - enable: 2 -rc_modes: - offboard: - rc_channel: 4 - rc_value: 1000 - joy_flags: [ [2,1] ] - auto: - rc_channel: 4 - rc_value: 1500 - joy_flags: [ [3,1] ] - manual: - rc_channel: 4 - rc_value: 2000 - joy_flags: [ [0,1] ] diff --git a/mavros_extras/launch/px4_image.launch b/mavros_extras/launch/px4_image.launch deleted file mode 100644 index c28b4b00a..000000000 --- a/mavros_extras/launch/px4_image.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/mavros_extras/launch/teleop.launch b/mavros_extras/launch/teleop.launch deleted file mode 100644 index 8d01dd9e3..000000000 --- a/mavros_extras/launch/teleop.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/mavros_extras/mavros_plugins.xml b/mavros_extras/mavros_plugins.xml index d82f76bbf..30e522c40 100644 --- a/mavros_extras/mavros_plugins.xml +++ b/mavros_extras/mavros_plugins.xml @@ -1,99 +1,230 @@ - - - Publish/subscribe to the location and information of an ADS-B vehicle. + + + + @brief ADS-B Vehicle plugin +@plugin adsb + +Publish/subscribe Automatic dependent surveillance-broadcast data to/from a vehicle. + + + @brief Camera IMU synchronisation plugin +@plugin cam_imu_sync + +This plugin publishes a timestamp for when a external camera system was +triggered by the FCU. Sequence ID from the message and the image sequence from +camera can be corellated to get the exact shutter trigger time. + + + @brief Camera plugin plugin +@plugin camera + +Plugin for interfacing on the mavlink camera protocol +@see command_cb() + + + @brief Cellular status plugin. +@plugin cellular_status + +Users must publish to the topic the CellularStatus message and it +will be relayed to the mavlink components. + + + @brief Obstacle companion process status plugin +@plugin companion_process_status + +Publishes the status of components running on the companion computer +@see status_cb() + + + @brief Plugin for Debug msgs from MAVLink API +@plugin debug_value + + + @brief Distance sensor plugin +@plugin distance_sensor + +This plugin allows publishing distance sensor data, which is connected to +an offboard/companion computer through USB/Serial, to the FCU or vice-versa. + + + @brief ESC status plugin +@plugin esc_status + + + @brief ESC telemetry plugin +@plugin esc_telemetry + +APM specific plugin. + + + @brief Fake GPS plugin. +@plugin fake_gps + +Sends fake GPS from local position estimation source data (motion capture, +vision) to FCU - processed in HIL mode or out of it if parameter MAV_USEHILGPS +is set on PX4 Pro Autopilot Firmware; Ardupilot Firmware already supports it +without a flag set. + + + @brief GPS_INPUT GPS plugin. +@plugin gps_input + +Sends <a href="https://mavlink.io/en/messages/common.html#GPS_INPUT">GPS_INPUT MAVLink messages</a> + + + @brief GPS RTK plugin +@plugin gps_rtk + +1. Publish the RTCM messages from ROS to the FCU +2. Publish RTK baseline data from the FCU to ROS + + + @brief Mavlink GPS status plugin. +@plugin gps_status + +This plugin publishes GPS sensor data from a Mavlink compatible FCU to ROS. + + + @brief guided target plugin +@plugin guided_target + +Send and receive setpoint positions from FCU controller. + + + @brief Hil plugin +@plugin hil + + + @brief Landing Target plugin +@plugin landing_target + +This plugin is intended to publish the location of a landing area captured from a downward facing camera +to the FCU and/or receive landing target tracking data coming from the FCU. + + + @brief Log Transfer plugin +@plugin log_transfer + + + @brief MagCalStatus plugin. +@plugin mag_calibration_status + +Example and "how to" for users. + + + @brief MocapPoseEstimate plugin +@plugin mocap_pose_estimate + +Sends motion capture data to FCU. + + + @brief Mount Control plugin +@plugin mount_control + +Publishes Mission commands to control the camera or antenna mount. +@see command_cb() + + + @brief Obstacle distance plugin +@plugin obstacle_distance + +Publishes obstacle distance array to the FCU, in order to assist in an obstacle +avoidance flight. +@see obstacle_cb() + + + @brief Odometry plugin +@plugin odometry + +Sends odometry data to the FCU estimator and +publishes odometry data that comes from FCU. + +This plugin is following ROS REP 147. Pose is expressed in parent frame. +(Quaternion rotates from child to parent) +The twist is expressed in the child frame. + +@see odom_cb() transforming and sending odometry to fcu +@see handle_odom() receiving and transforming odometry from fcu + + + @brief Onboard Computer Status plugin +@plugin onboard_computer_status + +Publishes the status of the onboard computer +@see status_cb() + + + @brief Play Tune service +@plugin play_tune + + + @brief PX4 Optical Flow plugin +@plugin px4flow + +This plugin can publish data from PX4Flow camera to ROS + + + @brief Ardupilot Rangefinder plugin. +@plugin rangefinder + +This plugin allows publishing rangefinder sensor data from Ardupilot FCU to ROS. + + + @brief 3DR Radio plugin. +@plugin tdr_radio + + + @brief Terrain height plugin. +@plugin terrain + +This plugin allows publishing of terrain height estimate from FCU to ROS. + + + @brief Trajectory plugin to receive planned path from the FCU and +send back to the FCU a corrected path (collision free, smoothed) +@plugin trajectory + +@see trajectory_cb() + + + @brief Tunnel plugin +@plugin tunnel + + + @brief VFR HUD plugin. +@plugin vfr_hud + + + @brief Vibration plugin +@plugin vibration + +This plugin is intended to publish MAV vibration levels and accelerometer clipping from FCU. + + + @brief Vision pose estimate plugin +@plugin vision_pose + +Send pose estimation from various vision estimators +to FCU position and attitude estimators. + + + @brief Vision speed estimate plugin +@plugin vision_speed + +Send velocity estimation from various vision estimators +to FCU position and attitude estimators. + + + @brief Wheel odometry plugin. +@plugin wheel_odomotry + +This plugin allows computing and publishing wheel odometry coming from FCU wheel encoders. +Can use either wheel's RPM or WHEEL_DISTANCE messages (the latter gives better accuracy). - - Publish camera trigger data for synchronisation of IMU and camera frames. - - - Plugin for the mavlink camera protocol. - - - Send Cellular status via ROS. - - - Send companion process status report to the FCU. - - - Send component hadware status to FCU and Groundstation. - - - Subs/Pubs debug msgs from and to the FCU. - - - Publish DISTANCE_SENSOR message data from FCU or connected sensors in companion computer. - - - Publish ESC data from FCU. - - - Publish ESC telemetry data from FCU. - - - Sends fake GPS from local position estimation source data (motion capture, vision) to FCU. - - - Send GPS_INPUT messages to the FCU. - - - Publish GPS_RAW and GPS_RTK messages from FCU. - - - Sends the RTCM messages to the FCU for the RTK Fix. - - - Publish/subscribe to data stream with location of a landing area captured from a downward facing camera. - - - Expose firmware functionality, that is related to log transfer - - - Send motion capture pose estimate to FCU. - - - Send obstacle distance report to the FCU. - - - Send odometry to FCU from another estimator. - - - Send tunes for the FCU to play. - - - Publish OPTICAL_FLOW message data from FCU or PX4Flow module. - - - Publish RANGEFINDER message data from FCU sensors in companion computer. - - - Publish TERRAIN_REPORT message data from FCU to companion computer. - - - Receive planned path from the FCU and send back corrected path (collision free, smoothed) to the FCU. - - - Send and receive TUNNEL messages. - - - Compute and publish wheel odometry coming from FCU sensors. - - - Publish VIBRATION message data from FCU. - - - Send vision pose estimate to FCU. - - - Send vision speed estimate to FCU. - - - Send Mission command to control a camera or a antenna mount. - - - Send calibration status to ROS. - - - Send guided target. - + diff --git a/mavros_extras/package.xml b/mavros_extras/package.xml index 81507c603..41c37b236 100644 --- a/mavros_extras/package.xml +++ b/mavros_extras/package.xml @@ -1,14 +1,13 @@ mavros_extras - 1.16.0 + 2.5.0 Extra nodes and plugins for MAVROS. Vladimir Ermakov - Vladimir Ermakov - Amilcar Lucas + GPLv3 LGPLv3 BSD @@ -17,22 +16,77 @@ https://github.com/mavlink/mavros.git https://github.com/mavlink/mavros/issues - catkin + Vladimir Ermakov + Amilcar Lucas + + ament_cmake + ament_cmake_python + eigen3_cmake_module + eigen3_cmake_module - cmake_modules + + eigen + eigen + mavlink + mavlink + geographiclib + geographiclib + geographiclib-tools + geographiclib-tools + + + angles + diagnostic_updater + message_filters + eigen_stl_containers mavros - roscpp + libmavconn + pluginlib + tf2_ros + tf2_eigen + rclcpp + rclcpp_components + rcpputils + urdf + yaml-cpp + yaml_cpp_vendor + + + diagnostic_msgs geometry_msgs mavros_msgs + nav_msgs sensor_msgs + geographic_msgs + trajectory_msgs std_msgs + std_srvs visualization_msgs - urdf - tf - tf2_eigen + + rosidl_default_runtime + + ament_cmake_gtest + ament_cmake_gmock + ament_lint_auto + ament_lint_common + gtest + google-mock + ament_cmake + + + + + uav + mav + mavlink + plugin + apm + px4 + + diff --git a/mavros_extras/scripts/mavftpfuse b/mavros_extras/scripts/mavftpfuse deleted file mode 100755 index 4b2d65ea2..000000000 --- a/mavros_extras/scripts/mavftpfuse +++ /dev/null @@ -1,200 +0,0 @@ -#!/usr/bin/env python -# vim:set ts=4 sw=4 et: -# -# Copyright 2015 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -from __future__ import print_function - -import os.path -import sys -import argparse - -import rospy -import mavros - -from mavros import ftp -from stat import S_IFDIR, S_IFREG - -try: - from fuse import FUSE, FuseOSError, Operations, \ - fuse_get_context, ENOENT -except ImportError: - print("Fuse wrapper module not found. Please install fusepy: pip install fusepy", file=os.stderr) - os.exit(1) - - -class RosLoggingMixIn(object): - """ - Same as fuse.LoggingMixIn but uses rospy logging - """ - - def __call__(self, op, path, *args): - rospy.logdebug('-> %s %s %s', op, path, repr(args)) - ret = '[Unhandled Exception]' - try: - ret = getattr(self, op)(path, *args) - return ret - except OSError, e: - ret = str(e) - raise - finally: - rospy.logdebug('<- %s %s', op, repr(ret)) - - -def ioerror_to_oserror(foo): - def decorator(*args, **kvargs): - try: - return foo(*args, **kvargs) - except IOError as e: - raise OSError(e.errno, e.message) - - return decorator - - -class MavFtp(RosLoggingMixIn, Operations): - """ - MAVLink-FTP wrapper for mavros ftp plugin. - - Based on fusepy SFTP example. - """ - - def __init__(self): - self._attr_cache = {} - - @staticmethod - def _make_attr(dir_path, file_entry): - """ - Make stat attr dict from FileEntry object. - - Item mode are set for PX4: - - / : ROMFS (read only) - - /fs/microsd : MicroSD FAT (read/write) - """ - - uid, gid, pid = fuse_get_context() - - dir_mode, file_mode = S_IFDIR | 0555, S_IFREG | 0444 - if dir_path.startswith('/fs/microsd'): - dir_mode, file_mode = S_IFDIR | 0755, S_IFREG | 0644 - - return { - 'st_mode': file_mode if file_entry.type == ftp.FileEntry.TYPE_FILE else dir_mode, - 'st_size': file_entry.size, - 'st_uid': uid, - 'st_gid': gid, - } - - def _update_attr_cache(self, dir_path, fe_list): - rospy.logdebug("update attr cache for: %s", ', '.join((os.path.join(dir_path, fe.name) for fe in fe_list))) - self.fsyncdir(dir_path) - for fe in fe_list: - key = os.path.join(dir_path, fe.name) - self._attr_cache[key] = MavFtp._make_attr(dir_path, fe) - - def _update_attr_size(self, path, size): - if self._attr_cache.has_key(path): - self._attr_cache[path]['st_size'] = size - - def destroy(self, path): - pass - - def getattr(self, path, fh=None): - if path == '/': - uid, gid, pid = fuse_get_context() - return { - 'st_mode': S_IFDIR | 0755, - 'st_uid': uid, - 'st_gid': gid, - } - - if self._attr_cache.has_key(path): - return self._attr_cache[path] - - try: - dir_path = os.path.dirname(path) - fe_list = ftp.listdir(dir_path) - self._update_attr_cache(dir_path, fe_list) - return self._attr_cache[path] - except IOError as e: - raise OSError(e.errno, e.message) - except KeyError: - raise FuseOSError(ENOENT) - - @ioerror_to_oserror - def readdir(self, path, fh): - fe_list = ftp.listdir(path) - self._update_attr_cache(path, fe_list) - return ['.', '..'] + [fe.name for fe in fe_list if fe not in ('.', '..')] - - def fsyncdir(self, path, datasync=None, fh=None): - for k in self._attr_cache.keys(): - if k.startswith(path): - del self._attr_cache[k] - - @ioerror_to_oserror - def mkdir(self, path, mode): - ftp.mkdir(path) - - @ioerror_to_oserror - def rmdir(self, path): - self.fsyncdir(path) - ftp.rmdir(path) - - @ioerror_to_oserror - def create(self, path, mode): - with ftp.open(path, 'cw'): - return 0 - - @ioerror_to_oserror - def read(self, path, size, offset, fh): - with ftp.open(path, 'r') as fd: - self._update_attr_size(path, fd.size) - fd.seek(offset) - return str(fd.read(size)) - - @ioerror_to_oserror - def write(self, path, data, offset, fh): - with ftp.open(path, 'w') as fd: - fd.seek(offset) - fd.write(data) - self._update_attr_size(path, fd.size) - return len(data) - - @ioerror_to_oserror - def unlink(self, path): - self.fsyncdir(path) - ftp.unlink(path) - - @ioerror_to_oserror - def rename(self, old, new): - self.fsyncdir(old) - ftp.rename(old, new) - - @ioerror_to_oserror - def truncate(self, path, length, fh=None): - with ftp.open(path, 'w') as fd: - fd.truncate(length) - - -def main(): - parser = argparse.ArgumentParser(description="FUSE for MAVLink-FTP mavros plugin") - parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) - parser.add_argument('-v', '--verbose', action='store_true', help="verbose output") - parser.add_argument('-d', '--debug', action='store_true', help="libfuse debug") - parser.add_argument('path', type=str, help="mount point") - - args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) - - rospy.init_node("mavftp_fuse", log_level=rospy.DEBUG if args.verbose else None) - mavros.set_namespace(args.mavros_ns) - - fuse = FUSE(MavFtp(), args.path, foreground=True, debug=args.debug) - - -if __name__ == '__main__': - main() - diff --git a/mavros_extras/scripts/mavteleop b/mavros_extras/scripts/mavteleop deleted file mode 100755 index 96dd6799a..000000000 --- a/mavros_extras/scripts/mavteleop +++ /dev/null @@ -1,311 +0,0 @@ -#!/usr/bin/env python -# vim:set ts=4 sw=4 et: -# -# Copyright 2014 Vladimir Ermakov. -# -# This file is part of the mavros package and subject to the license terms -# in the top-level LICENSE file of the mavros repository. -# https://github.com/mavlink/mavros/tree/master/LICENSE.md - -from __future__ import print_function - -import sys -import argparse -import rospy -import mavros - -from tf.transformations import quaternion_from_euler -from sensor_msgs.msg import Joy -from std_msgs.msg import Header, Float64 -from geometry_msgs.msg import PoseStamped, TwistStamped, Vector3, Quaternion, Point -from mavros_msgs.msg import OverrideRCIn -from mavros import command -from mavros import setpoint as SP - - -def arduino_map(x, inmin, inmax, outmin, outmax): - return (x - inmin) * (outmax - outmin) / (inmax - inmin) + outmin - - -class RCChan(object): - def __init__(self, name, chan, min_pos=-1.0): - self.name = name - self.chan = chan - self.min = 1000 - self.max = 2000 - self.min_pos = min_pos - - def load_param(self): - self.chan = rospy.get_param("~rc_map/" + self.name, self.chan) - self.min = rospy.get_param("~rc_min/" + self.name, self.min) - self.max = rospy.get_param("~rc_max/" + self.name, self.max) - - def calc_us(self, pos): - # warn: limit check - return arduino_map(pos, self.min_pos, 1.0, self.min, self.max) - -class RCMode(object): - def __init__( self, name, joy_flags, rc_channel, rc_value ): - self.name = name - self.joy_flags = joy_flags - self.rc_channel = rc_channel - self.rc_value = rc_value - - @staticmethod - def load_param(ns='~rc_modes/'): - yaml = rospy.get_param(ns) - return [ RCMode( name, data['joy_flags'], data['rc_channel'], data['rc_value'] ) - for name,data in yaml.items() ] - - def is_toggled(self,joy): - for btn,flag in self.joy_flags: - if joy.buttons[btn] != flag: - return False - return True - - def apply_mode(self,joy,rc): - if self.is_toggled(joy): - rc.channels[self.rc_channel]=self.rc_value - -# Mode 2 on Logitech F710 gamepad -axes_map = { - 'roll': 3, - 'pitch': 4, - 'yaw': 0, - 'throttle': 1 -} - -axes_scale = { - 'roll': 1.0, - 'pitch': 1.0, - 'yaw': 1.0, - 'throttle': 1.0 -} - -# XXX: todo -button_map = { - 'arm' : 0, - 'disarm' : 1, - 'takeoff': 2, - 'land': 3, - 'enable': 4 -} - - -rc_channels = { - 'roll': RCChan('roll', 0), - 'pitch': RCChan('pitch', 1), - 'yaw': RCChan('yaw', 3), - 'throttle': RCChan('throttle', 2, 0.0) -} - -def arm(args, state): - try: - command.arming(value=state) - except rospy.ServiceException as ex: - fault(ex) - - if not ret.success: - rospy.loginfo("Request failed.") - else: - rospy.loginfo("Request success.") - - -def load_map(m, n): - for k, v in m.items(): - m[k] = rospy.get_param(n + k, v) - - -def get_axis(j, n): - return j.axes[axes_map[n]] * axes_scale[n] - -def get_buttons(j, n): - return j.buttons[ button_map[n]] - -def rc_override_control(args): - rospy.loginfo("MAV-Teleop: RC Override control type.") - - load_map(axes_map, '~axes_map/') - load_map(axes_scale, '~axes_scale/') - load_map(button_map, '~button_map/') - for k, v in rc_channels.items(): - v.load_param() - - rc_modes = RCMode.load_param() - rc = OverrideRCIn() - - override_pub = rospy.Publisher(mavros.get_topic("rc", "override"), OverrideRCIn, queue_size=10) - - def joy_cb(joy): - # get axes normalized to -1.0..+1.0 RPY, 0.0..1.0 T - roll = get_axis(joy, 'roll') - pitch = get_axis(joy, 'pitch') - yaw = get_axis(joy, 'yaw') - throttle = arduino_map(get_axis(joy, 'throttle'), -1.0, 1.0, 0.0, 1.0) - - rospy.logdebug("RPYT: %f, %f, %f, %f", roll, pitch, yaw, throttle) - - def set_chan(n, v): - ch = rc_channels[n] - rc.channels[ch.chan] = ch.calc_us(v) - rospy.logdebug("RC%d (%s): %d us", ch.chan, ch.name, ch.calc_us(v)) - - - set_chan('roll', roll) - set_chan('pitch', pitch) - set_chan('yaw', yaw) - set_chan('throttle', throttle) - - for m in rc_modes: - m.apply_mode(joy,rc) - - override_pub.publish(rc) - - - jsub = rospy.Subscriber("joy", Joy, joy_cb) - rospy.spin() - - -def attitude_setpoint_control(args): - rospy.loginfo("MAV-Teleop: Attitude setpoint control type.") - - load_map(axes_map, '~axes_map/') - load_map(axes_scale, '~axes_scale/') - load_map(button_map, '~button_map/') - - att_pub = SP.get_pub_attitude_pose(queue_size=10) - thd_pub = SP.get_pub_attitude_throttle(queue_size=10) - - if rospy.get_param(mavros.get_topic("setpoint_attitude", "reverse_throttle"), False): - def thd_normalize(v): - return v - else: - def thd_normalize(v): - return arduino_map(v, -1.0, 1.0, 0.0, 1.0) - - def joy_cb(joy): - # get axes normalized to -1.0..+1.0 RPY, 0.0..1.0 T - roll = get_axis(joy, 'roll') - pitch = get_axis(joy, 'pitch') - yaw = get_axis(joy, 'yaw') - throttle = thd_normalize(get_axis(joy, 'throttle')) - - rospy.logdebug("RPYT: %f, %f, %f, %f", roll, pitch, yaw, throttle) - - if get_buttons(joy,'arm') == 1: - arm(args, True) - elif get_buttons(joy,'disarm') == 1: - arm(args, False) - - # TODO: Twist variation - pose = PoseStamped(header=Header(stamp=rospy.get_rostime())) - q = quaternion_from_euler(roll, pitch, yaw) - pose.pose.orientation = Quaternion(*q) - - att_pub.publish(pose) - thd_pub.publish(data=throttle) - - - jsub = rospy.Subscriber("joy", Joy, joy_cb) - rospy.spin() - - -def velocity_setpoint_control(args): - rospy.loginfo("MAV-Teleop: Velocity setpoint control type.") - - load_map(axes_map, '~axes_map/') - load_map(axes_scale, '~axes_scale/') - load_map(button_map, '~button_map/') - - vel_pub = SP.get_pub_velocity_cmd_vel(queue_size=10) - - def joy_cb(joy): - # get axes normalized to -1.0..+1.0 RPYT - roll = get_axis(joy, 'roll') - pitch = get_axis(joy, 'pitch') - yaw = get_axis(joy, 'yaw') - throttle = get_axis(joy, 'throttle') - - rospy.logdebug("RPYT: %f, %f, %f, %f", roll, pitch, yaw, throttle) - - # Based on QGC UAS joystickinput_settargets branch - # not shure that it really need inegrating, as it done in QGC. - twist = TwistStamped(header=Header(stamp=rospy.get_rostime())) - twist.twist.linear = Vector3(x=roll, y=pitch, z=throttle) - twist.twist.angular = Vector3(z=yaw) - - vel_pub.publish(twist) - - - jsub = rospy.Subscriber("joy", Joy, joy_cb) - rospy.spin() - - -px, py, pz = 0.0, 0.0, 0.0 - -def position_setpoint_control(args): - rospy.loginfo("MAV-Teleop: Position setpoint control type.") - - load_map(axes_map, '~axes_map/') - load_map(axes_scale, '~axes_scale/') - load_map(button_map, '~button_map/') - - pos_pub = SP.get_pub_position_local(queue_size=10) - - def joy_cb(joy): - global px, py, pz - # get axes normalized to -1.0..+1.0 RPY - roll = get_axis(joy, 'roll') - pitch = get_axis(joy, 'pitch') - yaw = get_axis(joy, 'yaw') - throttle = get_axis(joy, 'throttle') - - # TODO: need integrate by time, joy_cb() called with variable frequency - px -= pitch - py += roll - pz += throttle - - rospy.logdebug("RPYT: %f, %f, %f, %f", roll, pitch, yaw, throttle) - rospy.logdebug("Point(%f %f %f)", px, py, pz) - - # Based on QGC UAS joystickinput_settargets branch - pose = PoseStamped(header=Header(stamp=rospy.get_rostime())) - pose.pose.position = Point(x=px, y=py, z=pz) - q = quaternion_from_euler(0, 0, yaw) - pose.pose.orientation = Quaternion(*q) - - pos_pub.publish(pose) - - - jsub = rospy.Subscriber("joy", Joy, joy_cb) - rospy.spin() - - -def main(): - parser = argparse.ArgumentParser(description="Teleoperation script for Copter-UAV") - parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) - parser.add_argument('-v', '--verbose', action='store_true', help="verbose output") - mode_group = parser.add_mutually_exclusive_group(required=True) - mode_group.add_argument('-rc', '--rc-override', action='store_true', help="use rc override control type") - mode_group.add_argument('-att', '--sp-attitude', action='store_true', help="use attitude setpoint control type") - mode_group.add_argument('-vel', '--sp-velocity', action='store_true', help="use velocity setpoint control type") - mode_group.add_argument('-pos', '--sp-position', action='store_true', help="use position setpoint control type") - - args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) - - rospy.init_node("mavteleop") - mavros.set_namespace(args.mavros_ns) - - if args.rc_override: - rc_override_control(args) - elif args.sp_attitude: - attitude_setpoint_control(args) - elif args.sp_velocity: - velocity_setpoint_control(args) - elif args.sp_position: - position_setpoint_control(args) - - -if __name__ == '__main__': - main() - diff --git a/mavros_extras/src/lib/servo_state_publisher.cpp b/mavros_extras/src/lib/servo_state_publisher.cpp new file mode 100644 index 000000000..2cb4ca46a --- /dev/null +++ b/mavros_extras/src/lib/servo_state_publisher.cpp @@ -0,0 +1,148 @@ +/* + * Copyright 2015,2021 Vladimir Ermakov + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief Publish servo states as JointState message + * @file + * @author Vladimir Ermakov + */ + +#include + +#include "mavros_extras/servo_state_publisher.hpp" + +using namespace mavros::extras; // NOLINT +using namespace std::placeholders; // NOLINT +using rclcpp::QoS; + +ServoDescription::ServoDescription(urdf::Model & model, std::string joint_name_, YAML::Node config) +: ServoDescription(joint_name_) +{ + if (!config["rc_channel"]) { + throw std::invalid_argument("`rc_channel` field required"); + } + + rc_channel = config["rc_channel"].as(); + rc_min = config["rc_min"].as(1000); + rc_max = config["rc_max"].as(2000); + + if (auto rc_trim_n = config["rc_trim"]; !rc_trim_n) { + rc_trim = rc_min + (rc_max - rc_min) / 2; + } else { + rc_trim = rc_trim_n.as(); + } + + rc_dz = config["rc_dz"].as(0); + rc_rev = config["rc_rev"].as(false); + + auto joint = model.getJoint(joint_name); + if (!joint) { + throw std::runtime_error("Joint " + joint_name + " is not found in URDF"); + } + if (!joint->limits) { + throw std::runtime_error("URDF for joint " + joint_name + " must provide "); + } + + joint_lower = joint->limits->lower; + joint_upper = joint->limits->upper; +} + +ServoStatePublisher::ServoStatePublisher( + const rclcpp::NodeOptions & options, + const std::string & node_name) +: rclcpp::Node(node_name, options) +{ + // Declare configuration parameter + this->declare_parameter("config", ""); + + auto sensor_qos = rclcpp::SensorDataQoS(); + auto description_qos = QoS(1).transient_local(); + + // robot_state_publisher sends URDF in that topic + robot_description_sub = this->create_subscription( + "robot_description", + description_qos, std::bind( + &ServoStatePublisher::robot_description_cb, this, + _1)); + + // Create topics + rc_out_sub = + this->create_subscription( + "rc_out", sensor_qos, + std::bind(&ServoStatePublisher::rc_out_cb, this, _1)); + joint_states_pub = + this->create_publisher("joint_states", sensor_qos); +} + +void ServoStatePublisher::robot_description_cb(const std_msgs::msg::String::SharedPtr msg) +{ + std::unique_lock lock(mutex); + + servos.clear(); + + // 1. Load model + urdf::Model model; + if (!model.initString(msg->data)) { + throw std::runtime_error("Unable to initialize urdf::Model from robot description"); + } + + // 2. Load mapping config + YAML::Node root_node; + { + std::string configYaml; this->get_parameter("config", configYaml); + root_node = YAML::Load(configYaml); + } + if (!root_node.IsMap()) { + throw std::runtime_error("Mapping config must be a map"); + } + + // 3. Load servos + RCLCPP_INFO(get_logger(), "SSP: URDF robot: %s", model.getName().c_str()); + for (auto it = root_node.begin(); it != root_node.end(); ++it) { + auto joint_name = it->first.as(); + RCLCPP_INFO_STREAM(get_logger(), "SSP: " << joint_name << ": Loading joint: " << it->second); + + try { + auto joint = servos.emplace_back(model, joint_name, it->second); + RCLCPP_INFO( + get_logger(), "SSP: joint '%s' (RC%zu) loaded", + joint_name.c_str(), joint.rc_channel); + } catch (const std::exception & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), "SSP: " << joint_name << ": Failed to load mapping: " << ex.what()); + } + } +} + +void ServoStatePublisher::rc_out_cb(const mavros_msgs::msg::RCOut::SharedPtr msg) +{ + std::shared_lock lock(mutex); + + if (msg->channels.empty()) { + return; // nothing to do + } + + auto states = sensor_msgs::msg::JointState(); + states.header.stamp = msg->header.stamp; + + for (auto & desc : servos) { + if (!(desc.rc_channel != 0 && desc.rc_channel <= msg->channels.size())) { + continue; // prevent crash on servos not in that message + } + uint16_t pwm = msg->channels[desc.rc_channel - 1]; + if (pwm == 0 || pwm == UINT16_MAX) { + continue; // exclude unset channels + } + states.name.emplace_back(desc.joint_name); + states.position.emplace_back(desc.calculate_position(pwm)); + } + + joint_states_pub->publish(states); +} + +#include // NOLINT +RCLCPP_COMPONENTS_REGISTER_NODE(mavros::extras::ServoStatePublisher) diff --git a/mavros_extras/src/plugins/3dr_radio.cpp b/mavros_extras/src/plugins/3dr_radio.cpp new file mode 100644 index 000000000..3b8b2613d --- /dev/null +++ b/mavros_extras/src/plugins/3dr_radio.cpp @@ -0,0 +1,192 @@ +/* + * Copyright 2014,2015,2016,2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief 3DR Radio status plugin + * @file 3dr_radio.cpp + * @author Vladimir Ermakov + * + * @addtogroup plugin + * @{ + */ + +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/radio_status.hpp" + +namespace mavros +{ +namespace extra_plugins +{ + +class TDRFilter : public plugin::filter::Filter +{ +public: + inline bool operator()( + plugin::filter::UASPtr uas, const mavlink::mavlink_message_t * cmsg, + const plugin::filter::Framing framing) override + { + if (cmsg->sysid != '3' || cmsg->compid != 'D') { + RCLCPP_WARN_THROTTLE( + uas->get_logger(), + *uas->get_clock(), 30, "RADIO_STATUS not from 3DR modem?"); + } + + return framing == plugin::filter::Framing::ok; + } +}; + +/** + * @brief 3DR Radio plugin. + * @plugin tdr_radio + */ +class TDRRadioPlugin : public plugin::Plugin +{ +public: + explicit TDRRadioPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "tdr_radio"), + has_radio_status(false), + diag_added(false), + low_rssi(0) + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "low_rssi", 40, [&](const rclcpp::Parameter & p) { + low_rssi = p.as_int(); + }); + + auto sensor_qos = rclcpp::SensorDataQoS(); + + status_pub = node->create_publisher("radio_status", sensor_qos); + + enable_connection_cb(); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&TDRRadioPlugin::handle_radio_status), + make_handler(&TDRRadioPlugin::handle_radio), + }; + } + +private: + bool has_radio_status; + bool diag_added; + int low_rssi; + + rclcpp::Publisher::SharedPtr status_pub; + + std::mutex diag_mutex; + mavros_msgs::msg::RadioStatus::SharedPtr last_status; + + /* -*- message handlers -*- */ + + void handle_radio_status( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::RADIO_STATUS & rst, + TDRFilter filter [[maybe_unused]] + ) + { + has_radio_status = true; + handle_message(msg, rst); + } + + void handle_radio( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::ardupilotmega::msg::RADIO & rst, + TDRFilter filter [[maybe_unused]] + ) + { + if (has_radio_status) { + return; + } + + // actually the same data, but from earlier modems + handle_message(msg, rst); + } + + template + void handle_message(const mavlink::mavlink_message_t * mmsg [[maybe_unused]], msgT & rst) + { + auto msg = std::make_shared(); + + msg->header.stamp = node->now(); + +#define RST_COPY(field) msg->field = rst.field + RST_COPY(rssi); + RST_COPY(remrssi); + RST_COPY(txbuf); + RST_COPY(noise); + RST_COPY(remnoise); + RST_COPY(rxerrors); + RST_COPY(fixed); +#undef RST_COPY + + // valid for 3DR modem + msg->rssi_dbm = (rst.rssi / 1.9) - 127; + msg->remrssi_dbm = (rst.remrssi / 1.9) - 127; + + // add diag at first event + if (!diag_added) { + uas->diagnostic_updater.add("3DR Radio", this, &TDRRadioPlugin::diag_run); + diag_added = true; + } + + // store last status for diag + { + std::lock_guard lock(diag_mutex); + last_status = msg; + } + + status_pub->publish(*msg); + } + + + void diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + std::lock_guard lock(diag_mutex); + + if (!last_status) { + stat.summary(2, "No data"); + return; + } else if (last_status->rssi < low_rssi) { + stat.summary(1, "Low RSSI"); + } else if (last_status->remrssi < low_rssi) { + stat.summary(1, "Low remote RSSI"); + } else { + stat.summary(0, "Normal"); + } + + stat.addf("RSSI", "%u", last_status->rssi); + stat.addf("RSSI (dBm)", "%.1f", last_status->rssi_dbm); + stat.addf("Remote RSSI", "%u", last_status->remrssi); + stat.addf("Remote RSSI (dBm)", "%.1f", last_status->remrssi_dbm); + stat.addf("Tx buffer (%)", "%u", last_status->txbuf); + stat.addf("Noice level", "%u", last_status->noise); + stat.addf("Remote noice level", "%u", last_status->remnoise); + stat.addf("Rx errors", "%u", last_status->rxerrors); + stat.addf("Fixed", "%u", last_status->fixed); + } + + void connection_cb(bool connected [[maybe_unused]]) override + { + uas->diagnostic_updater.removeByName("3DR Radio"); + diag_added = false; + } +}; +} // namespace extra_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::TDRRadioPlugin) diff --git a/mavros_extras/src/plugins/adsb.cpp b/mavros_extras/src/plugins/adsb.cpp index 6d669bcab..92ffd20cc 100644 --- a/mavros_extras/src/plugins/adsb.cpp +++ b/mavros_extras/src/plugins/adsb.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2017 Nuno Marques. + * Copyright 2021 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Automatic dependent surveillance-broadcast Vehicle plugin * @file adsb.cpp @@ -6,148 +14,158 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2017 Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -#include +#include "mavros_msgs/msg/rc_in.hpp" +#include "mavros_msgs/msg/rc_out.hpp" +#include "mavros_msgs/msg/adsb_vehicle.hpp" -namespace mavros { -namespace extra_plugins { +namespace mavros +{ +namespace extra_plugins +{ using mavlink::common::ADSB_EMITTER_TYPE; using mavlink::common::ADSB_ALTITUDE_TYPE; /** * @brief ADS-B Vehicle plugin + * @plugin adsb * * Publish/subscribe Automatic dependent surveillance-broadcast data to/from a vehicle. */ -class ADSBPlugin : public plugin::PluginBase { +class ADSBPlugin : public plugin::Plugin +{ public: - ADSBPlugin() : PluginBase(), - adsb_nh("~adsb") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - adsb_pub = adsb_nh.advertise("vehicle", 10); - adsb_sub = adsb_nh.subscribe("send", 10, &ADSBPlugin::adsb_cb, this); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&ADSBPlugin::handle_adsb) - }; - } + explicit ADSBPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "adsb") + { + adsb_pub = node->create_publisher("~/vehicle", 10); + adsb_sub = + node->create_subscription( + "~/send", 10, + std::bind(&ADSBPlugin::adsb_cb, this, std::placeholders::_1)); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&ADSBPlugin::handle_adsb) + }; + } private: - ros::NodeHandle adsb_nh; - - ros::Publisher adsb_pub; - ros::Subscriber adsb_sub; - - void handle_adsb(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ADSB_VEHICLE &adsb) - { - auto adsb_msg = boost::make_shared(); - - adsb_msg->header.stamp = ros::Time::now(); //TODO: request add time_boot_ms to msg definition - // [[[cog: - // def ent(ros, mav=None, scale=None, to_ros=None, to_mav=None): - // return (ros, mav or ros, scale, to_ros, to_mav) - // - // TR_TAB = ( - // ent('ICAO_address'), - // ent('callsign', to_ros='mavlink::to_string', to_mav='mavlink::set_string_z({mm}.{mav}, {rmp}->{ros})'), - // ent('latitude', 'lat', '1e7'), - // ent('longitude', 'lon', '1e7'), - // ent('altitude', 'altitude', '1e3'), - // ent('altitude_type', ), - // ent('heading', scale='1e2'), - // ent('hor_velocity', scale='1e2'), - // ent('ver_velocity', scale='1e2'), - // ent('altitude_type'), - // ent('emitter_type'), - // ent('tslc', to_ros='ros::Duration', to_mav='{mm}.{mav} = {rmp}->{ros}.sec'), - // ent('flags'), - // ent('squawk'), - // ) - // - // for ros, mav, scale, to_ros, _ in TR_TAB: - // if to_ros is None: - // scale_ex = '' if scale is None else ' / ' + scale - // cog.outl("""adsb_msg->{ros} = adsb.{mav}{scale_ex};""".format(**locals())) - // else: - // cog.outl("""adsb_msg->{ros} = {to_ros}(adsb.{mav});""".format(**locals())) - // ]]] - adsb_msg->ICAO_address = adsb.ICAO_address; - adsb_msg->callsign = mavlink::to_string(adsb.callsign); - adsb_msg->latitude = adsb.lat / 1e7; - adsb_msg->longitude = adsb.lon / 1e7; - adsb_msg->altitude = adsb.altitude / 1e3; - adsb_msg->altitude_type = adsb.altitude_type; - adsb_msg->heading = adsb.heading / 1e2; - adsb_msg->hor_velocity = adsb.hor_velocity / 1e2; - adsb_msg->ver_velocity = adsb.ver_velocity / 1e2; - adsb_msg->altitude_type = adsb.altitude_type; - adsb_msg->emitter_type = adsb.emitter_type; - adsb_msg->tslc = ros::Duration(adsb.tslc); - adsb_msg->flags = adsb.flags; - adsb_msg->squawk = adsb.squawk; - // [[[end]]] (checksum: b9c515e7a6fe688b91f4e72e655b9154) - - ROS_DEBUG_STREAM_NAMED("adsb", "ADSB: recv type: " << utils::to_string_enum(adsb.altitude_type) - << " emitter: " << utils::to_string_enum(adsb.emitter_type) - << " flags: 0x" << std::hex << adsb.flags); - - adsb_pub.publish(adsb_msg); - } - - void adsb_cb(const mavros_msgs::ADSBVehicle::ConstPtr &req) - { - mavlink::common::msg::ADSB_VEHICLE adsb{}; - - // [[[cog: - // for ros, mav, scale, _, to_mav in TR_TAB: - // if to_mav is None: - // scale_ex = '' if scale is None else ' * ' + scale - // cog.outl("""adsb.{mav} = req->{ros}{scale_ex};""".format(**locals())) - // else: - // cog.outl(to_mav.format(mm='adsb', rmp='req', **locals()) + ';') - // ]]] - adsb.ICAO_address = req->ICAO_address; - mavlink::set_string_z(adsb.callsign, req->callsign); - adsb.lat = req->latitude * 1e7; - adsb.lon = req->longitude * 1e7; - adsb.altitude = req->altitude * 1e3; - adsb.altitude_type = req->altitude_type; - adsb.heading = req->heading * 1e2; - adsb.hor_velocity = req->hor_velocity * 1e2; - adsb.ver_velocity = req->ver_velocity * 1e2; - adsb.altitude_type = req->altitude_type; - adsb.emitter_type = req->emitter_type; - adsb.tslc = req->tslc.sec; - adsb.flags = req->flags; - adsb.squawk = req->squawk; - // [[[end]]] (checksum: 8583d9ea3a3eefae10ccd7037c06b46d) - - ROS_DEBUG_STREAM_NAMED("adsb", "ADSB: send type: " << utils::to_string_enum(adsb.altitude_type) - << " emitter: " << utils::to_string_enum(adsb.emitter_type) - << " flags: 0x" << std::hex << adsb.flags); - - UAS_FCU(m_uas)->send_message_ignore_drop(adsb); - } + rclcpp::Publisher::SharedPtr adsb_pub; + rclcpp::Subscription::SharedPtr adsb_sub; + + void handle_adsb( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::ADSB_VEHICLE & adsb, + plugin::filter::SystemAndOk filter [[maybe_unused]] + ) + { + auto adsb_msg = mavros_msgs::msg::ADSBVehicle(); + + // TODO(vooon): request add time_boot_ms to msg definition + adsb_msg.header.stamp = node->now(); + + // [[[cog: + // def ent(ros, mav=None, scale=None, to_ros=None, to_mav=None): + // return (ros, mav or ros, scale, to_ros, to_mav) + // + // TR_TAB = ( + // ent('icao_address', 'ICAO_address'), + // ent('callsign', to_ros='mavlink::to_string({mm}.{mav})', + // to_mav='mavlink::set_string_z({mm}.{mav}, {rmp}->{ros})'), + // ent('latitude', 'lat', '1e7'), + // ent('longitude', 'lon', '1e7'), + // ent('altitude', 'altitude', '1e3'), + // ent('altitude_type', ), + // ent('heading', scale='1e2'), + // ent('hor_velocity', scale='1e2'), + // ent('ver_velocity', scale='1e2'), + // ent('altitude_type'), + // ent('emitter_type'), + // ent('tslc', to_ros='rclcpp::Duration({mm}.{mav}, 0)', + // to_mav='{mm}.{mav} = {rmp}->{ros}.sec'), + // ent('flags'), + // ent('squawk'), + // ) + // + // for ros, mav, scale, to_ros, _ in TR_TAB: + // if to_ros is None: + // scale_ex = '' if scale is None else ' / ' + scale + // cog.outl(f"""adsb_msg.{ros} = adsb.{mav}{scale_ex};""") + // else: + // cog.outl(f"""adsb_msg.{ros} = {to_ros.format(mm='adsb', **locals())};""") + // ]]] + adsb_msg.icao_address = adsb.ICAO_address; + adsb_msg.callsign = mavlink::to_string(adsb.callsign); + adsb_msg.latitude = adsb.lat / 1e7; + adsb_msg.longitude = adsb.lon / 1e7; + adsb_msg.altitude = adsb.altitude / 1e3; + adsb_msg.altitude_type = adsb.altitude_type; + adsb_msg.heading = adsb.heading / 1e2; + adsb_msg.hor_velocity = adsb.hor_velocity / 1e2; + adsb_msg.ver_velocity = adsb.ver_velocity / 1e2; + adsb_msg.altitude_type = adsb.altitude_type; + adsb_msg.emitter_type = adsb.emitter_type; + adsb_msg.tslc = rclcpp::Duration(adsb.tslc, 0); + adsb_msg.flags = adsb.flags; + adsb_msg.squawk = adsb.squawk; + // [[[end]]] (checksum: ae8f818682cc2c23db50504f5af97127) + + RCLCPP_DEBUG_STREAM( + get_logger(), + "ADSB: recv type: " << utils::to_string_enum(adsb.altitude_type) << + " emitter: " << utils::to_string_enum(adsb.emitter_type) << + " flags: 0x" << std::hex << adsb.flags); + + adsb_pub->publish(adsb_msg); + } + + void adsb_cb(const mavros_msgs::msg::ADSBVehicle::SharedPtr req) + { + mavlink::common::msg::ADSB_VEHICLE adsb{}; + + // [[[cog: + // for ros, mav, scale, _, to_mav in TR_TAB: + // if to_mav is None: + // scale_ex = '' if scale is None else ' * ' + scale + // cog.outl(f"""adsb.{mav} = req->{ros}{scale_ex};""") + // else: + // cog.outl(to_mav.format(mm='adsb', rmp='req', **locals()) + ';') + // ]]] + adsb.ICAO_address = req->icao_address; + mavlink::set_string_z(adsb.callsign, req->callsign); + adsb.lat = req->latitude * 1e7; + adsb.lon = req->longitude * 1e7; + adsb.altitude = req->altitude * 1e3; + adsb.altitude_type = req->altitude_type; + adsb.heading = req->heading * 1e2; + adsb.hor_velocity = req->hor_velocity * 1e2; + adsb.ver_velocity = req->ver_velocity * 1e2; + adsb.altitude_type = req->altitude_type; + adsb.emitter_type = req->emitter_type; + adsb.tslc = req->tslc.sec; + adsb.flags = req->flags; + adsb.squawk = req->squawk; + // [[[end]]] (checksum: e586b680a3d86ec594e5b7f4a59bbe6c) + + RCLCPP_DEBUG_STREAM( + get_logger(), + "ADSB: send type: " << utils::to_string_enum(adsb.altitude_type) << + " emitter: " << utils::to_string_enum(adsb.emitter_type) << + " flags: 0x" << std::hex << adsb.flags); + + uas->send_message(adsb); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::ADSBPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::ADSBPlugin) diff --git a/mavros_extras/src/plugins/cam_imu_sync.cpp b/mavros_extras/src/plugins/cam_imu_sync.cpp index f18ca1477..5c45d0f0d 100644 --- a/mavros_extras/src/plugins/cam_imu_sync.cpp +++ b/mavros_extras/src/plugins/cam_imu_sync.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2015 Mohammed Kabir. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Camera IMU synchronisation plugin * @file cam_imu_sync.cpp @@ -6,64 +13,60 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2015 Mohammed Kabir. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -#include +#include "mavros_msgs/msg/cam_imu_stamp.hpp" -namespace mavros { -namespace extra_plugins { +namespace mavros +{ +namespace extra_plugins +{ /** * @brief Camera IMU synchronisation plugin + * @plugin cam_imu_sync * * This plugin publishes a timestamp for when a external camera system was * triggered by the FCU. Sequence ID from the message and the image sequence from * camera can be corellated to get the exact shutter trigger time. */ -class CamIMUSyncPlugin : public plugin::PluginBase { +class CamIMUSyncPlugin : public plugin::Plugin +{ public: - CamIMUSyncPlugin() : PluginBase(), - cam_imu_sync_nh("~cam_imu_sync") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); + explicit CamIMUSyncPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "cam_imu_sync") + { + cam_imu_pub = node->create_publisher("~/cam_imu_stamp", 10); + } - cam_imu_pub = cam_imu_sync_nh.advertise("cam_imu_stamp", 10); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&CamIMUSyncPlugin::handle_cam_trig) - }; - } + Subscriptions get_subscriptions() override + { + return { + make_handler(&CamIMUSyncPlugin::handle_cam_trig) + }; + } private: - ros::NodeHandle cam_imu_sync_nh; - - ros::Publisher cam_imu_pub; + rclcpp::Publisher::SharedPtr cam_imu_pub; - void handle_cam_trig(const mavlink::mavlink_message_t *msg, mavlink::common::msg::CAMERA_TRIGGER &ctrig) - { - auto sync_msg = boost::make_shared(); + void handle_cam_trig( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::CAMERA_TRIGGER & ctrig, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto sync_msg = mavros_msgs::msg::CamIMUStamp(); - sync_msg->frame_stamp = m_uas->synchronise_stamp(ctrig.time_usec); - sync_msg->frame_seq_id = ctrig.seq; + sync_msg.frame_stamp = uas->synchronise_stamp(ctrig.time_usec); + sync_msg.frame_seq_id = ctrig.seq; - cam_imu_pub.publish(sync_msg); - } + cam_imu_pub->publish(sync_msg); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::CamIMUSyncPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::CamIMUSyncPlugin) diff --git a/mavros_extras/src/plugins/camera.cpp b/mavros_extras/src/plugins/camera.cpp index a18d85212..81c13705f 100644 --- a/mavros_extras/src/plugins/camera.cpp +++ b/mavros_extras/src/plugins/camera.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2021 Jaeyoung Lim. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Camera plugin * @file camera.cpp @@ -6,20 +13,20 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2021 Jaeyoung Lim. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -#include +#include "mavros_msgs/msg/camera_image_captured.hpp" -namespace mavros { -namespace extra_plugins { +namespace mavros +{ +namespace extra_plugins +{ //! Mavlink enumerations using mavlink::common::MAV_CMD; @@ -27,63 +34,59 @@ using utils::enum_value; /** * @brief Camera plugin plugin + * @plugin camera * * Plugin for interfacing on the mavlink camera protocol * @see command_cb() */ -class CameraPlugin : public plugin::PluginBase { +class CameraPlugin : public plugin::Plugin +{ public: - CameraPlugin() : PluginBase(), - nh("~"), - camera_nh("~camera") - { } + explicit CameraPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "camera") + { + camera_image_captured_pub = node->create_publisher( + "~/image_captured", 10); + } - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - camera_image_captured_pub = camera_nh.advertise("image_captured", 10); - - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&CameraPlugin::handle_camera_image_captured) - }; - } + Subscriptions get_subscriptions() override + { + return { + make_handler(&CameraPlugin::handle_camera_image_captured) + }; + } private: - ros::NodeHandle nh; - ros::NodeHandle camera_nh; - ros::Publisher camera_image_captured_pub; + rclcpp::Publisher::SharedPtr camera_image_captured_pub; - /** - * @brief Publish camera image capture information - * - * Message specification: https://mavlink.io/en/messages/common.html#CAMERA_IMAGE_CAPTURED - * @param msg the mavlink message - * @param mo received CAMERA_IMAGE_CAPTURED msg - */ - void handle_camera_image_captured(const mavlink::mavlink_message_t *msg, mavlink::common::msg::CAMERA_IMAGE_CAPTURED &mo) - { - auto ic = boost::make_shared(); + /** + * @brief Publish camera image capture information + * + * Message specification: https://mavlink.io/en/messages/common.html#CAMERA_IMAGE_CAPTURED + * @param msg the mavlink message + * @param mo received CAMERA_IMAGE_CAPTURED msg + */ + void handle_camera_image_captured( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::CAMERA_IMAGE_CAPTURED & mo, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto ic = mavros_msgs::msg::CameraImageCaptured(); - ic->header.stamp = m_uas->synchronise_stamp(mo.time_boot_ms); - ic->geo.latitude = mo.lat/ 1E7; - ic->geo.longitude = mo.lon / 1E7; // deg - ic->geo.altitude = mo.alt / 1E3 + m_uas->geoid_to_ellipsoid_height(&ic->geo); // in meters - ic->relative_alt = mo.relative_alt / 1E3; - auto q = ftf::mavlink_to_quaternion(mo.q); - tf::quaternionEigenToMsg(q, ic->orientation); - ic->file_url = mavlink::to_string(mo.file_url); - - camera_image_captured_pub.publish(ic); - } + ic.header.stamp = uas->synchronise_stamp(mo.time_boot_ms); + ic.geo.latitude = mo.lat / 1E7; + ic.geo.longitude = mo.lon / 1E7; // deg + ic.geo.altitude = mo.alt / 1E3 + uas->data.geoid_to_ellipsoid_height(&ic.geo); // in meters + ic.relative_alt = mo.relative_alt / 1E3; + ic.orientation = tf2::toMsg(ftf::mavlink_to_quaternion(mo.q)); + ic.file_url = mavlink::to_string(mo.file_url); + camera_image_captured_pub->publish(ic); + } }; -} // namespace extra_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::CameraPlugin, mavros::plugin::PluginBase) +} // namespace extra_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::CameraPlugin) diff --git a/mavros_extras/src/plugins/cellular_status.cpp b/mavros_extras/src/plugins/cellular_status.cpp index 97db07e87..82fef5605 100644 --- a/mavros_extras/src/plugins/cellular_status.cpp +++ b/mavros_extras/src/plugins/cellular_status.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2021 Jaeyoung Lim. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Cellular status plugin * @file cellular_status.cpp @@ -6,79 +13,69 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2021 Jaeyoung Lim. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/cellular_status.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace extra_plugins { /** * @brief Cellular status plugin. + * @plugin cellular_status * * Users must publish to the topic the CellularStatus message and it * will be relayed to the mavlink components. */ -class CellularStatusPlugin : public plugin::PluginBase { +class CellularStatusPlugin : public plugin::Plugin +{ public: - CellularStatusPlugin() : PluginBase(), - cc_nh("~cellular_status") - { } - - /** - * Plugin initializer. Constructor should not do this. - */ - void initialize(UAS &uas_) - { - PluginBase::initialize(uas_); - subCellularStatus = cc_nh.subscribe("status", 1, &CellularStatusPlugin::cellularStatusCb, this); - } + explicit CellularStatusPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "cellular_status") + { + sub_status = node->create_subscription( + "~/status", 1, std::bind( + &CellularStatusPlugin::status_cb, this, + _1)); + } - /** - * This function returns message subscriptions. - * - * Each subscription made by PluginBase::make_handler() template. - * Two variations: - * - With automatic decoding and framing error filtering (see handle_heartbeat) - * - Raw message with framig status (see handle_systemtext) - */ - Subscriptions get_subscriptions() { - return {}; - } + Subscriptions get_subscriptions() + { + return {}; + } private: - ros::NodeHandle cc_nh; - ros::Subscriber subCellularStatus; + rclcpp::Subscription::SharedPtr sub_status; - /** - * @brief Send Cellular Status messages to mavlink system - * - * Message specification: https://mavlink.io/en/messages/common.html#CELLULAR_STATUS - * @param msg received CellularStatus msg - */ - void cellularStatusCb(const mavros_msgs::CellularStatus &msg) - { - mavlink::common::msg::CELLULAR_STATUS cs{}; + /** + * @brief Send Cellular Status messages to mavlink system + * + * Message specification: https://mavlink.io/en/messages/common.html#CELLULAR_STATUS + * @param msg received CellularStatus msg + */ + void status_cb(const mavros_msgs::msg::CellularStatus::SharedPtr msg) + { + mavlink::common::msg::CELLULAR_STATUS cs{}; - cs.status = msg.status; - cs.failure_reason = msg.failure_reason; - cs.type = msg.type; - cs.quality = msg.quality; - cs.mcc = msg.mcc; - cs.mnc = msg.mnc; - cs.lac = msg.lac; + cs.status = msg->status; + cs.failure_reason = msg->failure_reason; + cs.type = msg->type; + cs.quality = msg->quality; + cs.mcc = msg->mcc; + cs.mnc = msg->mnc; + cs.lac = msg->lac; - UAS_FCU(m_uas)->send_message_ignore_drop(cs); - } + uas->send_message(cs); + } }; -} // namespace std_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::CellularStatusPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::CellularStatusPlugin) diff --git a/mavros_extras/src/plugins/companion_process_status.cpp b/mavros_extras/src/plugins/companion_process_status.cpp index 23eddf58a..acd3adf60 100644 --- a/mavros_extras/src/plugins/companion_process_status.cpp +++ b/mavros_extras/src/plugins/companion_process_status.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2018 Tanja Baumann. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Companion Status plugin * @file companion_status.cpp @@ -6,20 +13,19 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2018 Tanja Baumann. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/companion_process_status.hpp" -#include +namespace mavros +{ +namespace extra_plugins +{ -namespace mavros { -namespace extra_plugins { //! Mavlink enumerations using mavlink::minimal::MAV_TYPE; using mavlink::minimal::MAV_STATE; @@ -30,56 +36,58 @@ using utils::enum_value; /** * @brief Obstacle companion process status plugin + * @plugin companion_process_status * * Publishes the status of components running on the companion computer * @see status_cb() */ -class CompanionProcessStatusPlugin : public plugin::PluginBase { +class CompanionProcessStatusPlugin : public plugin::Plugin +{ public: - CompanionProcessStatusPlugin() : PluginBase(), - status_nh("~companion_process") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - status_sub = status_nh.subscribe("status", 10, &CompanionProcessStatusPlugin::status_cb, this); - } + explicit CompanionProcessStatusPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "companion_process") + { + status_sub = node->create_subscription( + "~/status", 10, std::bind( + &CompanionProcessStatusPlugin::status_cb, this, + std:: + placeholders::_1)); + } - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - ros::NodeHandle status_nh; - ros::Subscriber status_sub; + rclcpp::Subscription::SharedPtr status_sub; - /** - * @brief Send companion process status to FCU over a heartbeat message - * - * Message specification: https://mavlink.io/en/messages/common.html#HEARTBEAT - * @param req received CompanionProcessStatus msg - */ - void status_cb(const mavros_msgs::CompanionProcessStatus::ConstPtr &req) - { - mavlink::minimal::msg::HEARTBEAT heartbeat {}; + /** + * @brief Send companion process status to FCU over a heartbeat message + * + * Message specification: https://mavlink.io/en/messages/common.html#HEARTBEAT + * @param req received CompanionProcessStatus msg + */ + void status_cb(const mavros_msgs::msg::CompanionProcessStatus::SharedPtr req) + { + mavlink::minimal::msg::HEARTBEAT heartbeat {}; - heartbeat.type = enum_value(MAV_TYPE::ONBOARD_CONTROLLER); - heartbeat.autopilot = enum_value(MAV_AUTOPILOT::PX4); - heartbeat.base_mode = enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED); - heartbeat.system_status = req->state; //enum="MAV_STATE" System status flag + heartbeat.type = enum_value(MAV_TYPE::ONBOARD_CONTROLLER); + heartbeat.autopilot = enum_value(MAV_AUTOPILOT::PX4); + heartbeat.base_mode = enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED); + heartbeat.system_status = req->state; // enum="MAV_STATE" System status flag - ROS_DEBUG_STREAM_NAMED("companion_process_status", "companion process component id: " << - utils::to_string_enum(req->component) << " companion process status: " << - utils::to_string_enum(heartbeat.system_status) << std::endl << heartbeat.to_yaml()); + RCLCPP_DEBUG_STREAM( + get_logger(), + "companion process component id: " << utils::to_string_enum(req->component) << + " companion process status: " << utils::to_string_enum( + heartbeat.system_status) << std::endl << heartbeat.to_yaml()); - UAS_FCU(m_uas)->send_message_ignore_drop(heartbeat, req->component); - } + uas->send_message(heartbeat, req->component); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::CompanionProcessStatusPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::CompanionProcessStatusPlugin) diff --git a/mavros_extras/src/plugins/debug_value.cpp b/mavros_extras/src/plugins/debug_value.cpp index 93b467ffc..066429088 100644 --- a/mavros_extras/src/plugins/debug_value.cpp +++ b/mavros_extras/src/plugins/debug_value.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2017 Nuno Marques. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Debug messages plugin * @file debug_value.cpp @@ -6,337 +13,353 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2017 Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include -#include #include +#include -namespace mavros { -namespace extra_plugins { -//! @brief Plugin for Debug msgs from MAVLink API -class DebugValuePlugin : public plugin::PluginBase { +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/debug_value.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT + +/** + * @brief Plugin for Debug msgs from MAVLink API + * @plugin debug_value + */ +class DebugValuePlugin : public plugin::Plugin +{ public: - DebugValuePlugin() : PluginBase(), - debug_nh("~debug_value") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - // subscribers - debug_sub = debug_nh.subscribe("send", 10, &DebugValuePlugin::debug_cb, this); - - // publishers - debug_pub = debug_nh.advertise("debug", 10); - debug_vector_pub = debug_nh.advertise("debug_vector", 10); - debug_float_array_pub = debug_nh.advertise("debug_float_array", 10); - named_value_float_pub = debug_nh.advertise("named_value_float", 10); - named_value_int_pub = debug_nh.advertise("named_value_int", 10); - } - - Subscriptions get_subscriptions() override { - return { - make_handler(&DebugValuePlugin::handle_debug), - make_handler(&DebugValuePlugin::handle_debug_vector), - make_handler(&DebugValuePlugin::handle_debug_float_array), - make_handler(&DebugValuePlugin::handle_named_value_float), - make_handler(&DebugValuePlugin::handle_named_value_int) - }; - } + explicit DebugValuePlugin(plugin::UASPtr uas_) + : Plugin(uas_, "debug_value") + { + // subscribers + debug_sub = + node->create_subscription( + "~/send", 10, + std::bind(&DebugValuePlugin::debug_cb, this, _1)); + + // publishers + debug_pub = node->create_publisher("~/debug", 10); + debug_vector_pub = node->create_publisher("~/debug_vector", 10); + debug_float_array_pub = node->create_publisher("~/debug_float_array", 10); + named_value_float_pub = node->create_publisher("~/named_value_float", 10); + named_value_int_pub = node->create_publisher("~/named_value_int", 10); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&DebugValuePlugin::handle_debug), + make_handler(&DebugValuePlugin::handle_debug_vector), + make_handler(&DebugValuePlugin::handle_debug_float_array), + make_handler(&DebugValuePlugin::handle_named_value_float), + make_handler(&DebugValuePlugin::handle_named_value_int) + }; + } private: - ros::NodeHandle debug_nh; - - ros::Subscriber debug_sub; - - ros::Publisher debug_pub; - ros::Publisher debug_vector_pub; - ros::Publisher debug_float_array_pub; - ros::Publisher named_value_float_pub; - ros::Publisher named_value_int_pub; - - /* -*- helpers -*- */ - - /** - * @brief Helper function to log debug messages - * @param type Type of debug message - * @param dv Data value - */ - void debug_logger(const std::string &type, const mavros_msgs::DebugValue &dv) - { - using DV = mavros_msgs::DebugValue; - - std::string name = (dv.name == "") ? "UNK" : dv.name; - - std::ostringstream ss; - if (dv.type == DV::TYPE_NAMED_VALUE_INT) { - ss << dv.value_int; - } - else if (dv.type == DV::TYPE_DEBUG_VECT) { - ss << "["; - bool is_first = true; - for (auto v : dv.data) { - if (!is_first) { - ss << ", "; - } - - ss << v; - is_first = false; - } - - ss << "]"; - } - else { - ss << dv.value_float; - } - - - ROS_DEBUG_STREAM_NAMED("debug_value", type << "\t" - << dv.header.stamp << "\t" - << name << "\t[" - << dv.index << "]\tvalue:" - << ss.str()); - } - - /* -*- message handlers -*- */ - - /** - * @brief Handle DEBUG message. - * Message specification: https://mavlink.io/en/messages/common.html#DEBUG - * @param msg Received Mavlink msg - * @param debug DEBUG msg - */ - void handle_debug(const mavlink::mavlink_message_t *msg, mavlink::common::msg::DEBUG &debug) - { - // [[[cog: - // p = "dv_msg" - // val = "debug" - // - // def common_filler(type_, time_f, index, name, disable_array_id = True): - // if isinstance(index, str): - // index = val + "." + index - // - // _args = globals() - // _args.update(locals()) - // - // cog.outl("""auto {p} = boost::make_shared();""".format(**_args)) - // cog.outl("""{p}->header.stamp = m_uas->synchronise_stamp({val}.{time_f});""".format(**_args)) - // cog.outl("""{p}->type = mavros_msgs::DebugValue::{type_};""".format(**_args)) - // cog.outl("""{p}->index = {index};""".format(**_args)) - // if disable_array_id: - // cog.outl("""{p}->array_id = -1;""".format(**_args)) - // if name: - // cog.outl("""{p}->name = mavlink::to_string({val}.{name});""".format(**_args)) - // - // common_filler("TYPE_DEBUG", "time_boot_ms", "ind", None) - // cog.outl("""{p}->value_float = {val}.value;""".format(**locals())) - // ]]] - auto dv_msg = boost::make_shared(); - dv_msg->header.stamp = m_uas->synchronise_stamp(debug.time_boot_ms); - dv_msg->type = mavros_msgs::DebugValue::TYPE_DEBUG; - dv_msg->index = debug.ind; - dv_msg->array_id = -1; - dv_msg->value_float = debug.value; - // [[[end]]] (checksum: 5ef05a58b0a7925a57b4602198097e30) - - debug_logger(debug.get_name(), *dv_msg); - debug_pub.publish(dv_msg); - } - - /** - * @brief Handle DEBUG_VECT message. - * Message specification: https://mavlink.io/en/messages/common.html#DEBUG_VECT - * @param msg Received Mavlink msg - * @param debug DEBUG_VECT msg - */ - void handle_debug_vector(const mavlink::mavlink_message_t *msg, mavlink::common::msg::DEBUG_VECT &debug) - { - // [[[cog: - // common_filler("TYPE_DEBUG_VECT", "time_usec", -1, "name") - // - // fields = "xyz" - // pd = p + "->data" - // cog.outl("""{pd}.resize({l});""".format(l=len(fields), **locals())) - // for i, f in enumerate(fields): - // cog.outl("""{pd}[{i}] = {val}.{f};""".format(**locals())) - // ]]] - auto dv_msg = boost::make_shared(); - dv_msg->header.stamp = m_uas->synchronise_stamp(debug.time_usec); - dv_msg->type = mavros_msgs::DebugValue::TYPE_DEBUG_VECT; - dv_msg->index = -1; - dv_msg->array_id = -1; - dv_msg->name = mavlink::to_string(debug.name); - dv_msg->data.resize(3); - dv_msg->data[0] = debug.x; - dv_msg->data[1] = debug.y; - dv_msg->data[2] = debug.z; - // [[[end]]] (checksum: 6537917118cc4121b7477a46788c5c4d) - - debug_logger(debug.get_name(), *dv_msg); - debug_vector_pub.publish(dv_msg); - } - - /** - * @brief Handle DEBUG_FLOAT_ARRAY message. - * Message specification: https://mavlink.io/en/messages/common.html#DEBUG_FLOAT_ARRAY - * @param msg Received Mavlink msg - * @param debug DEBUG_FLOAT_ARRAY msg - */ - void handle_debug_float_array(const mavlink::mavlink_message_t *msg, mavlink::common::msg::DEBUG_FLOAT_ARRAY &debug) - { - // [[[cog: - // common_filler("TYPE_DEBUG_FLOAT_ARRAY", "time_usec", -1, "name", False) - // - // cog.outl("{p}->array_id = {val}.array_id;".format(**locals())) - // cog.outl("{p}->data.assign({val}.data.begin(), {val}.data.end());".format(**locals())) - // ]]] - auto dv_msg = boost::make_shared(); - dv_msg->header.stamp = m_uas->synchronise_stamp(debug.time_usec); - dv_msg->type = mavros_msgs::DebugValue::TYPE_DEBUG_FLOAT_ARRAY; - dv_msg->index = -1; - dv_msg->name = mavlink::to_string(debug.name); - dv_msg->array_id = debug.array_id; - dv_msg->data.assign(debug.data.begin(), debug.data.end()); - // [[[end]]] (checksum: a27f0f0d80be19127fe9838a867e85b4) - - debug_logger(debug.get_name(), *dv_msg); - debug_float_array_pub.publish(dv_msg); - } - - /** - * @brief Handle NAMED_VALUE_FLOAT message. - * Message specification: https://mavlink.io/en/messages/common.html#NAMED_VALUE_FLOAT - * @param msg Received Mavlink msg - * @param value NAMED_VALUE_FLOAT msg - */ - void handle_named_value_float(const mavlink::mavlink_message_t *msg, mavlink::common::msg::NAMED_VALUE_FLOAT &value) - { - // [[[cog: - // val="value" - // common_filler("TYPE_NAMED_VALUE_FLOAT", "time_boot_ms", -1, "name") - // cog.outl("""{p}->value_float = {val}.value;""".format(**locals())) - // ]]] - auto dv_msg = boost::make_shared(); - dv_msg->header.stamp = m_uas->synchronise_stamp(value.time_boot_ms); - dv_msg->type = mavros_msgs::DebugValue::TYPE_NAMED_VALUE_FLOAT; - dv_msg->index = -1; - dv_msg->array_id = -1; - dv_msg->name = mavlink::to_string(value.name); - dv_msg->value_float = value.value; - // [[[end]]] (checksum: a4661d49c58aa52f3d870859ab5aefa6) - - debug_logger(value.get_name(), *dv_msg); - named_value_float_pub.publish(dv_msg); - } - - /** - * @brief Handle NAMED_VALUE_INT message. - * Message specification: https://mavlink.io/en/messages/common.html#NAMED_VALUE_INT - * @param msg Received Mavlink msg - * @param value NAMED_VALUE_INT msg - */ - void handle_named_value_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::NAMED_VALUE_INT &value) - { - // [[[cog: - // common_filler("TYPE_NAMED_VALUE_INT", "time_boot_ms", -1, "name") - // cog.outl("""{p}->value_int = {val}.value;""".format(**locals())) - // ]]] - auto dv_msg = boost::make_shared(); - dv_msg->header.stamp = m_uas->synchronise_stamp(value.time_boot_ms); - dv_msg->type = mavros_msgs::DebugValue::TYPE_NAMED_VALUE_INT; - dv_msg->index = -1; - dv_msg->array_id = -1; - dv_msg->name = mavlink::to_string(value.name); - dv_msg->value_int = value.value; - // [[[end]]] (checksum: 875d1469f398e89e17c5e988b3cfda56) - - debug_logger(value.get_name(), *dv_msg); - named_value_int_pub.publish(dv_msg); - } - - /* -*- callbacks -*- */ - - /** - * @brief Debug callbacks - * @param req pointer to mavros_msgs/Debug.msg being published - */ - void debug_cb(const mavros_msgs::DebugValue::ConstPtr &req) - { - switch (req->type) { - case mavros_msgs::DebugValue::TYPE_DEBUG: { - mavlink::common::msg::DEBUG debug {}; - - debug.time_boot_ms = req->header.stamp.toNSec() / 1000000; - debug.ind = req->index; - debug.value = req->value_float; - - UAS_FCU(m_uas)->send_message_ignore_drop(debug); - break; - } - case mavros_msgs::DebugValue::TYPE_DEBUG_VECT: { - mavlink::common::msg::DEBUG_VECT debug {}; - - debug.time_usec = req->header.stamp.toNSec() / 1000; - mavlink::set_string(debug.name, req->name); - // [[[cog: - // for i, f in enumerate("xyz"): - // cog.outl("debug.{f} = req->data[{i}];".format(**locals())) - // ]]] - debug.x = req->data[0]; - debug.y = req->data[1]; - debug.z = req->data[2]; - // [[[end]]] (checksum: f4918ce98ca3183f93f6aff20d4ab7ec) - - UAS_FCU(m_uas)->send_message_ignore_drop(debug); - break; - } - case mavros_msgs::DebugValue::TYPE_DEBUG_FLOAT_ARRAY: { - mavlink::common::msg::DEBUG_FLOAT_ARRAY debug {}; - - debug.time_usec = req->header.stamp.toNSec() / 1000; - mavlink::set_string(debug.name, req->name); - std::copy_n(req->data.begin(), std::min(req->data.size(), debug.data.size()), std::begin(debug.data)); - - UAS_FCU(m_uas)->send_message_ignore_drop(debug); - break; - } - case mavros_msgs::DebugValue::TYPE_NAMED_VALUE_FLOAT: { - mavlink::common::msg::NAMED_VALUE_FLOAT value {}; - - value.time_boot_ms = req->header.stamp.toNSec() / 1000000; - mavlink::set_string(value.name, req->name); - value.value = req->value_float; - - UAS_FCU(m_uas)->send_message_ignore_drop(value); - break; - } - case mavros_msgs::DebugValue::TYPE_NAMED_VALUE_INT: { - mavlink::common::msg::NAMED_VALUE_INT value {}; - - value.time_boot_ms = req->header.stamp.toNSec() / 1000000; - mavlink::set_string(value.name, req->name); - value.value = req->value_int; - - UAS_FCU(m_uas)->send_message_ignore_drop(value); - break; - } - default: - ROS_ERROR_NAMED("debug", "Wrong debug type (%d). Droping!...", req->type); - return; - } - } + using DV = mavros_msgs::msg::DebugValue; + + rclcpp::Subscription::SharedPtr debug_sub; + + rclcpp::Publisher::SharedPtr debug_pub; + rclcpp::Publisher::SharedPtr debug_vector_pub; + rclcpp::Publisher::SharedPtr debug_float_array_pub; + rclcpp::Publisher::SharedPtr named_value_float_pub; + rclcpp::Publisher::SharedPtr named_value_int_pub; + + /* -*- helpers -*- */ + + /** + * @brief Helper function to log debug messages + * @param type Type of debug message + * @param dv Data value + */ + void debug_logger(const std::string & type, const DV & dv) + { + std::string name = (dv.name == "") ? "UNK" : dv.name; + + std::ostringstream ss; + if (dv.type == DV::TYPE_NAMED_VALUE_INT) { + ss << dv.value_int; + } else if (dv.type == DV::TYPE_DEBUG_VECT) { + ss << "["; + bool is_first = true; + for (auto v : dv.data) { + if (!is_first) { + ss << ", "; + } + + ss << v; + is_first = false; + } + + ss << "]"; + } else { + ss << dv.value_float; + } + + RCLCPP_DEBUG_STREAM( + get_logger(), + type << "\t" << + dv.header.stamp.sec << "." << dv.header.stamp.nanosec << "\t" << + name << "\t[" << + dv.index << "]\tvalue:" << + ss.str()); + } + + /* -*- message handlers -*- */ + + /** + * @brief Handle DEBUG message. + * Message specification: https://mavlink.io/en/messages/common.html#DEBUG + * @param msg Received Mavlink msg + * @param debug DEBUG msg + */ + void handle_debug( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::DEBUG & debug, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // [[[cog: + // p = "dv_msg" + // val = "debug" + // + // def common_filler(type_, time_f, index, name, disable_array_id = True): + // if isinstance(index, str): + // index = val + "." + index + // + // cog.outl(f"""DV {p};""") + // cog.outl(f"""{p}.header.stamp = uas->synchronise_stamp({val}.{time_f});""") + // cog.outl(f"""{p}.type = DV::{type_};""") + // cog.outl(f"""{p}.index = {index};""") + // if disable_array_id: + // cog.outl(f"""{p}.array_id = -1;""") + // if name: + // cog.outl(f"""{p}.name = mavlink::to_string({val}.{name});""") + // + // common_filler("TYPE_DEBUG", "time_boot_ms", "ind", None) + // cog.outl(f"""{p}.value_float = {val}.value;""") + // ]]] + DV dv_msg; + dv_msg.header.stamp = uas->synchronise_stamp(debug.time_boot_ms); + dv_msg.type = DV::TYPE_DEBUG; + dv_msg.index = debug.ind; + dv_msg.array_id = -1; + dv_msg.value_float = debug.value; + // [[[end]]] (checksum: ef695729241176edd2e06592ed20549b) + + debug_logger(debug.get_name(), dv_msg); + debug_pub->publish(dv_msg); + } + + /** + * @brief Handle DEBUG_VECT message. + * Message specification: https://mavlink.io/en/messages/common.html#DEBUG_VECT + * @param msg Received Mavlink msg + * @param debug DEBUG_VECT msg + */ + void handle_debug_vector( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::DEBUG_VECT & debug, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // [[[cog: + // common_filler("TYPE_DEBUG_VECT", "time_usec", -1, "name") + // + // fields = "xyz" + // pd = p + ".data" + // cog.outl(f"""{pd}.resize({len(fields)});""") + // for i, f in enumerate(fields): + // cog.outl(f"""{pd}[{i}] = {val}.{f};""") + // ]]] + DV dv_msg; + dv_msg.header.stamp = uas->synchronise_stamp(debug.time_usec); + dv_msg.type = DV::TYPE_DEBUG_VECT; + dv_msg.index = -1; + dv_msg.array_id = -1; + dv_msg.name = mavlink::to_string(debug.name); + dv_msg.data.resize(3); + dv_msg.data[0] = debug.x; + dv_msg.data[1] = debug.y; + dv_msg.data[2] = debug.z; + // [[[end]]] (checksum: 8abb1284bdb29874a87fee9808570f05) + + debug_logger(debug.get_name(), dv_msg); + debug_vector_pub->publish(dv_msg); + } + + /** + * @brief Handle DEBUG_FLOAT_ARRAY message. + * Message specification: https://mavlink.io/en/messages/common.html#DEBUG_FLOAT_ARRAY + * @param msg Received Mavlink msg + * @param debug DEBUG_FLOAT_ARRAY msg + */ + void handle_debug_float_array( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::DEBUG_FLOAT_ARRAY & debug, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // [[[cog: + // common_filler("TYPE_DEBUG_FLOAT_ARRAY", "time_usec", -1, "name", False) + // + // cog.outl("{p}.array_id = {val}.array_id;".format(**locals())) + // cog.outl("{p}.data.assign({val}.data.begin(), {val}.data.end());".format(**locals())) + // ]]] + DV dv_msg; + dv_msg.header.stamp = uas->synchronise_stamp(debug.time_usec); + dv_msg.type = DV::TYPE_DEBUG_FLOAT_ARRAY; + dv_msg.index = -1; + dv_msg.name = mavlink::to_string(debug.name); + dv_msg.array_id = debug.array_id; + dv_msg.data.assign(debug.data.begin(), debug.data.end()); + // [[[end]]] (checksum: e13bbba22bff5b74db32092d8787c6b4) + + debug_logger(debug.get_name(), dv_msg); + debug_float_array_pub->publish(dv_msg); + } + + /** + * @brief Handle NAMED_VALUE_FLOAT message. + * Message specification: https://mavlink.io/en/messages/common.html#NAMED_VALUE_FLOAT + * @param msg Received Mavlink msg + * @param value NAMED_VALUE_FLOAT msg + */ + void handle_named_value_float( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::NAMED_VALUE_FLOAT & value, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // [[[cog: + // val="value" + // common_filler("TYPE_NAMED_VALUE_FLOAT", "time_boot_ms", -1, "name") + // cog.outl(f"""{p}.value_float = {val}.value;""") + // ]]] + DV dv_msg; + dv_msg.header.stamp = uas->synchronise_stamp(value.time_boot_ms); + dv_msg.type = DV::TYPE_NAMED_VALUE_FLOAT; + dv_msg.index = -1; + dv_msg.array_id = -1; + dv_msg.name = mavlink::to_string(value.name); + dv_msg.value_float = value.value; + // [[[end]]] (checksum: 8c243c3e607db7bf0758cd4ac3aca976) + + debug_logger(value.get_name(), dv_msg); + named_value_float_pub->publish(dv_msg); + } + + /** + * @brief Handle NAMED_VALUE_INT message. + * Message specification: https://mavlink.io/en/messages/common.html#NAMED_VALUE_INT + * @param msg Received Mavlink msg + * @param value NAMED_VALUE_INT msg + */ + void handle_named_value_int( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::NAMED_VALUE_INT & value, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // [[[cog: + // common_filler("TYPE_NAMED_VALUE_INT", "time_boot_ms", -1, "name") + // cog.outl(f"""{p}.value_int = {val}.value;""") + // ]]] + DV dv_msg; + dv_msg.header.stamp = uas->synchronise_stamp(value.time_boot_ms); + dv_msg.type = DV::TYPE_NAMED_VALUE_INT; + dv_msg.index = -1; + dv_msg.array_id = -1; + dv_msg.name = mavlink::to_string(value.name); + dv_msg.value_int = value.value; + // [[[end]]] (checksum: 32cb48d5dad85c622997aeb6d34c255e) + + debug_logger(value.get_name(), dv_msg); + named_value_int_pub->publish(dv_msg); + } + + /* -*- callbacks -*- */ + + /** + * @brief Debug callbacks + * @param req pointer to mavros_msgs/Debug.msg being published + */ + void debug_cb(const mavros_msgs::msg::DebugValue::SharedPtr req) + { + switch (req->type) { + case DV::TYPE_DEBUG: { + mavlink::common::msg::DEBUG debug {}; + + debug.time_boot_ms = get_time_boot_ms(req->header.stamp); + debug.ind = req->index; + debug.value = req->value_float; + + uas->send_message(debug); + break; + } + case DV::TYPE_DEBUG_VECT: { + mavlink::common::msg::DEBUG_VECT debug {}; + + debug.time_usec = get_time_usec(req->header.stamp); + mavlink::set_string(debug.name, req->name); + // [[[cog: + // for i, f in enumerate("xyz"): + // cog.outl(f"debug.{f} = req->data[{i}];") + // ]]] + debug.x = req->data[0]; + debug.y = req->data[1]; + debug.z = req->data[2]; + // [[[end]]] (checksum: e3359b14c75adf35f430840dcf01ef18) + + uas->send_message(debug); + break; + } + case DV::TYPE_DEBUG_FLOAT_ARRAY: { + mavlink::common::msg::DEBUG_FLOAT_ARRAY debug {}; + + debug.time_usec = get_time_usec(req->header.stamp); + mavlink::set_string(debug.name, req->name); + std::copy_n( + req->data.begin(), std::min(req->data.size(), debug.data.size()), + std::begin(debug.data)); + + uas->send_message(debug); + break; + } + case DV::TYPE_NAMED_VALUE_FLOAT: { + mavlink::common::msg::NAMED_VALUE_FLOAT value {}; + + value.time_boot_ms = get_time_boot_ms(req->header.stamp); + mavlink::set_string(value.name, req->name); + value.value = req->value_float; + + uas->send_message(value); + break; + } + case DV::TYPE_NAMED_VALUE_INT: { + mavlink::common::msg::NAMED_VALUE_INT value {}; + + value.time_boot_ms = get_time_boot_ms(req->header.stamp); + mavlink::set_string(value.name, req->name); + value.value = req->value_int; + + uas->send_message(value); + break; + } + default: + RCLCPP_ERROR(get_logger(), "Wrong debug type (%d). Droping!...", req->type); + return; + } + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::DebugValuePlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::DebugValuePlugin) diff --git a/mavros_extras/src/plugins/distance_sensor.cpp b/mavros_extras/src/plugins/distance_sensor.cpp index b8edc1dae..63bc201c2 100644 --- a/mavros_extras/src/plugins/distance_sensor.cpp +++ b/mavros_extras/src/plugins/distance_sensor.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2015 Nuno Marques. + * Copyright 2021 Vladimir Ermakov + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Distance Sensor plugin * @file distance_sensor.cpp @@ -6,420 +14,488 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2015 Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ + +#include +#include + #include -#include -#include -#include +#include +#include +#include +#include // NOLINT cpplint, that is almost 4 years since standard release! -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -namespace mavros { -namespace extra_plugins { +#include "sensor_msgs/msg/range.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT using utils::enum_value; +using sensor_msgs::msg::Range; + class DistanceSensorPlugin; /** * @brief Distance sensor mapping storage item */ -class DistanceSensorItem { +class DistanceSensorItem : public std::enable_shared_from_this +{ public: - typedef boost::shared_ptr Ptr; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - DistanceSensorItem() : - is_subscriber(false), - send_tf(false), - sensor_id(0), - field_of_view(0), - orientation(-1), - covariance(0), - horizontal_fov_ratio(1.0), - vertical_fov_ratio(1.0), - quaternion(0.f, 0.f, 0.f, 0.f), - owner(nullptr), - data_index(0) - { } - - // params - bool is_subscriber; //!< this item is a subscriber, else is a publisher - bool send_tf; //!< defines if a transform is sent or not - uint8_t sensor_id; //!< id of the sensor - double field_of_view; //!< FOV of the sensor - Eigen::Vector3d position; //!< sensor position - int orientation; //!< check orientation of sensor if != -1 - int covariance; //!< in centimeters, current specification - std::string frame_id; //!< frame id for send - double horizontal_fov_ratio; //!< horizontal fov ratio for ROS messages - double vertical_fov_ratio; //!< vertical fov ratio for ROS messages - Eigen::Quaternionf quaternion; //!< sensor orientation in vehicle body frame for MAV_SENSOR_ROTATION_CUSTOM - - // topic handle - ros::Publisher pub; - ros::Subscriber sub; - std::string topic_name; - - DistanceSensorPlugin *owner; - - void range_cb(const sensor_msgs::Range::ConstPtr &msg); - static Ptr create_item(DistanceSensorPlugin *owner, std::string topic_name); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit DistanceSensorItem( + DistanceSensorPlugin * owner_, std::string topic_name_, + YAML::Node config); private: - std::vector data; //!< array allocation for measurements - size_t data_index; //!< array index - - static constexpr size_t ACC_SIZE = 50; - - /** - * Calculate measurements variance to send to the FCU. - */ - float calculate_variance(float range) { - if (data.size() < ACC_SIZE) { - // limits the size of the array to 50 elements - data.reserve(ACC_SIZE); - data.push_back(range); - } - else { - data[data_index] = range; // it starts rewriting the values from 1st element - if (++data_index > ACC_SIZE - 1) - data_index = 0; // restarts the index when achieves the last element - } - - float average, variance, sum = 0, sum_ = 0; - - /* Compute the sum of all elements */ - for (auto d : data) - sum += d; - - average = sum / data.size(); - - /* Compute the variance */ - for (auto d : data) - sum_ += (d - average) * (d - average); - - variance = sum_ / data.size(); - - return variance; - } + friend class DistanceSensorPlugin; + + // params + bool is_subscriber; //!< this item is a subscriber, else is a publisher + bool send_tf; //!< defines if a transform is sent or not + uint8_t sensor_id; //!< id of the sensor + double field_of_view; //!< FOV of the sensor + Eigen::Vector3d position; //!< sensor position + int orientation; //!< check orientation of sensor if != -1 + int covariance; //!< in centimeters, current specification + std::string frame_id; //!< frame id for send + double horizontal_fov_ratio; //!< horizontal fov ratio for ROS messages + double vertical_fov_ratio; //!< vertical fov ratio for ROS messages + Eigen::Quaternionf quaternion; //!< Orientation in vehicle body frame for ROTATION_CUSTOM + + // topic handle + rclcpp::Publisher::SharedPtr pub; + rclcpp::Subscription::SharedPtr sub; + std::string topic_name; + + DistanceSensorPlugin * owner; + + std::vector data; //!< array allocation for measurements + size_t data_index; //!< array index + + static constexpr size_t ACC_SIZE = 50; + + /** + * Calculate measurements variance to send to the FCU. + */ + float calculate_variance(float range) + { + if (data.size() < ACC_SIZE) { + // limits the size of the array to 50 elements + data.reserve(ACC_SIZE); + data.push_back(range); + } else { + // it starts rewriting the values from 1st element + data[data_index] = range; + if (++data_index > ACC_SIZE - 1) { + // restarts the index when achieves the last element + data_index = 0; + } + } + + float average, variance, sum = 0, sum_ = 0; + + /* Compute the sum of all elements */ + for (auto d : data) { + sum += d; + } + + average = sum / data.size(); + + /* Compute the variance */ + for (auto d : data) { + sum_ += (d - average) * (d - average); + } + + variance = sum_ / data.size(); + + return variance; + } + + //! Copy of Plugin::get_time_boot_ms() because it is private. + inline uint32_t get_time_boot_ms(const builtin_interfaces::msg::Time & t) + { + return rclcpp::Time(t).nanoseconds() / 1000000; + } + + //! sensor_msgs/Range subscription callback + void range_cb(const Range::SharedPtr msg); }; + /** * @brief Distance sensor plugin + * @plugin distance_sensor * * This plugin allows publishing distance sensor data, which is connected to * an offboard/companion computer through USB/Serial, to the FCU or vice-versa. */ -class DistanceSensorPlugin : public plugin::PluginBase { +class DistanceSensorPlugin : public plugin::Plugin +{ public: - DistanceSensorPlugin() : PluginBase(), - dist_nh("~distance_sensor") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - dist_nh.param("base_frame_id", base_frame_id, "base_link"); - - XmlRpc::XmlRpcValue map_dict; - if (!dist_nh.getParam("", map_dict)) { - ROS_WARN_NAMED("distance_sensor", "DS: plugin not configured!"); - return; - } - - ROS_ASSERT(map_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct); - - for (auto &pair : map_dict) { - ROS_DEBUG_NAMED("distance_sensor", "DS: initializing mapping for %s", pair.first.c_str()); - auto it = DistanceSensorItem::create_item(this, pair.first); - - if (it) - sensor_map[it->sensor_id] = it; - else - ROS_ERROR_NAMED("distance_sensor", "DS: bad config for %s", pair.first.c_str()); - } - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&DistanceSensorPlugin::handle_distance_sensor), - }; - } + explicit DistanceSensorPlugin(plugin::UASPtr uas_) + : plugin::Plugin(uas_, "distance_sensor") + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "base_frame_id", "base_link", [&](const rclcpp::Parameter & p) { + base_frame_id = p.as_string(); + }); + node_declare_and_watch_parameter( + "config", "", [&](const rclcpp::Parameter & p) { + std::unique_lock lock(mutex); + + sensor_map.clear(); + + auto lg = get_logger(); + YAML::Node root_node; + + try { + root_node = YAML::Load(p.as_string()); + } catch (const YAML::ParserException & ex) { + RCLCPP_ERROR_STREAM(lg, "DS: Failed to parse config: " << ex.what()); + return; + } + + if (root_node.IsNull()) { + RCLCPP_INFO(lg, "DS: Plugin not configured!"); + return; + } else if (!root_node.IsMap()) { + RCLCPP_ERROR(lg, "DS: Config must be a map."); + return; + } + + for (auto it = root_node.begin(); it != root_node.end(); ++it) { + auto key_s = it->first.as(); + RCLCPP_INFO_STREAM(lg, "DS: " << key_s << ": Loading config: " << it->second); + + try { + auto item = std::make_shared(this, key_s, it->second); + sensor_map[item->sensor_id] = item; + } catch (const std::exception & ex) { + RCLCPP_ERROR_STREAM(lg, "DS: " << key_s << ": Failed to load mapping: " << ex.what()); + } + } + }); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&DistanceSensorPlugin::handle_distance_sensor), + }; + } private: - friend class DistanceSensorItem; - - ros::NodeHandle dist_nh; - - std::string base_frame_id; - - std::unordered_map sensor_map; - - /* -*- low-level send -*- */ - void distance_sensor(uint32_t time_boot_ms, - uint32_t min_distance, - uint32_t max_distance, - uint32_t current_distance, - uint8_t type, uint8_t id, - uint8_t orientation, uint8_t covariance, - float horizontal_fov, float vertical_fov, - std::array quaternion) - { - mavlink::common::msg::DISTANCE_SENSOR ds = {}; - - // [[[cog: - // for f in ('time_boot_ms', - // 'min_distance', - // 'max_distance', - // 'current_distance', - // 'type', - // 'id', - // 'orientation', - // 'covariance', - // 'horizontal_fov', - // 'vertical_fov', - // 'quaternion'): - // cog.outl("ds.%s = %s;" % (f, f)) - // ]]] - ds.time_boot_ms = time_boot_ms; - ds.min_distance = min_distance; - ds.max_distance = max_distance; - ds.current_distance = current_distance; - ds.type = type; - ds.id = id; - ds.orientation = orientation; - ds.covariance = covariance; - ds.horizontal_fov = horizontal_fov; - ds.vertical_fov = vertical_fov; - ds.quaternion = quaternion; - // [[[end]]] (checksum: 23b6c8fb20bd494eafdf08d5888c9c76) - - UAS_FCU(m_uas)->send_message_ignore_drop(ds); - } - - /* -*- mid-level helpers -*- */ - - /** - * Receive distance sensor data from FCU. - */ - void handle_distance_sensor(const mavlink::mavlink_message_t *msg, mavlink::common::msg::DISTANCE_SENSOR &dist_sen) - { - using mavlink::common::MAV_SENSOR_ORIENTATION; - using mavlink::common::MAV_DISTANCE_SENSOR; - - auto it = sensor_map.find(dist_sen.id); - if (it == sensor_map.end()) { - ROS_ERROR_NAMED("distance_sensor", - "DS: no mapping for sensor id: %d, type: %d, orientation: %d", - dist_sen.id, dist_sen.type, dist_sen.orientation); - return; - } - - auto sensor = it->second; - if (sensor->is_subscriber) { - ROS_ERROR_ONCE_NAMED("distance_sensor", - "DS: %s (id %d) is subscriber, but i got sensor data for that id from FCU", - sensor->topic_name.c_str(), sensor->sensor_id); - - return; - } - - if (sensor->orientation >= 0 && dist_sen.orientation != sensor->orientation) { - ROS_ERROR_NAMED("distance_sensor", - "DS: %s: received sensor data has different orientation (%s) than in config (%s)!", - sensor->topic_name.c_str(), - utils::to_string_enum(dist_sen.orientation).c_str(), - utils::to_string_enum(sensor->orientation).c_str()); - return; - } - - auto range = boost::make_shared(); - - range->header = m_uas->synchronized_header(sensor->frame_id, dist_sen.time_boot_ms); - - range->min_range = dist_sen.min_distance * 1E-2;// in meters - range->max_range = dist_sen.max_distance * 1E-2; - range->field_of_view = sensor->field_of_view; - - switch (dist_sen.type) { - case enum_value(MAV_DISTANCE_SENSOR::LASER): - case enum_value(MAV_DISTANCE_SENSOR::RADAR): - case enum_value(MAV_DISTANCE_SENSOR::UNKNOWN): - range->radiation_type = sensor_msgs::Range::INFRARED; - break; - case enum_value(MAV_DISTANCE_SENSOR::ULTRASOUND): - range->radiation_type = sensor_msgs::Range::ULTRASOUND; - break; - default: - ROS_ERROR_NAMED("distance_sensor", - "DS: %s: Wrong/undefined type of sensor (type: %d). Droping!...", - sensor->topic_name.c_str(), dist_sen.type); - return; - } - - range->range = dist_sen.current_distance * 1E-2; // in meters - - if (sensor->send_tf) { - /* variables init */ - auto q = utils::sensor_orientation_matching(static_cast(dist_sen.orientation)); - - geometry_msgs::TransformStamped transform; - - transform.header = m_uas->synchronized_header(base_frame_id, dist_sen.time_boot_ms); - transform.child_frame_id = sensor->frame_id; - - /* rotation and position set */ - tf::quaternionEigenToMsg(q, transform.transform.rotation); - tf::vectorEigenToMsg(sensor->position, transform.transform.translation); - - /* transform broadcast */ - m_uas->tf2_broadcaster.sendTransform(transform); - } - - sensor->pub.publish(range); - } + friend class DistanceSensorItem; + using ItemPtr = std::shared_ptr; + + std::string base_frame_id; + + std::shared_mutex mutex; + std::unordered_map sensor_map; + + /* -*- low-level send -*- */ + void distance_sensor( + uint32_t time_boot_ms, + uint32_t min_distance, + uint32_t max_distance, + uint32_t current_distance, + uint8_t type, uint8_t id, + uint8_t orientation, uint8_t covariance, + float horizontal_fov, float vertical_fov, + std::array quaternion, uint8_t signal_quality) + { + mavlink::common::msg::DISTANCE_SENSOR ds = {}; + + // [[[cog: + // for f in ('time_boot_ms', + // 'min_distance', + // 'max_distance', + // 'current_distance', + // 'type', + // 'id', + // 'orientation', + // 'covariance', + // 'horizontal_fov', + // 'vertical_fov', + // 'quaternion', + // 'signal_quality'): + // cog.outl(f"ds.{f} = {f};") + // ]]] + ds.time_boot_ms = time_boot_ms; + ds.min_distance = min_distance; + ds.max_distance = max_distance; + ds.current_distance = current_distance; + ds.type = type; + ds.id = id; + ds.orientation = orientation; + ds.covariance = covariance; + ds.horizontal_fov = horizontal_fov; + ds.vertical_fov = vertical_fov; + ds.quaternion = quaternion; + ds.signal_quality = signal_quality; + // [[[end]]] (checksum: b268a118afee5e2c6cb3e1094a578fff) + + uas->send_message(ds); + } + + /* -*- mid-level helpers -*- */ + + /** + * Receive distance sensor data from FCU. + */ + void handle_distance_sensor( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::DISTANCE_SENSOR & dist_sen, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + using mavlink::common::MAV_SENSOR_ORIENTATION; + using mavlink::common::MAV_DISTANCE_SENSOR; + + std::shared_lock lock(mutex); + + auto lg = get_logger(); + + auto it = sensor_map.find(dist_sen.id); + if (it == sensor_map.end()) { + RCLCPP_ERROR( + lg, + "DS: no mapping for sensor id: %d, type: %d, orientation: %d", + dist_sen.id, dist_sen.type, dist_sen.orientation); + return; + } + + auto sensor = it->second; + if (sensor->is_subscriber) { + RCLCPP_ERROR( + lg, + "DS: %s (id %d) is subscriber, but i got sensor data for that id from FCU", + sensor->topic_name.c_str(), sensor->sensor_id); + + return; + } + + if (sensor->orientation >= 0 && dist_sen.orientation != sensor->orientation) { + RCLCPP_ERROR( + lg, + "DS: %s: received sensor data has different orientation (%s) than in config (%s)!", + sensor->topic_name.c_str(), + utils::to_string_enum(dist_sen.orientation).c_str(), + utils::to_string_enum(sensor->orientation).c_str()); + return; + } + + auto range = Range(); + + range.header = uas->synchronized_header(sensor->frame_id, dist_sen.time_boot_ms); + + range.min_range = dist_sen.min_distance * 1E-2; // in meters + range.max_range = dist_sen.max_distance * 1E-2; + range.field_of_view = sensor->field_of_view; + + switch (dist_sen.type) { + case enum_value(MAV_DISTANCE_SENSOR::LASER): + case enum_value(MAV_DISTANCE_SENSOR::RADAR): + case enum_value(MAV_DISTANCE_SENSOR::UNKNOWN): + range.radiation_type = Range::INFRARED; + break; + case enum_value(MAV_DISTANCE_SENSOR::ULTRASOUND): + range.radiation_type = Range::ULTRASOUND; + break; + default: + RCLCPP_ERROR( + lg, + "DS: %s: Wrong/undefined type of sensor (type: %d). Dropping!...", + sensor->topic_name.c_str(), dist_sen.type); + return; + } + + range.range = dist_sen.current_distance * 1E-2; // in meters + + if (sensor->send_tf) { + /* variables init */ + Eigen::Quaterniond q; + if (dist_sen.orientation == enum_value(MAV_SENSOR_ORIENTATION::ROTATION_CUSTOM)) { + q = ftf::mavlink_to_quaternion(dist_sen.quaternion); + } else { + q = + utils::sensor_orientation_matching( + static_cast(dist_sen.orientation)); + } + + geometry_msgs::msg::TransformStamped transform; + + transform.header = uas->synchronized_header(base_frame_id, dist_sen.time_boot_ms); + transform.child_frame_id = sensor->frame_id; + + /* rotation and position set */ + transform.transform.rotation = tf2::toMsg(q); + tf2::toMsg(sensor->position, transform.transform.translation); + + /* transform broadcast */ + uas->tf2_broadcaster.sendTransform(transform); + } + + sensor->pub->publish(range); + } }; -void DistanceSensorItem::range_cb(const sensor_msgs::Range::ConstPtr &msg) +DistanceSensorItem::DistanceSensorItem( + DistanceSensorPlugin * owner_, std::string topic_name_, + YAML::Node config) +: is_subscriber(false), + send_tf(false), + sensor_id(0), + field_of_view(0), + position(0.0, 0.0, 0.0), + orientation(-1), + covariance(0), + horizontal_fov_ratio(1.0), + vertical_fov_ratio(1.0), + quaternion(0.f, 0.f, 0.f, 0.f), + topic_name(topic_name_), + owner(owner_), + data{}, + data_index(0) { - using mavlink::common::MAV_DISTANCE_SENSOR; - - uint8_t type = 0; - uint8_t covariance_ = 0; - - if (covariance > 0) covariance_ = covariance; - else covariance_ = uint8_t(calculate_variance(msg->range) * 1E2); // in cm - - /** @todo Propose changing covarince from uint8_t to float */ - ROS_DEBUG_NAMED("distance_sensor", "DS: %d: sensor variance: %f", sensor_id, calculate_variance(msg->range) * 1E2); - - // current mapping, may change later - if (msg->radiation_type == sensor_msgs::Range::INFRARED) - type = enum_value(MAV_DISTANCE_SENSOR::LASER); - else if (msg->radiation_type == sensor_msgs::Range::ULTRASOUND) - type = enum_value(MAV_DISTANCE_SENSOR::ULTRASOUND); - - std::array q; - ftf::quaternion_to_mavlink(quaternion, q); - - owner->distance_sensor( - msg->header.stamp.toNSec() / 1000000, - msg->min_range / 1E-2, - msg->max_range / 1E-2, - msg->range / 1E-2, - type, - sensor_id, - orientation, - covariance_, - msg->field_of_view * horizontal_fov_ratio, - msg->field_of_view * vertical_fov_ratio, - q); + using MSO = mavlink::common::MAV_SENSOR_ORIENTATION; + std::string orientation_str{}; + + // load and parse paras + // first decide the type of topic (sub or pub) + is_subscriber = config["subscriber"].as(false); + + // sensor id + if (auto idn = config["id"]; idn) { + sensor_id = idn.as(); + } else { + throw std::invalid_argument("`id` field required"); + } + + // orientation, checks later + if (auto qn = config["orientation"]; qn) { + // lookup for numeric value + orientation_str = qn.as(); + orientation = utils::sensor_orientation_from_str(orientation_str); + } else { + orientation = -1; // not set + } + + if (!is_subscriber) { + // publisher params + // frame_id and FOV is required + frame_id = config["frame_id"].as(); + field_of_view = config["field_of_view"].as(); + + // unset allowed, setted wrong - not + if (orientation == -1 && !orientation_str.empty()) { + throw std::invalid_argument("defined orientation is not valid!"); + } + + // optional + send_tf = config["send_tf"].as(false); + if (auto spn = config["sensor_position"]; spn && send_tf) { + // sensor position defined if 'send_tf' set to TRUE + position.x() = spn["x"].as(0.0); + position.y() = spn["y"].as(0.0); + position.z() = spn["z"].as(0.0); + } + } else { + // subscriber params + // orientation is required + if (orientation_str.empty()) { + throw std::invalid_argument("`orientation` field required"); + } + + if (orientation == -1) { + throw std::invalid_argument("defined orientation is not valid!"); + } + + if (orientation == enum_value(MSO::ROTATION_CUSTOM) && + !config["custom_orientation"]) + { + throw std::invalid_argument("`custom_orientation` required for orientation=CUSTOM"); + } + + + // optional + covariance = config["covariance"].as(0); + horizontal_fov_ratio = config["horizontal_fov_ratio"].as(0.0); + vertical_fov_ratio = config["vertical_fov_ratio"].as(0.0); + if (auto con = config["custom_orientation"]; + con && orientation == enum_value(MSO::ROTATION_CUSTOM)) + { + Eigen::Vector3d rpy; + + rpy.x() = con["roll"].as(0); + rpy.y() = con["pitch"].as(0); + rpy.z() = con["yaw"].as(0); + + constexpr auto DEG_TO_RAD = (M_PI / 180.0); + quaternion = Eigen::Quaternionf(ftf::quaternion_from_rpy(rpy * DEG_TO_RAD)); + } + } + + // create topic handles + auto sensor_qos = rclcpp::SensorDataQoS(); + if (!is_subscriber) { + pub = owner->node->create_publisher(topic_name, sensor_qos); + } else { + sub = + owner->node->create_subscription( + topic_name, sensor_qos, + std::bind(&DistanceSensorItem::range_cb, this, _1)); + } } -DistanceSensorItem::Ptr DistanceSensorItem::create_item(DistanceSensorPlugin *owner, std::string topic_name) +void DistanceSensorItem::range_cb(const Range::SharedPtr msg) { - auto p = boost::make_shared(); - std::string orientation_str; - - ros::NodeHandle pnh(owner->dist_nh, topic_name); - - p->owner = owner; - p->topic_name = topic_name; - - // load and parse params - // first decide the type of topic (sub or pub) - pnh.param("subscriber", p->is_subscriber, false); - - // sensor id - int id; - if (!pnh.getParam("id", id)) { - ROS_ERROR_NAMED("distance_sensor", "DS: %s: `id` not set!", topic_name.c_str()); - p.reset(); return p; // return nullptr because of a bug related to gcc 4.6 - } - p->sensor_id = id; - - // orientation, checks later - if (!pnh.getParam("orientation", orientation_str)) - p->orientation = -1; // not set - else - // lookup for numeric value - p->orientation = utils::sensor_orientation_from_str(orientation_str); - - - if (!p->is_subscriber) { - // publisher params - // frame_id and FOV is required - if (!pnh.getParam("frame_id", p->frame_id)) { - ROS_ERROR_NAMED("distance_sensor", "DS: %s: `frame_id` not set!", topic_name.c_str()); - p.reset(); return p; // nullptr - } - - if (!pnh.getParam("field_of_view", p->field_of_view)) { - ROS_ERROR_NAMED("distance_sensor", "DS: %s: sensor FOV not set!", topic_name.c_str()); - p.reset(); return p; // nullptr - } - - // unset allowed, setted wrong - not - if (p->orientation == -1 && !orientation_str.empty()) { - ROS_ERROR_NAMED("distance_sensor", "DS: %s: defined orientation (%s) is not valid!", - topic_name.c_str(), orientation_str.c_str()); - p.reset(); return p; // nullptr - } - - // optional - pnh.param("send_tf", p->send_tf, false); - if (p->send_tf) { // sensor position defined if 'send_tf' set to TRUE - pnh.param("sensor_position/x", p->position.x(), 0.0); - pnh.param("sensor_position/y", p->position.y(), 0.0); - pnh.param("sensor_position/z", p->position.z(), 0.0); - ROS_DEBUG_NAMED("sensor_position", "DS: %s: Sensor position at: %f, %f, %f", topic_name.c_str(), - p->position.x(), p->position.y(), p->position.z()); - } - } - else { - // subscriber params - // orientation is required - if (orientation_str.empty()) { - ROS_ERROR_NAMED("distance_sensor", "DS: %s: orientation not set!", topic_name.c_str()); - p.reset(); return p; // nullptr - } - else if (p->orientation == -1) { - ROS_ERROR_NAMED("distance_sensor", "DS: %s: defined orientation (%s) is not valid!", topic_name.c_str(), orientation_str.c_str()); - p.reset(); return p; // nullptr - } - - // optional - pnh.param("covariance", p->covariance, 0); - pnh.param("horizontal_fov_ratio", p->horizontal_fov_ratio, 1.0); - pnh.param("vertical_fov_ratio", p->vertical_fov_ratio, 1.0); - if (p->orientation == enum_value(mavlink::common::MAV_SENSOR_ORIENTATION::ROTATION_CUSTOM)) { - Eigen::Vector3d rpy; - pnh.param("custom_orientation/roll", rpy.x(), 0.0); - pnh.param("custom_orientation/pitch", rpy.y(), 0.0); - pnh.param("custom_orientation/yaw", rpy.z(), 0.0); - constexpr auto DEG_TO_RAD = (M_PI / 180.0); - p->quaternion = Eigen::Quaternionf(ftf::quaternion_from_rpy(rpy * DEG_TO_RAD)); - } - } - - // create topic handles - if (!p->is_subscriber) - p->pub = owner->dist_nh.advertise(topic_name, 10); - else - p->sub = owner->dist_nh.subscribe(topic_name, 10, &DistanceSensorItem::range_cb, p.get()); - - return p; + using mavlink::common::MAV_DISTANCE_SENSOR; + + uint8_t type = 0; + uint8_t covariance_ = 0; + + if (covariance > 0) { + covariance_ = covariance; + } else { + covariance_ = uint8_t(calculate_variance(msg->range) * 1E2); // in cm + } + + // current mapping, may change later + if (msg->radiation_type == Range::INFRARED) { + type = enum_value(MAV_DISTANCE_SENSOR::LASER); + } else if (msg->radiation_type == Range::ULTRASOUND) { + type = enum_value(MAV_DISTANCE_SENSOR::ULTRASOUND); + } + + std::array q; + ftf::quaternion_to_mavlink(quaternion, q); + + owner->distance_sensor( + get_time_boot_ms(msg->header.stamp), + msg->min_range / 1E-2, + msg->max_range / 1E-2, + msg->range / 1E-2, + type, + sensor_id, + orientation, + covariance_, + msg->field_of_view * horizontal_fov_ratio, + msg->field_of_view * vertical_fov_ratio, + q, + 0); } -} // namespace extra_plugins -} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::DistanceSensorPlugin, mavros::plugin::PluginBase) +} // namespace extra_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::DistanceSensorPlugin) diff --git a/mavros_extras/src/plugins/esc_status.cpp b/mavros_extras/src/plugins/esc_status.cpp index 8d8a5e2cd..c23516c59 100644 --- a/mavros_extras/src/plugins/esc_status.cpp +++ b/mavros_extras/src/plugins/esc_status.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2020 Ricardo Marques . + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief ESC status plugin * @file esc_status.cpp @@ -6,149 +13,144 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2020 Ricardo Marques . - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/esc_info.hpp" +#include "mavros_msgs/msg/esc_status.hpp" namespace mavros { namespace extra_plugins { +using namespace std::placeholders; // NOLINT + /** * @brief ESC status plugin + * @plugin esc_status */ -class ESCStatusPlugin : public plugin::PluginBase +class ESCStatusPlugin : public plugin::Plugin { public: - ESCStatusPlugin() : PluginBase(), - nh("~"), - _max_esc_count(0), - _max_esc_info_index(0), - _max_esc_status_index(0) - {} - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - esc_info_pub = nh.advertise("esc_info", 10); - esc_status_pub = nh.advertise("esc_status", 10); - - enable_connection_cb(); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&ESCStatusPlugin::handle_esc_info), - make_handler(&ESCStatusPlugin::handle_esc_status), - }; - } + explicit ESCStatusPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "esc_status"), + _max_esc_count(0), + _max_esc_info_index(0), + _max_esc_status_index(0) + { + esc_info_pub = node->create_publisher("~/info", 10); + esc_status_pub = node->create_publisher("~/status", 10); + + enable_connection_cb(); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&ESCStatusPlugin::handle_esc_info), + make_handler(&ESCStatusPlugin::handle_esc_status), + }; + } private: - using lock_guard = std::lock_guard; - std::mutex mutex; - - ros::NodeHandle nh; - - ros::Publisher esc_info_pub; - ros::Publisher esc_status_pub; - mavros_msgs::ESCInfo _esc_info; - mavros_msgs::ESCStatus _esc_status; - uint8_t _max_esc_count; - uint8_t _max_esc_info_index; - uint8_t _max_esc_status_index; - const uint8_t batch_size = 4; - - void handle_esc_info(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ESC_INFO &esc_info) - { - lock_guard lock(mutex); - - _esc_info.header.stamp = m_uas->synchronise_stamp(esc_info.time_usec); - - uint8_t esc_index = esc_info.index; - - _esc_info.counter = esc_info.counter; - _esc_info.count = esc_info.count; - _esc_info.connection_type = esc_info.connection_type; - _esc_info.info = esc_info.info; - - if (_esc_info.count > _max_esc_count) - { - _max_esc_count = _esc_info.count; - } - - if (_esc_info.esc_info.size() < _max_esc_count) - { - _esc_info.esc_info.resize(_max_esc_count); - } - - for (int i = 0; i < std::min(batch_size, ssize_t(_max_esc_count) - esc_index); i++) - { - _esc_info.esc_info[esc_index + i].header = _esc_info.header; - _esc_info.esc_info[esc_index + i].failure_flags = esc_info.failure_flags[i]; - _esc_info.esc_info[esc_index + i].error_count = esc_info.error_count[i]; - _esc_info.esc_info[esc_index + i].temperature = esc_info.temperature[i] * 1E2; //!< [degreesC] - } - - _max_esc_info_index = std::max(_max_esc_info_index, esc_info.index); - - if (_max_esc_info_index == esc_info.index) - { - esc_info_pub.publish(_esc_info); - } - } - - void handle_esc_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ESC_STATUS &esc_status) - { - lock_guard lock(mutex); - - uint8_t esc_index = esc_status.index; - - if (_esc_status.esc_status.size() < _max_esc_count) - { - _esc_status.esc_status.resize(_max_esc_count); - } - - _esc_status.header.stamp = m_uas->synchronise_stamp(esc_status.time_usec); - - for (int i = 0; i < std::min(batch_size, ssize_t(_max_esc_count) - esc_index); i++) - { - _esc_status.esc_status[esc_index + i].header = _esc_status.header; - _esc_status.esc_status[esc_index + i].rpm = esc_status.rpm[i]; - _esc_status.esc_status[esc_index + i].voltage = esc_status.voltage[i]; - _esc_status.esc_status[esc_index + i].current = esc_status.current[i]; - } - - _max_esc_status_index = std::max(_max_esc_status_index, esc_status.index); - - if (_max_esc_status_index == esc_status.index) - { - esc_status_pub.publish(_esc_status); - } - } - - void connection_cb(bool connected) override - { - lock_guard lock(mutex); - - _max_esc_count = 0; - _max_esc_status_index = 0; - _max_esc_info_index = 0; - _esc_info.esc_info.resize(0); - _esc_status.esc_status.resize(0); - } + using lock_guard = std::lock_guard; + + rclcpp::Publisher::SharedPtr esc_info_pub; + rclcpp::Publisher::SharedPtr esc_status_pub; + + std::mutex mutex; + mavros_msgs::msg::ESCInfo _esc_info; + mavros_msgs::msg::ESCStatus _esc_status; + uint8_t _max_esc_count; + uint8_t _max_esc_info_index; + uint8_t _max_esc_status_index; + const uint8_t batch_size = 4; + + void handle_esc_info( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::ESC_INFO & esc_info, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + lock_guard lock(mutex); + + _esc_info.header.stamp = uas->synchronise_stamp(esc_info.time_usec); + + uint8_t esc_index = esc_info.index; + + _esc_info.counter = esc_info.counter; + _esc_info.count = esc_info.count; + _esc_info.connection_type = esc_info.connection_type; + _esc_info.info = esc_info.info; + + if (_esc_info.count > _max_esc_count) { + _max_esc_count = _esc_info.count; + } + + if (_esc_info.esc_info.size() < _max_esc_count) { + _esc_info.esc_info.resize(_max_esc_count); + } + + for (int i = 0; i < std::min(batch_size, ssize_t(_max_esc_count) - esc_index); i++) { + _esc_info.esc_info[esc_index + i].header = _esc_info.header; + _esc_info.esc_info[esc_index + i].failure_flags = esc_info.failure_flags[i]; + _esc_info.esc_info[esc_index + i].error_count = esc_info.error_count[i]; + _esc_info.esc_info[esc_index + i].temperature = esc_info.temperature[i] * 1E2; //!< [degC] + } + + _max_esc_info_index = std::max(_max_esc_info_index, esc_info.index); + + if (_max_esc_info_index == esc_info.index) { + esc_info_pub->publish(_esc_info); + } + } + + void handle_esc_status( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::ESC_STATUS & esc_status, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + lock_guard lock(mutex); + + uint8_t esc_index = esc_status.index; + + if (_esc_status.esc_status.size() < _max_esc_count) { + _esc_status.esc_status.resize(_max_esc_count); + } + + _esc_status.header.stamp = uas->synchronise_stamp(esc_status.time_usec); + + for (int i = 0; i < std::min(batch_size, ssize_t(_max_esc_count) - esc_index); i++) { + _esc_status.esc_status[esc_index + i].header = _esc_status.header; + _esc_status.esc_status[esc_index + i].rpm = esc_status.rpm[i]; + _esc_status.esc_status[esc_index + i].voltage = esc_status.voltage[i]; + _esc_status.esc_status[esc_index + i].current = esc_status.current[i]; + } + + _max_esc_status_index = std::max(_max_esc_status_index, esc_status.index); + + if (_max_esc_status_index == esc_status.index) { + esc_status_pub->publish(_esc_status); + } + } + + void connection_cb(bool connected [[maybe_unused]]) override + { + lock_guard lock(mutex); + + _max_esc_count = 0; + _max_esc_status_index = 0; + _max_esc_info_index = 0; + _esc_info.esc_info.resize(0); + _esc_status.esc_status.resize(0); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::ESCStatusPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::ESCStatusPlugin) diff --git a/mavros_extras/src/plugins/esc_telemetry.cpp b/mavros_extras/src/plugins/esc_telemetry.cpp index 7264ca135..05dd8926e 100644 --- a/mavros_extras/src/plugins/esc_telemetry.cpp +++ b/mavros_extras/src/plugins/esc_telemetry.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2020 Braedon O'Meara . + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief APM ESC Telemetry plugin * @file esc_telemetry.cpp @@ -6,112 +13,116 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2021 Braedon O'Meara . - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/esc_telemetry.hpp" namespace mavros { namespace extra_plugins { +using namespace std::placeholders; // NOLINT + /** * @brief ESC telemetry plugin + * @plugin esc_telemetry * * APM specific plugin. */ -class ESCTelemetryPlugin : public plugin::PluginBase +class ESCTelemetryPlugin : public plugin::Plugin { public: - ESCTelemetryPlugin() : PluginBase(), - nh("~") - {} - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - esc_telemetry_pub = nh.advertise("esc_telemetry", 10); - - enable_connection_cb(); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&ESCTelemetryPlugin::handle_esc_telemetry_1_to_4), - make_handler(&ESCTelemetryPlugin::handle_esc_telemetry_5_to_8), - make_handler(&ESCTelemetryPlugin::handle_esc_telemetry_9_to_12), - }; - } + explicit ESCTelemetryPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "esc_telemetry") + { + esc_telemetry_pub = node->create_publisher("~/telemetry", 10); + + enable_connection_cb(); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&ESCTelemetryPlugin::handle_esc_telemetry_1_to_4), + make_handler(&ESCTelemetryPlugin::handle_esc_telemetry_5_to_8), + make_handler(&ESCTelemetryPlugin::handle_esc_telemetry_9_to_12), + }; + } private: - using lock_guard = std::lock_guard; - std::mutex mutex; - - ros::NodeHandle nh; - - ros::Publisher esc_telemetry_pub; - mavros_msgs::ESCTelemetry _esc_telemetry; - - template - void handle_esc_telemetry(const mavlink::mavlink_message_t *msg, msgT &et, size_t offset = 0) - { - lock_guard lock(mutex); - - size_t requred_size = offset + et.temperature.size(); - if (_esc_telemetry.esc_telemetry.size() < requred_size) { - _esc_telemetry.esc_telemetry.resize(requred_size); - } - - auto stamp = ros::Time::now(); - - _esc_telemetry.header.stamp = stamp; - for (size_t i = 0; i < et.temperature.size(); i++) { - auto &p = _esc_telemetry.esc_telemetry.at(offset + i); - - p.header.stamp = stamp; - p.temperature = et.temperature[i]; - p.voltage = et.voltage[i] / 100.0f; // centiV -> V - p.current = et.current[i] / 100.0f; // centiA -> A - p.totalcurrent = et.totalcurrent[i] / 1000.0f; // mAh -> Ah - p.rpm = et.rpm[i]; - p.count = et.count[i]; - } - - esc_telemetry_pub.publish(_esc_telemetry); - } - - void handle_esc_telemetry_1_to_4(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::ESC_TELEMETRY_1_TO_4 &esc_telemetry) - { - handle_esc_telemetry(msg, esc_telemetry, 0); - } - - void handle_esc_telemetry_5_to_8(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::ESC_TELEMETRY_5_TO_8 &esc_telemetry) - { - handle_esc_telemetry(msg, esc_telemetry, 4); - } - - void handle_esc_telemetry_9_to_12(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::ESC_TELEMETRY_9_TO_12 &esc_telemetry) - { - handle_esc_telemetry(msg, esc_telemetry, 8); - } - - void connection_cb(bool connected) override - { - lock_guard lock(mutex); - - _esc_telemetry.esc_telemetry.clear(); - } + using lock_guard = std::lock_guard; + std::mutex mutex; + + rclcpp::Publisher::SharedPtr esc_telemetry_pub; + mavros_msgs::msg::ESCTelemetry _esc_telemetry; + + template + void handle_esc_telemetry( + const mavlink::mavlink_message_t * msg [[maybe_unused]], msgT & et, + size_t offset = 0) + { + lock_guard lock(mutex); + + size_t requred_size = offset + et.temperature.size(); + if (_esc_telemetry.esc_telemetry.size() < requred_size) { + _esc_telemetry.esc_telemetry.resize(requred_size); + } + + auto stamp = node->now(); + + _esc_telemetry.header.stamp = stamp; + for (size_t i = 0; i < et.temperature.size(); i++) { + auto & p = _esc_telemetry.esc_telemetry.at(offset + i); + + p.header.stamp = stamp; + p.temperature = et.temperature[i]; + p.voltage = et.voltage[i] / 100.0f; // centiV -> V + p.current = et.current[i] / 100.0f; // centiA -> A + p.totalcurrent = et.totalcurrent[i] / 1000.0f; // mAh -> Ah + p.rpm = et.rpm[i]; + p.count = et.count[i]; + } + + esc_telemetry_pub->publish(_esc_telemetry); + } + + void handle_esc_telemetry_1_to_4( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::ardupilotmega::msg::ESC_TELEMETRY_1_TO_4 & esc_telemetry, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + handle_esc_telemetry(msg, esc_telemetry, 0); + } + + void handle_esc_telemetry_5_to_8( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::ardupilotmega::msg::ESC_TELEMETRY_5_TO_8 & esc_telemetry, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + handle_esc_telemetry(msg, esc_telemetry, 4); + } + + void handle_esc_telemetry_9_to_12( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::ardupilotmega::msg::ESC_TELEMETRY_9_TO_12 & esc_telemetry, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + handle_esc_telemetry(msg, esc_telemetry, 8); + } + + void connection_cb(bool connected [[maybe_unused]]) override + { + lock_guard lock(mutex); + + _esc_telemetry.esc_telemetry.clear(); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::ESCTelemetryPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::ESCTelemetryPlugin) diff --git a/mavros_extras/src/plugins/fake_gps.cpp b/mavros_extras/src/plugins/fake_gps.cpp index d0a06379f..cd86bf6e4 100644 --- a/mavros_extras/src/plugins/fake_gps.cpp +++ b/mavros_extras/src/plugins/fake_gps.cpp @@ -1,3 +1,12 @@ +/* + * Copyright 2015 Christoph Tobler. + * Copyright 2017 Nuno Marques. + * Copyright 2019 Amilcar Lucas. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Fake GPS with local position source plugin * @file fake_gps.cpp @@ -8,361 +17,437 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2015 Christoph Tobler. - * Copyright 2017 Nuno Marques. - * Copyright 2019 Amilcar Lucas. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include -#include +#include #include #include -#include -#include -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" +#include "mavros/setpoint_mixin.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" // the number of GPS leap seconds #define GPS_LEAPSECONDS_MILLIS 18000ULL #define MSEC_PER_WEEK (7ULL * 86400ULL * 1000ULL) -#define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS) - -namespace mavros { -namespace extra_plugins { +#define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * MSEC_PER_WEEK - \ + GPS_LEAPSECONDS_MILLIS) + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT using mavlink::common::GPS_FIX_TYPE; using mavlink::common::GPS_INPUT_IGNORE_FLAGS; /** * @brief Fake GPS plugin. + * @plugin fake_gps * * Sends fake GPS from local position estimation source data (motion capture, * vision) to FCU - processed in HIL mode or out of it if parameter MAV_USEHILGPS * is set on PX4 Pro Autopilot Firmware; Ardupilot Firmware already supports it * without a flag set. */ -class FakeGPSPlugin : public plugin::PluginBase, - private plugin::TF2ListenerMixin { +class FakeGPSPlugin : public plugin::Plugin, + private plugin::TF2ListenerMixin +{ public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - FakeGPSPlugin() : PluginBase(), - fp_nh("~fake_gps"), - gps_rate(5.0), - // WGS-84 ellipsoid (a - equatorial radius, f - flattening of ellipsoid) - earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()), - use_mocap(true), - use_vision(false), - use_hil_gps(true), - mocap_transform(true), - mocap_withcovariance(false), - tf_listen(false), - eph(2.0), - epv(2.0), - horiz_accuracy(0.0f), - vert_accuracy(0.0f), - speed_accuracy(0.0f), - gps_id(0), - satellites_visible(5), - fix_type(GPS_FIX_TYPE::NO_GPS), - tf_rate(10.0), - map_origin(0.0, 0.0, 0.0) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - double _gps_rate; - double origin_lat, origin_lon, origin_alt; - - last_pos_time = ros::Time(0.0); - - // general params - fp_nh.param("gps_id", gps_id, 0); - int ft_i; - fp_nh.param("fix_type", ft_i, utils::enum_value(GPS_FIX_TYPE::NO_GPS)); - fix_type = static_cast(ft_i); - fp_nh.param("gps_rate", _gps_rate, 5.0); // GPS data rate of 5hz - gps_rate = _gps_rate; - fp_nh.param("eph", eph, 2.0); - fp_nh.param("epv", epv, 2.0); - fp_nh.param("horiz_accuracy", horiz_accuracy, 0.0f); - fp_nh.param("vert_accuracy", vert_accuracy, 0.0f); - fp_nh.param("speed_accuracy", speed_accuracy, 0.0f); - fp_nh.param("satellites_visible", satellites_visible, 5); - - // default origin/starting point: Zürich geodetic coordinates - fp_nh.param("geo_origin/lat", origin_lat, 47.3667); // [degrees] - fp_nh.param("geo_origin/lon", origin_lon, 8.5500); // [degrees] - fp_nh.param("geo_origin/alt", origin_alt, 408.0); // [meters - height over the WGS-84 ellipsoid] - - // init map origin with geodetic coordinates - map_origin = {origin_lat, origin_lon, origin_alt}; - - try { - /** - * @brief Conversion of the origin from geodetic coordinates (LLA) - * to ECEF (Earth-Centered, Earth-Fixed) - */ - earth.Forward(map_origin.x(), map_origin.y(), map_origin.z(), - ecef_origin.x(), ecef_origin.y(), ecef_origin.z()); - } - catch (const std::exception& e) { - ROS_INFO_STREAM("FGPS: Caught exception: " << e.what() << std::endl); - } - - // source set params - fp_nh.param("use_mocap", use_mocap, true); // listen to MoCap source - fp_nh.param("mocap_transform", mocap_transform, true); // listen to MoCap source (TransformStamped if true; PoseStamped if false) - fp_nh.param("mocap_withcovariance", mocap_withcovariance, false); // ~mocap/pose uses PoseWithCovarianceStamped Message - - fp_nh.param("tf/listen", tf_listen, false); // listen to TF source - fp_nh.param("use_vision", use_vision, false); // listen to Vision source - fp_nh.param("use_hil_gps", use_hil_gps, true); // send HIL_GPS MAVLink messages if true, - // send GPS_INPUT mavlink messages if false - - // tf params - fp_nh.param("tf/frame_id", tf_frame_id, "map"); - fp_nh.param("tf/child_frame_id", tf_child_frame_id, "fix"); - fp_nh.param("tf/rate_limit", tf_rate, 10.0); - - if (use_mocap) { - if (mocap_transform) { // MoCap data in TransformStamped msg - mocap_tf_sub = fp_nh.subscribe("mocap/tf", 10, &FakeGPSPlugin::mocap_tf_cb, this); - } - else if (mocap_withcovariance) {// MoCap data in PoseWithCovarianceStamped msg - mocap_pose_cov_sub = fp_nh.subscribe("mocap/pose_cov", 10, &FakeGPSPlugin::mocap_pose_cov_cb, this); - } - else { // MoCap data in PoseStamped msg - mocap_pose_sub = fp_nh.subscribe("mocap/pose", 10, &FakeGPSPlugin::mocap_pose_cb, this); - } - } - else if (use_vision) { // Vision data in PoseStamped msg - vision_pose_sub = fp_nh.subscribe("vision", 10, &FakeGPSPlugin::vision_cb, this); - } - else if (tf_listen) { // Pose aquired from TF Listener - ROS_INFO_STREAM_NAMED("fake_gps", "Listen to transform " << tf_frame_id - << " -> " << tf_child_frame_id); - tf2_start("FakeGPSVisionTF", &FakeGPSPlugin::transform_cb); - } - else { - ROS_ERROR_NAMED("fake_gps", "No pose source!"); - } - } - - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit FakeGPSPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "fake_gps"), + // WGS-84 ellipsoid (a - equatorial radius, f - flattening of ellipsoid) + earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()), + use_mocap(true), + use_vision(false), + use_hil_gps(true), + mocap_transform(true), + mocap_withcovariance(false), + tf_listen(false), + eph(2.0), + epv(2.0), + horiz_accuracy(0.0f), + vert_accuracy(0.0f), + speed_accuracy(0.0f), + gps_id(0), + satellites_visible(5), + fix_type(GPS_FIX_TYPE::NO_GPS), + tf_rate(10.0), + map_origin(0.0, 0.0, 0.0) + { + enable_node_watch_parameters(); + + last_pos_time = rclcpp::Time(0.0); + + // general params + node_declare_and_watch_parameter( + "gps_id", 0, [&](const rclcpp::Parameter & p) { + gps_id = p.as_int(); + }); + node_declare_and_watch_parameter( + "fix_type", utils::enum_value(GPS_FIX_TYPE::NO_GPS), [&](const rclcpp::Parameter & p) { + fix_type = static_cast( p.as_int()); + }); + node_declare_and_watch_parameter( + "gps_rate", 5.0, [&](const rclcpp::Parameter & p) { + rclcpp::Rate rate(p.as_double()); + + gps_rate_period = rate.period(); + }); + node_declare_and_watch_parameter( + "eph", 2.0, [&](const rclcpp::Parameter & p) { + eph = p.as_double(); + }); + node_declare_and_watch_parameter( + "epv", 2.0, [&](const rclcpp::Parameter & p) { + epv = p.as_double(); + }); + node_declare_and_watch_parameter( + "horiz_accuracy", 0.0, [&](const rclcpp::Parameter & p) { + horiz_accuracy = p.as_double(); + }); + node_declare_and_watch_parameter( + "vert_accuracy", 0.0, [&](const rclcpp::Parameter & p) { + vert_accuracy = p.as_double(); + }); + node_declare_and_watch_parameter( + "speed_accuracy", 0.0, [&](const rclcpp::Parameter & p) { + speed_accuracy = p.as_double(); + }); + node_declare_and_watch_parameter( + "satellites_visible", 5, [&](const rclcpp::Parameter & p) { + satellites_visible = p.as_int(); + }); + + // default origin/starting point: Zürich geodetic coordinates + node_declare_and_watch_parameter( + "geo_origin.lat", 47.3667, [&](const rclcpp::Parameter & p) { + map_origin.x() = p.as_double(); + }); + node_declare_and_watch_parameter( + "geo_origin.lon", 8.5500, [&](const rclcpp::Parameter & p) { + map_origin.y() = p.as_double(); + }); + node_declare_and_watch_parameter( + "geo_origin.alt", 408.0, [&](const rclcpp::Parameter & p) { + map_origin.z() = p.as_double(); + }); + + try { + /** + * @brief Conversion of the origin from geodetic coordinates (LLA) + * to ECEF (Earth-Centered, Earth-Fixed) + */ + earth.Forward( + map_origin.x(), map_origin.y(), map_origin.z(), + ecef_origin.x(), ecef_origin.y(), ecef_origin.z()); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(get_logger(), "FGPS: Caught exception: " << e.what()); + } + + // source set params + node_declare_and_watch_parameter( + // listen to MoCap source + "use_mocap", true, [&](const rclcpp::Parameter & p) { + use_mocap = p.as_bool(); + }); + node_declare_and_watch_parameter( + // listen to MoCap source (TransformStamped if true; PoseStamped if false) + "mocap_transform", true, [&](const rclcpp::Parameter & p) { + mocap_transform = p.as_bool(); + }); + node_declare_and_watch_parameter( + // ~mocap/pose uses PoseWithCovarianceStamped Message + "mocap_withcovariance", false, [&](const rclcpp::Parameter & p) { + mocap_withcovariance = p.as_bool(); + }); + + node_declare_and_watch_parameter( + // listen to Vision source + "use_vision", false, [&](const rclcpp::Parameter & p) { + use_vision = p.as_bool(); + }); + node_declare_and_watch_parameter( + "use_hil_gps", false, [&](const rclcpp::Parameter & p) { + // send HIL_GPS MAVLink messages if true, + // send GPS_INPUT mavlink messages if false + use_hil_gps = p.as_bool(); + }); + + // tf params + node_declare_and_watch_parameter( + "tf.frame_id", "map", [&](const rclcpp::Parameter & p) { + tf_frame_id = p.as_string(); + }); + node_declare_and_watch_parameter( + "tf.child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { + tf_child_frame_id = p.as_string(); + }); + node_declare_and_watch_parameter( + "tf.rate_limit", 10.0, [&](const rclcpp::Parameter & p) { + tf_rate = p.as_double(); + }); + node_declare_and_watch_parameter( + "tf.listen", false, [&](const rclcpp::Parameter & p) { + tf_listen = p.as_bool(); + }); + + + if (use_mocap) { + if (mocap_transform) { // MoCap data in TransformStamped msg + mocap_tf_sub = + node->create_subscription( + "~/mocap/tf", 10, + std::bind(&FakeGPSPlugin::mocap_tf_cb, this, _1)); + } else if (mocap_withcovariance) { // MoCap data in PoseWithCovarianceStamped msg + mocap_pose_cov_sub = + node->create_subscription( + "~/mocap/pose_cov", 10, std::bind(&FakeGPSPlugin::mocap_pose_cov_cb, this, _1)); + } else { // MoCap data in PoseStamped msg + mocap_pose_sub = + node->create_subscription( + "~/mocap/pose", 10, + std::bind(&FakeGPSPlugin::mocap_pose_cb, this, _1)); + } + } else if (use_vision) { // Vision data in PoseStamped msg + vision_pose_sub = + node->create_subscription( + "~/vision", 10, + std::bind(&FakeGPSPlugin::vision_cb, this, _1)); + } else if (tf_listen) { // Pose acquired from TF Listener + RCLCPP_INFO_STREAM( + get_logger(), "Listen to transform " << tf_frame_id << + " -> " << tf_child_frame_id); + tf2_start("FakeGPSVisionTF", &FakeGPSPlugin::transform_cb); + } else { + RCLCPP_ERROR(get_logger(), "No pose source!"); + } + } + + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - friend class TF2ListenerMixin; - ros::NodeHandle fp_nh; - - ros::Rate gps_rate; - ros::Time last_pos_time; - - // Constructor for a ellipsoid - GeographicLib::Geocentric earth; - - ros::Subscriber mocap_tf_sub; - ros::Subscriber mocap_pose_cov_sub; - ros::Subscriber mocap_pose_sub; - ros::Subscriber vision_pose_sub; - - bool use_mocap; //!< set use of mocap data (PoseStamped msg) - bool use_vision; //!< set use of vision data - bool use_hil_gps; //!< set use of use_hil_gps MAVLink messages - bool mocap_transform; //!< set use of mocap data (TransformStamped msg) - bool mocap_withcovariance; //!< ~mocap/pose uses PoseWithCovarianceStamped Message - bool tf_listen; //!< set use of TF Listener data - - double eph, epv; - float horiz_accuracy; - float vert_accuracy; - float speed_accuracy; - int gps_id; - int satellites_visible; - GPS_FIX_TYPE fix_type; - - double tf_rate; - std::string tf_frame_id; - std::string tf_child_frame_id; - ros::Time last_transform_stamp; - - Eigen::Vector3d map_origin; //!< geodetic origin [lla] - Eigen::Vector3d ecef_origin; //!< geocentric origin [m] - Eigen::Vector3d old_ecef; //!< previous geocentric position [m] - double old_stamp; //!< previous stamp [s] - - /* -*- mid-level helpers and low-level send -*- */ - - /** - * @brief Send fake GPS coordinates through HIL_GPS or GPS_INPUT Mavlink msg - */ - void send_fake_gps(const ros::Time &stamp, const Eigen::Vector3d &ecef_offset) { - // Throttle incoming messages to 5hz - if ((ros::Time::now() - last_pos_time) < ros::Duration(gps_rate)) { - return; - } - last_pos_time = ros::Time::now(); - - Eigen::Vector3d geodetic; - Eigen::Vector3d current_ecef(ecef_origin.x() + ecef_offset.x(), - ecef_origin.y() + ecef_offset.y(), - ecef_origin.z() + ecef_offset.z()); - - try { - earth.Reverse(current_ecef.x(), current_ecef.y(), current_ecef.z(), - geodetic.x(), geodetic.y(), geodetic.z()); - } - catch (const std::exception& e) { - ROS_INFO_STREAM("FGPS: Caught exception: " << e.what() << std::endl); - } - - Eigen::Vector3d vel = (old_ecef - current_ecef) / (stamp.toSec() - old_stamp); // [m/s] - - // store old values - old_stamp = stamp.toSec(); - old_ecef = current_ecef; - - if (use_hil_gps) { - /** - * @note: HIL_GPS MAVLink message - * is supported by both Ardupilot and PX4 Firmware. - * But on PX4 Firmware are only acceped out of HIL mode - * if use_hil_gps flag is set (param MAV_USEHILGPS = 1). - */ - mavlink::common::msg::HIL_GPS hil_gps {}; - - vel *= 1e2; // [cm/s] - - // compute course over ground - double cog; - if (vel.x() == 0 && vel.y() == 0) { - cog = 0; - } - else if (vel.x() >= 0 && vel.y() < 0) { - cog = M_PI * 5 / 2 - atan2(vel.x(), vel.y()); - } - else { - cog = M_PI / 2 - atan2(vel.x(), vel.y()); - } - - // Fill in and send message - hil_gps.time_usec = stamp.toNSec() / 1000; // [useconds] - hil_gps.lat = geodetic.x() * 1e7; // [degrees * 1e7] - hil_gps.lon = geodetic.y() * 1e7; // [degrees * 1e7] - hil_gps.alt = (geodetic.z() + GeographicLib::Geoid::ELLIPSOIDTOGEOID * - (*m_uas->egm96_5)(geodetic.x(), geodetic.y())) * 1e3; // [meters * 1e3] - hil_gps.vel = vel.block<2, 1>(0, 0).norm(); // [cm/s] - hil_gps.vn = vel.x(); // [cm/s] - hil_gps.ve = vel.y(); // [cm/s] - hil_gps.vd = vel.z(); // [cm/s] - hil_gps.cog = cog * 1e2; // [degrees * 1e2] - hil_gps.eph = eph * 1e2; // [cm] - hil_gps.epv = epv * 1e2; // [cm] - hil_gps.fix_type = utils::enum_value(fix_type);; - hil_gps.satellites_visible = satellites_visible; - - UAS_FCU(m_uas)->send_message_ignore_drop(hil_gps); - } - else { - /** - * @note: GPS_INPUT MAVLink message - * is currently only supported by Ardupilot firmware - */ - mavlink::common::msg::GPS_INPUT gps_input {}; - - // Fill in and send message - gps_input.time_usec = stamp.toNSec() / 1000; // [useconds] - gps_input.gps_id = gps_id; // - gps_input.ignore_flags = 0; - if (speed_accuracy == 0.0f) - gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_SPEED_ACCURACY); - if (eph == 0.0f) - gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_HDOP); - if (epv == 0.0f) - gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VDOP); - if (fabs(vel.x()) <= 0.01f && fabs(vel.y()) <= 0.01f) - gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VEL_HORIZ); - if (fabs(vel.z()) <= 0.01f) - gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VEL_VERT); - int64_t tdiff = (gps_input.time_usec / 1000) - UNIX_OFFSET_MSEC; - gps_input.time_week = tdiff / MSEC_PER_WEEK; - gps_input.time_week_ms = tdiff - (gps_input.time_week * MSEC_PER_WEEK); - gps_input.speed_accuracy = speed_accuracy; // [m/s] TODO how can this be dynamicaly calculated ??? - gps_input.horiz_accuracy = horiz_accuracy; // [m] will either use the static parameter value, or the dynamic covariance from function mocap_pose_cov_cb() bellow - gps_input.vert_accuracy = vert_accuracy;// [m] will either use the static parameter value, or the dynamic covariance from function mocap_pose_cov_cb() bellow - gps_input.lat = geodetic.x() * 1e7; // [degrees * 1e7] - gps_input.lon = geodetic.y() * 1e7; // [degrees * 1e7] - gps_input.alt = (geodetic.z() + GeographicLib::Geoid::ELLIPSOIDTOGEOID * - (*m_uas->egm96_5)(geodetic.x(), geodetic.y())); // [meters] - gps_input.vn = vel.x(); // [m/s] - gps_input.ve = vel.y(); // [m/s] - gps_input.vd = vel.z(); // [m/s] - gps_input.hdop = eph; // [m] - gps_input.vdop = epv; // [m] - gps_input.fix_type = utils::enum_value(fix_type);; - gps_input.satellites_visible = satellites_visible; - - UAS_FCU(m_uas)->send_message_ignore_drop(gps_input); - } - } - - /* -*- callbacks -*- */ - void mocap_tf_cb(const geometry_msgs::TransformStamped::ConstPtr &trans) - { - Eigen::Affine3d pos_enu; - tf::transformMsgToEigen(trans->transform, pos_enu); - - send_fake_gps(trans->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); - } - - void mocap_pose_cov_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &req) - { - Eigen::Affine3d pos_enu; - tf::poseMsgToEigen(req->pose.pose, pos_enu); - horiz_accuracy = (req->pose.covariance[0] + req->pose.covariance[7]) / 2.0f; - vert_accuracy = req->pose.covariance[14]; - - send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); - } - - void mocap_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req) - { - Eigen::Affine3d pos_enu; - tf::poseMsgToEigen(req->pose, pos_enu); - - send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); - } - - void vision_cb(const geometry_msgs::PoseStamped::ConstPtr &req) - { - Eigen::Affine3d pos_enu; - tf::poseMsgToEigen(req->pose, pos_enu); - - send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); - } - - void transform_cb(const geometry_msgs::TransformStamped &trans) - { - Eigen::Affine3d pos_enu; - - tf::transformMsgToEigen(trans.transform, pos_enu); - - send_fake_gps(trans.header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); - } + friend class TF2ListenerMixin; + + std::chrono::nanoseconds gps_rate_period; + rclcpp::Time last_pos_time; + + // Constructor for a ellipsoid + GeographicLib::Geocentric earth; + + rclcpp::Subscription::SharedPtr mocap_tf_sub; + rclcpp::Subscription::SharedPtr mocap_pose_cov_sub; + rclcpp::Subscription::SharedPtr mocap_pose_sub; + rclcpp::Subscription::SharedPtr vision_pose_sub; + + bool use_mocap; //!< set use of mocap data (PoseStamped msg) + bool use_vision; //!< set use of vision data + bool use_hil_gps; //!< set use of use_hil_gps MAVLink messages + bool mocap_transform; //!< set use of mocap data (TransformStamped msg) + bool mocap_withcovariance; //!< ~mocap/pose uses PoseWithCovarianceStamped Message + bool tf_listen; //!< set use of TF Listener data + + double eph, epv; + float horiz_accuracy; + float vert_accuracy; + float speed_accuracy; + int gps_id; + int satellites_visible; + GPS_FIX_TYPE fix_type; + + double tf_rate; + std::string tf_frame_id; + std::string tf_child_frame_id; + rclcpp::Time last_transform_stamp; + + Eigen::Vector3d map_origin; //!< geodetic origin [lla] + Eigen::Vector3d ecef_origin; //!< geocentric origin [m] + Eigen::Vector3d old_ecef; //!< previous geocentric position [m] + double old_stamp; //!< previous stamp [s] + + /* -*- mid-level helpers and low-level send -*- */ + + /** + * @brief Send fake GPS coordinates through HIL_GPS or GPS_INPUT Mavlink msg + */ + void send_fake_gps(const rclcpp::Time & stamp, const Eigen::Vector3d & ecef_offset) + { + auto now_ = node->now(); + + // Throttle incoming messages + if ((now_ - last_pos_time).to_chrono() < gps_rate_period) { + return; + } + last_pos_time = now_; + + Eigen::Vector3d geodetic; + Eigen::Vector3d current_ecef(ecef_origin.x() + ecef_offset.x(), + ecef_origin.y() + ecef_offset.y(), + ecef_origin.z() + ecef_offset.z()); + + try { + earth.Reverse( + current_ecef.x(), current_ecef.y(), current_ecef.z(), + geodetic.x(), geodetic.y(), geodetic.z()); + } catch (const std::exception & e) { + RCLCPP_INFO_STREAM(get_logger(), "FGPS: Caught exception: " << e.what()); + } + + Eigen::Vector3d vel = (old_ecef - current_ecef) / (stamp.seconds() - old_stamp); // [m/s] + + // store old values + old_stamp = stamp.seconds(); + old_ecef = current_ecef; + + if (use_hil_gps) { + /** + * @note: HIL_GPS MAVLink message + * is supported by both Ardupilot and PX4 Firmware. + * But on PX4 Firmware are only acceped out of HIL mode + * if use_hil_gps flag is set (param MAV_USEHILGPS = 1). + */ + mavlink::common::msg::HIL_GPS hil_gps {}; + + vel *= 1e2; // [cm/s] + + // compute course over ground + double cog; + if (vel.x() == 0 && vel.y() == 0) { + cog = 0; + } else if (vel.x() >= 0 && vel.y() < 0) { + cog = M_PI * 5 / 2 - atan2(vel.x(), vel.y()); + } else { + cog = M_PI / 2 - atan2(vel.x(), vel.y()); + } + + // Fill in and send message + hil_gps.time_usec = get_time_usec(stamp); // [useconds] + hil_gps.lat = geodetic.x() * 1e7; // [degrees * 1e7] + hil_gps.lon = geodetic.y() * 1e7; // [degrees * 1e7] + hil_gps.alt = (geodetic.z() + GeographicLib::Geoid::ELLIPSOIDTOGEOID * + (*uas->data.egm96_5)(geodetic.x(), geodetic.y())) * 1e3; // [meters * 1e3] + hil_gps.vel = vel.block<2, 1>(0, 0).norm(); // [cm/s] + hil_gps.vn = vel.x(); // [cm/s] + hil_gps.ve = vel.y(); // [cm/s] + hil_gps.vd = vel.z(); // [cm/s] + hil_gps.cog = cog * 1e2; // [degrees * 1e2] + hil_gps.eph = eph * 1e2; // [cm] + hil_gps.epv = epv * 1e2; // [cm] + hil_gps.fix_type = utils::enum_value(fix_type); + hil_gps.satellites_visible = satellites_visible; + + uas->send_message(hil_gps); + } else { + /** + * @note: GPS_INPUT MAVLink message + * is currently only supported by Ardupilot firmware + */ + mavlink::common::msg::GPS_INPUT gps_input {}; + + // Fill in and send message + gps_input.time_usec = get_time_usec(stamp); // [useconds] + gps_input.gps_id = gps_id; // + gps_input.ignore_flags = 0; + if (speed_accuracy == 0.0f) { + gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_SPEED_ACCURACY); + } + if (eph == 0.0f) { + gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_HDOP); + } + if (epv == 0.0f) { + gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VDOP); + } + if (fabs(vel.x()) <= 0.01f && fabs(vel.y()) <= 0.01f) { + gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VEL_HORIZ); + } + if (fabs(vel.z()) <= 0.01f) { + gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VEL_VERT); + } + int64_t tdiff = (gps_input.time_usec / 1000) - UNIX_OFFSET_MSEC; + gps_input.time_week = tdiff / MSEC_PER_WEEK; + gps_input.time_week_ms = tdiff - (gps_input.time_week * MSEC_PER_WEEK); + gps_input.speed_accuracy = speed_accuracy; // [m/s] TODO how can this be dynamicaly calculated ??? // NOLINT + gps_input.horiz_accuracy = horiz_accuracy; // [m] will either use the static parameter value, or the dynamic covariance from function mocap_pose_cov_cb() bellow // NOLINT + gps_input.vert_accuracy = vert_accuracy; // [m] will either use the static parameter value, or the dynamic covariance from function mocap_pose_cov_cb() bellow // NOLINT + gps_input.lat = geodetic.x() * 1e7; // [degrees * 1e7] + gps_input.lon = geodetic.y() * 1e7; // [degrees * 1e7] + gps_input.alt = (geodetic.z() + GeographicLib::Geoid::ELLIPSOIDTOGEOID * + (*uas->data.egm96_5)(geodetic.x(), geodetic.y())); // [meters] + gps_input.vn = vel.x(); // [m/s] + gps_input.ve = vel.y(); // [m/s] + gps_input.vd = vel.z(); // [m/s] + gps_input.hdop = eph; // [m] + gps_input.vdop = epv; // [m] + gps_input.fix_type = utils::enum_value(fix_type); + gps_input.satellites_visible = satellites_visible; + + uas->send_message(gps_input); + } + } + + /* -*- callbacks -*- */ + void mocap_tf_cb(const geometry_msgs::msg::TransformStamped::SharedPtr trans) + { + Eigen::Affine3d pos_enu; tf2::fromMsg(trans->transform, pos_enu); + + send_fake_gps( + trans->header.stamp, + ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); + } + + void mocap_pose_cov_cb(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr req) + { + Eigen::Affine3d pos_enu; tf2::fromMsg(req->pose.pose, pos_enu); + horiz_accuracy = (req->pose.covariance[0] + req->pose.covariance[7]) / 2.0f; + vert_accuracy = req->pose.covariance[14]; + + send_fake_gps( + req->header.stamp, + ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); + } + + void mocap_pose_cb(const geometry_msgs::msg::PoseStamped::SharedPtr req) + { + Eigen::Affine3d pos_enu; tf2::fromMsg(req->pose, pos_enu); + + send_fake_gps( + req->header.stamp, + ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); + } + + void vision_cb(const geometry_msgs::msg::PoseStamped::SharedPtr req) + { + Eigen::Affine3d pos_enu; tf2::fromMsg(req->pose, pos_enu); + + send_fake_gps( + req->header.stamp, + ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); + } + + void transform_cb(const geometry_msgs::msg::TransformStamped & trans) + { + Eigen::Affine3d pos_enu; tf2::fromMsg(trans.transform, pos_enu); + + send_fake_gps( + trans.header.stamp, + ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin)); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::FakeGPSPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::FakeGPSPlugin) diff --git a/mavros_extras/src/plugins/gps_input.cpp b/mavros_extras/src/plugins/gps_input.cpp index d065b6cd9..06a971b97 100644 --- a/mavros_extras/src/plugins/gps_input.cpp +++ b/mavros_extras/src/plugins/gps_input.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2019 Amilcar Lucas. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief GPS_INPUT plugin * @file gps_input.cpp @@ -6,102 +13,116 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2019 Amilcar Lucas. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include "mavros_msgs/GPSINPUT.h" +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -namespace mavros { -namespace extra_plugins { +#include "mavros_msgs/msg/gpsinput.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT using mavlink::common::GPS_FIX_TYPE; using mavlink::common::GPS_INPUT_IGNORE_FLAGS; /** * @brief GPS_INPUT GPS plugin. + * @plugin gps_input * * Sends GPS_INPUT MAVLink messages */ -class GpsInputPlugin : public plugin::PluginBase { +class GpsInputPlugin : public plugin::Plugin +{ public: - GpsInputPlugin() : PluginBase(), - gps_input_nh("~gps_input"), - gps_rate(5.0) - { } - - void initialize(UAS &uas_) - { - PluginBase::initialize(uas_); + explicit GpsInputPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "gps_input") + { + enable_node_watch_parameters(); - double _gps_rate; + node_declare_and_watch_parameter( + "gps_rate", 5.0, [&](const rclcpp::Parameter & p) { + rclcpp::Rate rate(p.as_double()); - last_pos_time = ros::Time(0.0); - gps_input_nh.param("gps_rate", _gps_rate, 5.0); // GPS data rate of 5hz - gps_rate = _gps_rate; + rate_period = rate.period(); + }); - gps_input_sub = gps_input_nh.subscribe("gps_input", 1, &GpsInputPlugin::send_cb, this); - } + gps_input_sub = node->create_subscription( + "~/gps_input", 1, std::bind( + &GpsInputPlugin::send_cb, this, + _1)); + } - Subscriptions get_subscriptions() - { - return { /* Rx disabled */ }; - } + Subscriptions get_subscriptions() + { + return { /* Rx disabled */}; + } private: - ros::NodeHandle gps_input_nh; - ros::Subscriber gps_input_sub; - - ros::Rate gps_rate; - ros::Time last_pos_time; - - /* -*- callbacks -*- */ - - /** - * @brief Send GPS coordinates through GPS_INPUT Mavlink message - */ - void send_cb(const mavros_msgs::GPSINPUT::ConstPtr ros_msg) { - // Throttle incoming messages to 5hz - if ((ros::Time::now() - last_pos_time) < ros::Duration(gps_rate)) { - return; - } - last_pos_time = ros::Time::now(); - - /** - * @note: GPS_INPUT MAVLink message - * is currently only supported by Ardupilot firmware - */ - mavlink::common::msg::GPS_INPUT gps_input {}; - - // Fill in and send message - gps_input.time_usec = ros_msg->header.stamp.toNSec() / 1000; - gps_input.gps_id = ros_msg->gps_id; - gps_input.ignore_flags = ros_msg->ignore_flags; - gps_input.time_week_ms = ros_msg->time_week_ms; - gps_input.time_week = ros_msg->time_week; - gps_input.speed_accuracy = ros_msg->speed_accuracy; - gps_input.horiz_accuracy = ros_msg->horiz_accuracy; - gps_input.vert_accuracy = ros_msg->vert_accuracy; - gps_input.lat = ros_msg->lat; - gps_input.lon = ros_msg->lon; - gps_input.alt = ros_msg->alt; - gps_input.vn = ros_msg->vn; - gps_input.ve = ros_msg->ve; - gps_input.vd = ros_msg->vd; - gps_input.hdop = ros_msg->hdop; - gps_input.vdop = ros_msg->vdop; - gps_input.fix_type = ros_msg->fix_type; - gps_input.satellites_visible = ros_msg->satellites_visible; - - UAS_FCU(m_uas)->send_message_ignore_drop(gps_input); - } + rclcpp::Subscription::SharedPtr gps_input_sub; + + std::chrono::nanoseconds rate_period; + rclcpp::Time last_pos_time; + + /* -*- callbacks -*- */ + + /** + * @brief Send GPS coordinates through GPS_INPUT Mavlink message + */ + void send_cb(const mavros_msgs::msg::GPSINPUT::SharedPtr ros_msg) + { + auto now_ = node->now(); + + // Throttle incoming messages + if ((now_ - last_pos_time).to_chrono() < rate_period) { + return; + } + last_pos_time = now_; + + /** + * @note: GPS_INPUT MAVLink message + * is currently only supported by Ardupilot firmware + */ + mavlink::common::msg::GPS_INPUT gps_input {}; + + // [[[cog: + // import pymavlink.dialects.v20.common as common + // + // for field in common.MAVLink_gps_input_message.fieldnames: + // if field in ['time_usec']: + // continue + // cog.outl(f"gps_input.{field} = ros_msg->{field};") + // ]]] + gps_input.gps_id = ros_msg->gps_id; + gps_input.ignore_flags = ros_msg->ignore_flags; + gps_input.time_week_ms = ros_msg->time_week_ms; + gps_input.time_week = ros_msg->time_week; + gps_input.fix_type = ros_msg->fix_type; + gps_input.lat = ros_msg->lat; + gps_input.lon = ros_msg->lon; + gps_input.alt = ros_msg->alt; + gps_input.hdop = ros_msg->hdop; + gps_input.vdop = ros_msg->vdop; + gps_input.vn = ros_msg->vn; + gps_input.ve = ros_msg->ve; + gps_input.vd = ros_msg->vd; + gps_input.speed_accuracy = ros_msg->speed_accuracy; + gps_input.horiz_accuracy = ros_msg->horiz_accuracy; + gps_input.vert_accuracy = ros_msg->vert_accuracy; + gps_input.satellites_visible = ros_msg->satellites_visible; + gps_input.yaw = ros_msg->yaw; + // [[[end]]] (checksum: 303dffa9e430561ad0e254448d3f403a) + + gps_input.time_usec = get_time_usec(ros_msg->header.stamp); + + uas->send_message(gps_input); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::GpsInputPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::GpsInputPlugin) diff --git a/mavros_extras/src/plugins/gps_rtk.cpp b/mavros_extras/src/plugins/gps_rtk.cpp index cb088cc79..9b74df2b1 100644 --- a/mavros_extras/src/plugins/gps_rtk.cpp +++ b/mavros_extras/src/plugins/gps_rtk.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2018 Alexis Paques. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief GPS RTK plugin * @file gps_rtk.cpp @@ -6,130 +13,147 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2018 Alexis Paques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include -#include #include -namespace mavros { -namespace extra_plugins { +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/rtcm.hpp" +#include "mavros_msgs/msg/rtk_baseline.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT + /** * @brief GPS RTK plugin + * @plugin gps_rtk * * 1. Publish the RTCM messages from ROS to the FCU * 2. Publish RTK baseline data from the FCU to ROS */ -class GpsRtkPlugin : public plugin::PluginBase { +class GpsRtkPlugin : public plugin::Plugin +{ public: - GpsRtkPlugin() : PluginBase(), - gps_rtk_nh("~gps_rtk") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - gps_rtk_sub = gps_rtk_nh.subscribe("send_rtcm", 10, &GpsRtkPlugin::rtcm_cb, this); - rtk_baseline_pub_ = gps_rtk_nh.advertise("rtk_baseline", 1, true); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler( &GpsRtkPlugin::handle_baseline_msg ) - }; - } + explicit GpsRtkPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "gps_rtk") + { + gps_rtk_sub = + node->create_subscription( + "~/send_rtcm", 10, + std::bind(&GpsRtkPlugin::rtcm_cb, this, _1)); + + // TODO(vooon): set QoS for latched topic + rtk_baseline_pub = node->create_publisher("~/rtk_baseline", 1); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&GpsRtkPlugin::handle_baseline_msg) + }; + } private: - ros::NodeHandle gps_rtk_nh; - ros::Subscriber gps_rtk_sub; - - ros::Publisher rtk_baseline_pub_; - mavros_msgs::RTKBaseline rtk_baseline_; - - /* -*- callbacks -*- */ - /** - * @brief Handle mavros_msgs::RTCM message - * It converts the message to the MAVLink GPS_RTCM_DATA message for GPS injection. - * Message specification: https://mavlink.io/en/messages/common.html#GPS_RTCM_DATA - * @param msg Received ROS msg - */ - void rtcm_cb(const mavros_msgs::RTCM::ConstPtr &msg) - { - mavlink::common::msg::GPS_RTCM_DATA rtcm_data = {}; - const size_t max_frag_len = rtcm_data.data.size(); - - uint8_t seq_u5 = uint8_t(msg->header.seq & 0x1F) << 3; - - if (msg->data.size() > 4 * max_frag_len) { - ROS_FATAL("gps_rtk: RTCM message received is bigger than the maximal possible size."); - return; - } - - auto data_it = msg->data.begin(); - auto end_it = msg->data.end(); - - if (msg->data.size() <= max_frag_len) { - rtcm_data.len = msg->data.size(); - rtcm_data.flags = seq_u5; - std::copy(data_it, end_it, rtcm_data.data.begin()); - std::fill(rtcm_data.data.begin() + rtcm_data.len, rtcm_data.data.end(), 0); - UAS_FCU(m_uas)->send_message(rtcm_data); - } else { - for (uint8_t fragment_id = 0; fragment_id < 4 && data_it < end_it; fragment_id++) { - uint8_t len = std::min((size_t) std::distance(data_it, end_it), max_frag_len); - rtcm_data.flags = 1; // LSB set indicates message is fragmented - rtcm_data.flags |= fragment_id << 1; // Next 2 bits are fragment id - rtcm_data.flags |= seq_u5; // Next 5 bits are sequence id - rtcm_data.len = len; - - std::copy(data_it, data_it + len, rtcm_data.data.begin()); - std::fill(rtcm_data.data.begin() + len, rtcm_data.data.end(), 0); - UAS_FCU(m_uas)->send_message(rtcm_data); - std::advance(data_it, len); - } - } - } - - /* MAvlink msg handlers */ - /** - * @brief Publish GPS_RTK message (MAvlink Common) received from FCU. - * The message is already decoded by Mavlink, we only need to convert to ROS. - * Details and units: https://mavlink.io/en/messages/common.html#GPS_RTK - */ - - void handle_baseline_msg( const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RTK &rtk_bsln ) - { - /* Received a decoded packet containing mavlink's msg #127,#128 in Common. - Simply convert to ROS and publish. - */ - rtk_baseline_.time_last_baseline_ms = rtk_bsln.time_last_baseline_ms; - rtk_baseline_.rtk_receiver_id = rtk_bsln.rtk_receiver_id; - rtk_baseline_.wn = rtk_bsln.wn; // week num. - rtk_baseline_.tow = rtk_bsln.tow; // ms - rtk_baseline_.rtk_health = rtk_bsln.rtk_health; - rtk_baseline_.rtk_rate = rtk_bsln.rtk_rate; - rtk_baseline_.nsats = rtk_bsln.nsats; - rtk_baseline_.baseline_coords_type = rtk_bsln.baseline_coords_type; // 0: ECEF, 1: NED - rtk_baseline_.baseline_a_mm = rtk_bsln.baseline_a_mm; - rtk_baseline_.baseline_b_mm = rtk_bsln.baseline_b_mm; - rtk_baseline_.baseline_c_mm = rtk_bsln.baseline_c_mm; - rtk_baseline_.accuracy = rtk_bsln.accuracy; - rtk_baseline_.iar_num_hypotheses = rtk_bsln.iar_num_hypotheses; - - rtk_baseline_.header.stamp = ros::Time::now(); - rtk_baseline_pub_.publish( rtk_baseline_ ); - } + rclcpp::Subscription::SharedPtr gps_rtk_sub; + rclcpp::Publisher::SharedPtr rtk_baseline_pub; + + mavros_msgs::msg::RTKBaseline rtk_baseline_; + std::atomic_uint rtcm_seq; + + /* -*- callbacks -*- */ + + /** + * @brief Handle mavros_msgs::RTCM message + * It converts the message to the MAVLink GPS_RTCM_DATA message for GPS injection. + * Message specification: https://mavlink.io/en/messages/common.html#GPS_RTCM_DATA + * @param msg Received ROS msg + */ + void rtcm_cb(const mavros_msgs::msg::RTCM::SharedPtr msg) + { + mavlink::common::msg::GPS_RTCM_DATA rtcm_data = {}; + const size_t max_frag_len = rtcm_data.data.size(); + + uint8_t seq_u5 = uint8_t(rtcm_seq.fetch_add(1) & 0x1F) << 3; + + if (msg->data.size() > 4 * max_frag_len) { + RCLCPP_ERROR( + get_logger(), + "gps_rtk: RTCM message received is bigger than the maximal possible size."); + return; + } + + auto data_it = msg->data.begin(); + auto end_it = msg->data.end(); + + if (msg->data.size() <= max_frag_len) { + rtcm_data.len = msg->data.size(); + rtcm_data.flags = seq_u5; + std::copy(data_it, end_it, rtcm_data.data.begin()); + std::fill(rtcm_data.data.begin() + rtcm_data.len, rtcm_data.data.end(), 0); + uas->send_message(rtcm_data); + } else { + for (uint8_t fragment_id = 0; fragment_id < 4 && data_it < end_it; fragment_id++) { + uint8_t len = std::min(static_cast(std::distance(data_it, end_it)), max_frag_len); + rtcm_data.flags = 1; // LSB set indicates message is fragmented + rtcm_data.flags |= fragment_id << 1; // Next 2 bits are fragment id + rtcm_data.flags |= seq_u5; // Next 5 bits are sequence id + rtcm_data.len = len; + + std::copy(data_it, data_it + len, rtcm_data.data.begin()); + std::fill(rtcm_data.data.begin() + len, rtcm_data.data.end(), 0); + uas->send_message(rtcm_data); + std::advance(data_it, len); + } + } + } + + /* MAvlink msg handlers */ + /** + * @brief Publish GPS_RTK message (MAvlink Common) received from FCU. + * The message is already decoded by Mavlink, we only need to convert to ROS. + * Details and units: https://mavlink.io/en/messages/common.html#GPS_RTK + */ + + void handle_baseline_msg( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::GPS_RTK & rtk_bsln, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // [[[cog: + // import pymavlink.dialects.v20.common as common + // + // for field in common.MAVLink_gps_rtk_message.fieldnames: + // if field in ['time_usec']: + // continue + // cog.outl(f"rtk_baseline_.{field} = rtk_bsln.{field};") + // ]]] + rtk_baseline_.time_last_baseline_ms = rtk_bsln.time_last_baseline_ms; + rtk_baseline_.rtk_receiver_id = rtk_bsln.rtk_receiver_id; + rtk_baseline_.wn = rtk_bsln.wn; + rtk_baseline_.tow = rtk_bsln.tow; + rtk_baseline_.rtk_health = rtk_bsln.rtk_health; + rtk_baseline_.rtk_rate = rtk_bsln.rtk_rate; + rtk_baseline_.nsats = rtk_bsln.nsats; + rtk_baseline_.baseline_coords_type = rtk_bsln.baseline_coords_type; + rtk_baseline_.baseline_a_mm = rtk_bsln.baseline_a_mm; + rtk_baseline_.baseline_b_mm = rtk_bsln.baseline_b_mm; + rtk_baseline_.baseline_c_mm = rtk_bsln.baseline_c_mm; + rtk_baseline_.accuracy = rtk_bsln.accuracy; + rtk_baseline_.iar_num_hypotheses = rtk_bsln.iar_num_hypotheses; + // [[[end]]] (checksum: c123d29c2e0bce3becce956a29ed6152) + rtk_baseline_.header = uas->synchronized_header("", rtk_bsln.time_last_baseline_ms * 1000); + + rtk_baseline_pub->publish(rtk_baseline_); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::GpsRtkPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::GpsRtkPlugin) diff --git a/mavros_extras/src/plugins/gps_status.cpp b/mavros_extras/src/plugins/gps_status.cpp index c06eb4248..659d0a27e 100644 --- a/mavros_extras/src/plugins/gps_status.cpp +++ b/mavros_extras/src/plugins/gps_status.cpp @@ -14,172 +14,201 @@ * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ -#include -#include -#include +#include -namespace mavros { -namespace extra_plugins { +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/gpsraw.hpp" +#include "mavros_msgs/msg/gpsrtk.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT using mavlink::common::RTK_BASELINE_COORDINATE_SYSTEM; /** * @brief Mavlink GPS status plugin. + * @plugin gps_status * * This plugin publishes GPS sensor data from a Mavlink compatible FCU to ROS. */ -class GpsStatusPlugin : public plugin::PluginBase { +class GpsStatusPlugin : public plugin::Plugin +{ public: - GpsStatusPlugin() : PluginBase(), - gpsstatus_nh("~gpsstatus") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - gps1_raw_pub = gpsstatus_nh.advertise("gps1/raw", 10); - gps2_raw_pub = gpsstatus_nh.advertise("gps2/raw", 10); - gps1_rtk_pub = gpsstatus_nh.advertise("gps1/rtk", 10); - gps2_rtk_pub = gpsstatus_nh.advertise("gps2/rtk", 10); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&GpsStatusPlugin::handle_gps_raw_int), - make_handler(&GpsStatusPlugin::handle_gps2_raw), - make_handler(&GpsStatusPlugin::handle_gps_rtk), - make_handler(&GpsStatusPlugin::handle_gps2_rtk) - }; - } + explicit GpsStatusPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "gpsstatus") + { + gps1_raw_pub = node->create_publisher("~/gps1/raw", 10); + gps2_raw_pub = node->create_publisher("~/gps2/raw", 10); + gps1_rtk_pub = node->create_publisher("~/gps1/rtk", 10); + gps2_rtk_pub = node->create_publisher("~/gps2/rtk", 10); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&GpsStatusPlugin::handle_gps_raw_int), + make_handler(&GpsStatusPlugin::handle_gps2_raw), + make_handler(&GpsStatusPlugin::handle_gps_rtk), + make_handler(&GpsStatusPlugin::handle_gps2_rtk) + }; + } private: - ros::NodeHandle gpsstatus_nh; - - ros::Publisher gps1_raw_pub; - ros::Publisher gps2_raw_pub; - ros::Publisher gps1_rtk_pub; - ros::Publisher gps2_rtk_pub; - - /* -*- callbacks -*- */ - /** - * @brief Publish mavlink GPS_RAW_INT message into the gps1/raw topic. - */ - void handle_gps_raw_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RAW_INT &mav_msg) { - auto ros_msg = boost::make_shared(); - ros_msg->header = m_uas->synchronized_header("/wgs84", mav_msg.time_usec); - ros_msg->fix_type = mav_msg.fix_type; - ros_msg->lat = mav_msg.lat; - ros_msg->lon = mav_msg.lon; - ros_msg->alt = mav_msg.alt; - ros_msg->eph = mav_msg.eph; - ros_msg->epv = mav_msg.epv; - ros_msg->vel = mav_msg.vel; - ros_msg->cog = mav_msg.cog; - ros_msg->satellites_visible = mav_msg.satellites_visible; - ros_msg->alt_ellipsoid = mav_msg.alt_ellipsoid; - ros_msg->h_acc = mav_msg.h_acc; - ros_msg->v_acc = mav_msg.v_acc; - ros_msg->vel_acc = mav_msg.vel_acc; - ros_msg->hdg_acc = mav_msg.hdg_acc; - ros_msg->dgps_numch = UINT8_MAX; // information not available in GPS_RAW_INT mavlink message - ros_msg->dgps_age = UINT32_MAX;// information not available in GPS_RAW_INT mavlink message - - gps1_raw_pub.publish(ros_msg); - } - - /** - * @brief Publish mavlink GPS2_RAW message into the gps2/raw topic. - */ - void handle_gps2_raw(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS2_RAW &mav_msg) { - auto ros_msg = boost::make_shared(); - ros_msg->header = m_uas->synchronized_header("/wgs84", mav_msg.time_usec); - ros_msg->fix_type = mav_msg.fix_type; - ros_msg->lat = mav_msg.lat; - ros_msg->lon = mav_msg.lon; - ros_msg->alt = mav_msg.alt; - ros_msg->eph = mav_msg.eph; - ros_msg->epv = mav_msg.epv; - ros_msg->vel = mav_msg.vel; - ros_msg->cog = mav_msg.cog; - ros_msg->satellites_visible = mav_msg.satellites_visible; - ros_msg->alt_ellipsoid = INT32_MAX; // information not available in GPS2_RAW mavlink message - ros_msg->h_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message - ros_msg->v_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message - ros_msg->vel_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message - ros_msg->hdg_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message - ros_msg->dgps_numch = mav_msg.dgps_numch; - ros_msg->dgps_age = mav_msg.dgps_age; - - gps2_raw_pub.publish(ros_msg); - } - - /** - * @brief Publish mavlink GPS_RTK message into the gps1/rtk topic. - */ - void handle_gps_rtk(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RTK &mav_msg) { - auto ros_msg = boost::make_shared(); - switch (static_cast(mav_msg.baseline_coords_type)) - { - case RTK_BASELINE_COORDINATE_SYSTEM::ECEF: - ros_msg->header.frame_id = "earth"; - break; - case RTK_BASELINE_COORDINATE_SYSTEM::NED: - ros_msg->header.frame_id = "map"; - break; - default: - ROS_ERROR_NAMED("gps_status", "GPS_RTK.baseline_coords_type MAVLink field has unknown \"%d\" value", mav_msg.baseline_coords_type); - } - ros_msg->header = m_uas->synchronized_header(ros_msg->header.frame_id, mav_msg.time_last_baseline_ms * 1000); - ros_msg->rtk_receiver_id = mav_msg.rtk_receiver_id; - ros_msg->wn = mav_msg.wn; - ros_msg->tow = mav_msg.tow; - ros_msg->rtk_health = mav_msg.rtk_health; - ros_msg->rtk_rate = mav_msg.rtk_rate; - ros_msg->nsats = mav_msg.nsats; - ros_msg->baseline_a = mav_msg.baseline_a_mm; - ros_msg->baseline_b = mav_msg.baseline_b_mm; - ros_msg->baseline_c = mav_msg.baseline_c_mm; - ros_msg->accuracy = mav_msg.accuracy; - ros_msg->iar_num_hypotheses = mav_msg.iar_num_hypotheses; - - gps1_rtk_pub.publish(ros_msg); - } - - /** - * @brief Publish mavlink GPS2_RTK message into the gps2/rtk topic. - */ - void handle_gps2_rtk(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS2_RTK &mav_msg) { - auto ros_msg = boost::make_shared(); - switch (static_cast(mav_msg.baseline_coords_type)) - { - case RTK_BASELINE_COORDINATE_SYSTEM::ECEF: - ros_msg->header.frame_id = "earth"; - break; - case RTK_BASELINE_COORDINATE_SYSTEM::NED: - ros_msg->header.frame_id = "map"; - break; - default: - ROS_ERROR_NAMED("gps_status", "GPS_RTK2.baseline_coords_type MAVLink field has unknown \"%d\" value", mav_msg.baseline_coords_type); - } - ros_msg->header = m_uas->synchronized_header(ros_msg->header.frame_id, mav_msg.time_last_baseline_ms * 1000); - ros_msg->rtk_receiver_id = mav_msg.rtk_receiver_id; - ros_msg->wn = mav_msg.wn; - ros_msg->tow = mav_msg.tow; - ros_msg->rtk_health = mav_msg.rtk_health; - ros_msg->rtk_rate = mav_msg.rtk_rate; - ros_msg->nsats = mav_msg.nsats; - ros_msg->baseline_a = mav_msg.baseline_a_mm; - ros_msg->baseline_b = mav_msg.baseline_b_mm; - ros_msg->baseline_c = mav_msg.baseline_c_mm; - ros_msg->accuracy = mav_msg.accuracy; - ros_msg->iar_num_hypotheses = mav_msg.iar_num_hypotheses; - - gps2_rtk_pub.publish(ros_msg); - } + rclcpp::Publisher::SharedPtr gps1_raw_pub; + rclcpp::Publisher::SharedPtr gps2_raw_pub; + rclcpp::Publisher::SharedPtr gps1_rtk_pub; + rclcpp::Publisher::SharedPtr gps2_rtk_pub; + + /* -*- callbacks -*- */ + /** + * @brief Publish mavlink GPS_RAW_INT message into the gps1/raw topic. + */ + void handle_gps_raw_int( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::GPS_RAW_INT & mav_msg, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto ros_msg = mavros_msgs::msg::GPSRAW(); + + // [[[cog: + // import pymavlink.dialects.v20.common as common + // + // def outl_raw_msg(msg): + // for field in msg.fieldnames: + // if field in ['time_usec']: + // continue + // cog.outl(f"ros_msg.{field} = mav_msg.{field};") + // + // outl_raw_msg(common.MAVLink_gps_raw_int_message) + // ]]] + ros_msg.fix_type = mav_msg.fix_type; + ros_msg.lat = mav_msg.lat; + ros_msg.lon = mav_msg.lon; + ros_msg.alt = mav_msg.alt; + ros_msg.eph = mav_msg.eph; + ros_msg.epv = mav_msg.epv; + ros_msg.vel = mav_msg.vel; + ros_msg.cog = mav_msg.cog; + ros_msg.satellites_visible = mav_msg.satellites_visible; + ros_msg.alt_ellipsoid = mav_msg.alt_ellipsoid; + ros_msg.h_acc = mav_msg.h_acc; + ros_msg.v_acc = mav_msg.v_acc; + ros_msg.vel_acc = mav_msg.vel_acc; + ros_msg.hdg_acc = mav_msg.hdg_acc; + ros_msg.yaw = mav_msg.yaw; + // [[[end]]] (checksum: 5803a4026c6f569e7cc00b66156640f9) + ros_msg.header = uas->synchronized_header("/wgs84", mav_msg.time_usec); + ros_msg.dgps_numch = UINT8_MAX; // information not available in GPS_RAW_INT mavlink message + ros_msg.dgps_age = UINT32_MAX; // information not available in GPS_RAW_INT mavlink message + + gps1_raw_pub->publish(ros_msg); + } + + /** + * @brief Publish mavlink GPS2_RAW message into the gps2/raw topic. + */ + void handle_gps2_raw( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::GPS2_RAW & mav_msg, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto ros_msg = mavros_msgs::msg::GPSRAW(); + + // [[[cog: + // outl_raw_msg(common.MAVLink_gps2_raw_message) + // ]]] + ros_msg.fix_type = mav_msg.fix_type; + ros_msg.lat = mav_msg.lat; + ros_msg.lon = mav_msg.lon; + ros_msg.alt = mav_msg.alt; + ros_msg.eph = mav_msg.eph; + ros_msg.epv = mav_msg.epv; + ros_msg.vel = mav_msg.vel; + ros_msg.cog = mav_msg.cog; + ros_msg.satellites_visible = mav_msg.satellites_visible; + ros_msg.dgps_numch = mav_msg.dgps_numch; + ros_msg.dgps_age = mav_msg.dgps_age; + ros_msg.yaw = mav_msg.yaw; + ros_msg.alt_ellipsoid = mav_msg.alt_ellipsoid; + ros_msg.h_acc = mav_msg.h_acc; + ros_msg.v_acc = mav_msg.v_acc; + ros_msg.vel_acc = mav_msg.vel_acc; + ros_msg.hdg_acc = mav_msg.hdg_acc; + // [[[end]]] (checksum: 1d71e875394bf6abb6c46e39801cdc19) + ros_msg.header = uas->synchronized_header("/wgs84", mav_msg.time_usec); + + gps2_raw_pub->publish(ros_msg); + } + + template + mavros_msgs::msg::GPSRTK convert_rtk(MMsg mav_msg) + { + auto ros_msg = mavros_msgs::msg::GPSRTK(); + + std::string frame_id = "unknown"; + switch (static_cast(mav_msg.baseline_coords_type)) { + case RTK_BASELINE_COORDINATE_SYSTEM::ECEF: + frame_id = "earth"; + break; + case RTK_BASELINE_COORDINATE_SYSTEM::NED: + frame_id = "map"; + break; + default: + RCLCPP_ERROR( + get_logger(), + "GPS_RTK.baseline_coords_type MAVLink field has unknown \"%d\" value", + mav_msg.baseline_coords_type); + } + + ros_msg.header = uas->synchronized_header( + frame_id, + mav_msg.time_last_baseline_ms * 1000); + + ros_msg.rtk_receiver_id = mav_msg.rtk_receiver_id; + ros_msg.wn = mav_msg.wn; + ros_msg.tow = mav_msg.tow; + ros_msg.rtk_health = mav_msg.rtk_health; + ros_msg.rtk_rate = mav_msg.rtk_rate; + ros_msg.nsats = mav_msg.nsats; + ros_msg.baseline_a = mav_msg.baseline_a_mm; + ros_msg.baseline_b = mav_msg.baseline_b_mm; + ros_msg.baseline_c = mav_msg.baseline_c_mm; + ros_msg.accuracy = mav_msg.accuracy; + ros_msg.iar_num_hypotheses = mav_msg.iar_num_hypotheses; + + return ros_msg; + } + + /** + * @brief Publish mavlink GPS_RTK message into the gps1/rtk topic. + */ + void handle_gps_rtk( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::GPS_RTK & mav_msg, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + gps1_rtk_pub->publish(convert_rtk(mav_msg)); + } + + /** + * @brief Publish mavlink GPS2_RTK message into the gps2/rtk topic. + */ + void handle_gps2_rtk( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::GPS2_RTK & mav_msg, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + gps2_rtk_pub->publish(convert_rtk(mav_msg)); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::GpsStatusPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::GpsStatusPlugin) diff --git a/mavros_extras/src/plugins/guided_target.cpp b/mavros_extras/src/plugins/guided_target.cpp index 1245d3384..dfda747fb 100644 --- a/mavros_extras/src/plugins/guided_target.cpp +++ b/mavros_extras/src/plugins/guided_target.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2022 Sanket Sharma, Randy Mackay. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Guided target plugin * @file guided_target.cpp @@ -6,194 +13,176 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2022 Sanket Sharma, Randy Mackay. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include -#include +#include -#include - -#include -#include +#include #include -#include -#include -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/position_target.hpp" +#include "mavros_msgs/msg/global_position_target.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geographic_msgs/msg/geo_point_stamped.hpp" +#include "geographic_msgs/msg/geo_pose_stamped.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace extra_plugins { - /** * @brief guided target plugin + * @plugin guided_target * * Send and receive setpoint positions from FCU controller. */ -class GuidedTargetPlugin : public plugin::PluginBase, - private plugin::SetPositionTargetLocalNEDMixin, - private plugin::SetPositionTargetGlobalIntMixin, - private plugin::TF2ListenerMixin { +class GuidedTargetPlugin : public plugin::Plugin +{ public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - GuidedTargetPlugin() : PluginBase(), - sp_nh("~guided_target"), - spg_nh("~"), - tf_listen(false), - tf_rate(50.0), - is_map_init(false) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - // tf params - sp_nh.param("tf/listen", tf_listen, false); - sp_nh.param("tf/frame_id", tf_frame_id, "map"); - sp_nh.param("tf/child_frame_id", tf_child_frame_id, "target_position"); - sp_nh.param("tf/rate_limit", tf_rate, 50.0); - - // Publish targets received from FCU - setpointg_pub = spg_nh.advertise("/move_base_simple/goal", 10); - - - // Subscriber for global origin (aka map origin). - gp_origin_sub = spg_nh.subscribe("global_position/gp_origin", 10, &GuidedTargetPlugin::gp_origin_cb, this); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&GuidedTargetPlugin::handle_position_target_global_int) - }; - } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit GuidedTargetPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "guided_target"), + is_map_init(false), + prev() + { + enable_node_watch_parameters(); + + // frame params: + node_declare_and_watch_parameter( + "frame_id", "map", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + + // Publish targets received from FCU + setpointg_pub = node->create_publisher( + "/move_base_simple/goal", 10); + + + // Subscriber for global origin (aka map origin). + gp_origin_sub = node->create_subscription( + "global_position/gp_origin", 10, std::bind(&GuidedTargetPlugin::gp_origin_cb, this, _1)); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&GuidedTargetPlugin::handle_position_target_global_int) + }; + } private: - - - ros::NodeHandle sp_nh; - ros::NodeHandle spg_nh; //!< to get local position and gps coord which are not under sp_h() - - ros::Subscriber gp_origin_sub; //!< global origin LLA - - ros::Publisher setpointg_pub; //!< global position target from FCU - - /* Stores current gps state. */ - //sensor_msgs::NavSatFix current_gps_msg; - Eigen::Vector3d current_gps; //!< geodetic coordinates LLA - Eigen::Vector3d current_local_pos; //!< Current local position in ENU - - Eigen::Vector3d map_origin {}; //!< oigin of map frame [lla] - Eigen::Vector3d ecef_origin {}; //!< geocentric origin [m] - - uint32_t old_gps_stamp = 0; //!< old time gps time stamp in [ms], to check if new gps msg is received - - std::string tf_frame_id; - std::string tf_child_frame_id; - - bool tf_listen; - double tf_rate; - bool is_map_init; - - double arr[2] = {0, 0}; - - /* -*- mid-level helpers -*- */ - - /** - * @brief Send setpoint to FCU position controller. - * - * @warning Send only XYZ, Yaw. ENU frame. - */ - - - /** - * global origin in LLA - */ - void gp_origin_cb(const geographic_msgs::GeoPointStamped::ConstPtr &msg) - { - ecef_origin = {msg->position.latitude, msg->position.longitude, msg->position.altitude}; - /** - * @brief Conversion from ECEF (Earth-Centered, Earth-Fixed) to geodetic coordinates (LLA) - */ - GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); - try { - earth.Reverse(ecef_origin.x(), ecef_origin.y(), ecef_origin.z(), - map_origin.x(), map_origin.y(), map_origin.z()); - } - catch (const std::exception& e) { - ROS_WARN_STREAM("setpoint: Caught exception: " << e.what() << std::endl); - return; - } - is_map_init = true; - } - - - - - /* -*- rx handler -*- */ - - /** - * @brief handle POSITION_TARGET_GLOBAL_INT mavlink msg - * handles and publishes position target received from FCU - */ - - void handle_position_target_global_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::POSITION_TARGET_GLOBAL_INT &position_target) - { - /* check if type_mask field ignores position*/ - if (position_target.type_mask & (mavros_msgs::GlobalPositionTarget::IGNORE_LATITUDE | mavros_msgs::GlobalPositionTarget::IGNORE_LONGITUDE)) { - ROS_WARN_NAMED("setpoint", "lat and/or lon ignored"); - return; - } - - /* check origin has been set */ - if (!is_map_init) { - ROS_WARN_NAMED("setpoint", "PositionTargetGlobal failed because no origin"); - } - - /* convert lat/lon target to ECEF */ - Eigen::Vector3d pos_target_ecef {}; //!< local ECEF coordinates on map frame [m] - GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); - try { - earth.Forward(position_target.lat_int / 1E7, position_target.lon_int / 1E7, position_target.alt / 1E3, - pos_target_ecef.x(), pos_target_ecef.y(), pos_target_ecef.z()); - } - catch (const std::exception& e) { - ROS_WARN_STREAM("setpoint: Caught exception: " << e.what() << std::endl); - return; - } - - /* create position target PoseStamped message */ - auto pose = boost::make_shared(); - pose->header = m_uas->synchronized_header("map", position_target.time_boot_ms); - pose->pose.orientation.w = 1; // unit quaternion with no rotation - - /* convert ECEF target to ENU */ - const Eigen::Vector3d local_ecef = pos_target_ecef - ecef_origin; - tf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef, map_origin), pose->pose.position); - pose->pose.position.z = 0; // force z-axis to zero - - /* publish target */ - - if (pose->pose.position.x != arr[0] || pose->pose.position.y != arr[1]) { - setpointg_pub.publish(pose); - } - - arr[0] = pose->pose.position.x; - arr[1] = pose->pose.position.y; - } - + //! global position target from FCU + rclcpp::Publisher::SharedPtr setpointg_pub; + //! global origin LLA + rclcpp::Subscription::SharedPtr gp_origin_sub; + + Eigen::Vector3d current_gps; //!< geodetic coordinates LLA + Eigen::Vector3d current_local_pos; //!< Current local position in ENU + + Eigen::Vector3d map_origin {}; //!< oigin of map frame [lla] + Eigen::Vector3d ecef_origin {}; //!< geocentric origin [m] + + //! old time gps time stamp in [ms], to check if new gps msg is received + uint32_t old_gps_stamp = 0; + + std::string frame_id; + bool is_map_init; + + Eigen::Vector2d prev; + + /* -*- mid-level helpers -*- */ + + /** + * global origin in LLA + */ + void gp_origin_cb(const geographic_msgs::msg::GeoPointStamped::SharedPtr msg) + { + ecef_origin = {msg->position.latitude, msg->position.longitude, msg->position.altitude}; + /** + * @brief Conversion from ECEF (Earth-Centered, Earth-Fixed) to geodetic coordinates (LLA) + */ + GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f()); + try { + earth.Reverse( + ecef_origin.x(), ecef_origin.y(), ecef_origin.z(), + map_origin.x(), map_origin.y(), map_origin.z()); + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM(get_logger(), "setpoint: Caught exception: " << e.what() << std::endl); + return; + } + + is_map_init = true; + } + + + /* -*- rx handler -*- */ + + /** + * @brief handle POSITION_TARGET_GLOBAL_INT mavlink msg + * handles and publishes position target received from FCU + */ + void handle_position_target_global_int( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::POSITION_TARGET_GLOBAL_INT & position_target, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + using GPT = mavros_msgs::msg::GlobalPositionTarget; + auto lg = get_logger(); + + /* check if type_mask field ignores position*/ + if ((position_target.type_mask & (GPT::IGNORE_LATITUDE | GPT::IGNORE_LONGITUDE)) > 0) { + RCLCPP_WARN(lg, "lat and/or lon ignored"); + return; + } + + /* check origin has been set */ + if (!is_map_init) { + RCLCPP_WARN(lg, "PositionTargetGlobal failed because no origin"); + } + + /* convert lat/lon target to ECEF */ + Eigen::Vector3d pos_target_ecef {}; //!< local ECEF coordinates on map frame [m] + GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f()); + try { + earth.Forward( + position_target.lat_int / 1E7, position_target.lon_int / 1E7, position_target.alt / 1E3, + pos_target_ecef.x(), pos_target_ecef.y(), pos_target_ecef.z()); + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM(lg, "Caught exception: " << e.what() << std::endl); + return; + } + + /* create position target PoseStamped message */ + auto pose = geometry_msgs::msg::PoseStamped(); + pose.header = uas->synchronized_header(frame_id, position_target.time_boot_ms); + pose.pose.orientation.w = 1.0; // unit quaternion with no rotation + + /* convert ECEF target to ENU */ + const Eigen::Vector3d local_ecef = pos_target_ecef - ecef_origin; + pose.pose.position = tf2::toMsg(ftf::transform_frame_ecef_enu(local_ecef, map_origin)); + pose.pose.position.z = 0.0; // force z-axis to zero + + /* publish target */ + if (pose.pose.position.x != prev.x() || pose.pose.position.y != prev.y()) { + setpointg_pub->publish(pose); + } + + prev.x() = pose.pose.position.x; + prev.y() = pose.pose.position.y; + } }; -} // namespace std_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::GuidedTargetPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::GuidedTargetPlugin) diff --git a/mavros_extras/src/plugins/hil.cpp b/mavros_extras/src/plugins/hil.cpp new file mode 100644 index 000000000..e3a22636f --- /dev/null +++ b/mavros_extras/src/plugins/hil.cpp @@ -0,0 +1,373 @@ +/* + * Copyright 2016,2017 Mohamed Abdelkader, Nuno Marques, Pavel Vechersky, Beat Küng. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief Hil plugin + * @file hil.cpp + * @author Mohamed Abdelkader + * @author Nuno Marques + * @author Pavel Vechersky + * @author Beat Küng + * + * @addtogroup plugin + * @{ + */ + +#include + +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/hil_controls.hpp" +#include "mavros_msgs/msg/hil_actuator_controls.hpp" +#include "mavros_msgs/msg/hil_state_quaternion.hpp" +#include "mavros_msgs/msg/hil_gps.hpp" +#include "mavros_msgs/msg/hil_sensor.hpp" +#include "mavros_msgs/msg/optical_flow_rad.hpp" +#include "mavros_msgs/msg/rc_in.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT + +//! Tesla to Gauss coeff +static constexpr double TESLA_TO_GAUSS = 1.0e4; +//! Pascal to millBar coeff +static constexpr double PASCAL_TO_MILLIBAR = 1.0e-2; + +/** + * @brief Hil plugin + * @plugin hil + */ +class HilPlugin : public plugin::Plugin +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit HilPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "hil") + { + hil_state_quaternion_sub = node->create_subscription( + "~/state", 10, std::bind(&HilPlugin::state_quat_cb, this, _1)); + hil_gps_sub = + node->create_subscription( + "~/gps", 10, + std::bind(&HilPlugin::gps_cb, this, _1)); + hil_sensor_sub = node->create_subscription( + "~/imu_ned", 10, std::bind( + &HilPlugin::sensor_cb, this, + _1)); + hil_flow_sub = node->create_subscription( + "~/optical_flow", 10, std::bind( + &HilPlugin::optical_flow_cb, this, + _1)); + hil_rcin_sub = + node->create_subscription( + "~/rc_inputs", 10, + std::bind(&HilPlugin::rcin_raw_cb, this, _1)); + + hil_controls_pub = node->create_publisher("~/controls", 10); + hil_actuator_controls_pub = node->create_publisher( + "~/actuator_controls", 10); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&HilPlugin::handle_hil_controls), + make_handler(&HilPlugin::handle_hil_actuator_controls), + }; + } + +private: + rclcpp::Publisher::SharedPtr hil_controls_pub; + rclcpp::Publisher::SharedPtr hil_actuator_controls_pub; + + rclcpp::Subscription::SharedPtr hil_state_quaternion_sub; + rclcpp::Subscription::SharedPtr hil_gps_sub; + rclcpp::Subscription::SharedPtr hil_sensor_sub; + rclcpp::Subscription::SharedPtr hil_flow_sub; + rclcpp::Subscription::SharedPtr hil_rcin_sub; + + Eigen::Quaterniond enu_orientation; + + /* -*- rx handlers -*- */ + + void handle_hil_controls( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::HIL_CONTROLS & hil_controls, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto hil_controls_msg = mavros_msgs::msg::HilControls(); + + hil_controls_msg.header.stamp = uas->synchronise_stamp(hil_controls.time_usec); + // [[[cog: + // for f in ( + // 'roll_ailerons', 'pitch_elevator', 'yaw_rudder', 'throttle', + // 'aux1', 'aux2', 'aux3', 'aux4', 'mode', 'nav_mode'): + // cog.outl(f"hil_controls_msg.{f} = hil_controls.{f};") + // ]]] + hil_controls_msg.roll_ailerons = hil_controls.roll_ailerons; + hil_controls_msg.pitch_elevator = hil_controls.pitch_elevator; + hil_controls_msg.yaw_rudder = hil_controls.yaw_rudder; + hil_controls_msg.throttle = hil_controls.throttle; + hil_controls_msg.aux1 = hil_controls.aux1; + hil_controls_msg.aux2 = hil_controls.aux2; + hil_controls_msg.aux3 = hil_controls.aux3; + hil_controls_msg.aux4 = hil_controls.aux4; + hil_controls_msg.mode = hil_controls.mode; + hil_controls_msg.nav_mode = hil_controls.nav_mode; + // [[[end]]] (checksum: c213771db088869eb1a7776f03eb1c23) + + hil_controls_pub->publish(hil_controls_msg); + } + + void handle_hil_actuator_controls( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::HIL_ACTUATOR_CONTROLS & hil_actuator_controls, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto hil_actuator_controls_msg = mavros_msgs::msg::HilActuatorControls(); + + hil_actuator_controls_msg.header.stamp = uas->synchronise_stamp( + hil_actuator_controls.time_usec); + const auto & arr = hil_actuator_controls.controls; + std::copy(arr.cbegin(), arr.cend(), hil_actuator_controls_msg.controls.begin()); + hil_actuator_controls_msg.mode = hil_actuator_controls.mode; + hil_actuator_controls_msg.flags = hil_actuator_controls.flags; + + hil_actuator_controls_pub->publish(hil_actuator_controls_msg); + } + + /* -*- callbacks / low level send -*- */ + + /** + * @brief Send hil_state_quaternion to FCU. + * Message specification: @p https://mavlink.io/en/messages/common.html#HIL_STATE_QUATERNION + */ + void state_quat_cb(const mavros_msgs::msg::HilStateQuaternion::SharedPtr req) + { + mavlink::common::msg::HIL_STATE_QUATERNION state_quat = {}; + + state_quat.time_usec = get_time_usec(req->header.stamp); + auto q = ftf::transform_orientation_baselink_aircraft( + ftf::transform_orientation_enu_ned( + ftf::to_eigen(req->orientation))); + ftf::quaternion_to_mavlink(q, state_quat.attitude_quaternion); + state_quat.lat = req->geo.latitude * 1E7; + state_quat.lon = req->geo.longitude * 1E7; + // @warning geographic_msgs/GeoPoint.msg uses WGS 84 reference ellipsoid + // @TODO: Convert altitude to AMSL to be received by the FCU + // related to issue #529 + state_quat.alt = req->geo.altitude * 1E3; + state_quat.ind_airspeed = req->ind_airspeed * 1E2; + state_quat.true_airspeed = req->true_airspeed * 1E2; + // WRT world frame + auto ang_vel = ftf::transform_frame_enu_ned( + ftf::transform_frame_baselink_aircraft( + ftf::to_eigen(req->angular_velocity))); + auto lin_vel = ftf::transform_frame_enu_ned( + ftf::to_eigen(req->linear_velocity)) * 1E2; + // linear acceleration - WRT world frame + auto lin_acc = ftf::transform_frame_baselink_aircraft( + ftf::to_eigen(req->linear_acceleration)); + + // [[[cog: + // for a, b in zip(('rollspeed', 'pitchspeed', 'yawspeed'), "xyz"): + // cog.outl(f"state_quat.{a} = ang_vel.{b}();") + // for f in "xyz": + // cog.outl(f"state_quat.v{f} = lin_vel.{f}();") + // for f in "xyz": + // cog.outl(f"state_quat.{f}acc = lin_acc.{f}();") + // ]]] + state_quat.rollspeed = ang_vel.x(); + state_quat.pitchspeed = ang_vel.y(); + state_quat.yawspeed = ang_vel.z(); + state_quat.vx = lin_vel.x(); + state_quat.vy = lin_vel.y(); + state_quat.vz = lin_vel.z(); + state_quat.xacc = lin_acc.x(); + state_quat.yacc = lin_acc.y(); + state_quat.zacc = lin_acc.z(); + // [[[end]]] (checksum: 59683585adc102a8c5ec530d99f8664d) + + uas->send_message(state_quat); + } + + /** + * @brief Send hil_gps to FCU. + * Message specification: @p https://mavlink.io/en/messages/common.html#HIL_GPS + */ + void gps_cb(const mavros_msgs::msg::HilGPS::SharedPtr req) + { + mavlink::common::msg::HIL_GPS gps = {}; + + gps.time_usec = get_time_usec(req->header.stamp); + gps.fix_type = req->fix_type; + gps.lat = req->geo.latitude * 1E7; + gps.lon = req->geo.longitude * 1E7; + // @warning geographic_msgs/GeoPoint.msg uses WGS 84 reference ellipsoid + // @TODO: Convert altitude to AMSL to be received by the FCU + // related to issue #529 + gps.alt = req->geo.altitude * 1E3; + // [[[cog: + // for f in ( + // 'eph', 'epv', 'vel', 'vn', 've', 'vd', 'cog'): + // cog.outl(f"gps.{f} = req->{f} * 1E2;") + // ]]] + gps.eph = req->eph * 1E2; + gps.epv = req->epv * 1E2; + gps.vel = req->vel * 1E2; + gps.vn = req->vn * 1E2; + gps.ve = req->ve * 1E2; + gps.vd = req->vd * 1E2; + gps.cog = req->cog * 1E2; + // [[[end]]] (checksum: b71b4e33be4574667105126a43507e82) + gps.satellites_visible = req->satellites_visible; + + uas->send_message(gps); + } + + /** + * @brief Send hil_sensor to FCU. + * Message specification: @p https://mavlink.io/en/messages/common.html#HIL_SENSOR + */ + void sensor_cb(const mavros_msgs::msg::HilSensor::SharedPtr req) + { + mavlink::common::msg::HIL_SENSOR sensor = {}; + + sensor.time_usec = get_time_usec(req->header.stamp); + // WRT world frame + auto acc = ftf::transform_frame_baselink_aircraft( + ftf::to_eigen(req->acc)); + auto gyro = ftf::transform_frame_baselink_aircraft( + ftf::to_eigen(req->gyro)); + auto mag = ftf::transform_frame_baselink_aircraft( + ftf::to_eigen(req->mag) * TESLA_TO_GAUSS); + + // [[[cog: + // for a in ('acc', 'gyro', 'mag'): + // for b in "xyz": + // cog.outl(f"sensor.{b}{a} = {a}.{b}();") + // for f in (('abs_pressure', 'PASCAL_TO_MILLIBAR'), + // ('diff_pressure', 'PASCAL_TO_MILLIBAR'), + // 'pressure_alt', 'temperature', 'fields_updated'): + // f1 = f if isinstance(f, str) else f[0] + // f2 = f if isinstance(f, str) else f'{f[0]} * {f[1]}' + // cog.outl(f"sensor.{f1} = req->{f2};") + // ]]] + sensor.xacc = acc.x(); + sensor.yacc = acc.y(); + sensor.zacc = acc.z(); + sensor.xgyro = gyro.x(); + sensor.ygyro = gyro.y(); + sensor.zgyro = gyro.z(); + sensor.xmag = mag.x(); + sensor.ymag = mag.y(); + sensor.zmag = mag.z(); + sensor.abs_pressure = req->abs_pressure * PASCAL_TO_MILLIBAR; + sensor.diff_pressure = req->diff_pressure * PASCAL_TO_MILLIBAR; + sensor.pressure_alt = req->pressure_alt; + sensor.temperature = req->temperature; + sensor.fields_updated = req->fields_updated; + // [[[end]]] (checksum: e1f6502cf1195ffdf3018f0c4d0c9329) + + uas->send_message(sensor); + } + + /** + * @brief Send simulated optical flow to FCU. + * Message specification: @p https://mavlink.io/en/messages/common.html#HIL_OPTICAL_FLOW + */ + void optical_flow_cb(const mavros_msgs::msg::OpticalFlowRad::SharedPtr req) + { + mavlink::common::msg::HIL_OPTICAL_FLOW of = {}; + + auto int_xy = ftf::transform_frame_aircraft_baselink( + Eigen::Vector3d( + req->integrated_x, + req->integrated_y, + 0.0)); + auto int_gyro = ftf::transform_frame_aircraft_baselink( + Eigen::Vector3d( + req->integrated_xgyro, + req->integrated_ygyro, + req->integrated_zgyro)); + + of.time_usec = get_time_usec(req->header.stamp); + of.sensor_id = INT8_MAX; // while we don't find a better way of handling it + of.integration_time_us = req->integration_time_us; + // [[[cog: + // for f in "xy": + // cog.outl(f"of.integrated_{f} = int_xy.{f}();") + // for f in "xyz": + // cog.outl(f"of.integrated_{f}gyro = int_gyro.{f}();") + // for f in ('time_delta_distance_us', 'distance', 'quality'): + // cog.outl(f"of.{f} = req->{f};") + // ]]] + of.integrated_x = int_xy.x(); + of.integrated_y = int_xy.y(); + of.integrated_xgyro = int_gyro.x(); + of.integrated_ygyro = int_gyro.y(); + of.integrated_zgyro = int_gyro.z(); + of.time_delta_distance_us = req->time_delta_distance_us; + of.distance = req->distance; + of.quality = req->quality; + // [[[end]]] (checksum: 4dc7f3f9b5de60b4d1685bde42c66b26) + of.temperature = req->temperature * 100.0f; // in centi-degrees celsius + + uas->send_message(of); + } + + /** + * @brief Send simulated received RAW values of the RC channels to the FCU. + * Message specification: @p https://mavlink.io/en/messages/common.html#HIL_RC_INPUTS_RAW + */ + void rcin_raw_cb(const mavros_msgs::msg::RCIn::SharedPtr req) + { + mavlink::common::msg::HIL_RC_INPUTS_RAW rcin {}; + + constexpr size_t MAX_CHANCNT = 12; + + std::array channels; + auto n = std::min(req->channels.size(), channels.size()); + std::copy(req->channels.begin(), req->channels.begin() + n, channels.begin()); + std::fill(channels.begin() + n, channels.end(), UINT16_MAX); + + rcin.time_usec = get_time_usec(req->header.stamp); + // [[[cog: + // for i in range(1,13): + // cog.outl(f"rcin.chan{i}_raw = channels[{i-1}];") + // ]]] + rcin.chan1_raw = channels[0]; + rcin.chan2_raw = channels[1]; + rcin.chan3_raw = channels[2]; + rcin.chan4_raw = channels[3]; + rcin.chan5_raw = channels[4]; + rcin.chan6_raw = channels[5]; + rcin.chan7_raw = channels[6]; + rcin.chan8_raw = channels[7]; + rcin.chan9_raw = channels[8]; + rcin.chan10_raw = channels[9]; + rcin.chan11_raw = channels[10]; + rcin.chan12_raw = channels[11]; + // [[[end]]] (checksum: 342673b0690e47f16c8b89803ab00e68) + + uas->send_message(rcin); + } +}; +} // namespace extra_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::HilPlugin) diff --git a/mavros_extras/src/plugins/landing_target.cpp b/mavros_extras/src/plugins/landing_target.cpp index 58e58df07..fbb6311da 100644 --- a/mavros_extras/src/plugins/landing_target.cpp +++ b/mavros_extras/src/plugins/landing_target.cpp @@ -14,385 +14,473 @@ * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace mavros { -namespace extra_plugins { +#include + +#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/utils.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" +#include "mavros/setpoint_mixin.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" +#include "mavros_msgs/msg/landing_target.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; using mavlink::common::MAV_FRAME; using mavlink::common::LANDING_TARGET_TYPE; /** * @brief Landing Target plugin + * @plugin landing_target * * This plugin is intended to publish the location of a landing area captured from a downward facing camera * to the FCU and/or receive landing target tracking data coming from the FCU. */ -class LandingTargetPlugin : public plugin::PluginBase, - private plugin::TF2ListenerMixin { +class LandingTargetPlugin : public plugin::Plugin, + private plugin::TF2ListenerMixin +{ public: - LandingTargetPlugin() : - nh("~landing_target"), - tf_rate(10.0), - send_tf(true), - listen_tf(false), - listen_lt(false), - mav_frame("LOCAL_NED"), - target_size_x(1.0), - target_size_y(1.0), - image_width(640), - image_height(480), - fov_x(2.0071286398), - fov_y(2.0071286398), - focal_length(2.8), - land_target_type("VISION_FIDUCIAL") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - // general params - nh.param("frame_id", frame_id, "landing_target_1"); - nh.param("listen_lt", listen_lt, false); // subscribe to raw LadingTarget msg? - nh.param("mav_frame", mav_frame, "LOCAL_NED"); - frame = utils::mav_frame_from_str(mav_frame); // MAV_FRAME index based on given frame name (If unknown, defaults to GENERIC) - - nh.param("land_target_type", land_target_type, "VISION_FIDUCIAL"); - type = utils::landing_target_type_from_str(land_target_type); // LANDING_TARGET_TYPE index based on given type name (If unknown, defaults to LIGHT_BEACON) - - // target size - nh.param("target_size/x", target_size_x, 1.0); // [meters] - nh.param("target_size/y", target_size_y, 1.0); - - // image size - nh.param("image/width", image_width, 640); // [pixels] - nh.param("image/height", image_height, 480); - - // camera field-of-view -> should be precised using the calibrated camera intrinsics - nh.param("camera/fov_x", fov_x, 2.0071286398); // default: 115 [degrees] - nh.param("camera/fov_y", fov_y, 2.0071286398); - // camera focal length - nh.param("camera/focal_length", focal_length, 2.8); //ex: OpenMV Cam M7: 2.8 [mm] - - // tf subsection - nh.param("tf/send", send_tf, true); - nh.param("tf/listen", listen_tf, false); - nh.param("tf/frame_id", tf_frame_id, frame_id); - nh.param("tf/child_frame_id", tf_child_frame_id, "camera_center"); - nh.param("tf/rate_limit", tf_rate, 50.0); - - land_target_pub = nh.advertise("pose_in", 10); - lt_marker_pub = nh.advertise("lt_marker", 10); - - if (listen_tf) { // Listen to transform - ROS_INFO_STREAM_NAMED("landing_target", "Listen to landing_target transform " << tf_frame_id - << " -> " << tf_child_frame_id); - tf2_start("LandingTargetTF", &LandingTargetPlugin::transform_cb); - } - else if (listen_lt) { // Subscribe to LandingTarget msg - land_target_sub = nh.subscribe("raw", 10, &LandingTargetPlugin::landtarget_cb, this); - } - else { // Subscribe to PoseStamped msg - pose_sub = nh.subscribe("pose", 10, &LandingTargetPlugin::pose_cb, this); - } - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&LandingTargetPlugin::handle_landing_target) - }; - } + explicit LandingTargetPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "landing_target"), + tf_rate(50.0), + tf_send(true), + tf_listen(false), + frame_id("landing_target_1"), + tf_frame_id("landing_target_1"), + target_size_x(1.0), + target_size_y(1.0), + fov_x(2.0071286398), + fov_y(2.0071286398), + focal_length(2.8), + image_width(640), + image_height(480), + mav_frame("LOCAL_NED"), + land_target_type("VISION_FIDUCIAL") + { + enable_node_watch_parameters(); + + // general params + node_declare_and_watch_parameter( + "frame_id", "landing_target_1", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + + node_declare_and_watch_parameter( + "listen_lt", false, [&](const rclcpp::Parameter & p) { + auto listen_lt = p.as_bool(); + + land_target_sub.reset(); + + if (listen_lt) { + land_target_sub = node->create_subscription( + "~/raw", 10, std::bind( + &LandingTargetPlugin::landtarget_cb, this, + _1)); + } + }); + + node_declare_and_watch_parameter( + "mav_frame", "LOCAL_NED", [&](const rclcpp::Parameter & p) { + mav_frame = p.as_string(); + frame = utils::mav_frame_from_str(mav_frame); + // MAV_FRAME index based on given frame name (If unknown, defaults to GENERIC) + }); + + node_declare_and_watch_parameter( + "land_target_type", "VISION_FIDUCIAL", [&](const rclcpp::Parameter & p) { + land_target_type = p.as_string(); + type = utils::landing_target_type_from_str(land_target_type); + // LANDING_TARGET_TYPE index based on given type name (If unknown, defaults to LIGHT_BEACON) + }); + + // target size + node_declare_and_watch_parameter( + "target_size.x", 1.0, [&](const rclcpp::Parameter & p) { + target_size_x = p.as_double(); // [meters] + }); + + node_declare_and_watch_parameter( + "target_size.y", 1.0, [&](const rclcpp::Parameter & p) { + target_size_y = p.as_double(); + }); + + // image size + node_declare_and_watch_parameter( + "image.width", 640, [&](const rclcpp::Parameter & p) { + image_width = p.as_int(); // [pixels] + }); + + node_declare_and_watch_parameter( + "image.height", 480, [&](const rclcpp::Parameter & p) { + image_height = p.as_int(); + }); + + // camera field-of-view -> should be precised using the calibrated camera intrinsics + node_declare_and_watch_parameter( + "camera.fov_x", 2.0071286398, [&](const rclcpp::Parameter & p) { + fov_x = p.as_double(); // default: 115 degrees in [radians] + }); + + node_declare_and_watch_parameter( + "camera.fov_y", 2.0071286398, [&](const rclcpp::Parameter & p) { + fov_y = p.as_double(); // default: 115 degrees in [radians] + }); + + // camera focal length + node_declare_and_watch_parameter( + "camera.focal_length", 2.8, [&](const rclcpp::Parameter & p) { + focal_length = p.as_double(); // ex: OpenMV Cam M7: 2.8 [mm] + }); + + // tf subsection + node_declare_and_watch_parameter( + "tf.rate_limit", 50.0, [&](const rclcpp::Parameter & p) { + // no dynamic update here yet. need to modify the thread in + // setpoint_mixin to handle new rates + tf_rate = p.as_double(); + }); + + node_declare_and_watch_parameter( + "tf.send", true, [&](const rclcpp::Parameter & p) { + tf_send = p.as_bool(); + }); + + node_declare_and_watch_parameter( + "tf.frame_id", frame_id, [&](const rclcpp::Parameter & p) { + tf_frame_id = p.as_string(); + }); + + node_declare_and_watch_parameter( + "tf.child_frame_id", "camera_center", [&](const rclcpp::Parameter & p) { + tf_child_frame_id = p.as_string(); + }); + + node_declare_and_watch_parameter( + "tf.listen", false, [&](const rclcpp::Parameter & p) { + tf_listen = p.as_bool(); + if (!tf_listen) { + return; + } + + RCLCPP_INFO_STREAM( + get_logger(), + "LT: Listen to landing_target transform " << tf_frame_id << + " -> " << tf_child_frame_id); + tf2_start("LandingTargetTF", &LandingTargetPlugin::transform_cb); + }); + + + auto sensor_qos = rclcpp::SensorDataQoS(); + + land_target_pub = + node->create_publisher("~/pose_in", sensor_qos); + lt_marker_pub = node->create_publisher( + "~/lt_marker", + sensor_qos); + + pose_sub = node->create_subscription( + "~/pose", 10, std::bind( + &LandingTargetPlugin::pose_cb, this, _1)); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&LandingTargetPlugin::handle_landing_target) + }; + } private: - friend class TF2ListenerMixin; - ros::NodeHandle nh; - - bool send_tf; - bool listen_tf; - double tf_rate; - ros::Time last_transform_stamp; - - bool listen_lt; - - std::string frame_id; - std::string tf_frame_id; - std::string tf_child_frame_id; - - ros::Publisher land_target_pub; - ros::Publisher lt_marker_pub; - ros::Subscriber land_target_sub; - ros::Subscriber pose_sub; - - double target_size_x, target_size_y; - double fov_x, fov_y; - double focal_length; - int image_width, image_height; - - MAV_FRAME frame; - std::string mav_frame; - - LANDING_TARGET_TYPE type; - std::string land_target_type; - - /* -*- low-level send -*- */ - void landing_target(uint64_t time_usec, - uint8_t target_num, - uint8_t frame, - Eigen::Vector2f angle, - float distance, - Eigen::Vector2f size, - Eigen::Vector3d pos, - Eigen::Quaterniond q, - uint8_t type, - uint8_t position_valid) - { - mavlink::common::msg::LANDING_TARGET lt {}; - - lt.time_usec = time_usec; - lt.target_num = target_num; - lt.frame = frame; - lt.distance = distance; - lt.type = type; - lt.position_valid = position_valid; - lt.angle_x = angle.x(); - lt.angle_y = angle.y(); - lt.size_x = size.x(); - lt.size_y = size.y(); - lt.x = pos.x(); - lt.y = pos.y(); - lt.z = pos.z(); - - ftf::quaternion_to_mavlink(q, lt.q); - - UAS_FCU(m_uas)->send_message_ignore_drop(lt); - } - - /* -*- mid-level helpers -*- */ - /** - * @brief Displacement: (not to be mixed with angular displacement) - * - * WITH angle_rad = atan(y / x) * (π / 180) - * IF X & Y > 0: (1st quadrant) - * θ_x = angle_rad - * θ_y = - angle_rad - * IF X < 0 & Y > 0: (2nd quadrant) - * θ_x = π - angle_rad - * θ_y = angle_rad - * IF X < 0 & Y < 0: (3rd quadrant) - * θ_x = π + angle_rad - * θ_y = π - angle_rad - * IF X > 0 & Y < 0: (4th quadrant) - * θ_x = - angle_rad - * θ_y = π + angle_rad - */ - void inline cartesian_to_displacement(const Eigen::Vector3d &pos, Eigen::Vector2f &angle) { - float angle_rad = atan(pos.y() / pos.x()) * (M_PI / 180.0); - - if (pos.x() > 0 && pos.y() > 0) { - angle.x() = angle_rad; - angle.y() = -angle_rad; - } - else if (pos.x() < 0 && pos.y() > 0) { - angle.x() = M_PI - angle_rad; - angle.y() = angle_rad; - } - else if (pos.x() < 0 && pos.y() < 0) { - angle.x() = M_PI + angle_rad; - angle.y() = M_PI - angle_rad; - } - else if (pos.x() > 0 && pos.y() < 0) { - angle.x() = -angle_rad; - angle.y() = M_PI + angle_rad; - } - } - - /** - * @brief Send landing target transform to FCU - */ - void send_landing_target(const ros::Time &stamp, const Eigen::Affine3d &tr) { - /** - * @brief the position of the landing target WRT camera center - on the FCU, - * the position WRT to the origin local NED frame can be computed to allow - * the FCU to know where the landing target is in the local frame. - */ - auto pos = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation())); - - /** @brief the orientation of the landing target WRT camera frame */ - auto q = ftf::transform_orientation_enu_ned( - ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))); - - Eigen::Vector2f angle; - Eigen::Vector2f size_rad; - Eigen::Vector2f fov; - - // the norm of the position vector is considered the distance to the landing target - float distance = pos.norm(); - - // if the landing target type is a vision type, compute the angular offsets - if (land_target_type.find("VISION")) { - /** - * @brief: the camera angular offsets can be computed by knowing the position - * of the target center relative to the camera center, the field-of-view of - * the camera and the image resolution being considered. - * The target size is computed by the angle of view formula (similar to angular diameter). - */ - angle.x() = (pos.x() - image_width / 2.0) * fov.x() / image_width; - angle.y() = (pos.y() - image_height / 2.0) * fov.y() / image_height; - /** - * @brief Angular diameter: - * δ = 2 * atan(d / (2 * D)) - * where, d = actual diameter; D = distance to the object (or focal length of a camera) - */ - size_rad = {2 * (M_PI / 180.0) * atan(target_size_x / (2 * focal_length)), - 2 * (M_PI / 180.0) * atan(target_size_y / (2 * focal_length))}; - } - // else, the same values are computed considering the displacement relative to X and Y axes of the camera frame reference - else { - cartesian_to_displacement(pos, angle); - size_rad = {2 * (M_PI / 180.0) * atan(target_size_x / (2 * distance)), - 2 * (M_PI / 180.0) * atan(target_size_y / (2 * distance))}; - } - - if (last_transform_stamp == stamp) { - ROS_DEBUG_THROTTLE_NAMED(10, "landing_target", "LT: Same transform as last one, dropped."); - return; - } - last_transform_stamp = stamp; - - auto rpy = ftf::quaternion_to_rpy(q); - - // the last char of frame_id is considered the number of the target - uint8_t id = static_cast(frame_id.back()); - - ROS_DEBUG_THROTTLE_NAMED(10, "landing_target", "Tx landing target: " - "ID: %d frame: %s angular offset: X:%1.3frad, Y:%1.3frad) " - "distance: %1.3fm position: X:%1.3fm, Y:%1.3fm, Z:%1.3fm) " - "orientation: roll:%1.4frad pitch:%1.4frad yaw:%1.4frad " - "size: X:%1.3frad by Y:%1.3frad type: %s", - id, utils::to_string(static_cast(frame)).c_str(), - angle.x(), angle.y(), distance, pos.x(), pos.y(), pos.z(), - rpy.x(), rpy.y(), rpy.z(), size_rad.x(), size_rad.y(), - utils::to_string(static_cast(type)).c_str()); - - landing_target(stamp.toNSec() / 1000, - id, - utils::enum_value(frame), // by default, in LOCAL_NED - angle, - distance, - size_rad, - pos, - q, - utils::enum_value(type), - 1); // position is valid from the first received msg - } - - /** - * @brief Receive landing target from FCU. - */ - void handle_landing_target(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LANDING_TARGET &land_target) { - /** @todo these transforms should be applied according to the MAV_FRAME */ - auto position = ftf::transform_frame_ned_enu(Eigen::Vector3d(land_target.x, land_target.y, land_target.z)); - auto orientation = ftf::transform_orientation_aircraft_baselink( - ftf::transform_orientation_ned_enu( - ftf::mavlink_to_quaternion(land_target.q))); - - auto rpy = ftf::quaternion_to_rpy(orientation); - - ROS_DEBUG_THROTTLE_NAMED(10, "landing_target", "Rx landing target: " - "ID: %d frame: %s angular offset: X:%1.3frad, Y:%1.3frad) " - "distance: %1.3fm position: X:%1.3fm, Y:%1.3fm, Z:%1.3fm) " - "orientation: roll:%1.4frad pitch:%1.4frad yaw:%1.4frad " - "size: X:%1.3frad by Y:%1.3frad type: %s", - land_target.target_num, utils::to_string(static_cast(land_target.frame)).c_str(), - land_target.angle_x, land_target.angle_y, land_target.distance, - position.x(), position.y(), position.z(), rpy.x(), rpy.y(), rpy.z(), land_target.size_x, land_target.size_y, - utils::to_string(static_cast(land_target.type)).c_str()); - - auto pose = boost::make_shared(); - pose->header = m_uas->synchronized_header(frame_id, land_target.time_usec); - - tf::pointEigenToMsg(position, pose->pose.position); - tf::quaternionEigenToMsg(orientation, pose->pose.orientation); - - land_target_pub.publish(pose); - - if (send_tf) { - geometry_msgs::TransformStamped transform; - - transform.header.stamp = pose->header.stamp; - transform.header.frame_id = "landing_target_" + boost::lexical_cast(land_target.target_num); - transform.child_frame_id = tf_child_frame_id; - - transform.transform.rotation = pose->pose.orientation; - tf::vectorEigenToMsg(position, transform.transform.translation); - - m_uas->tf2_broadcaster.sendTransform(transform); - } - - auto tg_size_msg = boost::make_shared(); - Eigen::Vector3d target_size(target_size_x, target_size_y, 0.0); - - tf::vectorEigenToMsg(target_size, tg_size_msg->vector); - - lt_marker_pub.publish(tg_size_msg); - } - - /* -*- callbacks -*- */ - /** - * @brief callback for TF2 listener - */ - void transform_cb(const geometry_msgs::TransformStamped &transform) { - Eigen::Affine3d tr; - tf::transformMsgToEigen(transform.transform, tr); - - send_landing_target(transform.header.stamp, tr); - } - - /** - * @brief callback for PoseStamped msgs topic - */ - void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req) { - Eigen::Affine3d tr; - tf::poseMsgToEigen(req->pose, tr); - - send_landing_target(req->header.stamp, tr); - } - - /** - * @brief callback for raw LandingTarget msgs topic - useful if one has the - * data processed in another node - */ - void landtarget_cb(const mavros_msgs::LandingTarget::ConstPtr &req) { - Eigen::Affine3d tr; - tf::poseMsgToEigen(req->pose, tr); - - /** @todo these transforms should be applied according to the MAV_FRAME */ - auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation())); - auto orientation = ftf::transform_orientation_enu_ned( - ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))); - - landing_target( req->header.stamp.toNSec() / 1000, - req->target_num, - req->frame, // by default, in LOCAL_NED - Eigen::Vector2f(req->angle[0], req->angle[1]), - req->distance, - Eigen::Vector2f(req->size[0], req->size[1]), - position, - orientation, - req->type, - 1); // position is valid from the first received msg - } + friend class TF2ListenerMixin; + + double tf_rate; + bool tf_send; + bool tf_listen; + rclcpp::Time last_transform_stamp; + + std::string frame_id; + std::string tf_frame_id; + std::string tf_child_frame_id; + + rclcpp::Publisher::SharedPtr land_target_pub; + rclcpp::Publisher::SharedPtr lt_marker_pub; + rclcpp::Subscription::SharedPtr pose_sub; + rclcpp::Subscription::SharedPtr land_target_sub; + + double target_size_x, target_size_y; + double fov_x, fov_y; + double focal_length; + int image_width, image_height; + + MAV_FRAME frame; + std::string mav_frame; + + LANDING_TARGET_TYPE type; + std::string land_target_type; + + /* -*- low-level send -*- */ + void landing_target( + uint64_t time_usec, + uint8_t target_num, + uint8_t frame, + Eigen::Vector2f angle, + float distance, + Eigen::Vector2f size, + Eigen::Vector3d pos, + Eigen::Quaterniond q, + uint8_t type, + uint8_t position_valid) + { + mavlink::common::msg::LANDING_TARGET lt {}; + + lt.time_usec = time_usec; + lt.target_num = target_num; + lt.frame = frame; + lt.distance = distance; + lt.type = type; + lt.position_valid = position_valid; + lt.angle_x = angle.x(); + lt.angle_y = angle.y(); + lt.size_x = size.x(); + lt.size_y = size.y(); + lt.x = pos.x(); + lt.y = pos.y(); + lt.z = pos.z(); + + ftf::quaternion_to_mavlink(q, lt.q); + + uas->send_message(lt); + } + + /* -*- mid-level helpers -*- */ + /** + * @brief Displacement: (not to be mixed with angular displacement) + * + * WITH angle_rad = atan(y / x) * (π / 180) + * IF X & Y > 0: (1st quadrant) + * θ_x = angle_rad + * θ_y = - angle_rad + * IF X < 0 & Y > 0: (2nd quadrant) + * θ_x = π - angle_rad + * θ_y = angle_rad + * IF X < 0 & Y < 0: (3rd quadrant) + * θ_x = π + angle_rad + * θ_y = π - angle_rad + * IF X > 0 & Y < 0: (4th quadrant) + * θ_x = - angle_rad + * θ_y = π + angle_rad + */ + void inline cartesian_to_displacement(const Eigen::Vector3d & pos, Eigen::Vector2f & angle) + { + float angle_rad = atan(pos.y() / pos.x()) * (M_PI / 180.0); + + if (pos.x() > 0 && pos.y() > 0) { + angle.x() = angle_rad; + angle.y() = -angle_rad; + } else if (pos.x() < 0 && pos.y() > 0) { + angle.x() = M_PI - angle_rad; + angle.y() = angle_rad; + } else if (pos.x() < 0 && pos.y() < 0) { + angle.x() = M_PI + angle_rad; + angle.y() = M_PI - angle_rad; + } else if (pos.x() > 0 && pos.y() < 0) { + angle.x() = -angle_rad; + angle.y() = M_PI + angle_rad; + } + } + + /** + * @brief Send landing target transform to FCU + */ + void send_landing_target(const rclcpp::Time & stamp, const Eigen::Affine3d & tr) + { + /** + * @brief the position of the landing target WRT camera center - on the FCU, + * the position WRT to the origin local NED frame can be computed to allow + * the FCU to know where the landing target is in the local frame. + */ + auto pos = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation())); + + /** @brief the orientation of the landing target WRT camera frame */ + auto q = ftf::transform_orientation_enu_ned( + ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))); + + Eigen::Vector2f angle; + Eigen::Vector2f size_rad; + Eigen::Vector2f fov; + + // the norm of the position vector is considered the distance to the landing target + float distance = pos.norm(); + + // if the landing target type is a vision type, compute the angular offsets + if (land_target_type.find("VISION")) { + /** + * @brief: the camera angular offsets can be computed by knowing the position + * of the target center relative to the camera center, the field-of-view of + * the camera and the image resolution being considered. + * The target size is computed by the angle of view formula (similar to angular diameter). + */ + angle.x() = (pos.x() - image_width / 2.0) * fov.x() / image_width; + angle.y() = (pos.y() - image_height / 2.0) * fov.y() / image_height; + /** + * @brief Angular diameter: + * δ = 2 * atan(d / (2 * D)) + * where, d = actual diameter; D = distance to the object (or focal length of a camera) + */ + size_rad = {2 * (M_PI / 180.0) * atan(target_size_x / (2 * focal_length)), + 2 * (M_PI / 180.0) * atan(target_size_y / (2 * focal_length))}; + } else { + // else, the same values are computed considering the displacement + // relative to X and Y axes of the camera frame reference + cartesian_to_displacement(pos, angle); + size_rad = {2 * (M_PI / 180.0) * atan(target_size_x / (2 * distance)), + 2 * (M_PI / 180.0) * atan(target_size_y / (2 * distance))}; + } + + if (last_transform_stamp == stamp) { + RCLCPP_DEBUG_THROTTLE( + get_logger(), + *get_clock(), 10, "LT: Same transform as last one, dropped."); + return; + } + last_transform_stamp = stamp; + + + // the last char of frame_id is considered the number of the target + uint8_t id = static_cast(frame_id.back()); + + landing_target( + stamp.nanoseconds() / 1000, + id, + utils::enum_value(frame), // by default, in LOCAL_NED + angle, + distance, + size_rad, + pos, + q, + utils::enum_value(type), + 1); // position is valid from the first received msg + } + + /** + * @brief Receive landing target from FCU. + */ + void handle_landing_target( + [[maybe_unused]] const mavlink::mavlink_message_t * msg, + mavlink::common::msg::LANDING_TARGET & land_target, + [[maybe_unused]] plugin::filter::SystemAndOk filter) + { + /** @todo these transforms should be applied according to the MAV_FRAME */ + auto position = + ftf::transform_frame_ned_enu( + Eigen::Vector3d( + land_target.x, land_target.y, + land_target.z)); + auto orientation = ftf::transform_orientation_aircraft_baselink( + ftf::transform_orientation_ned_enu( + ftf::mavlink_to_quaternion(land_target.q))); + + // auto rpy = ftf::quaternion_to_rpy(orientation); + + RCLCPP_DEBUG_STREAM_THROTTLE( + get_logger(), + *get_clock(), 10, + "landing_target:\n" << + land_target.to_yaml()); + + geometry_msgs::msg::PoseStamped pose; + pose.header = uas->synchronized_header(frame_id, land_target.time_usec); + + pose.pose.position = tf2::toMsg(position); + pose.pose.orientation = tf2::toMsg(orientation); + + land_target_pub->publish(pose); + + if (tf_send) { + geometry_msgs::msg::TransformStamped transform; + + transform.header.stamp = pose.header.stamp; + transform.header.frame_id = "landing_target_" + std::to_string( + land_target.target_num); + transform.child_frame_id = tf_child_frame_id; + + transform.transform.rotation = pose.pose.orientation; + geometry_msgs::msg::Point translation_p = tf2::toMsg(position); + transform.transform.translation.x = translation_p.x; + transform.transform.translation.y = translation_p.y; + transform.transform.translation.z = translation_p.z; + + uas->tf2_broadcaster.sendTransform(transform); + } + + geometry_msgs::msg::Vector3Stamped tg_size_msg; + tg_size_msg.vector.x = target_size_x; + tg_size_msg.vector.y = target_size_y; + tg_size_msg.vector.z = 0.0; + + lt_marker_pub->publish(tg_size_msg); + } + + /* -*- callbacks -*- */ + /** + * @brief callback for TF2 listener + */ + void transform_cb(const geometry_msgs::msg::TransformStamped & transform) + { + Eigen::Affine3d tr = tf2::transformToEigen(transform.transform); + + send_landing_target(transform.header.stamp, tr); + } + + /** + * @brief callback for PoseStamped msgs topic + */ + void pose_cb(const geometry_msgs::msg::PoseStamped::SharedPtr req) + { + Eigen::Affine3d tr; + tf2::fromMsg(req->pose, tr); + + send_landing_target(req->header.stamp, tr); + } + + /** + * @brief callback for raw LandingTarget msgs topic - useful if one has the + * data processed in another node + */ + void landtarget_cb(const mavros_msgs::msg::LandingTarget::SharedPtr req) + { + Eigen::Affine3d tr; + tf2::fromMsg(req->pose, tr); + + /** @todo these transforms should be applied according to the MAV_FRAME */ + auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation())); + auto orientation = ftf::transform_orientation_enu_ned( + ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))); + + landing_target( + rclcpp::Time(req->header.stamp).nanoseconds() / 1000, + req->target_num, + req->frame, // by default, in LOCAL_NED + Eigen::Vector2f(req->angle[0], req->angle[1]), + req->distance, + Eigen::Vector2f(req->size[0], req->size[1]), + position, + orientation, + req->type, + 1); // position is valid from the first received msg + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::LandingTargetPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::LandingTargetPlugin) diff --git a/mavros_extras/src/plugins/log_transfer.cpp b/mavros_extras/src/plugins/log_transfer.cpp index 8a065e36f..89c143ba4 100644 --- a/mavros_extras/src/plugins/log_transfer.cpp +++ b/mavros_extras/src/plugins/log_transfer.cpp @@ -1,140 +1,179 @@ -#include -#include -#include -#include -#include -#include -#include - -namespace mavros { -namespace extra_plugins { -class LogTransferPlugin : public plugin::PluginBase { +/* + * Copyright 2018 mlvov . + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief Log Transfer plugin + * @file log_transfer.cpp + * @author mlvov + * + * @addtogroup plugin + * @{ + */ + +#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/log_data.hpp" +#include "mavros_msgs/msg/log_entry.hpp" +#include "mavros_msgs/srv/log_request_data.hpp" +#include "mavros_msgs/srv/log_request_end.hpp" +#include "mavros_msgs/srv/log_request_list.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT + +/** + * @brief Log Transfer plugin + * @plugin log_transfer + */ +class LogTransferPlugin : public plugin::Plugin +{ public: - LogTransferPlugin() : - nh("~log_transfer") {} - - void initialize(UAS& uas) override - { - PluginBase::initialize(uas); - - log_entry_pub = nh.advertise("raw/log_entry", 1000); - log_data_pub = nh.advertise("raw/log_data", 1000); - - log_request_list_srv = nh.advertiseService("raw/log_request_list", - &LogTransferPlugin::log_request_list_cb, this); - log_request_data_srv = nh.advertiseService("raw/log_request_data", - &LogTransferPlugin::log_request_data_cb, this); - log_request_end_srv = nh.advertiseService("raw/log_request_end", - &LogTransferPlugin::log_request_end_cb, this); - log_request_erase_srv = nh.advertiseService("raw/log_request_erase", - &LogTransferPlugin::log_request_erase_cb, this); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&LogTransferPlugin::handle_log_entry), - make_handler(&LogTransferPlugin::handle_log_data), - }; - } + explicit LogTransferPlugin(plugin::UASPtr uas_) + : plugin::Plugin(uas_, "log_transfer") + { + log_entry_pub = node->create_publisher("~/raw/log_entry", 1000); + log_data_pub = node->create_publisher("~/raw/log_data", 1000); + + log_request_list_srv = node->create_service( + "~/raw/log_request_list", std::bind(&LogTransferPlugin::log_request_list_cb, this, _1, _2)); + log_request_data_srv = node->create_service( + "~/raw/log_request_data", std::bind(&LogTransferPlugin::log_request_data_cb, this, _1, _2)); + log_request_end_srv = node->create_service( + "~/raw/log_request_end", std::bind(&LogTransferPlugin::log_request_end_cb, this, _1, _2)); + log_request_erase_srv = node->create_service( + "~/raw/log_request_erase", std::bind( + &LogTransferPlugin::log_request_erase_cb, this, _1, + _2)); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&LogTransferPlugin::handle_log_entry), + make_handler(&LogTransferPlugin::handle_log_data), + }; + } private: - ros::NodeHandle nh; - ros::Publisher log_entry_pub, log_data_pub; - ros::ServiceServer log_request_list_srv, log_request_data_srv, log_request_end_srv, log_request_erase_srv; - - void handle_log_entry(const mavlink::mavlink_message_t*, mavlink::common::msg::LOG_ENTRY& le) - { - auto msg = boost::make_shared(); - msg->header.stamp = ros::Time::now(); - msg->id = le.id; - msg->num_logs = le.num_logs; - msg->last_log_num = le.last_log_num; - msg->time_utc = ros::Time(le.time_utc); - msg->size = le.size; - log_entry_pub.publish(msg); - } - - void handle_log_data(const mavlink::mavlink_message_t*, mavlink::common::msg::LOG_DATA& ld) - { - auto msg = boost::make_shared(); - msg->header.stamp = ros::Time::now(); - msg->id = ld.id; - msg->offset = ld.ofs; - - auto count = ld.count; - if (count > ld.data.max_size()) { - count = ld.data.max_size(); - } - msg->data.insert(msg->data.cbegin(), ld.data.cbegin(), ld.data.cbegin() + count); - log_data_pub.publish(msg); - } - - bool log_request_list_cb(mavros_msgs::LogRequestList::Request &req, - mavros_msgs::LogRequestList::Response &res) - { - mavlink::common::msg::LOG_REQUEST_LIST msg = {}; - m_uas->msg_set_target(msg); - msg.start = req.start; - msg.end = req.end; - - res.success = true; - try { - UAS_FCU(m_uas)->send_message(msg); - } catch (std::length_error&) { - res.success = false; - } - return true; - } - - bool log_request_data_cb(mavros_msgs::LogRequestData::Request &req, - mavros_msgs::LogRequestData::Response &res) - { - mavlink::common::msg::LOG_REQUEST_DATA msg = {}; - m_uas->msg_set_target(msg); - msg.id = req.id; - msg.ofs = req.offset; - msg.count = req.count; - - res.success = true; - try { - UAS_FCU(m_uas)->send_message(msg); - } catch (std::length_error&) { - res.success = false; - } - return true; - } - - bool log_request_end_cb(mavros_msgs::LogRequestEnd::Request &, - mavros_msgs::LogRequestEnd::Response &res) - { - mavlink::common::msg::LOG_REQUEST_END msg = {}; - m_uas->msg_set_target(msg); - res.success = true; - try { - UAS_FCU(m_uas)->send_message(msg); - } catch (std::length_error&) { - res.success = false; - } - return true; - } - - bool log_request_erase_cb(std_srvs::Trigger::Request &, - std_srvs::Trigger::Response &res) - { - mavlink::common::msg::LOG_ERASE msg; - m_uas->msg_set_target(msg); - try { - UAS_FCU(m_uas)->send_message(msg); - } catch (std::length_error&) { - res.success = false; - } - res.success = true; - return true; - } + rclcpp::Publisher::SharedPtr log_entry_pub; + rclcpp::Publisher::SharedPtr log_data_pub; + + rclcpp::Service::SharedPtr log_request_list_srv; + rclcpp::Service::SharedPtr log_request_data_srv; + rclcpp::Service::SharedPtr log_request_end_srv; + rclcpp::Service::SharedPtr log_request_erase_srv; + + void handle_log_entry( + const mavlink::mavlink_message_t * mmsg [[maybe_unused]], + mavlink::common::msg::LOG_ENTRY & le, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto msg = mavros_msgs::msg::LogEntry(); + + msg.header.stamp = node->now(); + msg.id = le.id; + msg.num_logs = le.num_logs; + msg.last_log_num = le.last_log_num; + msg.time_utc = rclcpp::Time(le.time_utc); + msg.size = le.size; + + log_entry_pub->publish(msg); + } + + void handle_log_data( + const mavlink::mavlink_message_t * mmsg [[maybe_unused]], + mavlink::common::msg::LOG_DATA & ld, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto msg = mavros_msgs::msg::LogData(); + + msg.header.stamp = node->now(); + msg.id = ld.id; + msg.offset = ld.ofs; + + auto count = std::min(ld.count, ld.data.max_size()); + msg.data.insert(msg.data.cbegin(), ld.data.cbegin(), ld.data.cbegin() + count); + + log_data_pub->publish(msg); + } + + void log_request_list_cb( + const mavros_msgs::srv::LogRequestList::Request::SharedPtr req, + mavros_msgs::srv::LogRequestList::Response::SharedPtr res) + { + mavlink::common::msg::LOG_REQUEST_LIST msg = {}; + + uas->msg_set_target(msg); + msg.start = req->start; + msg.end = req->end; + + uas->send_message(msg); + + // NOTE(vooon): with ROS2 router it's not possible to detect drops + res->success = true; + } + + void log_request_data_cb( + mavros_msgs::srv::LogRequestData::Request::SharedPtr req, + mavros_msgs::srv::LogRequestData::Response::SharedPtr res) + { + mavlink::common::msg::LOG_REQUEST_DATA msg = {}; + + uas->msg_set_target(msg); + msg.id = req->id; + msg.ofs = req->offset; + msg.count = req->count; + + uas->send_message(msg); + + // NOTE(vooon): with ROS2 router it's not possible to detect drops + res->success = true; + } + + void log_request_end_cb( + mavros_msgs::srv::LogRequestEnd::Request::SharedPtr req [[maybe_unused]], + mavros_msgs::srv::LogRequestEnd::Response::SharedPtr res) + { + mavlink::common::msg::LOG_REQUEST_END msg = {}; + + uas->msg_set_target(msg); + + uas->send_message(msg); + + // NOTE(vooon): with ROS2 router it's not possible to detect drops + res->success = true; + } + + void log_request_erase_cb( + std_srvs::srv::Trigger::Request::SharedPtr req [[maybe_unused]], + std_srvs::srv::Trigger::Response::SharedPtr res) + { + mavlink::common::msg::LOG_ERASE msg{}; + + uas->msg_set_target(msg); + + uas->send_message(msg); + + // NOTE(vooon): with ROS2 router it's not possible to detect drops + res->success = true; + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::LogTransferPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::LogTransferPlugin) diff --git a/mavros_extras/src/plugins/mag_calibration_status.cpp b/mavros_extras/src/plugins/mag_calibration_status.cpp index 70c9d15de..5b5117ff8 100644 --- a/mavros_extras/src/plugins/mag_calibration_status.cpp +++ b/mavros_extras/src/plugins/mag_calibration_status.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2021 André Ferreira + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief MagCalStatus plugin * @file MagCalStatus.cpp @@ -8,97 +15,108 @@ * @{ */ -#include -#include -#include -namespace mavros { -namespace std_plugins { +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "std_msgs/msg/u_int8.hpp" +#include "mavros_msgs/msg/magnetometer_reporter.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT + /** * @brief MagCalStatus plugin. + * @plugin mag_calibration_status * * Example and "how to" for users. */ -class MagCalStatusPlugin : public plugin::PluginBase { +class MagCalStatusPlugin : public plugin::Plugin +{ public: - MagCalStatusPlugin() : PluginBase(), - mcs_nh("~mag_calibration") - { } - - /** - * Plugin initializer. Constructor should not do this. - */ - void initialize(UAS &uas_) - { - PluginBase::initialize(uas_); - mcs_pub = mcs_nh.advertise("status", 2, true); - mcr_pub = mcs_nh.advertise("report", 2, true); - } - - /** - * This function returns message subscriptions. - * - * Each subscription made by PluginBase::make_handler() template. - * Two variations: - * - With automatic decoding and framing error filtering (see handle_heartbeat) - * - Raw message with framig status (see handle_systemtext) - */ - Subscriptions get_subscriptions() { - return { - /* automatic message deduction by second argument */ - make_handler(&MagCalStatusPlugin::handle_status), - make_handler(&MagCalStatusPlugin::handle_report), - }; - } + explicit MagCalStatusPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "mag_calibration") + { + // TODO(vooon): use QoS for "latched" topics + mcs_pub = node->create_publisher("~/status", 2); + mcr_pub = node->create_publisher("~/report", 2); + } + + Subscriptions get_subscriptions() + { + return { + make_handler(&MagCalStatusPlugin::handle_status), + make_handler(&MagCalStatusPlugin::handle_report), + }; + } private: - ros::NodeHandle mcs_nh; - ros::Publisher mcs_pub; - ros::Publisher mcr_pub; - std::array calibration_show; - std::array _rg_compass_cal_progress; - //Send progress of magnetometer calibration - void handle_status(const mavlink::mavlink_message_t *, mavlink::ardupilotmega::msg::MAG_CAL_PROGRESS &mp) { - auto mcs = boost::make_shared(); - - // How many compasses are we calibrating? - std::bitset<8> compass_calibrating = mp.cal_mask; - - if (compass_calibrating[mp.compass_id]) { - // Each compass gets a portion of the overall progress - if (mp.completion_pct < 95) { - calibration_show[mp.compass_id] = true; - } - _rg_compass_cal_progress[mp.compass_id] = mp.completion_pct; - } - - // Prevent data over 100% after cal_mask reset bit assigned to compass_id - uint16_t total_percentage = 0; - for (size_t i = 0; i < 8 && (compass_calibrating >> i).any(); i++) { - if (compass_calibrating[i]) { - total_percentage += static_cast(_rg_compass_cal_progress[i]); - } - } - - mcs->data = total_percentage / compass_calibrating.count(); - - mcs_pub.publish(mcs); - } - - //Send report after calibration is done - void handle_report(const mavlink::mavlink_message_t *, mavlink::common::msg::MAG_CAL_REPORT &mr) { - if (calibration_show[mr.compass_id]) { - auto mcr = boost::make_shared(); - mcr->header.stamp = ros::Time::now(); - mcr->header.frame_id = std::to_string(mr.compass_id); - mcr->report = mr.cal_status; - mcr->confidence = mr.orientation_confidence; - mcr_pub.publish(mcr); - calibration_show[mr.compass_id] = false; - } - } + rclcpp::Publisher::SharedPtr mcs_pub; + rclcpp::Publisher::SharedPtr mcr_pub; + + std::array calibration_show; + std::array _rg_compass_cal_progress; + + // Send progress of magnetometer calibration + void handle_status( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::ardupilotmega::msg::MAG_CAL_PROGRESS & mp, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto mcs = std_msgs::msg::UInt8(); + + // How many compasses are we calibrating? + std::bitset<8> compass_calibrating = mp.cal_mask; + + if (compass_calibrating[mp.compass_id]) { + // Each compass gets a portion of the overall progress + if (mp.completion_pct < 95) { + calibration_show[mp.compass_id] = true; + } + _rg_compass_cal_progress[mp.compass_id] = mp.completion_pct; + } + + // Prevent data over 100% after cal_mask reset bit assigned to compass_id + uint16_t total_percentage = 0; + for (size_t i = 0; i < 8 && (compass_calibrating >> i).any(); i++) { + if (compass_calibrating[i]) { + total_percentage += static_cast(_rg_compass_cal_progress[i]); + } + } + + mcs.data = total_percentage / compass_calibrating.count(); + + mcs_pub->publish(mcs); + } + + // Send report after calibration is done + void handle_report( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::MAG_CAL_REPORT & mr, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + if (mr.compass_id >= calibration_show.size()) { + return; + } + if (calibration_show[mr.compass_id]) { + auto mcr = mavros_msgs::msg::MagnetometerReporter(); + + mcr.header.stamp = node->now(); + mcr.header.frame_id = std::to_string(mr.compass_id); + mcr.report = mr.cal_status; + mcr.confidence = mr.orientation_confidence; + + mcr_pub->publish(mcr); + calibration_show[mr.compass_id] = false; + } + } }; -} // namespace std_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::MagCalStatusPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::MagCalStatusPlugin) diff --git a/mavros_extras/src/plugins/mocap_pose_estimate.cpp b/mavros_extras/src/plugins/mocap_pose_estimate.cpp index 01712b4ab..6d8c65b5c 100644 --- a/mavros_extras/src/plugins/mocap_pose_estimate.cpp +++ b/mavros_extras/src/plugins/mocap_pose_estimate.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2014,2015,2016 Vladimir Ermakov, Tony Baltovski. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief MocapPoseEstimate plugin * @file mocap_pose_estimate.cpp @@ -7,129 +14,117 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014,2015,2016 Vladimir Ermakov, Tony Baltovski. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -#include -#include +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace extra_plugins { /** * @brief MocapPoseEstimate plugin + * @plugin mocap_pose_estimate * * Sends motion capture data to FCU. */ -class MocapPoseEstimatePlugin : public plugin::PluginBase +class MocapPoseEstimatePlugin : public plugin::Plugin { public: - MocapPoseEstimatePlugin() : PluginBase(), - mp_nh("~mocap") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - bool use_tf; - bool use_pose; - - /** @note For VICON ROS package, subscribe to TransformStamped topic */ - mp_nh.param("use_tf", use_tf, false); - - /** @note For Optitrack ROS package, subscribe to PoseStamped topic */ - mp_nh.param("use_pose", use_pose, true); - - if (use_tf && !use_pose) { - mocap_tf_sub = mp_nh.subscribe("tf", 1, &MocapPoseEstimatePlugin::mocap_tf_cb, this); - } - else if (use_pose && !use_tf) { - mocap_pose_sub = mp_nh.subscribe("pose", 1, &MocapPoseEstimatePlugin::mocap_pose_cb, this); - } - else { - ROS_ERROR_NAMED("mocap", "Use one motion capture source."); - } - } - - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + explicit MocapPoseEstimatePlugin(plugin::UASPtr uas_) + : Plugin(uas_, "mocap") + { + /** @note For VICON ROS package, subscribe to TransformStamped topic */ + mocap_tf_sub = node->create_subscription( + "~/tf", 1, std::bind( + &MocapPoseEstimatePlugin::mocap_tf_cb, this, + _1)); + /** @note For Optitrack ROS package, subscribe to PoseStamped topic */ + mocap_pose_sub = node->create_subscription( + "~/pose", 1, std::bind( + &MocapPoseEstimatePlugin::mocap_pose_cb, this, + _1)); + } + + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - ros::NodeHandle mp_nh; - - ros::Subscriber mocap_pose_sub; - ros::Subscriber mocap_tf_sub; - - /* -*- low-level send -*- */ - void mocap_pose_send - (uint64_t usec, - Eigen::Quaterniond &q, - Eigen::Vector3d &v) - { - mavlink::common::msg::ATT_POS_MOCAP pos = {}; - - pos.time_usec = usec; - ftf::quaternion_to_mavlink(q, pos.q); - pos.x = v.x(); - pos.y = v.y(); - pos.z = v.z(); - - UAS_FCU(m_uas)->send_message_ignore_drop(pos); - } - - /* -*- mid-level helpers -*- */ - void mocap_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &pose) - { - Eigen::Quaterniond q_enu; - - tf::quaternionMsgToEigen(pose->pose.orientation, q_enu); - auto q = ftf::transform_orientation_enu_ned( - ftf::transform_orientation_baselink_aircraft(q_enu)); - - auto position = ftf::transform_frame_enu_ned( - Eigen::Vector3d( - pose->pose.position.x, - pose->pose.position.y, - pose->pose.position.z)); - - mocap_pose_send(pose->header.stamp.toNSec() / 1000, - q, - position); - } - - /* -*- callbacks -*- */ - void mocap_tf_cb(const geometry_msgs::TransformStamped::ConstPtr &trans) - { - Eigen::Quaterniond q_enu; - - tf::quaternionMsgToEigen(trans->transform.rotation, q_enu); - auto q = ftf::transform_orientation_enu_ned( - ftf::transform_orientation_baselink_aircraft(q_enu)); - - auto position = ftf::transform_frame_enu_ned( - Eigen::Vector3d( - trans->transform.translation.x, - trans->transform.translation.y, - trans->transform.translation.z)); - - mocap_pose_send(trans->header.stamp.toNSec() / 1000, - q, - position); - } + rclcpp::Subscription::SharedPtr mocap_pose_sub; + rclcpp::Subscription::SharedPtr mocap_tf_sub; + + /* -*- low-level send -*- */ + void mocap_pose_send( + uint64_t usec, + Eigen::Quaterniond & q, + Eigen::Vector3d & v) + { + mavlink::common::msg::ATT_POS_MOCAP pos = {}; + + pos.time_usec = usec; + ftf::quaternion_to_mavlink(q, pos.q); + pos.x = v.x(); + pos.y = v.y(); + pos.z = v.z(); + + uas->send_message(pos); + } + + /* -*- callbacks -*- */ + + void mocap_pose_cb(const geometry_msgs::msg::PoseStamped::SharedPtr pose) + { + Eigen::Quaterniond q_enu; + + tf2::fromMsg(pose->pose.orientation, q_enu); + auto q = ftf::transform_orientation_enu_ned( + ftf::transform_orientation_baselink_aircraft(q_enu)); + + auto position = ftf::transform_frame_enu_ned( + Eigen::Vector3d( + pose->pose.position.x, + pose->pose.position.y, + pose->pose.position.z)); + + mocap_pose_send( + get_time_usec(pose->header.stamp), + q, + position); + } + + void mocap_tf_cb(const geometry_msgs::msg::TransformStamped::SharedPtr trans) + { + Eigen::Quaterniond q_enu; + + tf2::fromMsg(trans->transform.rotation, q_enu); + auto q = ftf::transform_orientation_enu_ned( + ftf::transform_orientation_baselink_aircraft(q_enu)); + + auto position = ftf::transform_frame_enu_ned( + Eigen::Vector3d( + trans->transform.translation.x, + trans->transform.translation.y, + trans->transform.translation.z)); + + mocap_pose_send( + get_time_usec(trans->header.stamp), + q, + position); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::MocapPoseEstimatePlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::MocapPoseEstimatePlugin) diff --git a/mavros_extras/src/plugins/mount_control.cpp b/mavros_extras/src/plugins/mount_control.cpp index 3c590adac..52d35114d 100644 --- a/mavros_extras/src/plugins/mount_control.cpp +++ b/mavros_extras/src/plugins/mount_control.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2019 Jaeyoung Lim. + * Copyright 2021 Dr.-Ing. Amilcar do Carmo Lucas . + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Mount Control plugin * @file mount_control.cpp @@ -7,25 +15,31 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2019 Jaeyoung Lim. - * Copyright 2021 Dr.-Ing. Amilcar do Carmo Lucas . - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include + +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -#include -#include -#include -#include -#include +#include "mavros_msgs/srv/command_long.hpp" +#include "mavros_msgs/msg/mount_control.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" +#include "mavros_msgs/srv/mount_configure.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + +using namespace std::placeholders; // NOLINT +using namespace std::chrono_literals; // NOLINT -namespace mavros { -namespace extra_plugins { //! Mavlink enumerations using mavlink::common::MAV_MOUNT_MODE; using mavlink::common::MAV_CMD; @@ -37,319 +51,347 @@ using utils::enum_value; class MountStatusDiag : public diagnostic_updater::DiagnosticTask { public: - MountStatusDiag(const std::string &name) : - diagnostic_updater::DiagnosticTask(name), - _last_orientation_update(0, 0), - _debounce_s(NAN), - _roll_deg(NAN), - _pitch_deg(NAN), - _yaw_deg(NAN), - _setpoint_roll_deg(NAN), - _setpoint_pitch_deg(NAN), - _setpoint_yaw_deg(NAN), - _err_threshold_deg(NAN), - _error_detected(false), - _mode(255) - { } - - void set_err_threshold_deg(float threshold_deg) { - std::lock_guard lock(mutex); - _err_threshold_deg = threshold_deg; - } - - void set_debounce_s(double debounce_s) { - std::lock_guard lock(mutex); - _debounce_s = debounce_s; - } - - void set_status(float roll_deg, float pitch_deg, float yaw_deg, ros::Time timestamp) { - std::lock_guard lock(mutex); - _roll_deg = roll_deg; - _pitch_deg = pitch_deg; - _yaw_deg = yaw_deg; - _last_orientation_update = timestamp; - } - - void set_setpoint(float roll_deg, float pitch_deg, float yaw_deg, uint8_t mode) { - std::lock_guard lock(mutex); - _setpoint_roll_deg = roll_deg; - _setpoint_pitch_deg = pitch_deg; - _setpoint_yaw_deg = yaw_deg; - _mode = mode; - } - - void run(diagnostic_updater::DiagnosticStatusWrapper &stat) - { - float roll_err_deg; - float pitch_err_deg; - float yaw_err_deg; - bool error_detected = false; - bool stale = false; - - if (_mode != mavros_msgs::MountControl::MAV_MOUNT_MODE_MAVLINK_TARGETING) { - // Can only directly compare the MAV_CMD_DO_MOUNT_CONTROL angles with the MOUNT_ORIENTATION angles when in MAVLINK_TARGETING mode - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Can not diagnose in this targeting mode"); - stat.addf("Mode", "%d", _mode); - return; - } - - const ros::Time now = ros::Time::now(); - { - std::lock_guard lock(mutex); - roll_err_deg = _setpoint_roll_deg - _roll_deg; - pitch_err_deg = _setpoint_pitch_deg - _pitch_deg; - yaw_err_deg = _setpoint_yaw_deg - _yaw_deg; - - // detect errors (setpoint != current angle) - if (fabs(roll_err_deg) > _err_threshold_deg) { - error_detected = true; - } - if (fabs(pitch_err_deg) > _err_threshold_deg) { - error_detected = true; - } - if (fabs(yaw_err_deg) > _err_threshold_deg) { - error_detected = true; - } - if (now - _last_orientation_update > ros::Duration(5, 0)) { - stale = true; - } - // accessing the _debounce_s variable should be done inside this mutex, - // but we can treat it as an atomic variable, and save the trouble - } - - // detect error state changes - if (!_error_detected && error_detected) { - _error_started = now; - _error_detected = true; - } - if (_error_detected && !error_detected) { - _error_detected = false; - } - - // debounce errors - if (stale) { - stat.summary(diagnostic_msgs::DiagnosticStatus::STALE, "No MOUNT_ORIENTATION received in the last 5 s"); - } else if (_error_detected && (now - _error_started > ros::Duration(_debounce_s))) { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "angle error too high"); - } else { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Normal"); - } - - stat.addf("Roll err (deg)", "%.1f", roll_err_deg); - stat.addf("Pitch err (deg)", "%.1f", pitch_err_deg); - stat.addf("Yaw err (deg)", "%.1f", yaw_err_deg); - } + explicit MountStatusDiag(const std::string & name) + : diagnostic_updater::DiagnosticTask(name), + _last_orientation_update(0, 0), + _debounce_s(NAN), + _roll_deg(NAN), + _pitch_deg(NAN), + _yaw_deg(NAN), + _setpoint_roll_deg(NAN), + _setpoint_pitch_deg(NAN), + _setpoint_yaw_deg(NAN), + _err_threshold_deg(NAN), + _error_detected(false), + _mode(255) + {} + + void set_err_threshold_deg(float threshold_deg) + { + std::lock_guard lock(mutex); + _err_threshold_deg = threshold_deg; + } + + void set_debounce_s(double debounce_s) + { + std::lock_guard lock(mutex); + _debounce_s = debounce_s; + } + + void set_status(float roll_deg, float pitch_deg, float yaw_deg, rclcpp::Time timestamp) + { + std::lock_guard lock(mutex); + _roll_deg = roll_deg; + _pitch_deg = pitch_deg; + _yaw_deg = yaw_deg; + _last_orientation_update = timestamp; + } + + void set_setpoint(float roll_deg, float pitch_deg, float yaw_deg, uint8_t mode) + { + std::lock_guard lock(mutex); + _setpoint_roll_deg = roll_deg; + _setpoint_pitch_deg = pitch_deg; + _setpoint_yaw_deg = yaw_deg; + _mode = mode; + } + + void run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + float roll_err_deg; + float pitch_err_deg; + float yaw_err_deg; + bool error_detected = false; + bool stale = false; + + if (_mode != mavros_msgs::msg::MountControl::MAV_MOUNT_MODE_MAVLINK_TARGETING) { + // Can only directly compare the MAV_CMD_DO_MOUNT_CONTROL angles with + // the MOUNT_ORIENTATION angles when in MAVLINK_TARGETING mode + stat.summary(DiagnosticStatus::WARN, "Can not diagnose in this targeting mode"); + stat.addf("Mode", "%d", _mode); + return; + } + + const rclcpp::Time now = clock.now(); + { + std::lock_guard lock(mutex); + roll_err_deg = _setpoint_roll_deg - _roll_deg; + pitch_err_deg = _setpoint_pitch_deg - _pitch_deg; + yaw_err_deg = _setpoint_yaw_deg - _yaw_deg; + + // detect errors (setpoint != current angle) + if (fabs(roll_err_deg) > _err_threshold_deg) { + error_detected = true; + } + if (fabs(pitch_err_deg) > _err_threshold_deg) { + error_detected = true; + } + if (fabs(yaw_err_deg) > _err_threshold_deg) { + error_detected = true; + } + if (now - _last_orientation_update > rclcpp::Duration(5s)) { + stale = true; + } + // accessing the _debounce_s variable should be done inside this mutex, + // but we can treat it as an atomic variable, and save the trouble + } + + // detect error state changes + if (!_error_detected && error_detected) { + _error_started = now; + _error_detected = true; + } + if (_error_detected && !error_detected) { + _error_detected = false; + } + + // debounce errors + // *INDENT-OFF* + if (stale) { + stat.summary(DiagnosticStatus::STALE, "No MOUNT_ORIENTATION received in the last 5 s"); + } else if (_error_detected && + (now - _error_started > rclcpp::Duration(std::chrono::duration(_debounce_s)))) { + stat.summary(DiagnosticStatus::ERROR, "angle error too high"); + } else { + stat.summary(DiagnosticStatus::OK, "Normal"); + } + // *INDENT-ON* + + stat.addf("Roll err (deg)", "%.1f", roll_err_deg); + stat.addf("Pitch err (deg)", "%.1f", pitch_err_deg); + stat.addf("Yaw err (deg)", "%.1f", yaw_err_deg); + } private: - std::mutex mutex; - ros::Time _error_started; - ros::Time _last_orientation_update; - double _debounce_s; - float _roll_deg; - float _pitch_deg; - float _yaw_deg; - float _setpoint_roll_deg; - float _setpoint_pitch_deg; - float _setpoint_yaw_deg; - float _err_threshold_deg; - bool _error_detected; - uint8_t _mode; + std::mutex mutex; + rclcpp::Clock clock; + rclcpp::Time _error_started; + rclcpp::Time _last_orientation_update; + double _debounce_s; + float _roll_deg; + float _pitch_deg; + float _yaw_deg; + float _setpoint_roll_deg; + float _setpoint_pitch_deg; + float _setpoint_yaw_deg; + float _err_threshold_deg; + bool _error_detected; + uint8_t _mode; }; /** * @brief Mount Control plugin + * @plugin mount_control * * Publishes Mission commands to control the camera or antenna mount. * @see command_cb() */ -class MountControlPlugin : public plugin::PluginBase { +class MountControlPlugin : public plugin::Plugin +{ public: - MountControlPlugin() : PluginBase(), - nh("~"), - mount_diag("Mount"), - mount_nh("~mount_control") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - command_sub = mount_nh.subscribe("command", 10, &MountControlPlugin::command_cb, this); - mount_orientation_pub = mount_nh.advertise("orientation", 10); - mount_status_pub = mount_nh.advertise("status", 10); - configure_srv = mount_nh.advertiseService("configure", &MountControlPlugin::mount_configure_cb, this); - - // some gimbals send negated/inverted angle measurements - // these parameters correct that to obey the MAVLink frame convention - mount_nh.param("negate_measured_roll", negate_measured_roll, false); - mount_nh.param("negate_measured_pitch", negate_measured_pitch, false); - mount_nh.param("negate_measured_yaw", negate_measured_yaw, false); - if (!mount_nh.getParam("negate_measured_roll", negate_measured_roll)) { - ROS_WARN("Could not retrive negate_measured_roll parameter value, using default (%d)", negate_measured_roll); - } - if (!mount_nh.getParam("negate_measured_pitch", negate_measured_pitch)) { - ROS_WARN("Could not retrive negate_measured_pitch parameter value, using default (%d)", negate_measured_pitch); - } - if (!mount_nh.getParam("negate_measured_yaw", negate_measured_yaw)) { - ROS_WARN("Could not retrive negate_measured_yaw parameter value, using default (%d)", negate_measured_yaw); - } - - bool disable_diag; - if (nh.getParam("sys/disable_diag", disable_diag) && !disable_diag) { - double debounce_s; - double err_threshold_deg; - mount_nh.param("debounce_s", debounce_s, 4.0); - mount_nh.param("err_threshold_deg", err_threshold_deg, 10.0); - if (!mount_nh.getParam("debounce_s", debounce_s)) { - ROS_WARN("Could not retrive debounce_s parameter value, using default (%f)", debounce_s); - } - if (!mount_nh.getParam("err_threshold_deg", err_threshold_deg)) { - ROS_WARN("Could not retrive err_threshold_deg parameter value, using default (%f)", err_threshold_deg); - } - mount_diag.set_debounce_s(debounce_s); - mount_diag.set_err_threshold_deg(err_threshold_deg); - UAS_DIAG(m_uas).add(mount_diag); - } - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&MountControlPlugin::handle_mount_orientation), - make_handler(&MountControlPlugin::handle_mount_status) - }; - } + explicit MountControlPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "mount_control"), + mount_diag("Mount") + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "negate_measured_roll", false, [&](const rclcpp::Parameter & p) { + negate_measured_roll = p.as_bool(); + }); + node_declare_and_watch_parameter( + "negate_measured_pitch", false, [&](const rclcpp::Parameter & p) { + negate_measured_pitch = p.as_bool(); + }); + node_declare_and_watch_parameter( + "negate_measured_yaw", false, [&](const rclcpp::Parameter & p) { + negate_measured_yaw = p.as_bool(); + }); + + node_declare_and_watch_parameter( + "debounce_s", 4.0, [&](const rclcpp::Parameter & p) { + auto debounce_s = p.as_double(); + mount_diag.set_debounce_s(debounce_s); + }); + node_declare_and_watch_parameter( + "err_threshold_deg", 10.0, [&](const rclcpp::Parameter & p) { + auto err_threshold_deg = p.as_double(); + mount_diag.set_err_threshold_deg(err_threshold_deg); + }); + node_declare_and_watch_parameter( + "disable_diag", false, [&](const rclcpp::Parameter & p) { + auto disable_diag = p.as_bool(); + + if (!disable_diag) { + uas->diagnostic_updater.add(mount_diag); + } else { + uas->diagnostic_updater.removeByName(mount_diag.getName()); + } + }); + + command_sub = node->create_subscription( + "~/command", 10, std::bind( + &MountControlPlugin::command_cb, this, + _1)); + + mount_orientation_pub = node->create_publisher( + "~/orientation", + 10); + mount_status_pub = node->create_publisher("~/status", 10); + + configure_srv = node->create_service( + "~/configure", std::bind( + &MountControlPlugin::mount_configure_cb, + this, _1, _2)); + } + + + Subscriptions get_subscriptions() override + { + return { + make_handler(&MountControlPlugin::handle_mount_orientation), + make_handler(&MountControlPlugin::handle_mount_status) + }; + } private: - ros::NodeHandle nh; - ros::NodeHandle mount_nh; - ros::Subscriber command_sub; - ros::Publisher mount_orientation_pub; - ros::Publisher mount_status_pub; - ros::ServiceServer configure_srv; - - MountStatusDiag mount_diag; - bool negate_measured_roll; - bool negate_measured_pitch; - bool negate_measured_yaw; - - /** - * @brief Publish the mount orientation - * - * Message specification: https://mavlink.io/en/messages/common.html#MOUNT_ORIENTATION - * @param msg the mavlink message - * @param mo received MountOrientation msg - */ - void handle_mount_orientation(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MOUNT_ORIENTATION &mo) - { - const auto timestamp = ros::Time::now(); - // some gimbals send negated/inverted angle measurements, correct that to obey the MAVLink frame convention - if (negate_measured_roll) { - mo.roll = -mo.roll; - } - if (negate_measured_pitch) { - mo.pitch = -mo.pitch; - } - if (negate_measured_yaw) { - mo.yaw = -mo.yaw; - mo.yaw_absolute = -mo.yaw_absolute; - } - const auto q = ftf::quaternion_from_rpy(Eigen::Vector3d(mo.roll, mo.pitch, mo.yaw) * M_PI / 180.0); - geometry_msgs::Quaternion quaternion_msg; - tf::quaternionEigenToMsg(q, quaternion_msg); - mount_orientation_pub.publish(quaternion_msg); - - mount_diag.set_status(mo.roll, mo.pitch, mo.yaw_absolute, timestamp); - } - - /** - * @brief Publish the mount status - * - * @param msg the mavlink message - * @param ms received MountStatus msg - */ - void handle_mount_status(const mavlink::mavlink_message_t *, mavlink::ardupilotmega::msg::MOUNT_STATUS &ms) - { - geometry_msgs::Vector3Stamped publish_msg; - - publish_msg.header.stamp = ros::Time::now(); - - publish_msg.header.frame_id = std::to_string(ms.target_component); - - auto vec = Eigen::Vector3d(ms.pointing_b, ms.pointing_a, ms.pointing_c) * M_PI / 18000.0; - tf::vectorEigenToMsg(vec, publish_msg.vector); - - mount_status_pub.publish(publish_msg); - - // pointing_X is cdeg - auto q = ftf::quaternion_from_rpy(Eigen::Vector3d(ms.pointing_b, ms.pointing_a, ms.pointing_c) * M_PI / 18000.0); - geometry_msgs::Quaternion quaternion_msg; - tf::quaternionEigenToMsg(q, quaternion_msg); - mount_orientation_pub.publish(quaternion_msg); - } - - /** - * @brief Send mount control commands to vehicle - * - * Message specification: https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONTROL - * @param req received MountControl msg - */ - void command_cb(const mavros_msgs::MountControl::ConstPtr &req) - { - mavlink::common::msg::COMMAND_LONG cmd {}; - - cmd.target_system = m_uas->get_tgt_system(); - cmd.target_component = m_uas->get_tgt_component(); - cmd.command = enum_value(MAV_CMD::DO_MOUNT_CONTROL); - cmd.param1 = req->pitch; - cmd.param2 = req->roll; - cmd.param3 = req->yaw; - cmd.param4 = req->altitude; // - cmd.param5 = req->latitude; // latitude in degrees * 1E7 - cmd.param6 = req->longitude; // longitude in degrees * 1E7 - cmd.param7 = req->mode; // MAV_MOUNT_MODE - - UAS_FCU(m_uas)->send_message_ignore_drop(cmd); - - mount_diag.set_setpoint(req->roll*0.01f, req->pitch*0.01f, req->yaw*0.01f, req->mode); - } - - bool mount_configure_cb(mavros_msgs::MountConfigure::Request &req, - mavros_msgs::MountConfigure::Response &res) - { - using mavlink::common::MAV_CMD; - - try { - auto client = nh.serviceClient("cmd/command"); - - mavros_msgs::CommandLong cmd{}; - - cmd.request.broadcast = false; - cmd.request.command = enum_value(MAV_CMD::DO_MOUNT_CONFIGURE); - cmd.request.confirmation = false; - cmd.request.param1 = req.mode; - cmd.request.param2 = req.stabilize_roll; - cmd.request.param3 = req.stabilize_pitch; - cmd.request.param4 = req.stabilize_yaw; - cmd.request.param5 = req.roll_input; - cmd.request.param6 = req.pitch_input; - cmd.request.param7 = req.yaw_input; - - ROS_DEBUG_NAMED("mount", "MountConfigure: Request mode %u ", req.mode); - client.call(cmd); - res.success = cmd.response.success; - } - catch (ros::InvalidNameException &ex) { - ROS_ERROR_NAMED("mount", "MountConfigure: %s", ex.what()); - } - - ROS_ERROR_COND_NAMED(!res.success, "mount", "MountConfigure: command plugin service call failed!"); - - return res.success; - } + rclcpp::Subscription::SharedPtr command_sub; + + rclcpp::Publisher::SharedPtr mount_orientation_pub; + rclcpp::Publisher::SharedPtr mount_status_pub; + + rclcpp::Service::SharedPtr configure_srv; + + MountStatusDiag mount_diag; + bool negate_measured_roll; + bool negate_measured_pitch; + bool negate_measured_yaw; + + /** + * @brief Publish the mount orientation + * + * Message specification: https://mavlink.io/en/messages/common.html#MOUNT_ORIENTATION + * @param msg the mavlink message + * @param mo received MountOrientation msg + */ + void handle_mount_orientation( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::MOUNT_ORIENTATION & mo, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + const auto timestamp = node->now(); + // some gimbals send negated/inverted angle measurements, + // correct that to obey the MAVLink frame convention + if (negate_measured_roll) { + mo.roll = -mo.roll; + } + if (negate_measured_pitch) { + mo.pitch = -mo.pitch; + } + if (negate_measured_yaw) { + mo.yaw = -mo.yaw; + mo.yaw_absolute = -mo.yaw_absolute; + } + + auto q = ftf::quaternion_from_rpy(Eigen::Vector3d(mo.roll, mo.pitch, mo.yaw) * M_PI / 180.0); + + geometry_msgs::msg::Quaternion quaternion_msg = tf2::toMsg(q); + + mount_orientation_pub->publish(quaternion_msg); + mount_diag.set_status(mo.roll, mo.pitch, mo.yaw_absolute, timestamp); + } + + /** + * @brief Publish the mount status + * + * @param msg the mavlink message + * @param ms received MountStatus msg + */ + void handle_mount_status( + const mavlink::mavlink_message_t * mmsg [[maybe_unused]], + mavlink::ardupilotmega::msg::MOUNT_STATUS & ms, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + geometry_msgs::msg::Vector3Stamped publish_msg; + + publish_msg.header.stamp = node->now(); + publish_msg.header.frame_id = std::to_string(ms.target_component); + + auto vec = Eigen::Vector3d(ms.pointing_b, ms.pointing_a, ms.pointing_c) * M_PI / 18000.0; + tf2::toMsg(vec, publish_msg.vector); + + mount_status_pub->publish(publish_msg); + + // pointing_X is cdeg + auto q = ftf::quaternion_from_rpy( + Eigen::Vector3d( + ms.pointing_b, ms.pointing_a, + ms.pointing_c) * M_PI / 18000.0); + geometry_msgs::msg::Quaternion quaternion_msg = tf2::toMsg(q); + mount_orientation_pub->publish(quaternion_msg); + } + + /** + * @brief Send mount control commands to vehicle + * + * Message specification: https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONTROL + * @param req received MountControl msg + */ + void command_cb(const mavros_msgs::msg::MountControl::SharedPtr req) + { + mavlink::common::msg::COMMAND_LONG cmd {}; + + uas->msg_set_target(cmd); + cmd.command = enum_value(MAV_CMD::DO_MOUNT_CONTROL); + cmd.param1 = req->pitch; + cmd.param2 = req->roll; + cmd.param3 = req->yaw; + cmd.param4 = req->altitude; // + cmd.param5 = req->latitude; // latitude in degrees * 1E7 + cmd.param6 = req->longitude; // longitude in degrees * 1E7 + cmd.param7 = req->mode; // MAV_MOUNT_MODE + + uas->send_message(cmd); + + mount_diag.set_setpoint(req->roll * 0.01f, req->pitch * 0.01f, req->yaw * 0.01f, req->mode); + } + + void mount_configure_cb( + mavros_msgs::srv::MountConfigure::Request::SharedPtr req, + mavros_msgs::srv::MountConfigure::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + + try { + auto client = node->create_client("cmd/command"); + + auto cmdrq = std::make_shared(); + + cmdrq->broadcast = false; + cmdrq->command = enum_value(MAV_CMD::DO_MOUNT_CONFIGURE); + cmdrq->confirmation = false; + cmdrq->param1 = req->mode; + cmdrq->param2 = req->stabilize_roll; + cmdrq->param3 = req->stabilize_pitch; + cmdrq->param4 = req->stabilize_yaw; + cmdrq->param5 = req->roll_input; + cmdrq->param6 = req->pitch_input; + cmdrq->param7 = req->yaw_input; + + RCLCPP_DEBUG(get_logger(), "MountConfigure: Request mode %u ", req->mode); + auto future = client->async_send_request(cmdrq); + auto response = future.get(); + res->success = response->success; + } catch (std::exception & ex) { + RCLCPP_ERROR(get_logger(), "MountConfigure: %s", ex.what()); + } + + RCLCPP_ERROR_EXPRESSION( + get_logger(), !res->success, "MountConfigure: command plugin service call failed!"); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::MountControlPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::MountControlPlugin) diff --git a/mavros_extras/src/plugins/obstacle_distance.cpp b/mavros_extras/src/plugins/obstacle_distance.cpp index c555f5aea..b73b389c4 100644 --- a/mavros_extras/src/plugins/obstacle_distance.cpp +++ b/mavros_extras/src/plugins/obstacle_distance.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2017 Nuno Marques. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Obstacle distance plugin * @file obstacle_distance.cpp @@ -6,21 +13,22 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2017 Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.mdA - */ -#include -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "sensor_msgs/msg/laser_scan.hpp" -#include +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace extra_plugins { //! Radians to degrees static constexpr double RAD_TO_DEG = 180.0 / M_PI; //! Mavlink MAV_DISTANCE_SENSOR enumeration @@ -28,99 +36,110 @@ using mavlink::common::MAV_DISTANCE_SENSOR; /** * @brief Obstacle distance plugin + * @plugin obstacle_distance * * Publishes obstacle distance array to the FCU, in order to assist in an obstacle * avoidance flight. * @see obstacle_cb() */ -class ObstacleDistancePlugin : public plugin::PluginBase { +class ObstacleDistancePlugin : public plugin::Plugin +{ public: - ObstacleDistancePlugin() : PluginBase(), - obstacle_nh("~obstacle") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - std::string mav_frame; - obstacle_nh.param("mav_frame", mav_frame, "GLOBAL"); - frame = utils::mav_frame_from_str(mav_frame); - - obstacle_sub = obstacle_nh.subscribe("send", 10, &ObstacleDistancePlugin::obstacle_cb, this); - } - - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + explicit ObstacleDistancePlugin(plugin::UASPtr uas_) + : Plugin(uas_, "obstacle") + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "mav_frame", "GLOBAL", [&](const rclcpp::Parameter & p) { + auto mav_frame = p.as_string(); + frame = utils::mav_frame_from_str(mav_frame); + // MAV_FRAME index based on given frame name (If unknown, defaults to GENERIC) + }); + + obstacle_sub = + node->create_subscription( + "~/send", 10, + std::bind(&ObstacleDistancePlugin::obstacle_cb, this, _1)); + } + + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - ros::NodeHandle obstacle_nh; - ros::Subscriber obstacle_sub; - - mavlink::common::MAV_FRAME frame; - - /** - * @brief Send obstacle distance array to the FCU. - * - * Message specification: https://mavlink.io/en/messages/common.html#OBSTACLE_DISTANCE - * @param req received ObstacleDistance msg - */ - void obstacle_cb(const sensor_msgs::LaserScan::ConstPtr &req) - { - mavlink::common::msg::OBSTACLE_DISTANCE obstacle {}; - - if (req->ranges.size() <= obstacle.distances.size()) { - // all distances from sensor will fit in obstacle distance message - for (int i = 0; i < req->ranges.size(); i++) { - float distance_cm = req->ranges[i] * 1e2; - if (std::isnan(distance_cm) || distance_cm >= UINT16_MAX || distance_cm < 0) { - obstacle.distances[i] = UINT16_MAX; - } else { - obstacle.distances[i] = static_cast(distance_cm); - } - } - std::fill(obstacle.distances.begin() + req->ranges.size(), obstacle.distances.end(), UINT16_MAX); //!< fill the rest of the array values as "Unknown" - - const float increment_deg = req->angle_increment * RAD_TO_DEG; - obstacle.increment = static_cast(increment_deg + 0.5f); //!< Round to nearest integer. - obstacle.increment_f = increment_deg; - } else { - // all distances from sensor will not fit so we combine adjacent distances always taking the shortest distance - const float scale_factor = double(req->ranges.size()) / obstacle.distances.size(); - for (size_t i = 0; i < obstacle.distances.size(); i++) { - obstacle.distances[i] = UINT16_MAX; - for (size_t j = 0; j < scale_factor; j++) { - size_t req_index = floor(i * scale_factor + j); - float distance_cm = req->ranges[req_index] * 1e2; - if (!std::isnan(distance_cm) && distance_cm < UINT16_MAX && distance_cm > 0) { - obstacle.distances[i] = std::min(obstacle.distances[i], static_cast(distance_cm)); - } - } - } - const float increment_deg = req->angle_increment * RAD_TO_DEG * scale_factor; - obstacle.increment = static_cast(increment_deg + 0.5f); //!< Round to nearest integer. - obstacle.increment_f = increment_deg; - - } - - obstacle.time_usec = req->header.stamp.toNSec() / 1000; //!< [microsecs] - obstacle.sensor_type = utils::enum_value(MAV_DISTANCE_SENSOR::LASER); //!< defaults is laser type (depth sensor, Lidar) - obstacle.min_distance = req->range_min * 1e2; //!< [centimeters] - obstacle.max_distance = req->range_max * 1e2; //!< [centimeters] - obstacle.frame = utils::enum_value(frame); - // Assume angle_increment is positive and incoming message is in a FRD/NED frame - obstacle.angle_offset = req->angle_min * RAD_TO_DEG; //!< [degrees] - - ROS_DEBUG_STREAM_NAMED("obstacle_distance", "OBSDIST: sensor type: " << utils::to_string_enum(obstacle.sensor_type) - << std::endl << obstacle.to_yaml()); - - UAS_FCU(m_uas)->send_message_ignore_drop(obstacle); - } + rclcpp::Subscription::SharedPtr obstacle_sub; + + mavlink::common::MAV_FRAME frame; + + /** + * @brief Send obstacle distance array to the FCU. + * + * Message specification: https://mavlink.io/en/messages/common.html#OBSTACLE_DISTANCE + * @param req received ObstacleDistance msg + */ + void obstacle_cb(const sensor_msgs::msg::LaserScan::SharedPtr req) + { + mavlink::common::msg::OBSTACLE_DISTANCE obstacle {}; + + if (req->ranges.size() <= obstacle.distances.size()) { + // all distances from sensor will fit in obstacle distance message + for (size_t i = 0; i < req->ranges.size(); i++) { + float distance_cm = req->ranges[i] * 1e2; + if (std::isnan(distance_cm) || distance_cm >= UINT16_MAX || distance_cm < 0) { + obstacle.distances[i] = UINT16_MAX; + } else { + obstacle.distances[i] = static_cast(distance_cm); + } + } + std::fill( + obstacle.distances.begin() + req->ranges.size(), + obstacle.distances.end(), UINT16_MAX); //!< fill the rest of the array values as "Unknown" + + const float increment_deg = req->angle_increment * RAD_TO_DEG; + obstacle.increment = static_cast(increment_deg + 0.5f); //!< Round to nearest int + obstacle.increment_f = increment_deg; + } else { + // all distances from sensor will not fit so we combine adjacent + // distances always taking the shortest distance + const float scale_factor = static_cast(req->ranges.size()) / + obstacle.distances.size(); + for (size_t i = 0; i < obstacle.distances.size(); i++) { + obstacle.distances[i] = UINT16_MAX; + for (size_t j = 0; j < scale_factor; j++) { + size_t req_index = floor(i * scale_factor + j); + float distance_cm = req->ranges[req_index] * 1e2; + if (!std::isnan(distance_cm) && distance_cm < UINT16_MAX && distance_cm > 0) { + obstacle.distances[i] = + std::min(obstacle.distances[i], static_cast(distance_cm)); + } + } + } + const float increment_deg = req->angle_increment * RAD_TO_DEG * scale_factor; + obstacle.increment = static_cast(increment_deg + 0.5f); //!< Round to nearest int + obstacle.increment_f = increment_deg; + } + + obstacle.time_usec = get_time_usec(req->header.stamp); //!< [microsecs] + obstacle.sensor_type = utils::enum_value(MAV_DISTANCE_SENSOR::LASER); //!< defaults is laser + obstacle.min_distance = req->range_min * 1e2; //!< [centimeters] + obstacle.max_distance = req->range_max * 1e2; //!< [centimeters] + obstacle.frame = utils::enum_value(frame); + // Assume angle_increment is positive and incoming message is in a FRD/NED frame + obstacle.angle_offset = req->angle_min * RAD_TO_DEG; //!< [degrees] + + RCLCPP_DEBUG_STREAM( + get_logger(), + "OBSDIST: sensor type: " << + utils::to_string_enum(obstacle.sensor_type) << + std::endl << obstacle.to_yaml()); + + uas->send_message(obstacle); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::ObstacleDistancePlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::ObstacleDistancePlugin) diff --git a/mavros_extras/src/plugins/odom.cpp b/mavros_extras/src/plugins/odom.cpp index aa0e81f0a..00ddf8f04 100644 --- a/mavros_extras/src/plugins/odom.cpp +++ b/mavros_extras/src/plugins/odom.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2017 James Goppert + * Copyright 2017,2018 Nuno Marques + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Odometry plugin * @file odom.cpp @@ -7,286 +15,319 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2017 James Goppert - * Copyright 2017,2018 Nuno Marques - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include #include -#include -#include -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" -namespace mavros { -namespace extra_plugins { +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT using mavlink::common::MAV_FRAME; using mavlink::common::MAV_ESTIMATOR_TYPE; using Matrix6d = Eigen::Matrix; /** * @brief Odometry plugin + * @plugin odometry * * Sends odometry data to the FCU estimator and * publishes odometry data that comes from FCU. * - * This plugin is following ROS REP 147. Pose is expressed in parent frame. (Quaternion rotates from child to parent) + * This plugin is following ROS REP 147. Pose is expressed in parent frame. + * (Quaternion rotates from child to parent) * The twist is expressed in the child frame. * * @see odom_cb() transforming and sending odometry to fcu * @see handle_odom() receiving and transforming odometry from fcu */ -class OdometryPlugin : public plugin::PluginBase { +class OdometryPlugin : public plugin::Plugin +{ public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW // XXX(vooon): added to try to fix #1223. Not sure that it is needed because class do not have Eigen:: fields. - - OdometryPlugin() : PluginBase(), - odom_nh("~odometry"), - fcu_odom_parent_id_des("map"), - fcu_odom_child_id_des("base_link") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - // frame params: - odom_nh.param("fcu/odom_parent_id_des", fcu_odom_parent_id_des, "map"); - odom_nh.param("fcu/odom_child_id_des", fcu_odom_child_id_des, "base_link"); - - // publishers - odom_pub = odom_nh.advertise("in", 10); - - // subscribers - odom_sub = odom_nh.subscribe("out", 1, &OdometryPlugin::odom_cb, this); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&OdometryPlugin::handle_odom) - }; - } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit OdometryPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "odometry"), + fcu_odom_parent_id_des("map"), + fcu_odom_child_id_des("base_link") + { + enable_node_watch_parameters(); + + // frame params: + node_declare_and_watch_parameter( + "fcu.odom_parent_id_des", "map", [&](const rclcpp::Parameter & p) { + fcu_odom_parent_id_des = p.as_string(); + }); + node_declare_and_watch_parameter( + "fcu.odom_child_id_des", "map", [&](const rclcpp::Parameter & p) { + fcu_odom_child_id_des = p.as_string(); + }); + + // publishers + odom_pub = node->create_publisher("~/in", 10); + + // subscribers + odom_sub = + node->create_subscription( + "~/out", 1, + std::bind(&OdometryPlugin::odom_cb, this, _1)); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&OdometryPlugin::handle_odom) + }; + } private: - ros::NodeHandle odom_nh; //!< node handler - ros::Publisher odom_pub; //!< nav_msgs/Odometry publisher - ros::Subscriber odom_sub; //!< nav_msgs/Odometry subscriber - - std::string fcu_odom_parent_id_des; //!< desired orientation of the fcu odometry message's parent frame - std::string fcu_odom_child_id_des; //!< desired orientation of the fcu odometry message's child frame - - /** - * @brief Lookup static transform with error handling - * @param[in] &target The parent frame of the transformation you want to get - * @param[in] &source The child frame of the transformation you want to get - * @param[in,out] &tf_source2target The affine transform from the source to target - */ - void lookup_static_transform(const std::string &target, const std::string &source, - Eigen::Affine3d &tf_source2target) - { - try { - // transform lookup at current time. - tf_source2target = tf2::transformToEigen(m_uas->tf2_buffer.lookupTransform( - target, source, ros::Time(0))); - } catch (tf2::TransformException &ex) { - ROS_ERROR_THROTTLE_NAMED(1, "odom", "ODOM: Ex: %s", ex.what()); - return; - } - } - - /** - * @brief Handle ODOMETRY MAVlink message. - * - * Message specification: https://mavlink.io/en/messages/common.html#ODOMETRY - * - * Callback for mavlink ODOMETRY messages sent from the FCU. According to the mavlink specification, - * all quantities are for the child frame (fcu_frd), expressed in the parent frame (local_origin_ned). - * To be compliant with ROS REP 147 for the published nav_msgs/Odometry, the data will be appropriately - * transformed and published. Frames for the publish message should be specified in specified - * in the rosparams "odometry/fcu/odom_*_id_des" (set in px4_config.yaml). - * - * @param msg Received Mavlink msg - * @param odom_msg ODOMETRY msg - */ - void handle_odom(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ODOMETRY &odom_msg) - { - /** - * Required rotations to transform the FCU's odometry msg tto desired parent and child frame - */ - Eigen::Affine3d tf_parent2parent_des; - Eigen::Affine3d tf_child2child_des; - - lookup_static_transform(fcu_odom_parent_id_des, "map_ned", tf_parent2parent_des); - lookup_static_transform( fcu_odom_child_id_des, "base_link_frd", tf_child2child_des); - - //! Build 6x6 pose covariance matrix to be transformed and sent - Matrix6d cov_pose = Matrix6d::Zero(); - ftf::mavlink_urt_to_covariance_matrix(odom_msg.pose_covariance, cov_pose); - - //! Build 6x6 velocity covariance matrix to be transformed and sent - Matrix6d cov_vel = Matrix6d::Zero(); - ftf::mavlink_urt_to_covariance_matrix(odom_msg.velocity_covariance, cov_vel); - - Eigen::Vector3d position {}; //!< Position vector. WRT frame_id - Eigen::Quaterniond orientation {}; //!< Attitude quaternion. WRT frame_id - Eigen::Vector3d lin_vel {}; //!< Linear velocity vector. WRT child_frame_id - Eigen::Vector3d ang_vel {}; //!< Angular velocity vector. WRT child_frame_id - Matrix6d r_pose = Matrix6d::Zero(); //!< Zero initialized pose 6-D Covariance matrix. WRT frame_id - Matrix6d r_vel = Matrix6d::Zero(); //!< Zero initialized velocity 6-D Covariance matrix. WRT child_frame_id - - auto odom = boost::make_shared(); - - odom->header = m_uas->synchronized_header(fcu_odom_parent_id_des, odom_msg.time_usec); - odom->child_frame_id = fcu_odom_child_id_des; - - /** - * Position parsing to desired parent - */ - position = Eigen::Vector3d(tf_parent2parent_des.linear() * Eigen::Vector3d(odom_msg.x, odom_msg.y, odom_msg.z)); - tf::pointEigenToMsg(position, odom->pose.pose.position); - - /** - * Orientation parsing. Quaternion has to be the rotation from desired child frame to desired parent frame - */ - Eigen::Quaterniond q_child2parent(ftf::mavlink_to_quaternion(odom_msg.q)); - Eigen::Affine3d tf_childDes2parentDes = tf_parent2parent_des * q_child2parent * tf_child2child_des.inverse(); - orientation = Eigen::Quaterniond(tf_childDes2parentDes.linear()); - tf::quaternionEigenToMsg(orientation, odom->pose.pose.orientation); - - /** - * Velocities parsing - * Linear and angular velocities are transforned to the desired child_frame. - */ - lin_vel = Eigen::Vector3d(tf_child2child_des.linear() * Eigen::Vector3d(odom_msg.vx, odom_msg.vy, odom_msg.vz)); - ang_vel = Eigen::Vector3d(tf_child2child_des.linear() * Eigen::Vector3d(odom_msg.rollspeed, odom_msg.pitchspeed, odom_msg.yawspeed)); - tf::vectorEigenToMsg(lin_vel, odom->twist.twist.linear); - tf::vectorEigenToMsg(ang_vel, odom->twist.twist.angular); - - /** - * Covariances parsing - */ - //! Transform pose covariance matrix - r_pose.block<3, 3>(0, 0) = r_pose.block<3, 3>(3, 3) = tf_parent2parent_des.linear(); - cov_pose = r_pose * cov_pose * r_pose.transpose(); - Eigen::Map(odom->pose.covariance.data(), cov_pose.rows(), cov_pose.cols()) = cov_pose; - - //! Transform twist covariance matrix - r_vel.block<3, 3>(0, 0) = r_vel.block<3, 3>(3, 3) = tf_child2child_des.linear(); - cov_vel = r_vel * cov_vel * r_vel.transpose(); - Eigen::Map(odom->twist.covariance.data(), cov_vel.rows(), cov_vel.cols()) = cov_vel; - - //! Publish the data - odom_pub.publish(odom); - } - - /** - * @brief Sends odometry data msgs to the FCU. - * - * Callback to odometry that should go to FCU. The frame_ids in the odom message - * have to fit the frames that are are added to the tf tree. The odometry message - * gets rotated such that the parent frame is "odom_ned" and the child frame is "base_link_frd". - * - * Message specification: https://mavlink.io/en/messages/common.html#ODOMETRY - * @param req received Odometry msg - */ - void odom_cb(const nav_msgs::Odometry::ConstPtr &odom) - { - /** - * Required affine rotations to apply transforms - */ - Eigen::Affine3d tf_parent2parent_des; - Eigen::Affine3d tf_child2child_des; - - lookup_static_transform("odom_ned", odom->header.frame_id, tf_parent2parent_des); - lookup_static_transform("base_link_frd", odom->child_frame_id, tf_child2child_des); - - //! Build 6x6 pose covariance matrix to be transformed and sent - ftf::Covariance6d cov_pose = odom->pose.covariance; - ftf::EigenMapCovariance6d cov_pose_map(cov_pose.data()); - - //! Build 6x6 velocity covariance matrix to be transformed and sent - ftf::Covariance6d cov_vel = odom->twist.covariance; - ftf::EigenMapCovariance6d cov_vel_map(cov_vel.data()); - - /** Apply transforms: - * According to nav_msgs/Odometry. - */ - Eigen::Vector3d position {}; //!< Position vector. WRT frame_id - Eigen::Quaterniond orientation {}; //!< Attitude quaternion. WRT frame_id - Eigen::Vector3d lin_vel {}; //!< Linear velocity vector. WRT child_frame_id - Eigen::Vector3d ang_vel {}; //!< Angular velocity vector. WRT child_frame_id - Matrix6d r_pose = Matrix6d::Zero(); //!< Zero initialized pose 6-D Covariance matrix. WRT frame_id - Matrix6d r_vel = Matrix6d::Zero(); //!< Zero initialized velocity 6-D Covariance matrix. WRT child_frame_id - - mavlink::common::msg::ODOMETRY msg {}; - msg.frame_id = utils::enum_value(MAV_FRAME::LOCAL_FRD); - msg.child_frame_id = utils::enum_value(MAV_FRAME::BODY_FRD); - msg.estimator_type = utils::enum_value(MAV_ESTIMATOR_TYPE::VISION); - - /** - * Position parsing from odometry's parent frame to "LOCAL_FRD" frame. - */ - position = Eigen::Vector3d(tf_parent2parent_des.linear() * ftf::to_eigen(odom->pose.pose.position)); - - /** - * Orientation parsing. - */ - Eigen::Quaterniond q_child2parent(ftf::to_eigen(odom->pose.pose.orientation)); - Eigen::Affine3d tf_childDes2parentDes = tf_parent2parent_des * q_child2parent * tf_child2child_des.inverse(); - orientation = Eigen::Quaterniond(tf_childDes2parentDes.linear()); - - /** - * Linear and angular velocities are transformed to base_link_frd - */ - lin_vel = Eigen::Vector3d(tf_child2child_des.linear() * ftf::to_eigen(odom->twist.twist.linear)); - ang_vel = Eigen::Vector3d(tf_child2child_des.linear() * ftf::to_eigen(odom->twist.twist.angular)); - - /** Apply covariance transforms */ - r_pose.block<3, 3>(0, 0) = r_pose.block<3, 3>(3, 3) = tf_parent2parent_des.linear(); - r_vel.block<3, 3>(0, 0) = r_vel.block<3, 3>(3, 3) = tf_child2child_des.linear(); - cov_pose_map = r_pose * cov_pose_map * r_pose.transpose(); - cov_vel_map = r_vel * cov_vel_map * r_vel.transpose(); - - ROS_DEBUG_STREAM_NAMED("odom", "ODOM: output: pose covariance matrix:" << std::endl << cov_pose_map); - ROS_DEBUG_STREAM_NAMED("odom", "ODOM: output: velocity covariance matrix:" << std::endl << cov_vel_map); - - /* -*- ODOMETRY msg parser -*- */ - msg.time_usec = odom->header.stamp.toNSec() / 1e3; - - // [[[cog: - // for a, b in (('', 'position'), ('v', 'lin_vel')): - // for f in 'xyz': - // cog.outl("msg.{a}{f} = {b}.{f}();".format(**locals())) - // for a, b in zip("xyz", ('rollspeed', 'pitchspeed', 'yawspeed')): - // cog.outl("msg.{b} = ang_vel.{a}();".format(**locals())) - // ]]] - msg.x = position.x(); - msg.y = position.y(); - msg.z = position.z(); - msg.vx = lin_vel.x(); - msg.vy = lin_vel.y(); - msg.vz = lin_vel.z(); - msg.rollspeed = ang_vel.x(); - msg.pitchspeed = ang_vel.y(); - msg.yawspeed = ang_vel.z(); - // [[[end]]] (checksum: ead24a1a6a14496c9de6c1951ccfbbd7) - - ftf::quaternion_to_mavlink(orientation, msg.q); - ftf::covariance_urt_to_mavlink(cov_pose_map, msg.pose_covariance); - ftf::covariance_urt_to_mavlink(cov_vel_map, msg.velocity_covariance); - - // send ODOMETRY msg - UAS_FCU(m_uas)->send_message_ignore_drop(msg); - } + rclcpp::Publisher::SharedPtr odom_pub; + rclcpp::Subscription::SharedPtr odom_sub; + + //!< desired orientation of the fcu odometry message's parent frame + std::string fcu_odom_parent_id_des; + //!< desired orientation of the fcu odometry message's child frame + std::string fcu_odom_child_id_des; + + /** + * @brief Lookup static transform with error handling + * @param[in] &target The parent frame of the transformation you want to get + * @param[in] &source The child frame of the transformation you want to get + * @param[in,out] &tf_source2target The affine transform from the source to target + */ + void lookup_static_transform( + const std::string & target, const std::string & source, + Eigen::Affine3d & tf_source2target) + { + try { + // transform lookup at current time. + tf_source2target = tf2::transformToEigen( + uas->tf2_buffer.lookupTransform( + target, source, rclcpp::Time(0))); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1, "ODOM: Ex: %s", ex.what()); + return; + } + } + + /** + * @brief Handle ODOMETRY MAVlink message. + * + * Message specification: https://mavlink.io/en/messages/common.html#ODOMETRY + * + * Callback for mavlink ODOMETRY messages sent from the FCU. According to the mavlink specification, + * all quantities are for the child frame (fcu_frd), expressed in the parent frame (local_origin_ned). + * To be compliant with ROS REP 147 for the published nav_msgs/Odometry, the data will be appropriately + * transformed and published. Frames for the publish message should be specified in specified + * in the rosparams "odometry/fcu/odom_*_id_des" (set in px4_config.yaml). + * + * @param msg Received Mavlink msg + * @param odom_msg ODOMETRY msg + */ + void handle_odom( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::ODOMETRY & odom_msg, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + /** + * Required rotations to transform the FCU's odometry msg tto desired parent and child frame + */ + Eigen::Affine3d tf_parent2parent_des; + Eigen::Affine3d tf_child2child_des; + + lookup_static_transform(fcu_odom_parent_id_des, "map_ned", tf_parent2parent_des); + lookup_static_transform(fcu_odom_child_id_des, "base_link_frd", tf_child2child_des); + + //! Build 6x6 pose covariance matrix to be transformed and sent + Matrix6d cov_pose = Matrix6d::Zero(); + ftf::mavlink_urt_to_covariance_matrix(odom_msg.pose_covariance, cov_pose); + + //! Build 6x6 velocity covariance matrix to be transformed and sent + Matrix6d cov_vel = Matrix6d::Zero(); + ftf::mavlink_urt_to_covariance_matrix(odom_msg.velocity_covariance, cov_vel); + + Eigen::Vector3d position {}; //!< Position vector. WRT frame_id + Eigen::Quaterniond orientation {}; //!< Attitude quaternion. WRT frame_id + Eigen::Vector3d lin_vel {}; //!< Linear velocity vector. WRT child_frame_id + Eigen::Vector3d ang_vel {}; //!< Angular velocity vector. WRT child_frame_id + Matrix6d r_pose = Matrix6d::Zero(); //!< Pose 6-D Covariance matrix. WRT frame_id + Matrix6d r_vel = Matrix6d::Zero(); //!< Velocity 6-D Covariance matrix. WRT child_frame_id + + auto odom = nav_msgs::msg::Odometry(); + + odom.header = uas->synchronized_header(fcu_odom_parent_id_des, odom_msg.time_usec); + odom.child_frame_id = fcu_odom_child_id_des; + + /** + * Position parsing to desired parent + */ + position = + Eigen::Vector3d( + tf_parent2parent_des.linear() * + Eigen::Vector3d(odom_msg.x, odom_msg.y, odom_msg.z)); + odom.pose.pose.position = tf2::toMsg(position); + + /** + * Orientation parsing. Quaternion has to be the rotation from desired child frame to desired parent frame + */ + Eigen::Quaterniond q_child2parent(ftf::mavlink_to_quaternion(odom_msg.q)); + Eigen::Affine3d tf_childDes2parentDes = tf_parent2parent_des * q_child2parent * + tf_child2child_des.inverse(); + orientation = Eigen::Quaterniond(tf_childDes2parentDes.linear()); + odom.pose.pose.orientation = tf2::toMsg(orientation); + + /** + * Velocities parsing + * Linear and angular velocities are transforned to the desired child_frame. + */ + lin_vel = + Eigen::Vector3d( + tf_child2child_des.linear() * + Eigen::Vector3d(odom_msg.vx, odom_msg.vy, odom_msg.vz)); + ang_vel = + Eigen::Vector3d( + tf_child2child_des.linear() * + Eigen::Vector3d(odom_msg.rollspeed, odom_msg.pitchspeed, odom_msg.yawspeed)); + tf2::toMsg(lin_vel, odom.twist.twist.linear); + tf2::toMsg(ang_vel, odom.twist.twist.angular); + + /** + * Covariances parsing + */ + //! Transform pose covariance matrix + r_pose.block<3, 3>(0, 0) = r_pose.block<3, 3>(3, 3) = tf_parent2parent_des.linear(); + cov_pose = r_pose * cov_pose * r_pose.transpose(); + Eigen::Map(odom.pose.covariance.data(), cov_pose.rows(), cov_pose.cols()) = cov_pose; + + //! Transform twist covariance matrix + r_vel.block<3, 3>(0, 0) = r_vel.block<3, 3>(3, 3) = tf_child2child_des.linear(); + cov_vel = r_vel * cov_vel * r_vel.transpose(); + Eigen::Map(odom.twist.covariance.data(), cov_vel.rows(), cov_vel.cols()) = cov_vel; + + //! Publish the data + odom_pub->publish(odom); + } + + /** + * @brief Sends odometry data msgs to the FCU. + * + * Callback to odometry that should go to FCU. The frame_ids in the odom message + * have to fit the frames that are are added to the tf tree. The odometry message + * gets rotated such that the parent frame is "odom_ned" and the child frame is "base_link_frd". + * + * Message specification: https://mavlink.io/en/messages/common.html#ODOMETRY + * @param req received Odometry msg + */ + void odom_cb(const nav_msgs::msg::Odometry::SharedPtr odom) + { + /** + * Required affine rotations to apply transforms + */ + Eigen::Affine3d tf_parent2parent_des; + Eigen::Affine3d tf_child2child_des; + + lookup_static_transform("odom_ned", odom->header.frame_id, tf_parent2parent_des); + lookup_static_transform("base_link_frd", odom->child_frame_id, tf_child2child_des); + + //! Build 6x6 pose covariance matrix to be transformed and sent + ftf::Covariance6d cov_pose = odom->pose.covariance; + ftf::EigenMapCovariance6d cov_pose_map(cov_pose.data()); + + //! Build 6x6 velocity covariance matrix to be transformed and sent + ftf::Covariance6d cov_vel = odom->twist.covariance; + ftf::EigenMapCovariance6d cov_vel_map(cov_vel.data()); + + /** Apply transforms: + * According to nav_msgs/Odometry. + */ + Eigen::Vector3d position {}; //!< Position vector. WRT frame_id + Eigen::Quaterniond orientation {}; //!< Attitude quaternion. WRT frame_id + Eigen::Vector3d lin_vel {}; //!< Linear velocity vector. WRT child_frame_id + Eigen::Vector3d ang_vel {}; //!< Angular velocity vector. WRT child_frame_id + Matrix6d r_pose = Matrix6d::Zero(); //!< Pose 6-D Covariance. WRT frame_id + Matrix6d r_vel = Matrix6d::Zero(); //!< Velocity 6-D Covariance. WRT child_frame_id + + mavlink::common::msg::ODOMETRY msg {}; + msg.frame_id = utils::enum_value(MAV_FRAME::LOCAL_FRD); + msg.child_frame_id = utils::enum_value(MAV_FRAME::BODY_FRD); + msg.estimator_type = utils::enum_value(MAV_ESTIMATOR_TYPE::VISION); + + /** + * Position parsing from odometry's parent frame to "LOCAL_FRD" frame. + */ + position = + Eigen::Vector3d(tf_parent2parent_des.linear() * ftf::to_eigen(odom->pose.pose.position)); + + /** + * Orientation parsing. + */ + Eigen::Quaterniond q_child2parent(ftf::to_eigen(odom->pose.pose.orientation)); + Eigen::Affine3d tf_childDes2parentDes = tf_parent2parent_des * q_child2parent * + tf_child2child_des.inverse(); + orientation = Eigen::Quaterniond(tf_childDes2parentDes.linear()); + + /** + * Linear and angular velocities are transformed to base_link_frd + */ + lin_vel = + Eigen::Vector3d(tf_child2child_des.linear() * ftf::to_eigen(odom->twist.twist.linear)); + ang_vel = + Eigen::Vector3d(tf_child2child_des.linear() * ftf::to_eigen(odom->twist.twist.angular)); + + /** Apply covariance transforms */ + r_pose.block<3, 3>(0, 0) = r_pose.block<3, 3>(3, 3) = tf_parent2parent_des.linear(); + r_vel.block<3, 3>(0, 0) = r_vel.block<3, 3>(3, 3) = tf_child2child_des.linear(); + cov_pose_map = r_pose * cov_pose_map * r_pose.transpose(); + cov_vel_map = r_vel * cov_vel_map * r_vel.transpose(); + + RCLCPP_DEBUG_STREAM( + get_logger(), + "ODOM: output: pose covariance matrix:" << std::endl << cov_pose_map); + RCLCPP_DEBUG_STREAM( + get_logger(), + "ODOM: output: velocity covariance matrix:" << std::endl << cov_vel_map); + + /* -*- ODOMETRY msg parser -*- */ + msg.time_usec = get_time_usec(odom->header.stamp); + + // [[[cog: + // for a, b in (('', 'position'), ('v', 'lin_vel')): + // for f in 'xyz': + // cog.outl(f"msg.{a}{f} = {b}.{f}();") + // for a, b in zip("xyz", ('rollspeed', 'pitchspeed', 'yawspeed')): + // cog.outl(f"msg.{b} = ang_vel.{a}();") + // ]]] + msg.x = position.x(); + msg.y = position.y(); + msg.z = position.z(); + msg.vx = lin_vel.x(); + msg.vy = lin_vel.y(); + msg.vz = lin_vel.z(); + msg.rollspeed = ang_vel.x(); + msg.pitchspeed = ang_vel.y(); + msg.yawspeed = ang_vel.z(); + // [[[end]]] (checksum: 47ddd5137f92fe5d094e7bfd7a5282ec) + + ftf::quaternion_to_mavlink(orientation, msg.q); + ftf::covariance_urt_to_mavlink(cov_pose_map, msg.pose_covariance); + ftf::covariance_urt_to_mavlink(cov_vel_map, msg.velocity_covariance); + + // send ODOMETRY msg + uas->send_message(msg); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::OdometryPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::OdometryPlugin) diff --git a/mavros_extras/src/plugins/onboard_computer_status.cpp b/mavros_extras/src/plugins/onboard_computer_status.cpp index 1638170c0..4899fc3fa 100644 --- a/mavros_extras/src/plugins/onboard_computer_status.cpp +++ b/mavros_extras/src/plugins/onboard_computer_status.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2019 Tanja Baumann. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Onboard Computer Status plugin * @file onboard_computer_status.cpp @@ -6,110 +13,111 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2019 Tanja Baumann. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -#include +#include "mavros_msgs/msg/onboard_computer_status.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace extra_plugins { /** * @brief Onboard Computer Status plugin + * @plugin onboard_computer_status * * Publishes the status of the onboard computer * @see status_cb() */ -class OnboardComputerStatusPlugin : public plugin::PluginBase { +class OnboardComputerStatusPlugin : public plugin::Plugin +{ public: - OnboardComputerStatusPlugin() : PluginBase(), - status_nh("~onboard_computer") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); + explicit OnboardComputerStatusPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "onboard_computer") + { + status_sub = node->create_subscription( + "~/status", 10, std::bind( + &OnboardComputerStatusPlugin::status_cb, this, + _1)); + } - status_sub = status_nh.subscribe("status", 10, &OnboardComputerStatusPlugin::status_cb, this); - } - - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - ros::NodeHandle status_nh; - ros::Subscriber status_sub; - - /** - * @brief Send onboard computer status to FCU and groundstation - * - * Message specification: https://mavlink.io/en/messages/common.html#ONBOARD_COMPUTER_STATUS - * @param req received OnboardComputerStatus msg - */ - void status_cb(const mavros_msgs::OnboardComputerStatus::ConstPtr &req) - { - mavlink::common::msg::ONBOARD_COMPUTER_STATUS status {}; - status.time_usec = req->header.stamp.toNSec() / 1000; //!< [microsecs] - // [[[cog: - // for f in ('uptime', - // 'type', - // 'temperature_board', - // 'ram_usage', - // 'ram_total'): - // cog.outl("status.%s = req->%s;" % (f, f)) - // - // for f in ('cpu_cores', - // 'cpu_combined', - // 'gpu_cores', - // 'gpu_combined', - // 'temperature_core', - // 'fan_speed', - // 'storage_type', - // 'storage_usage', - // 'storage_total', - // 'link_type', - // 'link_tx_rate', - // 'link_rx_rate', - // 'link_tx_max', - // 'link_rx_max'): - // cog.outl("std::copy(req->%s.cbegin(), req->%s.cend(), status.%s.begin());" % (f, f, f)) - // ]]] - status.uptime = req->uptime; - status.type = req->type; - status.temperature_board = req->temperature_board; - status.ram_usage = req->ram_usage; - status.ram_total = req->ram_total; - std::copy(req->cpu_cores.cbegin(), req->cpu_cores.cend(), status.cpu_cores.begin()); - std::copy(req->cpu_combined.cbegin(), req->cpu_combined.cend(), status.cpu_combined.begin()); - std::copy(req->gpu_cores.cbegin(), req->gpu_cores.cend(), status.gpu_cores.begin()); - std::copy(req->gpu_combined.cbegin(), req->gpu_combined.cend(), status.gpu_combined.begin()); - std::copy(req->temperature_core.cbegin(), req->temperature_core.cend(), status.temperature_core.begin()); - std::copy(req->fan_speed.cbegin(), req->fan_speed.cend(), status.fan_speed.begin()); - std::copy(req->storage_type.cbegin(), req->storage_type.cend(), status.storage_type.begin()); - std::copy(req->storage_usage.cbegin(), req->storage_usage.cend(), status.storage_usage.begin()); - std::copy(req->storage_total.cbegin(), req->storage_total.cend(), status.storage_total.begin()); - std::copy(req->link_type.cbegin(), req->link_type.cend(), status.link_type.begin()); - std::copy(req->link_tx_rate.cbegin(), req->link_tx_rate.cend(), status.link_tx_rate.begin()); - std::copy(req->link_rx_rate.cbegin(), req->link_rx_rate.cend(), status.link_rx_rate.begin()); - std::copy(req->link_tx_max.cbegin(), req->link_tx_max.cend(), status.link_tx_max.begin()); - std::copy(req->link_rx_max.cbegin(), req->link_rx_max.cend(), status.link_rx_max.begin()); - // [[[end]]] (checksum: 98538293a5932dfb5952d4badd311b39) + rclcpp::Subscription::SharedPtr status_sub; - std::cout << "timestamp: " << status.time_usec << "\n"; + /** + * @brief Send onboard computer status to FCU and groundstation + * + * Message specification: https://mavlink.io/en/messages/common.html#ONBOARD_COMPUTER_STATUS + * @param req received OnboardComputerStatus msg + */ + void status_cb(const mavros_msgs::msg::OnboardComputerStatus::SharedPtr req) + { + mavlink::common::msg::ONBOARD_COMPUTER_STATUS status {}; + status.time_usec = get_time_usec(req->header.stamp); //!< [microsecs] + // [[[cog: + // for f in ('uptime', + // 'type', + // 'temperature_board', + // 'ram_usage', + // 'ram_total'): + // cog.outl("status.%s = req->%s;" % (f, f)) + // + // for f in ('cpu_cores', + // 'cpu_combined', + // 'gpu_cores', + // 'gpu_combined', + // 'temperature_core', + // 'fan_speed', + // 'storage_type', + // 'storage_usage', + // 'storage_total', + // 'link_type', + // 'link_tx_rate', + // 'link_rx_rate', + // 'link_tx_max', + // 'link_rx_max'): + // cog.outl("std::copy(req->%s.cbegin(), req->%s.cend(), status.%s.begin());" % (f, f, f)) + // ]]] + status.uptime = req->uptime; + status.type = req->type; + status.temperature_board = req->temperature_board; + status.ram_usage = req->ram_usage; + status.ram_total = req->ram_total; + std::copy(req->cpu_cores.cbegin(), req->cpu_cores.cend(), status.cpu_cores.begin()); + std::copy(req->cpu_combined.cbegin(), req->cpu_combined.cend(), status.cpu_combined.begin()); + std::copy(req->gpu_cores.cbegin(), req->gpu_cores.cend(), status.gpu_cores.begin()); + std::copy(req->gpu_combined.cbegin(), req->gpu_combined.cend(), status.gpu_combined.begin()); + std::copy( + req->temperature_core.cbegin(), + req->temperature_core.cend(), status.temperature_core.begin()); + std::copy(req->fan_speed.cbegin(), req->fan_speed.cend(), status.fan_speed.begin()); + std::copy(req->storage_type.cbegin(), req->storage_type.cend(), status.storage_type.begin()); + std::copy(req->storage_usage.cbegin(), req->storage_usage.cend(), status.storage_usage.begin()); + std::copy(req->storage_total.cbegin(), req->storage_total.cend(), status.storage_total.begin()); + std::copy(req->link_type.cbegin(), req->link_type.cend(), status.link_type.begin()); + std::copy(req->link_tx_rate.cbegin(), req->link_tx_rate.cend(), status.link_tx_rate.begin()); + std::copy(req->link_rx_rate.cbegin(), req->link_rx_rate.cend(), status.link_rx_rate.begin()); + std::copy(req->link_tx_max.cbegin(), req->link_tx_max.cend(), status.link_tx_max.begin()); + std::copy(req->link_rx_max.cbegin(), req->link_rx_max.cend(), status.link_rx_max.begin()); + // [[[end]]] (checksum: b06efca90d9a1160c16350e0f0a0f060) - UAS_FCU(m_uas)->send_message_ignore_drop(status, req->component); - } + uas->send_message(status, req->component); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::OnboardComputerStatusPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::OnboardComputerStatusPlugin) diff --git a/mavros_extras/src/plugins/play_tune.cpp b/mavros_extras/src/plugins/play_tune.cpp index 1a775bbf5..3f0894f21 100644 --- a/mavros_extras/src/plugins/play_tune.cpp +++ b/mavros_extras/src/plugins/play_tune.cpp @@ -1,41 +1,71 @@ -#include -#include +/* + * Copyright 2020 Morten Fyhn Amundsen + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief Onboard Computer Status plugin + * @file onboard_computer_status.cpp + * @author Morten Fyhn Amundsen + * + * @addtogroup plugin + * @{ + */ + #include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/play_tune_v2.hpp" + namespace mavros { namespace extra_plugins { -class PlayTunePlugin : public plugin::PluginBase +using namespace std::placeholders; // NOLINT + +/** + * @brief Play Tune service + * @plugin play_tune + */ +class PlayTunePlugin : public plugin::Plugin { public: - PlayTunePlugin() : PluginBase(), nh("~") {} + explicit PlayTunePlugin(plugin::UASPtr uas_) + : Plugin(uas_, "play_tune") + { + sub = + node->create_subscription( + "play_tune", 1, + std::bind(&PlayTunePlugin::callback, this, _1)); + } - void initialize(UAS& uas_) override - { - PluginBase::initialize(uas_); - sub = nh.subscribe("play_tune", 1, &PlayTunePlugin::callback, this); - } - - Subscriptions get_subscriptions() override { - return { /* No subscriptions */ }; - } + Subscriptions get_subscriptions() override + { + return { /* No subscriptions */}; + } private: - ros::NodeHandle nh; - ros::Subscriber sub; - - void callback(const mavros_msgs::PlayTuneV2::ConstPtr& tune) - { - auto msg = mavlink::common::msg::PLAY_TUNE_V2{}; - m_uas->msg_set_target(msg); - msg.format = tune->format; - mavlink::set_string_z(msg.tune, tune->tune); - UAS_FCU(m_uas)->send_message_ignore_drop(msg); - } + rclcpp::Subscription::SharedPtr sub; + + void callback(const mavros_msgs::msg::PlayTuneV2::SharedPtr tune) + { + auto msg = mavlink::common::msg::PLAY_TUNE_V2{}; + + uas->msg_set_target(msg); + msg.format = tune->format; + mavlink::set_string_z(msg.tune, tune->tune); + + uas->send_message(msg); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::PlayTunePlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::PlayTunePlugin) diff --git a/mavros_extras/src/plugins/px4flow.cpp b/mavros_extras/src/plugins/px4flow.cpp index 96e74112b..c0607ac60 100644 --- a/mavros_extras/src/plugins/px4flow.cpp +++ b/mavros_extras/src/plugins/px4flow.cpp @@ -1,3 +1,11 @@ +/* + * Copyright 2014 M.H.Kabir. + * Copyright 2016,2021 Vladimir Ermakov + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief PX4Flow plugin * @file px4flow.cpp @@ -7,185 +15,203 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014 M.H.Kabir. - * Copyright 2016 Vladimir Ermakov - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/optical_flow_rad.hpp" +#include "sensor_msgs/msg/temperature.hpp" +#include "sensor_msgs/msg/range.hpp" -#include -#include -#include +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace extra_plugins { /** * @brief PX4 Optical Flow plugin + * @plugin px4flow * * This plugin can publish data from PX4Flow camera to ROS */ -class PX4FlowPlugin : public plugin::PluginBase { +class PX4FlowPlugin : public plugin::Plugin +{ public: - PX4FlowPlugin() : PluginBase(), - flow_nh("~px4flow"), - ranger_fov(0.0), - ranger_min_range(0.3), - ranger_max_range(5.0) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - flow_nh.param("frame_id", frame_id, "px4flow"); - - /** - * @note Default rangefinder is Maxbotix HRLV-EZ4 - * This is a narrow beam (60cm wide at 5 meters, - * but also at 1 meter). 6.8 degrees at 5 meters, 31 degrees - * at 1 meter - */ - flow_nh.param("ranger_fov", ranger_fov, 0.119428926); - - flow_nh.param("ranger_min_range", ranger_min_range, 0.3); - flow_nh.param("ranger_max_range", ranger_max_range, 5.0); - - flow_rad_pub = flow_nh.advertise("raw/optical_flow_rad", 10); - range_pub = flow_nh.advertise("ground_distance", 10); - temp_pub = flow_nh.advertise("temperature", 10); - - flow_rad_sub = flow_nh.subscribe("raw/send", 1, &PX4FlowPlugin::send_cb, this); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&PX4FlowPlugin::handle_optical_flow_rad) - }; - } + explicit PX4FlowPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "px4flow"), + ranger_fov(0.0), + ranger_min_range(0.3), + ranger_max_range(5.0) + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "frame_id", "px4flow", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + + /** + * @note Default rangefinder is Maxbotix HRLV-EZ4 + * This is a narrow beam (60cm wide at 5 meters, + * but also at 1 meter). 6.8 degrees at 5 meters, 31 degrees + * at 1 meter + */ + node_declare_and_watch_parameter( + "ranger_fov", 0.119428926, [&](const rclcpp::Parameter & p) { + ranger_fov = p.as_double(); + }); + + node_declare_and_watch_parameter( + "ranger_min_range", 0.3, [&](const rclcpp::Parameter & p) { + ranger_min_range = p.as_double(); + }); + + node_declare_and_watch_parameter( + "ranger_max_range", 5.0, [&](const rclcpp::Parameter & p) { + ranger_max_range = p.as_double(); + }); + + flow_rad_pub = node->create_publisher( + "~/raw/optical_flow_rad", 10); + range_pub = node->create_publisher("~/ground_distance", 10); + temp_pub = node->create_publisher("~/temperature", 10); + + flow_rad_sub = node->create_subscription( + "~/raw/send", 1, std::bind( + &PX4FlowPlugin::send_cb, this, + _1)); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&PX4FlowPlugin::handle_optical_flow_rad) + }; + } private: - ros::NodeHandle flow_nh; - - std::string frame_id; - - double ranger_fov; - double ranger_min_range; - double ranger_max_range; - - ros::Publisher flow_rad_pub; - ros::Publisher range_pub; - ros::Publisher temp_pub; - ros::Subscriber flow_rad_sub; - - void handle_optical_flow_rad(const mavlink::mavlink_message_t *msg, mavlink::common::msg::OPTICAL_FLOW_RAD &flow_rad) - { - auto header = m_uas->synchronized_header(frame_id, flow_rad.time_usec); - - /** - * Raw message with axes mapped to ROS conventions and temp in degrees celsius. - * - * The optical flow camera is essentially an angular sensor, so conversion is like - * gyroscope. (aircraft -> baselink) - */ - auto int_xy = ftf::transform_frame_aircraft_baselink( - Eigen::Vector3d( - flow_rad.integrated_x, - flow_rad.integrated_y, - 0.0)); - auto int_gyro = ftf::transform_frame_aircraft_baselink( - Eigen::Vector3d( - flow_rad.integrated_xgyro, - flow_rad.integrated_ygyro, - flow_rad.integrated_zgyro)); - - auto flow_rad_msg = boost::make_shared(); - - flow_rad_msg->header = header; - flow_rad_msg->integration_time_us = flow_rad.integration_time_us; - - flow_rad_msg->integrated_x = int_xy.x(); - flow_rad_msg->integrated_y = int_xy.y(); - - flow_rad_msg->integrated_xgyro = int_gyro.x(); - flow_rad_msg->integrated_ygyro = int_gyro.y(); - flow_rad_msg->integrated_zgyro = int_gyro.z(); - - flow_rad_msg->temperature = flow_rad.temperature / 100.0f; // in degrees celsius - flow_rad_msg->time_delta_distance_us = flow_rad.time_delta_distance_us; - flow_rad_msg->distance = flow_rad.distance; - flow_rad_msg->quality = flow_rad.quality; - - flow_rad_pub.publish(flow_rad_msg); - - // Temperature - auto temp_msg = boost::make_shared(); - - temp_msg->header = header; - temp_msg->temperature = flow_rad_msg->temperature; - - temp_pub.publish(temp_msg); - - // Rangefinder - /** - * @todo: use distance_sensor plugin only to publish this data - * (which receives DISTANCE_SENSOR msg with multiple rangefinder - * sensors data) - * - * @todo: suggest modification on MAVLink OPTICAL_FLOW_RAD msg - * which removes sonar data fields from it - */ - auto range_msg = boost::make_shared(); - - range_msg->header = header; - - range_msg->radiation_type = sensor_msgs::Range::ULTRASOUND; - range_msg->field_of_view = ranger_fov; - range_msg->min_range = ranger_min_range; - range_msg->max_range = ranger_max_range; - range_msg->range = flow_rad.distance; - - range_pub.publish(range_msg); - } - - void send_cb(const mavros_msgs::OpticalFlowRad::ConstPtr msg) - { - mavlink::common::msg::OPTICAL_FLOW_RAD flow_rad_msg = {}; - - auto int_xy = ftf::transform_frame_baselink_aircraft( - Eigen::Vector3d( - msg->integrated_x, - msg->integrated_y, - 0.0)); - auto int_gyro = ftf::transform_frame_baselink_aircraft( - Eigen::Vector3d( - msg->integrated_xgyro, - msg->integrated_ygyro, - msg->integrated_zgyro)); - - flow_rad_msg.time_usec = msg->header.stamp.toNSec() / 1000; - flow_rad_msg.sensor_id = 0; - flow_rad_msg.integration_time_us = msg->integration_time_us; - flow_rad_msg.integrated_x = int_xy.x(); - flow_rad_msg.integrated_y = int_xy.y(); - flow_rad_msg.integrated_xgyro = int_gyro.x(); - flow_rad_msg.integrated_ygyro = int_gyro.y(); - flow_rad_msg.integrated_zgyro = int_gyro.z(); - flow_rad_msg.temperature = msg->temperature * 100.0f; // temperature in centi-degrees Celsius - flow_rad_msg.quality = msg->quality; - flow_rad_msg.time_delta_distance_us = msg->time_delta_distance_us; - flow_rad_msg.distance = msg->distance; - - UAS_FCU(m_uas)->send_message_ignore_drop(flow_rad_msg); - } + std::string frame_id; + + double ranger_fov; + double ranger_min_range; + double ranger_max_range; + + rclcpp::Publisher::SharedPtr flow_rad_pub; + rclcpp::Publisher::SharedPtr range_pub; + rclcpp::Publisher::SharedPtr temp_pub; + rclcpp::Subscription::SharedPtr flow_rad_sub; + + void handle_optical_flow_rad( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::OPTICAL_FLOW_RAD & flow_rad, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto header = uas->synchronized_header(frame_id, flow_rad.time_usec); + + /** + * Raw message with axes mapped to ROS conventions and temp in degrees celsius. + * + * The optical flow camera is essentially an angular sensor, so conversion is like + * gyroscope. (aircraft -> baselink) + */ + auto int_xy = ftf::transform_frame_aircraft_baselink( + Eigen::Vector3d( + flow_rad.integrated_x, + flow_rad.integrated_y, + 0.0)); + auto int_gyro = ftf::transform_frame_aircraft_baselink( + Eigen::Vector3d( + flow_rad.integrated_xgyro, + flow_rad.integrated_ygyro, + flow_rad.integrated_zgyro)); + + auto flow_rad_msg = mavros_msgs::msg::OpticalFlowRad(); + + flow_rad_msg.header = header; + flow_rad_msg.integration_time_us = flow_rad.integration_time_us; + + flow_rad_msg.integrated_x = int_xy.x(); + flow_rad_msg.integrated_y = int_xy.y(); + + flow_rad_msg.integrated_xgyro = int_gyro.x(); + flow_rad_msg.integrated_ygyro = int_gyro.y(); + flow_rad_msg.integrated_zgyro = int_gyro.z(); + + flow_rad_msg.temperature = flow_rad.temperature / 100.0f; // in degrees celsius + flow_rad_msg.time_delta_distance_us = flow_rad.time_delta_distance_us; + flow_rad_msg.distance = flow_rad.distance; + flow_rad_msg.quality = flow_rad.quality; + + flow_rad_pub->publish(flow_rad_msg); + + // Temperature + auto temp_msg = sensor_msgs::msg::Temperature(); + + temp_msg.header = header; + temp_msg.temperature = flow_rad_msg.temperature; + + temp_pub->publish(temp_msg); + + // Rangefinder + /** + * @todo: use distance_sensor plugin only to publish this data + * (which receives DISTANCE_SENSOR msg with multiple rangefinder + * sensors data) + * + * @todo: suggest modification on MAVLink OPTICAL_FLOW_RAD msg + * which removes sonar data fields from it + */ + auto range_msg = sensor_msgs::msg::Range(); + + range_msg.header = header; + + range_msg.radiation_type = sensor_msgs::msg::Range::ULTRASOUND; + range_msg.field_of_view = ranger_fov; + range_msg.min_range = ranger_min_range; + range_msg.max_range = ranger_max_range; + range_msg.range = flow_rad.distance; + + range_pub->publish(range_msg); + } + + void send_cb(const mavros_msgs::msg::OpticalFlowRad::SharedPtr msg) + { + mavlink::common::msg::OPTICAL_FLOW_RAD flow_rad_msg = {}; + + auto int_xy = ftf::transform_frame_baselink_aircraft( + Eigen::Vector3d( + msg->integrated_x, + msg->integrated_y, + 0.0)); + auto int_gyro = ftf::transform_frame_baselink_aircraft( + Eigen::Vector3d( + msg->integrated_xgyro, + msg->integrated_ygyro, + msg->integrated_zgyro)); + + flow_rad_msg.time_usec = get_time_usec(msg->header.stamp); + flow_rad_msg.sensor_id = 0; + flow_rad_msg.integration_time_us = msg->integration_time_us; + flow_rad_msg.integrated_x = int_xy.x(); + flow_rad_msg.integrated_y = int_xy.y(); + flow_rad_msg.integrated_xgyro = int_gyro.x(); + flow_rad_msg.integrated_ygyro = int_gyro.y(); + flow_rad_msg.integrated_zgyro = int_gyro.z(); + flow_rad_msg.temperature = msg->temperature * 100.0f; // temperature in centi-degrees Celsius + flow_rad_msg.quality = msg->quality; + flow_rad_msg.time_delta_distance_us = msg->time_delta_distance_us; + flow_rad_msg.distance = msg->distance; + + uas->send_message(flow_rad_msg); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::PX4FlowPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::PX4FlowPlugin) diff --git a/mavros_extras/src/plugins/rangefinder.cpp b/mavros_extras/src/plugins/rangefinder.cpp index aeed57cb6..4d583fb0f 100644 --- a/mavros_extras/src/plugins/rangefinder.cpp +++ b/mavros_extras/src/plugins/rangefinder.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2016 Ardupilot. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Rangefinder plugin * @file rangefinder.cpp @@ -6,65 +13,65 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2016 Ardupilot. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "sensor_msgs/msg/range.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace extra_plugins { /** * @brief Ardupilot Rangefinder plugin. + * @plugin rangefinder * * This plugin allows publishing rangefinder sensor data from Ardupilot FCU to ROS. - * */ -class RangefinderPlugin : public plugin::PluginBase { +class RangefinderPlugin : public plugin::Plugin +{ public: - RangefinderPlugin() : PluginBase(), - rangefinder_nh("~rangefinder") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - rangefinder_pub = rangefinder_nh.advertise("rangefinder", 10); - } + explicit RangefinderPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "rangefinder") + { + rangefinder_pub = node->create_publisher("~/rangefinder", 10); + } - Subscriptions get_subscriptions() override - { - return { - make_handler(&RangefinderPlugin::handle_rangefinder) - }; - } + Subscriptions get_subscriptions() override + { + return { + make_handler(&RangefinderPlugin::handle_rangefinder) + }; + } private: - ros::NodeHandle rangefinder_nh; + rclcpp::Publisher::SharedPtr rangefinder_pub; - ros::Publisher rangefinder_pub; + void handle_rangefinder( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::ardupilotmega::msg::RANGEFINDER & rangefinder, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto rangefinder_msg = sensor_msgs::msg::Range(); - void handle_rangefinder(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::RANGEFINDER &rangefinder) { - auto rangefinder_msg = boost::make_shared(); - rangefinder_msg->header.stamp = ros::Time::now(); - rangefinder_msg->header.frame_id = "/rangefinder"; - rangefinder_msg->radiation_type = sensor_msgs::Range::INFRARED; - rangefinder_msg->field_of_view = 0; - rangefinder_msg->min_range = 0; - rangefinder_msg->max_range = 1000; - rangefinder_msg->range = rangefinder.distance; + rangefinder_msg.header.stamp = node->now(); + rangefinder_msg.header.frame_id = "/rangefinder"; + rangefinder_msg.radiation_type = sensor_msgs::msg::Range::INFRARED; + rangefinder_msg.field_of_view = 0; + rangefinder_msg.min_range = 0; + rangefinder_msg.max_range = 1000; + rangefinder_msg.range = rangefinder.distance; - rangefinder_pub.publish(rangefinder_msg); - } + rangefinder_pub->publish(rangefinder_msg); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::RangefinderPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::RangefinderPlugin) diff --git a/mavros_extras/src/plugins/terrain.cpp b/mavros_extras/src/plugins/terrain.cpp index 224d0d2af..428fcbfd2 100644 --- a/mavros_extras/src/plugins/terrain.cpp +++ b/mavros_extras/src/plugins/terrain.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2021 Ardupilot. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Terrain plugin * @file terrain.cpp @@ -6,69 +13,69 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2021 Ardupilot. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/terrain_report.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace extra_plugins { /** * @brief Terrain height plugin. + * @plugin terrain * * This plugin allows publishing of terrain height estimate from FCU to ROS. * */ -class TerrainPlugin : public plugin::PluginBase { +class TerrainPlugin : public plugin::Plugin +{ public: - TerrainPlugin() : PluginBase(), - terrain_nh("~terrain") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); + explicit TerrainPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "terrain") + { + terrain_report_pub = node->create_publisher("~/report", 10); + } - terrain_report_pub = terrain_nh.advertise("report", 10); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&TerrainPlugin::handle_terrain_report) - }; - } + Subscriptions get_subscriptions() override + { + return { + make_handler(&TerrainPlugin::handle_terrain_report) + }; + } private: - ros::NodeHandle terrain_nh; - - ros::Publisher terrain_report_pub; + rclcpp::Publisher::SharedPtr terrain_report_pub; - void handle_terrain_report(const mavlink::mavlink_message_t *msg, mavlink::common::msg::TERRAIN_REPORT &report) { - auto terrain_report_msg = boost::make_shared(); + void handle_terrain_report( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::TERRAIN_REPORT & report, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto terrain_report_msg = mavros_msgs::msg::TerrainReport(); - terrain_report_msg->header.stamp = ros::Time::now(); - terrain_report_msg->header.frame_id = "terrain"; + terrain_report_msg.header.stamp = node->now(); + terrain_report_msg.header.frame_id = "terrain"; - terrain_report_msg->latitude = (double) report.lat / 1e7; - terrain_report_msg->longitude = (double) report.lon / 1e7; - terrain_report_msg->spacing = report.spacing; - terrain_report_msg->terrain_height = report.terrain_height; - terrain_report_msg->current_height = report.current_height; - terrain_report_msg->pending = report.pending; - terrain_report_msg->loaded = report.loaded; + terrain_report_msg.latitude = static_cast(report.lat) / 1e7; + terrain_report_msg.longitude = static_cast(report.lon) / 1e7; + terrain_report_msg.spacing = report.spacing; + terrain_report_msg.terrain_height = report.terrain_height; + terrain_report_msg.current_height = report.current_height; + terrain_report_msg.pending = report.pending; + terrain_report_msg.loaded = report.loaded; - terrain_report_pub.publish(terrain_report_msg); - } + terrain_report_pub->publish(terrain_report_msg); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::TerrainPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::TerrainPlugin) diff --git a/mavros_extras/src/plugins/trajectory.cpp b/mavros_extras/src/plugins/trajectory.cpp index a96a87385..51963c748 100644 --- a/mavros_extras/src/plugins/trajectory.cpp +++ b/mavros_extras/src/plugins/trajectory.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2018 Martina Rivizzigno. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.mdA + */ /** * @brief Trajectory plugin * @file trajectory.cpp @@ -6,21 +13,23 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2018 Martina Rivizzigno. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.mdA - */ -#include -#include -#include -#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/trajectory.hpp" +#include "mavros_msgs/msg/position_target.hpp" +#include "nav_msgs/msg/path.hpp" -namespace mavros { -namespace extra_plugins { +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT using utils::enum_value; //! Points count in TRAJECTORY message @@ -29,363 +38,398 @@ static constexpr size_t NUM_POINTS = 5; //! Type matching mavlink::common::msg::TRAJECTORY::TRAJECTORY_REPRESENTATION_WAYPOINTS fields using MavPoints = std::array; -using RosPoints = mavros_msgs::PositionTarget; +using RosPoints = mavros_msgs::msg::PositionTarget; /** * @brief Trajectory plugin to receive planned path from the FCU and * send back to the FCU a corrected path (collision free, smoothed) + * @plugin trajectory * * @see trajectory_cb() */ -class TrajectoryPlugin : public plugin::PluginBase { +class TrajectoryPlugin : public plugin::Plugin +{ public: - TrajectoryPlugin() : PluginBase(), - trajectory_nh("~trajectory") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - trajectory_generated_sub = trajectory_nh.subscribe("generated", 10, &TrajectoryPlugin::trajectory_cb, this); - path_sub = trajectory_nh.subscribe("path", 10, &TrajectoryPlugin::path_cb, this); - trajectory_desired_pub = trajectory_nh.advertise("desired", 10); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&TrajectoryPlugin::handle_trajectory) - }; - } + explicit TrajectoryPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "trajectory") + { + trajectory_generated_sub = node->create_subscription( + "~/generated", 10, std::bind( + &TrajectoryPlugin::trajectory_cb, this, _1)); + path_sub = + node->create_subscription( + "~/path", 10, + std::bind(&TrajectoryPlugin::path_cb, this, _1)); + trajectory_desired_pub = node->create_publisher("~/desired", 10); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&TrajectoryPlugin::handle_trajectory) + }; + } private: - ros::NodeHandle trajectory_nh; - - ros::Subscriber trajectory_generated_sub; - ros::Subscriber path_sub; - - ros::Publisher trajectory_desired_pub; - - // [[[cog: - // def outl_fill_points_ned_vector(x, y, z, vec_name, vec_type, point_xyz): - // cog.outl( - // """void fill_points_{vec_name}(MavPoints &{x}, MavPoints &{y}, MavPoints &{z}, const geometry_msgs::{vec_type} &{vec_name}, const size_t i)\n""" - // """{{\n""" - // """\tauto {vec_name}_ned = ftf::transform_frame_enu_ned(ftf::to_eigen({vec_name}));\n""" - // .format(**locals()) - // ) - // - // for axis in "xyz": - // cog.outl("\t{axis}[i] = {vec_name}_ned.{axis}();".format(**locals())) - // - // cog.outl("}\n") - // - // - // outl_fill_points_ned_vector('x', 'y', 'z', 'position', 'Point', range(0, 3)) - // outl_fill_points_ned_vector('x', 'y', 'z', 'velocity', 'Vector3', range(3, 6)) - // outl_fill_points_ned_vector('x', 'y', 'z', 'acceleration', 'Vector3', range(6, 9)) - // ]]] - void fill_points_position(MavPoints &x, MavPoints &y, MavPoints &z, const geometry_msgs::Point &position, const size_t i) - { - auto position_ned = ftf::transform_frame_enu_ned(ftf::to_eigen(position)); - - x[i] = position_ned.x(); - y[i] = position_ned.y(); - z[i] = position_ned.z(); - } - - void fill_points_velocity(MavPoints &x, MavPoints &y, MavPoints &z, const geometry_msgs::Vector3 &velocity, const size_t i) - { - auto velocity_ned = ftf::transform_frame_enu_ned(ftf::to_eigen(velocity)); - - x[i] = velocity_ned.x(); - y[i] = velocity_ned.y(); - z[i] = velocity_ned.z(); - } - - void fill_points_acceleration(MavPoints &x, MavPoints &y, MavPoints &z, const geometry_msgs::Vector3 &acceleration, const size_t i) - { - auto acceleration_ned = ftf::transform_frame_enu_ned(ftf::to_eigen(acceleration)); - - x[i] = acceleration_ned.x(); - y[i] = acceleration_ned.y(); - z[i] = acceleration_ned.z(); - } - - // [[[end]]] (checksum: 92ea0f7f329c90c486fdb8b738c0e7c3) - - - void fill_points_yaw_wp(MavPoints &y, const double yaw, const size_t i) { - y[i] = wrap_pi(-yaw + (M_PI / 2.0f)); - } - - void fill_points_yaw_speed(MavPoints &yv, const double yaw_speed, const size_t i) { - yv[i] = yaw_speed; - } - - void fill_points_yaw_q(MavPoints &y, const geometry_msgs::Quaternion &orientation, const size_t i) { - auto q_wp = ftf::transform_orientation_enu_ned( - ftf::transform_orientation_baselink_aircraft( - ftf::to_eigen(orientation))); - auto yaw_wp = ftf::quaternion_get_yaw(q_wp); - - y[i] = wrap_pi(-yaw_wp + (M_PI / 2.0f)); - } - - void fill_points_delta(MavPoints &y, const double time_horizon, const size_t i) { - y[i] = time_horizon; - } - - auto fill_points_unused_path(mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, const size_t i) { - t.vel_x[i] = NAN; - t.vel_y[i] = NAN; - t.vel_z[i] = NAN; - t.acc_x[i] = NAN; - t.acc_y[i] = NAN; - t.acc_z[i] = NAN; - t.vel_yaw[i] = NAN; - } - - void fill_points_all_unused(mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS &t, const size_t i) { - t.pos_x[i] = NAN; - t.pos_y[i] = NAN; - t.pos_z[i] = NAN; - - t.vel_x[i] = NAN; - t.vel_y[i] = NAN; - t.vel_z[i] = NAN; - - t.acc_x[i] = NAN; - t.acc_y[i] = NAN; - t.acc_z[i] = NAN; - - t.pos_yaw[i] = NAN; - t.vel_yaw[i] = NAN; - } - - void fill_points_all_unused_bezier(mavlink::common::msg::TRAJECTORY_REPRESENTATION_BEZIER &t, const size_t i) { - t.pos_x[i] = NAN; - t.pos_y[i] = NAN; - t.pos_z[i] = NAN; - - t.pos_yaw[i] = NAN; - - t.delta[i] = NAN; - } - - void fill_msg_position(geometry_msgs::Point &position, const mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS &t, const size_t i) - { - auto position_enu = ftf::transform_frame_ned_enu(Eigen::Vector3d(t.pos_x[i], t.pos_y[i], t.pos_z[i])); - - position.x = position_enu.x(); - position.y = position_enu.y(); - position.z = position_enu.z(); - } - - void fill_msg_velocity(geometry_msgs::Vector3 &velocity, const mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS &t, const size_t i) - { - auto velocity_enu = ftf::transform_frame_ned_enu(Eigen::Vector3d(t.vel_x[i], t.vel_y[i], t.vel_z[i])); - - velocity.x = velocity_enu.x(); - velocity.y = velocity_enu.y(); - velocity.z = velocity_enu.z(); - } - - void fill_msg_acceleration(geometry_msgs::Vector3 &acceleration, const mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS &t, const size_t i) - { - auto acceleration_enu = ftf::transform_frame_ned_enu(Eigen::Vector3d(t.acc_x[i], t.acc_y[i], t.acc_z[i])); - - acceleration.x = acceleration_enu.x(); - acceleration.y = acceleration_enu.y(); - acceleration.z = acceleration_enu.z(); - } - - - // -*- callbacks -*- - - /** - * @brief Send corrected path to the FCU. - * - * Message specification: https://mavlink.io/en/messages/common.html#TRAJECTORY - * @param req received Trajectory msg - */ - void trajectory_cb(const mavros_msgs::Trajectory::ConstPtr &req) - { - ROS_ASSERT(NUM_POINTS == req->point_valid.size()); - - if (req->type == mavros_msgs::Trajectory::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS) { - mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS trajectory {}; - - auto fill_point_rep_waypoints = [&](mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, const RosPoints &rp, const size_t i) { - const auto valid = req->point_valid[i]; - - auto valid_so_far = trajectory.valid_points; - if (!valid) { - fill_points_all_unused(t, i); - return; - } - - trajectory.valid_points = valid_so_far + 1; - fill_points_position(t.pos_x, t.pos_y, t.pos_z, rp.position, i); - fill_points_velocity(t.vel_x, t.vel_y, t.vel_z, rp.velocity, i); - fill_points_acceleration(t.acc_x, t.acc_y, t.acc_z, rp.acceleration_or_force, i); - fill_points_yaw_wp(t.pos_yaw, rp.yaw, i); - fill_points_yaw_speed(t.vel_yaw, rp.yaw_rate, i); - t.command[i] = UINT16_MAX; - }; - - // [[[cog: - // for i in range(5): - // cog.outl( - // 'fill_point_rep_waypoints(trajectory, req->point_{i1}, {i0});' - // .format(i0=i, i1=i+1) - // ) - // ]]] - fill_point_rep_waypoints(trajectory, req->point_1, 0); - fill_point_rep_waypoints(trajectory, req->point_2, 1); - fill_point_rep_waypoints(trajectory, req->point_3, 2); - fill_point_rep_waypoints(trajectory, req->point_4, 3); - fill_point_rep_waypoints(trajectory, req->point_5, 4); - // [[[end]]] (checksum: e993aeb535c2df6f07bf7b4f1fcf3d2e) - - trajectory.time_usec = req->header.stamp.toNSec() / 1000; //!< [milisecs] - UAS_FCU(m_uas)->send_message_ignore_drop(trajectory); - } else { - mavlink::common::msg::TRAJECTORY_REPRESENTATION_BEZIER trajectory {}; - auto fill_point_rep_bezier = [&](mavlink::common::msg::TRAJECTORY_REPRESENTATION_BEZIER & t, const RosPoints &rp, const size_t i) { - const auto valid = req->point_valid[i]; - - auto valid_so_far = trajectory.valid_points; - if (!valid) { - fill_points_all_unused_bezier(t, i); - return; - } - - trajectory.valid_points = valid_so_far + 1; - fill_points_position(t.pos_x, t.pos_y, t.pos_z, rp.position, i); - fill_points_yaw_wp(t.pos_yaw, rp.yaw, i); - fill_points_delta(t.delta, req->time_horizon[i], i); - }; - // [[[cog: - // for i in range(5): - // cog.outl( - // 'fill_point_rep_bezier(trajectory, req->point_{i1}, {i0});' - // .format(i0=i, i1=i+1) - // ) - // ]]] - fill_point_rep_bezier(trajectory, req->point_1, 0); - fill_point_rep_bezier(trajectory, req->point_2, 1); - fill_point_rep_bezier(trajectory, req->point_3, 2); - fill_point_rep_bezier(trajectory, req->point_4, 3); - fill_point_rep_bezier(trajectory, req->point_5, 4); - // [[[end]]] (checksum: 3e6da5f06e0b33682c6122c40f05c1f6) - - trajectory.time_usec = req->header.stamp.toNSec() / 1000; //!< [milisecs] - UAS_FCU(m_uas)->send_message_ignore_drop(trajectory); - } - } - - - /** - * @brief Send corrected path to the FCU. - * - * Message specification: https://mavlink.io/en/messages/common.html#TRAJECTORY - * @param req received nav_msgs Path msg - */ - void path_cb(const nav_msgs::Path::ConstPtr &req) - { - mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS trajectory {}; - - trajectory.time_usec = req->header.stamp.toNSec() / 1000; //!< [milisecs] - trajectory.valid_points = std::min(NUM_POINTS, req->poses.size()); - - auto fill_point = [&](mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, const size_t i) { - t.command[i] = UINT16_MAX; - if (req->poses.size() < i + 1) { - fill_points_all_unused(t, i); - } - else { - auto &pose = req->poses[i].pose; - - fill_points_position(t.pos_x, t.pos_y, t.pos_z, pose.position, i); - fill_points_yaw_q(t.pos_yaw, pose.orientation, i); - fill_points_unused_path(t, i); - } - }; - - // [[[cog: - // for i in range(5): - // cog.outl( - // 'fill_point(trajectory, {i0});'.format(i0=i, i1=i+1)) - // ]]] - fill_point(trajectory, 0); - fill_point(trajectory, 1); - fill_point(trajectory, 2); - fill_point(trajectory, 3); - fill_point(trajectory, 4); - // [[[end]]] (checksum: 267a911c65a3768f04a8230fcb235bca) - - UAS_FCU(m_uas)->send_message_ignore_drop(trajectory); - } - - void handle_trajectory(const mavlink::mavlink_message_t *msg, mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS &trajectory) - { - auto tr_desired = boost::make_shared(); - - auto fill_msg_point = [&](RosPoints & p, const mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, const size_t i) { - fill_msg_position(p.position, t, i); - fill_msg_velocity(p.velocity, t, i); - fill_msg_acceleration(p.acceleration_or_force, t, i); - p.yaw = wrap_pi((M_PI / 2.0f) - t.pos_yaw[i]); - p.yaw_rate = t.vel_yaw[i]; - tr_desired->command[i] = t.command[i]; - }; - - tr_desired->header = m_uas->synchronized_header("local_origin", trajectory.time_usec); - - if (trajectory.valid_points > tr_desired->point_valid.size()) { - return; - } - - for (int i = 0; i < trajectory.valid_points; ++i) - { - tr_desired->point_valid[i] = true; - } - - for (int i = trajectory.valid_points; i < NUM_POINTS; ++i) - { - tr_desired->point_valid[i] = false; - } - - // [[[cog: - // for i in range(5): - // cog.outl( - // "fill_msg_point(tr_desired->point_{i1}, trajectory, {i0});" - // .format(i0=i, i1=i + 1, ) - // ) - // ]]] - fill_msg_point(tr_desired->point_1, trajectory, 0); - fill_msg_point(tr_desired->point_2, trajectory, 1); - fill_msg_point(tr_desired->point_3, trajectory, 2); - fill_msg_point(tr_desired->point_4, trajectory, 3); - fill_msg_point(tr_desired->point_5, trajectory, 4); - // [[[end]]] (checksum: b6aa0c7395a7a426c25d726b75b6efea) - - trajectory_desired_pub.publish(tr_desired); - } - - float wrap_pi(float a) - { - if (!std::isfinite(a)) { - return a; - } - - return fmod(a + M_PI, 2.0f * M_PI) - M_PI; - } + rclcpp::Subscription::SharedPtr trajectory_generated_sub; + rclcpp::Subscription::SharedPtr path_sub; + + rclcpp::Publisher::SharedPtr trajectory_desired_pub; + + // [[[cog: + // def outl_fill_points_ned_vector(x, y, z, vec_name, vec_type, point_xyz): + // cog.outl( + // f"""void fill_points_{vec_name}(\n""" + // f""" MavPoints & {x}, MavPoints & {y}, MavPoints & {z},\n""" + // f""" const geometry_msgs::msg::{vec_type} & {vec_name}, const size_t i)\n""" + // f"""{{\n""" + // f""" auto {vec_name}_ned = ftf::transform_frame_enu_ned(""" + // f"""ftf::to_eigen({vec_name}));\n""" + // ) + // + // for axis in "xyz": + // cog.outl(f" {axis}[i] = {vec_name}_ned.{axis}();") + // + // cog.outl("}\n") + // + // + // outl_fill_points_ned_vector('x', 'y', 'z', 'position', 'Point', range(0, 3)) + // outl_fill_points_ned_vector('x', 'y', 'z', 'velocity', 'Vector3', range(3, 6)) + // outl_fill_points_ned_vector('x', 'y', 'z', 'acceleration', 'Vector3', range(6, 9)) + // ]]] + void fill_points_position( + MavPoints & x, MavPoints & y, MavPoints & z, + const geometry_msgs::msg::Point & position, const size_t i) + { + auto position_ned = ftf::transform_frame_enu_ned(ftf::to_eigen(position)); + + x[i] = position_ned.x(); + y[i] = position_ned.y(); + z[i] = position_ned.z(); + } + + void fill_points_velocity( + MavPoints & x, MavPoints & y, MavPoints & z, + const geometry_msgs::msg::Vector3 & velocity, const size_t i) + { + auto velocity_ned = ftf::transform_frame_enu_ned(ftf::to_eigen(velocity)); + + x[i] = velocity_ned.x(); + y[i] = velocity_ned.y(); + z[i] = velocity_ned.z(); + } + + void fill_points_acceleration( + MavPoints & x, MavPoints & y, MavPoints & z, + const geometry_msgs::msg::Vector3 & acceleration, const size_t i) + { + auto acceleration_ned = ftf::transform_frame_enu_ned(ftf::to_eigen(acceleration)); + + x[i] = acceleration_ned.x(); + y[i] = acceleration_ned.y(); + z[i] = acceleration_ned.z(); + } + + // [[[end]]] (checksum: a0ed1550494e431a3ba599da8503c8b6) + + void fill_points_yaw_wp(MavPoints & y, const double yaw, const size_t i) + { + y[i] = wrap_pi(-yaw + (M_PI / 2.0f)); + } + + void fill_points_yaw_speed(MavPoints & yv, const double yaw_speed, const size_t i) + { + yv[i] = yaw_speed; + } + + void fill_points_yaw_q( + MavPoints & y, const geometry_msgs::msg::Quaternion & orientation, + const size_t i) + { + auto q_wp = ftf::transform_orientation_enu_ned( + ftf::transform_orientation_baselink_aircraft( + ftf::to_eigen(orientation))); + auto yaw_wp = ftf::quaternion_get_yaw(q_wp); + + y[i] = wrap_pi(-yaw_wp + (M_PI / 2.0f)); + } + + void fill_points_delta(MavPoints & y, const double time_horizon, const size_t i) + { + y[i] = time_horizon; + } + + auto fill_points_unused_path( + mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, + const size_t i) + { + t.vel_x[i] = NAN; + t.vel_y[i] = NAN; + t.vel_z[i] = NAN; + t.acc_x[i] = NAN; + t.acc_y[i] = NAN; + t.acc_z[i] = NAN; + t.vel_yaw[i] = NAN; + } + + void fill_points_all_unused( + mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, + const size_t i) + { + t.pos_x[i] = NAN; + t.pos_y[i] = NAN; + t.pos_z[i] = NAN; + + t.vel_x[i] = NAN; + t.vel_y[i] = NAN; + t.vel_z[i] = NAN; + + t.acc_x[i] = NAN; + t.acc_y[i] = NAN; + t.acc_z[i] = NAN; + + t.pos_yaw[i] = NAN; + t.vel_yaw[i] = NAN; + } + + void fill_points_all_unused_bezier( + mavlink::common::msg::TRAJECTORY_REPRESENTATION_BEZIER & t, + const size_t i) + { + t.pos_x[i] = NAN; + t.pos_y[i] = NAN; + t.pos_z[i] = NAN; + + t.pos_yaw[i] = NAN; + + t.delta[i] = NAN; + } + + void fill_msg_position( + geometry_msgs::msg::Point & position, + const mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, + const size_t i) + { + auto position_enu = + ftf::transform_frame_ned_enu(Eigen::Vector3d(t.pos_x[i], t.pos_y[i], t.pos_z[i])); + + position.x = position_enu.x(); + position.y = position_enu.y(); + position.z = position_enu.z(); + } + + void fill_msg_velocity( + geometry_msgs::msg::Vector3 & velocity, + const mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, + const size_t i) + { + auto velocity_enu = + ftf::transform_frame_ned_enu(Eigen::Vector3d(t.vel_x[i], t.vel_y[i], t.vel_z[i])); + + velocity.x = velocity_enu.x(); + velocity.y = velocity_enu.y(); + velocity.z = velocity_enu.z(); + } + + void fill_msg_acceleration( + geometry_msgs::msg::Vector3 & acceleration, + const mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, + const size_t i) + { + auto acceleration_enu = + ftf::transform_frame_ned_enu(Eigen::Vector3d(t.acc_x[i], t.acc_y[i], t.acc_z[i])); + + acceleration.x = acceleration_enu.x(); + acceleration.y = acceleration_enu.y(); + acceleration.z = acceleration_enu.z(); + } + + + // -*- callbacks -*- + + /** + * @brief Send corrected path to the FCU. + * + * Message specification: https://mavlink.io/en/messages/common.html#TRAJECTORY + * @param req received Trajectory msg + */ + void trajectory_cb(const mavros_msgs::msg::Trajectory::SharedPtr req) + { + rcpputils::require_true(NUM_POINTS == req->point_valid.size()); + + if (req->type == mavros_msgs::msg::Trajectory::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS) { + mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS trajectory {}; + + auto fill_point_rep_waypoints = + [&](mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, const RosPoints & rp, + const size_t i) { + const auto valid = req->point_valid[i]; + + auto valid_so_far = trajectory.valid_points; + if (!valid) { + fill_points_all_unused(t, i); + return; + } + + trajectory.valid_points = valid_so_far + 1; + fill_points_position(t.pos_x, t.pos_y, t.pos_z, rp.position, i); + fill_points_velocity(t.vel_x, t.vel_y, t.vel_z, rp.velocity, i); + fill_points_acceleration(t.acc_x, t.acc_y, t.acc_z, rp.acceleration_or_force, i); + fill_points_yaw_wp(t.pos_yaw, rp.yaw, i); + fill_points_yaw_speed(t.vel_yaw, rp.yaw_rate, i); + t.command[i] = UINT16_MAX; + }; + + // [[[cog: + // for i in range(5): + // cog.outl( + // f"fill_point_rep_waypoints(trajectory, req->point_{i+1}, {i});" + // ) + // ]]] + fill_point_rep_waypoints(trajectory, req->point_1, 0); + fill_point_rep_waypoints(trajectory, req->point_2, 1); + fill_point_rep_waypoints(trajectory, req->point_3, 2); + fill_point_rep_waypoints(trajectory, req->point_4, 3); + fill_point_rep_waypoints(trajectory, req->point_5, 4); + // [[[end]]] (checksum: 3378a593279611a83e25efee67393195) + + trajectory.time_usec = get_time_usec(req->header.stamp); //!< [milisecs] + uas->send_message(trajectory); + } else { + mavlink::common::msg::TRAJECTORY_REPRESENTATION_BEZIER trajectory {}; + auto fill_point_rep_bezier = + [&](mavlink::common::msg::TRAJECTORY_REPRESENTATION_BEZIER & t, const RosPoints & rp, + const size_t i) { + const auto valid = req->point_valid[i]; + + auto valid_so_far = trajectory.valid_points; + if (!valid) { + fill_points_all_unused_bezier(t, i); + return; + } + + trajectory.valid_points = valid_so_far + 1; + fill_points_position(t.pos_x, t.pos_y, t.pos_z, rp.position, i); + fill_points_yaw_wp(t.pos_yaw, rp.yaw, i); + fill_points_delta(t.delta, req->time_horizon[i], i); + }; + + // [[[cog: + // for i in range(5): + // cog.outl( + // f"fill_point_rep_bezier(trajectory, req->point_{i+1}, {i});" + // ) + // ]]] + fill_point_rep_bezier(trajectory, req->point_1, 0); + fill_point_rep_bezier(trajectory, req->point_2, 1); + fill_point_rep_bezier(trajectory, req->point_3, 2); + fill_point_rep_bezier(trajectory, req->point_4, 3); + fill_point_rep_bezier(trajectory, req->point_5, 4); + // [[[end]]] (checksum: a12a34d1190be94c777077f2d297918b) + + trajectory.time_usec = get_time_usec(req->header.stamp); //!< [milisecs] + uas->send_message(trajectory); + } + } + + /** + * @brief Send corrected path to the FCU. + * + * Message specification: https://mavlink.io/en/messages/common.html#TRAJECTORY + * @param req received nav_msgs Path msg + */ + void path_cb(const nav_msgs::msg::Path::SharedPtr req) + { + mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS trajectory {}; + + trajectory.time_usec = get_time_usec(req->header.stamp); //!< [milisecs] + trajectory.valid_points = std::min(NUM_POINTS, req->poses.size()); + + auto fill_point = + [&](mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, const size_t i) { + t.command[i] = UINT16_MAX; + if (req->poses.size() < i + 1) { + fill_points_all_unused(t, i); + } else { + auto & pose = req->poses[i].pose; + + fill_points_position(t.pos_x, t.pos_y, t.pos_z, pose.position, i); + fill_points_yaw_q(t.pos_yaw, pose.orientation, i); + fill_points_unused_path(t, i); + } + }; + + // [[[cog: + // for i in range(5): + // cog.outl(f"fill_point(trajectory, {i});") + // ]]] + fill_point(trajectory, 0); + fill_point(trajectory, 1); + fill_point(trajectory, 2); + fill_point(trajectory, 3); + fill_point(trajectory, 4); + // [[[end]]] (checksum: a63d2682cc16897f19da141e87ab5d60) + + uas->send_message(trajectory); + } + + void handle_trajectory( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & trajectory, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto tr_desired = mavros_msgs::msg::Trajectory(); + + auto fill_msg_point = + [&](RosPoints & p, const mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, + const size_t i) { + fill_msg_position(p.position, t, i); + fill_msg_velocity(p.velocity, t, i); + fill_msg_acceleration(p.acceleration_or_force, t, i); + p.yaw = wrap_pi((M_PI / 2.0f) - t.pos_yaw[i]); + p.yaw_rate = t.vel_yaw[i]; + tr_desired.command[i] = t.command[i]; + }; + + tr_desired.header = uas->synchronized_header("local_origin", trajectory.time_usec); + + if (trajectory.valid_points > tr_desired.point_valid.size()) { + return; + } + + for (int i = 0; i < trajectory.valid_points; ++i) { + tr_desired.point_valid[i] = true; + } + + for (size_t i = trajectory.valid_points; i < NUM_POINTS; ++i) { + tr_desired.point_valid[i] = false; + } + + // [[[cog: + // for i in range(5): + // cog.outl(f"fill_msg_point(tr_desired.point_{i+1}, trajectory, {i});") + // ]]] + fill_msg_point(tr_desired.point_1, trajectory, 0); + fill_msg_point(tr_desired.point_2, trajectory, 1); + fill_msg_point(tr_desired.point_3, trajectory, 2); + fill_msg_point(tr_desired.point_4, trajectory, 3); + fill_msg_point(tr_desired.point_5, trajectory, 4); + // [[[end]]] (checksum: a1d59b0aa0f24a18ca76f47397bca4ae) + + trajectory_desired_pub->publish(tr_desired); + } + + float wrap_pi(float a) + { + if (!std::isfinite(a)) { + return a; + } + + return fmod(a + M_PI, 2.0f * M_PI) - M_PI; + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::TrajectoryPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::TrajectoryPlugin) diff --git a/mavros_extras/src/plugins/tunnel.cpp b/mavros_extras/src/plugins/tunnel.cpp index 96a218fdf..f6382c9ae 100644 --- a/mavros_extras/src/plugins/tunnel.cpp +++ b/mavros_extras/src/plugins/tunnel.cpp @@ -1,95 +1,117 @@ +/* + * Copyright 2021 Morten Fyhn Amundsen + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief Tunnel plugin + * @file tunnel.cpp + * @author Morten Fyhn Amundsen + * + * @addtogroup plugin + * @{ + */ + #include -#include -#include #include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/tunnel.hpp" + namespace mavros { namespace extra_plugins { -class TunnelPlugin : public plugin::PluginBase +using namespace std::placeholders; // NOLINT + +/** + * @brief Tunnel plugin + * @plugin tunnel + */ +class TunnelPlugin : public plugin::Plugin { - public: - TunnelPlugin() : PluginBase(), nh_("~tunnel") {} - - void initialize(UAS& uas_) override - { - PluginBase::initialize(uas_); - sub_ = nh_.subscribe("in", 10, &TunnelPlugin::ros_callback, this); - pub_ = nh_.advertise("out", 10); - } +public: + explicit TunnelPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "tunnel") + { + sub_ = + node->create_subscription( + "~/in", 10, + std::bind(&TunnelPlugin::ros_callback, this, _1)); + pub_ = node->create_publisher("~/out", 10); + } - Subscriptions get_subscriptions() override - { - return {make_handler(&TunnelPlugin::mav_callback)}; - } + Subscriptions get_subscriptions() override + { + return { + make_handler(&TunnelPlugin::mav_callback), + }; + } + +private: + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr pub_; - private: - ros::NodeHandle nh_; - ros::Subscriber sub_; - ros::Publisher pub_; - - void ros_callback(const mavros_msgs::Tunnel::ConstPtr& ros_tunnel) - { - try - { - const auto mav_tunnel = - copy_tunnel( - *ros_tunnel); - - UAS_FCU(m_uas)->send_message_ignore_drop(mav_tunnel); - } - catch (const std::overflow_error& e) - { - ROS_ERROR_STREAM_NAMED("tunnel", e.what()); - } + void ros_callback(const mavros_msgs::msg::Tunnel::SharedPtr ros_tunnel) + { + try { + const auto mav_tunnel = + copy_tunnel( + *ros_tunnel); + + uas->send_message(mav_tunnel); + } catch (std::overflow_error & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "in error: " << ex.what()); } + } + + void mav_callback( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::TUNNEL & mav_tunnel, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + try { + const auto ros_tunnel = + copy_tunnel( + mav_tunnel); - void mav_callback(const mavlink::mavlink_message_t*, - mavlink::common::msg::TUNNEL& mav_tunnel) - { - try - { - const auto ros_tunnel = - copy_tunnel( - mav_tunnel); - - pub_.publish(ros_tunnel); - } - catch (const std::overflow_error& e) - { - ROS_ERROR_STREAM_NAMED("tunnel", e.what()); - } + pub_->publish(ros_tunnel); + } catch (std::overflow_error & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "out error: " << ex.what()); } + } + + template + static To copy_tunnel(const From & from) noexcept(false) + { + static const auto max_payload_length = mavlink::common::msg::TUNNEL().payload.max_size(); - template - static To copy_tunnel(const From& from) noexcept(false) - { - static constexpr auto max_payload_length = - sizeof(mavlink::common::msg::TUNNEL::payload) / - sizeof(mavlink::common::msg::TUNNEL::payload[0]); - - if (from.payload_length > max_payload_length) - { - throw std::overflow_error("too long payload length"); - } - - auto to = To{}; - - to.target_system = from.target_system; - to.target_component = from.target_component; - to.payload_type = from.payload_type; - to.payload_length = from.payload_length; - std::copy(from.payload.begin(), - from.payload.begin() + from.payload_length, - to.payload.begin()); - - return to; + if (from.payload_length > max_payload_length) { + throw std::overflow_error("too long payload length"); } + + auto to = To{}; + + to.target_system = from.target_system; + to.target_component = from.target_component; + to.payload_type = from.payload_type; + to.payload_length = from.payload_length; + std::copy( + from.payload.begin(), + from.payload.begin() + from.payload_length, + to.payload.begin()); + + return to; + } }; } // namespace extra_plugins } // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::TunnelPlugin, - mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::TunnelPlugin) diff --git a/mavros_extras/src/plugins/vfr_hud.cpp b/mavros_extras/src/plugins/vfr_hud.cpp new file mode 100644 index 000000000..8662baafc --- /dev/null +++ b/mavros_extras/src/plugins/vfr_hud.cpp @@ -0,0 +1,75 @@ +/* + * Copyright 2014,2016 Vladimir Ermakov. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief VFR HUD plugin + * @file vfr_hud.cpp + * @author Vladimir Ermakov + * + * @addtogroup plugin + * @{ + */ + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/vfr_hud.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT + +/** + * @brief VFR HUD plugin. + * @plugin vfr_hud + */ +class VfrHudPlugin : public plugin::Plugin +{ +public: + explicit VfrHudPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "vfr_hud") + { + vfr_pub = node->create_publisher("vfr_hud", 10); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&VfrHudPlugin::handle_vfr_hud), + }; + } + +private: + rclcpp::Publisher::SharedPtr vfr_pub; + + void handle_vfr_hud( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::VFR_HUD & vfr_hud, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto vmsg = mavros_msgs::msg::VfrHud(); + + vmsg.header.stamp = node->now(); + vmsg.airspeed = vfr_hud.airspeed; + vmsg.groundspeed = vfr_hud.groundspeed; + vmsg.heading = vfr_hud.heading; + vmsg.throttle = vfr_hud.throttle / 100.0; // comes in 0..100 range + vmsg.altitude = vfr_hud.alt; + vmsg.climb = vfr_hud.climb; + + vfr_pub->publish(vmsg); + } +}; +} // namespace extra_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::VfrHudPlugin) diff --git a/mavros_extras/src/plugins/vibration.cpp b/mavros_extras/src/plugins/vibration.cpp index d6a9bb947..c345a4978 100644 --- a/mavros_extras/src/plugins/vibration.cpp +++ b/mavros_extras/src/plugins/vibration.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2015 Nuno Marques. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Vibration plugin * @file vibration.cpp @@ -6,73 +13,79 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2015 Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include +#include + +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/vibration.hpp" -#include +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace extra_plugins { /** * @brief Vibration plugin + * @plugin vibration * * This plugin is intended to publish MAV vibration levels and accelerometer clipping from FCU. */ -class VibrationPlugin : public plugin::PluginBase { +class VibrationPlugin : public plugin::Plugin +{ public: - VibrationPlugin() : PluginBase(), - vibe_nh("~vibration") - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - vibe_nh.param("frame_id", frame_id, "base_link"); - - vibration_pub = vibe_nh.advertise("raw/vibration", 10); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&VibrationPlugin::handle_vibration) - }; - } + explicit VibrationPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "vibration") + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "frame_id", "base_link", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + + vibration_pub = node->create_publisher("~/raw/vibration", 10); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&VibrationPlugin::handle_vibration) + }; + } private: - ros::NodeHandle vibe_nh; - - std::string frame_id; + rclcpp::Publisher::SharedPtr vibration_pub; - ros::Publisher vibration_pub; + std::string frame_id; - void handle_vibration(const mavlink::mavlink_message_t *msg, mavlink::common::msg::VIBRATION &vibration) - { - auto vibe_msg = boost::make_shared(); + void handle_vibration( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::VIBRATION & vibration, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto vibe_msg = mavros_msgs::msg::Vibration(); - vibe_msg->header = m_uas->synchronized_header(frame_id, vibration.time_usec); + vibe_msg.header = uas->synchronized_header(frame_id, vibration.time_usec); - Eigen::Vector3d vib_enu = {vibration.vibration_x, vibration.vibration_y, vibration.vibration_z}; - tf::vectorEigenToMsg(ftf::transform_frame_ned_enu(vib_enu), vibe_msg->vibration); + Eigen::Vector3d vib_enu = {vibration.vibration_x, vibration.vibration_y, vibration.vibration_z}; + tf2::toMsg(ftf::transform_frame_ned_enu(vib_enu), vibe_msg.vibration); - vibe_msg->clipping[0] = vibration.clipping_0; - vibe_msg->clipping[1] = vibration.clipping_1; - vibe_msg->clipping[2] = vibration.clipping_2; + vibe_msg.clipping[0] = vibration.clipping_0; + vibe_msg.clipping[1] = vibration.clipping_1; + vibe_msg.clipping[2] = vibration.clipping_2; - vibration_pub.publish(vibe_msg); - } + vibration_pub->publish(vibe_msg); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::VibrationPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::VibrationPlugin) diff --git a/mavros_extras/src/plugins/vision_pose_estimate.cpp b/mavros_extras/src/plugins/vision_pose_estimate.cpp index 17d24b450..0d3cf2768 100644 --- a/mavros_extras/src/plugins/vision_pose_estimate.cpp +++ b/mavros_extras/src/plugins/vision_pose_estimate.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2014 M.H.Kabir. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief VisionPoseEstimate plugin * @file vision_pose_estimate.cpp @@ -7,161 +14,179 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014 M.H.Kabir. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include -#include -#include +#include + +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/utils.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" +#include "mavros/setpoint_mixin.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" +#include "mavros_msgs/msg/landing_target.hpp" -#include -#include +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; -namespace mavros { -namespace extra_plugins { /** * @brief Vision pose estimate plugin + * @plugin vision_pose * * Send pose estimation from various vision estimators * to FCU position and attitude estimators. * */ -class VisionPoseEstimatePlugin : public plugin::PluginBase, - private plugin::TF2ListenerMixin { +class VisionPoseEstimatePlugin : public plugin::Plugin, + private plugin::TF2ListenerMixin +{ public: - VisionPoseEstimatePlugin() : PluginBase(), - sp_nh("~vision_pose"), - tf_rate(10.0) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - bool tf_listen; - - // tf params - sp_nh.param("tf/listen", tf_listen, false); - sp_nh.param("tf/frame_id", tf_frame_id, "map"); - sp_nh.param("tf/child_frame_id", tf_child_frame_id, "vision_estimate"); - sp_nh.param("tf/rate_limit", tf_rate, 10.0); - - if (tf_listen) { - ROS_INFO_STREAM_NAMED("vision_pose", "Listen to vision transform " << tf_frame_id - << " -> " << tf_child_frame_id); - tf2_start("VisionPoseTF", &VisionPoseEstimatePlugin::transform_cb); - } - else { - vision_sub = sp_nh.subscribe("pose", 10, &VisionPoseEstimatePlugin::vision_cb, this); - vision_cov_sub = sp_nh.subscribe("pose_cov", 10, &VisionPoseEstimatePlugin::vision_cov_cb, this); - } - } - - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + explicit VisionPoseEstimatePlugin(plugin::UASPtr uas_) + : Plugin(uas_, "vision_pose"), + tf_rate(10.0) + { + enable_node_watch_parameters(); + + // tf params + node_declare_and_watch_parameter( + "tf/listen", false, [&](const rclcpp::Parameter & p) { + auto tf_listen = p.as_bool(); + if (tf_listen) { + RCLCPP_INFO_STREAM( + get_logger(), + "Listen to vision transform" << tf_frame_id << + " -> " << tf_child_frame_id); + tf2_start("VisionPoseTF", &VisionPoseEstimatePlugin::transform_cb); + } else { + vision_sub = node->create_subscription( + "~/pose", 10, std::bind( + &VisionPoseEstimatePlugin::vision_cb, this, _1)); + vision_cov_sub = node->create_subscription( + "~/pose_cov", 10, std::bind( + &VisionPoseEstimatePlugin::vision_cov_cb, this, _1)); + } + }); + + node_declare_and_watch_parameter( + "tf/frame_id", "map", [&](const rclcpp::Parameter & p) { + tf_frame_id = p.as_string(); + }); + + node_declare_and_watch_parameter( + "tf/child_frame_id", "vision_estimate", [&](const rclcpp::Parameter & p) { + tf_child_frame_id = p.as_string(); + }); + + node_declare_and_watch_parameter( + "tf/rate_limit", 10.0, [&](const rclcpp::Parameter & p) { + tf_rate = p.as_double(); + }); + } + + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - friend class TF2ListenerMixin; - ros::NodeHandle sp_nh; - - ros::Subscriber vision_sub; - ros::Subscriber vision_cov_sub; - - std::string tf_frame_id; - std::string tf_child_frame_id; - double tf_rate; - ros::Time last_transform_stamp; - - /* -*- low-level send -*- */ - /** - * @brief Send vision estimate transform to FCU position controller - */ - void send_vision_estimate(const ros::Time &stamp, const Eigen::Affine3d &tr, const geometry_msgs::PoseWithCovariance::_covariance_type &cov) - { - /** - * @warning Issue #60. - * This now affects pose callbacks too. - */ - if (last_transform_stamp == stamp) { - ROS_DEBUG_THROTTLE_NAMED(10, "vision_pose", "Vision: Same transform as last one, dropped."); - return; - } - last_transform_stamp = stamp; - - auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation())); - auto rpy = ftf::quaternion_to_rpy( - ftf::transform_orientation_enu_ned( - ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())))); - - auto cov_ned = ftf::transform_frame_enu_ned(cov); - ftf::EigenMapConstCovariance6d cov_map(cov_ned.data()); - - auto urt_view = Eigen::Matrix(cov_map.triangularView()); - ROS_DEBUG_STREAM_NAMED("vision_pose", "Vision: Covariance URT: " << std::endl << urt_view); - - mavlink::common::msg::VISION_POSITION_ESTIMATE vp{}; - - vp.usec = stamp.toNSec() / 1000; - // [[[cog: - // for f in "xyz": - // cog.outl("vp.%s = position.%s();" % (f, f)) - // for a, b in zip("xyz", ('roll', 'pitch', 'yaw')): - // cog.outl("vp.%s = rpy.%s();" % (b, a)) - // ]]] - vp.x = position.x(); - vp.y = position.y(); - vp.z = position.z(); - vp.roll = rpy.x(); - vp.pitch = rpy.y(); - vp.yaw = rpy.z(); - // [[[end]]] (checksum: 2048daf411780847e77f08fe5a0b9dd3) - - // just the URT of the 6x6 Pose Covariance Matrix, given - // that the matrix is symmetric - ftf::covariance_urt_to_mavlink(cov_map, vp.covariance); - - UAS_FCU(m_uas)->send_message_ignore_drop(vp); - } - - /* -*- callbacks -*- */ - - /* common TF listener moved to mixin */ - - void transform_cb(const geometry_msgs::TransformStamped &transform) - { - Eigen::Affine3d tr; - tf::transformMsgToEigen(transform.transform, tr); - ftf::Covariance6d cov {}; // zero initialized - - send_vision_estimate(transform.header.stamp, tr, cov); - } - - void vision_cb(const geometry_msgs::PoseStamped::ConstPtr &req) - { - Eigen::Affine3d tr; - tf::poseMsgToEigen(req->pose, tr); - ftf::Covariance6d cov {}; // zero initialized - - send_vision_estimate(req->header.stamp, tr, cov); - } - - void vision_cov_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &req) - { - Eigen::Affine3d tr; - tf::poseMsgToEigen(req->pose.pose, tr); - - send_vision_estimate(req->header.stamp, tr, req->pose.covariance); - } + friend class TF2ListenerMixin; + + rclcpp::Subscription::SharedPtr vision_sub; + rclcpp::Subscription::SharedPtr vision_cov_sub; + + std::string tf_frame_id; + std::string tf_child_frame_id; + + double tf_rate; + + rclcpp::Time last_transform_stamp{0, 0, RCL_ROS_TIME}; + + /* -*- low-level send -*- */ + /** + * @brief Send vision estimate transform to FCU position controller + */ + void send_vision_estimate( + const rclcpp::Time & stamp, const Eigen::Affine3d & tr, + const geometry_msgs::msg::PoseWithCovariance::_covariance_type & cov) + { + if (last_transform_stamp == stamp) { + RCLCPP_DEBUG_THROTTLE( + get_logger(), + *get_clock(), 10, "Vision: Same transform as last one, dropped."); + return; + } + last_transform_stamp = stamp; + + auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation())); + auto rpy = ftf::quaternion_to_rpy( + ftf::transform_orientation_enu_ned( + ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())))); + + auto cov_ned = ftf::transform_frame_enu_ned(cov); + ftf::EigenMapConstCovariance6d cov_map(cov_ned.data()); + + mavlink::common::msg::VISION_POSITION_ESTIMATE vp{}; + + vp.usec = stamp.nanoseconds() / 1000; + // [[[cog: + // for f in "xyz": + // cog.outl(f"vp.{f} = position.{f}();") + // for a, b in zip("xyz", ('roll', 'pitch', 'yaw')): + // cog.outl(f"vp.{b} = rpy.{a}();") + // ]]] + vp.x = position.x(); + vp.y = position.y(); + vp.z = position.z(); + vp.roll = rpy.x(); + vp.pitch = rpy.y(); + vp.yaw = rpy.z(); + // [[[end]]] (checksum: 0aed118405958e3f35e8e7c9386e812f) + + // just the URT of the 6x6 Pose Covariance Matrix, given + // that the matrix is symmetric + ftf::covariance_urt_to_mavlink(cov_map, vp.covariance); + + uas->send_message(vp); + } + + /* -*- callbacks -*- */ + + /* common TF listener moved to mixin */ + + void transform_cb(const geometry_msgs::msg::TransformStamped & transform) + { + Eigen::Affine3d tr = tf2::transformToEigen(transform.transform); + ftf::Covariance6d cov {}; // zero initialized + + send_vision_estimate(transform.header.stamp, tr, cov); + } + + void vision_cb(const geometry_msgs::msg::PoseStamped::SharedPtr req) + { + Eigen::Affine3d tr; + tf2::fromMsg(req->pose, tr); + ftf::Covariance6d cov {}; // zero initialized + + send_vision_estimate(req->header.stamp, tr, cov); + } + + void vision_cov_cb(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr req) + { + Eigen::Affine3d tr; + tf2::fromMsg(req->pose.pose, tr); + send_vision_estimate(req->header.stamp, tr, req->pose.covariance); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::VisionPoseEstimatePlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::VisionPoseEstimatePlugin) diff --git a/mavros_extras/src/plugins/vision_speed_estimate.cpp b/mavros_extras/src/plugins/vision_speed_estimate.cpp index a917feb16..6ea4bc767 100644 --- a/mavros_extras/src/plugins/vision_speed_estimate.cpp +++ b/mavros_extras/src/plugins/vision_speed_estimate.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2014, 2018 Nuno Marques. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief VisionSpeedEstimate plugin * @file vision_speed.cpp @@ -7,156 +14,151 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2014, 2018 Nuno Marques. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ -#include +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" -#include -#include +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" -#include +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT -namespace mavros { -namespace extra_plugins { /** * @brief Vision speed estimate plugin + * @plugin vision_speed * * Send velocity estimation from various vision estimators * to FCU position and attitude estimators. */ -class VisionSpeedEstimatePlugin : public plugin::PluginBase { +class VisionSpeedEstimatePlugin : public plugin::Plugin +{ public: - VisionSpeedEstimatePlugin() : PluginBase(), - sp_nh("~vision_speed"), - listen_twist(true), - twist_cov(true) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - sp_nh.param("listen_twist", listen_twist, true); - sp_nh.param("twist_cov", twist_cov, true); - - if (listen_twist) { - if (twist_cov) - vision_twist_cov_sub = sp_nh.subscribe("speed_twist_cov", 10, &VisionSpeedEstimatePlugin::twist_cov_cb, this); - else - vision_twist_sub = sp_nh.subscribe("speed_twist", 10, &VisionSpeedEstimatePlugin::twist_cb, this); - } - else - vision_vector_sub = sp_nh.subscribe("speed_vector", 10, &VisionSpeedEstimatePlugin::vector_cb, this); - } - - Subscriptions get_subscriptions() override - { - return { /* Rx disabled */ }; - } + explicit VisionSpeedEstimatePlugin(plugin::UASPtr uas_) + : Plugin(uas_, "vision_speed") + { + vision_twist_cov_sub = + node->create_subscription( + "~/speed_twist_cov", 10, std::bind(&VisionSpeedEstimatePlugin::twist_cov_cb, this, _1)); + vision_twist_sub = node->create_subscription( + "~/speed_twist", 10, + std::bind(&VisionSpeedEstimatePlugin::twist_cb, this, _1)); + vision_vector_sub = node->create_subscription( + "~/speed_vector", 10, std::bind(&VisionSpeedEstimatePlugin::vector_cb, this, _1)); + } + + Subscriptions get_subscriptions() override + { + return { /* Rx disabled */}; + } private: - ros::NodeHandle sp_nh; - - bool listen_twist; //!< If True, listen to Twist data topics - bool twist_cov; //!< If True, listen to TwistWithCovariance data topic - - ros::Subscriber vision_twist_sub; //!< Subscriber to geometry_msgs/TwistStamped msgs - ros::Subscriber vision_twist_cov_sub; //!< Subscriber to geometry_msgs/TwistWithCovarianceStamped msgs - ros::Subscriber vision_vector_sub; //!< Subscriber to geometry_msgs/Vector3Stamped msgs - - /* -*- low-level send -*- */ - /** - * @brief Send vision speed estimate on local NED frame to the FCU. - * - * Message specification: https://mavlink.io/en/messages/common.html#VISION_SPEED_ESTIMATE - * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) (us) - * @param v Velocity/speed vector in the local NED frame (meters) - * @param cov Linear velocity covariance matrix (local NED frame) - */ - void send_vision_speed_estimate(const uint64_t usec, const Eigen::Vector3d &v, const ftf::Covariance3d &cov) - { - mavlink::common::msg::VISION_SPEED_ESTIMATE vs {}; - - vs.usec = usec; - - // [[[cog: - // for f in "xyz": - // cog.outl("vs.%s = v.%s();" % (f, f)) - // ]]] - vs.x = v.x(); - vs.y = v.y(); - vs.z = v.z(); - // [[[end]]] (checksum: aee3cc9a73a2e736b7bc6c83ea93abdb) - - ftf::covariance_to_mavlink(cov, vs.covariance); - - UAS_FCU(m_uas)->send_message_ignore_drop(vs); - } - - /* -*- mid-level helpers -*- */ - /** - * @brief Convert vector and covariance from local ENU to local NED frame - * - * @param stamp ROS timestamp of the message - * @param vel_enu Velocity/speed vector in the ENU frame - * @param cov_enu Linear velocity/speed in the ENU frame - */ - void convert_vision_speed(const ros::Time &stamp, const Eigen::Vector3d &vel_enu, const ftf::Covariance3d &cov_enu) - { - // Send transformed data from local ENU to NED frame - send_vision_speed_estimate(stamp.toNSec() / 1000, - ftf::transform_frame_enu_ned(vel_enu), - ftf::transform_frame_enu_ned(cov_enu)); - } - - /* -*- callbacks -*- */ - /** - * @brief Callback to geometry_msgs/TwistStamped msgs - * - * @param req received geometry_msgs/TwistStamped msg - */ - void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req) { - ftf::Covariance3d cov {}; // zero initialized - - convert_vision_speed(req->header.stamp, ftf::to_eigen(req->twist.linear), cov); - } - - /** - * @brief Callback to geometry_msgs/TwistWithCovarianceStamped msgs - * - * @param req received geometry_msgs/TwistWithCovarianceStamped msg - */ - void twist_cov_cb(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &req) { - ftf::Covariance3d cov3d {}; // zero initialized - - ftf::EigenMapCovariance3d cov3d_map(cov3d.data()); - ftf::EigenMapConstCovariance6d cov6d_map(req->twist.covariance.data()); - - // only the linear velocity will be sent - cov3d_map = cov6d_map.block<3, 3>(0, 0); - - convert_vision_speed(req->header.stamp, ftf::to_eigen(req->twist.twist.linear), cov3d); - } - - /** - * @brief Callback to geometry_msgs/Vector3Stamped msgs - * - * @param req received geometry_msgs/Vector3Stamped msg - */ - void vector_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req) { - ftf::Covariance3d cov {}; // zero initialized - - convert_vision_speed(req->header.stamp, ftf::to_eigen(req->vector), cov); - } + rclcpp::Subscription::SharedPtr vision_twist_sub; + rclcpp::Subscription::SharedPtr + vision_twist_cov_sub; + rclcpp::Subscription::SharedPtr vision_vector_sub; + + /* -*- low-level send -*- */ + /** + * @brief Send vision speed estimate on local NED frame to the FCU. + * + * Message specification: https://mavlink.io/en/messages/common.html#VISION_SPEED_ESTIMATE + * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) (us) + * @param v Velocity/speed vector in the local NED frame (meters) + * @param cov Linear velocity covariance matrix (local NED frame) + */ + void send_vision_speed_estimate( + const uint64_t usec, const Eigen::Vector3d & v, + const ftf::Covariance3d & cov) + { + mavlink::common::msg::VISION_SPEED_ESTIMATE vs {}; + + vs.usec = usec; + + // [[[cog: + // for f in "xyz": + // cog.outl("vs.%s = v.%s();" % (f, f)) + // ]]] + vs.x = v.x(); + vs.y = v.y(); + vs.z = v.z(); + // [[[end]]] (checksum: c0c3a3d4dea27c5dc44e4d4f982ff1b6) + + ftf::covariance_to_mavlink(cov, vs.covariance); + + uas->send_message(vs); + } + + /* -*- mid-level helpers -*- */ + /** + * @brief Convert vector and covariance from local ENU to local NED frame + * + * @param stamp ROS timestamp of the message + * @param vel_enu Velocity/speed vector in the ENU frame + * @param cov_enu Linear velocity/speed in the ENU frame + */ + void convert_vision_speed( + const rclcpp::Time & stamp, const Eigen::Vector3d & vel_enu, + const ftf::Covariance3d & cov_enu) + { + // Send transformed data from local ENU to NED frame + send_vision_speed_estimate( + get_time_usec(stamp), + ftf::transform_frame_enu_ned(vel_enu), + ftf::transform_frame_enu_ned(cov_enu)); + } + + /* -*- callbacks -*- */ + /** + * @brief Callback to geometry_msgs/TwistStamped msgs + * + * @param req received geometry_msgs/TwistStamped msg + */ + void twist_cb(const geometry_msgs::msg::TwistStamped::SharedPtr req) + { + ftf::Covariance3d cov {}; // zero initialized + + convert_vision_speed(req->header.stamp, ftf::to_eigen(req->twist.linear), cov); + } + + /** + * @brief Callback to geometry_msgs/TwistWithCovarianceStamped msgs + * + * @param req received geometry_msgs/TwistWithCovarianceStamped msg + */ + void twist_cov_cb(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr req) + { + ftf::Covariance3d cov3d {}; // zero initialized + + ftf::EigenMapCovariance3d cov3d_map(cov3d.data()); + ftf::EigenMapConstCovariance6d cov6d_map(req->twist.covariance.data()); + + // only the linear velocity will be sent + cov3d_map = cov6d_map.block<3, 3>(0, 0); + + convert_vision_speed(req->header.stamp, ftf::to_eigen(req->twist.twist.linear), cov3d); + } + + /** + * @brief Callback to geometry_msgs/Vector3Stamped msgs + * + * @param req received geometry_msgs/Vector3Stamped msg + */ + void vector_cb(const geometry_msgs::msg::Vector3Stamped::SharedPtr req) + { + ftf::Covariance3d cov {}; // zero initialized + + convert_vision_speed(req->header.stamp, ftf::to_eigen(req->vector), cov); + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::VisionSpeedEstimatePlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::VisionSpeedEstimatePlugin) diff --git a/mavros_extras/src/plugins/wheel_odometry.cpp b/mavros_extras/src/plugins/wheel_odometry.cpp index 3f84eb4b5..b85eb2a37 100644 --- a/mavros_extras/src/plugins/wheel_odometry.cpp +++ b/mavros_extras/src/plugins/wheel_odometry.cpp @@ -1,3 +1,10 @@ +/* + * Copyright 2017 Pavlo Kolomiiets. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ /** * @brief Wheel odometry plugin * @file wheel_odometry.cpp @@ -6,587 +13,659 @@ * @addtogroup plugin * @{ */ -/* - * Copyright 2017 Pavlo Kolomiiets. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include -#include -#include -#include -#include -#include #include -namespace mavros { -namespace extra_plugins { +#include +#include +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/wheel_odom_stamped.hpp" +#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT + /** * @brief Wheel odometry plugin. + * @plugin wheel_odomotry * * This plugin allows computing and publishing wheel odometry coming from FCU wheel encoders. * Can use either wheel's RPM or WHEEL_DISTANCE messages (the latter gives better accuracy). - * */ -class WheelOdometryPlugin : public plugin::PluginBase { +class WheelOdometryPlugin : public plugin::Plugin +{ public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - WheelOdometryPlugin() : PluginBase(), - wo_nh("~wheel_odometry"), - count(0), - odom_mode(OM::NONE), - raw_send(false), - twist_send(false), - tf_send(false), - yaw_initialized(false), - rpose(Eigen::Vector3d::Zero()), - rtwist(Eigen::Vector3d::Zero()), - rpose_cov(Eigen::Matrix3d::Zero()), - rtwist_cov(Eigen::Vector3d::Zero()) - { } - - void initialize(UAS &uas_) override - { - PluginBase::initialize(uas_); - - // General params - wo_nh.param("send_raw", raw_send, false); - // Wheels configuration - wo_nh.param("count", count, 2); - count = std::max(1, count); // bound check - - bool use_rpm; - wo_nh.param("use_rpm", use_rpm, false); - if (use_rpm) - odom_mode = OM::RPM; - else - odom_mode = OM::DIST; - - // Odometry params - wo_nh.param("send_twist", twist_send, false); - wo_nh.param("frame_id", frame_id, "odom"); - wo_nh.param("child_frame_id", child_frame_id, "base_link"); - wo_nh.param("vel_error", vel_cov, 0.1); - vel_cov = vel_cov * vel_cov; // std -> cov - // TF subsection - wo_nh.param("tf/send", tf_send, false); - wo_nh.param("tf/frame_id", tf_frame_id, "odom"); - wo_nh.param("tf/child_frame_id", tf_child_frame_id, "base_link"); - - // Read parameters for each wheel. - { - int iwheel = 0; - while (true) { - // Build the string in the form "wheelX", where X is the wheel number. - // Check if we have "wheelX" parameter. - // Indices starts from 0 and should increase without gaps. - auto name = utils::format("wheel%i", iwheel++); - - // Check if we have "wheelX" parameter - if (!wo_nh.hasParam(name)) break; - - // Read - Eigen::Vector2d offset; - double radius; - - wo_nh.param(name + "/x", offset[0], 0.0); - wo_nh.param(name + "/y", offset[1], 0.0); - wo_nh.param(name + "/radius", radius, 0.05); - - wheel_offset.push_back(offset); - wheel_radius.push_back(radius); - } - - // Check for all wheels specified - if (wheel_offset.size() >= count) { - // Duplicate 1st wheel if only one is available. - // This generalizes odometry computations for 1- and 2-wheels configurations. - if (wheel_radius.size() == 1) { - wheel_offset.resize(2); - wheel_radius.resize(2); - wheel_offset[1].x() = wheel_offset[0].x(); - wheel_offset[1].y() = wheel_offset[0].y() + 1.0;// make separation non-zero to avoid div-by-zero - wheel_radius[1] = wheel_radius[0]; - } - - // Check for non-zero wheel separation (first two wheels) - double separation = std::abs(wheel_offset[1].y() - wheel_offset[0].y()); - if (separation < 1.e-5) { - odom_mode = OM::NONE; - ROS_WARN_NAMED("wo", "WO: Separation between the first two wheels is too small (%f).", separation); - } - - // Check for reasonable radiuses - for (int i = 0; i < wheel_radius.size(); i++) { - if (wheel_radius[i] <= 1.e-5) { - odom_mode = OM::NONE; - ROS_WARN_NAMED("wo", "WO: Wheel #%i has incorrect radius (%f).", i, wheel_radius[i]); - } - } - } - else { - odom_mode = OM::NONE; - ROS_WARN_NAMED("wo", "WO: Not all wheels have parameters specified (%lu/%i).", wheel_offset.size(), count); - } - } - - // Advertise RPM-s and distance-s - if (raw_send) { - rpm_pub = wo_nh.advertise("rpm", 10); - dist_pub = wo_nh.advertise("distance", 10); - } - - // Advertize topics - if (odom_mode != OM::NONE) { - if (twist_send) - twist_pub = wo_nh.advertise("velocity", 10); - else - odom_pub = wo_nh.advertise("odom", 10); - } - // No-odometry warning - else - ROS_WARN_NAMED("wo", "WO: No odometry computations will be performed."); - } - - Subscriptions get_subscriptions() override - { - return { - make_handler(&WheelOdometryPlugin::handle_rpm), - make_handler(&WheelOdometryPlugin::handle_wheel_distance) - }; - } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit WheelOdometryPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "wheel_odometry"), + odom_mode(OM::NONE), + count(0), + raw_send(false), + twist_send(false), + tf_send(false), + yaw_initialized(false), + rpose(Eigen::Vector3d::Zero()), + rtwist(Eigen::Vector3d::Zero()), + rpose_cov(Eigen::Matrix3d::Zero()), + rtwist_cov(Eigen::Vector3d::Zero()) + { + enable_node_watch_parameters(); + + // General params + node_declare_and_watch_parameter( + "send_raw", false, [&](const rclcpp::Parameter & p) { + raw_send = p.as_bool(); + + if (raw_send) { + rpm_pub = node->create_publisher("~/rpm", 10); + dist_pub = node->create_publisher("~/distance", 10); + } else { + rpm_pub.reset(); + dist_pub.reset(); + } + }); + + node_declare_and_watch_parameter( + "count", 2, [&](const rclcpp::Parameter & p) { + int count_ = p.as_int(); + count = std::max(2, count_); // bound check + }); + + node_declare_and_watch_parameter( + "use_rpm", false, [&](const rclcpp::Parameter & p) { + bool use_rpm = p.as_bool(); + if (use_rpm) { + odom_mode = OM::RPM; + } else { + odom_mode = OM::DIST; + } + }); + + // Odometry params + node_declare_and_watch_parameter( + "send_twist", false, [&](const rclcpp::Parameter & p) { + twist_send = p.as_bool(); + }); + + node_declare_and_watch_parameter( + "frame_id", "odom", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + + node_declare_and_watch_parameter( + "child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + + node_declare_and_watch_parameter( + "vel_error", 0.1, [&](const rclcpp::Parameter & p) { + double vel_error = p.as_double(); + vel_cov = vel_error * vel_error; // std -> cov + }); + + // TF subsection + node_declare_and_watch_parameter( + "tf.frame_id", "odom", [&](const rclcpp::Parameter & p) { + tf_frame_id = p.as_string(); + }); + + node_declare_and_watch_parameter( + "tf.child_frame_id", "base_link", [&](const rclcpp::Parameter & p) { + tf_child_frame_id = p.as_string(); + }); + + node_declare_and_watch_parameter( + "tf.send", false, [&](const rclcpp::Parameter & p) { + tf_send = p.as_bool(); + }); + + // Read parameters for each wheel. + wheel_offset.resize(count); + wheel_radius.resize(count); + + for (int wi = 0; wi < count; wi++) { + // Build the string in the form "wheelX", where X is the wheel number. + // Check if we have "wheelX" parameter. + // Indices starts from 0 and should increase without gaps. + + node_declare_and_watch_parameter( + utils::format("wheel%i.x", wi), 0.0, [wi, this](const rclcpp::Parameter & p) { + wheel_offset[wi][0] = p.as_double(); + }); + node_declare_and_watch_parameter( + utils::format("wheel%i.y", wi), 0.0, [wi, this](const rclcpp::Parameter & p) { + wheel_offset[wi][1] = p.as_double(); + }); + node_declare_and_watch_parameter( + utils::format("wheel%i.radius", wi), 0.05, [wi, this](const rclcpp::Parameter & p) { + wheel_radius[wi] = p.as_double(); + }); + } + +#if 0 // TODO(vooon): port this + // Check for all wheels specified + if (wheel_offset.size() >= count) { + // Duplicate 1st wheel if only one is available. + // This generalizes odometry computations for 1- and 2-wheels configurations. + if (wheel_radius.size() == 1) { + wheel_offset.resize(2); + wheel_radius.resize(2); + wheel_offset[1].x() = wheel_offset[0].x(); + // make separation non-zero to avoid div-by-zero + wheel_offset[1].y() = wheel_offset[0].y() + 1.0; + wheel_radius[1] = wheel_radius[0]; + } + + // Check for non-zero wheel separation (first two wheels) + double separation = std::abs(wheel_offset[1].y() - wheel_offset[0].y()); + if (separation < 1.e-5) { + odom_mode = OM::NONE; + ROS_WARN_NAMED( + "wo", "WO: Separation between the first two wheels is too small (%f).", + separation); + } + + // Check for reasonable radiuses + for (int i = 0; i < wheel_radius.size(); i++) { + if (wheel_radius[i] <= 1.e-5) { + odom_mode = OM::NONE; + ROS_WARN_NAMED("wo", "WO: Wheel #%i has incorrect radius (%f).", i, wheel_radius[i]); + } + } + } else { + odom_mode = OM::NONE; + ROS_WARN_NAMED( + "wo", "WO: Not all wheels have parameters specified (%lu/%i).", + wheel_offset.size(), count); + } +#endif + + // Advertize topics + if (odom_mode != OM::NONE) { + if (twist_send) { + twist_pub = node->create_publisher( + "~/velocity", 10); + } else { + odom_pub = node->create_publisher("~/odom", 10); + } + } else { + // No-odometry warning + RCLCPP_WARN(get_logger(), "WO: No odometry computations will be performed."); + } + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&WheelOdometryPlugin::handle_rpm), + make_handler(&WheelOdometryPlugin::handle_wheel_distance) + }; + } private: - ros::NodeHandle wo_nh; - - ros::Publisher rpm_pub; - ros::Publisher dist_pub; - ros::Publisher odom_pub; - ros::Publisher twist_pub; - - /// @brief Odometry computation modes - enum class OM { - NONE, //!< no odometry computation - RPM, //!< use wheel's RPM - DIST //!< use wheel's cumulative distance - }; - OM odom_mode; //!< odometry computation mode - - int count; //!< requested number of wheels to compute odometry - bool raw_send; //!< send wheel's RPM and cumulative distance - std::vector wheel_offset; //!< wheel x,y offsets (m,NED) - std::vector wheel_radius; //!< wheel radiuses (m) - - bool twist_send; //!< send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry - bool tf_send; //!< send TF - std::string frame_id; //!< origin frame for topic headers - std::string child_frame_id; //!< body-fixed frame for topic headers - std::string tf_frame_id; //!< origin for TF - std::string tf_child_frame_id; //!< frame for TF and Pose - double vel_cov; //!< wheel velocity measurement error 1-var (m/s) - - int count_meas; //!< number of wheels in measurements - ros::Time time_prev; //!< timestamp of previous measurement - std::vector measurement_prev; //!< previous measurement - - bool yaw_initialized; //!< initial yaw initialized (from IMU) - - /// @brief Robot origin 2D-state (SI units) - Eigen::Vector3d rpose; //!< pose (x, y, yaw) - Eigen::Vector3d rtwist; //!< twist (vx, vy, vyaw) - Eigen::Matrix3d rpose_cov; //!< pose error 1-var - Eigen::Vector3d rtwist_cov; //!< twist error 1-var (vx_cov, vy_cov, vyaw_cov) - - /** - * @brief Publish odometry. - * Odometry is computed from the very start but no pose info is published until we have initial orientation (yaw). - * Once we get it, the robot's current pose is updated with it and starts to be published. - * Twist info doesn't depend on initial orientation so is published from the very start. - * @param time measurement's ROS time stamp - */ - void publish_odometry(ros::Time time) - { - // Get initial yaw (from IMU) - // Check that IMU was already initialized - if (!yaw_initialized && m_uas->get_attitude_imu_enu()) { - double yaw = ftf::quaternion_get_yaw(ftf::to_eigen(m_uas->get_attitude_orientation_enu())); - - // Rotate current pose by initial yaw - Eigen::Rotation2Dd rot(yaw); - rpose.head(2) = rot * rpose.head(2); // x,y - rpose(2) += yaw;// yaw - - ROS_INFO_NAMED("wo", "WO: Initial yaw (deg): %f", yaw / M_PI * 180.0); - yaw_initialized = true; - } - - // Orientation (only if we have initial yaw) - geometry_msgs::Quaternion quat; - if (yaw_initialized) - quat = tf2::toMsg(ftf::quaternion_from_rpy(0.0, 0.0, rpose(2))); - - // Twist - geometry_msgs::TwistWithCovariance twist_cov; - // linear - twist_cov.twist.linear.x = rtwist(0); - twist_cov.twist.linear.y = rtwist(1); - twist_cov.twist.linear.z = 0.0; - // angular - twist_cov.twist.angular.x = 0.0; - twist_cov.twist.angular.y = 0.0; - twist_cov.twist.angular.z = rtwist(2); - // covariance - ftf::EigenMapCovariance6d twist_cov_map(twist_cov.covariance.data()); - twist_cov_map.setZero(); - twist_cov_map.block<2, 2>(0, 0).diagonal() << rtwist_cov(0), rtwist_cov(1); - twist_cov_map.block<1, 1>(5, 5).diagonal() << rtwist_cov(2); - - // Publish twist - if (twist_send) { - auto twist_cov_t = boost::make_shared(); - // header - twist_cov_t->header.stamp = time; - twist_cov_t->header.frame_id = frame_id; - // twist - twist_cov_t->twist = twist_cov; - // publish - twist_pub.publish(twist_cov_t); - } - // Publish odometry (only if we have initial yaw) - else if (yaw_initialized) { - auto odom = boost::make_shared(); - // header - odom->header.stamp = time; - odom->header.frame_id = frame_id; - odom->child_frame_id = child_frame_id; - // pose - odom->pose.pose.position.x = rpose(0); - odom->pose.pose.position.y = rpose(1); - odom->pose.pose.position.z = 0.0; - odom->pose.pose.orientation = quat; - ftf::EigenMapCovariance6d pose_cov_map(odom->pose.covariance.data()); - pose_cov_map.block<2, 2>(0, 0) << rpose_cov.block<2, 2>(0, 0); - pose_cov_map.block<2, 1>(0, 5) << rpose_cov.block<2, 1>(0, 2); - pose_cov_map.block<1, 2>(5, 0) << rpose_cov.block<1, 2>(2, 0); - pose_cov_map.block<1, 1>(5, 5) << rpose_cov.block<1, 1>(2, 2); - // twist - odom->twist = twist_cov; - // publish - odom_pub.publish(odom); - } - - // Publish TF (only if we have initial yaw) - if (tf_send && yaw_initialized) { - geometry_msgs::TransformStamped transform; - // header - transform.header.stamp = time; - transform.header.frame_id = tf_frame_id; - transform.child_frame_id = tf_child_frame_id; - // translation - transform.transform.translation.x = rpose(0); - transform.transform.translation.y = rpose(1); - transform.transform.translation.z = 0.0; - // rotation - transform.transform.rotation = quat; - // publish - m_uas->tf2_broadcaster.sendTransform(transform); - } - } - - /** - * @brief Update odometry for differential drive robot. - * Odometry is computed for robot's origin (IMU). - * The wheels are assumed to be parallel to the robot's x-direction (forward) and with the same x-offset. - * No slip is assumed (Instantaneous Center of Curvature (ICC) along the axis connecting the wheels). - * All computations are performed for ROS frame conventions. - * The approach is the extended and more accurate version of standard one described in the book - * https://github.com/correll/Introduction-to-Autonomous-Robots - * and at the page (with some typos fixed) - * http://correll.cs.colorado.edu/?p=1307 - * The extension is that exact pose update is used instead of approximate, - * and that the robot's origin can be specified anywhere instead of the middle-point between the wheels. - * @param distance distance traveled by each wheel since last odometry update - * @param dt time elapse since last odometry update (s) - */ - void update_odometry_diffdrive(std::vector distance, double dt) - { - double y0 = wheel_offset[0](1); - double y1 = wheel_offset[1](1); - double a = -wheel_offset[0](0); - double dy_inv = 1.0 / (y1 - y0); - double dt_inv = 1.0 / dt; - - // Rotation angle - double theta = (distance[1] - distance[0]) * dy_inv; - // Distance traveled by the projection of origin onto the axis connecting the wheels (Op) - double L = (y1 * distance[0] - y0 * distance[1]) * dy_inv; - - // Instantenous pose update in local (robot) coordinate system (vel*dt) - Eigen::Vector3d v(L, a * theta, theta); - // Instantenous local twist - rtwist = v * dt_inv; - - // Compute local pose update (approximate). - // In the book a=0 and |y0|=|y1|, additionally. - // dx = L*cos(theta/2) - a*theta*sin(theta/2) - // dy = L*sin(theta/2) + a*theta*cos(theta/2) - // Compute local pose update (exact) - // dx = a*(cos(theta)-1) + R*sin(theta) - // dy = a*sin(theta) - R*(cos(theta)-1) - // where R - rotation radius of Op around ICC (R = L/theta). - double cos_theta = std::cos(theta); - double sin_theta = std::sin(theta); - double p; // sin(theta)/theta - double q; // (1-cos(theta))/theta - if (std::abs(theta) > 1.e-5) { - p = sin_theta / theta; - q = (1.0 - cos_theta) / theta; - } - // Limits for theta -> 0 - else { - p = 1.0; - q = 0.0; - } - - // Local pose update matrix - Eigen::Matrix3d M; - M << p, -q, 0, - q, p, 0, - 0, 0, 1; - - // Local pose update - Eigen::Vector3d dpose = M * v; - - // Rotation by yaw - double cy = std::cos(rpose(2)); - double sy = std::sin(rpose(2)); - Eigen::Matrix3d R; - R << cy, -sy, 0, - sy, cy, 0, - 0, 0, 1; - - // World pose - rpose += R * dpose; - rpose(2) = fmod(rpose(2), 2.0 * M_PI); // Clamp to (-2*PI, 2*PI) - - // Twist errors (constant in time) - if (rtwist_cov(0) == 0.0) { - rtwist_cov(0) = vel_cov * (y0 * y0 + y1 * y1) * dy_inv * dy_inv;// vx_cov - rtwist_cov(1) = vel_cov * a * a * 2.0 * dy_inv * dy_inv + 0.001;// vy_cov (add extra error, otherwise vy_cov= 0 if a=0) - rtwist_cov(2) = vel_cov * 2.0 * dy_inv * dy_inv;// vyaw_cov - } - - // Pose errors (accumulated in time). - // Exact formulations respecting kinematic equations. - // dR/dYaw - Eigen::Matrix3d R_yaw; - R_yaw << -sy, -cy, 0, - cy, -sy, 0, - 0, 0, 0; - // dYaw/dPose - Eigen::Vector3d yaw_pose(0, 0, 1); - // Jacobian by previous pose - Eigen::Matrix3d J_pose = Eigen::Matrix3d::Identity() + R_yaw * dpose * yaw_pose.transpose(); - - // dL,dTheta / dL0,dL1 - double L_L0 = y1 * dy_inv; - double L_L1 = -y0 * dy_inv; - double theta_L0 = -dy_inv; - double theta_L1 = dy_inv; - // dv/dMeasurement - Eigen::Matrix v_meas; - v_meas << L_L0, L_L1, - a*theta_L0, a*theta_L1, - theta_L0, theta_L1; - // dTheta/dMeasurement - Eigen::Vector2d theta_meas(theta_L0, theta_L1); - // dM/dTheta - double px; // dP/dTheta - double qx; // dQ/dTheta - if (std::abs(theta) > 1.e-5) { - px = (theta * cos_theta - sin_theta) / (theta * theta); - qx = (theta * sin_theta - (1 - cos_theta)) / (theta * theta); - } - // Limits for theta -> 0 - else { - px = 0; - qx = 0.5; - } - // dM/dTheta - Eigen::Matrix3d M_theta; - M_theta << px, -qx, 0, - qx, px, 0, - 0, 0, 0; - // Jacobian by measurement - Eigen::Matrix J_meas = R * (M * v_meas + M_theta * v * theta_meas.transpose()); - - // Measurement cov - double L0_cov = vel_cov * dt * dt; - double L1_cov = vel_cov * dt * dt; - Eigen::Matrix2d meas_cov; - meas_cov << L0_cov, 0, - 0, L1_cov; - - // Update pose cov - rpose_cov = J_pose * rpose_cov * J_pose.transpose() + J_meas * meas_cov * J_meas.transpose(); - } - - /** - * @brief Update odometry (currently, only 2-wheels differential configuration implemented). - * Odometry is computed for robot's origin (IMU). - * @param distance distance traveled by each wheel since last odometry update - * @param dt time elapse since last odometry update (s) - */ - void update_odometry(std::vector distance, double dt) - { - // Currently, only 2-wheels configuration implemented - int nwheels = std::min(2, (int)distance.size()); - switch (nwheels) - { - // Differential drive robot. - case 2: - update_odometry_diffdrive(distance, dt); - break; - } - } - - /** - * @brief Process wheel measurement. - * @param measurement measurement - * @param rpm whether measurement contains RPM-s or cumulative wheel distances - * @param time measurement's internal time stamp (for accurate dt computations) - * @param time_pub measurement's time stamp for publish - */ - void process_measurement(std::vector measurement, bool rpm, ros::Time time, ros::Time time_pub) - { - // Initial measurement - if (time_prev == ros::Time(0)) { - count_meas = measurement.size(); - measurement_prev.resize(count_meas); - count = std::min(count, count_meas); // don't try to use more wheels than we have - } - // Same time stamp (messages are generated by FCU more often than the wheel state updated) - else if (time == time_prev) { - return; - } - // # of wheels differs from the initial value - else if (measurement.size() != count_meas) { - ROS_WARN_THROTTLE_NAMED(10, "wo", "WO: Number of wheels in measurement (%lu) differs from the initial value (%i).", measurement.size(), count_meas); - return; - } - // Compute odometry - else { - double dt = (time - time_prev).toSec(); // Time since previous measurement (s) - - // Distance traveled by each wheel since last measurement. - // Reserve for at least 2 wheels. - std::vector distance(std::max(2, count)); - // Compute using RPM-s - if (rpm) { - for (int i = 0; i < count; i++) { - double RPM_2_SPEED = wheel_radius[i] * 2.0 * M_PI / 60.0; // RPM -> speed (m/s) - double rpm = 0.5 * (measurement[i] + measurement_prev[i]); // Mean RPM during last dt seconds - distance[i] = rpm * RPM_2_SPEED * dt; - } - } - // Compute using cumulative distances - else { - for (int i = 0; i < count; i++) - distance[i] = measurement[i] - measurement_prev[i]; - } - - // Make distance of the 2nd wheel equal to that of the 1st one if requested or only one is available. - // This generalizes odometry computations for 1- and 2-wheels configurations. - if (count == 1) - distance[1] = distance[0]; - - // Update odometry - update_odometry(distance, dt); - - // Publish odometry - publish_odometry(time_pub); - } - - // Time step - time_prev = time; - std::copy_n(measurement.begin(), measurement.size(), measurement_prev.begin()); - } - - /* -*- message handlers -*- */ - - /** - * @brief Handle Ardupilot RPM MAVlink message. - * Message specification: http://mavlink.io/en/messages/ardupilotmega.html#RPM - * @param msg Received Mavlink msg - * @param rpm RPM msg - */ - void handle_rpm(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::RPM &rpm) - { - // Get ROS timestamp of the message - ros::Time timestamp = ros::Time::now(); - - // Publish RPM-s - if (raw_send) { - auto rpm_msg = boost::make_shared(); - - rpm_msg->header.stamp = timestamp; - rpm_msg->data.resize(2); - rpm_msg->data[0] = rpm.rpm1; - rpm_msg->data[1] = rpm.rpm2; - - rpm_pub.publish(rpm_msg); - } - - // Process measurement - if (odom_mode == OM::RPM) { - std::vector measurement{rpm.rpm1, rpm.rpm2}; - process_measurement(measurement, true, timestamp, timestamp); - } - } - - /** - * @brief Handle WHEEL_DISTANCE MAVlink message. - * Message specification: https://mavlink.io/en/messages/common.html#WHEEL_DISTANCE - * @param msg Received Mavlink msg - * @param dist WHEEL_DISTANCE msg - */ - void handle_wheel_distance(const mavlink::mavlink_message_t *msg, mavlink::common::msg::WHEEL_DISTANCE &wheel_dist) - { - // Check for bad wheels count - if (wheel_dist.count == 0) - return; - - // Get ROS timestamp of the message - ros::Time timestamp = m_uas->synchronise_stamp(wheel_dist.time_usec); - // Get internal timestamp of the message - ros::Time timestamp_int = ros::Time(wheel_dist.time_usec / 1000000UL, 1000UL * (wheel_dist.time_usec % 1000000UL)); - - // Publish distances - if (raw_send) { - auto wheel_dist_msg = boost::make_shared(); - - wheel_dist_msg->header.stamp = timestamp; - wheel_dist_msg->data.resize(wheel_dist.count); - std::copy_n(wheel_dist.distance.begin(), wheel_dist.count, wheel_dist_msg->data.begin()); - - dist_pub.publish(wheel_dist_msg); - } - - // Process measurement - if (odom_mode == OM::DIST) { - std::vector measurement(wheel_dist.count); - std::copy_n(wheel_dist.distance.begin(), wheel_dist.count, measurement.begin()); - process_measurement(measurement, false, timestamp_int, timestamp); - } - } + rclcpp::Publisher::SharedPtr rpm_pub; + rclcpp::Publisher::SharedPtr dist_pub; + rclcpp::Publisher::SharedPtr odom_pub; + rclcpp::Publisher::SharedPtr twist_pub; + + /// @brief Odometry computation modes + enum class OM + { + NONE, //!< no odometry computation + RPM, //!< use wheel's RPM + DIST //!< use wheel's cumulative distance + }; + OM odom_mode; //!< odometry computation mode + + int count; //!< requested number of wheels to compute odometry + bool raw_send; //!< send wheel's RPM and cumulative distance + std::vector wheel_offset; //!< wheel x,y offsets (m,NED) + std::vector wheel_radius; //!< wheel radiuses (m) + + bool twist_send; //!< send TwistWithCovarianceStamped instead of Odometry + bool tf_send; //!< send TF + std::string frame_id; //!< origin frame for topic headers + std::string child_frame_id; //!< body-fixed frame for topic headers + std::string tf_frame_id; //!< origin for TF + std::string tf_child_frame_id; //!< frame for TF and Pose + double vel_cov; //!< wheel velocity measurement error 1-var (m/s) + + int count_meas; //!< number of wheels in measurements + rclcpp::Time time_prev; //!< timestamp of previous measurement + std::vector measurement_prev; //!< previous measurement + + bool yaw_initialized; //!< initial yaw initialized (from IMU) + + /// @brief Robot origin 2D-state (SI units) + Eigen::Vector3d rpose; //!< pose (x, y, yaw) + Eigen::Vector3d rtwist; //!< twist (vx, vy, vyaw) + Eigen::Matrix3d rpose_cov; //!< pose error 1-var + Eigen::Vector3d rtwist_cov; //!< twist error 1-var (vx_cov, vy_cov, vyaw_cov) + + // XXX(vooon): attq != Eigen::Quaterniond::Identity(): + // error: no match for ‘operator!=’ (operand types are ‘Eigen::Quaternion’ and + // ‘Eigen::Quaternion’) + inline bool quaterniond_eq(Eigen::Quaterniond a, Eigen::Quaterniond b) + { + // [[[cog: + // parts = [f"a.{f}() == b.{f}()" for f in "wxyz"] + // cog.outl(f"return {' && '.join(parts)};"); + // ]]] + return a.w() == b.w() && a.x() == b.x() && a.y() == b.y() && a.z() == b.z(); + // [[[end]]] (checksum: af3c54c9f2c525c7a0c27a3151d69074) + } + + /** + * @brief Publish odometry. + * Odometry is computed from the very start but no pose info is published until we have initial orientation (yaw). + * Once we get it, the robot's current pose is updated with it and starts to be published. + * Twist info doesn't depend on initial orientation so is published from the very start. + * @param time measurement's ROS time stamp + */ + void publish_odometry(rclcpp::Time time) + { + // Get initial yaw (from IMU) + // Check that IMU was already initialized + auto attq = ftf::to_eigen(uas->data.get_attitude_orientation_enu()); + if (!yaw_initialized && !quaterniond_eq(attq, Eigen::Quaterniond::Identity())) { + double yaw = ftf::quaternion_get_yaw(attq); + + // Rotate current pose by initial yaw + Eigen::Rotation2Dd rot(yaw); + rpose.head(2) = rot * rpose.head(2); // x,y + rpose(2) += yaw; // yaw + + RCLCPP_INFO(get_logger(), "WO: Initial yaw (deg): %f", yaw / M_PI * 180.0); + yaw_initialized = true; + } + + // Orientation (only if we have initial yaw) + geometry_msgs::msg::Quaternion quat; + if (yaw_initialized) { + quat = tf2::toMsg(ftf::quaternion_from_rpy(0.0, 0.0, rpose(2))); + } + + // Twist + geometry_msgs::msg::TwistWithCovariance twist_cov; + // linear + twist_cov.twist.linear.x = rtwist(0); + twist_cov.twist.linear.y = rtwist(1); + twist_cov.twist.linear.z = 0.0; + // angular + twist_cov.twist.angular.x = 0.0; + twist_cov.twist.angular.y = 0.0; + twist_cov.twist.angular.z = rtwist(2); + // covariance + ftf::EigenMapCovariance6d twist_cov_map(twist_cov.covariance.data()); + twist_cov_map.setZero(); + twist_cov_map.block<2, 2>(0, 0).diagonal() << rtwist_cov(0), rtwist_cov(1); + twist_cov_map.block<1, 1>(5, 5).diagonal() << rtwist_cov(2); + + // Publish twist + if (twist_send && twist_pub) { + auto twist_cov_t = geometry_msgs::msg::TwistWithCovarianceStamped(); + // header + twist_cov_t.header.stamp = time; + twist_cov_t.header.frame_id = frame_id; + // twist + twist_cov_t.twist = twist_cov; + // publish + twist_pub->publish(twist_cov_t); + } else if (yaw_initialized) { + // Publish odometry (only if we have initial yaw) + auto odom = nav_msgs::msg::Odometry(); + // header + odom.header.stamp = time; + odom.header.frame_id = frame_id; + odom.child_frame_id = child_frame_id; + // pose + odom.pose.pose.position.x = rpose(0); + odom.pose.pose.position.y = rpose(1); + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = quat; + ftf::EigenMapCovariance6d pose_cov_map(odom.pose.covariance.data()); + pose_cov_map.block<2, 2>(0, 0) << rpose_cov.block<2, 2>(0, 0); + pose_cov_map.block<2, 1>(0, 5) << rpose_cov.block<2, 1>(0, 2); + pose_cov_map.block<1, 2>(5, 0) << rpose_cov.block<1, 2>(2, 0); + pose_cov_map.block<1, 1>(5, 5) << rpose_cov.block<1, 1>(2, 2); + // twist + odom.twist = twist_cov; + // publish + odom_pub->publish(odom); + } + + // Publish TF (only if we have initial yaw) + if (tf_send && yaw_initialized) { + geometry_msgs::msg::TransformStamped transform; + // header + transform.header.stamp = time; + transform.header.frame_id = tf_frame_id; + transform.child_frame_id = tf_child_frame_id; + // translation + transform.transform.translation.x = rpose(0); + transform.transform.translation.y = rpose(1); + transform.transform.translation.z = 0.0; + // rotation + transform.transform.rotation = quat; + // publish + uas->tf2_broadcaster.sendTransform(transform); + } + } + + /** + * @brief Update odometry for differential drive robot. + * Odometry is computed for robot's origin (IMU). + * The wheels are assumed to be parallel to the robot's x-direction (forward) and with the same x-offset. + * No slip is assumed (Instantaneous Center of Curvature (ICC) along the axis connecting the wheels). + * All computations are performed for ROS frame conventions. + * The approach is the extended and more accurate version of standard one described in the book + * https://github.com/correll/Introduction-to-Autonomous-Robots + * and at the page (with some typos fixed) + * http://correll.cs.colorado.edu/?p=1307 + * The extension is that exact pose update is used instead of approximate, + * and that the robot's origin can be specified anywhere instead of the middle-point between the wheels. + * @param distance distance traveled by each wheel since last odometry update + * @param dt time elapse since last odometry update (s) + */ + void update_odometry_diffdrive(std::vector distance, double dt) + { + double y0 = wheel_offset[0](1); + double y1 = wheel_offset[1](1); + double a = -wheel_offset[0](0); + double dy_inv = 1.0 / (y1 - y0); + double dt_inv = 1.0 / dt; + + // Rotation angle + double theta = (distance[1] - distance[0]) * dy_inv; + // Distance traveled by the projection of origin onto the axis connecting the wheels (Op) + double L = (y1 * distance[0] - y0 * distance[1]) * dy_inv; + + // Instantenous pose update in local (robot) coordinate system (vel*dt) + Eigen::Vector3d v(L, a * theta, theta); + // Instantenous local twist + rtwist = v * dt_inv; + + // Compute local pose update (approximate). + // In the book a=0 and |y0|=|y1|, additionally. + // dx = L*cos(theta/2) - a*theta*sin(theta/2) + // dy = L*sin(theta/2) + a*theta*cos(theta/2) + // Compute local pose update (exact) + // dx = a*(cos(theta)-1) + R*sin(theta) + // dy = a*sin(theta) - R*(cos(theta)-1) + // where R - rotation radius of Op around ICC (R = L/theta). + double cos_theta = std::cos(theta); + double sin_theta = std::sin(theta); + double p; // sin(theta)/theta + double q; // (1-cos(theta))/theta + if (std::abs(theta) > 1.e-5) { + p = sin_theta / theta; + q = (1.0 - cos_theta) / theta; + } else { + // Limits for theta -> 0 + p = 1.0; + q = 0.0; + } + + // Local pose update matrix + Eigen::Matrix3d M; + M << p, -q, 0, + q, p, 0, + 0, 0, 1; + + // Local pose update + Eigen::Vector3d dpose = M * v; + + // Rotation by yaw + double cy = std::cos(rpose(2)); + double sy = std::sin(rpose(2)); + Eigen::Matrix3d R; + R << cy, -sy, 0, + sy, cy, 0, + 0, 0, 1; + + // World pose + rpose += R * dpose; + rpose(2) = fmod(rpose(2), 2.0 * M_PI); // Clamp to (-2*PI, 2*PI) + + // Twist errors (constant in time) + if (rtwist_cov(0) == 0.0) { + // vx_cov + rtwist_cov(0) = vel_cov * (y0 * y0 + y1 * y1) * dy_inv * dy_inv; + // vy_cov (add extra error, otherwise vy_cov=0 if a=0) + rtwist_cov(1) = vel_cov * a * a * 2.0 * dy_inv * dy_inv + 0.001; + // vyaw_cov + rtwist_cov(2) = vel_cov * 2.0 * dy_inv * dy_inv; + } + + // Pose errors (accumulated in time). + // Exact formulations respecting kinematic equations. + // dR/dYaw + Eigen::Matrix3d R_yaw; + R_yaw << -sy, -cy, 0, + cy, -sy, 0, + 0, 0, 0; + // dYaw/dPose + Eigen::Vector3d yaw_pose(0, 0, 1); + // Jacobian by previous pose + Eigen::Matrix3d J_pose = Eigen::Matrix3d::Identity() + R_yaw * dpose * yaw_pose.transpose(); + + // dL,dTheta / dL0,dL1 + double L_L0 = y1 * dy_inv; + double L_L1 = -y0 * dy_inv; + double theta_L0 = -dy_inv; + double theta_L1 = dy_inv; + // dv/dMeasurement + Eigen::Matrix v_meas; + v_meas << L_L0, L_L1, + a * theta_L0, a * theta_L1, + theta_L0, theta_L1; + // dTheta/dMeasurement + Eigen::Vector2d theta_meas(theta_L0, theta_L1); + // dM/dTheta + double px; // dP/dTheta + double qx; // dQ/dTheta + if (std::abs(theta) > 1.e-5) { + px = (theta * cos_theta - sin_theta) / (theta * theta); + qx = (theta * sin_theta - (1 - cos_theta)) / (theta * theta); + } else { + // Limits for theta -> 0 + px = 0; + qx = 0.5; + } + // dM/dTheta + Eigen::Matrix3d M_theta; + M_theta << px, -qx, 0, + qx, px, 0, + 0, 0, 0; + // Jacobian by measurement + Eigen::Matrix J_meas = R * (M * v_meas + M_theta * v * theta_meas.transpose()); + + // Measurement cov + double L0_cov = vel_cov * dt * dt; + double L1_cov = vel_cov * dt * dt; + Eigen::Matrix2d meas_cov; + meas_cov << L0_cov, 0, + 0, L1_cov; + + // Update pose cov + rpose_cov = J_pose * rpose_cov * J_pose.transpose() + J_meas * meas_cov * J_meas.transpose(); + } + + /** + * @brief Update odometry (currently, only 2-wheels differential configuration implemented). + * Odometry is computed for robot's origin (IMU). + * @param distance distance traveled by each wheel since last odometry update + * @param dt time elapse since last odometry update (s) + */ + void update_odometry(std::vector distance, double dt) + { + // Currently, only 2-wheels configuration implemented + int nwheels = std::min(2, static_cast(distance.size())); + switch (nwheels) { + // Differential drive robot. + case 2: + update_odometry_diffdrive(distance, dt); + break; + } + } + + /** + * @brief Process wheel measurement. + * @param measurement measurement + * @param rpm whether measurement contains RPM-s or cumulative wheel distances + * @param time measurement's internal time stamp (for accurate dt computations) + * @param time_pub measurement's time stamp for publish + */ + void process_measurement( + std::vector measurement, bool rpm, rclcpp::Time time, + rclcpp::Time time_pub) + { + // Initial measurement + if (time_prev == rclcpp::Time(0)) { + count_meas = measurement.size(); + measurement_prev.resize(count_meas); + count = std::min(count, count_meas); // don't try to use more wheels than we have + } else if (time == time_prev) { + // Same time stamp (messages are generated by FCU more often than the wheel state updated) + return; + } else if (measurement.size() != static_cast(count_meas)) { + // # of wheels differs from the initial value + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 10, + "WO: Number of wheels in measurement (%lu) differs from the initial value (%i).", + measurement.size(), count_meas); + return; + } else { + // Compute odometry + double dt = (time - time_prev).seconds(); // Time since previous measurement (s) + + // Distance traveled by each wheel since last measurement. + // Reserve for at least 2 wheels. + std::vector distance(std::max(2, count)); + // Compute using RPM-s + if (rpm) { + for (int i = 0; i < count; i++) { + // RPM -> speed (m/s) + double RPM_2_SPEED = wheel_radius[i] * 2.0 * M_PI / 60.0; + // Mean RPM during last dt seconds + double rpm = 0.5 * (measurement[i] + measurement_prev[i]); + distance[i] = rpm * RPM_2_SPEED * dt; + } + } else { + // Compute using cumulative distances + for (int i = 0; i < count; i++) { + distance[i] = measurement[i] - measurement_prev[i]; + } + } + + // Make distance of the 2nd wheel equal to that of the 1st one + // if requested or only one is available. + // This generalizes odometry computations for 1- and 2-wheels configurations. + if (count == 1) { + distance[1] = distance[0]; + } + + // Update odometry + update_odometry(distance, dt); + + // Publish odometry + publish_odometry(time_pub); + } + + // Time step + time_prev = time; + std::copy_n(measurement.begin(), measurement.size(), measurement_prev.begin()); + } + + /* -*- message handlers -*- */ + + /** + * @brief Handle Ardupilot RPM MAVlink message. + * Message specification: http://mavlink.io/en/messages/ardupilotmega.html#RPM + * @param msg Received Mavlink msg + * @param rpm RPM msg + */ + void handle_rpm( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::ardupilotmega::msg::RPM & rpm, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // Get ROS timestamp of the message + auto timestamp = node->now(); + + // Publish RPM-s + if (raw_send) { + auto rpm_msg = mavros_msgs::msg::WheelOdomStamped(); + + rpm_msg.header.stamp = timestamp; + rpm_msg.data.resize(2); + rpm_msg.data[0] = rpm.rpm1; + rpm_msg.data[1] = rpm.rpm2; + + rpm_pub->publish(rpm_msg); + } + + // Process measurement + if (odom_mode == OM::RPM) { + std::vector measurement{rpm.rpm1, rpm.rpm2}; + process_measurement(measurement, true, timestamp, timestamp); + } + } + + /** + * @brief Handle WHEEL_DISTANCE MAVlink message. + * Message specification: https://mavlink.io/en/messages/common.html#WHEEL_DISTANCE + * @param msg Received Mavlink msg + * @param dist WHEEL_DISTANCE msg + */ + void handle_wheel_distance( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::WHEEL_DISTANCE & wheel_dist, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + // Check for bad wheels count + if (wheel_dist.count == 0) { + return; + } + + // Get ROS timestamp of the message + auto timestamp = uas->synchronise_stamp(wheel_dist.time_usec); + // Get internal timestamp of the message + rclcpp::Time timestamp_int(wheel_dist.time_usec / 1000000UL, + 1000UL * (wheel_dist.time_usec % 1000000UL)); + + // Publish distances + if (raw_send) { + auto wheel_dist_msg = mavros_msgs::msg::WheelOdomStamped(); + + wheel_dist_msg.header.stamp = timestamp; + wheel_dist_msg.data.resize(wheel_dist.count); + std::copy_n(wheel_dist.distance.begin(), wheel_dist.count, wheel_dist_msg.data.begin()); + + dist_pub->publish(wheel_dist_msg); + } + + // Process measurement + if (odom_mode == OM::DIST) { + std::vector measurement(wheel_dist.count); + std::copy_n(wheel_dist.distance.begin(), wheel_dist.count, measurement.begin()); + process_measurement(measurement, false, timestamp_int, timestamp); + } + } }; -} // namespace extra_plugins -} // namespace mavros +} // namespace extra_plugins +} // namespace mavros -#include -PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::WheelOdometryPlugin, mavros::plugin::PluginBase) +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::WheelOdometryPlugin) diff --git a/mavros_extras/src/servo_state_publisher.cpp b/mavros_extras/src/servo_state_publisher.cpp deleted file mode 100644 index 2b33b9fbe..000000000 --- a/mavros_extras/src/servo_state_publisher.cpp +++ /dev/null @@ -1,213 +0,0 @@ -/** - * @brief Publish servo states as JointState message - * @file - * @author Vladimir Ermakov - */ -/* - * Copyright 2015 Vladimir Ermakov - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include -#include - -#include -#include -#include - -class ServoDescription { -public: - std::string joint_name; - float joint_lower; - float joint_upper; - - size_t rc_channel; - - uint16_t rc_min; - uint16_t rc_max; - uint16_t rc_trim; - uint16_t rc_dz; - bool rc_rev; - - ServoDescription() : - joint_name{}, - joint_lower(-M_PI/4), - joint_upper(M_PI/4), - rc_channel(0), - rc_min(1000), - rc_max(2000), - rc_trim(1500), - rc_dz(0), - rc_rev(false) - { }; - - ServoDescription(std::string joint_name_, double lower_, double upper_, - int channel_, - int min_, int max_, int trim_, int dz_, - bool rev_) : - joint_name(joint_name_), - joint_lower(lower_), - joint_upper(upper_), - rc_channel(channel_), - rc_min(min_), - rc_max(max_), - rc_trim(trim_), - rc_dz(dz_), - rc_rev(rev_) - { }; - - /** - * Normalization code taken from PX4 Firmware - * src/modules/sensors/sensors.cpp Sensors::rc_poll() line 1966 - */ - inline float normalize(uint16_t pwm) { - // 1) fix bounds - pwm = std::max(pwm, rc_min); - pwm = std::min(pwm, rc_max); - - // 2) scale around mid point - float chan; - if (pwm > (rc_trim + rc_dz)) { - chan = (pwm - rc_trim - rc_dz) / (float)(rc_max - rc_trim - rc_dz); - } - else if (pwm < (rc_trim - rc_dz)) { - chan = (pwm - rc_trim + rc_dz) / (float)(rc_trim - rc_min - rc_dz); - } - else { - chan = 0.0; - } - - if (rc_rev) - chan *= -1; - - if (!std::isfinite(chan)) { - ROS_DEBUG("SSP: not finite result in RC%zu channel normalization!", rc_channel); - chan = 0.0; - } - - return chan; - } - - float calculate_position(uint16_t pwm) { - float channel = normalize(pwm); - - // not sure should i differently map -1..0 and 0..1 - // for now there arduino map() (explicit) - float position = (channel + 1.0) * (joint_upper - joint_lower) / (1.0 + 1.0) + joint_lower; - - return position; - } -}; - -class ServoStatePublisher { -public: - ServoStatePublisher() : - nh() - { - ros::NodeHandle priv_nh("~"); - - XmlRpc::XmlRpcValue param_dict; - priv_nh.getParam("", param_dict); - - ROS_ASSERT(param_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct); - - urdf::Model model; - model.initParam("robot_description"); - ROS_INFO("SSP: URDF robot: %s", model.getName().c_str()); - - for (auto &pair : param_dict) { - ROS_DEBUG("SSP: Loading joint: %s", pair.first.c_str()); - - // inefficient, but easier to program - ros::NodeHandle pnh(priv_nh, pair.first); - - bool rc_rev; - int rc_channel, rc_min, rc_max, rc_trim, rc_dz; - - if (!pnh.getParam("rc_channel", rc_channel)) { - ROS_ERROR("SSP: '%s' should provice rc_channel", pair.first.c_str()); - continue; - } - - pnh.param("rc_min", rc_min, 1000); - pnh.param("rc_max", rc_max, 2000); - if (!pnh.getParam("rc_trim", rc_trim)) { - rc_trim = rc_min + (rc_max - rc_min) / 2; - } - - pnh.param("rc_dz", rc_dz, 0); - pnh.param("rc_rev", rc_rev, false); - - auto joint = model.getJoint(pair.first); - if (!joint) { - ROS_ERROR("SSP: URDF: there no joint '%s'", pair.first.c_str()); - continue; - } - if (!joint->limits) { - ROS_ERROR("SSP: URDF: joint '%s' should provide ", pair.first.c_str()); - continue; - } - - double lower = joint->limits->lower; - double upper = joint->limits->upper; - - servos.emplace_back(pair.first, lower, upper, rc_channel, rc_min, rc_max, rc_trim, rc_dz, rc_rev); - ROS_INFO("SSP: joint '%s' (RC%d) loaded", pair.first.c_str(), rc_channel); - } - - rc_out_sub = nh.subscribe("rc_out", 10, &ServoStatePublisher::rc_out_cb, this); - joint_states_pub = nh.advertise("joint_states", 10); - } - - void spin() { - if (servos.empty()) { - ROS_WARN("SSP: there nothing to do, exiting"); - return; - } - - ROS_INFO("SSP: Initialization done. %zu joints served", servos.size()); - ros::spin(); - } - -private: - ros::NodeHandle nh; - ros::Subscriber rc_out_sub; - ros::Publisher joint_states_pub; - - std::list servos; - - void rc_out_cb(const mavros_msgs::RCOut::ConstPtr &msg) { - if (msg->channels.empty()) - return; // nothing to do - - auto states = boost::make_shared(); - states->header.stamp = msg->header.stamp; - - for (auto &desc : servos) { - if (!(desc.rc_channel != 0 && desc.rc_channel <= msg->channels.size())) - continue; // prevent crash on servos not in that message - - uint16_t pwm = msg->channels[desc.rc_channel - 1]; - if (pwm == 0 || pwm == UINT16_MAX) - continue; // exclude unset channels - - states->name.emplace_back(desc.joint_name); - states->position.emplace_back(desc.calculate_position(pwm)); - } - - joint_states_pub.publish(states); - } -}; - -int main(int argc, char *argv[]) -{ - ros::init(argc, argv, "servo_state_publisher"); - - ServoStatePublisher state_publisher; - state_publisher.spin(); - return 0; -} - diff --git a/mavros_extras/src/visualization.cpp b/mavros_extras/src/visualization.cpp deleted file mode 100644 index 0be665c3d..000000000 --- a/mavros_extras/src/visualization.cpp +++ /dev/null @@ -1,318 +0,0 @@ -/** - * @brief Visualization - * @file visualization.cpp - * @author M.H.Kabir - */ -/* - * Copyright 2014,2015 M.H.Kabir - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include - -#include - -#include -#include - -#include -#include -#include -#include - -// parameters -static std::string fixed_frame_id; -static std::string child_frame_id; -static double marker_scale; -static int max_track_size = 100; - -// source subscribers -ros::Subscriber local_position_sub; -ros::Subscriber landing_target_sub; -ros::Subscriber lt_marker_sub; - -// marker publishers -ros::Publisher track_marker_pub; -ros::Publisher vehicle_marker_pub; -ros::Publisher lt_marker_pub; -ros::Publisher wp_marker_pub; - -// landing target marker size -geometry_msgs::Vector3 lt_size; - -boost::shared_ptr vehicle_marker; - -/** - * @brief publish vehicle track - */ -static void publish_track_marker(const geometry_msgs::PoseStamped::ConstPtr &pose) -{ - static boost::shared_ptr track_marker; - - if ( !track_marker ) - { - track_marker = boost::make_shared(); - track_marker->type = visualization_msgs::Marker::CUBE_LIST; - track_marker->ns = "fcu"; - track_marker->action = visualization_msgs::Marker::ADD; - track_marker->scale.x = marker_scale * 0.015; - track_marker->scale.y = marker_scale * 0.015; - track_marker->scale.z = marker_scale * 0.015; - track_marker->color.a = 1.0; - track_marker->color.r = 0.0; - track_marker->color.g = 0.0; - track_marker->color.b = 0.5; - track_marker->points.reserve(max_track_size); - } - - static int marker_idx = 0; - - if ( track_marker->points.size() < max_track_size ) - track_marker->points.push_back(pose->pose.position); - else track_marker->points[marker_idx] = pose->pose.position; - - marker_idx = (marker_idx + 1) % max_track_size; - - track_marker->header = pose->header; - track_marker_pub.publish(track_marker); -} - -static void publish_wp_marker(const geometry_msgs::PoseStamped::ConstPtr &wp) -{ - static boost::shared_ptr marker; - - if ( !marker ) // only instantiate marker once - { - marker = boost::make_shared(); - - marker->header = wp->header; - marker->header.frame_id = fixed_frame_id; - marker->type = visualization_msgs::Marker::ARROW; - marker->ns = "wp"; - marker->action = visualization_msgs::Marker::ADD; - marker->scale.x = marker_scale * 1.0; - marker->scale.y = marker_scale * 0.1; - marker->scale.z = marker_scale * 0.1; - - marker->color.a = 1.0; - marker->color.r = 0.0; - marker->color.g = 1.0; - marker->color.b = 0.0; - } - - marker->pose = wp->pose; - wp_marker_pub.publish(marker); -} - - -/** - * @brief publish landing target marker - */ -static void publish_lt_marker(const geometry_msgs::PoseStamped::ConstPtr &target) -{ - static int marker_id = 2; // TODO: generate new marker for each target - - auto marker = boost::make_shared(); - - marker->header = target->header; - marker->ns = "landing_target"; - marker->id = marker_id; - marker->type = visualization_msgs::Marker::CUBE; - marker->action = visualization_msgs::Marker::ADD; - - marker->color.a = 1.0; - marker->color.r = 1.0; - marker->color.g = 0.0; - marker->color.b = 0.0; - - marker->scale.x = 1.0; - marker->scale.y = 1.0; - marker->scale.z = 1.0; - - // origin - marker->pose = target->pose; - lt_marker_pub.publish(marker); - - // cross arms - marker->pose.position.x = lt_size.x; - marker->pose.position.y = lt_size.y; - marker->pose.position.z = lt_size.z; - - marker->id = ++marker_id; - marker->pose.orientation.w = 0; - marker->pose.orientation.x = 0; - marker->pose.orientation.y = 0; - marker->pose.orientation.w = 0; - lt_marker_pub.publish(marker); - - marker->id = ++marker_id; - // 90 degrees rotation - marker->pose.orientation.w = 0.70711; - marker->pose.orientation.x = 0; - marker->pose.orientation.y = 0; - marker->pose.orientation.w = 0.70711; - lt_marker_pub.publish(marker); -} - -/** - * @brief publish vehicle - */ -static void create_vehicle_markers( int num_rotors, float arm_len, float body_width, float body_height, int prop_direction) -{ - if ( num_rotors <= 0 ) num_rotors = 2; - - /** Create markers only once for efficiency - * TODO use visualization_msgs::MarkerArray? - */ - - if ( vehicle_marker ) - return; - - vehicle_marker = boost::make_shared(); - vehicle_marker->markers.reserve( 2 * num_rotors + 1 ); - - /** Hexacopter marker code adapted from libsfly_viz - * thanks to Markus Achtelik. - */ - - // rotor marker template - visualization_msgs::Marker rotor; - rotor.header.stamp = ros::Time(); - rotor.header.frame_id = child_frame_id; - rotor.ns = "vehicle_rotor"; - rotor.action = visualization_msgs::Marker::ADD; - rotor.type = visualization_msgs::Marker::CYLINDER; - rotor.scale.x = 0.2 * marker_scale; - rotor.scale.y = 0.2 * marker_scale; - rotor.scale.z = 0.01 * marker_scale; - rotor.pose.position.z = 0; - - // arm marker template - visualization_msgs::Marker arm; - arm.header.stamp = ros::Time(); - arm.header.frame_id = child_frame_id; - arm.ns = "vehicle_arm"; - arm.action = visualization_msgs::Marker::ADD; - arm.type = visualization_msgs::Marker::CUBE; - arm.scale.x = arm_len * marker_scale; - arm.scale.y = 0.02 * marker_scale; - arm.scale.z = 0.01 * marker_scale; - arm.color.r = 0.0; - arm.color.g = 0.0; - arm.color.b = 1.0; - arm.color.a = 1.0; - arm.pose.position.z = -0.015 * marker_scale; - - float angle_increment = 2 * M_PI / num_rotors; - - for ( float angle = angle_increment / 2; angle <= (2 * M_PI); angle += angle_increment ) - { - if ( !prop_direction ) { - rotor.color.r = 0.4; - rotor.color.g = 0.4; - rotor.color.b = 0.4; - rotor.color.a = 0.8; - } else { - if ( angle <= (M_PI / 2) - 0.0175 || angle >= (M_PI * 3 / 2) + 0.0175 ) { - rotor.color.r = 0.8; - rotor.color.g = 0.8; - rotor.color.b = 0.8; - rotor.color.a = 0.8; - } else { - rotor.color.r = 1.0; - rotor.color.g = 0; - rotor.color.b = 0; - rotor.color.a = 0.8; - } - } - - - rotor.pose.position.x = arm_len * cos(angle) * marker_scale; - rotor.pose.position.y = arm_len * sin(angle) * marker_scale; - rotor.id++; - - arm.pose.position.x = rotor.pose.position.x / 2; - arm.pose.position.y = rotor.pose.position.y / 2; - arm.pose.orientation = tf::createQuaternionMsgFromYaw(angle); - arm.id++; - - vehicle_marker->markers.push_back(rotor); - vehicle_marker->markers.push_back(arm); - } - - // body marker template - visualization_msgs::Marker body; - body.header.stamp = ros::Time(); - body.header.frame_id = child_frame_id; - body.ns = "vehicle_body"; - body.action = visualization_msgs::Marker::ADD; - body.type = visualization_msgs::Marker::CUBE; - body.scale.x = body_width * marker_scale; - body.scale.y = body_width * marker_scale; - body.scale.z = body_height * marker_scale; - body.color.r = 0.0; - body.color.g = 1.0; - body.color.b = 0.0; - body.color.a = 0.8; - - vehicle_marker->markers.push_back(body); -} - -static void local_position_sub_cb(const geometry_msgs::PoseStamped::ConstPtr &pose) -{ - publish_track_marker(pose); - if (vehicle_marker) vehicle_marker_pub.publish(vehicle_marker); -} - -void setpoint_local_pos_sub_cb(const geometry_msgs::PoseStamped::ConstPtr &wp) -{ - publish_wp_marker(wp); -} - -static void landing_target_sub_cb(const geometry_msgs::PoseStamped::ConstPtr &target) -{ - publish_lt_marker(target); -} - -static void lt_marker_sub_cb(const geometry_msgs::Vector3Stamped::ConstPtr <_marker) -{ - lt_size = lt_marker->vector; -} - -int main(int argc, char *argv[]) -{ - ros::init(argc, argv, "copter_visualization"); - ros::NodeHandle nh; - ros::NodeHandle priv_nh("~"); - - int num_rotors, prop_direction; - double arm_len, body_width, body_height; - - priv_nh.param("fixed_frame_id", fixed_frame_id, "map"); - priv_nh.param("child_frame_id", child_frame_id, "base_link"); - - priv_nh.param("marker_scale", marker_scale, 1.0); - priv_nh.param("num_rotors", num_rotors, 6); - priv_nh.param("arm_len", arm_len, 0.22 ); - priv_nh.param("body_width", body_width, 0.15 ); - priv_nh.param("body_height", body_height, 0.10 ); - priv_nh.param("max_track_size", max_track_size, 1000 ); - priv_nh.param("prop_direction", prop_direction, 0); - - create_vehicle_markers( num_rotors, arm_len, body_width, body_height, prop_direction ); - - track_marker_pub = nh.advertise("track_markers", 10); - vehicle_marker_pub = nh.advertise("vehicle_marker", 10); - wp_marker_pub = nh.advertise("wp_markers", 10); - lt_marker_pub = nh.advertise("landing_target", 10); - - auto pos_sub = nh.subscribe("local_position", 10, local_position_sub_cb); - auto wp_sub = nh.subscribe("local_setpoint", 10, setpoint_local_pos_sub_cb); - lt_marker_sub = nh.subscribe("lt_marker", 10, lt_marker_sub_cb); - - ros::spin(); - return 0; -} diff --git a/mavros_msgs/CHANGELOG.rst b/mavros_msgs/CHANGELOG.rst index da128e071..d7f8798aa 100644 --- a/mavros_msgs/CHANGELOG.rst +++ b/mavros_msgs/CHANGELOG.rst @@ -2,8 +2,332 @@ Changelog for package mavros_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.16.0 (2023-05-05) -------------------- +2.5.0 (2023-05-05) +------------------ + +2.4.0 (2022-12-30) +------------------ +* msgs: re-generate +* Merge branch 'master' into ros2 + * master: + 1.15.0 + update changelog + ci: update actions + Implement debug float array handler + mavros_extras: Fix a sequence point warning + mavros_extras: Fix a comparison that shouldn't be bitwise + mavros: Fix some warnings + mavros_extras: Fix buggy check for lat/lon ignored + libmavconn: fix MAVLink v1.0 output selection +* 1.15.0 +* update changelog +* Merge pull request `#1811 `_ from scoutdi/debug-float-array + Implement debug float array handler +* Implement debug float array handler + Co-authored-by: Morten Fyhn Amundsen +* Contributors: Sverre Velten Rothmund, Vladimir Ermakov + +2.3.0 (2022-09-24) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.14.0 + update changelog + scripts: waypoint and param files are text, not binary + libmavconn: fix MAVLink v1.0 output selection + plugins: add guided_target to accept offboard position targets + add cmake module path for geographiclib on debian based systems + use already installed FindGeographicLib.cmake +* 1.14.0 +* update changelog +* Contributors: Vladimir Ermakov + +2.2.0 (2022-06-27) +------------------ +* Merge branch 'master' into ros2 + * master: + mount_control.cpp: detect MOUNT_ORIENTATION stale messages + ESCTelemetryItem.msg: correct RPM units + apm_config.yaml: add mount configuration + sys_status.cpp fix free memory for values > 64KiB + uncrustify cellular_status.cpp + Add CellularStatus plugin and message + *_config.yaml: document usage of multiple batteries diagnostics + sys_status.cpp: fix compilation + sys_status.cpp: support diagnostics on up-to 10 batteries + sys_status.cpp: do not use harcoded constants + sys_status.cpp: Timeout on MEMINFO and HWSTATUS mavlink messages and publish on the diagnostics + sys_status.cpp: fix enabling of mem_diag and hwst_diag + sys_status.cpp: Do not use battery1 voltage as voltage for all other batteries (bugfix). + sys_status.cpp: ignore sys_status mavlink messages from gimbals + mount_control.cpp: use mount_nh for params to keep similarities with other plugins set diag settings before add() + sys_status.cpp: remove deprecated BATTERY2 mavlink message support + Mount control plugin: add configurable diagnostics + Bugfix: increment_f had no value asigned when input LaserScan was bigger than obstacle.distances.size() + Bugfix: wrong interpolation when the reduction ratio (scale_factor) is not integer. + Disable startup_px4_usb_quirk in px4_config.yaml +* msgs: support humble + +2.1.1 (2022-03-02) +------------------ + +2.1.0 (2022-02-02) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.13.0 + update changelog + py-lib: fix compatibility with py3 for Noetic + re-generate all coglets + test: add checks for ROTATION_CUSTOM + lib: Fix rotation search for CUSTOM + Removed CamelCase for class members. Publish to "report" + More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages + Fixed callback name to match `handle\_{MESSAGE_NAME.lower()}` convention + Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html + Fixed topic names to match more closely what other plugins use. Fixed a typo. + Add plugin for reporting terrain height estimate from FCU + 1.12.2 + update changelog + Set time/publish_sim_time to false by default + plugin: setpoint_raw: move getParam to initializer + extras: trajectory: backport `#1667 `_ +* 1.13.0 +* update changelog +* Merge pull request `#1690 `_ from mavlink/fix-enum_sensor_orientation + Fix enum sensor_orientation +* re-generate all coglets +* Merge pull request `#1680 `_ from AndersonRayner/new_mav_frames + Add extra MAV_FRAMES to waypoint message +* Merge pull request `#1677 `_ from AndersonRayner/add_terrain + Add plugin for reporting terrain height estimate from the FCU +* More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages +* Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html +* Add plugin for reporting terrain height estimate from FCU +* 1.12.2 +* update changelog +* Merge branch 'master' into ros2 + * master: + 1.12.1 + update changelog + mavconn: fix connection issue introduced by `#1658 `_ + mavros_extras: Fix some warnings + mavros: Fix some warnings +* 1.12.1 +* update changelog +* Contributors: Vladimir Ermakov, matt + +2.0.5 (2021-11-28) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.12.0 + update changelog + Fix multiple bugs + lib: fix mission frame debug print + extras: distance_sensor: revert back to zero quaternion +* 1.12.0 +* update changelog +* extras: fix some more lint warns +* msgs: update conversion header +* Merge branch 'master' into ros2 + * master: + 1.11.1 + update changelog + lib: fix build +* 1.11.1 +* update changelog +* Merge branch 'master' into ros2 + * master: + 1.11.0 + update changelog + lib: fix ftf warnings + msgs: use pragmas to ignore unaligned pointer warnings + extras: landing_target: fix misprint + msgs: fix convert const + plugin: setpoint_raw: fix misprint + msgs: try to hide 'unaligned pointer' warning + plugin: sys: fix compillation error + plugin: initialize quaternions with identity + plugin: sys: Use wall timers for connection management + Use meters for relative altitude + distance_sensor: Initialize sensor orientation quaternion to zero + Address review comments + Add camera plugin for interfacing with mavlink camera protocol +* 1.11.0 +* update changelog +* msgs: use pragmas to ignore unaligned pointer warnings +* msgs: fix convert const +* msgs: try to hide 'unaligned pointer' warning +* Merge pull request `#1651 `_ from Jaeyoung-Lim/pr-image-capture-plugin + Add camera plugin for interfacing with mavlink camera protocol +* Address review comments +* Add camera plugin for interfacing with mavlink camera protocol + Add camera image captured message for handling camera trigger information +* Merge branch 'master' into ros2 + * master: + msgs: add yaw field to GPS_INPUT +* msgs: add yaw field to GPS_INPUT +* Contributors: Jaeyoung-Lim, Vladimir Ermakov + +2.0.4 (2021-11-04) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.10.0 + prepare release +* 1.10.0 +* prepare release +* Merge branch 'master' into ros2 + * master: + msgs: update gpsraw to have yaw field +* msgs: update gpsraw to have yaw field +* Merge branch 'master' into ros2 + * master: (25 commits) + Remove reference + Catch std::length_error in send_message + Show ENOTCONN error instead of crash + Tunnel: Check for invalid payload length + Tunnel.msg: Generate enum with cog + mavros_extras: Create tunnel plugin + mavros_msgs: Add Tunnel message + MountControl.msg: fix copy-paste + sys_time.cpp: typo + sys_time: publish /clock for simulation times + 1.9.0 + update changelog + Spelling corrections + Changed OverrideRCIn to 18 channels + This adds functionality to erase all logs on the SD card via mavlink + publish BATTERY2 message as /mavros/battery2 topic + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 + Added NAV_CONTROLLER_OUTPUT Plugin + Added GPS_INPUT plugin + Update esc_status plugin with datatype change on MAVLink. + ... +* Merge pull request `#1625 `_ from scoutdi/tunnel-plugin + Plugin for TUNNEL messages +* Tunnel.msg: Generate enum with cog +* mavros_msgs: Add Tunnel message +* Merge pull request `#1623 `_ from amilcarlucas/pr/more-typo-fixes + More typo fixes +* MountControl.msg: fix copy-paste +* 1.9.0 +* update changelog +* Merge pull request `#1616 `_ from amilcarlucas/pr/RC_CHANNELS-mavlink2-extensions + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels.… +* Changed OverrideRCIn to 18 channels +* Merge pull request `#1617 `_ from amilcarlucas/pr/NAV_CONTROLLER_OUTPUT-plugin + Added NAV_CONTROLLER_OUTPUT Plugin +* Merge pull request `#1618 `_ from amilcarlucas/pr/GPS_INPUT-plugin + Added GPS_INPUT plugin +* Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 +* Added NAV_CONTROLLER_OUTPUT Plugin +* Added GPS_INPUT plugin +* Merge branch 'master' into master +* Update esc_status plugin with datatype change on MAVLink. + ESC_INFO MAVLink message was updated to have negative temperates and also at a different resolution. This commit updates those changes on this side. +* Remove Mount_Status plugin. Add Status data to Mount_Control plugin. Remove Mount_Status message. +* msgs: re-generate file lists +* Merge branch 'master' into ros2 + * master: + extras: esc_telemetry: fix build + extras: fix esc_telemetry centi-volt/amp conversion + extras: uncrustify all plugins + plugins: reformat xml + extras: reformat plugins xml + extras: fix apm esc_telemetry + msgs: fix types for apm's esc telemetry + actually allocate memory for the telemetry information + fixed some compile errors + added esc_telemetry plugin + Reset calibration flag when re-calibrating. Prevent wrong data output. + Exclude changes to launch files. + Delete debug files. + Apply uncrustify changes. + Set progress array to global to prevent erasing data. + Move Compass calibration report to extras. Rewrite code based on instructions. + Remove extra message from CMakeLists. + Add message and service definition. + Add compass calibration feedback status. Add service to call the 'Next' button in calibrations. +* msgs: fix types for apm's esc telemetry +* actually allocate memory for the telemetry information +* added esc_telemetry plugin +* Add Mount angles message for communications with ardupilotmega. +* Remove extra message from CMakeLists. +* Add message and service definition. +* Contributors: Abhijith Thottumadayil Jagadeesh, André Filipe, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Morten Fyhn Amundsen, Ricardo Marques, Russell, Vladimir Ermakov + +2.0.3 (2021-06-20) +------------------ + +2.0.2 (2021-06-20) +------------------ + +2.0.1 (2021-06-06) +------------------ +* Add rcl_interfaces dependency +* Merge branch 'master' into ros2 + * master: + readme: update + 1.8.0 + update changelog + Create semgrep-analysis.yml + Create codeql-analysis.yml +* 1.8.0 +* update changelog +* Contributors: Rob Clarke, Vladimir Ermakov + +2.0.0 (2021-05-28) +------------------ +* msgs: update command codes +* msgs: update param services +* plugins: setpoint_velocity: port to ros2 +* Merge branch 'master' into ros2 + * master: + 1.7.1 + update changelog + re-generate all pymavlink enums + 1.7.0 + update changelog +* mavros: generate plugin list +* Merge branch 'master' into ros2 + * master: + msgs: re-generate the code + lib: re-generate the code + plugins: mission: re-generate the code + MissionBase: correction to file information + MissionBase: add copyright from origional waypoint.cpp + uncrustify + whitespace + add rallypoint and geofence plugins to mavros plugins xml + add rallypoint and geofence plugins to CMakeList + Geofence: add geofence plugin + Rallypoint: add rallypoint plugin + Waypoint: inherit MissionBase class for mission protocol + MissionBase: breakout mission protocol from waypoint.cpp + README: Update PX4 Autopilot references + Fix https://github.com/mavlink/mavros/issues/849 +* router: catch DeviceError +* router: weak_ptr segfaults, replace with shared_ptr +* router: implement params handler +* mavros: router decl done +* lib: port enum_to_string +* lib: update sensor_orientation +* msgs: add linter +* libmavconn: start porintg, will use plain asio, without boost +* msgs: remove redundant dependency which result in colcon warning +* msgs: cogify file lists +* Merge pull request `#1186 `_ from PickNikRobotics/ros2 + mavros_msgs Ros2 +* Merge branch 'ros2' into ros2 +* msgs: start porting to ROS2 +* fixing cmakelists +* updating msg and srv list +* reenable VfrHud once renamed to match ROS2 conventions + add ros1_bridge mapping rule for renamed VfrHud message +* make mavro_msgs compile in ROS 2 +* Contributors: Mikael Arguedas, Mike Lautman, Vladimir Ermakov 1.15.0 (2022-12-30) ------------------- diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt index e2fa4e2f3..dfbd4e3ac 100644 --- a/mavros_msgs/CMakeLists.txt +++ b/mavros_msgs/CMakeLists.txt @@ -1,135 +1,192 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(mavros_msgs) -find_package(catkin REQUIRED COMPONENTS geographic_msgs geometry_msgs message_generation sensor_msgs std_msgs) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # we dont use add_compile_options with pedantic in message packages + # because the Python C extensions dont comply with it + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() -include_directories(include) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(geographic_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) -add_message_files( - DIRECTORY msg - FILES - ADSBVehicle.msg - ActuatorControl.msg - Altitude.msg - AttitudeTarget.msg - BatteryStatus.msg - CamIMUStamp.msg - CellularStatus.msg - CameraImageCaptured.msg - CommandCode.msg - CompanionProcessStatus.msg - OnboardComputerStatus.msg - DebugValue.msg - ESCInfo.msg - ESCInfoItem.msg - ESCStatus.msg - ESCStatusItem.msg - ESCTelemetry.msg - ESCTelemetryItem.msg - EstimatorStatus.msg - ExtendedState.msg - FileEntry.msg - GlobalPositionTarget.msg - GPSINPUT.msg - GPSRAW.msg - GPSRTK.msg - HilActuatorControls.msg - HilControls.msg - HilGPS.msg - HilSensor.msg - HilStateQuaternion.msg - HomePosition.msg - LandingTarget.msg - LogData.msg - LogEntry.msg - MagnetometerReporter.msg - ManualControl.msg - Mavlink.msg - MountControl.msg - NavControllerOutput.msg - OpticalFlowRad.msg - OverrideRCIn.msg - Param.msg - ParamValue.msg - PlayTuneV2.msg - PositionTarget.msg - RCIn.msg - RCOut.msg - RTCM.msg - RadioStatus.msg - RTKBaseline.msg - State.msg - StatusText.msg - TerrainReport.msg - Thrust.msg - TimesyncStatus.msg - Trajectory.msg - Tunnel.msg - VFR_HUD.msg - VehicleInfo.msg - Vibration.msg - Waypoint.msg - WaypointList.msg - WaypointReached.msg - WheelOdomStamped.msg -) +# include_directories(include) + +# [[[cog: +# import mavros_cog +# ]]] +# [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) -add_service_files( - DIRECTORY srv - FILES - CommandAck.srv - CommandBool.srv - CommandHome.srv - CommandInt.srv - CommandLong.srv - CommandTOL.srv - CommandTriggerControl.srv - CommandTriggerInterval.srv - CommandVtolTransition.srv - FileChecksum.srv - FileClose.srv - FileList.srv - FileMakeDir.srv - FileOpen.srv - FileRead.srv - FileRemove.srv - FileRemoveDir.srv - FileRename.srv - FileTruncate.srv - FileWrite.srv - LogRequestData.srv - LogRequestEnd.srv - LogRequestList.srv - MountConfigure.srv - MessageInterval.srv - ParamGet.srv - ParamPull.srv - ParamPush.srv - ParamSet.srv - SetMavFrame.srv - SetMode.srv - StreamRate.srv - VehicleInfoGet.srv - WaypointClear.srv - WaypointPull.srv - WaypointPush.srv - WaypointSetCurrent.srv +set(msg_files + # [[[cog: + # mavros_cog.outl_glob_files('msg', '*.msg') + # ]]] + msg/ADSBVehicle.msg + msg/ActuatorControl.msg + msg/Altitude.msg + msg/AttitudeTarget.msg + msg/CamIMUStamp.msg + msg/CameraImageCaptured.msg + msg/CellularStatus.msg + msg/CommandCode.msg + msg/CompanionProcessStatus.msg + msg/DebugValue.msg + msg/ESCInfo.msg + msg/ESCInfoItem.msg + msg/ESCStatus.msg + msg/ESCStatusItem.msg + msg/ESCTelemetry.msg + msg/ESCTelemetryItem.msg + msg/EstimatorStatus.msg + msg/ExtendedState.msg + msg/FileEntry.msg + msg/GPSINPUT.msg + msg/GPSRAW.msg + msg/GPSRTK.msg + msg/GlobalPositionTarget.msg + msg/HilActuatorControls.msg + msg/HilControls.msg + msg/HilGPS.msg + msg/HilSensor.msg + msg/HilStateQuaternion.msg + msg/HomePosition.msg + msg/LandingTarget.msg + msg/LogData.msg + msg/LogEntry.msg + msg/MagnetometerReporter.msg + msg/ManualControl.msg + msg/Mavlink.msg + msg/MountControl.msg + msg/NavControllerOutput.msg + msg/OnboardComputerStatus.msg + msg/OpticalFlowRad.msg + msg/OverrideRCIn.msg + msg/Param.msg + msg/ParamEvent.msg + msg/ParamValue.msg + msg/PlayTuneV2.msg + msg/PositionTarget.msg + msg/RCIn.msg + msg/RCOut.msg + msg/RTCM.msg + msg/RTKBaseline.msg + msg/RadioStatus.msg + msg/State.msg + msg/StatusText.msg + msg/TerrainReport.msg + msg/Thrust.msg + msg/TimesyncStatus.msg + msg/Trajectory.msg + msg/Tunnel.msg + msg/VehicleInfo.msg + msg/VfrHud.msg + msg/Vibration.msg + msg/Waypoint.msg + msg/WaypointList.msg + msg/WaypointReached.msg + msg/WheelOdomStamped.msg + # [[[end]]] (checksum: cf55cc727ab8f7a2823267cab0dd1bf3) ) -# add_action_files( -# DIRECTORY action -# FILES -# Action1.action -# Action2.action -# ) +set(srv_files + # [[[cog: + # mavros_cog.outl_glob_files('srv', '*.srv') + # ]]] + srv/CommandAck.srv + srv/CommandBool.srv + srv/CommandHome.srv + srv/CommandInt.srv + srv/CommandLong.srv + srv/CommandTOL.srv + srv/CommandTriggerControl.srv + srv/CommandTriggerInterval.srv + srv/CommandVtolTransition.srv + srv/EndpointAdd.srv + srv/EndpointDel.srv + srv/FileChecksum.srv + srv/FileClose.srv + srv/FileList.srv + srv/FileMakeDir.srv + srv/FileOpen.srv + srv/FileRead.srv + srv/FileRemove.srv + srv/FileRemoveDir.srv + srv/FileRename.srv + srv/FileTruncate.srv + srv/FileWrite.srv + srv/LogRequestData.srv + srv/LogRequestEnd.srv + srv/LogRequestList.srv + srv/MessageInterval.srv + srv/MountConfigure.srv + srv/ParamGet.srv + srv/ParamPull.srv + srv/ParamPush.srv + srv/ParamSet.srv + srv/ParamSetV2.srv + srv/SetMavFrame.srv + srv/SetMode.srv + srv/StreamRate.srv + srv/VehicleInfoGet.srv + srv/WaypointClear.srv + srv/WaypointPull.srv + srv/WaypointPush.srv + srv/WaypointSetCurrent.srv + # [[[end]]] (checksum: a011691dec330a2a89d35288ebf05c55) +) -generate_messages(DEPENDENCIES geographic_msgs geometry_msgs sensor_msgs std_msgs) +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES + builtin_interfaces + rcl_interfaces + geographic_msgs + geometry_msgs + sensor_msgs + std_msgs +) -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS geographic_msgs geometry_msgs message_runtime sensor_msgs std_msgs) +ament_export_dependencies(rosidl_default_runtime) install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h") + FILES mavros_msgs_mapping_rule.yaml + DESTINATION share/${PROJECT_NAME} +) + +if(rcl_interfaces_VERSION VERSION_LESS "1.2.0") + install( + DIRECTORY include/ + DESTINATION include + FILES_MATCHING PATTERN "*.hpp" + ) +else() + # NOTE(vooon): Humble + install( + DIRECTORY include/ + DESTINATION include/mavros_msgs + FILES_MATCHING PATTERN "*.hpp" + ) +endif() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # NOTE(vooon): Does not support our custom triple-license, tiered to make it to work. + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() +# vim: ts=2 sw=2 et: diff --git a/mavros_msgs/include/mavros_msgs/mavlink_convert.h b/mavros_msgs/include/mavros_msgs/mavlink_convert.h deleted file mode 100644 index 26a3a08c4..000000000 --- a/mavros_msgs/include/mavros_msgs/mavlink_convert.h +++ /dev/null @@ -1,129 +0,0 @@ -/** - * @brief Mavlink convert utils - * @file - * @author Vladimir Ermakov - */ -/* - * Copyright 2015,2016 Vladimir Ermakov. - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#pragma once - -#include -#include -#include - -namespace mavros_msgs { -namespace mavlink { - -using ::mavlink::mavlink_message_t; - -// [[[cog: -// FIELD_NAMES = [ -// "magic", -// "len", -// "incompat_flags", -// "compat_flags", -// "seq", -// "sysid", -// "compid", -// "msgid", -// "checksum", -// ] -// ]]] -// [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) - -// NOTE(vooon): Ignore impossible warning as -// memcpy() should work with unaligned pointers without any trouble. -// -// warning: taking address of packed member of ‘mavlink::__mavlink_message’ may result in an unaligned pointer value -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Waddress-of-packed-member" - -/** - * @brief Convert mavros_msgs/Mavlink message to mavlink_message_t - * - * @note signature vector should be empty for unsigned OR - * MAVLINK_SIGNATURE_BLOCK size for signed messages - * - * @param[in] rmsg mavros_msgs/Mavlink message - * @param[out] mmsg mavlink_message_t struct - * @return true if success - */ -inline bool convert(const mavros_msgs::Mavlink &rmsg, mavlink_message_t &mmsg) -{ - if (rmsg.payload64.size() > sizeof(mmsg.payload64) / sizeof(mmsg.payload64[0])) { - return false; - } - - if (!rmsg.signature.empty() && rmsg.signature.size() != sizeof(mmsg.signature)) { - return false; - } - - // [[[cog: - // for f in FIELD_NAMES: - // cog.outl("mmsg.%s = rmsg.%s;" % (f, f)) - // ]]] - mmsg.magic = rmsg.magic; - mmsg.len = rmsg.len; - mmsg.incompat_flags = rmsg.incompat_flags; - mmsg.compat_flags = rmsg.compat_flags; - mmsg.seq = rmsg.seq; - mmsg.sysid = rmsg.sysid; - mmsg.compid = rmsg.compid; - mmsg.msgid = rmsg.msgid; - mmsg.checksum = rmsg.checksum; - // [[[end]]] (checksum: 2ef42a7798f261bfd367bf4157b11ec0) - std::copy(rmsg.payload64.begin(), rmsg.payload64.end(), mmsg.payload64); - std::copy(rmsg.signature.begin(), rmsg.signature.end(), mmsg.signature); - - return true; -} - -/** - * @brief Convert mavlink_message_t to mavros/Mavlink - * - * @param[in] mmsg mavlink_message_t struct - * @param[out] rmsg mavros_msgs/Mavlink message - * @param[in] framing_status framing parse result (OK, BAD_CRC or BAD_SIGNATURE) - * @return true, this convertion can't fail - */ -inline bool convert(const mavlink_message_t &mmsg, mavros_msgs::Mavlink &rmsg, uint8_t framing_status = mavros_msgs::Mavlink::FRAMING_OK) -{ - const size_t payload64_len = (mmsg.len + 7) / 8; - - rmsg.framing_status = framing_status; - - // [[[cog: - // for f in FIELD_NAMES: - // cog.outl("rmsg.%s = mmsg.%s;" % (f, f)) - // ]]] - rmsg.magic = mmsg.magic; - rmsg.len = mmsg.len; - rmsg.incompat_flags = mmsg.incompat_flags; - rmsg.compat_flags = mmsg.compat_flags; - rmsg.seq = mmsg.seq; - rmsg.sysid = mmsg.sysid; - rmsg.compid = mmsg.compid; - rmsg.msgid = mmsg.msgid; - rmsg.checksum = mmsg.checksum; - // [[[end]]] (checksum: 4f0a50d2fcd7eb8823aea3e0806cd698) - rmsg.payload64 = mavros_msgs::Mavlink::_payload64_type(mmsg.payload64, mmsg.payload64 + payload64_len); - - // copy signature block only if message is signed - if (mmsg.incompat_flags & MAVLINK_IFLAG_SIGNED) - rmsg.signature = mavros_msgs::Mavlink::_signature_type(mmsg.signature, mmsg.signature + sizeof(mmsg.signature)); - else - rmsg.signature.clear(); - - return true; -} - -#pragma GCC diagnostic pop - -} // namespace mavlink -} // namespace mavros_msgs diff --git a/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp b/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp new file mode 100644 index 000000000..6241e8af7 --- /dev/null +++ b/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp @@ -0,0 +1,145 @@ +// +// Copyright 2015,2016,2021 Vladimir Ermakov. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// +/** + * @brief Mavlink convert utils + * @file + * @author Vladimir Ermakov + */ + +#pragma once +#ifndef MAVROS_MSGS__MAVLINK_CONVERT_HPP_ +#define MAVROS_MSGS__MAVLINK_CONVERT_HPP_ + +#include +#include + +#include + +namespace mavros_msgs +{ +namespace mavlink +{ + +using ::mavlink::mavlink_message_t; +using mavros_msgs::msg::Mavlink; + +// [[[cog: +// FIELD_NAMES = [ +// "magic", +// "len", +// "incompat_flags", +// "compat_flags", +// "seq", +// "sysid", +// "compid", +// "msgid", +// "checksum", +// ] +// ]]] +// [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) + +// NOTE(vooon): Ignore impossible warning as +// memcpy() should work with unaligned pointers without any trouble. +// +// warning: taking address of packed member of ‘mavlink::__mavlink_message’ +// may result in an unaligned pointer value +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Waddress-of-packed-member" + +/** + * @brief Convert mavros_msgs/Mavlink message to mavlink_message_t + * + * @note signature vector should be empty for unsigned OR + * MAVLINK_SIGNATURE_BLOCK size for signed messages + * + * @param[in] rmsg mavros_msgs/Mavlink message + * @param[out] mmsg mavlink_message_t struct + * @return true if success + */ +inline bool convert(const Mavlink & rmsg, mavlink_message_t & mmsg) +{ + if (rmsg.payload64.size() > sizeof(mmsg.payload64) / sizeof(mmsg.payload64[0])) { + return false; + } + + if (!rmsg.signature.empty() && rmsg.signature.size() != sizeof(mmsg.signature)) { + return false; + } + + // [[[cog: + // for f in FIELD_NAMES: + // cog.outl(f"mmsg.{f} = rmsg.{f};") + // ]]] + mmsg.magic = rmsg.magic; + mmsg.len = rmsg.len; + mmsg.incompat_flags = rmsg.incompat_flags; + mmsg.compat_flags = rmsg.compat_flags; + mmsg.seq = rmsg.seq; + mmsg.sysid = rmsg.sysid; + mmsg.compid = rmsg.compid; + mmsg.msgid = rmsg.msgid; + mmsg.checksum = rmsg.checksum; + // [[[end]]] (checksum: 0b66f0fc1cd46db0f18a2429c56a6b8c) + std::copy(rmsg.payload64.begin(), rmsg.payload64.end(), mmsg.payload64); + std::copy(rmsg.signature.begin(), rmsg.signature.end(), mmsg.signature); + + return true; +} + +/** + * @brief Convert mavlink_message_t to mavros/Mavlink + * + * @param[in] mmsg mavlink_message_t struct + * @param[out] rmsg mavros_msgs/Mavlink message + * @param[in] framing_status framing parse result (OK, BAD_CRC or BAD_SIGNATURE) + * @return true, this convertion can't fail + */ +inline bool convert( + const mavlink_message_t & mmsg, Mavlink & rmsg, + uint8_t framing_status = Mavlink::FRAMING_OK) +{ + const size_t payload64_len = (mmsg.len + 7) / 8; + + rmsg.framing_status = framing_status; + + // [[[cog: + // for f in FIELD_NAMES: + // cog.outl(f"rmsg.{f} = mmsg.{f};") + // ]]] + rmsg.magic = mmsg.magic; + rmsg.len = mmsg.len; + rmsg.incompat_flags = mmsg.incompat_flags; + rmsg.compat_flags = mmsg.compat_flags; + rmsg.seq = mmsg.seq; + rmsg.sysid = mmsg.sysid; + rmsg.compid = mmsg.compid; + rmsg.msgid = mmsg.msgid; + rmsg.checksum = mmsg.checksum; + // [[[end]]] (checksum: 64ef6c1af60c622ed427e005d8ca4f2a) + rmsg.payload64.assign( + mmsg.payload64, + mmsg.payload64 + payload64_len); + + // copy signature block only if message is signed + if (mmsg.incompat_flags & MAVLINK_IFLAG_SIGNED) { + rmsg.signature.assign( + mmsg.signature, + mmsg.signature + sizeof(mmsg.signature)); + } else { + rmsg.signature.clear(); + } + + return true; +} + +#pragma GCC diagnostic pop + +} // namespace mavlink +} // namespace mavros_msgs + +#endif // MAVROS_MSGS__MAVLINK_CONVERT_HPP_ diff --git a/mavros_msgs/mavros_msgs_mapping_rule.yaml b/mavros_msgs/mavros_msgs_mapping_rule.yaml new file mode 100644 index 000000000..9f69364ab --- /dev/null +++ b/mavros_msgs/mavros_msgs_mapping_rule.yaml @@ -0,0 +1,8 @@ +# This file defines mappings between ROS 1 and ROS 2 interfaces. +# It is used with the ros1_bridge to allow for communcation between ROS 1 and ROS 2. + +- + ros1_package_name: 'mavros_msgs' + ros1_message_name: 'VFR_HUD' + ros2_package_name: 'mavros_msgs' + ros2_message_name: 'VfrHud' diff --git a/mavros_msgs/msg/ADSBVehicle.msg b/mavros_msgs/msg/ADSBVehicle.msg index 51d0258ca..430efe28f 100644 --- a/mavros_msgs/msg/ADSBVehicle.msg +++ b/mavros_msgs/msg/ADSBVehicle.msg @@ -60,7 +60,7 @@ uint16 FLAG_SOURCE_UAT = 32768 std_msgs/Header header -uint32 ICAO_address +uint32 icao_address string callsign float64 latitude @@ -74,6 +74,6 @@ float32 ver_velocity # m/s uint8 altitude_type # Type from ADSB_ALTITUDE_TYPE enum uint8 emitter_type # Type from ADSB_EMITTER_TYPE enum -duration tslc # Duration from last communication, seconds [0..255] +builtin_interfaces/Duration tslc # Duration from last communication, seconds [0..255] uint16 flags # ADSB_FLAGS bit field uint16 squawk # Squawk code diff --git a/mavros_msgs/msg/BatteryStatus.msg b/mavros_msgs/msg/BatteryStatus.msg deleted file mode 100644 index d5b175be2..000000000 --- a/mavros_msgs/msg/BatteryStatus.msg +++ /dev/null @@ -1,9 +0,0 @@ -# Represent battery status from SYSTEM_STATUS -# -# To be replaced when sensor_msgs/BatteryState PR will be merged -# https://github.com/ros/common_msgs/pull/74 - -std_msgs/Header header -float32 voltage # [V] -float32 current # [A] -float32 remaining # 0..1 diff --git a/mavros_msgs/msg/CamIMUStamp.msg b/mavros_msgs/msg/CamIMUStamp.msg index 63fe92d08..52b9e8b33 100644 --- a/mavros_msgs/msg/CamIMUStamp.msg +++ b/mavros_msgs/msg/CamIMUStamp.msg @@ -1,4 +1,4 @@ # IMU-Camera synchronisation data -time frame_stamp # Timestamp when the camera was triggered +builtin_interfaces/Time frame_stamp # Timestamp when the camera was triggered int32 frame_seq_id # Sequence number of the image frame diff --git a/mavros_msgs/msg/CommandCode.msg b/mavros_msgs/msg/CommandCode.msg index b1fe834d9..38de8774e 100644 --- a/mavros_msgs/msg/CommandCode.msg +++ b/mavros_msgs/msg/CommandCode.msg @@ -7,17 +7,19 @@ # from collections import OrderedDict # import re # +# # def wr_enum(enum, ename, pfx='', bsz=16): -# cog.outl("# " + ename + "_" + pfx) +# cog.outl(f"# {ename}_{pfx}") # for k, e in enum: # # exclude also deprecated commands -# if 'MAV_CMD' + "_" + pfx in e.name and not re.search('deprecated', e.description, re.IGNORECASE): -# sn = e.name[len('MAV_CMD') + 1:] -# l = "uint{bsz} {sn} = {k}".format(**locals()) +# if f"{ename}_{pfx}" in e.name and not re.search('deprecated', e.description, re.IGNORECASE): +# sn = e.name[len(ename) + 1:] +# l = f"uint{bsz} {sn} = {k}" # if e.description: -# l += ' ' * (50 - len(l)) + ' # ' + e.description +# l += f"{' ' * (50 - len(l))} # {e.description.strip()}" # cog.outl(l) -# cog.out('\n') +# cog.outl() +# # # def decl_enum(ename): # enum = sorted(common.enums[ename].items()) @@ -25,7 +27,7 @@ # # enumt = [] # # exception list of commands to not include -# exlist = ['SPATIAL', 'USER', 'WAYPOINT'] +# exlist = [] # ['SPATIAL', 'USER', 'WAYPOINT'] # for k, e in enum: # enumt.extend(e.name[len(ename) + 1:].split('_')[0:1]) # @@ -35,6 +37,7 @@ # for key in enumt: # wr_enum(enum, ename, key) # +# # decl_enum('MAV_CMD') # ]]] # MAV_CMD_AIRFRAME @@ -42,7 +45,9 @@ uint16 AIRFRAME_CONFIGURATION = 2520 # MAV_CMD_ARM uint16 ARM_AUTHORIZATION_REQUEST = 3001 # Request authorization to arm the vehicle to a external entity, the arm authorizer is responsible to request all data that is needs from the vehicle before authorize or deny the request. If approved the progress of command_ack message should be set with period of time that this authorization is valid in seconds or in case it was denied it should be set with one of the reasons in ARM_AUTH_DENIED_REASON. - + +# MAV_CMD_CAN +uint16 CAN_FORWARD = 32000 # Request forwarding of CAN packets from the given CAN bus to this interface. CAN Frames are sent using CAN_FRAME and CANFD_FRAME messages # MAV_CMD_COMPONENT uint16 COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component @@ -102,6 +107,7 @@ uint16 DO_ENGINE_CONTROL = 223 # Control vehicle engine. Thi uint16 DO_SET_MISSION_CURRENT = 224 # Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between). uint16 DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration uint16 DO_JUMP_TAG = 601 # Jump to the matching tag in the mission list. Repeat this action for the specified number of times. A mission should contain a single matching tag for each jump. If this is not the case then a jump to a missing tag should complete the mission, and a jump where there are multiple matching tags should always select the one with the lowest mission sequence number. +uint16 DO_GIMBAL_MANAGER_PITCHYAW = 1000 # High level setpoint to be sent to a gimbal manager to set a gimbal attitude. It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: a gimbal is never to react to this command but only the gimbal manager. uint16 DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system. uint16 DO_VTOL_TRANSITION = 3000 # Request VTOL transition uint16 DO_ADSB_OUT_IDENT = 10001 # Trigger the start of an ADSB-out IDENT. This should only be used when requested to do so by an Air Traffic Controller in controlled airspace. This starts the IDENT which is then typically held for 18 seconds by the hardware per the Mode A, C, and S transponder spec. @@ -144,7 +150,7 @@ uint16 NAV_LOITER_TO_ALT = 31 # Begin loiter at the specifi uint16 NAV_ROI = 80 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. uint16 NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV. uint16 NAV_SPLINE_WAYPOINT = 82 # Navigate to waypoint using a spline path. -uint16 NAV_VTOL_TAKEOFF = 84 # Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. +uint16 NAV_VTOL_TAKEOFF = 84 # Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. The command should be ignored by vehicles that dont support both VTOL and fixed-wing flight (multicopters, boats,etc.). uint16 NAV_VTOL_LAND = 85 # Land using VTOL mode uint16 NAV_GUIDED_ENABLE = 92 # hand control over to an external controller uint16 NAV_DELAY = 93 # Delay the next navigation command a number of seconds or until a specified time @@ -153,15 +159,10 @@ uint16 NAV_LAST = 95 # NOP - This command is only uint16 NAV_SET_YAW_SPEED = 213 # Sets a desired vehicle turn angle and speed change. uint16 NAV_FENCE_RETURN_POINT = 5000 # Fence return point (there can only be one such point in a geofence definition). If rally points are supported they should be used instead. uint16 NAV_FENCE_POLYGON_VERTEX_INCLUSION = 5001 # Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). The vehicle must stay within this area. Minimum of 3 vertices required. - uint16 NAV_FENCE_POLYGON_VERTEX_EXCLUSION = 5002 # Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). The vehicle must stay outside this area. Minimum of 3 vertices required. - uint16 NAV_FENCE_CIRCLE_INCLUSION = 5003 # Circular fence area. The vehicle must stay inside this area. - uint16 NAV_FENCE_CIRCLE_EXCLUSION = 5004 # Circular fence area. The vehicle must stay outside this area. - uint16 NAV_RALLY_POINT = 5100 # Rally point. You can have multiple rally points defined. - # MAV_CMD_OBLIQUE uint16 OBLIQUE_SURVEY = 260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey (Replaces CAM_TRIGG_DIST for this purpose). The camera is triggered each time this distance is exceeded, then the mount moves to the next position. Params 4~6 set-up the angle limits and number of positions for oblique survey, where mount-enabled vehicles automatically roll the camera between shots to emulate an oblique camera setup (providing an increased HFOV). This command can also be used to set the shutter integration time for the camera. @@ -205,9 +206,14 @@ uint16 RUN_PREARM_CHECKS = 401 # Instructs system to run pre uint16 SET_MESSAGE_INTERVAL = 511 # Set the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM. uint16 SET_CAMERA_MODE = 530 # Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS command after a mode change if the camera supports video streaming. uint16 SET_GUIDED_SUBMODE_STANDARD = 4000 # This command sets the submode to standard guided when vehicle is in guided mode. The vehicle holds position and altitude and the user can input the desired velocities along all three axes. - uint16 SET_GUIDED_SUBMODE_CIRCLE = 4001 # This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position. - + +# MAV_CMD_SPATIAL +uint16 SPATIAL_USER_1 = 31005 # User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. +uint16 SPATIAL_USER_2 = 31006 # User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. +uint16 SPATIAL_USER_3 = 31007 # User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. +uint16 SPATIAL_USER_4 = 31008 # User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. +uint16 SPATIAL_USER_5 = 31009 # User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. # MAV_CMD_START uint16 START_RX_PAIR = 500 # Starts receiver pairing. @@ -218,10 +224,24 @@ uint16 STORAGE_FORMAT = 526 # Format a storage medium. On # MAV_CMD_UAVCAN uint16 UAVCAN_GET_NODE_INFO = 5200 # Commands the vehicle to respond with a sequence of messages UAVCAN_NODE_INFO, one message per every UAVCAN node that is online. Note that some of the response messages can be lost, which the receiver can detect easily by checking whether every received UAVCAN_NODE_STATUS has a matching message UAVCAN_NODE_INFO received earlier; if not, this command should be sent again in order to request re-transmission of the node information messages. +# MAV_CMD_USER +uint16 USER_1 = 31010 # User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. +uint16 USER_2 = 31011 # User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. +uint16 USER_3 = 31012 # User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. +uint16 USER_4 = 31013 # User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. +uint16 USER_5 = 31014 # User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. + # MAV_CMD_VIDEO uint16 VIDEO_START_CAPTURE = 2500 # Starts video capture (recording). uint16 VIDEO_STOP_CAPTURE = 2501 # Stop the current video capture (recording). uint16 VIDEO_START_STREAMING = 2502 # Start video streaming uint16 VIDEO_STOP_STREAMING = 2503 # Stop the given video stream -# [[[end]]] (checksum: 01fa1723eb3702c301c5562fdf293bbc) +# MAV_CMD_WAYPOINT +uint16 WAYPOINT_USER_1 = 31000 # User defined waypoint item. Ground Station will show the Vehicle as flying through this item. +uint16 WAYPOINT_USER_2 = 31001 # User defined waypoint item. Ground Station will show the Vehicle as flying through this item. +uint16 WAYPOINT_USER_3 = 31002 # User defined waypoint item. Ground Station will show the Vehicle as flying through this item. +uint16 WAYPOINT_USER_4 = 31003 # User defined waypoint item. Ground Station will show the Vehicle as flying through this item. +uint16 WAYPOINT_USER_5 = 31004 # User defined waypoint item. Ground Station will show the Vehicle as flying through this item. + +# [[[end]]] (checksum: 77e70a4f67838b47c3d086432dd58cb7) diff --git a/mavros_msgs/msg/FileEntry.msg b/mavros_msgs/msg/FileEntry.msg index 398154fdd..e73332695 100644 --- a/mavros_msgs/msg/FileEntry.msg +++ b/mavros_msgs/msg/FileEntry.msg @@ -8,5 +8,5 @@ uint8 type uint64 size # Not supported by MAVLink FTP -#time atime +#builtin_interfaces/Time atime #int32 access_flags diff --git a/mavros_msgs/msg/LogEntry.msg b/mavros_msgs/msg/LogEntry.msg index edb817f89..b2126866a 100644 --- a/mavros_msgs/msg/LogEntry.msg +++ b/mavros_msgs/msg/LogEntry.msg @@ -11,5 +11,5 @@ std_msgs/Header header uint16 id uint16 num_logs uint16 last_log_num -time time_utc +builtin_interfaces/Time time_utc uint32 size diff --git a/mavros_msgs/msg/Mavlink.msg b/mavros_msgs/msg/Mavlink.msg index b58123d12..afb80ed3b 100644 --- a/mavros_msgs/msg/Mavlink.msg +++ b/mavros_msgs/msg/Mavlink.msg @@ -10,7 +10,7 @@ # :magic: # STX byte, used to determine protocol version v1.0 or v2.0. # -# Please use mavros_msgs::mavlink::convert() from +# Please use mavros_msgs::mavlink::convert() from # to convert between ROS and MAVLink message type # mavlink_framing_t enum @@ -34,5 +34,5 @@ uint8 sysid uint8 compid uint32 msgid # 24-bit message id uint16 checksum -uint64[] payload64 -uint8[] signature # optional signature +uint64[<=33] payload64 # max size: (255+2+7)/8 +uint8[<=13] signature # optional signature, max size: 13 diff --git a/mavros_msgs/msg/Param.msg b/mavros_msgs/msg/Param.msg index 9d9abb3c7..f2ccfaa63 100644 --- a/mavros_msgs/msg/Param.msg +++ b/mavros_msgs/msg/Param.msg @@ -1,5 +1,7 @@ # Parameter msg. +# XXX DEPRECATED: replaced by ParamEvent + std_msgs/Header header string param_id diff --git a/mavros_msgs/msg/ParamEvent.msg b/mavros_msgs/msg/ParamEvent.msg new file mode 100644 index 000000000..493df0a1b --- /dev/null +++ b/mavros_msgs/msg/ParamEvent.msg @@ -0,0 +1,14 @@ +# Parameter Event +# +# That messages replaces mavros_msgs/Param from mavros v1. +# Reason for that: ROS2 have native message for parameters +# +# ROS2 also have it's own ParameterEvent stream, which could be used +# to get FCU updates too. But that message is simpler to use. + +std_msgs/Header header + +string param_id +rcl_interfaces/ParameterValue value +uint16 param_index +uint16 param_count diff --git a/mavros_msgs/msg/ParamValue.msg b/mavros_msgs/msg/ParamValue.msg index 377d67521..4a98639c4 100644 --- a/mavros_msgs/msg/ParamValue.msg +++ b/mavros_msgs/msg/ParamValue.msg @@ -6,5 +6,7 @@ # else if real != 0.0: it is float value # else: it is zero. +# XXX DEPRECATED: replaced by rmw_interfaces/ParameterValue + int64 integer float64 real diff --git a/mavros_msgs/msg/RTCM.msg b/mavros_msgs/msg/RTCM.msg index 048e02a5a..8c7449825 100644 --- a/mavros_msgs/msg/RTCM.msg +++ b/mavros_msgs/msg/RTCM.msg @@ -1,5 +1,5 @@ # RTCM message for the gps_rtk plugin -# The gps_rtk plugin will fragment the data if necessary and +# The gps_rtk plugin will fragment the data if necessary and # forward it to the FCU via Mavlink through the available link. # data should be <= 4*180, higher will be discarded. std_msgs/Header header diff --git a/mavros_msgs/msg/VFR_HUD.msg b/mavros_msgs/msg/VfrHud.msg similarity index 100% rename from mavros_msgs/msg/VFR_HUD.msg rename to mavros_msgs/msg/VfrHud.msg diff --git a/mavros_msgs/msg/WaypointReached.msg b/mavros_msgs/msg/WaypointReached.msg index 40bcf7999..6fc36a908 100644 --- a/mavros_msgs/msg/WaypointReached.msg +++ b/mavros_msgs/msg/WaypointReached.msg @@ -2,6 +2,6 @@ # # :wp_seq: index number of reached waypoint -Header header +std_msgs/Header header uint16 wp_seq diff --git a/mavros_msgs/package.xml b/mavros_msgs/package.xml index 59e6019bc..941874dd2 100644 --- a/mavros_msgs/package.xml +++ b/mavros_msgs/package.xml @@ -1,13 +1,14 @@ - + + mavros_msgs - 1.16.0 + 2.5.0 mavros_msgs defines messages for MAVROS. Vladimir Ermakov - Vladimir Ermakov + GPLv3 LGPLv3 BSD @@ -16,19 +17,30 @@ https://github.com/mavlink/mavros.git https://github.com/mavlink/mavros/issues - catkin + Vladimir Ermakov + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime - message_generation - message_runtime - std_msgs + + rcl_interfaces + geographic_msgs geometry_msgs sensor_msgs - geographic_msgs + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + - + ament_cmake + diff --git a/mavros_msgs/srv/EndpointAdd.srv b/mavros_msgs/srv/EndpointAdd.srv new file mode 100644 index 000000000..c102adb92 --- /dev/null +++ b/mavros_msgs/srv/EndpointAdd.srv @@ -0,0 +1,14 @@ +# +# Adds endpoint to router +# + +uint8 TYPE_FCU = 0 +uint8 TYPE_GCS = 1 +uint8 TYPE_UAS = 2 + +string url # mavconn URL or topic prefix for TYPE_UAS +uint8 type # should be set to one of the TYPE_xxx +--- +bool successful # true if endpoint added and opened +string reason # returns error description if open fails +uint32 id # ID of new endpoint, should be > 0 if endpoint created diff --git a/mavros_msgs/srv/EndpointDel.srv b/mavros_msgs/srv/EndpointDel.srv new file mode 100644 index 000000000..04d1ff307 --- /dev/null +++ b/mavros_msgs/srv/EndpointDel.srv @@ -0,0 +1,17 @@ +# +# Removes endpoint from router +# + +uint8 TYPE_FCU = 0 +uint8 TYPE_GCS = 1 +uint8 TYPE_UAS = 2 + +# delete by ID, leave 0 for second option +uint32 id + +# delete by url+type pair +string url +uint8 type + +--- +bool successful diff --git a/mavros_msgs/srv/ParamGet.srv b/mavros_msgs/srv/ParamGet.srv index 858b85ce1..cf092f06e 100644 --- a/mavros_msgs/srv/ParamGet.srv +++ b/mavros_msgs/srv/ParamGet.srv @@ -1,5 +1,7 @@ # Request parameter from attached device +# XXX DEPRECATED: use ros2 parameters api instead + string param_id --- bool success diff --git a/mavros_msgs/srv/ParamPush.srv b/mavros_msgs/srv/ParamPush.srv index 794a7df94..fe0d4f876 100644 --- a/mavros_msgs/srv/ParamPush.srv +++ b/mavros_msgs/srv/ParamPush.srv @@ -2,6 +2,8 @@ # # Returns success status and param_transfered count +# XXX DEPRECATED: not used. param plugin listen to parameter changes + --- bool success uint32 param_transfered diff --git a/mavros_msgs/srv/ParamSet.srv b/mavros_msgs/srv/ParamSet.srv index 3cb3c7220..f5b64d96c 100644 --- a/mavros_msgs/srv/ParamSet.srv +++ b/mavros_msgs/srv/ParamSet.srv @@ -1,5 +1,7 @@ # Request set parameter value +# XXX DEPRECATED: replaced by ParamSetV2 + string param_id mavros_msgs/ParamValue value --- diff --git a/mavros_msgs/srv/ParamSetV2.srv b/mavros_msgs/srv/ParamSetV2.srv new file mode 100644 index 000000000..e06b85a43 --- /dev/null +++ b/mavros_msgs/srv/ParamSetV2.srv @@ -0,0 +1,14 @@ +# Request set parameter value +# +# That interface allow to bypass some checks +# and send value as is to the FCU if force_set is true. +# +# Use that api only if ROS2 Parameter API is not sufficient +# for your application. + +bool force_set +string param_id +rcl_interfaces/ParameterValue value +--- +bool success +rcl_interfaces/ParameterValue value diff --git a/mavros_msgs/srv/SetMavFrame.srv b/mavros_msgs/srv/SetMavFrame.srv index 97f3552b9..342b8d39e 100644 --- a/mavros_msgs/srv/SetMavFrame.srv +++ b/mavros_msgs/srv/SetMavFrame.srv @@ -1,5 +1,7 @@ # Set MAV_FRAME for setpoints +# XXX DEPRECATED + # [[[cog: # from pymavlink.dialects.v20 import common # @@ -19,18 +21,18 @@ # ]]] # MAV_FRAME uint8 FRAME_GLOBAL = 0 # Global (WGS84) coordinate frame + MSL altitude. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). -uint8 FRAME_LOCAL_NED = 1 # Local coordinate frame, Z-down (x: North, y: East, z: Down). +uint8 FRAME_LOCAL_NED = 1 # NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. uint8 FRAME_MISSION = 2 # NOT a coordinate frame, indicates a mission command. uint8 FRAME_GLOBAL_RELATIVE_ALT = 3 # Global (WGS84) coordinate frame + altitude relative to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. -uint8 FRAME_LOCAL_ENU = 4 # Local coordinate frame, Z-up (x: East, y: North, z: Up). -uint8 FRAME_GLOBAL_INT = 5 # Global (WGS84) coordinate frame (scaled) + MSL altitude. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL). -uint8 FRAME_GLOBAL_RELATIVE_ALT_INT = 6 # Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location. -uint8 FRAME_LOCAL_OFFSET_NED = 7 # Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. -uint8 FRAME_BODY_NED = 8 # Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. -uint8 FRAME_BODY_OFFSET_NED = 9 # Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. +uint8 FRAME_LOCAL_ENU = 4 # ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. +uint8 FRAME_GLOBAL_INT = 5 # Global (WGS84) coordinate frame (scaled) + MSL altitude. First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude over mean sea level (MSL). +uint8 FRAME_GLOBAL_RELATIVE_ALT_INT = 6 # Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude with 0 being at the altitude of the home location. +uint8 FRAME_LOCAL_OFFSET_NED = 7 # NED local tangent frame (x: North, y: East, z: Down) with origin that travels with the vehicle. +uint8 FRAME_BODY_NED = 8 # Same as MAV_FRAME_LOCAL_NED when used to represent position values. Same as MAV_FRAME_BODY_FRD when used with velocity/accelaration values. +uint8 FRAME_BODY_OFFSET_NED = 9 # This is the same as MAV_FRAME_BODY_FRD. uint8 FRAME_GLOBAL_TERRAIN_ALT = 10 # Global (WGS84) coordinate frame with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. -uint8 FRAME_GLOBAL_TERRAIN_ALT_INT = 11 # Global (WGS84) coordinate frame (scaled) with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. -uint8 FRAME_BODY_FRD = 12 # Body fixed frame of reference, Z-down (x: Forward, y: Right, z: Down). +uint8 FRAME_GLOBAL_TERRAIN_ALT_INT = 11 # Global (WGS84) coordinate frame (scaled) with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. +uint8 FRAME_BODY_FRD = 12 # FRD local tangent frame (x: Forward, y: Right, z: Down) with origin that travels with vehicle. The forward axis is aligned to the front of the vehicle in the horizontal plane. uint8 FRAME_RESERVED_13 = 13 # MAV_FRAME_BODY_FLU - Body fixed frame of reference, Z-up (x: Forward, y: Left, z: Up). uint8 FRAME_RESERVED_14 = 14 # MAV_FRAME_MOCAP_NED - Odometry local coordinate frame of data given by a motion capture system, Z-down (x: North, y: East, z: Down). uint8 FRAME_RESERVED_15 = 15 # MAV_FRAME_MOCAP_ENU - Odometry local coordinate frame of data given by a motion capture system, Z-up (x: East, y: North, z: Up). @@ -38,9 +40,9 @@ uint8 FRAME_RESERVED_16 = 16 # MAV_FRAME_VISION_NED - Odometry local uint8 FRAME_RESERVED_17 = 17 # MAV_FRAME_VISION_ENU - Odometry local coordinate frame of data given by a vision estimation system, Z-up (x: East, y: North, z: Up). uint8 FRAME_RESERVED_18 = 18 # MAV_FRAME_ESTIM_NED - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-down (x: North, y: East, z: Down). uint8 FRAME_RESERVED_19 = 19 # MAV_FRAME_ESTIM_ENU - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-up (x: East, y: North, z: Up). -uint8 FRAME_LOCAL_FRD = 20 # Forward, Right, Down coordinate frame. This is a local frame with Z-down and arbitrary F/R alignment (i.e. not aligned with NED/earth frame). -uint8 FRAME_LOCAL_FLU = 21 # Forward, Left, Up coordinate frame. This is a local frame with Z-up and arbitrary F/L alignment (i.e. not aligned with ENU/earth frame). -# [[[end]]] (checksum: 013a057712ce80e0a4bffa7c5e2c05a9) +uint8 FRAME_LOCAL_FRD = 20 # FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane. +uint8 FRAME_LOCAL_FLU = 21 # FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane. +# [[[end]]] (checksum: c5ddb537c91e87c4efba8b24c9cde50e) uint8 mav_frame --- diff --git a/test_mavros/AMENT_IGNORE b/test_mavros/AMENT_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/test_mavros/CHANGELOG.rst b/test_mavros/CHANGELOG.rst index ef383ebec..3f7fe76c7 100644 --- a/test_mavros/CHANGELOG.rst +++ b/test_mavros/CHANGELOG.rst @@ -2,9 +2,6 @@ Changelog for package test_mavros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.16.0 (2023-05-05) -------------------- - 1.15.0 (2022-12-30) ------------------- diff --git a/test_mavros/COLCON_IGNORE b/test_mavros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/test_mavros/package.xml b/test_mavros/package.xml index 4bfcbbd23..f1994fe26 100644 --- a/test_mavros/package.xml +++ b/test_mavros/package.xml @@ -1,7 +1,7 @@ test_mavros - 1.16.0 + 1.15.0 Tests for MAVROS package Vladimir Ermakov diff --git a/test_mavros/scripts/setpoint_position_demo b/test_mavros/scripts/setpoint_position_demo index 3caf837ef..078dd00b6 100755 --- a/test_mavros/scripts/setpoint_position_demo +++ b/test_mavros/scripts/setpoint_position_demo @@ -20,17 +20,18 @@ # - Changed topic names after re-factoring : https://github.com/mavlink/mavros/issues/233 # - Use mavros.setpoint module for topics -import rospy -import thread import threading import time -import mavros - from math import * -from mavros.utils import * -from mavros import setpoint as SP + +import rospy +import thread from tf.transformations import quaternion_from_euler +import mavros +from mavros import setpoint as SP +from mavros.utils import * + class SetpointPosition: """ @@ -57,12 +58,12 @@ class SetpointPosition: self.done_evt = threading.Event() def navigate(self): - rate = rospy.Rate(10) # 10hz + rate = rospy.Rate(10) # 10hz msg = SP.PoseStamped( header=SP.Header( frame_id="base_footprint", # no matter, plugin don't use TF - stamp=rospy.Time.now()), # stamp should update + stamp=rospy.Time.now()), # stamp should update ) while not rospy.is_shutdown(): @@ -132,7 +133,7 @@ def setpoint_demo(): radius = 20 rospy.loginfo("Fly in a circle") - setpoint.set(0.0, 0.0, 10.0, 3) # Climb to the starting height first + setpoint.set(0.0, 0.0, 10.0, 3) # Climb to the starting height first i = 0 while not rospy.is_shutdown(): x = radius * cos(i * 2 * pi / sides) + offset_x @@ -157,11 +158,11 @@ def setpoint_demo(): break # Simulate a slow landing. - setpoint.set(0.0, 0.0, 8.0, 5) - setpoint.set(0.0, 0.0, 3.0, 5) - setpoint.set(0.0, 0.0, 2.0, 2) - setpoint.set(0.0, 0.0, 1.0, 2) - setpoint.set(0.0, 0.0, 0.0, 2) + setpoint.set(0.0, 0.0, 8.0, 5) + setpoint.set(0.0, 0.0, 3.0, 5) + setpoint.set(0.0, 0.0, 2.0, 2) + setpoint.set(0.0, 0.0, 1.0, 2) + setpoint.set(0.0, 0.0, 0.0, 2) setpoint.set(0.0, 0.0, -0.2, 2) rospy.loginfo("Bye!") diff --git a/test_mavros/scripts/test_mavlink_convert_to_rosmsg.py b/test_mavros/scripts/test_mavlink_convert_to_rosmsg.py new file mode 100644 index 000000000..576845b16 --- /dev/null +++ b/test_mavros/scripts/test_mavlink_convert_to_rosmsg.py @@ -0,0 +1,99 @@ +""" +When build mavlink message and pub it to mavlink_sink +the payload_octets can be float number +just need int casting to fix the issue + +mavlink write demo +Traceback (most recent call last): + File "/home/user/apm_ws/install/apm_demos/lib/apm_demos/mav_writer_demo", line 33, in + sys.exit(load_entry_point('apm-demos==0.0.0', 'console_scripts', 'mav_writer_demo')()) + File "/home/user/apm_ws/install/apm_demos/lib/python3.10/site-packages/apm_demos/mav_writer_demo.py", line 73, in main + node = MyNode() + File "/home/user/apm_ws/install/apm_demos/lib/python3.10/site-packages/apm_demos/mav_writer_demo.py", line 39, in __init__ + self.set_home() + File "/home/user/apm_ws/install/apm_demos/lib/python3.10/site-packages/apm_demos/mav_writer_demo.py", line 67, in set_home + ros_msg = convert_to_rosmsg(msg, stamp=stamp) + File "/opt/ros/humble/local/lib/python3.10/dist-packages/mavros/mavlink.py", line 141, in convert_to_rosmsg + payload64=convert_to_payload64(mavmsg.get_payload()), + File "/opt/ros/humble/local/lib/python3.10/dist-packages/mavros/mavlink.py", line 92, in convert_to_payload64 + return struct.unpack(f"<{payload_octets}Q", payload_bytes) +struct.error: bad char in struct format + +""" + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from rclpy.clock import Clock +from builtin_interfaces.msg import Time as HTime +from mavros_msgs.msg import Mavlink +from builtin_interfaces.msg import Time +from pymavlink.dialects.v10 import ardupilotmega +from mavros.mavlink import convert_to_rosmsg +from pymavlink.dialects.v20 import ardupilotmega as apm + +DRONE_NO = 1 +TOPIC_MAVLINK = f"/uas{DRONE_NO}/mavlink_sink" + +class fifo(object): + """ A simple buffer """ + def __init__(self): + self.buf = [] + def write(self, data): + self.buf += data + return len(data) + def read(self): + return self.buf.pop(0) + +class MyNode(Node): + def __init__(self) -> None: + node_name = "mav_writer" + super().__init__(node_name) + f = fifo() + self.__mav = apm.MAVLink(f, srcSystem=1, srcComponent=1) + self.__pub_mavlink = self.create_publisher( + Mavlink, + TOPIC_MAVLINK, + qos_profile=qos_profile_sensor_data, + ) + self.get_logger().info("init mavlink write demo") + self.set_home() + + def set_home(self) -> None: + target_system = 0 # broadcast to everyone + msg = ardupilotmega.MAVLink_command_long_message( + target_system, + 1, + ardupilotmega.MAV_CMD_DO_SET_HOME, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ) + + msg.pack(self.__mav) + current = Clock().now() + sec, nanosec = current.seconds_nanoseconds() + stamp = HTime(sec=sec, nanosec=nanosec) + ros_msg = convert_to_rosmsg(msg, stamp=stamp) + self.__pub_mavlink.publish(ros_msg) + print(ros_msg) + +def main(args=None): + rclpy.init(args=args) + node = MyNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + print("User exit") + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main()