Fix: bt_navigator crashes on lc transitions (#2848)

* fix empty address access on halt all actions

* fix unsafe declaration of parameters

* restore odom smoother

* fix styling issues

* add missing semicolumn

Author: Erwin Lejeune

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -87,6 +87,10 @@ BehaviorTreeEngine::createTreeFromFile(
 void
 BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node)
 {
+  if (!root_node) {
+    return;
+  }
+  
   // this halt signal should propagate through the entire tree.
   root_node->halt();
 

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -29,11 +29,17 @@ NavigateThroughPosesNavigator::configure(
 {
   start_time_ = rclcpp::Time(0);
   auto node = parent_node.lock();
-  node->declare_parameter("goals_blackboard_id", std::string("goals"));
+
+  if (!node->has_parameter("goals_blackboard_id")) {
+    node->declare_parameter("goals_blackboard_id", std::string("goals"));
+  }
+
   goals_blackboard_id_ = node->get_parameter("goals_blackboard_id").as_string();
+
   if (!node->has_parameter("path_blackboard_id")) {
     node->declare_parameter("path_blackboard_id", std::string("path"));
   }
+
   path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();
 
   // Odometry smoother object for getting current speed
@@ -48,12 +54,16 @@ NavigateThroughPosesNavigator::getDefaultBTFilepath(
 {
   std::string default_bt_xml_filename;
   auto node = parent_node.lock();
-  std::string pkg_share_dir =
-    ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-  node->declare_parameter<std::string>(
-    "default_nav_through_poses_bt_xml",
-    pkg_share_dir +
-    "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml");
+
+  if (!node->has_parameter("default_nav_through_poses_bt_xml")) {
+    std::string pkg_share_dir =
+      ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+    node->declare_parameter<std::string>(
+      "default_nav_through_poses_bt_xml",
+      pkg_share_dir +
+      "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml");
+  }
+  
   node->get_parameter("default_nav_through_poses_bt_xml", default_bt_xml_filename);
 
   return default_bt_xml_filename;

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -29,11 +29,17 @@ NavigateToPoseNavigator::configure(
 {
   start_time_ = rclcpp::Time(0);
   auto node = parent_node.lock();
-  node->declare_parameter("goal_blackboard_id", std::string("goal"));
+
+  if (!node->has_parameter("goal_blackboard_id")) {
+    node->declare_parameter("goal_blackboard_id", std::string("goal"));
+  }
+
   goal_blackboard_id_ = node->get_parameter("goal_blackboard_id").as_string();
+
   if (!node->has_parameter("path_blackboard_id")) {
     node->declare_parameter("path_blackboard_id", std::string("path"));
   }
+
   path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();
 
   // Odometry smoother object for getting current speed
@@ -54,12 +60,16 @@ NavigateToPoseNavigator::getDefaultBTFilepath(
 {
   std::string default_bt_xml_filename;
   auto node = parent_node.lock();
-  std::string pkg_share_dir =
-    ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-  node->declare_parameter<std::string>(
-    "default_nav_to_pose_bt_xml",
-    pkg_share_dir +
-    "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml");
+  
+  if (!node->has_parameter("default_nav_to_pose_bt_xml")) {
+    std::string pkg_share_dir =
+      ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+    node->declare_parameter<std::string>(
+      "default_nav_to_pose_bt_xml",
+      pkg_share_dir +
+      "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml");
+  }
+  
   node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename);
 
   return default_bt_xml_filename;