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 ==
@@ -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