Merge branch 'ros-planning:main' into manual_composited_bringup

nav2_amcl/CMakeLists.txt

@@ -5,6 +5,7 @@ find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -50,6 +51,7 @@ endif()
 set(dependencies
   rclcpp
   rclcpp_lifecycle
+  rclcpp_components
   message_filters
   tf2_geometry_msgs
   geometry_msgs
@@ -78,6 +80,8 @@ target_link_libraries(${library_name}
   map_lib motions_lib sensors_lib
 )
 
+rclcpp_components_register_nodes(${library_name} "nav2_amcl::AmclNode")
+
 install(TARGETS ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

nav2_amcl/src/amcl_node.cpp

@@ -1376,3 +1376,10 @@ AmclNode::initLaserScan()
 }
 
 }  // namespace nav2_amcl
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_amcl::AmclNode)

nav2_bt_navigator/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_behavior_tree REQUIRED)
@@ -35,6 +36,7 @@ set(dependencies
   rclcpp
   rclcpp_action
   rclcpp_lifecycle
+  rclcpp_components
   std_msgs
   geometry_msgs
   nav2_behavior_tree
@@ -63,6 +65,8 @@ ament_target_dependencies(${library_name}
   ${dependencies}
 )
 
+rclcpp_components_register_nodes(${library_name} "nav2_bt_navigator::BtNavigator")
+
 install(TARGETS ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

nav2_bt_navigator/src/bt_navigator.cpp

@@ -182,3 +182,10 @@ BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
 }
 
 }  // namespace nav2_bt_navigator
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_bt_navigator::BtNavigator)

nav2_controller/CMakeLists.txt

@@ -7,6 +7,7 @@ find_package(nav2_common REQUIRED)
 find_package(angles REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_msgs REQUIRED)
@@ -28,14 +29,15 @@ add_executable(${executable_name}
 
 set(library_name ${executable_name}_core)
 
-add_library(${library_name}
+add_library(${library_name} SHARED
   src/nav2_controller.cpp
 )
 
 set(dependencies
   angles
   rclcpp
   rclcpp_action
+  rclcpp_components
   std_msgs
   nav2_msgs
   nav_2d_utils
@@ -75,6 +77,8 @@ ament_target_dependencies(${executable_name}
 
 target_link_libraries(${executable_name} ${library_name})
 
+rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer")
+
 install(TARGETS simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

nav2_controller/src/nav2_controller.cpp

@@ -580,3 +580,10 @@ void ControllerServer::speedLimitCallback(const nav2_msgs::msg::SpeedLimit::Shar
 }
 
 }  // namespace nav2_controller
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_controller::ControllerServer)

nav2_lifecycle_manager/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(nav2_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
@@ -36,6 +37,7 @@ set(dependencies
   rclcpp
   rclcpp_action
   rclcpp_lifecycle
+  rclcpp_components
   std_msgs
   std_srvs
   tf2_geometry_msgs
@@ -58,6 +60,8 @@ ament_target_dependencies(lifecycle_manager
   ${dependencies}
 )
 
+rclcpp_components_register_nodes(${library_name} "nav2_lifecycle_manager::LifecycleManager")
+
 install(TARGETS
   ${library_name}
   ARCHIVE DESTINATION lib

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -375,3 +375,10 @@ LifecycleManager::message(const std::string & msg)
 }
 
 }  // namespace nav2_lifecycle_manager
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_lifecycle_manager::LifecycleManager)

nav2_map_server/CMakeLists.txt

@@ -7,6 +7,7 @@ find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
@@ -61,6 +62,7 @@ set(map_io_dependencies
 set(map_server_dependencies
   rclcpp
   rclcpp_lifecycle
+  rclcpp_components
   nav_msgs
   nav2_msgs
   yaml_cpp_vendor
@@ -123,6 +125,9 @@ if(WIN32)
     YAML_CPP_DLL)
 endif()
 
+rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapSaver")
+rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapServer")
+
 install(TARGETS
     ${library_name} ${map_io_library_name}
   ARCHIVE DESTINATION lib

nav2_map_server/src/map_saver/map_saver.cpp

@@ -228,3 +228,10 @@ bool MapSaver::saveMapTopicToFile(
 }
 
 }  // namespace nav2_map_server
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::MapSaver)

nav2_map_server/src/map_server/map_server.cpp

@@ -235,3 +235,10 @@ void MapServer::updateMsgHeader()
 }
 
 }  // namespace nav2_map_server
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::MapServer)

nav2_planner/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
@@ -31,6 +32,7 @@ set(dependencies
   rclcpp
   rclcpp_action
   rclcpp_lifecycle
+  rclcpp_components
   std_msgs
   visualization_msgs
   nav2_util
@@ -62,6 +64,8 @@ ament_target_dependencies(${executable_name}
   ${dependencies}
 )
 
+rclcpp_components_register_nodes(${library_name} "nav2_planner::PlannerServer")
+
 install(TARGETS ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

nav2_planner/src/planner_server.cpp

@@ -511,3 +511,10 @@ PlannerServer::publishPlan(const nav_msgs::msg::Path & path)
 }
 
 }  // namespace nav2_planner
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(nav2_planner::PlannerServer)

