RVO2Plannerでのプロファイルの値を出力

Author: HansRobo

crane_local_planner/include/crane_local_planner/rvo2_planner.hpp

@@ -26,7 +26,7 @@ class RVO2Planner : public LocalPlannerBase
 public:
   explicit RVO2Planner(rclcpp::Node & node);
 
-  void reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg);
+  void reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg);
 
   crane_msgs::msg::RobotCommands extractRobotCommandsFromRVOSim(
     const crane_msgs::msg::RobotCommands & msg);

crane_local_planner/src/rvo2_planner.cpp

@@ -55,7 +55,7 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node)
     [this](const crane_msgs::msg::RobotFeedbackArray & msg) { latest_feedback = msg; });
 }
 
-void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg)
+void RVO2Planner::reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg)
 {
   if (
     world_model->getMsg().play_situation.command_raw.value ==
@@ -70,7 +70,7 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms
     }
   }
   // 味方ロボット：RVO内の位置・速度（＝進みたい方向）の更新
-  for (const auto & command : msg.robot_commands) {
+  for (auto & command : msg.robot_commands) {
     rvo_sim->setAgentPosition(
       command.robot_id, RVO::Vector2(command.current_pose.x, command.current_pose.y));
     rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(0.f, 0.f));
@@ -151,6 +151,9 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms
           max_vel = std::min(max_vel, 1.0);
         }
 
+        command.local_planner_config.final_planned_max_acceleration = acceleration;
+        command.local_planner_config.final_planned_max_velocity = max_vel;
+
         target_vel = target_vel.normalized() * max_vel;
 
         if (target_vel.norm() < command.local_planner_config.terminal_velocity) {

crane_msgs/msg/control/LocalPlannerConfig.msg

@@ -10,3 +10,6 @@ float32 terminal_velocity 0
 uint8 priority 0
 
 float32 theta_tolerance 0.0
+
+float32 final_planned_max_acceleration -1.0
+float32 final_planned_max_velocity -1.0