From 420a6ffd3c6b154c4aec2c5f2a24e4af4bd971a9 Mon Sep 17 00:00:00 2001 From: gezp Date: Wed, 15 Sep 2021 11:18:37 +0800 Subject: [PATCH 1/6] Register nodes as components (#2562) Signed-off-by: zhenpeng ge --- nav2_amcl/CMakeLists.txt | 4 ++++ nav2_amcl/src/amcl_node.cpp | 7 +++++++ nav2_bt_navigator/CMakeLists.txt | 4 ++++ nav2_bt_navigator/src/bt_navigator.cpp | 7 +++++++ nav2_controller/CMakeLists.txt | 6 +++++- nav2_controller/src/nav2_controller.cpp | 7 +++++++ nav2_lifecycle_manager/CMakeLists.txt | 4 ++++ nav2_lifecycle_manager/src/lifecycle_manager.cpp | 7 +++++++ nav2_map_server/CMakeLists.txt | 5 +++++ nav2_map_server/src/map_saver/map_saver.cpp | 7 +++++++ nav2_map_server/src/map_server/map_server.cpp | 7 +++++++ nav2_planner/CMakeLists.txt | 4 ++++ nav2_planner/src/planner_server.cpp | 7 +++++++ nav2_recoveries/CMakeLists.txt | 5 ++++- nav2_recoveries/src/recovery_server.cpp | 7 +++++++ nav2_waypoint_follower/CMakeLists.txt | 4 ++++ nav2_waypoint_follower/src/waypoint_follower.cpp | 7 +++++++ 17 files changed, 97 insertions(+), 2 deletions(-) diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt index be83000bd4c..af2e6bf422b 100644 --- a/nav2_amcl/CMakeLists.txt +++ b/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 diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 9f3eb0bd7d7..df550bde953 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/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) diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index df14b1e15a0..e36fb0de03b 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/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 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index b35e48a71ab..7f62c05e12b 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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) diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 570671dade9..f8e7bd91a0b 100644 --- a/nav2_controller/CMakeLists.txt +++ b/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,7 +29,7 @@ add_executable(${executable_name} set(library_name ${executable_name}_core) -add_library(${library_name} +add_library(${library_name} SHARED src/nav2_controller.cpp ) @@ -36,6 +37,7 @@ 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 diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index bca6a72bbd6..1c2dd23bb82 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/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) diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt index 6d812c35834..e0980df7b48 100644 --- a/nav2_lifecycle_manager/CMakeLists.txt +++ b/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 diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index fee3d7cfcf6..7022681e6fa 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/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) diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 8ffe285cb7b..c24e5e5c4d5 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/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 diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 1cc68431838..580825f8ed4 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/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) diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index fffc1609aca..332c67ee5eb 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/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) diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt index 2fbbc470126..68cf595f626 100644 --- a/nav2_planner/CMakeLists.txt +++ b/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 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 15b127741bc..e72abb3223e 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/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) diff --git a/nav2_recoveries/CMakeLists.txt b/nav2_recoveries/CMakeLists.txt index 9f7b716319a..8af7c5b2f03 100644 --- a/nav2_recoveries/CMakeLists.txt +++ b/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 diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index e10e136bb5b..e9c28c0fdb3 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/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) diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 720bee0dd4d..a3b46942bcd 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -15,6 +15,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(nav_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) @@ -46,6 +47,7 @@ set(dependencies rclcpp rclcpp_action rclcpp_lifecycle + rclcpp_components nav_msgs nav2_msgs nav2_util @@ -76,6 +78,8 @@ ament_target_dependencies(photo_at_waypoint ${dependencies}) add_library(input_at_waypoint SHARED plugins/input_at_waypoint.cpp) ament_target_dependencies(input_at_waypoint ${dependencies}) +rclcpp_components_register_nodes(${library_name} "nav2_waypoint_follower::WaypointFollower") + install(TARGETS ${library_name} wait_at_waypoint photo_at_waypoint input_at_waypoint ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 2c7c27aaa1a..a52d4b1f815 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -306,3 +306,10 @@ WaypointFollower::goalResponseCallback( } } // namespace nav2_waypoint_follower + +#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_waypoint_follower::WaypointFollower) From 62ef71998dbb0065e55a068b214d70fc35a506a4 Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Thu, 16 Sep 2021 00:25:46 +0530 Subject: [PATCH 2/6] Updates to Nav2 Theta Star Planner docs (#2559) * - removing 'w_heuristic_cost_' from Nav2 Theta Star Planner as a user configurable parameter - updates to `nav2_theta_star_planner/README.md` to remove its reference wherever applicable - updates to `nav2_theta_star_planner/src/theta_star_planner.cpp` to set `w_heuristic_cost` as `std::min(1.0, w_euc_cost_)` - fixed an incorrect check of whether the start and goal pose are the same in `nav2_theta_star_planner/src/theta_star_planner.cpp` * Update theta_star_planner.hpp clean up stuff from forced merge * Update theta_star_planner.cpp clean up stuff from a forced merge * Update README.md * Update theta_star_planner.cpp Re-adding the goal to ensure, that the last pose, is the goal exactly * fix linting issues * Update theta_star_planner.cpp Co-authored-by: Steve Macenski --- nav2_theta_star_planner/README.md | 13 +++++-------- .../src/theta_star_planner.cpp | 16 +++++++++------- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index ed3f810f095..c888d0a6a45 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -13,7 +13,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - ![example.png](img/00-37.png) -The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` +The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` ## Cost Function Details ### Symbols and their meanings @@ -51,8 +51,6 @@ The parameters of the planner are : - ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 - ` .w_euc_cost ` : weight applied on the length of the path. - ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs. -- ` .w_heuristic_cost ` : it has been provided to have an admissible heuristic - Below are the default values of the parameters : ``` planner_server: @@ -64,7 +62,6 @@ planner_server: how_many_corners: 8 w_euc_cost: 1.0 w_traversal_cost: 2.0 - w_heuristic_cost: 1.0 ``` ## Usage Notes @@ -76,9 +73,9 @@ This planner uses the costs associated with each cell from the `global_costmap` Providing a gentle potential field over the region allows the planner to exchange the increase in path lengths for a decrease in the accumulated traversal cost, thus leading to an increase in the distance from the obstacles. This around a corner allows for naturally smoothing the turns and removes the requirement for an external path smoother. -To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversly make the paths less taut. So it is recommend ed that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. +`w_heuristic_cost` is an internal setting that is not user changable. It has been provided to have an admissible heuristic, restricting its value to the minimum of `w_euc_cost` and `1.0` to make sure the heuristic and travel costs are admissible and consistent. -Usually set `w_heuristic_cost` at the same value as `w_euc_cost` or 1.0 (whichever is lower). Though as a last resort you may increase the value of `w_heuristic_cost` to speed up the planning process, this is not recommended as it might not always lead to shorter query times, but would definitely give you sub optimal paths +To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversly make the paths less taut. So it is recommend ed that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. Also note that changes to `w_traversal_cost` might cause slow downs, in case the number of node expanisions increase thus tuning it along with `w_euc_cost` is the way to go to. @@ -91,5 +88,5 @@ This planner is recommended to be used with local planners like DWB or TEB (or o While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so because it might increase the query times from the planner. Test the planners performance on the higher cm/px costmaps before making a switch to finer costmaps. -### Possible Applications -This planner could be used in scenarios where planning speed matters over an extremely smooth path, which could anyways be handled by using a local planner/controller as mentioned above. Because of the any-angled nature of paths you can traverse environments diagonally (wherever it is allowed, eg: in a wide open region). +### When to use this planner? +This planner could be used in scenarios where planning speed matters over an extremely smooth path, which could anyways be handled by using a local planner/controller as mentioned above. Because of the any-angled nature of paths you can traverse environments diagonally (wherever it is allowed, eg: in a wide open region). Another use case would be when you have corridors that are misaligned in the map image, in such a case this planner would be able to give straight-line like paths as it considers line of sight and thus avoid giving jagged paths which arises with other planners because of the finite directions of motion they consider. diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 18ab9a7759a..3134315fc83 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -52,6 +52,7 @@ void ThetaStarPlanner::configure( node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(2.0)); node->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_); + planner_->w_heuristic_cost_ = planner_->w_euc_cost_ < 1.0 ? planner_->w_euc_cost_ : 1.0; nav2_util::declare_parameter_if_not_declared( node, name_ + ".w_heuristic_cost", rclcpp::ParameterValue(1.0)); node->get_parameter(name_ + ".w_heuristic_cost", planner_->w_heuristic_cost_); @@ -87,7 +88,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( // Corner case of start and goal beeing on the same cell unsigned int mx_start, my_start, mx_goal, my_goal; planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); - planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_goal, my_goal); + planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); if (mx_start == mx_goal && my_start == my_goal) { if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); @@ -115,6 +116,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( logger_, "Got the src and dst... (%i, %i) && (%i, %i)", planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); getPlan(global_path); + global_path.poses.back().pose.orientation = goal.pose.orientation; // If use_final_approach_orientation=true, interpolate the last pose orientation from the // previous pose to set the orientation to the 'final approach' orientation of the robot so @@ -138,7 +140,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( auto stop_time = std::chrono::steady_clock::now(); auto dur = std::chrono::duration_cast(stop_time - start_time); - RCLCPP_DEBUG(logger_, "the time taken for pointy is : %i", static_cast(dur.count())); + RCLCPP_DEBUG(logger_, "the time taken is : %i", static_cast(dur.count())); RCLCPP_DEBUG(logger_, "the nodes_opened are: %i", planner_->nodes_opened); return global_path; } @@ -165,15 +167,14 @@ nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation( { nav_msgs::msg::Path pa; + geometry_msgs::msg::PoseStamped p1; for (unsigned int j = 0; j < raw_path.size() - 1; j++) { - geometry_msgs::msg::PoseStamped p; coordsW pt1 = raw_path[j]; - p.pose.position.x = pt1.x; - p.pose.position.y = pt1.y; - pa.poses.push_back(p); + p1.pose.position.x = pt1.x; + p1.pose.position.y = pt1.y; + pa.poses.push_back(p1); coordsW pt2 = raw_path[j + 1]; - geometry_msgs::msg::PoseStamped p1; double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); int loops = static_cast(distance / dist_bw_points); double sin_alpha = (pt2.y - pt1.y) / distance; @@ -184,6 +185,7 @@ nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation( pa.poses.push_back(p1); } } + return pa; } From 7be0af97168ea066fc9af4882af61c96c8707b56 Mon Sep 17 00:00:00 2001 From: zoltan-lengyel <79632988+zoltan-lengyel@users.noreply.github.com> Date: Wed, 15 Sep 2021 20:58:33 +0200 Subject: [PATCH 3/6] Fixed vector::reserve exception in smac planner due to precision error (#2563) - Related issue: https://github.com/ros-planning/navigation2/issues/2547 --- nav2_smac_planner/src/a_star.cpp | 6 +++--- nav2_smac_planner/src/node_hybrid.cpp | 8 ++++---- nav2_smac_planner/src/smac_planner_hybrid.cpp | 8 ++++++++ 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 66acd4b005c..11d1976dae3 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -575,12 +575,12 @@ typename AStarAlgorithm::AnalyticExpansionNodes AStarAlgorithm::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(reals[0]), diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 059f9f5445f..b02d99f326c 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/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, diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index f4e64986543..d86f6d2afd7 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/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(_angle_quantizations); } + // This is needed to handle precision issues + if (orientation_bin >= static_cast(_angle_quantizations)) { + orientation_bin -= static_cast(_angle_quantizations); + } unsigned int orientation_bin_id = static_cast(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(_angle_quantizations); } + // This is needed to handle precision issues + if (orientation_bin >= static_cast(_angle_quantizations)) { + orientation_bin -= static_cast(_angle_quantizations); + } orientation_bin_id = static_cast(floor(orientation_bin)); _a_star->setGoal(mx, my, orientation_bin_id); From e5a4c844c748233dc4d3039b1ea5809c81dac45f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 15 Sep 2021 18:17:34 -0700 Subject: [PATCH 4/6] Fixing minor build warnings (#2564) --- nav2_simple_commander/setup.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_simple_commander/setup.cfg b/nav2_simple_commander/setup.cfg index 2d91d903923..040b9290ca6 100644 --- a/nav2_simple_commander/setup.cfg +++ b/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 From d837c28850080f0c986b2c27be0192edc3512334 Mon Sep 17 00:00:00 2001 From: Luca Bonamini Date: Mon, 27 Sep 2021 21:27:31 +0200 Subject: [PATCH 5/6] Fixed test_bt_navigator_with_groot_monitoring (#2568) --- .../src/system/nav_to_pose_tester_node.py | 243 ++++++++++-------- 1 file changed, 136 insertions(+), 107 deletions(-) diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index b58d2f8aa35..efc6dffa5eb 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -41,51 +41,48 @@ class NavTester(Node): - - def __init__( - self, - initial_pose: Pose, - goal_pose: Pose, - namespace: str = '' - ): - super().__init__(node_name='nav2_tester', namespace=namespace) - self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, - 'initialpose', 10) - self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ""): + super().__init__(node_name="nav2_tester", namespace=namespace) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, "initialpose", 10 + ) + self.goal_pub = self.create_publisher(PoseStamped, "goal_pose", 10) pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1) - - self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', self.poseCallback, pose_qos) + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, "amcl_pose", self.poseCallback, pose_qos + ) self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose - self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + self.action_client = ActionClient(self, NavigateToPose, "navigate_to_pose") def info_msg(self, msg: str): - self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + self.get_logger().info("\033[1;37;44m" + msg + "\033[0m") def warn_msg(self, msg: str): - self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + self.get_logger().warn("\033[1;37;43m" + msg + "\033[0m") def error_msg(self, msg: str): - self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + self.get_logger().error("\033[1;37;41m" + msg + "\033[0m") def setInitialPose(self): msg = PoseWithCovarianceStamped() msg.pose.pose = self.initial_pose - msg.header.frame_id = 'map' - self.info_msg('Publishing Initial Pose') + msg.header.frame_id = "map" + self.info_msg("Publishing Initial Pose") self.initial_pose_pub.publish(msg) self.currentPose = self.initial_pose def getStampedPoseMsg(self, pose: Pose): msg = PoseStamped() - msg.header.frame_id = 'map' + msg.header.frame_id = "map" msg.pose = pose return msg @@ -99,54 +96,54 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg("'NavigateToPose' action server not available, waiting...") - if (os.getenv('GROOT_MONITORING') == 'True'): + if os.getenv("GROOT_MONITORING") == "True": if self.grootMonitoringGetStatus(): - self.error_msg('Behavior Tree must not be running already!') - self.error_msg('Are you running multiple goals/bts..?') + self.error_msg("Behavior Tree must not be running already!") + self.error_msg("Are you running multiple goals/bts..?") return False - self.info_msg('This Error above MUST Fail and is o.k.!') + self.info_msg("This Error above MUST Fail and is o.k.!") self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateToPose.Goal() goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) - self.info_msg('Sending goal request...') + self.info_msg("Sending goal request...") send_goal_future = self.action_client.send_goal_async(goal_msg) rclpy.spin_until_future_complete(self, send_goal_future) goal_handle = send_goal_future.result() if not goal_handle.accepted: - self.error_msg('Goal rejected') + self.error_msg("Goal rejected") return False - self.info_msg('Goal accepted') + self.info_msg("Goal accepted") get_result_future = goal_handle.get_result_async() future_return = True - if (os.getenv('GROOT_MONITORING') == 'True'): + if os.getenv("GROOT_MONITORING") == "True": try: if not self.grootMonitoringReloadTree(): - self.error_msg('Failed GROOT_BT - Reload Tree from ZMQ Server') + self.error_msg("Failed GROOT_BT - Reload Tree from ZMQ Server") future_return = False if not self.grootMonitoringGetStatus(): - self.error_msg('Failed GROOT_BT - Get Status from ZMQ Publisher') + self.error_msg("Failed GROOT_BT - Get Status from ZMQ Publisher") future_return = False except Exception as e: # noqa: B902 - self.error_msg(f'Failed GROOT_BT - ZMQ Tests: {e}') + self.error_msg(f"Failed GROOT_BT - ZMQ Tests: {e}") future_return = False self.info_msg("Waiting for 'NavigateToPose' action to complete") rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status if status != GoalStatus.STATUS_SUCCEEDED: - self.info_msg(f'Goal failed with status code: {status}') + self.info_msg(f"Goal failed with status code: {status}") return False if not future_return: return False - self.info_msg('Goal succeeded!') + self.info_msg("Goal succeeded!") return True def grootMonitoringReloadTree(self): @@ -159,38 +156,38 @@ def grootMonitoringReloadTree(self): sock.setsockopt(zmq.RCVTIMEO, 1000) # sock.setsockopt(zmq.LINGER, 0) - sock.connect(f'tcp://localhost: {port}') - self.info_msg(f'ZMQ Server Port: {port}') + sock.connect(f"tcp://localhost:{port}") + self.info_msg(f"ZMQ Server Port:{port}") # this should fail try: sock.recv() - self.error_msg('ZMQ Reload Tree Test 1/3 - This should have failed!') + self.error_msg("ZMQ Reload Tree Test 1/3 - This should have failed!") # Only works when ZMQ server receives a request first sock.close() return False except zmq.error.ZMQError: - self.info_msg('ZMQ Reload Tree Test 1/3: Check') + self.info_msg("ZMQ Reload Tree Test 1/3: Check") try: # request tree from server - sock.send_string('') + sock.send_string("") # receive tree from server as flat_buffer sock.recv() - self.info_msg('ZMQ Reload Tree Test 2/3: Check') + self.info_msg("ZMQ Reload Tree Test 2/3: Check") except zmq.error.Again: - self.info_msg('ZMQ Reload Tree Test 2/3 - Failed to load tree') + self.info_msg("ZMQ Reload Tree Test 2/3 - Failed to load tree") sock.close() return False # this should fail try: sock.recv() - self.error_msg('ZMQ Reload Tree Test 3/3 - This should have failed!') + self.error_msg("ZMQ Reload Tree Test 3/3 - This should have failed!") # Tree should only be loadable ONCE after ZMQ server received a request sock.close() return False except zmq.error.ZMQError: - self.info_msg('ZMQ Reload Tree Test 3/3: Check') + self.info_msg("ZMQ Reload Tree Test 3/3: Check") return True @@ -204,22 +201,22 @@ def grootMonitoringGetStatus(self): # sock.setsockopt(zmq.LINGER, 0) # Define subscription and messages with prefix to accept. - sock.setsockopt_string(zmq.SUBSCRIBE, '') + sock.setsockopt_string(zmq.SUBSCRIBE, "") port = 1666 # default publishing port for groot monitoring - sock.connect(f'tcp://127.0.0.1:{port}') + sock.connect(f"tcp://127.0.0.1:{port}") for request in range(3): try: sock.recv() except zmq.error.Again: - self.error_msg('ZMQ - Did not receive any status') + self.error_msg("ZMQ - Did not receive any status") sock.close() return False - self.info_msg('ZMQ - Did receive status') + self.info_msg("ZMQ - Did receive status") return True def poseCallback(self, msg): - self.info_msg('Received amcl_pose') + self.info_msg("Received amcl_pose") self.current_pose = msg.pose.pose self.initial_pose_received = True @@ -231,109 +228,111 @@ def reachesGoal(self, timeout, distance): rclpy.spin_once(self, timeout_sec=1) if self.distanceFromGoal() < distance: goalReached = True - self.info_msg('*** GOAL REACHED ***') + self.info_msg("*** GOAL REACHED ***") return True elif timeout is not None: if (time.time() - start_time) > timeout: - self.error_msg('Robot timed out reaching its goal!') + self.error_msg("Robot timed out reaching its goal!") return False def distanceFromGoal(self): d_x = self.current_pose.position.x - self.goal_pose.position.x d_y = self.current_pose.position.y - self.goal_pose.position.y distance = math.sqrt(d_x * d_x + d_y * d_y) - self.info_msg(f'Distance from goal is: {distance}') + self.info_msg(f"Distance from goal is: {distance}") return distance def wait_for_node_active(self, node_name: str): # Waits for the node within the tester namespace to become active - self.info_msg(f'Waiting for {node_name} to become active') - node_service = f'{node_name}/get_state' + self.info_msg(f"Waiting for {node_name} to become active") + node_service = f"{node_name}/get_state" state_client = self.create_client(GetState, node_service) while not state_client.wait_for_service(timeout_sec=1.0): - self.info_msg(f'{node_service} service not available, waiting...') + self.info_msg(f"{node_service} service not available, waiting...") req = GetState.Request() # empty request - state = 'UNKNOWN' - while (state != 'active'): - self.info_msg(f'Getting {node_name} state...') + state = "UNKNOWN" + while state != "active": + self.info_msg(f"Getting {node_name} state...") future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None: state = future.result().current_state.label - self.info_msg(f'Result of get_state: {state}') + self.info_msg(f"Result of get_state: {state}") else: - self.error_msg(f'Exception while calling service: {future.exception()!r}') + self.error_msg( + f"Exception while calling service: {future.exception()!r}" + ) time.sleep(5) def shutdown(self): - self.info_msg('Shutting down') + self.info_msg("Shutting down") self.action_client.destroy() - transition_service = 'lifecycle_manager_navigation/manage_nodes' + transition_service = "lifecycle_manager_navigation/manage_nodes" mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(f'{transition_service} service not available, waiting...') + self.info_msg(f"{transition_service} service not available, waiting...") req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN future = mgr_client.call_async(req) try: - self.info_msg('Shutting down navigation lifecycle manager...') + self.info_msg("Shutting down navigation lifecycle manager...") rclpy.spin_until_future_complete(self, future) future.result() - self.info_msg('Shutting down navigation lifecycle manager complete.') + self.info_msg("Shutting down navigation lifecycle manager complete.") except Exception as e: # noqa: B902 - self.error_msg(f'Service call failed {e!r}') - transition_service = 'lifecycle_manager_localization/manage_nodes' + self.error_msg(f"Service call failed {e!r}") + transition_service = "lifecycle_manager_localization/manage_nodes" mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(f'{transition_service} service not available, waiting...') + self.info_msg(f"{transition_service} service not available, waiting...") req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN future = mgr_client.call_async(req) try: - self.info_msg('Shutting down localization lifecycle manager...') + self.info_msg("Shutting down localization lifecycle manager...") rclpy.spin_until_future_complete(self, future) future.result() - self.info_msg('Shutting down localization lifecycle manager complete') + self.info_msg("Shutting down localization lifecycle manager complete") except Exception as e: # noqa: B902 - self.error_msg(f'Service call failed {e!r}') + self.error_msg(f"Service call failed {e!r}") def wait_for_initial_pose(self): self.initial_pose_received = False while not self.initial_pose_received: - self.info_msg('Setting initial pose') + self.info_msg("Setting initial pose") self.setInitialPose() - self.info_msg('Waiting for amcl_pose to be received') + self.info_msg("Waiting for amcl_pose to be received") rclpy.spin_once(self, timeout_sec=1) def test_RobotMovesToGoal(robot_tester): - robot_tester.info_msg('Setting goal pose') + robot_tester.info_msg("Setting goal pose") robot_tester.publishGoalPose() - robot_tester.info_msg('Waiting 60 seconds for robot to reach goal') + robot_tester.info_msg("Waiting 60 seconds for robot to reach goal") return robot_tester.reachesGoal(timeout=60, distance=0.5) def run_all_tests(robot_tester): # set transforms to use_sim_time result = True - if (result): - robot_tester.wait_for_node_active('amcl') + if result: + robot_tester.wait_for_node_active("amcl") robot_tester.wait_for_initial_pose() - robot_tester.wait_for_node_active('bt_navigator') + robot_tester.wait_for_node_active("bt_navigator") result = robot_tester.runNavigateAction() - if (result): + if result: result = test_RobotMovesToGoal(robot_tester) # Add more tests here if desired - if (result): - robot_tester.info_msg('Test PASSED') + if result: + robot_tester.info_msg("Test PASSED") else: - robot_tester.error_msg('Test FAILED') + robot_tester.error_msg("Test FAILED") return result @@ -358,10 +357,19 @@ def get_testers(args): init_x, init_y, final_x, final_y = args.robot[0] tester = NavTester( initial_pose=fwd_pose(float(init_x), float(init_y)), - goal_pose=fwd_pose(float(final_x), float(final_y))) + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) tester.info_msg( - 'Starting tester, robot going from ' + init_x + ', ' + init_y + - ' to ' + final_x + ', ' + final_y + '.') + "Starting tester, robot going from " + + init_x + + ", " + + init_y + + " to " + + final_x + + ", " + + final_y + + "." + ) testers.append(tester) return testers @@ -371,19 +379,30 @@ def get_testers(args): tester = NavTester( namespace=namespace, initial_pose=fwd_pose(float(init_x), float(init_y)), - goal_pose=fwd_pose(float(final_x), float(final_y))) + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) tester.info_msg( - 'Starting tester for ' + namespace + - ' going from ' + init_x + ', ' + init_y + - ' to ' + final_x + ', ' + final_y) + "Starting tester for " + + namespace + + " going from " + + init_x + + ", " + + init_y + + " to " + + final_x + + ", " + + final_y + ) testers.append(tester) return testers def check_args(expect_failure: str): # Check if --expect_failure is True or False - if expect_failure != 'True' and expect_failure != 'False': - print('\033[1;37;41m' + ' -e flag must be set to True or False only. ' + '\033[0m') + if expect_failure != "True" and expect_failure != "False": + print( + "\033[1;37;41m" + " -e flag must be set to True or False only. " + "\033[0m" + ) exit(1) else: return eval(expect_failure) @@ -391,16 +410,26 @@ def check_args(expect_failure: str): def main(argv=sys.argv[1:]): # The robot(s) positions from the input arguments - parser = argparse.ArgumentParser(description='System-level navigation tester node') - parser.add_argument('-e', '--expect_failure') + parser = argparse.ArgumentParser(description="System-level navigation tester node") + parser.add_argument("-e", "--expect_failure") group = parser.add_mutually_exclusive_group(required=True) - group.add_argument('-r', '--robot', action='append', nargs=4, - metavar=('init_x', 'init_y', 'final_x', 'final_y'), - help='The robot starting and final positions.') - group.add_argument('-rs', '--robots', action='append', nargs=5, - metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), - help="The robot's namespace and starting and final positions. " + - 'Repeating the argument for multiple robots is supported.') + group.add_argument( + "-r", + "--robot", + action="append", + nargs=4, + metavar=("init_x", "init_y", "final_x", "final_y"), + help="The robot starting and final positions.", + ) + group.add_argument( + "-rs", + "--robots", + action="append", + nargs=5, + metavar=("name", "init_x", "init_y", "final_x", "final_y"), + help="The robot's namespace and starting and final positions. " + + "Repeating the argument for multiple robots is supported.", + ) args, unknown = parser.parse_known_args() @@ -423,15 +452,15 @@ def main(argv=sys.argv[1:]): # stop and shutdown the nav stack to exit cleanly tester.shutdown() - testers[0].info_msg('Done Shutting Down.') + testers[0].info_msg("Done Shutting Down.") if passed != expect_failure: - testers[0].info_msg('Exiting failed') + testers[0].info_msg("Exiting failed") exit(1) else: - testers[0].info_msg('Exiting passed') + testers[0].info_msg("Exiting passed") exit(0) -if __name__ == '__main__': +if __name__ == "__main__": main() From 406bbbe112fd74e01953640ce6a0f87cfe66c288 Mon Sep 17 00:00:00 2001 From: relffok <57466265+relffok@users.noreply.github.com> Date: Mon, 27 Sep 2021 22:02:21 +0200 Subject: [PATCH 6/6] removed redundant params in smac example (#2570) --- nav2_smac_planner/README.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 39f710965ae..281f6c027f4 100644 --- a/nav2_smac_planner/README.md +++ b/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