Merge remote-tracking branch 'upstream/BR2' into fix-devcontainer

nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp

@@ -29,6 +29,7 @@ class DistanceTraveledConditionTestFixture : public nav2_behavior_tree::Behavior
   {
     config_->input_ports["global_frame"] = "map";
     config_->input_ports["robot_base_frame"] = "base_link";
+    config_->input_ports["distance"] = 1.0;
     bt_node_ = std::make_shared<nav2_behavior_tree::DistanceTraveledCondition>(
       "distance_traveled", *config_);
   }
@@ -89,6 +90,5 @@ int main(int argc, char ** argv)
 
   // shutdown ROS
   rclcpp::shutdown();
-
   return all_successful;
 }

nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp

@@ -27,6 +27,8 @@ class GloballyUpdatedGoalConditionTestFixture : public nav2_behavior_tree::Behav
 public:
   void SetUp()
   {
+    config_->input_ports["goals"] = "";
+    config_->input_ports["goal"] = "";
     bt_node_ = std::make_shared<nav2_behavior_tree::GloballyUpdatedGoalCondition>(
       "globally_updated_goal", *config_);
   }

nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp

@@ -28,6 +28,8 @@ class GoalUpdatedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT
 public:
   void SetUp()
   {
+    config_->input_ports["goals"] = "";
+    config_->input_ports["goal"] = "";
     bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedCondition>(
       "goal_updated", *config_);
   }

nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp

@@ -26,6 +26,7 @@ class InitialPoseReceivedConditionTestFixture : public nav2_behavior_tree::Behav
 public:
   void SetUp()
   {
+    config_->input_ports["initial_pose_received"] = false;
     bt_node_ = std::make_shared<nav2_behavior_tree::InitialPoseReceived>("TestNode", *config_);
   }
 

nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp

@@ -18,10 +18,12 @@
 #include <set>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
 #include "nav2_util/robot_utils.hpp"
 
 #include "utils/test_behavior_tree_fixture.hpp"
 #include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 using namespace std::chrono;  // NOLINT
 using namespace std::chrono_literals;  // NOLINT
