Nav2 Sync 2 Nov 16

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);

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<void *>(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<void *>(map_));
+    (pf_init_model_fn_t)AmclNode::uniformPoseGenerator);
   pf_->pop_err = pf_err_;
   pf_->pop_z = pf_z_;
 

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.

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

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -221,6 +221,11 @@ bool BtActionServer<ActionT>::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<rclcpp::Node::SharedPtr>("node", client_node_);
+      blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
+      blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
+    }
   } catch (const std::exception & e) {
     RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());
     return false;

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(

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<T>::Image(size_t rows, size_t columns, T * data, size_t step)
 }
 
 template<class T>
-Image<T>::Image(Image & other)
+Image<T>::Image(const Image & other)
 : data_start_{other.data_start_},
   rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {}
 

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};

nav2_mppi_controller/src/controller.cpp

@@ -91,9 +91,12 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   }
   last_time_called_ = clock_->now();
 
-  std::lock_guard<std::mutex> lock(*parameters_handler_->getLock());
+  std::lock_guard<std::mutex> 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<nav2_costmap_2d::Costmap2D::mutex_t> costmap_lock(*(costmap->getMutex()));
+
   geometry_msgs::msg::TwistStamped cmd =
     optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker);
 

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<float>(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<float>(result);
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(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)) {

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;
     }
   }
 

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -282,6 +282,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 {
   std::lock_guard<std::mutex> lock_reinit(mutex_);
 
+  nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
   // Update for the current goal checker's state
   geometry_msgs::msg::Pose pose_tolerance;
   geometry_msgs::msg::Twist vel_tolerance;

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<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
     std::lock_guard<std::mutex> lock_reinit(mutex_);
     try {
       geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(getSampledPathPt());

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<nav2_costmap_2d::Costmap2D::mutex_t> 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(