Large Nav2 Node, Utils, and Interface Refactor (#5288)

* initial unorganized prototype

Signed-off-by: Steve Macenski <[email protected]>

* break out files and add doxygen

Signed-off-by: Steve Macenski <[email protected]>

* Adding refactor for nav2_ros_common and new ROS interface factories

Signed-off-by: Steve Macenski <[email protected]>

* fixing CI - not sure how that got through merge conflicts

Signed-off-by: Steve Macenski <[email protected]>

* Lifecycle publisher a missing test

Signed-off-by: Steve Macenski <[email protected]>

* system tests

Signed-off-by: Steve Macenski <[email protected]>

* default

Signed-off-by: Steve Macenski <[email protected]>

* activating publishers

Signed-off-by: Steve Macenski <[email protected]>

* temp disable allow param qos overrides

Signed-off-by: Steve Macenski <[email protected]>

* API update for new constructor option

Signed-off-by: Steve Macenski <[email protected]>

* Supporting Jazzy and abstracting util

Signed-off-by: Steve Macenski <[email protected]>

* Review round 1

Signed-off-by: Steve Macenski <[email protected]>

* Adding Nav2 Publisher and Subscriber objects to later build upon

Signed-off-by: Steve Macenski <[email protected]>

* Adding additional ::SharedPtr for readability

Signed-off-by: Steve Macenski <[email protected]>

* fix bug

Signed-off-by: Steve Macenski <[email protected]>

* fixing Jazzy support

Signed-off-by: Steve Macenski <[email protected]>

* missed one last spot

Signed-off-by: Steve Macenski <[email protected]>

* Adding migration instructions

Signed-off-by: Steve Macenski <[email protected]>

* more context

Signed-off-by: Steve Macenski <[email protected]>

* Adding migration context

Signed-off-by: Steve Macenski <[email protected]>

* precommit

Signed-off-by: Steve Macenski <[email protected]>

* adding missing dep

Signed-off-by: Steve Macenski <[email protected]>

* Updating system tess

Signed-off-by: Steve Macenski <[email protected]>

* more

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>

Author: SteveMacenski

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v35\
+              - "<< parameters.key >>-v36\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v35\
+              - "<< parameters.key >>-v36\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v35\
+            key: "<< parameters.key >>-v36\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

nav2_amcl/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(std_srvs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(nav2_ros_common REQUIRED)
 
 nav2_package()
 
@@ -38,7 +39,8 @@ add_library(pf_lib SHARED
 target_include_directories(pf_lib
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
 if(HAVE_DRAND48)
   target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48")
 endif()
@@ -52,7 +54,8 @@ add_library(map_lib SHARED
 target_include_directories(map_lib
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
 
 add_library(motions_lib SHARED
   src/motion_model/omni_motion_model.cpp
@@ -61,7 +64,8 @@ add_library(motions_lib SHARED
 target_include_directories(motions_lib
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
 target_link_libraries(motions_lib PUBLIC
   pf_lib
   pluginlib::pluginlib
@@ -77,7 +81,8 @@ add_library(sensors_lib SHARED
 target_include_directories(sensors_lib
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
 target_link_libraries(sensors_lib PUBLIC
   pf_lib
   map_lib
@@ -96,7 +101,8 @@ endif()
 target_include_directories(${library_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
 target_link_libraries(${library_name} PUBLIC
   ${geometry_msgs_TARGETS}
   message_filters::message_filters
@@ -123,7 +129,8 @@ add_executable(${executable_name}
 target_include_directories(${executable_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  "$<BUILD_INTERFACE:${nav2_ros_common_INCLUDE_DIRS}>")
 target_link_libraries(${executable_name} PRIVATE
   ${library_name}
 )
@@ -169,6 +176,7 @@ ament_export_dependencies(
   std_srvs
   tf2
   tf2_ros
+  nav2_ros_common
 )
 ament_export_targets(${library_name})
 pluginlib_export_plugin_description_file(nav2_amcl plugins.xml)

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -32,7 +32,7 @@
 #include "rclcpp/version.h"
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
 #include "nav2_amcl/motion_model/motion_model.hpp"
 #include "nav2_amcl/sensors/laser/laser.hpp"
 #include "nav2_msgs/msg/particle.hpp"
@@ -42,7 +42,7 @@
 #include "pluginlib/class_loader.hpp"
 #include "rclcpp/node_options.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
-#include "nav2_util/service_server.hpp"
+#include "nav2_ros_common/service_server.hpp"
 #include "std_srvs/srv/empty.hpp"
 #include "tf2_ros/message_filter.h"
 #include "tf2_ros/transform_broadcaster.h"
@@ -56,7 +56,7 @@ namespace nav2_amcl
  * @class AmclNode
  * @brief ROS wrapper for AMCL
  */
-class AmclNode : public nav2_util::LifecycleNode
+class AmclNode : public nav2::LifecycleNode
 {
 public:
   /*
@@ -73,23 +73,23 @@ class AmclNode : public nav2_util::LifecycleNode
   /*
    * @brief Lifecycle configure
    */
-  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
   /*
    * @brief Lifecycle activate
    */
-  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
   /*
    * @brief Lifecycle deactivate
    */
-  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
   /*
    * @brief Lifecycle cleanup
    */
-  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
   /*
    * @brief Lifecycle shutdown
    */
-  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+  nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
   /**
    * @brief Callback executed when a parameter change is detected
@@ -109,7 +109,7 @@ class AmclNode : public nav2_util::LifecycleNode
   // in order to isolate TF timer used in message filter.
   rclcpp::CallbackGroup::SharedPtr callback_group_;
   rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
-  std::unique_ptr<nav2_util::NodeThread> executor_thread_;
+  std::unique_ptr<nav2::NodeThread> executor_thread_;
 
   // Pose hypothesis
   typedef struct
@@ -149,7 +149,7 @@ class AmclNode : public nav2_util::LifecycleNode
   std::atomic<bool> first_map_received_{false};
   amcl_hyp_t * initial_pose_hyp_;
   std::recursive_mutex mutex_;
-  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
+  nav2::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
 #if NEW_UNIFORM_SAMPLING
   struct Point2D { int32_t x; int32_t y; };
   static std::vector<Point2D> free_space_indices;
@@ -180,6 +180,7 @@ class AmclNode : public nav2_util::LifecycleNode
   std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
     rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
   #endif
+
   std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
   message_filters::Connection laser_scan_connection_;
 
@@ -188,11 +189,11 @@ class AmclNode : public nav2_util::LifecycleNode
    * @brief Initialize pub subs of AMCL
    */
   void initPubSub();
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::ConstSharedPtr
+  nav2::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::ConstSharedPtr
     initial_pose_sub_;
-  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+  nav2::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     pose_pub_;
-  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::ParticleCloud>::SharedPtr
+  nav2::Publisher<nav2_msgs::msg::ParticleCloud>::SharedPtr
     particle_cloud_pub_;
   /*
    * @brief Handle with an initial pose estimate is received
@@ -208,8 +209,7 @@ class AmclNode : public nav2_util::LifecycleNode
    * @brief Initialize state services
    */
   void initServices();
-  nav2_util::ServiceServer<std_srvs::srv::Empty,
-    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr global_loc_srv_;
+  nav2::ServiceServer<std_srvs::srv::Empty>::SharedPtr global_loc_srv_;
   /*
    * @brief Service callback for a global relocalization request
    */
@@ -219,8 +219,7 @@ class AmclNode : public nav2_util::LifecycleNode
     std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
   // service server for providing an initial pose guess
-  nav2_util::ServiceServer<nav2_msgs::srv::SetInitialPose,
-    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr initial_guess_srv_;
+  nav2::ServiceServer<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
   /*
    * @brief Service callback for an initial pose guess request
    */
@@ -230,8 +229,7 @@ class AmclNode : public nav2_util::LifecycleNode
     std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);
 
   // Let amcl update samples without requiring motion
-  nav2_util::ServiceServer<std_srvs::srv::Empty,
-    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr nomotion_update_srv_;
+  nav2::ServiceServer<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
   /*
    * @brief Request an AMCL update even though the robot hasn't moved
    */

nav2_amcl/package.xml

@@ -38,6 +38,7 @@
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>nav2_ros_common</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>