From cc5a43ab2d0dc7ba7b4289fdb072f11e50eeef53 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 23 Oct 2023 11:50:16 -0700 Subject: [PATCH 1/9] Use mutex to protect costmap reads. (backport #3877) (#3897) * Use mutex to protect costmap reads. (#3877) * Use mutex to protect costmap reads. Otherwise costmap can be read during a map update and return 0. * Revert "Use mutex to protect costmap reads." This reverts commit e16a44c65ee7064e2271118894b92bb6e24ce28d. * Lock costmap before running MPPI controller. * Fix typo. * Protect against costmap updates in MPP and RotationShim controllers. --------- Co-authored-by: Leif Terry (cherry picked from commit a1c9fd5ad29bb00e40ce6e696d899a2bcd50cde5) # Conflicts: # nav2_mppi_controller/src/controller.cpp * fix merge conflict --------- Co-authored-by: LeifHookedWireless Co-authored-by: Steve Macenski --- nav2_mppi_controller/src/controller.cpp | 5 ++++- .../src/regulated_pure_pursuit_controller.cpp | 3 +++ .../src/nav2_rotation_shim_controller.cpp | 3 +++ 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index b87373ace87..8a631f97526 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -91,9 +91,12 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( } last_time_called_ = clock_->now(); - std::lock_guard lock(*parameters_handler_->getLock()); + std::lock_guard param_lock(*parameters_handler_->getLock()); nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock costmap_lock(*(costmap->getMutex())); + geometry_msgs::msg::TwistStamped cmd = optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker); diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index d5c648bd1a0..acf4c1e7cc9 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -282,6 +282,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity { std::lock_guard lock_reinit(mutex_); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock lock(*(costmap->getMutex())); + // Update for the current goal checker's state geometry_msgs::msg::Pose pose_tolerance; geometry_msgs::msg::Twist vel_tolerance; diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 7d32283db7c..afd32381459 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -140,6 +140,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands nav2_core::GoalChecker * goal_checker) { if (path_updated_) { + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock lock(*(costmap->getMutex())); + std::lock_guard lock_reinit(mutex_); try { geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(getSampledPathPt()); From 8a329f55de7a368786de41a758aadfa222cc4d03 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 24 Oct 2023 09:31:57 -0700 Subject: [PATCH 2/9] Adjust the Variable types in Nav2_costmap_2d pkg in [nav2_humble] #3891 (#3900) (#3902) * image.hpp #3891 * Update image.hpp (cherry picked from commit 7a7c6da59a923f06f51ca98ad0b0cc412801ad12) Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> --- nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp index db7ae8fce84..888455168d3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp @@ -50,7 +50,7 @@ class Image * Share image data between new and old object. * Changing data in a new object will affect the given one and vice versa */ - Image(Image & other); + Image(const Image & other); /** * @brief Create image from the other (move constructor) @@ -132,7 +132,7 @@ Image::Image(size_t rows, size_t columns, T * data, size_t step) } template -Image::Image(Image & other) +Image::Image(const Image & other) : data_start_{other.data_start_}, rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {} From e9f21c4f8669da1a84ad0cafcf9700ae67956430 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Sat, 28 Oct 2023 15:29:05 -0700 Subject: [PATCH 3/9] Log if BT rate is exceeded (#3909) (#3913) (cherry picked from commit a11cdd80665238628be9eef4b439067d4b675b7b) Co-authored-by: Steve Macenski --- nav2_behavior_tree/src/behavior_tree_engine.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 7bfc003cf1c..a5ee96af5e1 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -55,7 +55,12 @@ BehaviorTreeEngine::run( onLoop(); - loopRate.sleep(); + if (!loopRate.sleep()) { + RCLCPP_WARN( + rclcpp::get_logger("BehaviorTreeEngine"), + "Behavior Tree tick rate %0.2f was exceeded!", + 1.0 / (loopRate.period().count() * 1.0e-9)); + } } } catch (const std::exception & ex) { RCLCPP_ERROR( From 7c832e6b3d3dd30454476a2110064b6929120006 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 30 Oct 2023 13:41:26 -0700 Subject: [PATCH 4/9] Update theta_star_planner.cpp (#3918) (#3922) (cherry picked from commit 0629ff36e36ecc135b9b13fc78f213cfe0a361ef) Co-authored-by: Steve Macenski --- nav2_theta_star_planner/src/theta_star_planner.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index c509afb065c..f93eaa44d73 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -91,6 +91,8 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( nav_msgs::msg::Path global_path; auto start_time = std::chrono::steady_clock::now(); + std::unique_lock lock(*(planner_->costmap_->getMutex())); + // Corner case of start and goal beeing on the same cell unsigned int mx_start, my_start, mx_goal, my_goal; if (!planner_->costmap_->worldToMap( From d84b8f8704298e24dde39f9084f34962951e3d2c Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 30 Oct 2023 13:56:21 -0700 Subject: [PATCH 5/9] Fixing subtree issues with blackboard shared resources (3640) (backport #3911) (#3916) * Fixing subtree issues with blackboard shared resources (3640) (#3911) * fixing subtree issues * Update bt_action_server_impl.hpp (cherry picked from commit 4b4465dfc9427b95e98aef70620bedd933cfbe56) # Conflicts: # nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp * Update bt_action_server_impl.hpp --------- Co-authored-by: Steve Macenski --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index dd0400223d8..92526a7206f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -221,6 +221,11 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena // Create the Behavior Tree from the XML input try { tree_ = bt_->createTreeFromText(xml_string, blackboard_); + for (auto & blackboard : tree_.blackboard_stack) { + blackboard->set("node", client_node_); + blackboard->set("server_timeout", default_server_timeout_); + blackboard->set("bt_loop_duration", bt_loop_duration_); + } } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); return false; From 96e60672bee15e18c5a4ab7c17692e8a3ba498a1 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 3 Nov 2023 11:19:03 -0700 Subject: [PATCH 6/9] Handle NaNs in AMCL beam sensor model (#3929) (#3937) * Handle NaNs in AMCL beam sensor model Signed-off-by: Michel Hidalgo * Use proper isnan check Signed-off-by: Michel Hidalgo --------- Signed-off-by: Michel Hidalgo (cherry picked from commit 06c35504e461d8c3233ce76ca35d63e61f95429a) Co-authored-by: Michel Hidalgo --- nav2_amcl/src/sensors/laser/beam_model.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/nav2_amcl/src/sensors/laser/beam_model.cpp b/nav2_amcl/src/sensors/laser/beam_model.cpp index ebbfedcc7be..b6f281cbf78 100644 --- a/nav2_amcl/src/sensors/laser/beam_model.cpp +++ b/nav2_amcl/src/sensors/laser/beam_model.cpp @@ -72,6 +72,12 @@ BeamModel::sensorFunction(LaserData * data, pf_sample_set_t * set) step = (data->range_count - 1) / (self->max_beams_ - 1); for (i = 0; i < data->range_count; i += step) { obs_range = data->ranges[i][0]; + + // Check for NaN + if (isnan(obs_range)) { + continue; + } + obs_bearing = data->ranges[i][1]; // Compute the range according to the map From 054f3f5ed7f0a11428fb114d777b8ce7a293cb82 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 3 Nov 2023 11:33:34 -0700 Subject: [PATCH 7/9] Fix Wrong Map Pointer (#3311) (#3315) (#3938) Not assigning fixed map pointer to particle filter, using latest when resample. (cherry picked from commit cc6f205b5140b2485c1f01a7249b6cbddad69ab4) Signed-off-by: Borong Yuan Signed-off-by: Borong Yuan (cherry picked from commit 85735ea61b97da1401edcf397ec2e5f1ebc5ff55) Co-authored-by: Borong Yuan --- nav2_amcl/include/nav2_amcl/pf/pf.hpp | 5 ++--- nav2_amcl/src/amcl_node.cpp | 5 ++--- nav2_amcl/src/pf/pf.c | 7 +++---- 3 files changed, 7 insertions(+), 10 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/pf/pf.hpp b/nav2_amcl/include/nav2_amcl/pf/pf.hpp index afcb9daafbb..c4759d06b48 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf.hpp @@ -129,7 +129,6 @@ typedef struct _pf_t // Function used to draw random pose samples pf_init_model_fn_t random_pose_fn; - void * random_pose_data; double dist_threshold; // distance threshold in each axis over which the pf is considered to not // be converged @@ -141,7 +140,7 @@ typedef struct _pf_t pf_t * pf_alloc( int min_samples, int max_samples, double alpha_slow, double alpha_fast, - pf_init_model_fn_t random_pose_fn, void * random_pose_data); + pf_init_model_fn_t random_pose_fn); // Free an existing filter void pf_free(pf_t * pf); @@ -159,7 +158,7 @@ void pf_init_model(pf_t * pf, pf_init_model_fn_t init_fn, void * init_data); void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data); // Resample the distribution -void pf_update_resample(pf_t * pf); +void pf_update_resample(pf_t * pf, void * random_pose_data); // Compute the CEP statistics (mean and variance). // void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var); diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 4afae2a122a..65df56dac15 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -679,7 +679,7 @@ AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) // Resample the particles if (!(++resample_count_ % resample_interval_)) { - pf_update_resample(pf_); + pf_update_resample(pf_, reinterpret_cast(map_)); resampled = true; } @@ -1571,8 +1571,7 @@ AmclNode::initParticleFilter() // Create the particle filter pf_ = pf_alloc( min_particles_, max_particles_, alpha_slow_, alpha_fast_, - (pf_init_model_fn_t)AmclNode::uniformPoseGenerator, - reinterpret_cast(map_)); + (pf_init_model_fn_t)AmclNode::uniformPoseGenerator); pf_->pop_err = pf_err_; pf_->pop_z = pf_z_; diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index 63d71b99bd0..de4e3247d10 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -47,7 +47,7 @@ static int pf_resample_limit(pf_t * pf, int k); pf_t * pf_alloc( int min_samples, int max_samples, double alpha_slow, double alpha_fast, - pf_init_model_fn_t random_pose_fn, void * random_pose_data) + pf_init_model_fn_t random_pose_fn) { int i, j; pf_t * pf; @@ -59,7 +59,6 @@ pf_t * pf_alloc( pf = calloc(1, sizeof(pf_t)); pf->random_pose_fn = random_pose_fn; - pf->random_pose_data = random_pose_data; pf->min_samples = min_samples; pf->max_samples = max_samples; @@ -291,7 +290,7 @@ void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_d // Resample the distribution -void pf_update_resample(pf_t * pf) +void pf_update_resample(pf_t * pf, void * random_pose_data) { int i; double total; @@ -344,7 +343,7 @@ void pf_update_resample(pf_t * pf) sample_b = set_b->samples + set_b->sample_count++; if (drand48() < w_diff) { - sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data); + sample_b->pose = (pf->random_pose_fn)(random_pose_data); } else { // Can't (easily) combine low-variance sampler with KLD adaptive // sampling, so we'll take the more traditional route. From 42c9de41ee6e0600e238d99252e2cb7fa9f132c7 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 6 Nov 2023 10:55:41 -0800 Subject: [PATCH 8/9] Fix NaN in Updated PathAlign (#3943) (#3945) (cherry picked from commit 3d14d98e4a69de546036ceeb3919592c8f5646f4) Co-authored-by: Steve Macenski --- nav2_mppi_controller/src/critics/path_align_critic.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 4b4d93abf3f..cd913e5b0ee 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -126,6 +126,8 @@ void PathAlignCritic::score(CriticData & data) } if (num_samples > 0) { cost[t] = summed_path_dist / num_samples; + } else { + cost[t] = 0.0f; } } From a31db293f76fa391b33d892139add02ed952e6ee Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 6 Nov 2023 13:48:02 -0800 Subject: [PATCH 9/9] Fix for robot footprint collision in obstacles critic (#3878) (#3947) * Inscribed/Circumscribed costs must be updated to take into account the current shape of the robot. Was previous only being called once in initialize(). * Add early return to avoid calculations if footprint has not changed. * Only update radius if using footprint. Add perf timers. * Remove perf timers. * Update comments. --------- Co-authored-by: Leif Terry (cherry picked from commit 98af3b9a28d9180ba22b17573f01cccaf7cbe04b) Co-authored-by: LeifHookedWireless --- .../critics/obstacles_critic.hpp | 1 + .../src/critics/obstacles_critic.cpp | 18 ++++++++++++++++-- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index fe17906bae4..fd6eeab0128 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -92,6 +92,7 @@ class ObstaclesCritic : public CriticFunction float possibly_inscribed_cost_; float collision_margin_distance_; float near_goal_distance_; + float circumscribed_cost_{0}, circumscribed_radius_{0}; unsigned int power_{0}; float repulsion_weight_, critical_weight_{0}; diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index bbca7f7fc71..af1b6636d18 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -55,6 +55,13 @@ float ObstaclesCritic::findCircumscribedCost( { double result = -1.0; bool inflation_layer_found = false; + + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + // check if the costmap has an inflation layer for (auto layer = costmap->getLayeredCostmap()->getPlugins()->begin(); layer != costmap->getLayeredCostmap()->getPlugins()->end(); @@ -66,7 +73,6 @@ float ObstaclesCritic::findCircumscribedCost( } inflation_layer_found = true; - const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); const double resolution = costmap->getCostmap()->getResolution(); result = inflation_layer->computeCost(circum_radius / resolution); auto getParam = parameters_handler_->getParamGetter(name_); @@ -84,7 +90,10 @@ float ObstaclesCritic::findCircumscribedCost( "significantly slow down planning times and not avoid anything but absolute collisions!"); } - return static_cast(result); + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; } float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) @@ -109,6 +118,11 @@ void ObstaclesCritic::score(CriticData & data) return; } + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + } + // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {