Large Nav2 Node, Utils, and Interface Refactor

.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
@@ -178,8 +178,9 @@ class AmclNode : public nav2_util::LifecycleNode
   std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> laser_scan_sub_;
   #else
   std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
-    rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
+    nav2::LifecycleNode>> laser_scan_sub_;
   #endif
+
   std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
   message_filters::Connection laser_scan_connection_;
 
@@ -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>