From 564cb477f61f72b20deff33c792d068d2027dd39 Mon Sep 17 00:00:00 2001 From: mickey li Date: Fri, 10 Dec 2021 11:07:19 +0000 Subject: [PATCH 01/25] Updated all to use ROS2 docker --- system/mavros/Dockerfile | 43 +- system/mavros/config/mavros_config_px4.yaml | 530 +++++++++--------- .../mavros/config/mavros_pluginlists_px4.yaml | 62 +- system/mavros/mavros.launch | 32 -- system/mavros/mavros.launch.xml | 28 + system/mavros/mavros_bridge.launch.xml | 38 -- system/mavros/mavros_setup.sh | 30 +- system/mavros/ros_entrypoint.sh | 12 +- system/mavros/run_ros1.sh | 5 - 9 files changed, 343 insertions(+), 437 deletions(-) delete mode 100644 system/mavros/mavros.launch create mode 100644 system/mavros/mavros.launch.xml delete mode 100644 system/mavros/mavros_bridge.launch.xml delete mode 100755 system/mavros/run_ros1.sh diff --git a/system/mavros/Dockerfile b/system/mavros/Dockerfile index a0a3a77..f02483f 100644 --- a/system/mavros/Dockerfile +++ b/system/mavros/Dockerfile @@ -1,42 +1,13 @@ -FROM ros:foxy-ros1-bridge +FROM ros:foxy-ros-base-focal # Install mavros RUN apt-get update \ && apt-get install --no-install-recommends -y \ - ros-${ROS1_DISTRO}-mavros \ - ros-${ROS1_DISTRO}-mavros-extras \ - ros-${ROS2_DISTRO}-mavros-msgs \ + ros-${ROS_DISTRO}-mavros \ && rm -rf /var/lib/apt/lists/ # Part of MAVROS setup -RUN /opt/ros/${ROS1_DISTRO}/lib/mavros/install_geographiclib_datasets.sh - -# We then need to build the bridge from source -# (So we probably don't need to use the ros1-bridge image...) -# Remove the preinstalled version -RUN apt-get remove -y ros-foxy-ros1-bridge -# Get the source (use specific commit hash instead of foxy due to this issue when building mavros: https://github.com/ros2/ros1_bridge/issues/313) -RUN git clone -b foxy https://github.com/ros2/ros1_bridge /bridge_ws/src/ros1_bridge \ - && cd /bridge_ws/src/ros1_bridge \ - && git checkout 82d1f4588761d6e54eeaca821398137be0e475f1 - -# Swap line 36 and 41 to fix this issue: https://github.com/ros2/ros1_bridge/issues/316 -# For use of parameter server. -RUN sed -i "41d" /bridge_ws/src/ros1_bridge/src/parameter_bridge.cpp \ - && sed -i "36 i rclcpp::init(argc, argv);" /bridge_ws/src/ros1_bridge/src/parameter_bridge.cpp - -# Build with ROS2 mavros_msgs on path -RUN cd /bridge_ws \ - && . /opt/ros/${ROS1_DISTRO}/setup.sh \ - && . /opt/ros/${ROS2_DISTRO}/setup.sh \ - && export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH \ - && colcon build --packages-select ros1_bridge --cmake-force-configure \ - && rm -r build - -RUN mkdir /ros_ws - -COPY mavros_bridge.launch.xml /ros_ws/launch/ -COPY run_ros1.sh /ros_ws/scripts/ +RUN /opt/ros/${ROS_DISTRO}/lib/mavros/install_geographiclib_datasets.sh COPY ros_entrypoint.sh /ros_entrypoint.sh @@ -44,18 +15,14 @@ WORKDIR /ros_ws COPY mavros_setup.sh . -COPY mavros.launch /ros_ws/launch/mavros.launch +COPY mavros.launch.xml /ros_ws/launch/mavros.launch.xml COPY config/mavros_config_*.yaml / COPY config/mavros_pluginlists_*.yaml / -COPY config/bridge_config.yaml / ENV MAVROS_MOD_CONFIG_PATH="/mavros_config_mod.yaml" ENV MAVROS_CONFIG_PATH="/mavros_config_px4.yaml" ENV MAVROS_PLUGINLISTS_PATH="/mavros_pluginlists_px4.yaml" -ENV BRIDGE_MOD_CONFIG_PATH="/bridge_config_mod.yaml" -ENV BRIDGE_CONFIG_PATH="/bridge_config.yaml" - # Add custom ROS DDS configuration (force UDP always) COPY fastrtps_profiles.xml /ros_ws/ ENV FASTRTPS_DEFAULT_PROFILES_FILE /ros_ws/fastrtps_profiles.xml @@ -71,4 +38,4 @@ ENV PX4_INSTANCE_BASE=0 ENV MAVROS_TGT_FIRMWARE="px4" ENV MAVROS_GCS_URL="udp-pb://@:14550" -CMD [ "ros2", "launch", "launch/mavros_bridge.launch.xml"] +CMD [ "ros2", "launch", "launch/mavros.launch.xml"] diff --git a/system/mavros/config/mavros_config_px4.yaml b/system/mavros/config/mavros_config_px4.yaml index 53c9efc..219ce67 100644 --- a/system/mavros/config/mavros_config_px4.yaml +++ b/system/mavros/config/mavros_config_px4.yaml @@ -1,264 +1,266 @@ -# Common configuration for PX4 autopilot -# -# node: -startup_px4_usb_quirk: true - -# --- system plugins --- - -# sys_status & sys_time connection options -conn: - heartbeat_rate: 1.0 # send hertbeat rate in Hertz - timeout: 10.0 # hertbeat 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 - 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 plugins (alphabetical order) --- - -# 3dr_radio -tdr_radio: - 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 - -# dummy -# None - -# ftp -# 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 - -# 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 - -# local_position -local_position: - frame_id: "map" - tf: - send: false - frame_id: "map" - child_frame_id: "base_link" - send_fcu: false - -# param -# None, used for FCU params - -# 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 - -# 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. - -# 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 - -# setpoint_velocity -setpoint_velocity: - 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 extras plugins (same order) --- - -# adsb -# None - -# debug_value -# None - -# distance_sensor -## Currently available orientations: -# 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 - rangefinder: - id: 4 - frame_id: "rangefinder" - orientation: PITCH_270 - field_of_view: 0.5 - # rangefinder_sub: - # subscriber: true - # id: 5 - # frame_id: "rangefinder" - # orientation: PITCH_270 - # field_of_view: 0.5 - - -# image_pub -image: - 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 - -# 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} - -# mocap_pose_estimate -mocap: - # select mocap source - use_tf: false # ~mocap/tf - use_pose: true # ~mocap/pose - -# 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 - -# 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 - -# 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 - -# 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 - -# vibration -vibration: - frame_id: "base_link" - -# vim:set ts=2 sw=2 et: \ No newline at end of file +mavros_node: + ros__parameters: + # Common configuration for PX4 autopilot + # + # node: + startup_px4_usb_quirk: true + + # --- system plugins --- + + # sys_status & sys_time connection options + conn: + heartbeat_rate: 1.0 # send hertbeat rate in Hertz + timeout: 10.0 # hertbeat 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 + 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 plugins (alphabetical order) --- + + # 3dr_radio + tdr_radio: + 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 + + # dummy + # None + + # ftp + # 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 + + # 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 + + # local_position + local_position: + frame_id: "map" + tf: + send: false + frame_id: "map" + child_frame_id: "base_link" + send_fcu: false + + # param + # None, used for FCU params + + # 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 + + # 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. + + # 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 + + # setpoint_velocity + setpoint_velocity: + 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 extras plugins (same order) --- + + # adsb + # None + + # debug_value + # None + + # distance_sensor + ## Currently available orientations: + # 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 + rangefinder: + id: 4 + frame_id: "rangefinder" + orientation: PITCH_270 + field_of_view: 0.5 + # rangefinder_sub: + # subscriber: true + # id: 5 + # frame_id: "rangefinder" + # orientation: PITCH_270 + # field_of_view: 0.5 + + + # image_pub + image: + 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 + + # 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} + + # mocap_pose_estimate + mocap: + # select mocap source + use_tf: false # ~mocap/tf + use_pose: true # ~mocap/pose + + # 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 + + # 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 + + # 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 + + # 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 + + # vibration + vibration: + frame_id: "base_link" + + # vim:set ts=2 sw=2 et: \ No newline at end of file diff --git a/system/mavros/config/mavros_pluginlists_px4.yaml b/system/mavros/config/mavros_pluginlists_px4.yaml index ed4317c..b07cebc 100644 --- a/system/mavros/config/mavros_pluginlists_px4.yaml +++ b/system/mavros/config/mavros_pluginlists_px4.yaml @@ -1,34 +1,28 @@ -plugin_blacklist: [] -# common -# - safety_area -# extras -# - image_pub -# - vibration -# - distance_sensor -# - rangefinder -# - wheel_odometry - - -plugin_whitelist: -#- 'sys_*' -- altitude -- command -- distance_sensor -- ftp -- global_position -- imu -- local_position -- manual_control -# - mocap_pose_estimate -- param -- px4flow -- rc_io -- setpoint_attitude -- setpoint_position -- setpoint_raw -- setpoint_velocity -- sys_status -- sys_time -- vision_pose_estimate -# - vision_speed_estimate -# - waypoint \ No newline at end of file +mavros: + ros__parameters: + component_id: 191 + plugin_denylist: + - "*" + plugin_allowlist: + #- 'sys_*' + - altitude + - command + - distance_sensor + # - ftp + - global_position + - imu + - local_position + - manual_control + - param + - px4flow + # - rc_io + - setpoint_attitude + - setpoint_position + - setpoint_raw + - setpoint_velocity + - sys_status + - sys_time + - vision_pose_estimate + # - vision_speed_estimate + # - waypoint + diff --git a/system/mavros/mavros.launch b/system/mavros/mavros.launch deleted file mode 100644 index cc38f41..0000000 --- a/system/mavros/mavros.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/system/mavros/mavros.launch.xml b/system/mavros/mavros.launch.xml new file mode 100644 index 0000000..7a534ff --- /dev/null +++ b/system/mavros/mavros.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/mavros/mavros_bridge.launch.xml b/system/mavros/mavros_bridge.launch.xml deleted file mode 100644 index 0c6fee2..0000000 --- a/system/mavros/mavros_bridge.launch.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/system/mavros/mavros_setup.sh b/system/mavros/mavros_setup.sh index 97ff92b..c3179c5 100755 --- a/system/mavros/mavros_setup.sh +++ b/system/mavros/mavros_setup.sh @@ -118,18 +118,18 @@ else fi # Modify ros 1bridge for topic/service paths -BRIDGE_MOD_CONFIG_PATH=${BRIDGE_MOD_CONFIG_PATH:-"/bridge_config_mod.yaml"} -BRIDGE_CONFIG_PATH=${BRIDGE_CONFIG_PATH:-"/bridge_config.yaml"} -if [ ! -f "$BRIDGE_MOD_CONFIG_PATH" ]; then - echo "Modified Bridge Config for $VEHICLE_NAMESPACE does not exist at $BRIDGE_MOD_CONFIG_PATH. Generating specialised configuration for launch." - # This line: - # -E allow groupings defined by parenthesis to be used in replace by \1 \2 - # /\"map\"/! selects lines which do not contain "map" or "earth" (Global frame) - # The rest then adds $VEHICLE_NAMESPACE in as a prefix to whatever the frame_id was - # This captures both frame_id: and child_frame_id - # This then saves the output into $PX4_MOD_CONFIG_PATH - sed -E "s/(topic: )(.+)/\1\/$VEHICLE_NAMESPACE\/\2/g" $BRIDGE_CONFIG_PATH > $BRIDGE_MOD_CONFIG_PATH - sed -E -i "s/(service: )(.+)/\1\/$VEHICLE_NAMESPACE\/\2/g" $BRIDGE_MOD_CONFIG_PATH -else - echo "Modified Bridge Config for $VEHICLE_NAMESPACE already exists at $BRIDGE_MOD_CONFIG_PATH. Using for launch" -fi +# BRIDGE_MOD_CONFIG_PATH=${BRIDGE_MOD_CONFIG_PATH:-"/bridge_config_mod.yaml"} +# BRIDGE_CONFIG_PATH=${BRIDGE_CONFIG_PATH:-"/bridge_config.yaml"} +# if [ ! -f "$BRIDGE_MOD_CONFIG_PATH" ]; then +# echo "Modified Bridge Config for $VEHICLE_NAMESPACE does not exist at $BRIDGE_MOD_CONFIG_PATH. Generating specialised configuration for launch." +# # This line: +# # -E allow groupings defined by parenthesis to be used in replace by \1 \2 +# # /\"map\"/! selects lines which do not contain "map" or "earth" (Global frame) +# # The rest then adds $VEHICLE_NAMESPACE in as a prefix to whatever the frame_id was +# # This captures both frame_id: and child_frame_id +# # This then saves the output into $PX4_MOD_CONFIG_PATH +# sed -E "s/(topic: )(.+)/\1\/$VEHICLE_NAMESPACE\/\2/g" $BRIDGE_CONFIG_PATH > $BRIDGE_MOD_CONFIG_PATH +# sed -E -i "s/(service: )(.+)/\1\/$VEHICLE_NAMESPACE\/\2/g" $BRIDGE_MOD_CONFIG_PATH +# else +# echo "Modified Bridge Config for $VEHICLE_NAMESPACE already exists at $BRIDGE_MOD_CONFIG_PATH. Using for launch" +# fi diff --git a/system/mavros/ros_entrypoint.sh b/system/mavros/ros_entrypoint.sh index 4299c8d..064699d 100755 --- a/system/mavros/ros_entrypoint.sh +++ b/system/mavros/ros_entrypoint.sh @@ -1,18 +1,8 @@ #!/bin/bash set -e -# unsetting ROS_DISTRO to silence ROS_DISTRO override warning -unset ROS_DISTRO -# setup ros1 environment -source "/opt/ros/$ROS1_DISTRO/setup.bash" - -# unsetting ROS_DISTRO to silence ROS_DISTRO override warning -unset ROS_DISTRO # setup ros2 environment -source "/opt/ros/$ROS2_DISTRO/setup.bash" - -# Source the from-source ros1_bridge workspace -source "/bridge_ws/install/setup.bash" +source "/opt/ros/$ROS_DISTRO/setup.bash" # Source the mavros_setup for any user defined edits to the environment source "/ros_ws/mavros_setup.sh" diff --git a/system/mavros/run_ros1.sh b/system/mavros/run_ros1.sh deleted file mode 100755 index c5a7060..0000000 --- a/system/mavros/run_ros1.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash - -source /opt/ros/${ROS1_DISTRO}/setup.bash - -exec "$@" \ No newline at end of file From 7aefb8558de8b90715f1e4b6b6b2d10d132f5eb5 Mon Sep 17 00:00:00 2001 From: mickey li Date: Fri, 10 Dec 2021 11:42:23 +0000 Subject: [PATCH 02/25] Added basic configuration for px4 --- system/mavros/config/bridge_config.yaml | 181 ----------- .../mavros/config/mavros_config_px4 copy.yaml | 266 +++++++++++++++ system/mavros/config/mavros_config_px4.yaml | 306 ++++-------------- .../mavros/config/mavros_pluginlists_px4.yaml | 3 +- system/mavros/mavros.launch.xml | 2 +- 5 files changed, 324 insertions(+), 434 deletions(-) delete mode 100644 system/mavros/config/bridge_config.yaml create mode 100644 system/mavros/config/mavros_config_px4 copy.yaml diff --git a/system/mavros/config/bridge_config.yaml b/system/mavros/config/bridge_config.yaml deleted file mode 100644 index e9f4e79..0000000 --- a/system/mavros/config/bridge_config.yaml +++ /dev/null @@ -1,181 +0,0 @@ -# This Config details the list of topics we wish to forward from ros1 to ros2 -# Note that this file will be copied with topics specified for $VEHICLE_NAMESPACE -# i.e. /$VEHICLE_NAMESPACE/ -# MAVROS: http://wiki.ros.org/mavros -# MAVROS EXTRAS: http://wiki.ros.org/mavros_extras - -# These are bidrectional from ROS1 to ROS2 -# The Type must be a ROS2 type -topics: - - queue_size: 1 - topic: mavros/state - type: mavros_msgs/msg/State - - queue_size: 1 - topic: mavros/battery - type: sensor_msgs/msg/BatteryState - - queue_size: 1 - topic: mavlink/to - type: mavros_msgs/msg/Mavlink - - queue_size: 1 - topic: mavlink/from - type: mavros_msgs/msg/Mavlink - # 6.6 Global position - #- queue_size: 1 # TODO set up custom message to use nav_msgs/Odom - # topic: mavros/global_position/local - # type: geometry_msgs/msg/PoseWithCovarianceStamped - - queue_size: 1 - topic: mavros/global_position/global - type: sensor_msgs/msg/NavSatFix - - # 6.8 Local Position - - queue_size: 5 - topic: mavros/local_position/pose - type: geometry_msgs/msg/PoseStamped - - queue_size: 5 - topic: mavros/global_position/velocity - type: geometry_msgs/msg/TwistStamped - - # 6.9 Manual Control - - queue_size: 1 - topic: mavros/manual_control/send - type: mavros_msgs/msg/ManualControl - - queue_size: 1 - topic: mavros/manual_control/control - type: mavros_msgs/msg/ManualControl - - # 6.12 Safety Area - - queue_size: 1 - topic: mavros/safety_area - type: geometry_msgs/msg/PolygonStamped - - # 6.13 Setpoint Accel - - queue_size: 1 - topic: mavros/setpoint_accel/accel - type: geometry_msgs/msg/Vector3Stamped - - # 6.14 Setpoint Attitude - - queue_size: 1 - topic: mavros/setpoint_attitude/cmd_vel - type: geometry_msgs/msg/TwistStamped - - queue_size: 1 - topic: mavros/setpoint_attitude/attitude - type: geometry_msgs/msg/PoseStamped - - queue_size: 1 - topic: mavros/setpoint_attitude/thrust - type: mavros_msgs/msg/Thrust - - # 6.15 Setpoint Position - - queue_size: 1 - topic: mavros/setpoint_position/global - type: geographic_msgs/msg/GeoPoseStamped - - queue_size: 1 - topic: mavros/setpoint_position/local - type: geometry_msgs/msg/PoseStamped - - queue_size: 1 - topic: mavros/setpoint_position/global_to_local - type: geographic_msgs/msg/GeoPoseStamped - - # 6.16 Setpoint Raw - - queue_size: 1 - topic: mavros/setpoint_raw/local - type: mavros_msgs/msg/PositionTarget - - queue_size: 1 - topic: mavros/setpoint_raw/global - type: mavros_msgs/msg/GlobalPositionTarget - - queue_size: 1 - topic: mavros/setpoint_raw/attitude - type: mavros_msgs/msg/AttitudeTarget - - # 6.17 Setpoint Velocity - - queue_size: 1 - topic: mavros/setpoint_velocity/cmd_vel_unstamped - type: geometry_msgs/msg/Twist - - # 6.21 Waypoints - - queue_size: 1 - topic: mavros/mission/reached - type: mavros_msgs/msg/WaypointReached - - queue_size: 1 - topic: mavros/mission/waypoints - type: mavros_msgs/msg/WaypointList - - # Mavros extras 4.2 distance sensor (matches mavros_config.yaml) - - queue_size: 1 - topic: mavros/distance_sensor/hrlv_ez4_sonar - type: sensor_msgs/msg/Range - - queue_size: 1 - topic: mavros/distance_sensor/lidarlite_laser - type: sensor_msgs/msg/Range - - queue_size: 1 - topic: mavros/distance_sensor/rangefinder - type: sensor_msgs/msg/Range - - # Mavros extras image pub - - queue_size: 1 - topic: mavros/image/camera_image - type: sensor_msgs/msg/Image - - # Mavros extras 4.5 px4flow - - queue_size: 1 - topic: mavros/px4flow/raw/optical_flow_rad - type: mavros_msgs/msg/OpticalFlowRad - - queue_size: 1 - topic: mavros/px4flow/ground_distance - type: sensor_msgs/msg/Range - - queue_size: 1 - topic: mavros/distance_sensor/temperature - type: sensor_msgs/msg/Temperature - - # Mavros Extras 4.7 vision pose estimate - - queue_size: 1 - topic: mavros/vision_pose/pose - type: geometry_msgs/msg/PoseStamped - - queue_size: 1 - topic: mavros/vision_pose/pose_cov - type: geometry_msgs/msg/PoseWithCovarianceStamped - - # Mavros Extras 4.8 vision speed estimate - - queue_size: 1 - topic: mavros/vision_speed/speed_vector - type: geometry_msgs/msg/Vector3Stamped - - queue_size: 1 - topic: mavros/vision_speed/speed_twist - type: geometry_msgs/msg/TwistStamped - -# service requests going from 2 to 1 -# server on ROS1 side, client on ROS2 side -# Types must be *ROS1* Types -services_2to1: - # 6.4 Command - - service: mavros/cmd/command - type: mavros_msgs/CommandLong - - service: mavros/cmd/command_int - type: mavros_msgs/CommandInt - - service: mavros/cmd/arming - type: mavros_msgs/CommandBool - - service: mavros/cmd/set_home - type: mavros_msgs/CommandHome - - service: mavros/cmd/takeoff - type: mavros_msgs/CommandTOL - - service: mavros/cmd/land - type: mavros_msgs/CommandTOL - - service: mavros/cmd/trigger_control - type: mavros_msgs/CommandTriggerControl - - # 6.18 Sys Status - - service: mavros/set_stream_rate - type: mavros_msgs/StreamRate - - service: mavros/set_mode - type: mavros_msgs/SetMode - - # 6.21 Waypoints - - service: mavros/mission/pull - type: mavros_msgs/WaypointPull - - service: mavros/mission/push - type: mavros_msgs/WaypointPush - - service: mavros/mission/clear - type: mavros_msgs/WaypointClear - - service: mavros/mission/set_current - type: mavros_msgs/WaypointSetCurrent - -services_1to2: [] diff --git a/system/mavros/config/mavros_config_px4 copy.yaml b/system/mavros/config/mavros_config_px4 copy.yaml new file mode 100644 index 0000000..219ce67 --- /dev/null +++ b/system/mavros/config/mavros_config_px4 copy.yaml @@ -0,0 +1,266 @@ +mavros_node: + ros__parameters: + # Common configuration for PX4 autopilot + # + # node: + startup_px4_usb_quirk: true + + # --- system plugins --- + + # sys_status & sys_time connection options + conn: + heartbeat_rate: 1.0 # send hertbeat rate in Hertz + timeout: 10.0 # hertbeat 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 + 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 plugins (alphabetical order) --- + + # 3dr_radio + tdr_radio: + 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 + + # dummy + # None + + # ftp + # 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 + + # 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 + + # local_position + local_position: + frame_id: "map" + tf: + send: false + frame_id: "map" + child_frame_id: "base_link" + send_fcu: false + + # param + # None, used for FCU params + + # 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 + + # 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. + + # 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 + + # setpoint_velocity + setpoint_velocity: + 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 extras plugins (same order) --- + + # adsb + # None + + # debug_value + # None + + # distance_sensor + ## Currently available orientations: + # 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 + rangefinder: + id: 4 + frame_id: "rangefinder" + orientation: PITCH_270 + field_of_view: 0.5 + # rangefinder_sub: + # subscriber: true + # id: 5 + # frame_id: "rangefinder" + # orientation: PITCH_270 + # field_of_view: 0.5 + + + # image_pub + image: + 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 + + # 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} + + # mocap_pose_estimate + mocap: + # select mocap source + use_tf: false # ~mocap/tf + use_pose: true # ~mocap/pose + + # 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 + + # 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 + + # 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 + + # 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 + + # vibration + vibration: + frame_id: "base_link" + + # vim:set ts=2 sw=2 et: \ No newline at end of file diff --git a/system/mavros/config/mavros_config_px4.yaml b/system/mavros/config/mavros_config_px4.yaml index 219ce67..4d044dc 100644 --- a/system/mavros/config/mavros_config_px4.yaml +++ b/system/mavros/config/mavros_config_px4.yaml @@ -1,266 +1,70 @@ -mavros_node: +mavros/altitude: ros__parameters: - # Common configuration for PX4 autopilot - # - # node: - startup_px4_usb_quirk: true + frame_id: map + use_sim_time: false - # --- system plugins --- - - # sys_status & sys_time connection options - conn: - heartbeat_rate: 1.0 # send hertbeat rate in Hertz - timeout: 10.0 # hertbeat 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 - 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 plugins (alphabetical order) --- - - # 3dr_radio - tdr_radio: - 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 - - # dummy - # None - - # ftp - # 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/cmd: + ros__parameters: + command_ack_timeout: 5.0 + use_comp_id_system_control: false + use_sim_time: false - # imu_pub - imu: +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 + angular_velocity_stdev: 0.0003490659 orientation_stdev: 1.0 magnetic_stdev: 0.0 - # local_position - local_position: - frame_id: "map" - tf: - send: false - frame_id: "map" - child_frame_id: "base_link" - send_fcu: false - - # param - # None, used for FCU params - - # 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 - - # 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. - - # 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 - - # setpoint_velocity - setpoint_velocity: - 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 extras plugins (same order) --- - - # adsb - # None - - # debug_value - # None - - # distance_sensor - ## Currently available orientations: - # 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 - rangefinder: - id: 4 - frame_id: "rangefinder" - orientation: PITCH_270 - field_of_view: 0.5 - # rangefinder_sub: - # subscriber: true - # id: 5 - # frame_id: "rangefinder" - # orientation: PITCH_270 - # field_of_view: 0.5 - - - # image_pub - image: - 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 - - # 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} - - # mocap_pose_estimate - mocap: - # select mocap source - use_tf: false # ~mocap/tf - use_pose: true # ~mocap/pose +mavros/global_position: + ros__parameters: + child_frame_id: base_link + frame_id: map + gps_uere: 1.0 + rot_covariance: 99999.0 + tf: + child_frame_id: base_link + frame_id: map + global_frame_id: earth + send: false + use_relative_alt: true + use_sim_time: false + +mavros/local_position: + ros__parameters: + frame_id: map + tf: + child_frame_id: base_link + frame_id: map + send: false + use_sim_time: false + +mavros/setpoint_attitude: + ros__parameters: + reverse_thrust: false + use_quaternion: false + use_sim_time: 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/setpoint_position: + ros__parameters: + mav_frame: LOCAL_NED + use_sim_time: false - # 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/setpoint_raw: + ros__parameters: + thrust_scaling: 1.0 + use_sim_time: false - # 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/setpoint_velocity: + ros__parameters: + mav_frame: LOCAL_NED + use_sim_time: false - # 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/setpoint_accel: + ros__parameters: + send_force: false + use_sim_time: false - # vibration - vibration: - frame_id: "base_link" - # vim:set ts=2 sw=2 et: \ No newline at end of file diff --git a/system/mavros/config/mavros_pluginlists_px4.yaml b/system/mavros/config/mavros_pluginlists_px4.yaml index b07cebc..ba08210 100644 --- a/system/mavros/config/mavros_pluginlists_px4.yaml +++ b/system/mavros/config/mavros_pluginlists_px4.yaml @@ -4,7 +4,7 @@ mavros: plugin_denylist: - "*" plugin_allowlist: - #- 'sys_*' + - 'sys_*' - altitude - command - distance_sensor @@ -16,6 +16,7 @@ mavros: - param - px4flow # - rc_io + - setpoint_accel - setpoint_attitude - setpoint_position - setpoint_raw diff --git a/system/mavros/mavros.launch.xml b/system/mavros/mavros.launch.xml index 7a534ff..1f962da 100644 --- a/system/mavros/mavros.launch.xml +++ b/system/mavros/mavros.launch.xml @@ -21,7 +21,7 @@ - + From f55e06f38df77b1dc4302474872222e05014856a Mon Sep 17 00:00:00 2001 From: mickey li Date: Fri, 10 Dec 2021 12:13:56 +0000 Subject: [PATCH 03/25] Added distance sensor config --- system/mavros/Dockerfile | 2 ++ system/mavros/config/mavros_config_px4.yaml | 16 +++++++++++++++- system/mavros/config/mavros_pluginlists_px4.yaml | 2 +- 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/system/mavros/Dockerfile b/system/mavros/Dockerfile index f02483f..7a42377 100644 --- a/system/mavros/Dockerfile +++ b/system/mavros/Dockerfile @@ -4,6 +4,8 @@ FROM ros:foxy-ros-base-focal RUN apt-get update \ && apt-get install --no-install-recommends -y \ ros-${ROS_DISTRO}-mavros \ + ros-${ROS_DISTRO}-mavros-extras \ + ros-${ROS_DISTRO}-mavros-msgs \ && rm -rf /var/lib/apt/lists/ # Part of MAVROS setup diff --git a/system/mavros/config/mavros_config_px4.yaml b/system/mavros/config/mavros_config_px4.yaml index 4d044dc..89f3488 100644 --- a/system/mavros/config/mavros_config_px4.yaml +++ b/system/mavros/config/mavros_config_px4.yaml @@ -62,9 +62,23 @@ mavros/setpoint_velocity: mav_frame: LOCAL_NED use_sim_time: false -/mavros/setpoint_accel: +mavros/setpoint_accel: ros__parameters: send_force: false use_sim_time: false +mavros/distance_sensor: + ros__parameters: + base_frame_id: base_link + config: "{rangefinder: { + subscriber: true, + id: 0, + frame_id: rangefinder, + orientation: PITCH_270, + horizontal_fov_ratio: 0.5, + vertical_fov_ratio: 0.5} + }" + use_sim_time: false + + diff --git a/system/mavros/config/mavros_pluginlists_px4.yaml b/system/mavros/config/mavros_pluginlists_px4.yaml index ba08210..d49418b 100644 --- a/system/mavros/config/mavros_pluginlists_px4.yaml +++ b/system/mavros/config/mavros_pluginlists_px4.yaml @@ -13,7 +13,7 @@ mavros: - imu - local_position - manual_control - - param + # - param - px4flow # - rc_io - setpoint_accel From 1b0fd226ebd3bb22a5b425f5084ec5736849536e Mon Sep 17 00:00:00 2001 From: mickey li Date: Fri, 10 Dec 2021 12:15:54 +0000 Subject: [PATCH 04/25] Added distance sensor config --- system/mavros/config/mavros_pluginlists_px4.yaml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/system/mavros/config/mavros_pluginlists_px4.yaml b/system/mavros/config/mavros_pluginlists_px4.yaml index d49418b..d4fc4e4 100644 --- a/system/mavros/config/mavros_pluginlists_px4.yaml +++ b/system/mavros/config/mavros_pluginlists_px4.yaml @@ -23,7 +23,5 @@ mavros: - setpoint_velocity - sys_status - sys_time - - vision_pose_estimate - # - vision_speed_estimate - # - waypoint + - vision_pose From 8fe19201fd7f19cf743f7cdd30d2c820a25f55be Mon Sep 17 00:00:00 2001 From: mickey li Date: Fri, 10 Dec 2021 12:24:09 +0000 Subject: [PATCH 05/25] Added extra plugins, needs testing --- system/mavros/config/mavros_config_px4.yaml | 11 +++++++++++ system/mavros/config/mavros_pluginlists_px4.yaml | 2 ++ system/mavros/mavros_setup.sh | 2 -- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/system/mavros/config/mavros_config_px4.yaml b/system/mavros/config/mavros_config_px4.yaml index 89f3488..8c7ebc3 100644 --- a/system/mavros/config/mavros_config_px4.yaml +++ b/system/mavros/config/mavros_config_px4.yaml @@ -81,4 +81,15 @@ mavros/distance_sensor: use_sim_time: false +mavros/vision_pose: + ros__parameters: + tf/child_frame_id: vision_estimate + tf/frame_id: map + tf/listen: false + tf/rate_limit: 10.0 + use_sim_time: false +mavros/vibration: + ros__parameters: + frame_id: base_link + use_sim_time: false diff --git a/system/mavros/config/mavros_pluginlists_px4.yaml b/system/mavros/config/mavros_pluginlists_px4.yaml index d4fc4e4..7ec5484 100644 --- a/system/mavros/config/mavros_pluginlists_px4.yaml +++ b/system/mavros/config/mavros_pluginlists_px4.yaml @@ -24,4 +24,6 @@ mavros: - sys_status - sys_time - vision_pose + - onboard_computer_status + - vibration diff --git a/system/mavros/mavros_setup.sh b/system/mavros/mavros_setup.sh index c3179c5..105d918 100755 --- a/system/mavros/mavros_setup.sh +++ b/system/mavros/mavros_setup.sh @@ -111,8 +111,6 @@ if [ ! -f "$MAVROS_MOD_CONFIG_PATH" ]; then # This then saves the output into $PX4_MOD_CONFIG_PATH sed -E "/\"map\"|\"earth\"/! s/(frame_id: )\"(.+)\"/\1\"$VEHICLE_NAMESPACE\/\2\"/g" $MAVROS_CONFIG_PATH > $MAVROS_MOD_CONFIG_PATH - # Also fix outliers such as the odometry id_des parameter - sed -i -E "/\"map\"|\"earth\"/! s/(id_des: )\"(.+)\"/\1\"$VEHICLE_NAMESPACE\/\2\"/g" $MAVROS_MOD_CONFIG_PATH else echo "Modified Mavros Config for $VEHICLE_NAMESPACE already exists at $MAVROS_MOD_CONFIG_PATH. Using for launch" fi From bbdb5f79dba78bb2cae4fc0b541bef092a75224f Mon Sep 17 00:00:00 2001 From: mickey li Date: Mon, 13 Dec 2021 14:20:19 +0000 Subject: [PATCH 06/25] Testing Different Configurations WIP --- system/mavros/Dockerfile | 1 + system/mavros/mavros.launch.xml | 10 +++++++--- system/mavros/mavros_setup.sh | 29 ++++++++++++----------------- 3 files changed, 20 insertions(+), 20 deletions(-) diff --git a/system/mavros/Dockerfile b/system/mavros/Dockerfile index 7a42377..bf74308 100644 --- a/system/mavros/Dockerfile +++ b/system/mavros/Dockerfile @@ -23,6 +23,7 @@ COPY config/mavros_pluginlists_*.yaml / ENV MAVROS_MOD_CONFIG_PATH="/mavros_config_mod.yaml" ENV MAVROS_CONFIG_PATH="/mavros_config_px4.yaml" +ENV MAVROS_MOD_PLUGINLISTS_PATH="/mavros_pluginlists_mod.yaml" ENV MAVROS_PLUGINLISTS_PATH="/mavros_pluginlists_px4.yaml" # Add custom ROS DDS configuration (force UDP always) diff --git a/system/mavros/mavros.launch.xml b/system/mavros/mavros.launch.xml index 1f962da..bb60a8d 100644 --- a/system/mavros/mavros.launch.xml +++ b/system/mavros/mavros.launch.xml @@ -7,15 +7,15 @@ - + - + exec="mavros_node" + namespace="$(var vehicle_namespace)"> @@ -23,6 +23,10 @@ + + + + diff --git a/system/mavros/mavros_setup.sh b/system/mavros/mavros_setup.sh index 105d918..07ffd4c 100755 --- a/system/mavros/mavros_setup.sh +++ b/system/mavros/mavros_setup.sh @@ -110,24 +110,19 @@ if [ ! -f "$MAVROS_MOD_CONFIG_PATH" ]; then # This captures both frame_id: and child_frame_id # This then saves the output into $PX4_MOD_CONFIG_PATH sed -E "/\"map\"|\"earth\"/! s/(frame_id: )\"(.+)\"/\1\"$VEHICLE_NAMESPACE\/\2\"/g" $MAVROS_CONFIG_PATH > $MAVROS_MOD_CONFIG_PATH - + sed -i "s#mavros#/$VEHICLE_NAMESPACE/mavros#g" $MAVROS_MOD_CONFIG_PATH else echo "Modified Mavros Config for $VEHICLE_NAMESPACE already exists at $MAVROS_MOD_CONFIG_PATH. Using for launch" fi -# Modify ros 1bridge for topic/service paths -# BRIDGE_MOD_CONFIG_PATH=${BRIDGE_MOD_CONFIG_PATH:-"/bridge_config_mod.yaml"} -# BRIDGE_CONFIG_PATH=${BRIDGE_CONFIG_PATH:-"/bridge_config.yaml"} -# if [ ! -f "$BRIDGE_MOD_CONFIG_PATH" ]; then -# echo "Modified Bridge Config for $VEHICLE_NAMESPACE does not exist at $BRIDGE_MOD_CONFIG_PATH. Generating specialised configuration for launch." -# # This line: -# # -E allow groupings defined by parenthesis to be used in replace by \1 \2 -# # /\"map\"/! selects lines which do not contain "map" or "earth" (Global frame) -# # The rest then adds $VEHICLE_NAMESPACE in as a prefix to whatever the frame_id was -# # This captures both frame_id: and child_frame_id -# # This then saves the output into $PX4_MOD_CONFIG_PATH -# sed -E "s/(topic: )(.+)/\1\/$VEHICLE_NAMESPACE\/\2/g" $BRIDGE_CONFIG_PATH > $BRIDGE_MOD_CONFIG_PATH -# sed -E -i "s/(service: )(.+)/\1\/$VEHICLE_NAMESPACE\/\2/g" $BRIDGE_MOD_CONFIG_PATH -# else -# echo "Modified Bridge Config for $VEHICLE_NAMESPACE already exists at $BRIDGE_MOD_CONFIG_PATH. Using for launch" -# fi +# Modify mavros PX4 config for frame ids +MAVROS_MOD_PLUGINLISTS_PATH=${MAVROS_MOD_PLUGINLISTS_PATH:-"/mavros_pluginlists_mod.yaml"} +MAVROS_PLUGINLISTS_PATH=${MAVROS_PLUGINLISTS_PATH:-"/mavros_pluginlists_px4.yaml"} +if [ ! -f "$MAVROS_MOD_PLUGINLISTS_PATH" ]; then + echo "Modified Mavros Pluginlist for $VEHICLE_NAMESPACE does not exist at $MAVROS_PLUGINLISTS_PATH. Generating specialised configuration for launch." + sed -E "s#mavros#/$VEHICLE_NAMESPACE/mavros#g" $MAVROS_PLUGINLISTS_PATH > $MAVROS_MOD_PLUGINLISTS_PATH +else + echo "Modified Mavros Pluginlist for $VEHICLE_NAMESPACE already exists at $MAVROS_MOD_PLUGINLISTS_PATH. Using for launch" +fi + + From 3947e4838a94b2e0df878fc024741b5c8f050123 Mon Sep 17 00:00:00 2001 From: Rob Clarke Date: Fri, 4 Mar 2022 16:47:11 +0000 Subject: [PATCH 07/25] Add mavros2 source build --- .gitmodules | 4 ++ buildtools/docker-bake.hcl | 12 ++++++ system/Makefile | 3 ++ system/mavros/Dockerfile | 45 +++++++++++++++----- system/mavros/core.Dockerfile | 74 +++++++++++++++++++++++++++++++++ system/mavros/ros_entrypoint.sh | 2 + 6 files changed, 129 insertions(+), 11 deletions(-) create mode 100644 system/mavros/core.Dockerfile diff --git a/.gitmodules b/.gitmodules index a0d366e..201d97f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,7 @@ [submodule "system/vicon/vicon_udp"] path = system/vicon/vicon_udp url = https://github.com/UoBFlightLab/vicon_udp.git +[submodule "system/mavros/mavros"] + path = system/mavros/mavros + url = ssh://github.com/mavlink/mavros + branch = ros2 diff --git a/buildtools/docker-bake.hcl b/buildtools/docker-bake.hcl index bd2ae9d..42d2fec 100644 --- a/buildtools/docker-bake.hcl +++ b/buildtools/docker-bake.hcl @@ -81,6 +81,18 @@ target "starling-mavros" { cache-from = [ notequal("",BAKE_CACHEFROM_NAME) ? "${BAKE_CACHEFROM_REGISTRY}uobflightlabstarling/starling-mavros:${BAKE_CACHEFROM_NAME}" : "" ] } +target "starling-mavros2" { + context = "system/mavros" + dockerfile = "core.Dockerfile" + tags = [ + "${BAKE_REGISTRY}uobflightlabstarling/starling-mavros2:${BAKE_VERSION}", + notequal("",BAKE_RELEASENAME) ? "${BAKE_REGISTRY}uobflightlabstarling/starling-mavros2:${BAKE_RELEASENAME}": "", + ] + platforms = ["linux/amd64", "linux/arm64"] + cache-to = [ notequal("",BAKE_CACHETO_NAME) ? "${BAKE_CACHETO_REGISTRY}uobflightlabstarling/starling-mavros2:${BAKE_CACHETO_NAME}" : "" ] + cache-from = [ notequal("",BAKE_CACHEFROM_NAME) ? "${BAKE_CACHEFROM_REGISTRY}uobflightlabstarling/starling-mavros2:${BAKE_CACHEFROM_NAME}" : "" ] +} + target "mavp2p" { context = "system/mavp2p" tags = [ diff --git a/system/Makefile b/system/Makefile index 0ec0622..210f2a6 100644 --- a/system/Makefile +++ b/system/Makefile @@ -11,6 +11,9 @@ controller-base: mavros: $(BAKE) starling-mavros +mavros2: + $(BAKE) starling-mavros2 + mavros_all_arch: cd $(MAKEFILE_DIR)/.. && docker buildx bake --builder default --load -f $(BAKE_SCRIPT) diff --git a/system/mavros/Dockerfile b/system/mavros/Dockerfile index bf74308..8281425 100644 --- a/system/mavros/Dockerfile +++ b/system/mavros/Dockerfile @@ -1,20 +1,43 @@ -FROM ros:foxy-ros-base-focal +FROM ros:foxy-ros-base-focal AS builder -# Install mavros -RUN apt-get update \ - && apt-get install --no-install-recommends -y \ - ros-${ROS_DISTRO}-mavros \ - ros-${ROS_DISTRO}-mavros-extras \ - ros-${ROS_DISTRO}-mavros-msgs \ - && rm -rf /var/lib/apt/lists/ +WORKDIR /ros_ws -# Part of MAVROS setup -RUN /opt/ros/${ROS_DISTRO}/lib/mavros/install_geographiclib_datasets.sh +# Install mavros dependencies +COPY mavros/libmavconn/package.xml /ros_ws/src/mavros/libmavconn/package.xml +COPY mavros/mavros/package.xml /ros_ws/src/mavros/mavros/package.xml +COPY mavros/mavros_msgs/package.xml /ros_ws/src/mavros/mavros_msgs/package.xml +COPY mavros/mavros_extras/package.xml /ros_ws/src/mavros/mavros_extras/package.xml -COPY ros_entrypoint.sh /ros_entrypoint.sh +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && apt-get update \ + && rosdep install -y --from-paths src --ignore-src . + +# Install mavros itself +COPY mavros /ros_ws/src/mavros + +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && colcon build + +# Start the main image build +FROM ros:foxy-ros-base-focal AS image WORKDIR /ros_ws +# Copy over the build artifacts +COPY --from=builder /ros_ws/install /ros_ws/install + +# Install exec dependencies +RUN rm install/COLCON_IGNORE \ + && . /opt/ros/${ROS_DISTRO}/setup.sh \ + && apt-get update \ + && rosdep install -t exec -y --from-paths install \ + && rm -rf /var/lib/apt/lists/* + +# Part of MAVROS setup +RUN /ros_ws/install/mavros/lib/mavros/install_geographiclib_datasets.sh + +COPY ros_entrypoint.sh /ros_entrypoint.sh + COPY mavros_setup.sh . COPY mavros.launch.xml /ros_ws/launch/mavros.launch.xml diff --git a/system/mavros/core.Dockerfile b/system/mavros/core.Dockerfile new file mode 100644 index 0000000..08e9ff4 --- /dev/null +++ b/system/mavros/core.Dockerfile @@ -0,0 +1,74 @@ +FROM ros:foxy-ros-base-focal AS builder + +WORKDIR /ros_ws + +# Install mavros dependencies +COPY mavros/libmavconn/package.xml /ros_ws/src/mavros/libmavconn/package.xml +COPY mavros/mavros/package.xml /ros_ws/src/mavros/mavros/package.xml +COPY mavros/mavros_msgs/package.xml /ros_ws/src/mavros/mavros_msgs/package.xml +COPY mavros/mavros_extras/package.xml /ros_ws/src/mavros/mavros_extras/package.xml + +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && apt-get update \ + && rosdep install -y --from-paths src --ignore-src . + +# Install mavros itself +COPY mavros /ros_ws/src/mavros + +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && colcon build + +RUN /ros_ws/install/mavros/lib/mavros/install_geographiclib_datasets.sh + +# Start the main image build +FROM ros:foxy-ros-core-focal AS image + +WORKDIR /ros_ws + +# Copy over the build artifacts +COPY --from=builder /ros_ws/install /ros_ws/install + +# Install exec dependencies +RUN rm install/COLCON_IGNORE \ + && apt-get update \ + && apt-get install -y --no-install-recommends python3-rosdep \ + && rosdep init \ + && rosdep update --rosdistro $ROS_DISTRO \ + && . /opt/ros/${ROS_DISTRO}/setup.sh \ + && rosdep install -t exec -y --from-paths install --ignore-src \ + && apt-get remove -y python3-rosdep \ + && SUDO_FORCE_REMOVE=yes apt-get autoremove -y \ + && rm -rf /var/lib/apt/lists/* + +# Part of MAVROS setup +COPY --from=builder /usr/share/GeographicLib /usr/share/GeographicLib + +COPY ros_entrypoint.sh /ros_entrypoint.sh + +COPY mavros_setup.sh . + +COPY mavros.launch.xml /ros_ws/launch/mavros.launch.xml +COPY config/mavros_config_*.yaml / +COPY config/mavros_pluginlists_*.yaml / + +ENV MAVROS_MOD_CONFIG_PATH="/mavros_config_mod.yaml" +ENV MAVROS_CONFIG_PATH="/mavros_config_px4.yaml" +ENV MAVROS_MOD_PLUGINLISTS_PATH="/mavros_pluginlists_mod.yaml" +ENV MAVROS_PLUGINLISTS_PATH="/mavros_pluginlists_px4.yaml" + +# Add custom ROS DDS configuration (force UDP always) +COPY fastrtps_profiles.xml /ros_ws/ +ENV FASTRTPS_DEFAULT_PROFILES_FILE /ros_ws/fastrtps_profiles.xml + +# TODO: Rename to FCU_PROTOCOL +ENV MAVROS_FCU_CONN="udp" +# TODO: Rename to FCU_HOST +ENV MAVROS_FCU_IP="127.0.0.1" +# TODO: Rename to PORT_BASE +ENV MAVROS_FCU_UDP_BASE="14830" +ENV MAVROS_TGT_SYSTEM="auto" +ENV PX4_INSTANCE_BASE=0 +ENV MAVROS_TGT_FIRMWARE="px4" +ENV MAVROS_GCS_URL="udp-pb://@:14550" + +CMD [ "ros2", "launch", "launch/mavros.launch.xml"] diff --git a/system/mavros/ros_entrypoint.sh b/system/mavros/ros_entrypoint.sh index 064699d..ad3caa4 100755 --- a/system/mavros/ros_entrypoint.sh +++ b/system/mavros/ros_entrypoint.sh @@ -4,6 +4,8 @@ set -e # setup ros2 environment source "/opt/ros/$ROS_DISTRO/setup.bash" +source "/ros_ws/install/setup.bash" + # Source the mavros_setup for any user defined edits to the environment source "/ros_ws/mavros_setup.sh" From 8e49f4f1ba0d3ad5eb12180c76756ac31985a4a4 Mon Sep 17 00:00:00 2001 From: Rob Clarke Date: Fri, 4 Mar 2022 16:48:20 +0000 Subject: [PATCH 08/25] Move to mavros2 folder --- system/{mavros => mavros2}/Dockerfile | 0 system/{mavros => mavros2}/README.md | 0 system/{mavros => mavros2}/config/mavros_config_ap.yaml | 0 system/{mavros => mavros2}/config/mavros_config_px4 copy.yaml | 0 system/{mavros => mavros2}/config/mavros_config_px4.yaml | 0 system/{mavros => mavros2}/config/mavros_pluginlists_ap.yaml | 0 system/{mavros => mavros2}/config/mavros_pluginlists_px4.yaml | 0 system/{mavros => mavros2}/core.Dockerfile | 0 system/{mavros => mavros2}/fastrtps_profiles.xml | 0 system/{mavros => mavros2}/mavros.launch.xml | 0 system/{mavros => mavros2}/mavros_setup.sh | 0 system/{mavros => mavros2}/ros_entrypoint.sh | 0 12 files changed, 0 insertions(+), 0 deletions(-) rename system/{mavros => mavros2}/Dockerfile (100%) rename system/{mavros => mavros2}/README.md (100%) rename system/{mavros => mavros2}/config/mavros_config_ap.yaml (100%) rename system/{mavros => mavros2}/config/mavros_config_px4 copy.yaml (100%) rename system/{mavros => mavros2}/config/mavros_config_px4.yaml (100%) rename system/{mavros => mavros2}/config/mavros_pluginlists_ap.yaml (100%) rename system/{mavros => mavros2}/config/mavros_pluginlists_px4.yaml (100%) rename system/{mavros => mavros2}/core.Dockerfile (100%) rename system/{mavros => mavros2}/fastrtps_profiles.xml (100%) rename system/{mavros => mavros2}/mavros.launch.xml (100%) rename system/{mavros => mavros2}/mavros_setup.sh (100%) rename system/{mavros => mavros2}/ros_entrypoint.sh (100%) diff --git a/system/mavros/Dockerfile b/system/mavros2/Dockerfile similarity index 100% rename from system/mavros/Dockerfile rename to system/mavros2/Dockerfile diff --git a/system/mavros/README.md b/system/mavros2/README.md similarity index 100% rename from system/mavros/README.md rename to system/mavros2/README.md diff --git a/system/mavros/config/mavros_config_ap.yaml b/system/mavros2/config/mavros_config_ap.yaml similarity index 100% rename from system/mavros/config/mavros_config_ap.yaml rename to system/mavros2/config/mavros_config_ap.yaml diff --git a/system/mavros/config/mavros_config_px4 copy.yaml b/system/mavros2/config/mavros_config_px4 copy.yaml similarity index 100% rename from system/mavros/config/mavros_config_px4 copy.yaml rename to system/mavros2/config/mavros_config_px4 copy.yaml diff --git a/system/mavros/config/mavros_config_px4.yaml b/system/mavros2/config/mavros_config_px4.yaml similarity index 100% rename from system/mavros/config/mavros_config_px4.yaml rename to system/mavros2/config/mavros_config_px4.yaml diff --git a/system/mavros/config/mavros_pluginlists_ap.yaml b/system/mavros2/config/mavros_pluginlists_ap.yaml similarity index 100% rename from system/mavros/config/mavros_pluginlists_ap.yaml rename to system/mavros2/config/mavros_pluginlists_ap.yaml diff --git a/system/mavros/config/mavros_pluginlists_px4.yaml b/system/mavros2/config/mavros_pluginlists_px4.yaml similarity index 100% rename from system/mavros/config/mavros_pluginlists_px4.yaml rename to system/mavros2/config/mavros_pluginlists_px4.yaml diff --git a/system/mavros/core.Dockerfile b/system/mavros2/core.Dockerfile similarity index 100% rename from system/mavros/core.Dockerfile rename to system/mavros2/core.Dockerfile diff --git a/system/mavros/fastrtps_profiles.xml b/system/mavros2/fastrtps_profiles.xml similarity index 100% rename from system/mavros/fastrtps_profiles.xml rename to system/mavros2/fastrtps_profiles.xml diff --git a/system/mavros/mavros.launch.xml b/system/mavros2/mavros.launch.xml similarity index 100% rename from system/mavros/mavros.launch.xml rename to system/mavros2/mavros.launch.xml diff --git a/system/mavros/mavros_setup.sh b/system/mavros2/mavros_setup.sh similarity index 100% rename from system/mavros/mavros_setup.sh rename to system/mavros2/mavros_setup.sh diff --git a/system/mavros/ros_entrypoint.sh b/system/mavros2/ros_entrypoint.sh similarity index 100% rename from system/mavros/ros_entrypoint.sh rename to system/mavros2/ros_entrypoint.sh From bdf92a38a3b26a532a0d73229ce839d188b11673 Mon Sep 17 00:00:00 2001 From: Rob Clarke Date: Fri, 4 Mar 2022 16:48:55 +0000 Subject: [PATCH 09/25] Update bake to mavros2 folder --- buildtools/docker-bake.hcl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buildtools/docker-bake.hcl b/buildtools/docker-bake.hcl index 42d2fec..f4a6c30 100644 --- a/buildtools/docker-bake.hcl +++ b/buildtools/docker-bake.hcl @@ -82,7 +82,7 @@ target "starling-mavros" { } target "starling-mavros2" { - context = "system/mavros" + context = "system/mavros2" dockerfile = "core.Dockerfile" tags = [ "${BAKE_REGISTRY}uobflightlabstarling/starling-mavros2:${BAKE_VERSION}", From 6eb146c671e692604c8091366657ec0ca28a138a Mon Sep 17 00:00:00 2001 From: Rob Clarke Date: Fri, 4 Mar 2022 16:49:40 +0000 Subject: [PATCH 10/25] Fix makefile --- system/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/Makefile b/system/Makefile index 210f2a6..727a835 100644 --- a/system/Makefile +++ b/system/Makefile @@ -26,4 +26,4 @@ mavp2p: vicon: $(BAKE) starling-vicon -.PHONY: mavros mavros_all_arch controller-base rosbridge-suite vicon mavp2p all +.PHONY: mavros mavros2 mavros_all_arch controller-base rosbridge-suite vicon mavp2p all From a788908c9526142437ae4a422cb5a7c3c83c3ed4 Mon Sep 17 00:00:00 2001 From: Rob Clarke Date: Fri, 4 Mar 2022 17:01:43 +0000 Subject: [PATCH 11/25] Update submodule location --- .gitmodules | 4 ++-- system/mavros2/mavros | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) create mode 160000 system/mavros2/mavros diff --git a/.gitmodules b/.gitmodules index 201d97f..edfec9a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,7 +1,7 @@ [submodule "system/vicon/vicon_udp"] path = system/vicon/vicon_udp url = https://github.com/UoBFlightLab/vicon_udp.git -[submodule "system/mavros/mavros"] - path = system/mavros/mavros +[submodule "system/mavros2/mavros"] + path = system/mavros2/mavros url = ssh://github.com/mavlink/mavros branch = ros2 diff --git a/system/mavros2/mavros b/system/mavros2/mavros new file mode 160000 index 0000000..f4898bb --- /dev/null +++ b/system/mavros2/mavros @@ -0,0 +1 @@ +Subproject commit f4898bbd0aa24f9e05bac467d3fc23fbbb36ad24 From e48b4ad9986770288193d5c5d7c0494e3dd28754 Mon Sep 17 00:00:00 2001 From: Rob Clarke Date: Fri, 4 Mar 2022 17:12:59 +0000 Subject: [PATCH 12/25] Add mavros from dev --- system/mavros/Dockerfile | 74 +++++ system/mavros/README.md | 140 ++++++++++ system/mavros/config/bridge_config.yaml | 205 ++++++++++++++ system/mavros/config/mavros_config_ap.yaml | 257 +++++++++++++++++ system/mavros/config/mavros_config_px4.yaml | 264 ++++++++++++++++++ .../mavros/config/mavros_pluginlists_ap.yaml | 17 ++ .../mavros/config/mavros_pluginlists_px4.yaml | 34 +++ system/mavros/fastrtps_profiles.xml | 19 ++ system/mavros/mavros.launch | 32 +++ system/mavros/mavros_bridge.launch.xml | 38 +++ system/mavros/mavros_setup.sh | 135 +++++++++ system/mavros/ros_entrypoint.sh | 20 ++ system/mavros/run_ros1.sh | 5 + 13 files changed, 1240 insertions(+) create mode 100644 system/mavros/Dockerfile create mode 100644 system/mavros/README.md create mode 100644 system/mavros/config/bridge_config.yaml create mode 100644 system/mavros/config/mavros_config_ap.yaml create mode 100644 system/mavros/config/mavros_config_px4.yaml create mode 100644 system/mavros/config/mavros_pluginlists_ap.yaml create mode 100644 system/mavros/config/mavros_pluginlists_px4.yaml create mode 100644 system/mavros/fastrtps_profiles.xml create mode 100644 system/mavros/mavros.launch create mode 100644 system/mavros/mavros_bridge.launch.xml create mode 100755 system/mavros/mavros_setup.sh create mode 100755 system/mavros/ros_entrypoint.sh create mode 100755 system/mavros/run_ros1.sh diff --git a/system/mavros/Dockerfile b/system/mavros/Dockerfile new file mode 100644 index 0000000..a0a3a77 --- /dev/null +++ b/system/mavros/Dockerfile @@ -0,0 +1,74 @@ +FROM ros:foxy-ros1-bridge + +# Install mavros +RUN apt-get update \ + && apt-get install --no-install-recommends -y \ + ros-${ROS1_DISTRO}-mavros \ + ros-${ROS1_DISTRO}-mavros-extras \ + ros-${ROS2_DISTRO}-mavros-msgs \ + && rm -rf /var/lib/apt/lists/ + +# Part of MAVROS setup +RUN /opt/ros/${ROS1_DISTRO}/lib/mavros/install_geographiclib_datasets.sh + +# We then need to build the bridge from source +# (So we probably don't need to use the ros1-bridge image...) +# Remove the preinstalled version +RUN apt-get remove -y ros-foxy-ros1-bridge +# Get the source (use specific commit hash instead of foxy due to this issue when building mavros: https://github.com/ros2/ros1_bridge/issues/313) +RUN git clone -b foxy https://github.com/ros2/ros1_bridge /bridge_ws/src/ros1_bridge \ + && cd /bridge_ws/src/ros1_bridge \ + && git checkout 82d1f4588761d6e54eeaca821398137be0e475f1 + +# Swap line 36 and 41 to fix this issue: https://github.com/ros2/ros1_bridge/issues/316 +# For use of parameter server. +RUN sed -i "41d" /bridge_ws/src/ros1_bridge/src/parameter_bridge.cpp \ + && sed -i "36 i rclcpp::init(argc, argv);" /bridge_ws/src/ros1_bridge/src/parameter_bridge.cpp + +# Build with ROS2 mavros_msgs on path +RUN cd /bridge_ws \ + && . /opt/ros/${ROS1_DISTRO}/setup.sh \ + && . /opt/ros/${ROS2_DISTRO}/setup.sh \ + && export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH \ + && colcon build --packages-select ros1_bridge --cmake-force-configure \ + && rm -r build + +RUN mkdir /ros_ws + +COPY mavros_bridge.launch.xml /ros_ws/launch/ +COPY run_ros1.sh /ros_ws/scripts/ + +COPY ros_entrypoint.sh /ros_entrypoint.sh + +WORKDIR /ros_ws + +COPY mavros_setup.sh . + +COPY mavros.launch /ros_ws/launch/mavros.launch +COPY config/mavros_config_*.yaml / +COPY config/mavros_pluginlists_*.yaml / +COPY config/bridge_config.yaml / + +ENV MAVROS_MOD_CONFIG_PATH="/mavros_config_mod.yaml" +ENV MAVROS_CONFIG_PATH="/mavros_config_px4.yaml" +ENV MAVROS_PLUGINLISTS_PATH="/mavros_pluginlists_px4.yaml" + +ENV BRIDGE_MOD_CONFIG_PATH="/bridge_config_mod.yaml" +ENV BRIDGE_CONFIG_PATH="/bridge_config.yaml" + +# Add custom ROS DDS configuration (force UDP always) +COPY fastrtps_profiles.xml /ros_ws/ +ENV FASTRTPS_DEFAULT_PROFILES_FILE /ros_ws/fastrtps_profiles.xml + +# TODO: Rename to FCU_PROTOCOL +ENV MAVROS_FCU_CONN="udp" +# TODO: Rename to FCU_HOST +ENV MAVROS_FCU_IP="127.0.0.1" +# TODO: Rename to PORT_BASE +ENV MAVROS_FCU_UDP_BASE="14830" +ENV MAVROS_TGT_SYSTEM="auto" +ENV PX4_INSTANCE_BASE=0 +ENV MAVROS_TGT_FIRMWARE="px4" +ENV MAVROS_GCS_URL="udp-pb://@:14550" + +CMD [ "ros2", "launch", "launch/mavros_bridge.launch.xml"] diff --git a/system/mavros/README.md b/system/mavros/README.md new file mode 100644 index 0000000..0536f7d --- /dev/null +++ b/system/mavros/README.md @@ -0,0 +1,140 @@ +# `starling-mavros` Container + +This container holds both MAVROS and a ROS1/2 bridge. In the future, we expect to use the ROS2 version of MAVROS which +will eliminate the need for the ROS1/2 bridge. + +## Contents +[TOC] + +## Launch Process + +The initial process of starting the container goes through `ros_entrypoint.sh`. This file sets up both ROS versions and +runs a setup script which configures the environment to tell MAVROS how to talk to the drone and where to publish its +topics. The setup script works in a couple of different ways depending on where the container is running. + +If the container is running on a drone, it expects to be able to find the `/etc/starling/vehicle.config` file. This file +contains some information that MAVROS needs to be able to communicate with the flight controller. An example +`vehicle.config` file is included below. + +If the container is started as part of a Kubernetes StatefulSet deployment, the setup script will attemp# Mavros /ROS12 Bridge Container + +To run: +```bash +make build # Builds docker container +make run # Runs docker container +``` + +Default action of launching the container (make run) will be to run `launch.sh` which will source mavros_setup.sh, and run the launch file. + +## Launch file + +The launch file `mavros_bridge.launch` runs the following +Running the container will run 2 ros nodes in parallel: +1. A Mavros node listening on fcu_url, targeting drone with sysid tgt_system + - `roslaunch mavros apm.launch fcu_url:=${MAVROS_FCU_URL} tgt_system:=${MAVROS_TGT_SYSTEM}` +2. A ros1 bridge node with option to forward all mavros topics to ros2 + - `ros2 run ros1_bridge dynamic_bridge --bridge-all-1to2-topics` + +## Contents +There are two docker files: + +- `Dockerfile.mavros` is the base container which is based on Ubuntu 20.04. It runs ROS2 Foxy, ROS1 Noetic and MAVROS via the ROS1_bridge. +- `mavros_setup.sh` is sourced at the end of the Dockerfile if any environment variables need modification. +- `mavros_bridge.launch` is a ros1 launch file, and launches both mavros and the ros1 bridge. It has 4 parameters: + - fcu_url - Flight control url, point it at the physical autopilot, ardupilot sitl or px4 sitl, no default in launch file. Defailts to `MAVROS_FCU_URL="udp://127.0.0.1:14550@14555"` using launch.sh + - target_system - target id to filter for from mavlink stream, defaults to 1, or MAVROS_TGT_SYSTEM using launch.sh + - system_id - sysid of mavros node, defaults to 1 + - firmware - `px4` or `apm`, name of the mavros launch file. Defaults to `px4`. + +## Configuration + +We have identified that mavros is a base requirement for any drone running onboard compute in order to communicate with both the autopilot and GCS. + +The mavros container exposes an environment variable `MAVROS_FCU_URL` which is passed to mavros's `fcu_url` option for configuring the mavlink connection to mavros. The default value is: +> `MAVROS_FCU_URL=udp://127.0.0.1:14550@14555` + +It also exposes environment variabel `MAVROS_TGT_SYSTEM` which is the `SYSID` of the ardupilot instance that mavros wants to connect to. This is usually an integer value. However, an IP address can also be passed in and `mavros_setup.sh` will extract the last numver of the IPv4 address as the value. Default is 1 + +To modify this at runtime, pass the `-e MAVROS_FCU_URL=` to `docker run` like so +> `docker run -e MAVROS_FCU_URL= -e MAVROS_TGT_SYSTEM=<#n>...` + +See Mavros for possible connection values + +This container builds the ros1_bridge from scratch in order to incorporate the `mavros_msgs` package. +t to get the +ordinal of its containing pod from the hostname. It will then use this ordinal to set up the ports and the system ID to +match those generated by a PX4 SITL instance set up with the same ordinal. If the setup script fails to get the ordinal +from the hostname, it will attempt to connect to a PX4 SITL with `PX4_INSTANCE=0`. + +Once the setup script has run, the default behaviour is to launch the `mavros_bridge.launch.xml` file. This behaviour +should be usable in almost all cases. This is a __ROS2__ launch file. It instructs `ros2 launch` to run the +`ros1_bridge` node and an instance of __ROS1__'s `roslaunch`. This in turn launches the __ROS1__ `mavros.launch` file, +which contains instructions to run the MAVROS node. + +The __ROS2__ `mavros_bridge.launch.xml` script defines a set of arguments to enable configuration of the MAVROS node. +These are ususally filled by the environment variables defined below. If the configurability provided here is +insufficient, the image can be run with a different command. + +## Environment Variables + +Name | Default Value | Description +----------------------|--------------------|------------ +`MAVROS_FCU_CONN` | "udp" | Protocol for autogenerated FCU URL +`MAVROS_FCU_IP` | "127.0.0.1" | IP for autogenerated FCU_URL +`MAVROS_FCU_UDP_BASE` | "14830" | Base port for autogenerated FCU_URL +`MAVROS_TGT_SYSTEM` | "auto" | Target system ID, if set to a number, this will __override__ the automatic behaviour +`PX4_INSTANCE_BASE` | 0 | Base instance for autogenerated instance matching +`MAVROS_TGT_FIRMWARE` | "px4" | Firmware profile used by MAVROS. Only other valid value currently is "apm" +`MAVROS_GCS_URL` | "udp-pb://@:14550" | MAVROS URL for ground control station connection +`MAVROS_FCU_URL` | {unset} | MAVROS URL for FCU connection. Set to __override__ automatic behaviour +`VEHICLE_NAMESPACE` | {unset} | Namespace for mavros topics. Set to __override__ default value of `vehicle_${TGT_SYSTEM}` + +`MAVROS_FCU_URL` is autogenerated as: +> `$MAVROS_FCU_CONN://$MAVROS_FCU_IP:$((MAVROS_FCU_UDP_BASE + INSTANCE))@/` + +If no Kubernetes deployment is detected, this ends up as: +> `udp://127.0.0.1:14830@/` + +Similarly, if no Kubernetes is detected, `MAVROS_TGT_SYSTEM` will end up as `1`, + +When setting `MAVROS_FCU_URL` manually, remember that a query string (_e.g._ `?ids=n,240`) will be added during launch. +You need to ensure that your input for `MAVROS_FCU_URL` supports this syntax. Of particular note is the need for a +trailing `/` in most formats, but not for the `serial://` format. Also note that the plain file format does not support +this. See [the MAVROS docs](https://github.com/mavlink/mavros/blob/master/mavros/README.md#connection-url) for more +information on URL formats. + +## Build Process + +At the time of writing, there is not a binary distribution available for `mavros_msgs` under ROS2. Therefore, this +package needs to be built from source. In addition, to ensure support for custom message types, `ros1_bridge` also needs +to be built from source. The build process also includes some additional steps to complete the installation of MAVROS +under ROS1. + +## Example `vehicle.config` + +Note that the extended form of the serial URL is required for MAVROS's target "query string" to work. + +```bash +VEHICLE_FCU_URL=serial:///dev/px4fmu:115200 +VEHICLE_FIRMWARE=px4 +VEHICLE_MAVLINK_SYSID=23 +VEHICLE_NAME=clover23 +``` + +## Behaviour Notes + +### Running on a vehicle + +Ensure `/etc/starling/vehicle.config` is mounted. The container is then configured from the contents of that file. + +### Running under Kubernetes StatefulSet + +The container is configured based on the detected ordinal from the hostname. + +`MAVROS_FCU_URL` is autogenerated as: `udp://127.0.0.1:$((14830 + ORDINAL))@` + +`MAVROS_TGT_SYSTEM` will end up as `$((ORDINAL + 1))` + +### Running isolated + +Default values will be used, equivalent to the Kubernetes case with `ORDINAL=0` diff --git a/system/mavros/config/bridge_config.yaml b/system/mavros/config/bridge_config.yaml new file mode 100644 index 0000000..2ad8861 --- /dev/null +++ b/system/mavros/config/bridge_config.yaml @@ -0,0 +1,205 @@ +# This Config details the list of topics we wish to forward from ros1 to ros2 +# Note that this file will be copied with topics specified for $VEHICLE_NAMESPACE +# i.e. /$VEHICLE_NAMESPACE/ +# MAVROS: http://wiki.ros.org/mavros +# MAVROS EXTRAS: http://wiki.ros.org/mavros_extras + +# These are bidrectional from ROS1 to ROS2 +# The Type must be a ROS2 type +topics: + - queue_size: 1 + topic: mavros/state + type: mavros_msgs/msg/State + - queue_size: 1 + topic: mavros/battery + type: sensor_msgs/msg/BatteryState + - queue_size: 1 + topic: mavros/altitude + type: sensor_msgs/msg/Altitude + - queue_size: 1 + topic: mavros/extended_state + type: mavros_msgs/msg/ExtendedState + - queue_size: 1 + topic: mavros/imu/data + type: sensor_msgs/msg/Imu + - queue_size: 1 + topic: mavlink/to + type: mavros_msgs/msg/Mavlink + - queue_size: 1 + topic: mavlink/from + type: mavros_msgs/msg/Mavlink + # 6.6 Global position + #- queue_size: 1 # TODO set up custom message to use nav_msgs/Odom + # topic: mavros/global_position/local + # type: geometry_msgs/msg/PoseWithCovarianceStamped + - queue_size: 1 + topic: mavros/global_position/global + type: sensor_msgs/msg/NavSatFix + + # 6.8 Local Position + - queue_size: 5 + topic: mavros/local_position/pose + type: geometry_msgs/msg/PoseStamped + - queue_size: 5 + topic: mavros/local_position/pose_cov + type: geometry_msgs/msg/PoseWithCovarianceStamped + - queue_size: 5 + topic: mavros/local_position/velocity_local + type: geometry_msgs/msg/TwistStamped + - queue_size: 5 + topic: mavros/local_position/velocity_body + type: geometry_msgs/msg/TwistStamped + - queue_size: 5 + topic: mavros/local_position/velocity_body_cov + type: geometry_msgs/msg/TwistWithCovarianceStamped + - queue_size: 5 + topic: mavros/local_position/accel + type: geometry_msgs/msg/AccelWithCovarianceStamped + - queue_size: 5 + topic: mavros/local_position/odom + type: nav_msgs/msg/Odometry + + # 6.9 Manual Control + - queue_size: 1 + topic: mavros/manual_control/send + type: mavros_msgs/msg/ManualControl + - queue_size: 1 + topic: mavros/manual_control/control + type: mavros_msgs/msg/ManualControl + + # 6.12 Safety Area + - queue_size: 1 + topic: mavros/safety_area + type: geometry_msgs/msg/PolygonStamped + + # 6.13 Setpoint Accel + - queue_size: 1 + topic: mavros/setpoint_accel/accel + type: geometry_msgs/msg/Vector3Stamped + + # 6.14 Setpoint Attitude + - queue_size: 1 + topic: mavros/setpoint_attitude/cmd_vel + type: geometry_msgs/msg/TwistStamped + - queue_size: 1 + topic: mavros/setpoint_attitude/attitude + type: geometry_msgs/msg/PoseStamped + - queue_size: 1 + topic: mavros/setpoint_attitude/thrust + type: mavros_msgs/msg/Thrust + + # 6.15 Setpoint Position + - queue_size: 1 + topic: mavros/setpoint_position/global + type: geographic_msgs/msg/GeoPoseStamped + - queue_size: 1 + topic: mavros/setpoint_position/local + type: geometry_msgs/msg/PoseStamped + - queue_size: 1 + topic: mavros/setpoint_position/global_to_local + type: geographic_msgs/msg/GeoPoseStamped + + # 6.16 Setpoint Raw + - queue_size: 1 + topic: mavros/setpoint_raw/local + type: mavros_msgs/msg/PositionTarget + - queue_size: 1 + topic: mavros/setpoint_raw/global + type: mavros_msgs/msg/GlobalPositionTarget + - queue_size: 1 + topic: mavros/setpoint_raw/attitude + type: mavros_msgs/msg/AttitudeTarget + + # 6.17 Setpoint Velocity + - queue_size: 1 + topic: mavros/setpoint_velocity/cmd_vel_unstamped + type: geometry_msgs/msg/Twist + + # 6.21 Waypoints + - queue_size: 1 + topic: mavros/mission/reached + type: mavros_msgs/msg/WaypointReached + - queue_size: 1 + topic: mavros/mission/waypoints + type: mavros_msgs/msg/WaypointList + + # Mavros extras 4.2 distance sensor (matches mavros_config.yaml) + - queue_size: 1 + topic: mavros/distance_sensor/hrlv_ez4_sonar + type: sensor_msgs/msg/Range + - queue_size: 1 + topic: mavros/distance_sensor/lidarlite_laser + type: sensor_msgs/msg/Range + - queue_size: 1 + topic: mavros/distance_sensor/rangefinder + type: sensor_msgs/msg/Range + + # Mavros extras image pub + - queue_size: 1 + topic: mavros/image/camera_image + type: sensor_msgs/msg/Image + + # Mavros extras 4.5 px4flow + - queue_size: 1 + topic: mavros/px4flow/raw/optical_flow_rad + type: mavros_msgs/msg/OpticalFlowRad + - queue_size: 1 + topic: mavros/px4flow/ground_distance + type: sensor_msgs/msg/Range + - queue_size: 1 + topic: mavros/distance_sensor/temperature + type: sensor_msgs/msg/Temperature + + # Mavros Extras 4.7 vision pose estimate + - queue_size: 1 + topic: mavros/vision_pose/pose + type: geometry_msgs/msg/PoseStamped + - queue_size: 1 + topic: mavros/vision_pose/pose_cov + type: geometry_msgs/msg/PoseWithCovarianceStamped + + # Mavros Extras 4.8 vision speed estimate + - queue_size: 1 + topic: mavros/vision_speed/speed_vector + type: geometry_msgs/msg/Vector3Stamped + - queue_size: 1 + topic: mavros/vision_speed/speed_twist + type: geometry_msgs/msg/TwistStamped + +# service requests going from 2 to 1 +# server on ROS1 side, client on ROS2 side +# Types must be *ROS1* Types +services_2to1: + # 6.4 Command + - service: mavros/cmd/command + type: mavros_msgs/CommandLong + - service: mavros/cmd/command_int + type: mavros_msgs/CommandInt + - service: mavros/cmd/arming + type: mavros_msgs/CommandBool + - service: mavros/cmd/set_home + type: mavros_msgs/CommandHome + - service: mavros/cmd/takeoff + type: mavros_msgs/CommandTOL + - service: mavros/cmd/land + type: mavros_msgs/CommandTOL + - service: mavros/cmd/trigger_control + type: mavros_msgs/CommandTriggerControl + + # 6.18 Sys Status + - service: mavros/set_stream_rate + type: mavros_msgs/StreamRate + - service: mavros/set_mode + type: mavros_msgs/SetMode + + # 6.21 Waypoints + - service: mavros/mission/pull + type: mavros_msgs/WaypointPull + - service: mavros/mission/push + type: mavros_msgs/WaypointPush + - service: mavros/mission/clear + type: mavros_msgs/WaypointClear + - service: mavros/mission/set_current + type: mavros_msgs/WaypointSetCurrent + +services_1to2: [] diff --git a/system/mavros/config/mavros_config_ap.yaml b/system/mavros/config/mavros_config_ap.yaml new file mode 100644 index 0000000..34f5526 --- /dev/null +++ b/system/mavros/config/mavros_config_ap.yaml @@ -0,0 +1,257 @@ +# Common configuration for APM2 autopilot +# +# node: +startup_px4_usb_quirk: false + +# --- system plugins --- + +# sys_status & sys_time connection options +conn: + heartbeat_rate: 1.0 # send hertbeat rate in Hertz + heartbeat_mav_type: "ONBOARD_CONTROLLER" + timeout: 10.0 # hertbeat 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 + 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 plugins (alphabetical order) --- + +# 3dr_radio +tdr_radio: + 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 + +# dummy +# None + +# ftp +# 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 + +# 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 + +# local_position +local_position: + frame_id: "map" + tf: + send: false + frame_id: "map" + child_frame_id: "base_link" + send_fcu: false + +# param +# None, used for FCU params + +# 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 + +# 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 +setpoint_raw: + 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 + +# setpoint_velocity +setpoint_velocity: + 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 extras plugins (same order) --- + +# adsb +# None + +# debug_value +# None + +# distance_sensor +## Currently available orientations: +# 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+ + +# image_pub +image: + 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 + +# 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} + +# mocap_pose_estimate +mocap: + # select mocap source + use_tf: false # ~mocap/tf + use_pose: true # ~mocap/pose + +# odom +odometry: + frame_tf: + desired_frame: "ned" + estimator_type: 3 # check enum MAV_ESTIMATOR_TYPE in + +# 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 + +# 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 + +# 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 + +# vibration +vibration: + 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" + +# vim:set ts=2 sw=2 et: diff --git a/system/mavros/config/mavros_config_px4.yaml b/system/mavros/config/mavros_config_px4.yaml new file mode 100644 index 0000000..53c9efc --- /dev/null +++ b/system/mavros/config/mavros_config_px4.yaml @@ -0,0 +1,264 @@ +# Common configuration for PX4 autopilot +# +# node: +startup_px4_usb_quirk: true + +# --- system plugins --- + +# sys_status & sys_time connection options +conn: + heartbeat_rate: 1.0 # send hertbeat rate in Hertz + timeout: 10.0 # hertbeat 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 + 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 plugins (alphabetical order) --- + +# 3dr_radio +tdr_radio: + 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 + +# dummy +# None + +# ftp +# 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 + +# 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 + +# local_position +local_position: + frame_id: "map" + tf: + send: false + frame_id: "map" + child_frame_id: "base_link" + send_fcu: false + +# param +# None, used for FCU params + +# 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 + +# 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. + +# 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 + +# setpoint_velocity +setpoint_velocity: + 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 extras plugins (same order) --- + +# adsb +# None + +# debug_value +# None + +# distance_sensor +## Currently available orientations: +# 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 + rangefinder: + id: 4 + frame_id: "rangefinder" + orientation: PITCH_270 + field_of_view: 0.5 + # rangefinder_sub: + # subscriber: true + # id: 5 + # frame_id: "rangefinder" + # orientation: PITCH_270 + # field_of_view: 0.5 + + +# image_pub +image: + 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 + +# 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} + +# mocap_pose_estimate +mocap: + # select mocap source + use_tf: false # ~mocap/tf + use_pose: true # ~mocap/pose + +# 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 + +# 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 + +# 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 + +# 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 + +# vibration +vibration: + frame_id: "base_link" + +# vim:set ts=2 sw=2 et: \ No newline at end of file diff --git a/system/mavros/config/mavros_pluginlists_ap.yaml b/system/mavros/config/mavros_pluginlists_ap.yaml new file mode 100644 index 0000000..9f9e0d3 --- /dev/null +++ b/system/mavros/config/mavros_pluginlists_ap.yaml @@ -0,0 +1,17 @@ +plugin_blacklist: +# common +- actuator_control +- ftp +- safety_area +- hil +# extras +- altitude +- debug_value +- image_pub +- px4flow +- vibration +- vision_speed_estimate +- wheel_odometry + +plugin_whitelist: [] +#- 'sys_* \ No newline at end of file diff --git a/system/mavros/config/mavros_pluginlists_px4.yaml b/system/mavros/config/mavros_pluginlists_px4.yaml new file mode 100644 index 0000000..ed4317c --- /dev/null +++ b/system/mavros/config/mavros_pluginlists_px4.yaml @@ -0,0 +1,34 @@ +plugin_blacklist: [] +# common +# - safety_area +# extras +# - image_pub +# - vibration +# - distance_sensor +# - rangefinder +# - wheel_odometry + + +plugin_whitelist: +#- 'sys_*' +- altitude +- command +- distance_sensor +- ftp +- global_position +- imu +- local_position +- manual_control +# - mocap_pose_estimate +- param +- px4flow +- rc_io +- setpoint_attitude +- setpoint_position +- setpoint_raw +- setpoint_velocity +- sys_status +- sys_time +- vision_pose_estimate +# - vision_speed_estimate +# - waypoint \ No newline at end of file diff --git a/system/mavros/fastrtps_profiles.xml b/system/mavros/fastrtps_profiles.xml new file mode 100644 index 0000000..4d3a443 --- /dev/null +++ b/system/mavros/fastrtps_profiles.xml @@ -0,0 +1,19 @@ + + + + + CustomUdpTransport + UDPv4 + + + + + + + CustomUdpTransport + + + false + + + \ No newline at end of file diff --git a/system/mavros/mavros.launch b/system/mavros/mavros.launch new file mode 100644 index 0000000..cc38f41 --- /dev/null +++ b/system/mavros/mavros.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/mavros/mavros_bridge.launch.xml b/system/mavros/mavros_bridge.launch.xml new file mode 100644 index 0000000..0c6fee2 --- /dev/null +++ b/system/mavros/mavros_bridge.launch.xml @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/system/mavros/mavros_setup.sh b/system/mavros/mavros_setup.sh new file mode 100755 index 0000000..97ff92b --- /dev/null +++ b/system/mavros/mavros_setup.sh @@ -0,0 +1,135 @@ +#!/bin/bash + +HAS_VEHICLE_CONFIG=false +TGT_SYSTEM_SET=false + +# If we have a vehicle.config file, assume we are running on a real vehicle +if [ -f "/etc/starling/vehicle.config" ]; then + echo "Found vehicle.config" + HAS_VEHICLE_CONFIG=true + # Source VEHICLE_MAVLINK_SYSID, VEHICLE_NAME, VEHICLE_FCU_URL and VEHICLE_FIRMWARE + source /etc/starling/vehicle.config +fi + +#TODO: VEHICLE_FIRMWARE should affect choice of default parameter file + +function get_instance { + # Attempt to get ordinal + ORDINAL="${HOSTNAME##*-}" + + if [ -z "$ORDINAL" ]; then + # Parsing ordinal failed, assume non-kubernetes container and use default of 0 + echo "0" + return 2 + fi + + if ! [ "$ORDINAL" -ge 0 ] 2>/dev/null; then + # Ordinal is not an integer, assume non-kubernetes container and use default of 0 + echo "0" + return 3 + fi + + echo $((PX4_INSTANCE_BASE + ORDINAL)) +} + +# If MAVROS_TGT_SYSTEM is auto, then set instance id to take MAVROS_TGT_SYSTEM_BASE + ORDINAL from StatefulSet hostname +# Hostname is of the form '-' +if [ "$MAVROS_TGT_SYSTEM" = "auto" ]; then + echo "MAVROS_TGT_SYSTEM set to auto" + if [ $HAS_VEHICLE_CONFIG = true ]; then + echo "MAVROS_TGT_SYSTEM set to VEHICLE_MAVLINK_SYSID ($VEHICLE_MAVLINK_SYSID) from /etc/starling/vehicle.config" + MAVROS_TGT_SYSTEM=$VEHICLE_MAVLINK_SYSID + # TODO: Add validity check for SYSID + else + set +e + INSTANCE=$(get_instance) + RESULT=$? + MAVROS_TGT_SYSTEM=$((INSTANCE + 1)) + set -e + if [ $RESULT -ne 0 ]; then + echo "Could not parse ordinal, using default instance of 0. MAVROS_TGT_SYSTEM will be 1" + else + echo "MAVROS_TGT_SYSTEM set to $MAVROS_TGT_SYSTEM (from hostname: $HOSTNAME)" + fi + fi +elif (($MAVROS_TGT_SYSTEM >= 1 && $MAVROS_TGT_SYSTEM <= 255 )); then + echo "MAVROS_TGT_SYSTEM setting as specified: $MAVROS_TGT_SYSTEM" + TGT_SYSTEM_SET=true +else + # TODO: Fail early here + echo "MAVROS_TGT_SYSTEM (set to $MAVROS_TGT_SYSTEM) is invalid, setting to 1. Must either be set to 'auto' where it will either look or number between 1 and 256" + MAVROS_TGT_SYSTEM=1 + TGT_SYSTEM_SET=true +fi + +export MAVROS_TGT_SYSTEM=$MAVROS_TGT_SYSTEM + + +if [ ! -v $VEHICLE_NAMESPACE ]; then + # If set Ensure VEHICLE_NAMESPACE is a valid topic name + # Replace all '-' with '_' + export VEHICLE_NAMESPACE=${VEHICLE_NAMESPACE//-/_} + echo "VEHICLE_NAMESPACE setting to $VEHICLE_NAMESPACE" +else + export VEHICLE_NAMESPACE="vehicle_$MAVROS_TGT_SYSTEM" + echo "VEHICLE_NAMESPACE not set, default to auto generated default based on mavros target ($MAVROS_TGT_SYSTEM) of $VEHICLE_NAMESPACE" +fi + + +if [ -v $MAVROS_FCU_URL ]; then + # If not set, then generate automatically + if ! [ $HAS_VEHICLE_CONFIG = true ]; then + # No vehicle.config, generate based on subparameters and the target system id dynamically + INSTANCE=$((MAVROS_TGT_SYSTEM - 1)) # Default is based on Mavros TGT System + if [ $TGT_SYSTEM_SET = false ]; then # If Mavros TGT System was set automatically, use get_instance. + set +e + INSTANCE=$(get_instance) + set -e + fi + MAVROS_FCU_URL="$MAVROS_FCU_CONN://$MAVROS_FCU_IP:$((MAVROS_FCU_UDP_BASE + INSTANCE))@/" + export MAVROS_FCU_URL=$MAVROS_FCU_URL + echo "MAVROS_FCU_URL automatically set to: $MAVROS_FCU_URL" + else + # Use value from vehicle.config + export MAVROS_FCU_URL=$VEHICLE_FCU_URL + echo "MAVROS_FCU_URL set to: $MAVROS_FCU_URL from vehicle.config" + fi +else + echo "MAVROS_FCU_URL was set to $MAVROS_FCU_URL" +fi + +# Modify mavros PX4 config for frame ids +MAVROS_MOD_CONFIG_PATH=${MAVROS_MOD_CONFIG_PATH:-"/mavros_config_mod.yaml"} +MAVROS_CONFIG_PATH=${MAVROS_CONFIG_PATH:-"/mavros_config.yaml"} +if [ ! -f "$MAVROS_MOD_CONFIG_PATH" ]; then + echo "Modified Mavros Config for $VEHICLE_NAMESPACE does not exist at $MAVROS_MOD_CONFIG_PATH. Generating specialised configuration for launch." + # This line: + # -E allow groupings defined by parenthesis to be used in replace by \1 \2 + # /\"map\"/! selects lines which do not contain "map" or "earth" (Global frame) + # The rest then adds $VEHICLE_NAMESPACE in as a prefix to whatever the frame_id was + # This captures both frame_id: and child_frame_id + # This then saves the output into $PX4_MOD_CONFIG_PATH + sed -E "/\"map\"|\"earth\"/! s/(frame_id: )\"(.+)\"/\1\"$VEHICLE_NAMESPACE\/\2\"/g" $MAVROS_CONFIG_PATH > $MAVROS_MOD_CONFIG_PATH + + # Also fix outliers such as the odometry id_des parameter + sed -i -E "/\"map\"|\"earth\"/! s/(id_des: )\"(.+)\"/\1\"$VEHICLE_NAMESPACE\/\2\"/g" $MAVROS_MOD_CONFIG_PATH +else + echo "Modified Mavros Config for $VEHICLE_NAMESPACE already exists at $MAVROS_MOD_CONFIG_PATH. Using for launch" +fi + +# Modify ros 1bridge for topic/service paths +BRIDGE_MOD_CONFIG_PATH=${BRIDGE_MOD_CONFIG_PATH:-"/bridge_config_mod.yaml"} +BRIDGE_CONFIG_PATH=${BRIDGE_CONFIG_PATH:-"/bridge_config.yaml"} +if [ ! -f "$BRIDGE_MOD_CONFIG_PATH" ]; then + echo "Modified Bridge Config for $VEHICLE_NAMESPACE does not exist at $BRIDGE_MOD_CONFIG_PATH. Generating specialised configuration for launch." + # This line: + # -E allow groupings defined by parenthesis to be used in replace by \1 \2 + # /\"map\"/! selects lines which do not contain "map" or "earth" (Global frame) + # The rest then adds $VEHICLE_NAMESPACE in as a prefix to whatever the frame_id was + # This captures both frame_id: and child_frame_id + # This then saves the output into $PX4_MOD_CONFIG_PATH + sed -E "s/(topic: )(.+)/\1\/$VEHICLE_NAMESPACE\/\2/g" $BRIDGE_CONFIG_PATH > $BRIDGE_MOD_CONFIG_PATH + sed -E -i "s/(service: )(.+)/\1\/$VEHICLE_NAMESPACE\/\2/g" $BRIDGE_MOD_CONFIG_PATH +else + echo "Modified Bridge Config for $VEHICLE_NAMESPACE already exists at $BRIDGE_MOD_CONFIG_PATH. Using for launch" +fi diff --git a/system/mavros/ros_entrypoint.sh b/system/mavros/ros_entrypoint.sh new file mode 100755 index 0000000..4299c8d --- /dev/null +++ b/system/mavros/ros_entrypoint.sh @@ -0,0 +1,20 @@ +#!/bin/bash +set -e + +# unsetting ROS_DISTRO to silence ROS_DISTRO override warning +unset ROS_DISTRO +# setup ros1 environment +source "/opt/ros/$ROS1_DISTRO/setup.bash" + +# unsetting ROS_DISTRO to silence ROS_DISTRO override warning +unset ROS_DISTRO +# setup ros2 environment +source "/opt/ros/$ROS2_DISTRO/setup.bash" + +# Source the from-source ros1_bridge workspace +source "/bridge_ws/install/setup.bash" + +# Source the mavros_setup for any user defined edits to the environment +source "/ros_ws/mavros_setup.sh" + +exec "$@" diff --git a/system/mavros/run_ros1.sh b/system/mavros/run_ros1.sh new file mode 100755 index 0000000..c5a7060 --- /dev/null +++ b/system/mavros/run_ros1.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +source /opt/ros/${ROS1_DISTRO}/setup.bash + +exec "$@" \ No newline at end of file From 253a26ec3e59f528926959489f1001e43e12a906 Mon Sep 17 00:00:00 2001 From: Rob Clarke Date: Fri, 4 Mar 2022 17:16:23 +0000 Subject: [PATCH 13/25] Add mavros2 to build scripts --- .github/workflows/remove_container_on_close.yaml | 1 + .github/workflows/update-image.yml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/.github/workflows/remove_container_on_close.yaml b/.github/workflows/remove_container_on_close.yaml index 4cd72a7..e5c0d6b 100644 --- a/.github/workflows/remove_container_on_close.yaml +++ b/.github/workflows/remove_container_on_close.yaml @@ -21,6 +21,7 @@ jobs: - rosbridge-suite - starling-controller-base - starling-mavros + - starling-mavros2 - starling-vicon - starling-sim-base-core - starling-sim-base-px4 diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml index 62bcd1a..1f46293 100644 --- a/.github/workflows/update-image.yml +++ b/.github/workflows/update-image.yml @@ -41,6 +41,8 @@ jobs: arm64: true - target: starling-mavros arm64: true + - target: starling-mavros2 + arm64: true - target: starling-vicon arm64: true - target: starling-sim-base-core From 5763d7c58561b6ab60e904e5a9d27071faf00083 Mon Sep 17 00:00:00 2001 From: Rob Clarke Date: Fri, 4 Mar 2022 17:18:50 +0000 Subject: [PATCH 14/25] Update gitmodule remote --- .gitmodules | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index edfec9a..f4ea8fe 100644 --- a/.gitmodules +++ b/.gitmodules @@ -3,5 +3,4 @@ url = https://github.com/UoBFlightLab/vicon_udp.git [submodule "system/mavros2/mavros"] path = system/mavros2/mavros - url = ssh://github.com/mavlink/mavros - branch = ros2 + url = ssh://github.com/rob-clarke/mavros From 57416920ddf6b2fe2de1e2105e0016d785ae4edf Mon Sep 17 00:00:00 2001 From: Rob Clarke Date: Fri, 4 Mar 2022 17:25:24 +0000 Subject: [PATCH 15/25] Use https for submodule --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index f4ea8fe..5ee3760 100644 --- a/.gitmodules +++ b/.gitmodules @@ -3,4 +3,4 @@ url = https://github.com/UoBFlightLab/vicon_udp.git [submodule "system/mavros2/mavros"] path = system/mavros2/mavros - url = ssh://github.com/rob-clarke/mavros + url = https://github.com/rob-clarke/mavros From 6b7816ac1fb290b377883be93e7c85a62a69cd4a Mon Sep 17 00:00:00 2001 From: Mickey Li Date: Thu, 24 Nov 2022 11:26:13 +0000 Subject: [PATCH 16/25] Fixed libgeographic install issue --- .../config/mavros_config_px4 copy.yaml | 266 ------------------ system/mavros2/core.Dockerfile | 25 +- 2 files changed, 16 insertions(+), 275 deletions(-) delete mode 100644 system/mavros2/config/mavros_config_px4 copy.yaml diff --git a/system/mavros2/config/mavros_config_px4 copy.yaml b/system/mavros2/config/mavros_config_px4 copy.yaml deleted file mode 100644 index 219ce67..0000000 --- a/system/mavros2/config/mavros_config_px4 copy.yaml +++ /dev/null @@ -1,266 +0,0 @@ -mavros_node: - ros__parameters: - # Common configuration for PX4 autopilot - # - # node: - startup_px4_usb_quirk: true - - # --- system plugins --- - - # sys_status & sys_time connection options - conn: - heartbeat_rate: 1.0 # send hertbeat rate in Hertz - timeout: 10.0 # hertbeat 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 - 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 plugins (alphabetical order) --- - - # 3dr_radio - tdr_radio: - 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 - - # dummy - # None - - # ftp - # 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 - - # 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 - - # local_position - local_position: - frame_id: "map" - tf: - send: false - frame_id: "map" - child_frame_id: "base_link" - send_fcu: false - - # param - # None, used for FCU params - - # 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 - - # 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. - - # 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 - - # setpoint_velocity - setpoint_velocity: - 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 extras plugins (same order) --- - - # adsb - # None - - # debug_value - # None - - # distance_sensor - ## Currently available orientations: - # 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 - rangefinder: - id: 4 - frame_id: "rangefinder" - orientation: PITCH_270 - field_of_view: 0.5 - # rangefinder_sub: - # subscriber: true - # id: 5 - # frame_id: "rangefinder" - # orientation: PITCH_270 - # field_of_view: 0.5 - - - # image_pub - image: - 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 - - # 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} - - # mocap_pose_estimate - mocap: - # select mocap source - use_tf: false # ~mocap/tf - use_pose: true # ~mocap/pose - - # 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 - - # 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 - - # 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 - - # 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 - - # vibration - vibration: - frame_id: "base_link" - - # vim:set ts=2 sw=2 et: \ No newline at end of file diff --git a/system/mavros2/core.Dockerfile b/system/mavros2/core.Dockerfile index 08e9ff4..15c4e5f 100644 --- a/system/mavros2/core.Dockerfile +++ b/system/mavros2/core.Dockerfile @@ -3,23 +3,23 @@ FROM ros:foxy-ros-base-focal AS builder WORKDIR /ros_ws # Install mavros dependencies -COPY mavros/libmavconn/package.xml /ros_ws/src/mavros/libmavconn/package.xml -COPY mavros/mavros/package.xml /ros_ws/src/mavros/mavros/package.xml -COPY mavros/mavros_msgs/package.xml /ros_ws/src/mavros/mavros_msgs/package.xml -COPY mavros/mavros_extras/package.xml /ros_ws/src/mavros/mavros_extras/package.xml +# COPY mavros/libmavconn/package.xml /ros_ws/src/mavros/libmavconn/package.xml +# COPY mavros/mavros/package.xml /ros_ws/src/mavros/mavros/package.xml +# COPY mavros/mavros_msgs/package.xml /ros_ws/src/mavros/mavros_msgs/package.xml +# COPY mavros/mavros_extras/package.xml /ros_ws/src/mavros/mavros_extras/package.xml + +RUN git clone --depth 1 --branch 2.3.0 https://github.com/mavlink/mavros.git /ros_ws/src/mavros RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ && apt-get update \ && rosdep install -y --from-paths src --ignore-src . # Install mavros itself -COPY mavros /ros_ws/src/mavros +# COPY mavros /ros_ws/src/mavros RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ && colcon build -RUN /ros_ws/install/mavros/lib/mavros/install_geographiclib_datasets.sh - # Start the main image build FROM ros:foxy-ros-core-focal AS image @@ -40,8 +40,15 @@ RUN rm install/COLCON_IGNORE \ && SUDO_FORCE_REMOVE=yes apt-get autoremove -y \ && rm -rf /var/lib/apt/lists/* -# Part of MAVROS setup -COPY --from=builder /usr/share/GeographicLib /usr/share/GeographicLib +# Part of MAVROS setup for lib geographic +## Ensure geographiclib is properly installed with FindGeographicLib available +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + libgeographic-dev \ + geographiclib-tools \ + && rm -rf /var/lib/apt/lists/* \ + && /ros_ws/install/mavros/lib/mavros/install_geographiclib_datasets.sh \ + && ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3.16/Modules/ COPY ros_entrypoint.sh /ros_entrypoint.sh From 27eeebbbbea89a2a4974adda4c616eb3c556123e Mon Sep 17 00:00:00 2001 From: Mickey Li Date: Thu, 24 Nov 2022 11:26:38 +0000 Subject: [PATCH 17/25] Removed mavros as a submodule --- .gitmodules | 3 --- system/mavros2/mavros | 1 - 2 files changed, 4 deletions(-) delete mode 160000 system/mavros2/mavros diff --git a/.gitmodules b/.gitmodules index 5ee3760..a0d366e 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,3 @@ [submodule "system/vicon/vicon_udp"] path = system/vicon/vicon_udp url = https://github.com/UoBFlightLab/vicon_udp.git -[submodule "system/mavros2/mavros"] - path = system/mavros2/mavros - url = https://github.com/rob-clarke/mavros diff --git a/system/mavros2/mavros b/system/mavros2/mavros deleted file mode 160000 index f4898bb..0000000 --- a/system/mavros2/mavros +++ /dev/null @@ -1 +0,0 @@ -Subproject commit f4898bbd0aa24f9e05bac467d3fc23fbbb36ad24 From 8e4a5e6fb20ec36d3489c00a1084d5917e0a1961 Mon Sep 17 00:00:00 2001 From: Mickey Li Date: Thu, 24 Nov 2022 12:22:08 +0000 Subject: [PATCH 18/25] Migrated back to using ubuntu ap repo for mavros instead of source build --- buildtools/docker-bake.hcl | 2 +- system/mavros2/Dockerfile | 47 ++++++------------ system/mavros2/core.Dockerfile | 81 ------------------------------- system/mavros2/docker-compose.yml | 31 ++++++++++++ system/mavros2/ros_entrypoint.sh | 2 +- 5 files changed, 48 insertions(+), 115 deletions(-) delete mode 100644 system/mavros2/core.Dockerfile create mode 100644 system/mavros2/docker-compose.yml diff --git a/buildtools/docker-bake.hcl b/buildtools/docker-bake.hcl index 001eb4a..6cc1b9b 100644 --- a/buildtools/docker-bake.hcl +++ b/buildtools/docker-bake.hcl @@ -98,7 +98,7 @@ target "starling-mavros" { target "starling-mavros2" { context = "system/mavros2" - dockerfile = "core.Dockerfile" + dockerfile = "Dockerfile" tags = [ "${BAKE_REGISTRY}uobflightlabstarling/starling-mavros2:${BAKE_VERSION}", notequal("",BAKE_RELEASENAME) ? "${BAKE_REGISTRY}uobflightlabstarling/starling-mavros2:${BAKE_RELEASENAME}": "", diff --git a/system/mavros2/Dockerfile b/system/mavros2/Dockerfile index 8281425..b5b9d6f 100644 --- a/system/mavros2/Dockerfile +++ b/system/mavros2/Dockerfile @@ -1,40 +1,23 @@ -FROM ros:foxy-ros-base-focal AS builder +FROM ros:foxy-ros-base-focal WORKDIR /ros_ws -# Install mavros dependencies -COPY mavros/libmavconn/package.xml /ros_ws/src/mavros/libmavconn/package.xml -COPY mavros/mavros/package.xml /ros_ws/src/mavros/mavros/package.xml -COPY mavros/mavros_msgs/package.xml /ros_ws/src/mavros/mavros_msgs/package.xml -COPY mavros/mavros_extras/package.xml /ros_ws/src/mavros/mavros_extras/package.xml - -RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ - && apt-get update \ - && rosdep install -y --from-paths src --ignore-src . - -# Install mavros itself -COPY mavros /ros_ws/src/mavros - -RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ - && colcon build - -# Start the main image build -FROM ros:foxy-ros-base-focal AS image - -WORKDIR /ros_ws - -# Copy over the build artifacts -COPY --from=builder /ros_ws/install /ros_ws/install - -# Install exec dependencies -RUN rm install/COLCON_IGNORE \ - && . /opt/ros/${ROS_DISTRO}/setup.sh \ - && apt-get update \ - && rosdep install -t exec -y --from-paths install \ +# Install mavros from apt +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + wget \ + python3-pip \ + ros-${ROS_DISTRO}-mavros\ + ros-${ROS_DISTRO}-mavros-extras \ + ros-${ROS_DISTRO}-mavros-msgs \ + libgeographic-dev \ + geographiclib-tools \ && rm -rf /var/lib/apt/lists/* -# Part of MAVROS setup -RUN /ros_ws/install/mavros/lib/mavros/install_geographiclib_datasets.sh +## Ensure geographiclib is properly installed with FindGeographicLib available +RUN wget https://raw.githubusercontent.com/mavlink/mavros/ros2/mavros/scripts/install_geographiclib_datasets.sh \ + && bash install_geographiclib_datasets.sh \ + && ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3.16/Modules/ COPY ros_entrypoint.sh /ros_entrypoint.sh diff --git a/system/mavros2/core.Dockerfile b/system/mavros2/core.Dockerfile deleted file mode 100644 index 15c4e5f..0000000 --- a/system/mavros2/core.Dockerfile +++ /dev/null @@ -1,81 +0,0 @@ -FROM ros:foxy-ros-base-focal AS builder - -WORKDIR /ros_ws - -# Install mavros dependencies -# COPY mavros/libmavconn/package.xml /ros_ws/src/mavros/libmavconn/package.xml -# COPY mavros/mavros/package.xml /ros_ws/src/mavros/mavros/package.xml -# COPY mavros/mavros_msgs/package.xml /ros_ws/src/mavros/mavros_msgs/package.xml -# COPY mavros/mavros_extras/package.xml /ros_ws/src/mavros/mavros_extras/package.xml - -RUN git clone --depth 1 --branch 2.3.0 https://github.com/mavlink/mavros.git /ros_ws/src/mavros - -RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ - && apt-get update \ - && rosdep install -y --from-paths src --ignore-src . - -# Install mavros itself -# COPY mavros /ros_ws/src/mavros - -RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ - && colcon build - -# Start the main image build -FROM ros:foxy-ros-core-focal AS image - -WORKDIR /ros_ws - -# Copy over the build artifacts -COPY --from=builder /ros_ws/install /ros_ws/install - -# Install exec dependencies -RUN rm install/COLCON_IGNORE \ - && apt-get update \ - && apt-get install -y --no-install-recommends python3-rosdep \ - && rosdep init \ - && rosdep update --rosdistro $ROS_DISTRO \ - && . /opt/ros/${ROS_DISTRO}/setup.sh \ - && rosdep install -t exec -y --from-paths install --ignore-src \ - && apt-get remove -y python3-rosdep \ - && SUDO_FORCE_REMOVE=yes apt-get autoremove -y \ - && rm -rf /var/lib/apt/lists/* - -# Part of MAVROS setup for lib geographic -## Ensure geographiclib is properly installed with FindGeographicLib available -RUN apt-get update \ - && apt-get install -y --no-install-recommends \ - libgeographic-dev \ - geographiclib-tools \ - && rm -rf /var/lib/apt/lists/* \ - && /ros_ws/install/mavros/lib/mavros/install_geographiclib_datasets.sh \ - && ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3.16/Modules/ - -COPY ros_entrypoint.sh /ros_entrypoint.sh - -COPY mavros_setup.sh . - -COPY mavros.launch.xml /ros_ws/launch/mavros.launch.xml -COPY config/mavros_config_*.yaml / -COPY config/mavros_pluginlists_*.yaml / - -ENV MAVROS_MOD_CONFIG_PATH="/mavros_config_mod.yaml" -ENV MAVROS_CONFIG_PATH="/mavros_config_px4.yaml" -ENV MAVROS_MOD_PLUGINLISTS_PATH="/mavros_pluginlists_mod.yaml" -ENV MAVROS_PLUGINLISTS_PATH="/mavros_pluginlists_px4.yaml" - -# Add custom ROS DDS configuration (force UDP always) -COPY fastrtps_profiles.xml /ros_ws/ -ENV FASTRTPS_DEFAULT_PROFILES_FILE /ros_ws/fastrtps_profiles.xml - -# TODO: Rename to FCU_PROTOCOL -ENV MAVROS_FCU_CONN="udp" -# TODO: Rename to FCU_HOST -ENV MAVROS_FCU_IP="127.0.0.1" -# TODO: Rename to PORT_BASE -ENV MAVROS_FCU_UDP_BASE="14830" -ENV MAVROS_TGT_SYSTEM="auto" -ENV PX4_INSTANCE_BASE=0 -ENV MAVROS_TGT_FIRMWARE="px4" -ENV MAVROS_GCS_URL="udp-pb://@:14550" - -CMD [ "ros2", "launch", "launch/mavros.launch.xml"] diff --git a/system/mavros2/docker-compose.yml b/system/mavros2/docker-compose.yml new file mode 100644 index 0000000..cf71b6f --- /dev/null +++ b/system/mavros2/docker-compose.yml @@ -0,0 +1,31 @@ + +version: '3' + +services: + simhost: # Must be called simhost + image: uobflightlabstarling/starling-sim-iris:latest + ports: + - "8080:8080" + + sitl: + image: uobflightlabstarling/starling-sim-px4-sitl:nightly + environment: + - "PX4_SIM_HOST=simhost" + - "PX4_OFFBOARD_HOST=mavros" + ports: + - "18570:18570/udp" + + mavros: + image: uobflightlabstarling/starling-mavros2:latest + # command: ros2 launch launch/mavros_bridge.launch.xml + environment: + - "MAVROS_TGT_SYSTEM=1" + - "MAVROS_FCU_IP=0.0.0.0" + - "MAVROS_GCS_URL=tcp-l://0.0.0.0:5760" + ports: + - "5760:5760" + + # rosbridge-suite: + # image: uobflightlabstarling/rosbridge-suite:latest + # ports: + # - "9090:9090" diff --git a/system/mavros2/ros_entrypoint.sh b/system/mavros2/ros_entrypoint.sh index ad3caa4..35daf8a 100755 --- a/system/mavros2/ros_entrypoint.sh +++ b/system/mavros2/ros_entrypoint.sh @@ -4,7 +4,7 @@ set -e # setup ros2 environment source "/opt/ros/$ROS_DISTRO/setup.bash" -source "/ros_ws/install/setup.bash" +# source "/ros_ws/install/setup.bash" # Source the mavros_setup for any user defined edits to the environment source "/ros_ws/mavros_setup.sh" From f2ba66317dd275e0f1566cc1e1e744debf8bfdd4 Mon Sep 17 00:00:00 2001 From: Mickey Li Date: Thu, 24 Nov 2022 12:24:32 +0000 Subject: [PATCH 19/25] Added build from source example --- system/mavros2/buildfromsource.Dockerfile | 85 +++++++++++++++++++++++ 1 file changed, 85 insertions(+) create mode 100644 system/mavros2/buildfromsource.Dockerfile diff --git a/system/mavros2/buildfromsource.Dockerfile b/system/mavros2/buildfromsource.Dockerfile new file mode 100644 index 0000000..c484859 --- /dev/null +++ b/system/mavros2/buildfromsource.Dockerfile @@ -0,0 +1,85 @@ +# This Dockerfile is now not used +# However it is left in as future reference for how to build from mavros source +# If that is ever needed in the future + +FROM ros:foxy-ros-base-focal AS builder + +WORKDIR /ros_ws + +# Install mavros dependencies +# COPY mavros/libmavconn/package.xml /ros_ws/src/mavros/libmavconn/package.xml +# COPY mavros/mavros/package.xml /ros_ws/src/mavros/mavros/package.xml +# COPY mavros/mavros_msgs/package.xml /ros_ws/src/mavros/mavros_msgs/package.xml +# COPY mavros/mavros_extras/package.xml /ros_ws/src/mavros/mavros_extras/package.xml + +RUN git clone --depth 1 --branch 2.3.0 https://github.com/mavlink/mavros.git /ros_ws/src/mavros + +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && apt-get update \ + && rosdep install -y --from-paths src --ignore-src . + +# Install mavros itself +# COPY mavros /ros_ws/src/mavros + +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && colcon build + +# Start the main image build +FROM ros:foxy-ros-core-focal AS image + +WORKDIR /ros_ws + +# Copy over the build artifacts +COPY --from=builder /ros_ws/install /ros_ws/install + +# Install exec dependencies +RUN rm install/COLCON_IGNORE \ + && apt-get update \ + && apt-get install -y --no-install-recommends python3-rosdep \ + && rosdep init \ + && rosdep update --rosdistro $ROS_DISTRO \ + && . /opt/ros/${ROS_DISTRO}/setup.sh \ + && rosdep install -t exec -y --from-paths install --ignore-src \ + && apt-get remove -y python3-rosdep \ + && SUDO_FORCE_REMOVE=yes apt-get autoremove -y \ + && rm -rf /var/lib/apt/lists/* + +# Part of MAVROS setup for lib geographic +## Ensure geographiclib is properly installed with FindGeographicLib available +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + libgeographic-dev \ + geographiclib-tools \ + && rm -rf /var/lib/apt/lists/* \ + && /ros_ws/install/mavros/lib/mavros/install_geographiclib_datasets.sh \ + && ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3.16/Modules/ + +COPY ros_entrypoint.sh /ros_entrypoint.sh + +COPY mavros_setup.sh . + +COPY mavros.launch.xml /ros_ws/launch/mavros.launch.xml +COPY config/mavros_config_*.yaml / +COPY config/mavros_pluginlists_*.yaml / + +ENV MAVROS_MOD_CONFIG_PATH="/mavros_config_mod.yaml" +ENV MAVROS_CONFIG_PATH="/mavros_config_px4.yaml" +ENV MAVROS_MOD_PLUGINLISTS_PATH="/mavros_pluginlists_mod.yaml" +ENV MAVROS_PLUGINLISTS_PATH="/mavros_pluginlists_px4.yaml" + +# Add custom ROS DDS configuration (force UDP always) +COPY fastrtps_profiles.xml /ros_ws/ +ENV FASTRTPS_DEFAULT_PROFILES_FILE /ros_ws/fastrtps_profiles.xml + +# TODO: Rename to FCU_PROTOCOL +ENV MAVROS_FCU_CONN="udp" +# TODO: Rename to FCU_HOST +ENV MAVROS_FCU_IP="127.0.0.1" +# TODO: Rename to PORT_BASE +ENV MAVROS_FCU_UDP_BASE="14830" +ENV MAVROS_TGT_SYSTEM="auto" +ENV PX4_INSTANCE_BASE=0 +ENV MAVROS_TGT_FIRMWARE="px4" +ENV MAVROS_GCS_URL="udp-pb://@:14550" + +CMD [ "ros2", "launch", "launch/mavros.launch.xml"] From 0a1f9fb0379f4d73b8ba9d65257601dc9c4699bf Mon Sep 17 00:00:00 2001 From: Mickey Li Date: Thu, 24 Nov 2022 12:33:18 +0000 Subject: [PATCH 20/25] Encorporated mavros extras --- system/mavros2/Dockerfile | 9 +++ system/mavros2/mavros.launch.xml | 18 ++++-- system/mavros2/ros_entrypoint.sh | 3 +- .../starling_mavros_extras/CMakeLists.txt | 56 +++++++++++++++++++ .../starling_mavros_extras/include/estop.hpp | 41 ++++++++++++++ .../starling_mavros_extras/include/ping.hpp | 41 ++++++++++++++ .../starling_mavros_extras/package.xml | 22 ++++++++ .../starling_mavros_extras/src/estop.cpp | 53 ++++++++++++++++++ .../starling_mavros_extras/src/ping.cpp | 45 +++++++++++++++ 9 files changed, 282 insertions(+), 6 deletions(-) create mode 100644 system/mavros2/starling_mavros_extras/CMakeLists.txt create mode 100644 system/mavros2/starling_mavros_extras/include/estop.hpp create mode 100644 system/mavros2/starling_mavros_extras/include/ping.hpp create mode 100644 system/mavros2/starling_mavros_extras/package.xml create mode 100644 system/mavros2/starling_mavros_extras/src/estop.cpp create mode 100644 system/mavros2/starling_mavros_extras/src/ping.cpp diff --git a/system/mavros2/Dockerfile b/system/mavros2/Dockerfile index b5b9d6f..c1f0ff3 100644 --- a/system/mavros2/Dockerfile +++ b/system/mavros2/Dockerfile @@ -5,6 +5,7 @@ WORKDIR /ros_ws # Install mavros from apt RUN apt-get update \ && apt-get install -y --no-install-recommends \ + libboost-system-dev \ wget \ python3-pip \ ros-${ROS_DISTRO}-mavros\ @@ -32,6 +33,14 @@ ENV MAVROS_CONFIG_PATH="/mavros_config_px4.yaml" ENV MAVROS_MOD_PLUGINLISTS_PATH="/mavros_pluginlists_mod.yaml" ENV MAVROS_PLUGINLISTS_PATH="/mavros_pluginlists_px4.yaml" +# Build extra custom nodes in Mavros +COPY starling_mavros_extras /ros_ws/src/starling_mavros_extras +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH \ + && cd /ros_ws \ + && colcon build --packages-select starling_mavros_extras \ + && rm -r build + # Add custom ROS DDS configuration (force UDP always) COPY fastrtps_profiles.xml /ros_ws/ ENV FASTRTPS_DEFAULT_PROFILES_FILE /ros_ws/fastrtps_profiles.xml diff --git a/system/mavros2/mavros.launch.xml b/system/mavros2/mavros.launch.xml index bb60a8d..b26718c 100644 --- a/system/mavros2/mavros.launch.xml +++ b/system/mavros2/mavros.launch.xml @@ -12,8 +12,8 @@ - - @@ -24,9 +24,17 @@ - - - + + + + + + + + + + + diff --git a/system/mavros2/ros_entrypoint.sh b/system/mavros2/ros_entrypoint.sh index 35daf8a..c6d05e9 100755 --- a/system/mavros2/ros_entrypoint.sh +++ b/system/mavros2/ros_entrypoint.sh @@ -4,7 +4,8 @@ set -e # setup ros2 environment source "/opt/ros/$ROS_DISTRO/setup.bash" -# source "/ros_ws/install/setup.bash" +# setup the local environment +source "/ros_ws/install/setup.bash" # Source the mavros_setup for any user defined edits to the environment source "/ros_ws/mavros_setup.sh" diff --git a/system/mavros2/starling_mavros_extras/CMakeLists.txt b/system/mavros2/starling_mavros_extras/CMakeLists.txt new file mode 100644 index 0000000..a4bac66 --- /dev/null +++ b/system/mavros2/starling_mavros_extras/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.5) +project(starling_mavros_extras) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_executable(estop src/estop.cpp include/estop.hpp) +ament_target_dependencies(estop + rclcpp std_msgs mavros_msgs +) +target_include_directories(estop PRIVATE include) + +add_executable(ping src/ping.cpp include/ping.hpp) +ament_target_dependencies(ping + rclcpp std_msgs +) +target_include_directories(ping PRIVATE include) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(TARGETS + estop + ping + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/system/mavros2/starling_mavros_extras/include/estop.hpp b/system/mavros2/starling_mavros_extras/include/estop.hpp new file mode 100644 index 0000000..df8eefa --- /dev/null +++ b/system/mavros2/starling_mavros_extras/include/estop.hpp @@ -0,0 +1,41 @@ +#ifndef ESTOP_HPP +#define ESTOP_HPP + +/* + * Trajectory follower + * Copyright (C) 2021 University of Bristol + * + * Author: Mickey Li (University of Bristol) + * + * Distributed under MIT License (available at https://opensource.org/licenses/MIT). + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + */ +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "mavros_msgs/srv/command_long.hpp" + + +class EstopHandler : public rclcpp::Node +{ + public: + EstopHandler(); + + private: + void emergency_stop(); + rclcpp::Client::SharedPtr mavros_command_srv; + rclcpp::Subscription::SharedPtr estop_sub; +}; + + +#endif diff --git a/system/mavros2/starling_mavros_extras/include/ping.hpp b/system/mavros2/starling_mavros_extras/include/ping.hpp new file mode 100644 index 0000000..f054684 --- /dev/null +++ b/system/mavros2/starling_mavros_extras/include/ping.hpp @@ -0,0 +1,41 @@ +#ifndef PING_HPP +#define PING_HPP + +/* + * Copyright (C) 2022 University of Bristol + * + * Author: Mickey Li (University of Bristol) + * + * Distributed under MIT License (available at https://opensource.org/licenses/MIT). + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + */ + +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include + +class PingHandler : public rclcpp::Node +{ + public: + PingHandler(); + + private: + void handle_ping(const std_msgs::msg::Header::SharedPtr s); + + rclcpp::Subscription::SharedPtr ping_sub; + rclcpp::Publisher::SharedPtr ping_pub; + + std::string vehicle_id; +}; + + +#endif \ No newline at end of file diff --git a/system/mavros2/starling_mavros_extras/package.xml b/system/mavros2/starling_mavros_extras/package.xml new file mode 100644 index 0000000..0845439 --- /dev/null +++ b/system/mavros2/starling_mavros_extras/package.xml @@ -0,0 +1,22 @@ + + + + starling_mavros_extras + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + mavros_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/system/mavros2/starling_mavros_extras/src/estop.cpp b/system/mavros2/starling_mavros_extras/src/estop.cpp new file mode 100644 index 0000000..219fa0a --- /dev/null +++ b/system/mavros2/starling_mavros_extras/src/estop.cpp @@ -0,0 +1,53 @@ +/* + * Trajectory follower + * Copyright (C) 2021 University of Bristol + * + * Author: Mickey Li (University of Bristol) + * + * Distributed under MIT License (available at https://opensource.org/licenses/MIT). + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + */ +#include "estop.hpp" + +EstopHandler::EstopHandler() : + Node("estop_handler") +{ + this->mavros_command_srv = this->create_client("mavros/cmd/command", rmw_qos_profile_services_default); + this->estop_sub = this->create_subscription( + "/emergency_stop", 1, + [this](const std_msgs::msg::Empty::SharedPtr s){ + (void)s; + this->emergency_stop(); + } + ); + + RCLCPP_INFO(this->get_logger(), "ESTOP Initialised"); +} + +void EstopHandler::emergency_stop() { + RCLCPP_ERROR(this->get_logger(), "EMERGENCY STOP RECIEVED, SENDING KILL MESSAGE TO AUTOPILOT"); + // Kill Drone Rotors + auto commandCall = std::make_shared(); + commandCall->broadcast = false; + commandCall->command = 400; + commandCall->param2 = 21196.0; + while (!this->mavros_command_srv->wait_for_service(std::chrono::duration(0.1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for command service. Exiting."); + throw std::runtime_error("Interrupted while waiting for command service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + } + auto result_future = this->mavros_command_srv->async_send_request(commandCall); +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto handler = std::make_shared(); + rclcpp::spin(handler); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/system/mavros2/starling_mavros_extras/src/ping.cpp b/system/mavros2/starling_mavros_extras/src/ping.cpp new file mode 100644 index 0000000..10e7f5f --- /dev/null +++ b/system/mavros2/starling_mavros_extras/src/ping.cpp @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2021 University of Bristol + * + * Author: Mickey Li (University of Bristol) + * + * Distributed under MIT License (available at https://opensource.org/licenses/MIT). + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + */ +#include "ping.hpp" + +PingHandler::PingHandler() : + Node("ping_handler") +{ + this->declare_parameter("vehicle_id"); + rclcpp::Parameter id_param = this->get_parameter("vehicle_id"); + this->vehicle_id = id_param.as_string(); + + this->ping_sub = this->create_subscription( + "/ping_source", 1, + [this](const std_msgs::msg::Header::SharedPtr s){ + this->handle_ping(s); + } + ); + + this->ping_pub = this->create_publisher("/ping_sink", 10); + + RCLCPP_INFO(this->get_logger(), "Ping Initialised"); +} + +void PingHandler::handle_ping(const std_msgs::msg::Header::SharedPtr s) { + std_msgs::msg::Header msg; + msg.frame_id = this->vehicle_id; + msg.stamp = s->stamp; + this->ping_pub->publish(msg); +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto handler = std::make_shared(); + rclcpp::spin(handler); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From a8d4cd05ab661c7f95ab769f47958fc9d1f513ff Mon Sep 17 00:00:00 2001 From: Mickey Li Date: Thu, 24 Nov 2022 15:38:35 +0000 Subject: [PATCH 21/25] Fixed /mavros subnamespacing issue and updated README --- .../config/mavros_pluginlists_px4.yaml | 27 +++++++++---------- system/mavros2/mavros.launch.xml | 8 +++--- system/mavros2/mavros_setup.sh | 4 +-- 3 files changed, 18 insertions(+), 21 deletions(-) diff --git a/system/mavros2/config/mavros_pluginlists_px4.yaml b/system/mavros2/config/mavros_pluginlists_px4.yaml index 7ec5484..c31ae3c 100644 --- a/system/mavros2/config/mavros_pluginlists_px4.yaml +++ b/system/mavros2/config/mavros_pluginlists_px4.yaml @@ -3,27 +3,26 @@ mavros: component_id: 191 plugin_denylist: - "*" - plugin_allowlist: - - 'sys_*' - - altitude + plugin_allowlist: + # - altitude - command - distance_sensor # - ftp - - global_position - - imu + # - global_position + # - imu - local_position - - manual_control + # - manual_control # - param - - px4flow + # - px4flow # - rc_io - - setpoint_accel - - setpoint_attitude + # - setpoint_accel + # - setpoint_attitude - setpoint_position - - setpoint_raw - - setpoint_velocity + # - setpoint_raw + # - setpoint_velocity - sys_status - sys_time - vision_pose - - onboard_computer_status - - vibration - + - log_transfer + # - onboard_computer_status + # - vibration diff --git a/system/mavros2/mavros.launch.xml b/system/mavros2/mavros.launch.xml index b26718c..9d3b4a6 100644 --- a/system/mavros2/mavros.launch.xml +++ b/system/mavros2/mavros.launch.xml @@ -15,7 +15,7 @@ + namespace="$(var vehicle_namespace)/mavros"> @@ -24,9 +24,9 @@ - - - + + + diff --git a/system/mavros2/mavros_setup.sh b/system/mavros2/mavros_setup.sh index 07ffd4c..7e40054 100755 --- a/system/mavros2/mavros_setup.sh +++ b/system/mavros2/mavros_setup.sh @@ -120,9 +120,7 @@ MAVROS_MOD_PLUGINLISTS_PATH=${MAVROS_MOD_PLUGINLISTS_PATH:-"/mavros_pluginlists_ MAVROS_PLUGINLISTS_PATH=${MAVROS_PLUGINLISTS_PATH:-"/mavros_pluginlists_px4.yaml"} if [ ! -f "$MAVROS_MOD_PLUGINLISTS_PATH" ]; then echo "Modified Mavros Pluginlist for $VEHICLE_NAMESPACE does not exist at $MAVROS_PLUGINLISTS_PATH. Generating specialised configuration for launch." - sed -E "s#mavros#/$VEHICLE_NAMESPACE/mavros#g" $MAVROS_PLUGINLISTS_PATH > $MAVROS_MOD_PLUGINLISTS_PATH + sed -E "s#mavros#/$VEHICLE_NAMESPACE/mavros/mavros#g" $MAVROS_PLUGINLISTS_PATH > $MAVROS_MOD_PLUGINLISTS_PATH else echo "Modified Mavros Pluginlist for $VEHICLE_NAMESPACE already exists at $MAVROS_MOD_PLUGINLISTS_PATH. Using for launch" fi - - From 0743a563e828acef42faf8f2ef9ee1ba536bb2d9 Mon Sep 17 00:00:00 2001 From: Mickey Li Date: Thu, 24 Nov 2022 15:38:47 +0000 Subject: [PATCH 22/25] Updated README.md --- system/mavros2/README.md | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/system/mavros2/README.md b/system/mavros2/README.md index 0536f7d..ff377f8 100644 --- a/system/mavros2/README.md +++ b/system/mavros2/README.md @@ -1,7 +1,6 @@ -# `starling-mavros` Container +# `starling-mavros2` Container -This container holds both MAVROS and a ROS1/2 bridge. In the future, we expect to use the ROS2 version of MAVROS which -will eliminate the need for the ROS1/2 bridge. +This container is the native ROS2 MAVROS container. Most of the below still holds. ## Contents [TOC] @@ -48,7 +47,7 @@ There are two docker files: ## Configuration -We have identified that mavros is a base requirement for any drone running onboard compute in order to communicate with both the autopilot and GCS. +We have identified that mavros is a base requirement for any drone running onboard compute in order to communicate with both the autopilot and GCS. The mavros container exposes an environment variable `MAVROS_FCU_URL` which is passed to mavros's `fcu_url` option for configuring the mavlink connection to mavros. The default value is: > `MAVROS_FCU_URL=udp://127.0.0.1:14550@14555` @@ -138,3 +137,11 @@ The container is configured based on the detected ordinal from the hostname. ### Running isolated Default values will be used, equivalent to the Kubernetes case with `ORDINAL=0` + +### Weird Namespacing for mavros pluginlists + +In ROS2 a parameter file has an annoying specific format. The first entry should be the name of the node which the following ros2 parameters apply to. This must directly map to the node name. + +We would like our node to be under `/mavros/...`. But by default the topics all come out on `...` essentially root. Therefore in the `mavros.launch.xml` we set the mavros node to use namespace `/mavros`. However in order for the param file to match, we need to match the root mavros node name which is `/mavros/mavros`. + +Therefore the top of `mavros_pluginlists_px4.yaml` must specify the node name `/mavros/mavros`. The template pluginlists only specifies `mavros`, so the `mavros_setup.sh` dynamically renames the pluginlist yaml file with the correct namespace. From e07eac2dff5bf9067454ee98488438875a370fce Mon Sep 17 00:00:00 2001 From: Rob Clarke Date: Thu, 1 Dec 2022 16:39:41 +0000 Subject: [PATCH 23/25] Use BAKE_PREFIX in mavros2 & ping-monitor targets --- buildtools/docker-bake.hcl | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/buildtools/docker-bake.hcl b/buildtools/docker-bake.hcl index 6cc1b9b..367d9e5 100644 --- a/buildtools/docker-bake.hcl +++ b/buildtools/docker-bake.hcl @@ -55,12 +55,12 @@ group "system" { target "ping-monitor" { context = "system/ping_monitor" tags = [ - "${BAKE_REGISTRY}uobflightlabstarling/ping-monitor:${BAKE_VERSION}", - notequal("",BAKE_RELEASENAME) ? "${BAKE_REGISTRY}uobflightlabstarling/ping-monitor:${BAKE_RELEASENAME}": "", + "${BAKE_REGISTRY}${BAKE_PREFIX}/ping-monitor:${BAKE_VERSION}", + notequal("",BAKE_RELEASENAME) ? "${BAKE_REGISTRY}${BAKE_PREFIX}/ping-monitor:${BAKE_RELEASENAME}": "", ] platforms = ["linux/amd64"] - cache-to = [ notequal("",BAKE_CACHETO_NAME) ? "${BAKE_CACHETO_REGISTRY}uobflightlabstarling/ping-monitor:${BAKE_CACHETO_NAME}" : "" ] - cache-from = [ notequal("",BAKE_CACHEFROM_NAME) ? "${BAKE_CACHEFROM_REGISTRY}uobflightlabstarling/ping-monitor:${BAKE_CACHEFROM_NAME}" : "" ] + cache-to = [ notequal("",BAKE_CACHETO_NAME) ? "type=registry,mode=max,ref=${BAKE_CACHETO_REGISTRY}${BAKE_PREFIX}/ping-monitor:${BAKE_CACHETO_NAME}" : "" ] + cache-from = [ notequal("",BAKE_CACHEFROM_NAME) ? "${BAKE_CACHEFROM_REGISTRY}${BAKE_PREFIX}/ping-monitor:${BAKE_CACHEFROM_NAME}" : "" ] } target "rosbridge-suite" { @@ -100,12 +100,12 @@ target "starling-mavros2" { context = "system/mavros2" dockerfile = "Dockerfile" tags = [ - "${BAKE_REGISTRY}uobflightlabstarling/starling-mavros2:${BAKE_VERSION}", - notequal("",BAKE_RELEASENAME) ? "${BAKE_REGISTRY}uobflightlabstarling/starling-mavros2:${BAKE_RELEASENAME}": "", + "${BAKE_REGISTRY}${BAKE_PREFIX}/starling-mavros2:${BAKE_VERSION}", + notequal("",BAKE_RELEASENAME) ? "${BAKE_REGISTRY}${BAKE_PREFIX}/starling-mavros2:${BAKE_RELEASENAME}": "", ] platforms = ["linux/amd64", "linux/arm64"] - cache-to = [ notequal("",BAKE_CACHETO_NAME) ? "${BAKE_CACHETO_REGISTRY}uobflightlabstarling/starling-mavros2:${BAKE_CACHETO_NAME}" : "" ] - cache-from = [ notequal("",BAKE_CACHEFROM_NAME) ? "${BAKE_CACHEFROM_REGISTRY}uobflightlabstarling/starling-mavros2:${BAKE_CACHEFROM_NAME}" : "" ] + cache-to = [ notequal("",BAKE_CACHETO_NAME) ? "type=registry,mode=max,ref=${BAKE_CACHETO_REGISTRY}${BAKE_PREFIX}/starling-mavros2:${BAKE_CACHETO_NAME}" : "" ] + cache-from = [ notequal("",BAKE_CACHEFROM_NAME) ? "${BAKE_CACHEFROM_REGISTRY}${BAKE_PREFIX}/starling-mavros2:${BAKE_CACHEFROM_NAME}" : "" ] } target "mavp2p" { From a5f52766f10a18f8bf6d4fc2f4307f3b810147e3 Mon Sep 17 00:00:00 2001 From: Mickey Li Date: Sun, 4 Dec 2022 16:58:09 +0000 Subject: [PATCH 24/25] Updated geographiclib install to use local --- system/mavros2/Dockerfile | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/system/mavros2/Dockerfile b/system/mavros2/Dockerfile index c1f0ff3..cb43549 100644 --- a/system/mavros2/Dockerfile +++ b/system/mavros2/Dockerfile @@ -16,9 +16,8 @@ RUN apt-get update \ && rm -rf /var/lib/apt/lists/* ## Ensure geographiclib is properly installed with FindGeographicLib available -RUN wget https://raw.githubusercontent.com/mavlink/mavros/ros2/mavros/scripts/install_geographiclib_datasets.sh \ - && bash install_geographiclib_datasets.sh \ - && ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3.16/Modules/ +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && ros2 run mavros install_geographiclib_datasets.sh COPY ros_entrypoint.sh /ros_entrypoint.sh From 3ce704850124ad2936c2b9d722baac4abf0a9e1c Mon Sep 17 00:00:00 2001 From: Mickey Li Date: Sun, 4 Dec 2022 18:11:07 +0000 Subject: [PATCH 25/25] Moved mavros docs to main docs, linked to from local readme. Removed references to ros1_bridge --- docs/docker-images/mavros2.md | 239 ++++++++++++++++++ mkdocs.yml | 1 + system/mavros/README.md | 139 +--------- system/mavros2/README.md | 146 +---------- .../mavros2/config/mavros_pluginlists_ap.yaml | 63 +++-- system/mavros2/mavros.launch.xml | 4 - 6 files changed, 289 insertions(+), 303 deletions(-) create mode 100644 docs/docker-images/mavros2.md diff --git a/docs/docker-images/mavros2.md b/docs/docker-images/mavros2.md new file mode 100644 index 0000000..a7a3c00 --- /dev/null +++ b/docs/docker-images/mavros2.md @@ -0,0 +1,239 @@ +# `starling-mavros2` Container + +This container is the core Starling container. It contains the primary interface between the flight controller (simulated or real) and the users application written in ROS2. + +This container is mainly comprised of the MAVROS, MAVLINK/ROS2 ros node which provides a standard ROS2 interface to the user. This container performs an automated set of modifications to allow easy connection to SITL or the real vehicle. It also contains a set of general nodes which should be part of every user application. + +## Contents + +[TOC] + +## Overview + +The image is a wrapper around MAVROS, as such, most of the documentation for MAVROS is of use when working with this +image. The image has both `mavros` and `mavros-extras` installed so all plugins should be available. + +The published topics are namespaced to allow for running multiple vehicles on the same ROS network. By default, this +namespace will be `/vehicle_N`, where `N` is the MAVLink system ID of the vehicle. MAVROS topics are published within +this namespace, *e.g.* `/vehicle_1/mavros/state`. + +The image has been setup to automatically configure itself in some scenarios: + + - Running on a vehicle + - Running in Kubernetes + +There are also some additional general nodes running in the background. In particular: + + - Emergency Stop listener on the `/emergency_stop` topic of type `std_msgs/msg/Empty`. It is hard-wired to forcefully disarm the vehicle (stop motors) for both PX4 and Ardupilot in simulation and reality. + - Ping Listener on the `/ping_source` and `/ping_sink` topics which are used in conjunction with the `ping-monitor` to measure vehicle round trip time. + + +## Launch Process + +The initial process of starting the container goes through `ros_entrypoint.sh`. This file sources ROS2 and +runs a setup script `mavros_setup.sh` which configures the environment to tell MAVROS how to talk to the drone and where to publish its +topics. The setup script works in a couple of different ways depending on where the container is running. + +- If the container is running on a drone, it expects to be able to find the `/etc/starling/vehicle.config` file. This filecontains some information that MAVROS needs to be able to communicate with the flight controller. An example `vehicle.config` file is included below. + +If the container is started as part of a Kubernetes StatefulSet deployment, the setup script will start a MAVROS2 Container instance based on the set numerical given by Kubernetes. + +To build this container clone the ProjectStarling repo and cd in system. + +```bash +cd system +make mavros2 # Builds docker container +docker run -it --rm uobflightlabstarling/starling-mavros2:latest +``` + +Default action of launching the container (make run) will be to launch the `mavros.launch.xml`. + +## Launch file + +The launch file `mavros.launch.xml` runs the following +Running the container will run the following ros nodes in parallel: + +This launch file starts the following: + +1. Mavros2 ROS2 node +2. The EStop Listener +3. The Ping Responder (To measure Round Trip Time) + + +## Contents + +The container is built out of `Dockerfile` which is the base container which is based on Ubuntu 20.04. It runs ROS2 Foxy and MAVROS 2 with extra nodes. + +> The `buildfromsource.Dockerfile` is an example Dockerfile for building MAVROS2 from source which has been kept for reference. + +The other files include: + +- `mavros_setup.sh` is sourced at the end of the Dockerfile if any environment variables need modification. +- `mavros.launch.xml` is a ros2 launch file, and launches both mavros and the extra nodes. It has 4 main parameters: + - `fcu_url` - Flight control url, point it at the physical autopilot, ardupilot sitl or px4 sitl, no default in launch file. Defailts to `MAVROS_FCU_URL="udp://127.0.0.1:14550@14555"` using launch.sh + - `target_system` - target id to filter for from mavlink stream, defaults to 1, or MAVROS_TGT_SYSTEM using launch.sh + - `system_id` - sysid of mavros node, defaults to 1 + - `firmware` - `px4` or `apm`, name of the mavros launch file. Defaults to `px4`. +- `config` folder contains the parameter files which `mavros.launch.xml` reads in to setup what mavros2 functionality is available and how it is configured. See below. +- `starling_mavros_extras` folder contains the source code for any extra ROS2 nodes we wish to run inside this core container. +- `fastrtps_profiles.xml` is a ROS2 configuration file which specifies that it should always use UDP for sending data. + +## Configuration + +We have identified that mavros is a base requirement for any drone running onboard compute in order to communicate with bot h the autopilot and GCS. + +The mavros container exposes an environment variable `MAVROS_FCU_URL` which is passed to mavros's `fcu_url` option for configuring the mavlink connection to mavros. The default value is: +> `MAVROS_FCU_URL=udp://127.0.0.1:14550@14555` + +It also exposes environment variabel `MAVROS_TGT_SYSTEM` which is the `SYSID` of the ardupilot instance that mavros wants to connect to. This is usually an integer value. However, an IP address can also be passed in and `mavros_setup.sh` will extract the last numver of the IPv4 address as the value. Default is 1 + +To modify this at runtime, pass the `-e MAVROS_FCU_URL=` to `docker run` like so +> `docker run -e MAVROS_FCU_URL= -e MAVROS_TGT_SYSTEM=<#n>...` + +See Mavros for possible connection values + +It to gets the +ordinal of its containing pod from the hostname. It will then use this ordinal to set up the ports and the system ID to +match those generated by a PX4 SITL instance set up with the same ordinal. If the setup script fails to get the ordinal +from the hostname, it will attempt to connect to a PX4 SITL with `PX4_INSTANCE=0`. + +Once the setup script has run, the default behaviour is to launch the `mavros.launch.xml` file. This behaviour +should be usable in almost all cases. + +The __ROS2__ `mavros.launch.xml` script defines a set of arguments to enable configuration of the MAVROS node. +These are ususally filled by the environment variables defined below. If the configurability provided here is +insufficient, the image can be run with a different command. + +In addition there exists the configuration parameter files in `config` which are used by mavros to specify which mavros plugins are running, and their associated parameters. These configuration files are copied from `config` into the root directory of the container (`/`). See below for how to use custom configurations. + +During runtime, the `mavros_setup.sh` produces a modified version of these files to ensure that vehicle namespacing is understood. These are suffixed with `_mod` and should not be modified. + +## Configuration Options + +There are many configuration options available for this image to allow it to be used flexibly across different +deployment scenarios. The most important of these are those relating to the vehicle and GCS connections, and MAVROS +configuration. A full summary is given in the table below. + +Name | Default Value | Description +--------------------------|--------------------------------|------------ +`MAVROS_FCU_CONN` | "udp" | Protocol for autogenerated FCU URL +`MAVROS_FCU_IP` | "127.0.0.1" | IP for autogenerated FCU_URL +`MAVROS_FCU_UDP_BASE` | "14830" | Base port for autogenerated FCU_URL +`MAVROS_TGT_SYSTEM` | "auto" | Target system ID, if set to a number, this will __override__ the automatic behaviour +`PX4_INSTANCE_BASE` | 0 | Base instance for autogenerated instance matching +`MAVROS_TGT_FIRMWARE` | "px4" | Firmware profile used by MAVROS. Only other valid value currently is "apm" +`MAVROS_GCS_URL` | "udp-pb://@:14550" | MAVROS URL for ground control station connection +`MAVROS_FCU_URL` | {unset} | MAVROS URL for FCU connection. Set to __override__ automatic behaviour +`VEHICLE_NAMESPACE` | {unset} | Namespace for mavros topics. Set to __override__ default value of `vehicle_${TGT_SYSTEM}` +`MAVROS_PLUGINLISTS_PATH` | "/mavros_pluginlists_px4.yaml" | Path for MAVROS pluginlists file +`MAVROS_CONFIG_PATH` | "/mavros_config_px4.yaml" | Path for initial MAVROS configuration file +`MAVROS_MOD_CONFIG_PATH` | "/mavros_config_mod.yaml" | Path for modified MAVROS config to be written to + +### Configuring the Connection + +MAVROS is told to connect to a MAVLink source with a URL. This URL can be configured in a number of ways. The most +straightforward is to simply set the `MAVROS_FCU_URL` environment variable. This will override any other behaviour. + +```sh +docker run -e MAVROS_FCU_URL=serial:///dev/ttyUSB0:115200 uobflightlabstarling/starling-mavros +``` + +When setting `MAVROS_FCU_URL` directly, note that a query string (_e.g._ `?ids=n,240`) will be added during launch. +You need to ensure that your input for `MAVROS_FCU_URL` supports this syntax. Of particular note is the need for a +trailing `/` in most formats, but not for the `serial://` format. Also note that the plain file format does not support +this. See [the MAVROS docs](https://github.com/mavlink/mavros/blob/master/mavros/README.md#connection-url) for more +information on URL formats. + +`MAVROS_FCU_URL` is can also be set automatically by the container depending on its environment. If there is a +`vehicle.config` file mounted in the image, the value of `VEHICLE_FCU_URL` from that file will be used as the `fcu_url`. +This is especially useful when deploying the container onto physical vehicles. The same warning about trailing slashes +above goes for the value put in `vehicle.config`. + +If there is no `vehicle.config` file, the image will configure itself based on the values of `MAVROS_FCU_CONN`, +`MAVROS_FCU_IP` and `MAVROS_FCU_UDP_BASE`. An additional parameter, `INSTANCE` is also used in the construction of the +URL. This is generated based on the container hostname and is intended for use in Kubernetes deployments to distinguish +multiple instances. `PX4_INSTANCE_BASE` can be used to offset the `fcu_url` will be constructed as below: +> `$MAVROS_FCU_CONN://$MAVROS_FCU_IP:$((MAVROS_FCU_UDP_BASE + INSTANCE))@/` + +With all values at default, this ends up as: +> `udp://127.0.0.1:14830@/` + +### Configuring the Target System + +Another important configuration option is the target system ID. This controls the target system that MAVROS sends in +some messages. As for the `fcu_url`, the value can be explicitly overridden, this time using the `MAVROS_TGT_SYSTEM` +environment variable. Setting this will overrise all other values. If it is set to an invalid system ID, MAVROS will +be set to use a target ID of `1`. + +If the environment variable is left at its default value of `auto`, a similar flow to the `fcu_url` occurs: if a +`vehicle.config` file exists, the value of `VEHICLE_MAVLINK_SYSID` from that file will be used. Otherwise, the value is +autogenerated based on the `INSTANCE` parameter derived from the container hostname. Note that the `INSTANCE` number is +0-indexed, while MAVLink system IDs start at `1`. Therefore, the system ID is set to one more than the `INSTANCE`. + +### Configuring the MAVROS Configuration + +Two sets of `config.yaml` and `pluginlists.yaml` files are installed in the root directory to provide alternatives for +PX4 and ArduPilot autopilots. These are named: `/mavros_config_px4.yaml` and `/mavros_pluginlists_px4.yaml` for the PX4 +versions and `/mavros_config_ap.yaml` and `/mavros_pluginlists_ap.yaml` for the ArduPilot versions. + +The easiest way to choose between the two is to set the `MAVROS_CONFIG_PATH` and `MAVROS_PLUGINLISTS_PATH` environment +variables. By default these point to the PX4 versions. To use the ArduPilot versions set both variables as below: + +```sh +docker run -e MAVROS_CONFIG_PATH=/mavros_config_ap.yaml -e MAVROS_PLUGINLISTS_PATH=/mavros_pluginlists_ap.yaml ... +``` + +It is also possible to mount alternative configurations into the image and use the environment variables to configure +MAVROS with them. + +TODO: Example of mounted configuration + +## Example `vehicle.config` + +Note that the extended form of the serial URL is required for MAVROS's target "query string" to work. + +```bash +VEHICLE_FCU_URL=serial:///dev/px4fmu:115200 +VEHICLE_FIRMWARE=px4 +VEHICLE_MAVLINK_SYSID=23 +VEHICLE_NAME=clover23 +``` + +## Implementation Details + +### Running on a vehicle + +Ensure `/etc/starling/vehicle.config` is mounted. The container is then configured from the contents of that file. + +### Running under Kubernetes StatefulSet + +The container is configured based on the detected ordinal from the hostname. + +`MAVROS_FCU_URL` is autogenerated as: `udp://127.0.0.1:$((14830 + ORDINAL))@` + +`MAVROS_TGT_SYSTEM` will end up as `$((ORDINAL + 1))` + +### Running isolated + +Default values will be used, equivalent to the Kubernetes case with `ORDINAL=0` + +### Using custom mavros configurations + +By default we have only enabled a subset of all the mavros plugins which are available. This is so that we limit the data flow. If your application requires other plugins, this can really easily be done. + +First make a local copy of the `mavros_pluginlists....yaml` file and modify it to what you wish. The `plugin_denylist` specifies a list of plugins which should not run. The `plugin_allowlist` speicifes a list of plugins which should be run. Make sure not to change the name of the parameters (`mavros`). Then either manually using bind mounting, or within the docker-compose file, you should mount this file at `/mavros_pluginlists_px4.yaml`. This overwrites the default file. + +### Weird Namespacing for mavros pluginlists + +In ROS2 a parameter file has an annoying specific format. The first entry should be the name of the node which the following ros2 parameters apply to. This must directly map to the node name. + +We would like our node to be under `/mavros/...`. But by default the topics all come out on `...` essentially root. Therefore in the `mavros.launch.xml` we set the mavros node to use namespace `/mavros`. However in order for the param file to match, we need to match the root mavros node name which is `/mavros/mavros`. + +Therefore the top of `mavros_pluginlists_px4.yaml` must specify the node name `/mavros/mavros`. The template pluginlists only specifies `mavros`, so the `mavros_setup.sh` dynamically renames the pluginlist yaml file with the correct namespace. + +## Advanced Topics + +### Adding additional MAVROS plugins + +This should be possible by mounting a volume with your plugin into the container. Assuming ROS tools are able to find +it, MAVROS should load the plugin (if directed by the pluginlists file). \ No newline at end of file diff --git a/mkdocs.yml b/mkdocs.yml index 7983d50..3823f5e 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -57,6 +57,7 @@ nav: - "Image Documentation": - "starling-ui": "docker-images/ui.md" - "starling-mavros": "docker-images/mavros.md" + - "starling-mavros2": "docker-images/mavros2.md" - "starling-controller-base": "docker-images/controller-base.md" - "starling-sim-base-core": "docker-images/sim-base-core.md" - "starling-sim-base-px4": "docker-images/sim-base-px4.md" diff --git a/system/mavros/README.md b/system/mavros/README.md index 0536f7d..c7e2f09 100644 --- a/system/mavros/README.md +++ b/system/mavros/README.md @@ -1,140 +1,3 @@ # `starling-mavros` Container -This container holds both MAVROS and a ROS1/2 bridge. In the future, we expect to use the ROS2 version of MAVROS which -will eliminate the need for the ROS1/2 bridge. - -## Contents -[TOC] - -## Launch Process - -The initial process of starting the container goes through `ros_entrypoint.sh`. This file sets up both ROS versions and -runs a setup script which configures the environment to tell MAVROS how to talk to the drone and where to publish its -topics. The setup script works in a couple of different ways depending on where the container is running. - -If the container is running on a drone, it expects to be able to find the `/etc/starling/vehicle.config` file. This file -contains some information that MAVROS needs to be able to communicate with the flight controller. An example -`vehicle.config` file is included below. - -If the container is started as part of a Kubernetes StatefulSet deployment, the setup script will attemp# Mavros /ROS12 Bridge Container - -To run: -```bash -make build # Builds docker container -make run # Runs docker container -``` - -Default action of launching the container (make run) will be to run `launch.sh` which will source mavros_setup.sh, and run the launch file. - -## Launch file - -The launch file `mavros_bridge.launch` runs the following -Running the container will run 2 ros nodes in parallel: -1. A Mavros node listening on fcu_url, targeting drone with sysid tgt_system - - `roslaunch mavros apm.launch fcu_url:=${MAVROS_FCU_URL} tgt_system:=${MAVROS_TGT_SYSTEM}` -2. A ros1 bridge node with option to forward all mavros topics to ros2 - - `ros2 run ros1_bridge dynamic_bridge --bridge-all-1to2-topics` - -## Contents -There are two docker files: - -- `Dockerfile.mavros` is the base container which is based on Ubuntu 20.04. It runs ROS2 Foxy, ROS1 Noetic and MAVROS via the ROS1_bridge. -- `mavros_setup.sh` is sourced at the end of the Dockerfile if any environment variables need modification. -- `mavros_bridge.launch` is a ros1 launch file, and launches both mavros and the ros1 bridge. It has 4 parameters: - - fcu_url - Flight control url, point it at the physical autopilot, ardupilot sitl or px4 sitl, no default in launch file. Defailts to `MAVROS_FCU_URL="udp://127.0.0.1:14550@14555"` using launch.sh - - target_system - target id to filter for from mavlink stream, defaults to 1, or MAVROS_TGT_SYSTEM using launch.sh - - system_id - sysid of mavros node, defaults to 1 - - firmware - `px4` or `apm`, name of the mavros launch file. Defaults to `px4`. - -## Configuration - -We have identified that mavros is a base requirement for any drone running onboard compute in order to communicate with both the autopilot and GCS. - -The mavros container exposes an environment variable `MAVROS_FCU_URL` which is passed to mavros's `fcu_url` option for configuring the mavlink connection to mavros. The default value is: -> `MAVROS_FCU_URL=udp://127.0.0.1:14550@14555` - -It also exposes environment variabel `MAVROS_TGT_SYSTEM` which is the `SYSID` of the ardupilot instance that mavros wants to connect to. This is usually an integer value. However, an IP address can also be passed in and `mavros_setup.sh` will extract the last numver of the IPv4 address as the value. Default is 1 - -To modify this at runtime, pass the `-e MAVROS_FCU_URL=` to `docker run` like so -> `docker run -e MAVROS_FCU_URL= -e MAVROS_TGT_SYSTEM=<#n>...` - -See Mavros for possible connection values - -This container builds the ros1_bridge from scratch in order to incorporate the `mavros_msgs` package. -t to get the -ordinal of its containing pod from the hostname. It will then use this ordinal to set up the ports and the system ID to -match those generated by a PX4 SITL instance set up with the same ordinal. If the setup script fails to get the ordinal -from the hostname, it will attempt to connect to a PX4 SITL with `PX4_INSTANCE=0`. - -Once the setup script has run, the default behaviour is to launch the `mavros_bridge.launch.xml` file. This behaviour -should be usable in almost all cases. This is a __ROS2__ launch file. It instructs `ros2 launch` to run the -`ros1_bridge` node and an instance of __ROS1__'s `roslaunch`. This in turn launches the __ROS1__ `mavros.launch` file, -which contains instructions to run the MAVROS node. - -The __ROS2__ `mavros_bridge.launch.xml` script defines a set of arguments to enable configuration of the MAVROS node. -These are ususally filled by the environment variables defined below. If the configurability provided here is -insufficient, the image can be run with a different command. - -## Environment Variables - -Name | Default Value | Description -----------------------|--------------------|------------ -`MAVROS_FCU_CONN` | "udp" | Protocol for autogenerated FCU URL -`MAVROS_FCU_IP` | "127.0.0.1" | IP for autogenerated FCU_URL -`MAVROS_FCU_UDP_BASE` | "14830" | Base port for autogenerated FCU_URL -`MAVROS_TGT_SYSTEM` | "auto" | Target system ID, if set to a number, this will __override__ the automatic behaviour -`PX4_INSTANCE_BASE` | 0 | Base instance for autogenerated instance matching -`MAVROS_TGT_FIRMWARE` | "px4" | Firmware profile used by MAVROS. Only other valid value currently is "apm" -`MAVROS_GCS_URL` | "udp-pb://@:14550" | MAVROS URL for ground control station connection -`MAVROS_FCU_URL` | {unset} | MAVROS URL for FCU connection. Set to __override__ automatic behaviour -`VEHICLE_NAMESPACE` | {unset} | Namespace for mavros topics. Set to __override__ default value of `vehicle_${TGT_SYSTEM}` - -`MAVROS_FCU_URL` is autogenerated as: -> `$MAVROS_FCU_CONN://$MAVROS_FCU_IP:$((MAVROS_FCU_UDP_BASE + INSTANCE))@/` - -If no Kubernetes deployment is detected, this ends up as: -> `udp://127.0.0.1:14830@/` - -Similarly, if no Kubernetes is detected, `MAVROS_TGT_SYSTEM` will end up as `1`, - -When setting `MAVROS_FCU_URL` manually, remember that a query string (_e.g._ `?ids=n,240`) will be added during launch. -You need to ensure that your input for `MAVROS_FCU_URL` supports this syntax. Of particular note is the need for a -trailing `/` in most formats, but not for the `serial://` format. Also note that the plain file format does not support -this. See [the MAVROS docs](https://github.com/mavlink/mavros/blob/master/mavros/README.md#connection-url) for more -information on URL formats. - -## Build Process - -At the time of writing, there is not a binary distribution available for `mavros_msgs` under ROS2. Therefore, this -package needs to be built from source. In addition, to ensure support for custom message types, `ros1_bridge` also needs -to be built from source. The build process also includes some additional steps to complete the installation of MAVROS -under ROS1. - -## Example `vehicle.config` - -Note that the extended form of the serial URL is required for MAVROS's target "query string" to work. - -```bash -VEHICLE_FCU_URL=serial:///dev/px4fmu:115200 -VEHICLE_FIRMWARE=px4 -VEHICLE_MAVLINK_SYSID=23 -VEHICLE_NAME=clover23 -``` - -## Behaviour Notes - -### Running on a vehicle - -Ensure `/etc/starling/vehicle.config` is mounted. The container is then configured from the contents of that file. - -### Running under Kubernetes StatefulSet - -The container is configured based on the detected ordinal from the hostname. - -`MAVROS_FCU_URL` is autogenerated as: `udp://127.0.0.1:$((14830 + ORDINAL))@` - -`MAVROS_TGT_SYSTEM` will end up as `$((ORDINAL + 1))` - -### Running isolated - -Default values will be used, equivalent to the Kubernetes case with `ORDINAL=0` +Please see [docs for more details](https://docs.starlinguas.dev/docker-images/mavros/) \ No newline at end of file diff --git a/system/mavros2/README.md b/system/mavros2/README.md index ff377f8..34ba24e 100644 --- a/system/mavros2/README.md +++ b/system/mavros2/README.md @@ -1,147 +1,3 @@ # `starling-mavros2` Container -This container is the native ROS2 MAVROS container. Most of the below still holds. - -## Contents -[TOC] - -## Launch Process - -The initial process of starting the container goes through `ros_entrypoint.sh`. This file sets up both ROS versions and -runs a setup script which configures the environment to tell MAVROS how to talk to the drone and where to publish its -topics. The setup script works in a couple of different ways depending on where the container is running. - -If the container is running on a drone, it expects to be able to find the `/etc/starling/vehicle.config` file. This file -contains some information that MAVROS needs to be able to communicate with the flight controller. An example -`vehicle.config` file is included below. - -If the container is started as part of a Kubernetes StatefulSet deployment, the setup script will attemp# Mavros /ROS12 Bridge Container - -To run: -```bash -make build # Builds docker container -make run # Runs docker container -``` - -Default action of launching the container (make run) will be to run `launch.sh` which will source mavros_setup.sh, and run the launch file. - -## Launch file - -The launch file `mavros_bridge.launch` runs the following -Running the container will run 2 ros nodes in parallel: -1. A Mavros node listening on fcu_url, targeting drone with sysid tgt_system - - `roslaunch mavros apm.launch fcu_url:=${MAVROS_FCU_URL} tgt_system:=${MAVROS_TGT_SYSTEM}` -2. A ros1 bridge node with option to forward all mavros topics to ros2 - - `ros2 run ros1_bridge dynamic_bridge --bridge-all-1to2-topics` - -## Contents -There are two docker files: - -- `Dockerfile.mavros` is the base container which is based on Ubuntu 20.04. It runs ROS2 Foxy, ROS1 Noetic and MAVROS via the ROS1_bridge. -- `mavros_setup.sh` is sourced at the end of the Dockerfile if any environment variables need modification. -- `mavros_bridge.launch` is a ros1 launch file, and launches both mavros and the ros1 bridge. It has 4 parameters: - - fcu_url - Flight control url, point it at the physical autopilot, ardupilot sitl or px4 sitl, no default in launch file. Defailts to `MAVROS_FCU_URL="udp://127.0.0.1:14550@14555"` using launch.sh - - target_system - target id to filter for from mavlink stream, defaults to 1, or MAVROS_TGT_SYSTEM using launch.sh - - system_id - sysid of mavros node, defaults to 1 - - firmware - `px4` or `apm`, name of the mavros launch file. Defaults to `px4`. - -## Configuration - -We have identified that mavros is a base requirement for any drone running onboard compute in order to communicate with both the autopilot and GCS. - -The mavros container exposes an environment variable `MAVROS_FCU_URL` which is passed to mavros's `fcu_url` option for configuring the mavlink connection to mavros. The default value is: -> `MAVROS_FCU_URL=udp://127.0.0.1:14550@14555` - -It also exposes environment variabel `MAVROS_TGT_SYSTEM` which is the `SYSID` of the ardupilot instance that mavros wants to connect to. This is usually an integer value. However, an IP address can also be passed in and `mavros_setup.sh` will extract the last numver of the IPv4 address as the value. Default is 1 - -To modify this at runtime, pass the `-e MAVROS_FCU_URL=` to `docker run` like so -> `docker run -e MAVROS_FCU_URL= -e MAVROS_TGT_SYSTEM=<#n>...` - -See Mavros for possible connection values - -This container builds the ros1_bridge from scratch in order to incorporate the `mavros_msgs` package. -t to get the -ordinal of its containing pod from the hostname. It will then use this ordinal to set up the ports and the system ID to -match those generated by a PX4 SITL instance set up with the same ordinal. If the setup script fails to get the ordinal -from the hostname, it will attempt to connect to a PX4 SITL with `PX4_INSTANCE=0`. - -Once the setup script has run, the default behaviour is to launch the `mavros_bridge.launch.xml` file. This behaviour -should be usable in almost all cases. This is a __ROS2__ launch file. It instructs `ros2 launch` to run the -`ros1_bridge` node and an instance of __ROS1__'s `roslaunch`. This in turn launches the __ROS1__ `mavros.launch` file, -which contains instructions to run the MAVROS node. - -The __ROS2__ `mavros_bridge.launch.xml` script defines a set of arguments to enable configuration of the MAVROS node. -These are ususally filled by the environment variables defined below. If the configurability provided here is -insufficient, the image can be run with a different command. - -## Environment Variables - -Name | Default Value | Description -----------------------|--------------------|------------ -`MAVROS_FCU_CONN` | "udp" | Protocol for autogenerated FCU URL -`MAVROS_FCU_IP` | "127.0.0.1" | IP for autogenerated FCU_URL -`MAVROS_FCU_UDP_BASE` | "14830" | Base port for autogenerated FCU_URL -`MAVROS_TGT_SYSTEM` | "auto" | Target system ID, if set to a number, this will __override__ the automatic behaviour -`PX4_INSTANCE_BASE` | 0 | Base instance for autogenerated instance matching -`MAVROS_TGT_FIRMWARE` | "px4" | Firmware profile used by MAVROS. Only other valid value currently is "apm" -`MAVROS_GCS_URL` | "udp-pb://@:14550" | MAVROS URL for ground control station connection -`MAVROS_FCU_URL` | {unset} | MAVROS URL for FCU connection. Set to __override__ automatic behaviour -`VEHICLE_NAMESPACE` | {unset} | Namespace for mavros topics. Set to __override__ default value of `vehicle_${TGT_SYSTEM}` - -`MAVROS_FCU_URL` is autogenerated as: -> `$MAVROS_FCU_CONN://$MAVROS_FCU_IP:$((MAVROS_FCU_UDP_BASE + INSTANCE))@/` - -If no Kubernetes deployment is detected, this ends up as: -> `udp://127.0.0.1:14830@/` - -Similarly, if no Kubernetes is detected, `MAVROS_TGT_SYSTEM` will end up as `1`, - -When setting `MAVROS_FCU_URL` manually, remember that a query string (_e.g._ `?ids=n,240`) will be added during launch. -You need to ensure that your input for `MAVROS_FCU_URL` supports this syntax. Of particular note is the need for a -trailing `/` in most formats, but not for the `serial://` format. Also note that the plain file format does not support -this. See [the MAVROS docs](https://github.com/mavlink/mavros/blob/master/mavros/README.md#connection-url) for more -information on URL formats. - -## Build Process - -At the time of writing, there is not a binary distribution available for `mavros_msgs` under ROS2. Therefore, this -package needs to be built from source. In addition, to ensure support for custom message types, `ros1_bridge` also needs -to be built from source. The build process also includes some additional steps to complete the installation of MAVROS -under ROS1. - -## Example `vehicle.config` - -Note that the extended form of the serial URL is required for MAVROS's target "query string" to work. - -```bash -VEHICLE_FCU_URL=serial:///dev/px4fmu:115200 -VEHICLE_FIRMWARE=px4 -VEHICLE_MAVLINK_SYSID=23 -VEHICLE_NAME=clover23 -``` - -## Behaviour Notes - -### Running on a vehicle - -Ensure `/etc/starling/vehicle.config` is mounted. The container is then configured from the contents of that file. - -### Running under Kubernetes StatefulSet - -The container is configured based on the detected ordinal from the hostname. - -`MAVROS_FCU_URL` is autogenerated as: `udp://127.0.0.1:$((14830 + ORDINAL))@` - -`MAVROS_TGT_SYSTEM` will end up as `$((ORDINAL + 1))` - -### Running isolated - -Default values will be used, equivalent to the Kubernetes case with `ORDINAL=0` - -### Weird Namespacing for mavros pluginlists - -In ROS2 a parameter file has an annoying specific format. The first entry should be the name of the node which the following ros2 parameters apply to. This must directly map to the node name. - -We would like our node to be under `/mavros/...`. But by default the topics all come out on `...` essentially root. Therefore in the `mavros.launch.xml` we set the mavros node to use namespace `/mavros`. However in order for the param file to match, we need to match the root mavros node name which is `/mavros/mavros`. - -Therefore the top of `mavros_pluginlists_px4.yaml` must specify the node name `/mavros/mavros`. The template pluginlists only specifies `mavros`, so the `mavros_setup.sh` dynamically renames the pluginlist yaml file with the correct namespace. +Please see [docs for more details](https://docs.starlinguas.dev/docker-images/mavros2/) \ No newline at end of file diff --git a/system/mavros2/config/mavros_pluginlists_ap.yaml b/system/mavros2/config/mavros_pluginlists_ap.yaml index 9f9e0d3..436752d 100644 --- a/system/mavros2/config/mavros_pluginlists_ap.yaml +++ b/system/mavros2/config/mavros_pluginlists_ap.yaml @@ -1,17 +1,48 @@ -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: + component_id: 191 + plugin_denylist: + - "*" + plugin_allowlist: + # - altitude + - command + - distance_sensor + # - ftp + # - global_position + # - imu + - local_position + # - manual_control + # - param + # - px4flow + # - rc_io + # - setpoint_accel + # - setpoint_attitude + - setpoint_position + # - setpoint_raw + # - setpoint_velocity + - sys_status + - sys_time + - vision_pose + - log_transfer + # - onboard_computer_status + # - vibration -plugin_whitelist: [] -#- 'sys_* \ No newline at end of file +# Old format + +# plugin_blacklist: +# # common +# - actuator_control +# - ftp +# - safety_area +# - hil +# # extras +# - altitude +# - debug_value +# - image_pub +# - px4flow +# - vibration +# - vision_speed_estimate +# - wheel_odometry + +# plugin_whitelist: [] +# #- 'sys_* \ No newline at end of file diff --git a/system/mavros2/mavros.launch.xml b/system/mavros2/mavros.launch.xml index 9d3b4a6..ae19e31 100644 --- a/system/mavros2/mavros.launch.xml +++ b/system/mavros2/mavros.launch.xml @@ -23,10 +23,6 @@ - - - -