@@ -33,6 +35,9 @@ class PathExpiringTimerConditionTestFixture : public nav2_behavior_tree::Behavio
   {
     node_ = std::make_shared<rclcpp::Node>("test_path_expiring_condition");
     config_ = new BT::NodeConfiguration();
+    config_->input_ports["seconds"] = 1.0;
+    config_->input_ports["path"] = "";
+
     config_->blackboard = BT::Blackboard::create();
     config_->blackboard->set("node", node_);
     bt_node_ = std::make_shared<nav2_behavior_tree::PathExpiringTimerCondition>(

nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp

@@ -32,6 +32,7 @@ class TimeExpiredConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT
 public:
   void SetUp()
   {
+    config_->input_ports["seconds"] = 1.0;
     bt_node_ = std::make_shared<nav2_behavior_tree::TimeExpiredCondition>(
       "time_expired", *config_);
   }

nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp

@@ -58,6 +58,7 @@ class RecoveryNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixtu
 public:
   void SetUp() override
   {
+    config_->input_ports["number_of_retries"] = 1;
     bt_node_ = std::make_shared<nav2_behavior_tree::RecoveryNode>(
       "recovery_node", *config_);
     first_child_ = std::make_shared<RecoveryDummy>();

nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp

@@ -30,6 +30,7 @@ class DistanceControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTes
 public:
   void SetUp()
   {
+    config_->input_ports["distance"] = 1.0;
     config_->input_ports["global_frame"] = "map";
     config_->input_ports["robot_base_frame"] = "base_link";
     bt_node_ = std::make_shared<nav2_behavior_tree::DistanceController>(

nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp

@@ -36,7 +36,8 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree
     poses1.push_back(goal1);
     config_->blackboard->set("goal", goal1);
     config_->blackboard->set("goals", poses1);
-
+    config_->input_ports["goals"] = "";
+    config_->input_ports["goal"] = "";
     bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedController>(
       "goal_updated_controller", *config_);
     dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();

nav2_behavior_tree/test/plugins/decorator/test_rate_controller.cpp

@@ -29,6 +29,7 @@ class RateControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFix
 public:
   void SetUp()
   {
+    config_->input_ports["hz"] = 10.0;
     bt_node_ = std::make_shared<nav2_behavior_tree::RateController>(
       "rate_controller", *config_);
     dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();

nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp

@@ -45,6 +45,13 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi
     std::vector<geometry_msgs::msg::PoseStamped> fake_poses;
     config_->blackboard->set("goals", fake_poses);  // NOLINT
 
+    config_->input_ports["min_rate"] = 0.1;
+    config_->input_ports["max_rate"] = 1.0;
+    config_->input_ports["min_speed"] = 0.0;
+    config_->input_ports["max_speed"] = 0.5;
+    config_->input_ports["goals"] = "";
+    config_->input_ports["goal"] = "";
+
     bt_node_ = std::make_shared<nav2_behavior_tree::SpeedController>("speed_controller", *config_);
     dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();
     bt_node_->setChild(dummy_node_.get());

nav2_behavior_tree/test/test_bt_utils.cpp

@@ -59,13 +59,7 @@ TEST(PointPortTest, test_wrong_syntax)
 
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<geometry_msgs::msg::Point>>("PointPort");
-  auto tree = factory.createTreeFromText(xml_txt);
-
-  geometry_msgs::msg::Point value;
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(value.x, 0.0);
-  EXPECT_EQ(value.y, 0.0);
-  EXPECT_EQ(value.z, 0.0);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 
   xml_txt =
     R"(
@@ -75,11 +69,7 @@ TEST(PointPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  tree = factory.createTreeFromText(xml_txt);
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(value.x, 0.0);
-  EXPECT_EQ(value.y, 0.0);
-  EXPECT_EQ(value.z, 0.0);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 }
 
 TEST(PointPortTest, test_correct_syntax)
@@ -115,14 +105,8 @@ TEST(QuaternionPortTest, test_wrong_syntax)
 
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<geometry_msgs::msg::Quaternion>>("QuaternionPort");
-  auto tree = factory.createTreeFromText(xml_txt);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 
-  geometry_msgs::msg::Quaternion value;
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(value.x, 0.0);
-  EXPECT_EQ(value.y, 0.0);
-  EXPECT_EQ(value.z, 0.0);
-  EXPECT_EQ(value.w, 1.0);
 
   xml_txt =
     R"(
@@ -132,12 +116,7 @@ TEST(QuaternionPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  tree = factory.createTreeFromText(xml_txt);
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(value.x, 0.0);
-  EXPECT_EQ(value.y, 0.0);
-  EXPECT_EQ(value.z, 0.0);
-  EXPECT_EQ(value.w, 1.0);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 }
 
 TEST(QuaternionPortTest, test_correct_syntax)
@@ -174,19 +153,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax)
 
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<geometry_msgs::msg::PoseStamped>>("PoseStampedPort");
-  auto tree = factory.createTreeFromText(xml_txt);
-
-  geometry_msgs::msg::PoseStamped value;
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0);
-  EXPECT_EQ(value.header.frame_id, "");
-  EXPECT_EQ(value.pose.position.x, 0.0);
-  EXPECT_EQ(value.pose.position.y, 0.0);
-  EXPECT_EQ(value.pose.position.z, 0.0);
-  EXPECT_EQ(value.pose.orientation.x, 0.0);
-  EXPECT_EQ(value.pose.orientation.y, 0.0);
-  EXPECT_EQ(value.pose.orientation.z, 0.0);
-  EXPECT_EQ(value.pose.orientation.w, 1.0);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 
   xml_txt =
     R"(
@@ -196,17 +163,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  tree = factory.createTreeFromText(xml_txt);
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0);
-  EXPECT_EQ(value.header.frame_id, "");
-  EXPECT_EQ(value.pose.position.x, 0.0);
-  EXPECT_EQ(value.pose.position.y, 0.0);
-  EXPECT_EQ(value.pose.position.z, 0.0);
-  EXPECT_EQ(value.pose.orientation.x, 0.0);
-  EXPECT_EQ(value.pose.orientation.y, 0.0);
-  EXPECT_EQ(value.pose.orientation.z, 0.0);
-  EXPECT_EQ(value.pose.orientation.w, 1.0);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 }
 
 TEST(PoseStampedPortTest, test_correct_syntax)

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -463,6 +463,16 @@ class NodeHybrid
    */
   bool backtracePath(CoordinateVector & path);
 
+  /**
+    * @brief Destroy shared pointer assets at the end of the process that don't
+    * require normal destruction handling
+    */
+  static void destroyStaticAssets()
+  {
+    inflation_layer.reset();
+    costmap_ros.reset();
+  }
+
   NodeHybrid * parent;
   Coordinates pose;
 

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -319,6 +319,7 @@ void SmacPlannerHybrid::cleanup()
   RCLCPP_INFO(
     _logger, "Cleaning up plugin %s of type SmacPlannerHybrid",
     _name.c_str());
+  nav2_smac_planner::NodeHybrid::destroyStaticAssets();
   _a_star.reset();
   _smoother.reset();
   if (_costmap_downsampler) {

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -258,6 +258,7 @@ void SmacPlannerLattice::cleanup()
   RCLCPP_INFO(
     _logger, "Cleaning up plugin %s of type SmacPlannerLattice",
     _name.c_str());
+  nav2_smac_planner::NodeHybrid::destroyStaticAssets();
   _a_star.reset();
   _smoother.reset();
   _raw_plan_publisher.reset();

nav2_smac_planner/test/test_a_star.cpp

@@ -215,6 +215,7 @@ TEST(AStarTest, test_a_star_se2)
   EXPECT_GT(expansions->size(), 5u);
 
   delete costmapA;
+  nav2_smac_planner::NodeHybrid::destroyStaticAssets();
 }
 
 TEST(AStarTest, test_a_star_lattice)
@@ -290,6 +291,7 @@ TEST(AStarTest, test_a_star_lattice)
   }
 
   delete costmapA;
+  nav2_smac_planner::NodeHybrid::destroyStaticAssets();
 }
 
 TEST(AStarTest, test_se2_single_pose_path)
@@ -348,6 +350,7 @@ TEST(AStarTest, test_se2_single_pose_path)
   EXPECT_GE(path.size(), 1u);
 
   delete costmapA;
+  nav2_smac_planner::NodeHybrid::destroyStaticAssets();
 }
 
 TEST(AStarTest, test_constants)

nav2_smac_planner/test/test_nodehybrid.cpp

@@ -236,6 +236,7 @@ TEST(NodeHybridTest, test_obstacle_heuristic)
   EXPECT_EQ(wide_passage_cost, two_passages_cost);
 
   delete costmapA;
+  nav2_smac_planner::NodeHybrid::destroyStaticAssets();
 }
 
 TEST(NodeHybridTest, test_node_debin_neighbors)

nav2_smac_planner/test/test_smoother.cpp

@@ -189,4 +189,5 @@ TEST(SmootherTest, test_full_smoother)
   EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.w, 0.0, 1e-3);
 
   delete costmap;
+  nav2_smac_planner::NodeHybrid::destroyStaticAssets();
 }

nav2_util/test/test_actions.cpp

@@ -81,6 +81,13 @@ class FibonacciServerNode : public rclcpp::Node
     // assert(action_server_->is_cancel_requested() == false);
     // auto feedback = std::make_shared<Fibonacci::Feedback>();
     // action_server_->publish_feedback(feedback);
+    omit_preempt_subs_.reset();
+    activate_subs_.reset();
+    deactivate_subs_.reset();
+    while (action_server_->is_running()) {
+      std::this_thread::sleep_for(10ms);
+    }
+    action_server_->deactivate();
     action_server_.reset();
   }
 
@@ -150,21 +157,33 @@ class RclCppFixture
       std::make_shared<std::thread>(std::bind(&RclCppFixture::server_thread_func, this));
   }
 
+  void TearDown()
+  {
+    stop_.store(true);
+    if (server_thread_ && server_thread_->joinable()) {
+      server_thread_->join();
+      server_thread_.reset();
+    }
+  }
+
   ~RclCppFixture()
   {
-    server_thread_->join();
   }
 
   void server_thread_func()
   {
     auto node = std::make_shared<FibonacciServerNode>();
     node->on_init();
-    rclcpp::spin(node->get_node_base_interface());
+    while (rclcpp::ok() && !stop_.load()) {
+      rclcpp::spin_some(node->get_node_base_interface());
+      std::this_thread::sleep_for(10ms);
+    }
     node->on_term();
     node.reset();
   }
 
   std::shared_ptr<std::thread> server_thread_;
+  std::atomic<bool> stop_{false};
 };
 
 RclCppFixture g_rclcppfixture;
@@ -189,6 +208,9 @@ class ActionTestNode : public rclcpp::Node
 
   void on_term()
   {
+    omit_prempt_pub_.reset();
+    activate_pub_.reset();
+    deactivate_pub_.reset();
     action_client_.reset();
   }
 
@@ -547,6 +569,7 @@ int main(int argc, char ** argv)
   g_rclcppfixture.Setup();
   ::testing::InitGoogleTest(&argc, argv);
   auto result = RUN_ALL_TESTS();
+  g_rclcppfixture.TearDown();
   rclcpp::shutdown();
   return result;
 }