nav2_recoveries/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(nav2_behavior_tree REQUIRED)
 find_package(nav2_util REQUIRED)
@@ -31,6 +32,7 @@ set(dependencies
   rclcpp
   rclcpp_action
   rclcpp_lifecycle
+  rclcpp_components
   std_msgs
   nav2_util
   nav2_behavior_tree
@@ -72,7 +74,7 @@ ament_target_dependencies(nav2_wait_recovery
 pluginlib_export_plugin_description_file(nav2_core recovery_plugin.xml)
 
 # Library
-add_library(${library_name}
+add_library(${library_name} SHARED
   src/recovery_server.cpp
 )
 
@@ -91,6 +93,7 @@ ament_target_dependencies(${executable_name}
   ${dependencies}
 )
 
+rclcpp_components_register_nodes(${library_name} "recovery_server::RecoveryServer")
 
 install(TARGETS ${library_name}
                 nav2_backup_recovery

nav2_recoveries/src/recovery_server.cpp

@@ -182,3 +182,10 @@ RecoveryServer::on_shutdown(const rclcpp_lifecycle::State &)
 }
 
 }  // end namespace recovery_server
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(recovery_server::RecoveryServer)

nav2_simple_commander/setup.cfg

@@ -1,4 +1,4 @@
 [develop]
-script-dir=$base/lib/nav2_simple_commander
+script_dir=$base/lib/nav2_simple_commander
 [install]
-install-scripts=$base/lib/nav2_simple_commander
+install_scripts=$base/lib/nav2_simple_commander

nav2_smac_planner/README.md

@@ -97,8 +97,6 @@ planner_server:
       max_planning_time: 3.5              # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
       motion_model_for_search: "DUBIN"    # 2D Moore, Von Neumann; Hybrid Dubin, Redds-Shepp; State Lattice set internally
       cost_travel_multiplier: 2.0         # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
-      angle_quantization_bins: 64         # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
-      minimum_turning_radius: 0.40        # For Hybrid nodes: minimum turning radius in m of path / vehicle
       angle_quantization_bins: 64         # For Hybrid/Lattice nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
       analytic_expansion_ratio: 3.5       # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
       minimum_turning_radius: 0.40        # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle

nav2_smac_planner/src/a_star.cpp

@@ -575,12 +575,12 @@ typename AStarAlgorithm<NodeT>::AnalyticExpansionNodes AStarAlgorithm<NodeT>::ge
     node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s());
     reals = s.reals();
     angle = reals[2] / node->motion_table.bin_size;
-    while (angle >= node->motion_table.num_angle_quantization_float) {
-      angle -= node->motion_table.num_angle_quantization_float;
-    }
     while (angle < 0.0) {
       angle += node->motion_table.num_angle_quantization_float;
     }
+    while (angle >= node->motion_table.num_angle_quantization_float) {
+      angle -= node->motion_table.num_angle_quantization_float;
+    }
     // Turn the pose into a node, and check if it is valid
     index = NodeT::getIndex(
       static_cast<unsigned int>(reals[0]),

nav2_smac_planner/src/node_hybrid.cpp

@@ -230,14 +230,14 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node)
     const float & node_heading = node->pose.theta;
     float new_heading = node_heading + motion_model._theta;
 
-    if (new_heading >= num_angle_quantization_float) {
-      new_heading -= num_angle_quantization_float;
-    }
-
     if (new_heading < 0.0) {
       new_heading += num_angle_quantization_float;
     }
 
+    if (new_heading >= num_angle_quantization_float) {
+      new_heading -= num_angle_quantization_float;
+    }
+
     projection_list.emplace_back(
       delta_xs[i][node_heading] + node->pose.x,
       delta_ys[i][node_heading] + node->pose.y,

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -263,6 +263,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   while (orientation_bin < 0.0) {
     orientation_bin += static_cast<float>(_angle_quantizations);
   }
+  // This is needed to handle precision issues
+  if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
+    orientation_bin -= static_cast<float>(_angle_quantizations);
+  }
   unsigned int orientation_bin_id = static_cast<unsigned int>(floor(orientation_bin));
   _a_star->setStart(mx, my, orientation_bin_id);
 
@@ -272,6 +276,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   while (orientation_bin < 0.0) {
     orientation_bin += static_cast<float>(_angle_quantizations);
   }
+  // This is needed to handle precision issues
+  if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
+    orientation_bin -= static_cast<float>(_angle_quantizations);
+  }
   orientation_bin_id = static_cast<unsigned int>(floor(orientation_bin));
   _a_star->setGoal(mx, my, orientation_bin_id);