ros2: update services and launch files to enable PX4 example (#27)

* ros2: update terrain_planner launch file and rviz config

- Update mavros launch file for ardupilot
- Update rviz config to use descriptive names for markers
- Add groups in rviz config
- Update use of mesh resource in publish vehicle pose
- Use file:// prefix on fully qualified resource path.
- Update rviz config file to display paths by defaults

Signed-off-by: Rhys Mainwaring <[email protected]>

* ros2: update terrain_navigation_ros services and launch files

- Update launch file for ardupilot
- Install config folder.
- Add Python launch file for mavros.
- Add ardupilot sitl node.
- Update mavros launch file for ardupilot
- Update async service calls
- Change QoS to best effort for some mavros subscriptions
- Move and install resource directory
- Add latching qos for grid map.
- Update launch file for terrain planner
- Fix typo in resources path.
- Add Python launch file for visualize_path.
- Use mode GUIDED for off-board control
- Update formatting
- Update terrain planner launch to use quadplane
- Add sample mission
- Move implementation of geo_conversions and visualization into cpp
- Update use of mesh resource in publish vehicle pose
- Use file:// prefix on fully qualified resource path.
- Add todo notes to terrain_planner_ros
- Add separate launch file for running ardupilot sitl
- Allow the terrain planner to be run in isolation
- Update planning duration calculation
- Ensure rclcpp::Time variables are initialised to consistent time sources
- Update example mission to include NAV_LOITER_UNLIM required by planner
- Add further comments and reduce debug print output
- Accept all NAV_LOITER command codes in terrain planner
- Print global origin details
- Update home location in launch scripts
- Add px4 config to mavros launch script
- Sync guidance constants with values used for px4 standard_vtol
- Disable sitl_dds in terrain planner launch
- Declare altitude control variables as node parameters
- Declare params before topics in terrain planner node
- Add alt control launch args to terrain planner launch script
- Clean up terrain planner ros launch files
- Add px4 loiter mission for davos
- Remove unused publishers
- Document the reason for the conversions used in the global origin callback.
- Simplify the example ArduPilot mission.
- Add takeoff to ArduPilot davosdorf mission.
- Add Python node to relay from /mavros/set_point/global to /ap/cmd_gps_pose.
- Update relay from /mavros/set_point/global to /ap/cmd_gps_pose.

Signed-off-by: Rhys Mainwaring <[email protected]>

* ros2: update mav_planning_rviz planning panel and service calls

- Build standalone example.
- Add test rviz config for plugin.
- Fix unused variable warning.
- Remap grid_map_geo elevation_map to grid_map used in mav_planning_rviz.
- Use display context to set fixed frame in mav_planning_rviz
- Use mode GUIDED for off-board control
- Add debug logging for interactive markers
- Update formatting in goal_marker
- Add mutex to planning panel
- Move inlined functions to cpp in goal_marker
- Revert plugin to issue px4 offboard commands

Signed-off-by: Rhys Mainwaring <[email protected]>

* ros2: satisfy code checks for terrain_planner

Signed-off-by: Rhys Mainwaring <[email protected]>

* ros2: satisfy code checks for mav_planning_rviz

Signed-off-by: Rhys Mainwaring <[email protected]>

* ros2: satisfy code checks for terrain_navigation

Signed-off-by: Rhys Mainwaring <[email protected]>

* ros2: satisfy code checks for terrain_navigation_ros

Signed-off-by: Rhys Mainwaring <[email protected]>

* ros2: satisfy code checks for terrain_planner_benchmark

Signed-off-by: Rhys Mainwaring <[email protected]>

* ros2: contruct goal marker on initialize

Use Rviz node for interactive markers

This solves the problem of interactive markers not registering properly in rviz

* ros2: add shebang to global_position_relay script.

Signed-off-by: Rhys Mainwaring <[email protected]>

* ros2: upstream interface changes to grid_map_geo

Signed-off-by: Rhys Mainwaring <[email protected]>

---------

Signed-off-by: Rhys Mainwaring <[email protected]>
Co-authored-by: Jaeyoung Lim <[email protected]>

Author: srmainwaring

mav_planning_rviz/CMakeLists.txt

@@ -107,13 +107,13 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDIN
 
 pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
 
-# add_executable(standalone_test
-#   src/standalone_test.cpp
-# )
+add_executable(standalone_test
+  src/standalone_test.cpp
+)
 
-# target_link_libraries(standalone_test PUBLIC
-#   ${PROJECT_NAME}
-# )
+target_link_libraries(standalone_test PUBLIC
+  ${PROJECT_NAME}
+)
 
 # Install
 install(
@@ -139,11 +139,17 @@ ament_export_dependencies(
   rclcpp
 )
 
-# install(
-#   TARGETS
-#   standalone_test
-#   DESTINATION lib/${PROJECT_NAME}
-# )
+install(
+  TARGETS
+  standalone_test
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+  launch
+  rviz
+  DESTINATION share/${PROJECT_NAME}/
+)
 
 if(BUILD_TESTING)
   # find_package(ament_lint_auto REQUIRED)

mav_planning_rviz/include/mav_planning_rviz/edit_button.h

@@ -2,9 +2,9 @@
 #define MAV_PLANNING_RVIZ_EDIT_BUTTON_H_
 
 #ifndef Q_MOC_RUN
-#include <mav_msgs/eigen_mav_msgs.hpp>
 #include <QPushButton>
 #include <QWidget>
+#include <mav_msgs/eigen_mav_msgs.hpp>
 #endif
 
 namespace mav_planning_rviz {

mav_planning_rviz/include/mav_planning_rviz/goal_marker.h

@@ -2,28 +2,24 @@
 #ifndef MAV_PLANNING_RVIZ_GOAL_MARKER_H_
 #define MAV_PLANNING_RVIZ_GOAL_MARKER_H_
 
-#include <mutex>
+#include <visualization_msgs/msg/interactive_marker_feedback.h>
 
 #include <Eigen/Dense>
-#include <rclcpp/rclcpp.hpp>
-
-#include <grid_map_msgs/msg/grid_map.hpp>
-#include <interactive_markers/interactive_marker_server.hpp>
-#include <visualization_msgs/msg/interactive_marker_feedback.h>
 #include <grid_map_core/GridMap.hpp>
+#include <grid_map_msgs/msg/grid_map.hpp>
 #include <grid_map_ros/GridMapRosConverter.hpp>
+#include <interactive_markers/interactive_marker_server.hpp>
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
 
 class GoalMarker {
  public:
   GoalMarker(rclcpp::Node::SharedPtr node);
   virtual ~GoalMarker();
-  Eigen::Vector3d getGoalPosition() { return goal_pos_; };
+  Eigen::Vector3d getGoalPosition();
 
  private:
-  Eigen::Vector3d toEigen(const geometry_msgs::msg::Pose &p) {
-    Eigen::Vector3d position(p.position.x, p.position.y, p.position.z);
-    return position;
-  }
+  Eigen::Vector3d toEigen(const geometry_msgs::msg::Pose &p);
   void processSetPoseFeedback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &feedback);
   void GridmapCallback(const grid_map_msgs::msg::GridMap &msg);
 

mav_planning_rviz/include/mav_planning_rviz/planning_interactive_markers.h

@@ -2,12 +2,10 @@
 #define MAV_PLANNING_RVIZ_PLANNING_INTERACTIVE_MARKERS_H_
 
 #include <functional>
-
 #include <interactive_markers/interactive_marker_server.hpp>
-#include <rclcpp/rclcpp.hpp>
-
 #include <mav_msgs/conversions.hpp>
 #include <mav_msgs/eigen_mav_msgs.hpp>
+#include <rclcpp/rclcpp.hpp>
 
 namespace mav_planning_rviz {
 

mav_planning_rviz/include/mav_planning_rviz/planning_panel.h

@@ -2,13 +2,14 @@
 #define MAV_PLANNING_RVIZ_PLANNING_PANEL_H_
 
 #ifndef Q_MOC_RUN
+//! @todo(srmainwaring) prevent race condition with async service calls
+#include <QGroupBox>
+#include <mutex>
 #include <nav_msgs/msg/odometry.hpp>
 #include <planner_msgs/msg/navigation_status.hpp>
 #include <rclcpp/rclcpp.hpp>
-
 #include <rviz_common/panel.hpp>
 
-#include <QGroupBox>
 #include "mav_planning_rviz/edit_button.h"
 #include "mav_planning_rviz/goal_marker.h"
 #include "mav_planning_rviz/planning_interactive_markers.h"
@@ -100,6 +101,9 @@ class PlanningPanel : public rviz_common::Panel {
 
   std::shared_ptr<GoalMarker> goal_marker_;
 
+  //! @todo(srmainwaring) prevent race condition with async service calls
+  std::mutex node_mutex_;  // protects node_
+
   // QT stuff:
   QLineEdit* namespace_editor_;
   QLineEdit* planner_name_editor_;

mav_planning_rviz/include/mav_planning_rviz/pose_widget.h

@@ -2,11 +2,11 @@
 #define MAV_PLANNING_RVIZ_POSE_WIDGET_H_
 
 #ifndef Q_MOC_RUN
-#include <mav_msgs/eigen_mav_msgs.hpp>
 #include <QItemDelegate>
 #include <QLineEdit>
 #include <QStringList>
 #include <QTableWidget>
+#include <mav_msgs/eigen_mav_msgs.hpp>
 #endif
 
 class QLineEdit;

mav_planning_rviz/launch/run_rviz.launch

@@ -1,7 +1,16 @@
 <launch>
-    <arg name="visualization" default="true"/>
+  <arg name="location" default="davosdorf"/>
+  <arg name="rviz" default="true"/>
 
-    <group if="$(arg visualization)">
-        <node type="rviz" name="rviz" pkg="rviz" args="-d $(find terrain_planner)/launch/config.rviz" output="screen"/>
-    </group>
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="world_map" args="--frame-id world --child-frame-id map"/>
+
+  <node pkg="grid_map_geo" exec="test_tif_loader" name="test_tif_loader" output="screen">
+    <param name="tif_path" value="$(find-pkg-share terrain_navigation_ros)/resources/davosdorf.tif"/>
+    <param name="tif_color_path" value="$(find-pkg-share terrain_navigation_ros)/resources/davosdorf_color.tif"/>
+    <remap from="elevation_map" to="grid_map" />
+  </node>
+
+  <group if="$(var rviz)">
+    <node pkg="rviz2" exec="rviz2" name="rviz2" args="-d $(find-pkg-share mav_planning_rviz)/rviz/config.rviz" output="screen"/>
+  </group>
 </launch>

mav_planning_rviz/rviz/config.rviz

@@ -0,0 +1,190 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 290
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+  - Class: mav_planning_rviz/PlanningPanel
+    Name: PlanningPanel
+    namespace: ""
+    odometry_topic: ""
+    planner_name: davosdorf
+    planning_budget: 4
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1000
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.8999999761581421
+      Autocompute Intensity Bounds: true
+      Class: grid_map_rviz_plugin/GridMap
+      Color: 200; 200; 200
+      Color Layer: color
+      Color Transformer: ColorLayer
+      Enabled: true
+      Height Layer: elevation
+      Height Transformer: Layer
+      History Length: 1
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 10
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: GridMap
+      Show Grid Lines: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /grid_map
+      Use Rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Marker
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: visualization_marker
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: visualization_marker_array
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 255; 255; 255
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /move_base_simple/goal
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 8401.0107421875
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -515.1697998046875
+        Y: 526.9537353515625
+        Z: 855.5363159179688
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5897997617721558
+      Target Frame: map
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 5.818671703338623
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 950
+  Hide Left Dock: false
+  Hide Right Dock: true
+  PlanningPanel:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000002ad00000335fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006600000171000000e300fffffffb0000001a0050006c0061006e006e0069006e006700500061006e0065006c01000001d8000001c30000016d00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000fb0000000c00540065006c0065006f00700000000368000000b20000000000000000000000010000010f00000317fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000007c00000317000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000005dc00000039fc0100000002fb0000000800540069006d00650100000000000005dc0000023d00fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000014efc0100000001fb0000000800540069006d006501000000000000045000000000000000000000032e0000033500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1500
+  X: 0
+  Y: 25

mav_planning_rviz/src/edit_button.cpp

@@ -1,11 +1,11 @@
+#include "mav_planning_rviz/edit_button.h"
+
 #include <QApplication>
 #include <QDesktopWidget>
 #include <QHeaderView>
 #include <QTableView>
 #include <QTableWidget>
 
-#include "mav_planning_rviz/edit_button.h"
-
 namespace mav_planning_rviz {
 
 EditButton::EditButton(const std::string& id, QWidget* parent) : QWidget(parent), id_(id), editing_(false) {