Switch nav2_rviz_plugins to modern CMake idioms. (#4600)

* Switch nav2_rviz_plugins to modern CMake idioms. In addition, make some fixes to the code to more appropriately hold on to the Node object (there were warnings from the compiler without these fixes). Signed-off-by: Chris Lalancette <[email protected]> * Feedback from review. Signed-off-by: Chris Lalancette <[email protected]> --------- Signed-off-by: Chris Lalancette <[email protected]>
ros-navigation · Aug 7, 2024 · 9a5f7b2 · 9a5f7b2
1 parent 4d5b902
commit 9a5f7b2
Show file tree

Hide file tree

Showing 8 changed files with 109 additions and 80 deletions.
diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt
@@ -1,29 +1,17 @@
 cmake_minimum_required(VERSION 3.5)
 project(nav2_rviz_plugins)
 
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-
-if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
-endif()
-
-# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html
-set(CMAKE_INCLUDE_CURRENT_DIR ON)
-set(CMAKE_AUTOMOC ON)
-
 find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
-find_package(nav2_util REQUIRED)
+find_package(nav2_common REQUIRED)
 find_package(nav2_lifecycle_manager REQUIRED)
 find_package(nav2_msgs REQUIRED)
-find_package(nav_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent)
 find_package(rclcpp REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(rviz_common REQUIRED)
 find_package(rviz_default_plugins REQUIRED)
 find_package(rviz_ogre_vendor REQUIRED)
@@ -32,23 +20,25 @@ find_package(std_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+nav2_package()
 
+# We specifically don't turn on CMAKE_AUTOMOC, since it generates one huge
+# mocs_compilation.cpp file that takes a lot of memory to compile.  Instead
+# we create individual moc files that can be compiled separately.
 set(nav2_rviz_plugins_headers_to_moc
   include/nav2_rviz_plugins/costmap_cost_tool.hpp
   include/nav2_rviz_plugins/docking_panel.hpp
   include/nav2_rviz_plugins/goal_pose_updater.hpp
-  include/nav2_rviz_plugins/goal_common.hpp
   include/nav2_rviz_plugins/goal_tool.hpp
   include/nav2_rviz_plugins/nav2_panel.hpp
   include/nav2_rviz_plugins/selector.hpp
-  include/nav2_rviz_plugins/utils.hpp
-  include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp
   include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp
 )
-
-include_directories(
-  include
-)
+foreach(header "${nav2_rviz_plugins_headers_to_moc}")
+  qt5_wrap_cpp(nav2_rviz_plugins_moc_files "${header}")
+endforeach()
 
 set(library_name ${PROJECT_NAME})
 
@@ -61,45 +51,33 @@ add_library(${library_name} SHARED
   src/utils.cpp
   src/particle_cloud_display/flat_weighted_arrows_array.cpp
   src/particle_cloud_display/particle_cloud_display.cpp
-  ${nav2_rviz_plugins_headers_to_moc}
-)
-
-set(dependencies
-  geometry_msgs
-  nav2_util
-  nav2_lifecycle_manager
-  nav2_msgs
-  nav_msgs
-  pluginlib
-  Qt5
-  rclcpp
-  rclcpp_lifecycle
-  rviz_common
-  rviz_default_plugins
-  rviz_ogre_vendor
-  rviz_rendering
-  std_msgs
-  tf2_geometry_msgs
-  yaml_cpp_vendor
-)
-
-ament_target_dependencies(${library_name}
-  ${dependencies}
+  ${nav2_rviz_plugins_moc_files}
 )
-
 target_include_directories(${library_name} PUBLIC
   ${Qt5Widgets_INCLUDE_DIRS}
   ${OGRE_INCLUDE_DIRS}
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
-
-target_link_libraries(${library_name}
+target_link_libraries(${library_name} PUBLIC
+  ${geometry_msgs_TARGETS}
+  nav2_lifecycle_manager::nav2_lifecycle_manager_core
+  ${nav2_msgs_TARGETS}
+  nav2_util::nav2_util_core
+  rclcpp::rclcpp
+  rclcpp_action::rclcpp_action
   rviz_common::rviz_common
+  rviz_default_plugins::rviz_default_plugins
+  rviz_rendering::rviz_rendering
+  ${std_msgs_TARGETS}
+  tf2_geometry_msgs::tf2_geometry_msgs
+  ${visualization_msgs_TARGETS}
+)
+target_link_libraries(${library_name} PRIVATE
+  ament_index_cpp::ament_index_cpp
+  pluginlib::pluginlib
+  yaml-cpp::yaml-cpp
 )
-
-# Causes the visibility macros to use dllexport rather than dllimport,
-# which is appropriate when building the dll but not consuming it.
-# TODO: Make this specific to this project (not rviz default plugins)
-target_compile_definitions(${library_name} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY")
 
 pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
 
@@ -109,12 +87,11 @@ install(
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
-  INCLUDES DESTINATION include
 )
 
 install(
   DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
 install(
@@ -127,15 +104,22 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_export_include_directories(include)
+ament_export_include_directories(include/${PROJECT_NAME})
 ament_export_targets(${library_name} HAS_LIBRARY_TARGET)
 ament_export_dependencies(
-  Qt5
-  rviz_common
   geometry_msgs
-  map_msgs
-  nav_msgs
+  nav2_lifecycle_manager
+  nav2_msgs
+  nav2_util
+  Qt5
   rclcpp
+  rclcpp_action
+  rviz_common
+  rviz_default_plugins
+  rviz_rendering
+  std_msgs
+  tf2_geometry_msgs
+  visualization_msgs
 )
 
 ament_package()
diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp
@@ -15,7 +15,10 @@
 #ifndef NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
 #define NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
 
+#include <memory>
+
 #include <nav2_msgs/srv/get_cost.hpp>
+#include <rviz_common/ros_integration/ros_node_abstraction_iface.hpp>
 #include <rviz_common/tool.hpp>
 #include <rviz_default_plugins/tools/point/point_tool.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -47,7 +50,8 @@ private Q_SLOTS:
 private:
   rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr local_cost_client_;
   rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr global_cost_client_;
-  rclcpp::Node::SharedPtr node_;
+  // The Node pointer that we need to keep alive for the duration of this plugin.
+  std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
 
   QCursor std_cursor_;
   QCursor hit_cursor_;

diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp
@@ -19,12 +19,14 @@
 #include <QtWidgets>
 #include <QBasicTimer>
 
+#include <memory>
 #include <string>
 
 // ROS
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rviz_common/panel.hpp"
+#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp"
 #include "sensor_msgs/msg/battery_state.hpp"
 #include "nav2_msgs/action/dock_robot.hpp"
 #include "nav2_msgs/action/undock_robot.hpp"
@@ -125,6 +127,9 @@ private Q_SLOTS:
   DockGoalHandle::SharedPtr dock_goal_handle_;
   UndockGoalHandle::SharedPtr undock_goal_handle_;
 
+  // The Node pointer that we need to keep alive for the duration of this plugin.
+  std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
+
   // Docking / Undocking action feedback subscribers
   rclcpp::Subscription<Dock::Impl::FeedbackMessage>::SharedPtr docking_feedback_sub_;
   rclcpp::Subscription<Undock::Impl::FeedbackMessage>::SharedPtr undocking_feedback_sub_;

diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
@@ -31,6 +31,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rviz_common/panel.hpp"
+#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 #include "nav2_util/geometry_utils.hpp"
@@ -92,6 +93,9 @@ private Q_SLOTS:
   std::string loop_no_ = "0";
   std::string base_frame_;
 
+  // The Node pointer that we need to keep alive for the duration of this plugin.
+  std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
+
   // Call to send NavigateToPose action request for goal poses
   geometry_msgs::msg::PoseStamped convert_to_msg(
     std::vector<double> pose,

diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml
@@ -8,24 +8,23 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
   <build_depend>qtbase5-dev</build_depend>
 
+  <depend>ament_index_cpp</depend>
   <depend>geometry_msgs</depend>
-  <depend>nav2_util</depend>
   <depend>nav2_lifecycle_manager</depend>
   <depend>nav2_msgs</depend>
-  <depend>nav_msgs</depend>
+  <depend>nav2_util</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
-  <depend>rclcpp_lifecycle</depend>
-  <depend>resource_retriever</depend>
+  <depend>rclcpp_action</depend>
   <depend>rviz_common</depend>
   <depend>rviz_default_plugins</depend>
   <depend>rviz_ogre_vendor</depend>
   <depend>rviz_rendering</depend>
   <depend>std_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>urdf</depend>
   <depend>visualization_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
 

diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp
@@ -43,11 +43,20 @@ void CostmapCostTool::onInitialize()
   setName("Costmap Cost");
   setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/PointStamped.png"));
 
-  node_ = context_->getRosNodeAbstraction().lock()->get_raw_node();
+  node_ptr_ = context_->getRosNodeAbstraction().lock();
+  if (node_ptr_ == nullptr) {
+    // The node no longer exists, so just don't initialize
+    RCLCPP_ERROR(
+      rclcpp::get_logger("costmap_cost_tool"),
+      "Underlying ROS node no longer exists, initialization failed");
+    return;
+  }
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
+
   local_cost_client_ =
-    node_->create_client<nav2_msgs::srv::GetCost>("/local_costmap/get_cost_local_costmap");
+    node->create_client<nav2_msgs::srv::GetCost>("/local_costmap/get_cost_local_costmap");
   global_cost_client_ =
-    node_->create_client<nav2_msgs::srv::GetCost>("/global_costmap/get_cost_global_costmap");
+    node->create_client<nav2_msgs::srv::GetCost>("/global_costmap/get_cost_global_costmap");
 }
 
 void CostmapCostTool::activate() {}
@@ -106,22 +115,24 @@ void CostmapCostTool::callCostService(float x, float y)
 void CostmapCostTool::handleLocalCostResponse(
   rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture future)
 {
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   auto response = future.get();
   if (response->cost != -1) {
-    RCLCPP_INFO(node_->get_logger(), "Local costmap cost: %.1f", response->cost);
+    RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->cost);
   } else {
-    RCLCPP_ERROR(node_->get_logger(), "Failed to get local costmap cost");
+    RCLCPP_ERROR(node->get_logger(), "Failed to get local costmap cost");
   }
 }
 
 void CostmapCostTool::handleGlobalCostResponse(
   rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture future)
 {
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   auto response = future.get();
   if (response->cost != -1) {
-    RCLCPP_INFO(node_->get_logger(), "Global costmap cost: %.1f", response->cost);
+    RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->cost);
   } else {
-    RCLCPP_ERROR(node_->get_logger(), "Failed to get global costmap cost");
+    RCLCPP_ERROR(node->get_logger(), "Failed to get global costmap cost");
   }
 }
 }  // namespace nav2_rviz_plugins

diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp
@@ -12,14 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// C++
-#include <stdio.h>
-
 // QT
 #include <QLineEdit>
 #include <QVBoxLayout>
 #include <QHBoxLayout>
 #include <QLabel>
+
+// C++
+#include <chrono>
+#include <memory>
+#include <sstream>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
 #include <rviz_common/display_context.hpp>
 
 #include "nav2_util/geometry_utils.hpp"
@@ -114,7 +119,15 @@ DockingPanel::DockingPanel(QWidget * parent)
 
 void DockingPanel::onInitialize()
 {
-  auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+  node_ptr_ = getDisplayContext()->getRosNodeAbstraction().lock();
+  if (node_ptr_ == nullptr) {
+    // The node no longer exists, so just don't initialize
+    RCLCPP_ERROR(
+      rclcpp::get_logger("docking_panel"),
+      "Underlying ROS node no longer exists, initialization failed");
+    return;
+  }
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
 
   // Create action feedback subscriber
   docking_feedback_sub_ = node->create_subscription<Dock::Impl::FeedbackMessage>(

diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp
@@ -29,6 +29,7 @@
 
 #include "nav2_rviz_plugins/goal_common.hpp"
 #include "nav2_rviz_plugins/utils.hpp"
+#include "rclcpp/rclcpp.hpp"
 #include "rviz_common/display_context.hpp"
 #include "rviz_common/load_resource.hpp"
 #include "ament_index_cpp/get_package_share_directory.hpp"
@@ -777,7 +778,15 @@ void Nav2Panel::handleGoalSaver()
 void
 Nav2Panel::onInitialize()
 {
-  auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+  node_ptr_ = getDisplayContext()->getRosNodeAbstraction().lock();
+  if (node_ptr_ == nullptr) {
+    // The node no longer exists, so just don't initialize
+    RCLCPP_ERROR(
+      rclcpp::get_logger("nav2_panel"),
+      "Underlying ROS node no longer exists, initialization failed");
+    return;
+  }
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
 
   // declaring parameter to get the base frame
   node->declare_parameter("base_frame", rclcpp::ParameterValue(std::string("base_footprint")));