EnjoyRobotics
diff --git a/‎README.md‎
Lines changed: 4 additions & 2 deletions b/‎README.md‎
Lines changed: 4 additions & 2 deletions
diff --git a/‎nav2_amcl/include/nav2_amcl/amcl_node.hpp‎
Lines changed: 11 additions & 0 deletions b/‎nav2_amcl/include/nav2_amcl/amcl_node.hpp‎
Lines changed: 11 additions & 0 deletions
diff --git a/‎nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp‎
Lines changed: 2 additions & 2 deletions b/‎nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp‎
Lines changed: 2 additions & 2 deletions
diff --git a/‎nav2_amcl/package.xml‎
Lines changed: 1 addition & 1 deletion b/‎nav2_amcl/package.xml‎
Lines changed: 1 addition & 1 deletion
diff --git a/‎nav2_amcl/src/amcl_node.cpp‎
Lines changed: 21 additions & 5 deletions b/‎nav2_amcl/src/amcl_node.cpp‎
Lines changed: 21 additions & 5 deletions
diff --git a/‎nav2_amcl/src/pf/pf.c‎
Lines changed: 0 additions & 5 deletions b/‎nav2_amcl/src/pf/pf.c‎
Lines changed: 0 additions & 5 deletions
diff --git a/‎nav2_amcl/src/pf/pf_vector.c‎
Lines changed: 2 additions & 2 deletions b/‎nav2_amcl/src/pf/pf_vector.c‎
Lines changed: 2 additions & 2 deletions
diff --git a/‎nav2_amcl/src/sensors/laser/likelihood_field_model.cpp‎
Lines changed: 11 additions & 11 deletions b/‎nav2_amcl/src/sensors/laser/likelihood_field_model.cpp‎
Lines changed: 11 additions & 11 deletions
diff --git a/‎nav2_behavior_tree/CMakeLists.txt‎
Lines changed: 3 additions & 0 deletions b/‎nav2_behavior_tree/CMakeLists.txt‎
Lines changed: 3 additions & 0 deletions
diff --git a/‎nav2_behavior_tree/README.md‎
Lines changed: 1 addition & 1 deletion b/‎nav2_behavior_tree/README.md‎
Lines changed: 1 addition & 1 deletion
@@ -9,8 +9,8 @@
 For detailed instructions on how to:
 - [Getting Started](https://navigation.ros.org/getting_started/index.html)
 - [Concepts](https://navigation.ros.org/concepts/index.html)
-- [Build](https://navigation.ros.org/build_instructions/index.html#build)
-- [Install](https://navigation.ros.org/build_instructions/index.html#install)
+- [Build](https://navigation.ros.org/development_guides/build_docs/index.html#build)
+- [Install](https://navigation.ros.org/development_guides/build_docs/index.html#install)
 - [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html)
 - [Configure](https://navigation.ros.org/configuration/index.html)
 - [Navigation Plugins](https://navigation.ros.org/plugins/index.html)
@@ -20,6 +20,8 @@ For detailed instructions on how to:
 
 Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate).
 
+If you need professional services related to Nav2, please contact Open Navigation at [email protected].
+
 ## Our Sponsors
 
 Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. 
 
@@ -35,6 +35,7 @@
 #include "nav2_amcl/sensors/laser/laser.hpp"
 #include "nav2_msgs/msg/particle.hpp"
 #include "nav2_msgs/msg/particle_cloud.hpp"
+#include "nav2_msgs/srv/set_initial_pose.hpp"
 #include "nav_msgs/srv/set_map.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "std_srvs/srv/empty.hpp"
@@ -210,6 +211,16 @@ class AmclNode : public nav2_util::LifecycleNode
     const std::shared_ptr<std_srvs::srv::Empty::Request> request,
     std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
+  // service server for providing an initial pose guess
+  rclcpp::Service<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
+  /*
+   * @brief Service callback for an initial pose guess request
+   */
+  void initialPoseReceivedSrv(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> request,
+    std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);
+
   // Let amcl update samples without requiring motion
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
   /*
 
@@ -49,7 +49,7 @@ typedef struct
 
 
 // Return a zero vector
-pf_vector_t pf_vector_zero();
+pf_vector_t pf_vector_zero(void);
 
 // Check for NAN or INF in any component
 // int pf_vector_finite(pf_vector_t a);
@@ -71,7 +71,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
 
 
 // Return a zero matrix
-pf_matrix_t pf_matrix_zero();
+pf_matrix_t pf_matrix_zero(void);
 
 // Check for NAN or INF in any component
 // int pf_matrix_finite(pf_matrix_t a);
 
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.1.12</version>
+  <version>1.1.14</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in
 
@@ -325,13 +325,17 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   // Get rid of the inputs first (services and message filter input), so we
   // don't continue to process incoming messages
   global_loc_srv_.reset();
+  initial_guess_srv_.reset();
   nomotion_update_srv_.reset();
+  executor_thread_.reset();  //  to make sure initial_pose_sub_ completely exit
   initial_pose_sub_.reset();
   laser_scan_connection_.disconnect();
+  tf_listener_.reset();  //  listener may access lase_scan_filter_, so it should be reset earlier
   laser_scan_filter_.reset();
   laser_scan_sub_.reset();
 
   // Map
+  map_sub_.reset();  //  map_sub_ may access map_, so it should be reset earlier
   if (map_ != NULL) {
     map_free(map_);
     map_ = nullptr;
@@ -341,7 +345,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 
   // Transforms
   tf_broadcaster_.reset();
-  tf_listener_.reset();
   tf_buffer_.reset();
 
   // PubSub
@@ -493,6 +496,15 @@ AmclNode::globalLocalizationCallback(
   pf_init_ = false;
 }
 
+void
+AmclNode::initialPoseReceivedSrv(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> req,
+  std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response>/*res*/)
+{
+  initialPoseReceived(std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(req->pose));
+}
+
 // force nomotion updates (amcl updating without requiring motion)
 void
 AmclNode::nomotionUpdateCallback(
@@ -1093,20 +1105,20 @@ AmclNode::initParameters()
   // Semantic checks
   if (laser_likelihood_max_dist_ < 0) {
     RCLCPP_WARN(
-      get_logger(), "You've set laser_likelihood_max_dist to be negtive,"
+      get_logger(), "You've set laser_likelihood_max_dist to be negative,"
       " this isn't allowed so it will be set to default value 2.0.");
     laser_likelihood_max_dist_ = 2.0;
   }
   if (max_particles_ < 0) {
     RCLCPP_WARN(
-      get_logger(), "You've set max_particles to be negtive,"
+      get_logger(), "You've set max_particles to be negative,"
       " this isn't allowed so it will be set to default value 2000.");
     max_particles_ = 2000;
   }
 
   if (min_particles_ < 0) {
     RCLCPP_WARN(
-      get_logger(), "You've set min_particles to be negtive,"
+      get_logger(), "You've set min_particles to be negative,"
       " this isn't allowed so it will be set to default value 500.");
     min_particles_ = 500;
   }
@@ -1120,7 +1132,7 @@ AmclNode::initParameters()
 
   if (resample_interval_ <= 0) {
     RCLCPP_WARN(
-      get_logger(), "You've set resample_interval to be zero or negtive,"
+      get_logger(), "You've set resample_interval to be zero or negative,"
       " this isn't allowed so it will be set to default value to 1.");
     resample_interval_ = 1;
   }
@@ -1533,6 +1545,10 @@ AmclNode::initServices()
     "reinitialize_global_localization",
     std::bind(&AmclNode::globalLocalizationCallback, this, _1, _2, _3));
 
+  initial_guess_srv_ = create_service<nav2_msgs::srv::SetInitialPose>(
+    "set_initial_pose",
+    std::bind(&AmclNode::initialPoseReceivedSrv, this, _1, _2, _3));
+
   nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(
     "request_nomotion_update",
     std::bind(&AmclNode::nomotionUpdateCallback, this, _1, _2, _3));
 
@@ -463,7 +463,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
   // Workspace
   double m[4], c[2][2];
-  size_t count;
   double weight;
 
   // Cluster the samples
@@ -474,7 +473,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
   for (i = 0; i < set->cluster_max_count; i++) {
     cluster = set->clusters + i;
-    cluster->count = 0;
     cluster->weight = 0;
     cluster->mean = pf_vector_zero();
     cluster->cov = pf_matrix_zero();
@@ -490,7 +488,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
   }
 
   // Initialize overall filter stats
-  count = 0;
   weight = 0.0;
   set->mean = pf_vector_zero();
   set->cov = pf_matrix_zero();
@@ -521,10 +518,8 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
     cluster = set->clusters + cidx;
 
-    cluster->count += 1;
     cluster->weight += sample->weight;
 
-    count += 1;
     weight += sample->weight;
 
     // Compute mean
 
@@ -35,7 +35,7 @@
 
 
 // Return a zero vector
-pf_vector_t pf_vector_zero()
+pf_vector_t pf_vector_zero(void)
 {
   pf_vector_t c;
 
@@ -130,7 +130,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
 
 
 // Return a zero matrix
-pf_matrix_t pf_matrix_zero()
+pf_matrix_t pf_matrix_zero(void)
 {
   int i, j;
   pf_matrix_t c;
 
@@ -53,6 +53,17 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
 
   self = reinterpret_cast<LikelihoodFieldModel *>(data->laser);
 
+  // Pre-compute a couple of things
+  double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
+  double z_rand_mult = 1.0 / data->range_max;
+
+  step = (data->range_count - 1) / (self->max_beams_ - 1);
+
+  // Step size must be at least 1
+  if (step < 1) {
+    step = 1;
+  }
+
   total_weight = 0.0;
 
   // Compute the sample weights
@@ -65,17 +76,6 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
 
     p = 1.0;
 
-    // Pre-compute a couple of things
-    double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
-    double z_rand_mult = 1.0 / data->range_max;
-
-    step = (data->range_count - 1) / (self->max_beams_ - 1);
-
-    // Step size must be at least 1
-    if (step < 1) {
-      step = 1;
-    }
-
     for (i = 0; i < data->range_count; i += step) {
       obs_range = data->ranges[i][0];
       obs_bearing = data->ranges[i][1];
 
@@ -204,6 +204,9 @@ list(APPEND plugin_libs nav2_smoother_selector_bt_node)
 add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp)
 list(APPEND plugin_libs nav2_goal_checker_selector_bt_node)
 
+add_library(nav2_progress_checker_selector_bt_node SHARED plugins/action/progress_checker_selector_node.cpp)
+list(APPEND plugin_libs nav2_progress_checker_selector_bt_node)
+
 add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
 list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)
 
 
@@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT
 
 See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine.
 
-For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/learn-the-basics/bt_basics/
+For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics
Original file line number	Diff line number	Diff line change
`@@ -35,7 +35,7 @@`
`35`	`35`
`36`	`36`
`37`	`37`	`// Return a zero vector`
`38`		`-pf_vector_t pf_vector_zero()`
	`38`	`+pf_vector_t pf_vector_zero(void)`
`39`	`39`	`{`
`40`	`40`	`pf_vector_t c;`
`41`	`41`
`@@ -130,7 +130,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)`
`130`	`130`
`131`	`131`
`132`	`132`	`// Return a zero matrix`
`133`		`-pf_matrix_t pf_matrix_zero()`
	`133`	`+pf_matrix_t pf_matrix_zero(void)`
`134`	`134`	`{`
`135`	`135`	`int i, j;`
`136`	`136`	`pf_matrix_t c;`
Original file line number	Diff line number	Diff line change
`@@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT`
`63`	`63`
`64`	`64`	`See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine.`
`65`	`65`
`66`		`-For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/learn-the-basics/bt_basics/`
	`66`	`+For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics`