pulling main

Signed-off-by: Steve Macenski <[email protected]>

.github/workflows/update_ci_image.yaml

@@ -99,7 +99,7 @@ jobs:
       - name: Build and push ${{ github.ref_name }}
         if: steps.config.outputs.trigger == 'true'
         id: docker_build
-        uses: docker/build-push-action@v5
+        uses: docker/build-push-action@v6
         with:
           context: .
           pull: true

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -152,7 +152,8 @@ class AmclNode : public nav2_util::LifecycleNode
   std::recursive_mutex mutex_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
 #if NEW_UNIFORM_SAMPLING
-  static std::vector<std::pair<int, int>> free_space_indices;
+  struct Point2D { int32_t x; int32_t y; };
+  static std::vector<Point2D> free_space_indices;
 #endif
 
   // Transforms
@@ -391,6 +392,7 @@ class AmclNode : public nav2_util::LifecycleNode
   double z_rand_;
   std::string scan_topic_{"scan"};
   std::string map_topic_{"map"};
+  bool freespace_downsampling_ = false;
 };
 
 }  // namespace nav2_amcl

nav2_amcl/include/nav2_amcl/map/map.hpp

@@ -41,20 +41,21 @@ struct _rtk_fig_t;
 // Limits
 #define MAP_WIFI_MAX_LEVELS 8
 
-
+// make sure that the sizeof(map_cell_t) == 5
+#pragma pack(push, 1)
 // Description for a single map cell.
 typedef struct
 {
   // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
-  int occ_state;
+  int8_t occ_state;
 
   // Distance to the nearest occupied cell
-  double occ_dist;
+  float occ_dist;
 
   // Wifi levels
   // int wifi_levels[MAP_WIFI_MAX_LEVELS];
 } map_cell_t;
-
+#pragma pack(pop)
 
 // Description for a map
 typedef struct

nav2_amcl/src/amcl_node.cpp

@@ -227,6 +227,11 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options)
   add_parameter(
     "first_map_only", rclcpp::ParameterValue(false),
     "Set this to true, when you want to load a new map published from the map_server");
+
+  add_parameter(
+    "freespace_downsampling", rclcpp::ParameterValue(
+      false),
+    "Downsample the free space used by the Pose Generator. Use it with large maps to save memory");
 }
 
 AmclNode::~AmclNode()
@@ -404,7 +409,7 @@ AmclNode::checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time las
 }
 
 #if NEW_UNIFORM_SAMPLING
-std::vector<std::pair<int, int>> AmclNode::free_space_indices;
+std::vector<AmclNode::Point2D> AmclNode::free_space_indices;
 #endif
 
 bool
@@ -446,10 +451,10 @@ AmclNode::uniformPoseGenerator(void * arg)
 
 #if NEW_UNIFORM_SAMPLING
   unsigned int rand_index = drand48() * free_space_indices.size();
-  std::pair<int, int> free_point = free_space_indices[rand_index];
+  AmclNode::Point2D free_point = free_space_indices[rand_index];
   pf_vector_t p;
-  p.v[0] = MAP_WXGX(map, free_point.first);
-  p.v[1] = MAP_WYGY(map, free_point.second);
+  p.v[0] = MAP_WXGX(map, free_point.x);
+  p.v[1] = MAP_WYGY(map, free_point.y);
   p.v[2] = drand48() * 2 * M_PI - M_PI;
 #else
   double min_x, max_x, min_y, max_y;
@@ -1106,6 +1111,7 @@ AmclNode::initParameters()
   get_parameter("always_reset_initial_pose", always_reset_initial_pose_);
   get_parameter("scan_topic", scan_topic_);
   get_parameter("map_topic", map_topic_);
+  get_parameter("freespace_downsampling", freespace_downsampling_);
 
   save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
   transform_tolerance_ = tf2::durationFromSec(tmp_tol);
@@ -1435,12 +1441,14 @@ AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg)
 void
 AmclNode::createFreeSpaceVector()
 {
+  int delta = freespace_downsampling_ ? 2 : 1;
   // Index of free space
   free_space_indices.resize(0);
-  for (int i = 0; i < map_->size_x; i++) {
-    for (int j = 0; j < map_->size_y; j++) {
+  for (int i = 0; i < map_->size_x; i += delta) {
+    for (int j = 0; j < map_->size_y; j += delta) {
       if (map_->cells[MAP_INDEX(map_, i, j)].occ_state == -1) {
-        free_space_indices.push_back(std::make_pair(i, j));
+        AmclNode::Point2D point = {i, j};
+        free_space_indices.push_back(point);
       }
     }
   }

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -47,7 +47,8 @@ class BehaviorTreeEngine
    * @param plugin_libraries vector of BT plugin library names to load
    */
   explicit BehaviorTreeEngine(
-    const std::vector<std::string> & plugin_libraries);
+    const std::vector<std::string> & plugin_libraries,
+    rclcpp::Node::SharedPtr node);
   virtual ~BehaviorTreeEngine() {}
 
   /**
@@ -93,6 +94,9 @@ class BehaviorTreeEngine
 protected:
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;
+
+  // Clock
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -173,7 +173,7 @@ bool BtActionServer<ActionT>::on_configure()
   error_code_names_ = node->get_parameter("error_code_names").as_string_array();
 
   // Create the class that registers our custom nodes and executes the BT
-  bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);
+  bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_, client_node_);
 
   // Create the blackboard that will be shared by all of the nodes in the tree
   blackboard_ = BT::Blackboard::create();

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -26,13 +26,16 @@ namespace nav2_behavior_tree
 {
 
 BehaviorTreeEngine::BehaviorTreeEngine(
-  const std::vector<std::string> & plugin_libraries)
+  const std::vector<std::string> & plugin_libraries, rclcpp::Node::SharedPtr node)
 {
   BT::SharedLibrary loader;
   for (const auto & p : plugin_libraries) {
     factory_.registerFromPlugin(loader.getOSName(p));
   }
 
+  // clock for throttled debug log
+  clock_ = node->get_clock();
+
   // FIXME: the next two line are needed for back-compatibility with BT.CPP 3.8.x
   // Note that the can be removed, once we migrate from BT.CPP 4.5.x to 4.6+
   BT::ReactiveSequence::EnableException(false);
@@ -62,8 +65,9 @@ BehaviorTreeEngine::run(
       onLoop();
 
       if (!loopRate.sleep()) {
-        RCLCPP_WARN(
+        RCLCPP_DEBUG_THROTTLE(
           rclcpp::get_logger("BehaviorTreeEngine"),
+          *clock_, 1000,
           "Behavior Tree tick rate %0.2f was exceeded!",
           1.0 / (loopRate.period().count() * 1.0e-9));
       }

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);
 
-  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);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 
   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)