BallNearByPositionerSkillPlannerが余分な空コマンドを発行してしまうバグを修正 (#650)

ibis-ssl · Dec 17, 2024 · 7562782 · 7562782
1 parent fb7a18a
commit 7562782
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 11 deletions.
diff --git a/crane_msgs/msg/control/RobotCommand.msg b/crane_msgs/msg/control/RobotCommand.msg
@@ -43,6 +43,7 @@ uint8 POLAR_VELOCITY_TARGET_MODE=3
 uint8 control_mode
 
 string skill_name
+string planner_name
 
 LocalCameraMode[<=1] local_camera_mode
 PositionTargetMode[<=1] position_target_mode

diff --git a/session/crane_planner_plugins/src/skill_planners.cpp b/session/crane_planner_plugins/src/skill_planners.cpp
@@ -239,11 +239,11 @@ std::pair<PlannerBase::Status, std::vector<crane_msgs::msg::RobotCommand>>
 BallNearByPositionerSkillPlanner::calculateRobotCommand(
   [[maybe_unused]] const std::vector<RobotIdentifier> & robots, PlannerContext & context)
 {
-  std::vector<crane_msgs::msg::RobotCommand> robot_commands(skills.size());
-  std::ranges::transform(skills, std::back_inserter(robot_commands), [&](const auto & skill) {
-    skill->run();
-    return skill->getRobotCommand();
-  });
+  auto robot_commands = skills | ranges::views::transform([this](const auto & skill) {
+                          skill->run();
+                          return skill->getRobotCommand();
+                        }) |
+                        ranges::to<std::vector<crane_msgs::msg::RobotCommand>>();
   return {PlannerBase::Status::RUNNING, robot_commands};
 }
 

diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp
@@ -44,20 +44,22 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
       RCLCPP_INFO(
         get_logger(), "セッション設定を読み込みます : %s", config_file.filename().string().c_str());
       auto config = YAML::LoadFile(config_file.c_str());
-      std::cout << "NAME : " << config["name"] << std::endl;
-      std::cout << "DESCRIPTION : " << config["description"] << std::endl;
-      std::cout << "SESSIONS : " << std::endl;
+      std::stringstream ss;
+      ss << "NAME : " << config["name"] << std::endl;
+      ss << "DESCRIPTION : " << config["description"] << std::endl;
+      ss << "SESSIONS : " << std::endl;
 
       std::vector<SessionCapacity> session_capacity_list;
       for (auto session_node : config["sessions"]) {
-        std::cout << "\tNAME     : " << session_node["name"] << std::endl;
-        std::cout << "\tCAPACITY : " << session_node["capacity"] << std::endl;
+        ss << "\tNAME     : " << session_node["name"] << std::endl;
+        ss << "\tCAPACITY : " << session_node["capacity"] << std::endl;
         session_capacity_list.emplace_back(SessionCapacity(
           {session_node["name"].as<std::string>(), session_node["capacity"].as<int>()}));
       }
       robot_selection_priority_map[config["name"].as<std::string>()] = session_capacity_list;
 
-      std::cout << "----------------------------------------" << std::endl;
+      ss << "----------------------------------------" << std::endl;
+      RCLCPP_DEBUG(get_logger(), "%s", ss.str().c_str());
     }
   };
 
@@ -186,6 +188,10 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
     PlannerContext planner_context;
     for (const auto & planner : available_planners) {
       auto commands_msg = planner->getRobotCommands(planner_context);
+      ranges::for_each(
+        commands_msg.robot_commands, [&](crane_msgs::msg::RobotCommand & robot_command) {
+          robot_command.planner_name = planner->name;
+        });
       msg.robot_commands.insert(
         msg.robot_commands.end(), commands_msg.robot_commands.begin(),
         commands_msg.robot_commands.end());