diff --git a/crane_bringup/launch/crane.launch.xml b/crane_bringup/launch/crane.launch.xml
index 58461919d..3f848c809 100644
--- a/crane_bringup/launch/crane.launch.xml
+++ b/crane_bringup/launch/crane.launch.xml
@@ -55,7 +55,7 @@ limitations under the License.
-
+
@@ -69,15 +69,15 @@ limitations under the License.
-
+
-
-
+
+
-
+
@@ -140,6 +140,10 @@ limitations under the License.
+
+
+
+
diff --git a/crane_local_planner/package.xml b/crane_local_planner/package.xml
index 36ebcf6cc..c6059a66f 100755
--- a/crane_local_planner/package.xml
+++ b/crane_local_planner/package.xml
@@ -11,6 +11,7 @@
closest_point_vendor
crane_comm
+ crane_physics
crane_msg_wrappers
crane_msgs
crane_utils
diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp
index d6ac1cc2e..11f56b84d 100644
--- a/crane_local_planner/src/rvo2_planner.cpp
+++ b/crane_local_planner/src/rvo2_planner.cpp
@@ -180,36 +180,12 @@ auto RVO2Planner::reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg) ->
position_diff << pos_mode.target_x - current_position.x(),
pos_mode.target_y - current_position.y();
- double pre_vel = [&]() {
- if (
- auto it = ranges::find_if(
- pre_commands.robot_commands,
- [&](const auto & c) { return c.robot_id == command.robot_id; });
- it != ranges::end(pre_commands.robot_commands)) {
- if (it->position_target_mode.empty()) {
- return 0.0;
- }
- return std::hypot(
- it->position_target_mode.front().target_x - current_position.x(),
- it->position_target_mode.front().target_y - current_position.y()) > 0.01
- ? vel
- : 0.0;
- } else {
- return 0.0;
- }
- }();
+ // 現在速度ベースで減速度を選択
+ double current_speed = std::hypot(command.current_velocity.x, command.current_velocity.y);
+ double max_brk = (current_speed >= planning_deceleration_velocity_threshold)
+ ? planning_deceleration_high_speed
+ : planning_deceleration_low_speed;
- // 減速計算用の減速度を選択（現在速度に応じて高速域・低速域を選択）
- double deceleration_for_planning;
- if (pre_vel >= planning_deceleration_velocity_threshold) {
- deceleration_for_planning = planning_deceleration_high_speed;
- } else {
- deceleration_for_planning = planning_deceleration_low_speed;
- }
-
- // max_brk: 停止のための減速度（planning_decelerationの値を使用）
- double max_brk = deceleration_for_planning;
- // max_acc: 加速フェーズの加速度（planning_accelerationの値を使用、スキルによる制限も適用）
command.local_planner_config.max_acceleration_factors.emplace_back(
crane_msgs::msg::NamedFloat()
.set__name("RVO2Planner::max_acc from parameter")
@@ -228,48 +204,43 @@ auto RVO2Planner::reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg) ->
}
double max_vel = resolveMaxVelocityFactors(command, MAX_VEL);
+ max_vel = std::max(0.4, max_vel);
+ // 1D BangBangTrajectoryで速度プロファイルを計算し、方向は目標への単位ベクトルを使用
+ // 横方向の速度成分をBangBangに渡さないことで旧実装の周回問題を解消
Velocity target_vel;
- target_vel << (pos_mode.target_x - current_position.x()),
- pos_mode.target_y - current_position.y();
+ double distance_to_target = position_diff.norm();
- // 速度超過クランプ: Vision観測誤差でmax_velをわずかに超えた速度を正規化（Sumatra adaptVel相当）
- constexpr double MAX_VEL_TOLERANCE = 0.2;
- Eigen::Vector2d v0(command.current_velocity.x, command.current_velocity.y);
- if (vel > max_vel && vel < max_vel + MAX_VEL_TOLERANCE) {
- v0 = v0 * (max_vel / vel);
- }
+ if (distance_to_target > 0.01) {
+ Eigen::Vector2d dir = position_diff.normalized();
- BangBangTrajectory2D trajectory;
- trajectory.generate(
- Eigen::Vector2d(current_position.x(), current_position.y()),
- Eigen::Vector2d(pos_mode.target_x, pos_mode.target_y), v0, max_vel, max_acc, max_brk);
-
- // BangBangTrajectoryから速度を取得
- // Vision遅延（~100ms）を考慮してlookahead時間を設定
- const double lookahead_time = 0.1;
- Eigen::Vector2d next_vel = trajectory.getVelocity(lookahead_time);
-
- // 目標との距離を計算
- double distance_to_target = std::hypot(
- pos_mode.target_x - current_position.x(), pos_mode.target_y - current_position.y());
-
- // terminal_velocity: スキルが明示的に設定した場合のみ疑似I項として適用する
- // 0（デフォルト・未指定）のときはBangBang軌道の出力に従い、目標で停止する
- const double terminal_vel = command.local_planner_config.terminal_velocity;
- const double min_distance =
- std::max(static_cast(pos_mode.position_tolerance) * 2, 0.03); // 最低3cm
-
- if (terminal_vel > 0 && next_vel.norm() < terminal_vel && distance_to_target > min_distance) {
- // terminal_velocityが指定されており、低速かつ目標から離れている場合に補正
- Eigen::Vector2d direction =
- (Eigen::Vector2d(pos_mode.target_x, pos_mode.target_y) - current_position).normalized();
- target_vel = direction * terminal_vel;
- } else if (next_vel.norm() < 1e-6) {
- // 目標到達済み
- target_vel.setZero();
+ // 現在速度を目標方向に射影してスカラー初速度を算出
+ Eigen::Vector2d v0(command.current_velocity.x, command.current_velocity.y);
+ double v0_along_target = std::clamp(v0.dot(dir), 0.0, max_vel);
+
+ // 1D BangBangで目標方向の速度プロファイルを計算
+ BangBangTrajectory1D bb_trajectory;
+ bb_trajectory.generate(0.0, distance_to_target, v0_along_target, max_vel, max_acc, max_brk);
+
+ const double lookahead_time = 0.1;
+ double speed = bb_trajectory.getVelocity(lookahead_time);
+
+ addOrUpdatePlanningFactor(command, "BBAccel", formatPlanningDouble(max_acc, 1));
+ addOrUpdatePlanningFactor(command, "BBBrk", formatPlanningDouble(max_brk, 1));
+ addOrUpdatePlanningFactor(command, "BBMaxVel", formatPlanningDouble(max_vel, 1));
+ addOrUpdatePlanningFactor(command, "BBSpeed", formatPlanningDouble(speed, 2));
+
+ target_vel = dir * speed;
+
+ // terminal_velocity: パス等で目標通過速度が指定されている場合
+ const double terminal_vel = command.local_planner_config.terminal_velocity;
+ const double min_distance =
+ std::max(static_cast(pos_mode.position_tolerance) * 2, 0.03);
+ if (terminal_vel > 0 && speed < terminal_vel && distance_to_target > min_distance) {
+ target_vel = dir * terminal_vel;
+ }
} else {
- target_vel << next_vel.x(), next_vel.y();
+ target_vel.setZero();
}
// 衝突ファール (crashing) 回避:
// SSLルールでは衝突時の速度ベクトル差をロボット間直線に射影した値が
@@ -367,23 +338,27 @@ auto RVO2Planner::reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg) ->
? 0.0f
: PENALTY_AREA_TIME_HORIZON_OBST);
+ // 診断用: RVO2入力速度（crash/penalty修正後）
+ addOrUpdatePlanningFactor(
+ command, "PrefVel",
+ formatPlanningDouble(target_vel.x(), 2) + "," + formatPlanningDouble(target_vel.y(), 2));
+ addOrUpdatePlanningFactor(
+ command, "PrefVelAngle",
+ formatPlanningDouble(std::atan2(target_vel.y(), target_vel.x()) * 180.0 / M_PI, 1));
+
rvo_sim->setAgentPrefVelocity(command.robot_id, toRVO(target_vel));
rvo_sim->setAgentMaxSpeed(command.robot_id, max_vel);
- // 速度計画トレースに計画点を追加
- if (enable_velocity_plan_trace && !command.velocity_plan_trace.empty()) {
- // 現在時刻から100ms後の予測位置・速度を記録
- const int32_t target_time_us =
- static_cast(lookahead_time * 1e6); // 100ms = 100000us
- Eigen::Vector2d predicted_pos = trajectory.getPosition(lookahead_time);
- Eigen::Vector2d predicted_vel = next_vel;
-
- VelocityPlanTracker::addPlanPoint(
- command.velocity_plan_trace[0], "local_planner", predicted_pos, predicted_vel,
- target_time_us,
- 0 // estimated_arrival_time_us（後で実装可能）
- );
- }
+ // [BangBang無効化] 速度計画トレース（trajectory/next_velに依存するためコメントアウト）
+ // if (enable_velocity_plan_trace && !command.velocity_plan_trace.empty()) {
+ // const int32_t target_time_us =
+ // static_cast(lookahead_time * 1e6);
+ // Eigen::Vector2d predicted_pos = trajectory.getPosition(lookahead_time);
+ // Eigen::Vector2d predicted_vel = next_vel;
+ // VelocityPlanTracker::addPlanPoint(
+ // command.velocity_plan_trace[0], "local_planner", predicted_pos, predicted_vel,
+ // target_time_us, 0);
+ // }
}
for (const auto & enemy_robot : world_model->theirs().robots) {
@@ -420,6 +395,13 @@ auto RVO2Planner::extractVelocityCommandsFromRVOSim(
auto pref_vel = toPoint(rvo_sim->getAgentPrefVelocity(original_command.robot_id));
auto vel = toPoint(rvo_sim->getAgentVelocity(original_command.robot_id));
addOrUpdatePlanningFactor(command, "RVO2PrefSpeed", formatPlanningDouble(pref_vel.norm()));
+ // 診断用: RVO2 prefVelocityと出力速度の角度差
+ addOrUpdatePlanningFactor(
+ command, "RVO2PrefVelAngle",
+ formatPlanningDouble(std::atan2(pref_vel.y(), pref_vel.x()) * 180.0 / M_PI, 1));
+ addOrUpdatePlanningFactor(
+ command, "RVO2OutVelAngle",
+ formatPlanningDouble(std::atan2(vel.y(), vel.x()) * 180.0 / M_PI, 1));
// 速度修正をトレースに記録（RVO2による修正）
if (enable_velocity_plan_trace && !command.velocity_plan_trace.empty()) {
@@ -475,8 +457,9 @@ auto RVO2Planner::extractVelocityCommandsFromRVOSim(
command.polar_velocity_target_mode.front().target_velocity_theta =
std::atan2(vel.y(), vel.x()) + theta_offset;
- // 効率的な加速のための回転制御
- if (command.local_planner_config.enable_rotation_stop_on_accel) {
+// 効率的な加速のための回転制御
+ // if (command.local_planner_config.enable_rotation_stop_on_accel) {
+ if(true){
double move_angle = std::atan2(vel.y(), vel.x());
double angle_diff = getAngleDiff(robot->pose.theta, move_angle);
@@ -497,7 +480,6 @@ auto RVO2Planner::extractVelocityCommandsFromRVOSim(
command.omega_limit = 0.0;
}
}
-
commands.robot_commands.emplace_back(command);
}
@@ -718,15 +700,12 @@ auto RVO2Planner::adjustForPenaltyAreaAvoidance(
} else if (isInBox(penalty_area, closest_point_2, PENALTY_AREA_OFFSET)) {
target_pos = around_corner_2;
} else {
- static rclcpp::Clock steady_clock(RCL_STEADY_TIME);
- RCLCPP_WARN_THROTTLE(
- rclcpp::get_logger("rvo2_local_planner"), steady_clock, 1000,
- "Failed to find a target position outside the penalty area. Fallback to current "
- "position. current_pos:(%.3f, %.3f) target_pos:(%.3f, %.3f) "
- "closest_point_1:(%.3f, %.3f) closest_point_2:(%.3f, %.3f)",
- current_pos.x(), current_pos.y(), target_pos.x(), target_pos.y(), closest_point_1.x(),
- closest_point_1.y(), closest_point_2.x(), closest_point_2.y());
- target_pos = current_pos;
+ // フォールバック: 各角を経由した総距離が短い方を選択
+ const double dist_via_1 =
+ (around_corner_1 - current_pos).norm() + (target_pos - around_corner_1).norm();
+ const double dist_via_2 =
+ (around_corner_2 - current_pos).norm() + (target_pos - around_corner_2).norm();
+ target_pos = (dist_via_1 <= dist_via_2) ? around_corner_1 : around_corner_2;
}
}
};
diff --git a/crane_msgs/msg/LocalPlannerConfig.msg b/crane_msgs/msg/LocalPlannerConfig.msg
index 267ec818a..bea4d41c1 100644
--- a/crane_msgs/msg/LocalPlannerConfig.msg
+++ b/crane_msgs/msg/LocalPlannerConfig.msg
@@ -4,7 +4,6 @@ bool disable_placement_avoidance false
bool disable_ball_avoidance false
bool disable_field_boundary false
bool disable_crash_avoidance false
-bool enable_rotation_stop_on_accel true
NamedFloat[] max_velocity_factors
NamedFloat final_planned_max_velocity
diff --git a/crane_msgs/msg/RobotFeedback.msg b/crane_msgs/msg/RobotFeedback.msg
index a55e768b9..e3aa04bab 100755
--- a/crane_msgs/msg/RobotFeedback.msg
+++ b/crane_msgs/msg/RobotFeedback.msg
@@ -1,6 +1,10 @@
builtin_interfaces/Time received_stamp
uint8 robot_id
float32 packet_frequency_hz
+float32 receive_interval_mean_ms
+float32 receive_interval_min_ms
+float32 receive_interval_max_ms
+float32 receive_interval_stddev_ms
uint8 counter
diff --git a/crane_robot_receiver/src/robot_receiver_node.cpp b/crane_robot_receiver/src/robot_receiver_node.cpp
index 69b9e8628..30751cfe4 100644
--- a/crane_robot_receiver/src/robot_receiver_node.cpp
+++ b/crane_robot_receiver/src/robot_receiver_node.cpp
@@ -6,6 +6,7 @@
#include
#include
+#include
#include
#include
#include
@@ -13,144 +14,194 @@
#include
#include
#include
+#include
+#include
#include
+#include
#include
-using boost::asio::ip::udp;
+namespace asio = boost::asio;
using crane::robot_receiver::RobotFeedback;
namespace protocol = crane::robot_receiver::protocol;
class RobotFeedbackReceiver
{
public:
- RobotFeedbackReceiver(const std::string & host, const int port)
+ RobotFeedbackReceiver(asio::io_context & io_ctx, const std::string & host, const int port)
: robot_id(port - 50100),
- receiver_(std::make_unique(host, port)),
- buffer_(protocol::BUFFER_SIZE),
+ async_receiver_(
+ std::make_unique(io_ctx, host, port, protocol::BUFFER_SIZE)),
clock(RCL_ROS_TIME)
{
- // 初回比較時のエラー回避
- robot_feedback.received_stamp = rclcpp::Time(0, 0, RCL_ROS_TIME);
+ async_receiver_->startReceive(
+ [this](const std::vector & buf, size_t size) { onReceive(buf, size); });
}
- auto receive() -> bool
+ // asioスレッドから呼ばれる
+ void onReceive(const std::vector & buf, size_t /*size*/)
{
- if (receiver_->available()) {
- receiver_->receive(buffer_);
- // 受信タイムスタンプを記録
- receive_timestamps_.push_back(clock.now());
- return true;
+ auto stamp = clock.now();
+ RobotFeedback feedback = parseBuffer(buf, stamp);
+ std::lock_guard lock(mutex_);
+ latest_feedback_ = feedback;
+ receive_timestamps_.push_back(stamp);
+ }
+
+ // ROS2タイマーから呼ばれる
+ auto getLatestFeedback() -> std::optional
+ {
+ std::lock_guard lock(mutex_);
+ return latest_feedback_;
+ }
+
+ struct IntervalStats
+ {
+ float mean_ms = 0.f;
+ float min_ms = 0.f;
+ float max_ms = 0.f;
+ float stddev_ms = 0.f;
+ };
+
+ auto getPacketFrequency() -> float
+ {
+ std::lock_guard lock(mutex_);
+ pruneTimestampsLocked();
+ return static_cast(receive_timestamps_.size());
+ }
+
+ auto getReceiveIntervalStats() -> IntervalStats
+ {
+ std::lock_guard lock(mutex_);
+ pruneTimestampsLocked();
+ if (receive_timestamps_.size() < 2) {
+ return {};
+ }
+
+ std::vector intervals;
+ intervals.reserve(receive_timestamps_.size() - 1);
+ for (size_t i = 1; i < receive_timestamps_.size(); ++i) {
+ float dt = static_cast(
+ (receive_timestamps_[i] - receive_timestamps_[i - 1]).nanoseconds() * 1e-6);
+ intervals.push_back(dt);
+ }
+
+ float sum = 0.f;
+ float min_val = intervals[0];
+ float max_val = intervals[0];
+ for (float v : intervals) {
+ sum += v;
+ if (v < min_val) min_val = v;
+ if (v > max_val) max_val = v;
+ }
+ float mean = sum / static_cast(intervals.size());
+
+ float var = 0.f;
+ for (float v : intervals) {
+ float d = v - mean;
+ var += d * d;
}
- return false;
+ float stddev = std::sqrt(var / static_cast(intervals.size()));
+
+ return {mean, min_val, max_val, stddev};
}
- auto updateFeedback() -> void
+ const int robot_id;
+
+private:
+ auto parseBuffer(const std::vector & buf, rclcpp::Time stamp) -> RobotFeedback
{
RobotFeedback feedback;
- // 受信タイムスタンプを更新
- feedback.received_stamp = clock.now();
+ feedback.received_stamp = stamp;
// 基本情報
- feedback.counter = protocol::readByte(buffer_, protocol::offset::COUNTER);
- feedback.yaw_angle = protocol::readFloat(buffer_, protocol::offset::YAW_ANGLE);
- feedback.voltage[0] = protocol::readFloat(buffer_, protocol::offset::VOLTAGE_0);
+ feedback.counter = protocol::readByte(buf, protocol::offset::COUNTER);
+ feedback.yaw_angle = protocol::readFloat(buf, protocol::offset::YAW_ANGLE);
+ feedback.voltage[0] = protocol::readFloat(buf, protocol::offset::VOLTAGE_0);
// ボール検出
- feedback.ball_detection[0] = protocol::readByte(buffer_, protocol::offset::BALL_DETECTION_0);
- feedback.ball_detection[1] = protocol::readByte(buffer_, protocol::offset::BALL_DETECTION_1);
- feedback.ball_detection[2] = protocol::readByte(buffer_, protocol::offset::BALL_DETECTION_2);
- feedback.ball_detection[3] = protocol::readByte(buffer_, protocol::offset::BALL_DETECTION_3);
+ feedback.ball_detection[0] = protocol::readByte(buf, protocol::offset::BALL_DETECTION_0);
+ feedback.ball_detection[1] = protocol::readByte(buf, protocol::offset::BALL_DETECTION_1);
+ feedback.ball_detection[2] = protocol::readByte(buf, protocol::offset::BALL_DETECTION_2);
+ feedback.ball_detection[3] = protocol::readByte(buf, protocol::offset::BALL_DETECTION_3);
feedback.kick_state =
- protocol::readByte(buffer_, protocol::offset::KICK_STATE) * protocol::KICK_STATE_SCALE;
+ protocol::readByte(buf, protocol::offset::KICK_STATE) * protocol::KICK_STATE_SCALE;
// エラー情報
- feedback.error_id = protocol::readUint16(buffer_, protocol::offset::ERROR_ID);
- feedback.error_info = protocol::readUint16(buffer_, protocol::offset::ERROR_INFO);
- feedback.error_value = protocol::readFloat(buffer_, protocol::offset::ERROR_VALUE);
+ feedback.error_id = protocol::readUint16(buf, protocol::offset::ERROR_ID);
+ feedback.error_info = protocol::readUint16(buf, protocol::offset::ERROR_INFO);
+ feedback.error_value = protocol::readFloat(buf, protocol::offset::ERROR_VALUE);
// モーター電流
- feedback.motor_current[0] = protocol::readByte(buffer_, protocol::offset::MOTOR_CURRENT_0) /
- protocol::MOTOR_CURRENT_SCALE;
- feedback.motor_current[1] = protocol::readByte(buffer_, protocol::offset::MOTOR_CURRENT_1) /
- protocol::MOTOR_CURRENT_SCALE;
- feedback.motor_current[2] = protocol::readByte(buffer_, protocol::offset::MOTOR_CURRENT_2) /
- protocol::MOTOR_CURRENT_SCALE;
- feedback.motor_current[3] = protocol::readByte(buffer_, protocol::offset::MOTOR_CURRENT_3) /
- protocol::MOTOR_CURRENT_SCALE;
+ feedback.motor_current[0] =
+ protocol::readByte(buf, protocol::offset::MOTOR_CURRENT_0) / protocol::MOTOR_CURRENT_SCALE;
+ feedback.motor_current[1] =
+ protocol::readByte(buf, protocol::offset::MOTOR_CURRENT_1) / protocol::MOTOR_CURRENT_SCALE;
+ feedback.motor_current[2] =
+ protocol::readByte(buf, protocol::offset::MOTOR_CURRENT_2) / protocol::MOTOR_CURRENT_SCALE;
+ feedback.motor_current[3] =
+ protocol::readByte(buf, protocol::offset::MOTOR_CURRENT_3) / protocol::MOTOR_CURRENT_SCALE;
// 温度
- feedback.temperature[0] = protocol::readByte(buffer_, protocol::offset::TEMPERATURE_0);
- feedback.temperature[1] = protocol::readByte(buffer_, protocol::offset::TEMPERATURE_1);
- feedback.temperature[2] = protocol::readByte(buffer_, protocol::offset::TEMPERATURE_2);
- feedback.temperature[3] = protocol::readByte(buffer_, protocol::offset::TEMPERATURE_3);
- feedback.temperature[4] = protocol::readByte(buffer_, protocol::offset::TEMPERATURE_4);
- feedback.temperature[5] = protocol::readByte(buffer_, protocol::offset::TEMPERATURE_5);
- feedback.temperature[6] = protocol::readByte(buffer_, protocol::offset::TEMPERATURE_6);
+ feedback.temperature[0] = protocol::readByte(buf, protocol::offset::TEMPERATURE_0);
+ feedback.temperature[1] = protocol::readByte(buf, protocol::offset::TEMPERATURE_1);
+ feedback.temperature[2] = protocol::readByte(buf, protocol::offset::TEMPERATURE_2);
+ feedback.temperature[3] = protocol::readByte(buf, protocol::offset::TEMPERATURE_3);
+ feedback.temperature[4] = protocol::readByte(buf, protocol::offset::TEMPERATURE_4);
+ feedback.temperature[5] = protocol::readByte(buf, protocol::offset::TEMPERATURE_5);
+ feedback.temperature[6] = protocol::readByte(buf, protocol::offset::TEMPERATURE_6);
// 角度と電圧
- feedback.diff_angle = protocol::readFloat(buffer_, protocol::offset::DIFF_ANGLE);
- feedback.voltage[1] = protocol::readFloat(buffer_, protocol::offset::VOLTAGE_1);
+ feedback.diff_angle = protocol::readFloat(buf, protocol::offset::DIFF_ANGLE);
+ feedback.voltage[1] = protocol::readFloat(buf, protocol::offset::VOLTAGE_1);
// オドメトリ
- feedback.odom[0] = protocol::readFloat(buffer_, protocol::offset::ODOM_X);
- feedback.odom[1] = protocol::readFloat(buffer_, protocol::offset::ODOM_Y);
- feedback.odom_speed[0] = protocol::readFloat(buffer_, protocol::offset::ODOM_SPEED_X);
- feedback.odom_speed[1] = protocol::readFloat(buffer_, protocol::offset::ODOM_SPEED_Y);
+ feedback.odom[0] = protocol::readFloat(buf, protocol::offset::ODOM_X);
+ feedback.odom[1] = protocol::readFloat(buf, protocol::offset::ODOM_Y);
+ feedback.odom_speed[0] = protocol::readFloat(buf, protocol::offset::ODOM_SPEED_X);
+ feedback.odom_speed[1] = protocol::readFloat(buf, protocol::offset::ODOM_SPEED_Y);
- feedback.check_ver = protocol::readByte(buffer_, protocol::offset::CHECK_VER);
+ feedback.check_ver = protocol::readByte(buf, protocol::offset::CHECK_VER);
// マウスセンサー
- feedback.mouse_odom[0] = protocol::readFloat(buffer_, protocol::offset::MOUSE_ODOM_X);
- feedback.mouse_odom[1] = protocol::readFloat(buffer_, protocol::offset::MOUSE_ODOM_Y);
- feedback.mouse_vel[0] = protocol::readFloat(buffer_, protocol::offset::MOUSE_VEL_X);
- feedback.mouse_vel[1] = protocol::readFloat(buffer_, protocol::offset::MOUSE_VEL_Y);
+ feedback.mouse_odom[0] = protocol::readFloat(buf, protocol::offset::MOUSE_ODOM_X);
+ feedback.mouse_odom[1] = protocol::readFloat(buf, protocol::offset::MOUSE_ODOM_Y);
+ feedback.mouse_vel[0] = protocol::readFloat(buf, protocol::offset::MOUSE_VEL_X);
+ feedback.mouse_vel[1] = protocol::readFloat(buf, protocol::offset::MOUSE_VEL_Y);
// デバッグ値
feedback.values.clear();
for (size_t i = protocol::offset::DEBUG_VALUES_START;
i < protocol::DEBUG_VALUES_END - protocol::FLOAT_SIZE; i += protocol::FLOAT_SIZE) {
- feedback.values.push_back(protocol::readFloat(buffer_, static_cast(i)));
+ feedback.values.push_back(protocol::readFloat(buf, static_cast(i)));
}
- robot_feedback = feedback;
+ return feedback;
}
- auto getFeedback() const -> RobotFeedback { return robot_feedback; }
-
- auto getPacketFrequency() -> float
+ // mutex_ を保持した状態で呼ぶこと
+ void pruneTimestampsLocked()
{
using std::chrono::operator""ms;
auto now = clock.now();
-
- // 1秒より古いタイムスタンプを削除
while (!receive_timestamps_.empty() && (now - receive_timestamps_.front()) > 1000ms) {
receive_timestamps_.pop_front();
}
-
- // 最近1秒間の受信回数を返す
- return static_cast(receive_timestamps_.size());
}
- const int robot_id;
-
-private:
- std::unique_ptr receiver_;
-
- std::vector buffer_;
-
- RobotFeedback robot_feedback;
-
- rclcpp::Clock clock;
-
- // パケット受信頻度計算用のタイムスタンプキュー
+ std::unique_ptr async_receiver_;
+ std::mutex mutex_;
+ std::optional latest_feedback_;
std::deque receive_timestamps_;
+ rclcpp::Clock clock;
};
class RobotReceiverNode : public rclcpp::Node
{
public:
explicit RobotReceiverNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
- : rclcpp::Node("robot_receiver_node", options), clock(RCL_ROS_TIME)
+ : rclcpp::Node("robot_receiver_node", options),
+ work_guard_(asio::make_work_guard(io_context_)),
+ clock(RCL_ROS_TIME)
{
crane::CraneVisualizerBuffer::activate(*this);
publisher = create_publisher("/robot_feedback", 10);
@@ -169,63 +220,82 @@ class RobotReceiverNode : public rclcpp::Node
for (int i = 0; i <= max_robot_id; i++) {
std::string ip;
if (sim_mode) {
- // シミュレータモード: 127.0.0.1 (固定)
ip = "127.0.0.1";
} else {
- // 実機モード: 224.5.20.{100+robot_id}
ip = std::format("{}.{}", ip_base, i + ip_offset);
}
int port = port_base + i;
- receivers.push_back(std::make_shared(ip, port));
+ receivers.push_back(std::make_shared(io_context_, ip, port));
}
+ // asioイベントループを専用スレッドで開始
+ io_thread_ = std::thread([this]() { io_context_.run(); });
+
using std::chrono::operator""ms;
+ using std::chrono::operator""s;
+ log_timer = rclcpp::create_timer(this, get_clock(), 5s, [&]() {
+ for (auto & receiver : receivers) {
+ float freq = receiver->getPacketFrequency();
+ if (freq <= 0.f) continue;
+ auto stats = receiver->getReceiveIntervalStats();
+ RCLCPP_INFO(
+ get_logger(),
+ "[Robot %d] interval: mean=%.1fms, min=%.1fms, max=%.1fms, stddev=%.1fms, freq=%.1fHz",
+ receiver->robot_id, stats.mean_ms, stats.min_ms, stats.max_ms, stats.stddev_ms, freq);
+ }
+ });
+
timer = rclcpp::create_timer(this, get_clock(), 10ms, [&]() {
crane_msgs::msg::RobotFeedbackArray msg;
auto now = clock.now();
for (auto & receiver : receivers) {
- while (receiver->receive()) {
- receiver->updateFeedback();
- }
+ auto robot_feedback = receiver->getLatestFeedback();
+ if (!robot_feedback) continue;
// 古いデータは入れない(100msより古いデータはVisionより価値が薄い可能性が高い)
- if (
- auto robot_feedback = receiver->getFeedback();
- (now - robot_feedback.received_stamp) < 100ms) {
- crane_msgs::msg::RobotFeedback robot_feedback_msg;
- robot_feedback_msg.received_stamp = robot_feedback.received_stamp;
- robot_feedback_msg.robot_id = receiver->robot_id;
- robot_feedback_msg.packet_frequency_hz = receiver->getPacketFrequency();
- robot_feedback_msg.counter = robot_feedback.counter;
- robot_feedback_msg.kick_state = robot_feedback.kick_state;
- robot_feedback_msg.temperatures.assign(
- std::begin(robot_feedback.temperature), std::end(robot_feedback.temperature));
-
- robot_feedback_msg.error_id = robot_feedback.error_id;
- robot_feedback_msg.error_info = robot_feedback.error_info;
- robot_feedback_msg.error_value = robot_feedback.error_value;
-
- robot_feedback_msg.motor_current.assign(
- std::begin(robot_feedback.motor_current), std::end(robot_feedback.motor_current));
- robot_feedback_msg.ball_detection.assign(
- std::begin(robot_feedback.ball_detection), std::end(robot_feedback.ball_detection));
- robot_feedback_msg.ball_sensor = (robot_feedback_msg.ball_detection[0] == 1);
- robot_feedback_msg.yaw_angle = robot_feedback.yaw_angle;
- robot_feedback_msg.diff_angle = robot_feedback.diff_angle;
- robot_feedback_msg.odom.assign(robot_feedback.odom.begin(), robot_feedback.odom.end());
- robot_feedback_msg.odom_speed.assign(
- robot_feedback.odom_speed.begin(), robot_feedback.odom_speed.end());
- robot_feedback_msg.mouse_odom.assign(
- robot_feedback.mouse_odom.begin(), robot_feedback.mouse_odom.end());
- robot_feedback_msg.mouse_vel.assign(
- robot_feedback.mouse_vel.begin(), robot_feedback.mouse_vel.end());
- robot_feedback_msg.voltage.assign(
- robot_feedback.voltage.begin(), robot_feedback.voltage.end());
- robot_feedback_msg.check_ver = robot_feedback.check_ver;
- robot_feedback_msg.values = robot_feedback.values;
- msg.feedback.push_back(robot_feedback_msg);
+ using std::chrono::operator""ms;
+ if ((now - robot_feedback->received_stamp) >= 100ms) continue;
+
+ crane_msgs::msg::RobotFeedback robot_feedback_msg;
+ robot_feedback_msg.received_stamp = robot_feedback->received_stamp;
+ robot_feedback_msg.robot_id = receiver->robot_id;
+ robot_feedback_msg.packet_frequency_hz = receiver->getPacketFrequency();
+ {
+ auto stats = receiver->getReceiveIntervalStats();
+ robot_feedback_msg.receive_interval_mean_ms = stats.mean_ms;
+ robot_feedback_msg.receive_interval_min_ms = stats.min_ms;
+ robot_feedback_msg.receive_interval_max_ms = stats.max_ms;
+ robot_feedback_msg.receive_interval_stddev_ms = stats.stddev_ms;
}
+ robot_feedback_msg.counter = robot_feedback->counter;
+ robot_feedback_msg.kick_state = robot_feedback->kick_state;
+ robot_feedback_msg.temperatures.assign(
+ std::begin(robot_feedback->temperature), std::end(robot_feedback->temperature));
+
+ robot_feedback_msg.error_id = robot_feedback->error_id;
+ robot_feedback_msg.error_info = robot_feedback->error_info;
+ robot_feedback_msg.error_value = robot_feedback->error_value;
+
+ robot_feedback_msg.motor_current.assign(
+ std::begin(robot_feedback->motor_current), std::end(robot_feedback->motor_current));
+ robot_feedback_msg.ball_detection.assign(
+ std::begin(robot_feedback->ball_detection), std::end(robot_feedback->ball_detection));
+ robot_feedback_msg.ball_sensor = (robot_feedback_msg.ball_detection[0] == 1);
+ robot_feedback_msg.yaw_angle = robot_feedback->yaw_angle;
+ robot_feedback_msg.diff_angle = robot_feedback->diff_angle;
+ robot_feedback_msg.odom.assign(robot_feedback->odom.begin(), robot_feedback->odom.end());
+ robot_feedback_msg.odom_speed.assign(
+ robot_feedback->odom_speed.begin(), robot_feedback->odom_speed.end());
+ robot_feedback_msg.mouse_odom.assign(
+ robot_feedback->mouse_odom.begin(), robot_feedback->mouse_odom.end());
+ robot_feedback_msg.mouse_vel.assign(
+ robot_feedback->mouse_vel.begin(), robot_feedback->mouse_vel.end());
+ robot_feedback_msg.voltage.assign(
+ robot_feedback->voltage.begin(), robot_feedback->voltage.end());
+ robot_feedback_msg.check_ver = robot_feedback->check_ver;
+ robot_feedback_msg.values = robot_feedback->values;
+ msg.feedback.push_back(robot_feedback_msg);
}
publisher->publish(msg);
visualizer->flush();
@@ -233,7 +303,19 @@ class RobotReceiverNode : public rclcpp::Node
});
}
+ ~RobotReceiverNode()
+ {
+ work_guard_.reset();
+ io_context_.stop();
+ if (io_thread_.joinable()) io_thread_.join();
+ }
+
+ asio::io_context io_context_;
+ asio::executor_work_guard work_guard_;
+ std::thread io_thread_;
+
rclcpp::TimerBase::SharedPtr timer;
+ rclcpp::TimerBase::SharedPtr log_timer;
std::vector> receivers;
diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp
index 2620928ec..b8c955cd7 100644
--- a/crane_robot_skills/src/goalie.cpp
+++ b/crane_robot_skills/src/goalie.cpp
@@ -21,7 +21,7 @@ void Goalie::initialize()
{
setParameter("run_inplay", true);
setParameter("block_distance", 0.5);
- setParameter("emit_force_timeout", 3.0);
+ setParameter("emit_force_timeout", 1.5);
setParameter("emit_enemy_distance_threshold", 0.5);
setParameter("emit_ball_speed_threshold", 0.5);
}
diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp
index 1ed3396d0..5090a4fdf 100644
--- a/crane_robot_skills/src/kick.cpp
+++ b/crane_robot_skills/src/kick.cpp
@@ -76,6 +76,7 @@ void Kick::initialize()
return computeAroundBallApproachTargetDynamic(
ball_pos, safe_target, robot()->pose.pos, interval, max_interval);
}();
+ approach += (approach - robot()->pose.pos) * 1.0;
command->disableBallAvoidance();
using boost::math::constants::degree;
diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp
index 76cf7279f..f5ed5a675 100644
--- a/crane_robot_skills/src/single_ball_placement.cpp
+++ b/crane_robot_skills/src/single_ball_placement.cpp
@@ -291,7 +291,7 @@ void SingleBallPlacement::initialize()
command->dribble(0.0);
command->setOmegaLimit(10.0);
- if (command->getTargetDistance() < 0.02) {
+ if (command->getTargetDistance() < 0.05) {
skill_status = Status::SUCCESS;
} else {
skill_status = Status::RUNNING;
diff --git a/crane_sender/include/crane_sender/sim_sender.hpp b/crane_sender/include/crane_sender/sim_sender.hpp
index 2676dec45..b31754051 100644
--- a/crane_sender/include/crane_sender/sim_sender.hpp
+++ b/crane_sender/include/crane_sender/sim_sender.hpp
@@ -67,7 +67,7 @@ class SimSenderComponent : public SenderBase
p_gain.getValue(), i_gain.getValue(), d_gain.getValue(), i_saturation_);
state.theta_controller.setGain(
theta_p_gain.getValue(), theta_i_gain.getValue(), theta_d_gain.getValue());
- state.previous_velocity = Velocity::Zero();
+ state.previous_velocity_global = Velocity::Zero();
}
// the parameters of the PID controller
@@ -109,7 +109,10 @@ class SimSenderComponent : public SenderBase
PIDController vx_controller;
PIDController vy_controller;
PIDController theta_controller;
- Velocity previous_velocity;
+ // 前回出力した速度をグローバルフレームで保持する。
+ // ローカルフレームで保持するとロボット回転時にフレーム不一致が生じ、
+ // Δv の計算が誤るため、グローバルフレームで統一する。
+ Velocity previous_velocity_global;
};
// 全ロボットの状態
diff --git a/crane_sender/src/sender_base.cpp b/crane_sender/src/sender_base.cpp
index d27f0b31a..f66de8a77 100644
--- a/crane_sender/src/sender_base.cpp
+++ b/crane_sender/src/sender_base.cpp
@@ -50,6 +50,12 @@ SenderBase::SenderBase(const std::string & name, const rclcpp::NodeOptions & opt
get_parameter("robot_acceleration.velocity_threshold", robot_acceleration_velocity_threshold_);
world_model = std::make_shared(*this);
+
+ RCLCPP_INFO(
+ get_logger(),
+ "[DIAG] robot_acceleration: acc=%.2f decel_high=%.2f decel_low=%.2f thresh=%.2f",
+ robot_acceleration_acceleration_, robot_acceleration_deceleration_high_,
+ robot_acceleration_deceleration_low_, robot_acceleration_velocity_threshold_);
}
void SenderBase::callback(const VelocityCommandsMsg & msg)
diff --git a/crane_sender/src/sim_sender.cpp b/crane_sender/src/sim_sender.cpp
index e70bd3edf..7a2dde554 100644
--- a/crane_sender/src/sim_sender.cpp
+++ b/crane_sender/src/sim_sender.cpp
@@ -58,8 +58,14 @@ void SimSenderComponent::sendCommands(const crane_msgs::msg::RobotCommands & msg
Eigen::Vector2d target_vel_local(
target_velocity_r * std::cos(velocity_theta), target_velocity_r * std::sin(velocity_theta));
- // Step 2: ローカル座標系での現在速度（前回の出力速度を使用）
- Eigen::Vector2d current_vel_local = robot_states_[command.robot_id].previous_velocity;
+ // Step 2: 前回の出力速度（グローバルフレーム）を現在のローカルフレームへ変換
+ // グローバル→ローカル変換: R(-θ) = [[cos θ, sin θ], [-sin θ, cos θ]]
+ const Eigen::Vector2d & prev_vel_global =
+ robot_states_[command.robot_id].previous_velocity_global;
+ Eigen::Vector2d current_vel_local(
+ prev_vel_global.x() * std::cos(current_theta) + prev_vel_global.y() * std::sin(current_theta),
+ -prev_vel_global.x() * std::sin(current_theta) +
+ prev_vel_global.y() * std::cos(current_theta));
// Step 3: 加速度制限を選択（速度の大きさで加速/減速を判定）
double current_speed = current_vel_local.norm();
@@ -82,8 +88,11 @@ void SimSenderComponent::sendCommands(const crane_msgs::msg::RobotCommands & msg
move_local_velocity->set_forward(vx);
move_local_velocity->set_left(vy);
- // 次回の計算用に保存
- robot_states_[command.robot_id].previous_velocity = Velocity(vx, vy);
+ // 次回の計算用に保存（ローカル→グローバルへ変換）
+ // グローバル→ローカル変換の逆: R(θ) = [[cos θ, -sin θ], [sin θ, cos θ]]
+ robot_states_[command.robot_id].previous_velocity_global = Velocity(
+ vx * std::cos(current_theta) - vy * std::sin(current_theta),
+ vx * std::sin(current_theta) + vy * std::cos(current_theta));
// ストップ
if (command.stop_flag) {
diff --git a/crane_session_coordinator/config/unified_session_config.yaml b/crane_session_coordinator/config/unified_session_config.yaml
index 7f297111f..7c0134480 100644
--- a/crane_session_coordinator/config/unified_session_config.yaml
+++ b/crane_session_coordinator/config/unified_session_config.yaml
@@ -16,15 +16,15 @@ situations:
- name: defender
max_robots: 3
params:
- reduced_robots: 2
+ reduced_robots: 1
- name: pass_receive
max_robots: 1
- - name: second_threat_defender
- max_robots: 1
- name: sub_attacker_skill
max_robots: 1
+ - name: second_threat_defender
+ max_robots: 1
- name: marker
- max_robots: 3
+ max_robots: 2
- name: forward
max_robots: 20
diff --git a/crane_sessions/config/test_planner.yaml b/crane_sessions/config/test_planner.yaml
index 94e62f898..fb8a2611d 100644
--- a/crane_sessions/config/test_planner.yaml
+++ b/crane_sessions/config/test_planner.yaml
@@ -16,7 +16,7 @@ waypoints:
# - position: [-3.5, -2.0, -90]
# - position: [-3.5, 2.0, 0]
# - position: [0.0, 0.0] # 角度なし（位置のみ）
- - position: [4.5, 2.5]
- - position: [-4.5, 2.5]
+ - position: [2.0, 2.5]
+ - position: [2.0, -2.5]
# このプランナで動かす自チームロボットID
-robot_id: 0
+robot_id: 10
diff --git a/crane_sessions/include/crane_sessions/session_base.hpp b/crane_sessions/include/crane_sessions/session_base.hpp
index c9d882dd5..901c5bfc9 100644
--- a/crane_sessions/include/crane_sessions/session_base.hpp
+++ b/crane_sessions/include/crane_sessions/session_base.hpp
@@ -117,9 +117,12 @@ class SessionBase
msg.is_yellow = world_model->isYellow();
msg.on_positive_half = world_model->onPositiveHalf();
for (auto command : position_commands) {
- // WorldModelから現在の速度情報を取得して設定
+ // WorldModelから現在の位置・速度情報を取得して設定
auto robot = world_model->getOurRobot(command.robot_id);
if (robot) {
+ command.current_pose.x = robot->pose.pos.x();
+ command.current_pose.y = robot->pose.pos.y();
+ command.current_pose.theta = robot->pose.theta;
command.current_velocity.x = robot->vel.linear.x();
command.current_velocity.y = robot->vel.linear.y();
command.current_velocity.theta = robot->vel.omega;
diff --git a/docker/dev/docker-compose.yaml b/docker/dev/docker-compose.yaml
index 7b00d8ab5..c7077f5d5 100644
--- a/docker/dev/docker-compose.yaml
+++ b/docker/dev/docker-compose.yaml
@@ -1,6 +1,6 @@
services:
grsim:
- image: ghcr.io/ibis-ssl/grsim:latest
+ image: grsim-local:dev
container_name: grsim
network_mode: host
command: |
@@ -70,8 +70,3 @@ services:
# image: voicevox/voicevox_engine:nvidia-latest
ports:
- 127.0.0.1:50021:50021
-
- aivis-speech:
- image: ghcr.io/aivis-project/aivisspeech-engine:cpu-latest
- ports:
- - 127.0.0.1:10101:10101
diff --git a/utility/crane_comm/include/crane_comm/unicast.hpp b/utility/crane_comm/include/crane_comm/unicast.hpp
index 2dd0c3571..fe158aacf 100644
--- a/utility/crane_comm/include/crane_comm/unicast.hpp
+++ b/utility/crane_comm/include/crane_comm/unicast.hpp
@@ -21,6 +21,7 @@
#include
#include
+#include
#include
#include
#include
@@ -148,6 +149,108 @@ class UdpReceiver
asio::ip::udp::socket socket;
};
+class AsyncUdpReceiver
+{
+public:
+ AsyncUdpReceiver(
+ asio::io_context & io_ctx, const std::string & host, const int port, size_t buffer_size = 2048)
+ : socket_(io_ctx, asio::ip::udp::v4()), recv_buffer_(buffer_size)
+ {
+ asio::ip::address addr = asio::ip::address::from_string(host);
+ boost::system::error_code ec;
+
+ socket_.set_option(asio::socket_base::reuse_address(true), ec);
+ socket_.set_option(reuse_port(true), ec);
+
+ if (addr.is_multicast()) {
+ socket_.bind(asio::ip::udp::endpoint(asio::ip::udp::v4(), port), ec);
+ if (ec) {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("crane_comm"), "[ERROR] bind失敗: %s", ec.message().c_str());
+ throw std::runtime_error("bind failed: " + ec.message());
+ }
+
+ try {
+ struct ifaddrs * interfaces = nullptr;
+ if (getifaddrs(&interfaces) == -1) {
+ throw std::runtime_error("Error: getifaddrs failed.");
+ }
+
+ std::unordered_set joined_interfaces;
+ for (struct ifaddrs * ifa = interfaces; ifa != nullptr; ifa = ifa->ifa_next) {
+ if (ifa->ifa_addr == nullptr || ifa->ifa_addr->sa_family != AF_INET) continue;
+
+ char ip[INET_ADDRSTRLEN];
+ auto * addr_in = reinterpret_cast(ifa->ifa_addr);
+ inet_ntop(AF_INET, &(addr_in->sin_addr), ip, INET_ADDRSTRLEN);
+
+ std::string if_name(ifa->ifa_name);
+ if (joined_interfaces.count(if_name) > 0) continue;
+
+ boost::asio::ip::detail::socket_option::multicast_request<
+ IPPROTO_IP, IP_ADD_MEMBERSHIP, IPPROTO_IPV6, IPV6_JOIN_GROUP>
+ join_device(addr.to_v4(), asio::ip::address::from_string(ip).to_v4());
+
+ boost::system::error_code join_ec;
+ socket_.set_option(join_device, join_ec);
+ if (!join_ec) {
+ joined_interfaces.insert(if_name);
+ }
+ }
+ freeifaddrs(interfaces);
+ } catch (std::exception & e) {
+ RCLCPP_ERROR(rclcpp::get_logger("crane_comm"), "%s", e.what());
+ }
+ } else {
+ socket_.bind(asio::ip::udp::endpoint(addr, port), ec);
+ if (ec) {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("crane_comm"), "[ERROR] bind失敗: %s", ec.message().c_str());
+ throw std::runtime_error("bind failed: " + ec.message());
+ }
+ }
+ }
+
+ void startReceive(std::function &, size_t)> callback)
+ {
+ callback_ = std::move(callback);
+ running_ = true;
+ doReceive();
+ }
+
+ void stop()
+ {
+ running_ = false;
+ boost::system::error_code ec;
+ socket_.cancel(ec);
+ }
+
+private:
+ void doReceive()
+ {
+ socket_.async_receive_from(
+ asio::buffer(recv_buffer_), sender_endpoint_,
+ [this](const boost::system::error_code & ec, size_t bytes_transferred) {
+ if (!ec) {
+ try {
+ callback_(recv_buffer_, bytes_transferred);
+ } catch (const std::exception & e) {
+ RCLCPP_ERROR(rclcpp::get_logger("crane_comm"), "受信コールバックエラー: %s", e.what());
+ }
+ } else if (ec != asio::error::operation_aborted) {
+ RCLCPP_ERROR(rclcpp::get_logger("crane_comm"), "UDP受信エラー: %s", ec.message().c_str());
+ }
+ if (running_) doReceive();
+ });
+ }
+
+ asio::ip::udp::socket socket_;
+ asio::ip::udp::endpoint sender_endpoint_;
+ std::vector recv_buffer_;
+ std::function &, size_t)> callback_;
+ bool running_ = false;
+};
+
} // namespace crane
#endif // CRANE_COMM__UNICAST_HPP_
diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp
index d41dc28dc..790e9ed6d 100644
--- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp
+++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp
@@ -291,18 +291,6 @@ class RobotCommandWrapper : public CommandWrapperBase,
return *this;
}
- auto enableRotationStopOnAccel() -> RobotCommandWrapper &
- {
- latest_msg.local_planner_config.enable_rotation_stop_on_accel = true;
- return *this;
- }
-
- auto disableRotationStopOnAccel() -> RobotCommandWrapper &
- {
- latest_msg.local_planner_config.enable_rotation_stop_on_accel = false;
- return *this;
- }
-
auto disableAnyAreaAvoidance() -> RobotCommandWrapper &
{
return disableGoalAreaAvoidance()