From 5e04eb2adae3245a1fed819f223ad5e24c81a4ac Mon Sep 17 00:00:00 2001 From: ARK3r Date: Tue, 6 Feb 2024 16:22:06 -0500 Subject: [PATCH 1/7] update smac_planner README Signed-off-by: ARK3r --- nav2_smac_planner/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 7130253241..3a992088b6 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -172,7 +172,7 @@ By default, `0.4m` is the setting which I think is "reasonable" for the smaller ### Costmap Resolutions -We provide for the Hybrid-A\*, State Lattice, and 2D A\* implementations a costmap downsampler option. This can be **incredible** beneficial when planning very long paths in larger spaces. The motion models for SE2 planning and neighborhood search in 2D planning is proportional to the costmap resolution. By downsampling it, you can N^2 reduce the number of expansions required to achieve a particular goal. However, the lower the resolution, the larger small obstacles appear and you won't be able to get super close to obstacles. This is a trade-off to make and test. Some numbers I've seen are 2-4x drops in planning CPU time for a 2-3x downsample rate. For long and complex paths, I was able to get it << 100ms at only a 2x downsample rate from a plan that otherwise took upward of 400ms. +We provide for the Hybrid-A\*, State Lattice, and 2D A\* implementations a costmap downsampler option. This can be **incredibly** beneficial when planning very long paths in larger spaces. The motion models for SE2 planning and neighborhood search in 2D planning is proportional to the costmap resolution. By downsampling it, you can N^2 reduce the number of expansions required to achieve a particular goal. However, the lower the resolution, the larger small obstacles appear and you won't be able to get super close to obstacles. This is a trade-off to make and test. Some numbers I've seen are 2-4x drops in planning CPU time for a 2-3x downsample rate. For long and complex paths, I was able to get it << 100ms at only a 2x downsample rate from a plan that otherwise took upward of 400ms. I recommend users using a 5cm resolution costmap and playing with the different values of downsampling rate until they achieve what they think is optimal performance (lowest number of expansions vs. necessity to achieve fine goal poses). Then, I would recommend to change the global costmap resolution to this new value. That way you don't own the compute of downsampling and maintaining a higher-resolution costmap that isn't used. @@ -209,4 +209,4 @@ As such, it is recommended if you have sparse SLAM maps, gaps or holes in your m One interesting thing to note from the second figure is that you see a number of expansions in open space. This is due to travel / heuristic values being so similar, tuning values of the penalty weights can have a decent impact there. The defaults are set as a good middle ground between large open spaces and confined aisles (environment specific tuning could be done to reduce the number of expansions for a specific map, speeding up the planner). The planner actually runs substantially faster the more confined the areas of search / environments are -- but still plenty fast for even wide open areas! -Sometimes visualizing the expansions is very useful to debug potential concerns (why does this goal take longer to compute, why can't I find a path, etc), should you on rare occasion run into an issue. You can enable the publication of the expansions on the `/expansions` topic for SmacHybrid with the parameter `viz_expansions: True`, beware that it should be used only for debug as it increases a lot the CPU usage. +Sometimes visualizing the expansions is very useful to debug potential concerns (why does this goal take longer to compute, why can't I find a path, etc), should you on rare occasion run into an issue. You can enable the publication of the expansions on the `/expansions` topic for SmacHybrid with the parameter `debug_visualizations: True`, beware that it should be used only for debug as it increases a lot the CPU usage. From af6fd1adac1d90567953365255e3696621db7a3d Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 7 Mar 2024 20:54:23 -0500 Subject: [PATCH 2/7] added current controller loop rate logging Signed-off-by: ARK3r --- nav2_controller/src/controller_server.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index fa25e0b137..c6ccbcfdb9 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -419,6 +419,8 @@ void ControllerServer::computeControl() { std::lock_guard lock(dynamic_params_lock_); + auto start_time = this->now(); + RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { @@ -479,10 +481,12 @@ void ControllerServer::computeControl() break; } + auto cycle_duration = this->now() - start_time; + if (!loop_rate.sleep()) { RCLCPP_WARN( - get_logger(), "Control loop missed its desired rate of %.4fHz", - controller_frequency_); + get_logger(), "Control loop missed its desired rate of %.4fHz. Current loop rate is %.4f Hz", + controller_frequency_, 1 / cycle_duration.seconds()); } } } catch (nav2_core::InvalidController & e) { @@ -815,3 +819,4 @@ ControllerServer::dynamicParametersCallback(std::vector param // 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) + From 698ab9431d35cc8bc1ac5c1693fc1e6c84f6c1a8 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Thu, 7 Mar 2024 20:58:48 -0500 Subject: [PATCH 3/7] linting Signed-off-by: ARK3r --- nav2_controller/src/controller_server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index c6ccbcfdb9..bfea59e0bd 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -485,7 +485,8 @@ void ControllerServer::computeControl() if (!loop_rate.sleep()) { RCLCPP_WARN( - get_logger(), "Control loop missed its desired rate of %.4fHz. Current loop rate is %.4f Hz", + get_logger(), + "Control loop missed its desired rate of %.4fHz. Current loop rate is %.4f Hz", controller_frequency_, 1 / cycle_duration.seconds()); } } From 5160ca7a884729a86ebb4f3c350294ee57750082 Mon Sep 17 00:00:00 2001 From: ARK3r Date: Fri, 8 Mar 2024 12:58:36 -0500 Subject: [PATCH 4/7] uncrustify lint Signed-off-by: ARK3r --- nav2_controller/src/controller_server.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index bfea59e0bd..dffdfbef3c 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -486,7 +486,7 @@ void ControllerServer::computeControl() if (!loop_rate.sleep()) { RCLCPP_WARN( get_logger(), - "Control loop missed its desired rate of %.4fHz. Current loop rate is %.4f Hz", + "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", controller_frequency_, 1 / cycle_duration.seconds()); } } @@ -820,4 +820,3 @@ ControllerServer::dynamicParametersCallback(std::vector param // 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) - From 763d814ea4f7685fe3ab2e052c4a09b8ad747bfa Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 8 Mar 2024 10:25:06 -0800 Subject: [PATCH 5/7] Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index dffdfbef3c..d0f9b48326 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -486,7 +486,7 @@ void ControllerServer::computeControl() if (!loop_rate.sleep()) { RCLCPP_WARN( get_logger(), - "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", + "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", controller_frequency_, 1 / cycle_duration.seconds()); } } From c70b5a138e6c660af948bcea7bf7b027a613dbd1 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 8 Mar 2024 10:25:10 -0800 Subject: [PATCH 6/7] Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index d0f9b48326..fe67680043 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -482,7 +482,6 @@ void ControllerServer::computeControl() } auto cycle_duration = this->now() - start_time; - if (!loop_rate.sleep()) { RCLCPP_WARN( get_logger(), From 7b4aa9385904111ad273f551830f205403e4ae38 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 8 Mar 2024 10:25:15 -0800 Subject: [PATCH 7/7] Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index fe67680043..85aae171cc 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -420,7 +420,6 @@ void ControllerServer::computeControl() std::lock_guard lock(dynamic_params_lock_); auto start_time = this->now(); - RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try {