Waypoint nav markers (#1426)

* Markers for navigation waypoints

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Fix test lint error

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Revert unnecessary changes in rviz default conf

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Revert unnecessary changes in rviz default conf

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Starting wp count from 1

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

nav2_bringup/bringup/rviz/nav2_default_view.rviz

@@ -9,6 +9,7 @@ Panels:
         - /TF1/Frames1
         - /TF1/Tree1
         - /Global Planner1/Global Costmap1
+        - /MarkerArray1
       Splitter Ratio: 0.5833333134651184
     Tree Height: 464
   - Class: rviz_common/Selection
@@ -315,6 +316,18 @@ Visualization Manager:
           Value: true
       Enabled: true
       Name: Controller
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /waypoints
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

nav2_rviz_plugins/CMakeLists.txt

@@ -30,6 +30,7 @@ find_package(rviz_ogre_vendor REQUIRED)
 find_package(rviz_rendering REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 
 set(nav2_rviz_plugins_headers_to_moc
   include/nav2_rviz_plugins/goal_pose_updater

nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp

@@ -30,6 +30,7 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rviz_common/panel.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "visualization_msgs/msg/marker_array.hpp"
 #include "nav2_util/geometry_utils.hpp"
 
 class QPushButton;
@@ -123,6 +124,11 @@ private:
   QState * accumulated_{nullptr};
 
   std::vector<geometry_msgs::msg::PoseStamped> acummulated_poses_;
+
+  // Publish the visual markers with the waypoints
+  void updateWpNavigationMarkers();
+  // Waypoint navigation visual markers publisher
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr wp_navigation_markers_pub_;
 };
 
 class InitialThread : public QThread

nav2_rviz_plugins/package.xml

@@ -25,7 +25,8 @@
   <depend>rviz_rendering</depend>
   <depend>std_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
-
+  <depend>visualization_msgs</depend>
+  
   <exec_depend>libqt5-core</exec_depend>
   <exec_depend>libqt5-gui</exec_depend>
   <exec_depend>libqt5-opengl</exec_depend>

nav2_rviz_plugins/src/nav2_panel.cpp

@@ -233,6 +233,10 @@ Nav2Panel::Nav2Panel(QWidget * parent)
   navigation_goal_ = nav2_msgs::action::NavigateToPose::Goal();
   waypoint_follower_goal_ = nav2_msgs::action::FollowWaypoints::Goal();
 
+  wp_navigation_markers_pub_ =
+    client_node_->create_publisher<visualization_msgs::msg::MarkerArray>("waypoints",
+      rclcpp::QoS(1).transient_local());
+
   QObject::connect(&GoalUpdater, SIGNAL(updateGoal(double,double,double,QString)),  // NOLINT
     this, SLOT(onNewGoal(double,double,double,QString)));  // NOLINT
 }
@@ -312,6 +316,8 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame)
     std::cout << "Start navigation" << std::endl;
     startNavigation(pose);
   }
+
+  updateWpNavigationMarkers();
 }
 
 void
@@ -499,6 +505,60 @@ Nav2Panel::load(const rviz_common::Config & config)
   Panel::load(config);
 }
 
+void
+Nav2Panel::updateWpNavigationMarkers()
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+
+  for (size_t i = 0; i < acummulated_poses_.size(); i++) {
+    // Draw a green ball at the waypoint pose
+    visualization_msgs::msg::Marker marker;
+    marker.header = acummulated_poses_[i].header;
+    marker.id = i * 2;
+    marker.type = visualization_msgs::msg::Marker::SPHERE;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    marker.pose = acummulated_poses_[i].pose;
+    marker.scale.x = 0.1;
+    marker.scale.y = 0.1;
+    marker.scale.z = 0.1;
+    marker.color.r = 0;
+    marker.color.g = 255;
+    marker.color.b = 0;
+    marker.color.a = 1.0f;
+    marker.lifetime = rclcpp::Duration(0);
+    marker.frame_locked = false;
+    marker_array.markers.push_back(marker);
+
+    // Draw the waypoint number
+    visualization_msgs::msg::Marker marker_text;
+    marker_text.header = acummulated_poses_[i].header;
+    marker_text.id = i * 2 + 1;
+    marker_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+    marker_text.action = visualization_msgs::msg::Marker::ADD;
+    marker_text.pose = acummulated_poses_[i].pose;
+    marker_text.pose.position.z += 0.2;  // draw it on top of the waypoint
+    marker_text.scale.x = 0.1;
+    marker_text.scale.y = 0.1;
+    marker_text.scale.z = 0.1;
+    marker_text.color.r = 0;
+    marker_text.color.g = 255;
+    marker_text.color.b = 0;
+    marker_text.color.a = 1.0f;
+    marker_text.lifetime = rclcpp::Duration(0);
+    marker_text.frame_locked = false;
+    marker_text.text = "wp_" + std::to_string(i + 1);
+    marker_array.markers.push_back(marker_text);
+  }
+
+  if (marker_array.markers.empty()) {
+    visualization_msgs::msg::Marker clear_all_marker;
+    clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
+    marker_array.markers.push_back(clear_all_marker);
+  }
+
+  wp_navigation_markers_pub_->publish(marker_array);
+}
+
 }  // namespace nav2_rviz_plugins
 
 #include <pluginlib/class_list_macros.hpp>  // NOLINT