rvoプランナ調整 (#615)

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
ibis-ssl · Oct 20, 2024 · cd015fe · cd015fe
1 parent 20c3344
commit cd015fe
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 4 deletions.
diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp
@@ -288,8 +288,14 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg)
       }();
       if (not command.local_planner_config.disable_goal_area_avoidance) {
         bool is_in_penalty_area = isInBox(penalty_area, target_pos, 0.2);
-        constexpr double SURROUNDING_OFFSET = 0.3;
-        constexpr double PENALTY_AREA_OFFSET = 0.1;
+        double SURROUNDING_OFFSET = 0.3;
+        double PENALTY_AREA_OFFSET = 0.1;
+        if (
+          world_model->play_situation.getSituationCommandID() ==
+          crane_msgs::msg::PlaySituation::STOP) {
+          PENALTY_AREA_OFFSET = 0.5;
+          SURROUNDING_OFFSET = 0.6;
+        }
         if (isInBox(
               penalty_area, Point(command.current_pose.x, command.current_pose.y),
               PENALTY_AREA_OFFSET)) {

diff --git a/crane_msgs/msg/control/RobotCommand.msg b/crane_msgs/msg/control/RobotCommand.msg
@@ -29,7 +29,7 @@ geometry_msgs/Pose2D current_pose
 bool enable_local_feedback
 
 float32 target_theta
-float32 omega_limit 10.0
+float32 omega_limit 100.0
 # for local planner
 float32 theta_tolerance 0.0
 

diff --git a/crane_sender/src/ibis_sender_node.cpp b/crane_sender/src/ibis_sender_node.cpp
@@ -152,7 +152,7 @@ class IbisSenderNode : public SenderBase
       packet.stop_emergency = command.stop_flag;
       packet.acceleration_limit = command.local_planner_config.max_acceleration;
       packet.linear_velocity_limit = command.local_planner_config.max_velocity;
-      packet.angular_velocity_limit = command.omega_limit;
+      packet.angular_velocity_limit = 10.;
       packet.prioritize_move = true;
       packet.prioritize_accurate_acceleration = true;