From cd015fe0257ce65ef7a9754fbd6793979255e7a8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 20 Oct 2024 16:50:24 +0900 Subject: [PATCH] =?UTF-8?q?rvo=E3=83=97=E3=83=A9=E3=83=B3=E3=83=8A?= =?UTF-8?q?=E8=AA=BF=E6=95=B4=20(#615)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- crane_local_planner/src/rvo2_planner.cpp | 10 ++++++++-- crane_msgs/msg/control/RobotCommand.msg | 2 +- crane_sender/src/ibis_sender_node.cpp | 2 +- 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp index 15f64e43f..5aea3643a 100644 --- 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 index 14e19d1a1..5abca865c 100755 --- 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 index aa11e6a33..b00fa492d 100644 --- 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;