perf(parking_container): reduce callback of freespace_planner and cos…

…tmap_generator Signed-off-by: Takayuki AKAMINE <[email protected]>
takam5f2 · Feb 28, 2024 · 73a96b9 · 73a96b9
1 parent aead86a
commit 73a96b9
Show file tree

Hide file tree

Showing 4 changed files with 72 additions and 8 deletions.
diff --git a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp
@@ -139,6 +139,8 @@ class CostmapGenerator : public rclcpp::Node
     static constexpr const char * combined = "combined";
   };
 
+  void takeData();
+
   /// \brief wait for lanelet2 map to load and build routing graph
   void initLaneletMap();
 

diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp
@@ -195,17 +195,23 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options)
   }
 
   // Subscribers
+   auto noexec_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  auto noexec_subscription_options = rclcpp::SubscriptionOptions();
+  noexec_subscription_options.callback_group = noexec_callback_group;
+
   using std::placeholders::_1;
   sub_objects_ = this->create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>(
-    "~/input/objects", 1, std::bind(&CostmapGenerator::onObjects, this, _1));
+    "~/input/objects", 1, std::bind(&CostmapGenerator::onObjects, this, _1), noexec_subscription_options);
   sub_points_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
-    "~/input/points_no_ground", rclcpp::SensorDataQoS(),
-    std::bind(&CostmapGenerator::onPoints, this, _1));
-  sub_lanelet_bin_map_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
+    "~/input/points_no_ground", rclcpp::SensorDataQoS().keep_last(1),
+    std::bind(&CostmapGenerator::onPoints, this, _1), noexec_subscription_options);
+  sub_lanelet_bin_map_ = 
+  // sub_lanelet_bin_map_ allows to apply callback function to a thread.  
+  this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
     "~/input/vector_map", rclcpp::QoS{1}.transient_local(),
     std::bind(&CostmapGenerator::onLaneletMapBin, this, _1));
   sub_scenario_ = this->create_subscription<tier4_planning_msgs::msg::Scenario>(
-    "~/input/scenario", 1, std::bind(&CostmapGenerator::onScenario, this, _1));
+    "~/input/scenario", 1, std::bind(&CostmapGenerator::onScenario, this, _1), noexec_subscription_options);
 
   // Publishers
   pub_costmap_ = this->create_publisher<grid_map_msgs::msg::GridMap>("~/output/grid_map", 1);
@@ -280,21 +286,48 @@ void CostmapGenerator::onLaneletMapBin(
 void CostmapGenerator::onObjects(
   const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg)
 {
+  assert(false);
   objects_ = msg;
 }
 
 void CostmapGenerator::onPoints(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
 {
+  assert(false);
   points_ = msg;
 }
 
 void CostmapGenerator::onScenario(const tier4_planning_msgs::msg::Scenario::ConstSharedPtr msg)
 {
+  assert(false);
   scenario_ = msg;
 }
 
+void CostmapGenerator::takeData() {
+  rclcpp::MessageInfo message_info;
+
+  autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr objects_msg = std::make_shared<autoware_auto_perception_msgs::msg::PredictedObjects>();
+
+  if (sub_objects_->take(*objects_msg, message_info)) {
+    objects_ = objects_msg;
+  }
+
+  sensor_msgs::msg::PointCloud2::SharedPtr points_msg = std::make_shared<sensor_msgs::msg::PointCloud2>();
+  if (sub_points_->take(*points_msg, message_info)) {
+    points_ = points_msg;
+  }
+
+  tier4_planning_msgs::msg::Scenario::SharedPtr scenario_msg = std::make_shared<tier4_planning_msgs::msg::Scenario>();
+
+  if (sub_scenario_->take(*scenario_msg, message_info)) {
+    scenario_ = scenario_msg;
+  }
+}
+
 void CostmapGenerator::onTimer()
 {
+  // take data
+  takeData();
+
   if (!isActive()) {
     return;
   }

diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp
@@ -153,6 +153,8 @@ class FreespacePlannerNode : public rclcpp::Node
 
   void onTimer();
 
+  void takeData();
+
   void reset();
   bool isPlanRequired();
   void planTrajectory();

diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp
@@ -252,21 +252,27 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti
 
   // Subscribers
   {
+    auto noexec_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    auto noexec_subscription_options = rclcpp::SubscriptionOptions();
+    noexec_subscription_options.callback_group = noexec_callback_group;
+
+    // route_sub_ allows callback to be executed in another thread.
     route_sub_ = create_subscription<LaneletRoute>(
       "~/input/route", rclcpp::QoS{1}.transient_local(),
       std::bind(&FreespacePlannerNode::onRoute, this, _1));
     occupancy_grid_sub_ = create_subscription<OccupancyGrid>(
-      "~/input/occupancy_grid", 1, std::bind(&FreespacePlannerNode::onOccupancyGrid, this, _1));
+      "~/input/occupancy_grid", 1, std::bind(&FreespacePlannerNode::onOccupancyGrid, this, _1), noexec_subscription_options);
     scenario_sub_ = create_subscription<Scenario>(
-      "~/input/scenario", 1, std::bind(&FreespacePlannerNode::onScenario, this, _1));
+      "~/input/scenario", 1, std::bind(&FreespacePlannerNode::onScenario, this, _1), noexec_subscription_options);
     odom_sub_ = create_subscription<Odometry>(
-      "~/input/odometry", 100, std::bind(&FreespacePlannerNode::onOdometry, this, _1));
+      "~/input/odometry", 100, std::bind(&FreespacePlannerNode::onOdometry, this, _1), noexec_subscription_options);
   }
 
   // Publishers
   {
     rclcpp::QoS qos{1};
     qos.transient_local();  // latch
+
     trajectory_pub_ = create_publisher<Trajectory>("~/output/trajectory", qos);
     debug_pose_array_pub_ = create_publisher<PoseArray>("~/debug/pose_array", qos);
     debug_partial_pose_array_pub_ = create_publisher<PoseArray>("~/debug/partial_pose_array", qos);
@@ -324,6 +330,26 @@ void FreespacePlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg)
   reset();
 }
 
+void FreespacePlannerNode::takeData() {
+  rclcpp::MessageInfo message_info;
+
+  OccupancyGrid::SharedPtr occupancy_grid_msg = std::make_shared<OccupancyGrid>();
+
+  if (occupancy_grid_sub_->take(*occupancy_grid_msg, message_info)) {
+    occupancy_grid_ = occupancy_grid_msg;
+  }
+
+  Scenario::SharedPtr scenario_msg = std::make_shared<Scenario>();
+  if (scenario_sub_->take(*scenario_msg, message_info)) {
+    scenario_ = scenario_msg;
+  }
+
+  std::shared_ptr<void> odom_type_erased_msg = odom_sub_->create_message();
+  while (odom_sub_->take_type_erased(odom_type_erased_msg.get(), message_info)) {
+    odom_sub_->handle_message(odom_type_erased_msg, message_info);
+  }
+}
+
 void FreespacePlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg)
 {
   occupancy_grid_ = msg;
@@ -419,6 +445,7 @@ void FreespacePlannerNode::updateTargetIndex()
 
 void FreespacePlannerNode::onTimer()
 {
+  takeData();
   // Check all inputs are ready
   if (!occupancy_grid_ || !route_ || !scenario_ || !odom_) {
     return;