Merge branch 'develop' into jazzy

.pre-commit-config.yaml

@@ -3,7 +3,7 @@ ci:
 
 repos:
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.6.0
+    rev: v5.0.0
     hooks:
       - id: check-json
       - id: check-merge-conflict
@@ -18,7 +18,7 @@ repos:
         args: [--markdown-linebreak-ext=md]
 
   - repo: https://github.com/igorshubovych/markdownlint-cli
-    rev: v0.41.0
+    rev: v0.42.0
     hooks:
       - id: markdownlint
         args: [-c, .markdownlint.yaml, --fix]
@@ -49,7 +49,7 @@ repos:
       - id: shellcheck
 
   - repo: https://github.com/scop/pre-commit-shfmt
-    rev: v3.8.0-1
+    rev: v3.10.0-1
     hooks:
       - id: shfmt
         args: [-w, -s, -i=4]
@@ -60,7 +60,7 @@ repos:
 #      - id: isort
 
   - repo: https://github.com/psf/black
-    rev: 24.4.2
+    rev: 24.10.0
     hooks:
       - id: black
         args: [--line-length=99]
@@ -72,7 +72,7 @@ repos:
         types_or: [c++, c, cuda]
 
   - repo: https://github.com/cpplint/cpplint
-    rev: 80e97a67b2a10643d764dad98cde723f8dd1f1cf # NOLINTBEGIN/NOINTENDサポート
+    rev: 2.0.0
     hooks:
       - id: cpplint
         args: [--quiet]

crane_local_planner/src/crane_local_planner.cpp

@@ -53,13 +53,13 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
           RCLCPP_ERROR(get_logger(), what.str().c_str());
         }
         break;
-      case crane_msgs::msg::RobotCommand::VELOCITY_TARGET_MODE:
-        if (raw_command.velocity_target_mode.empty()) {
+      case crane_msgs::msg::RobotCommand::POLAR_VELOCITY_TARGET_MODE:
+        if (raw_command.polar_velocity_target_mode.empty()) {
           is_valid = false;
           std::stringstream what;
           what << "The robot " << static_cast<int>(raw_command.robot_id)
-               << " is specified as VELOCITY_TARGET_MODE by \"" << raw_command.skill_name
-               << "\" skill , but no velocity_target_mode is set.";
+               << " is specified as POLAR_VELOCITY_TARGET_MODE by \"" << raw_command.skill_name
+               << "\" skill , but no polar_velocity_target_mode is set.";
           RCLCPP_ERROR(get_logger(), what.str().c_str());
         }
         break;

crane_local_planner/src/rvo2_planner.cpp

@@ -126,9 +126,15 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms
                 pre_commands.robot_commands.begin(), pre_commands.robot_commands.end(),
                 [&](const auto & c) { return c.robot_id == command.robot_id; });
               it != pre_commands.robot_commands.end()) {
-            return static_cast<double>(std::hypot(
-              it->simple_velocity_target_mode.front().target_vx,
-              it->simple_velocity_target_mode.front().target_vy));
+            if (it->simple_velocity_target_mode.size() > 0) {
+              return static_cast<double>(std::hypot(
+                it->simple_velocity_target_mode.front().target_vx,
+                it->simple_velocity_target_mode.front().target_vy));
+            } else if (it->polar_velocity_target_mode.size() > 0) {
+              return static_cast<double>(it->polar_velocity_target_mode.front().target_velocity_r);
+            } else {
+              return 0.0;
+            }
           } else {
             // 履歴が見つからなければ0
             return 0.0;
@@ -180,10 +186,18 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms
                               command.simple_velocity_target_mode.front().target_vy));
         break;
       }
+      case crane_msgs::msg::RobotCommand::POLAR_VELOCITY_TARGET_MODE: {
+        double v_r = command.polar_velocity_target_mode.front().target_velocity_r;
+        double v_theta = command.polar_velocity_target_mode.front().target_velocity_theta;
+        rvo_sim->setAgentPrefVelocity(
+          command.robot_id, RVO::Vector2(v_r * cos(v_theta), v_r * sin(v_theta)));
+        break;
+      }
       default: {
         std::stringstream what;
         what << "Unsupported control mode: " << command.control_mode;
-        what << ", expected: POSITION_TARGET_MODE, SIMPLE_VELOCITY_TARGET_MODE";
+        what << ", expected: POSITION_TARGET_MODE, SIMPLE_VELOCITY_TARGET_MODE, "
+                "POLAR_VELOCITY_TARGET_MODE";
         throw std::runtime_error(what.str());
       }
     }
@@ -212,11 +226,11 @@ crane_msgs::msg::RobotCommands RVO2Planner::extractRobotCommandsFromRVOSim(
     // RVOシミュレータの出力をコピーする
     // NOTE: RVOシミュレータは角度を扱わないので角度はそのまま
 
-    command.control_mode = crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE;
-    command.simple_velocity_target_mode.clear();
-    command.simple_velocity_target_mode.reserve(1);
+    command.control_mode = crane_msgs::msg::RobotCommand::POLAR_VELOCITY_TARGET_MODE;
+    command.polar_velocity_target_mode.clear();
+    command.polar_velocity_target_mode.reserve(1);
 
-    crane_msgs::msg::SimpleVelocityTargetMode target;
+    crane_msgs::msg::PolarVelocityTargetMode target;
     auto vel = toPoint(rvo_sim->getAgentVelocity(original_command.robot_id));
 
     // 障害物回避を無効にする場合、目標速度をそのまま使う
@@ -234,10 +248,10 @@ crane_msgs::msg::RobotCommands RVO2Planner::extractRobotCommandsFromRVOSim(
       }
     }
 
-    target.target_vx = vel.x();
-    target.target_vy = vel.y();
+    target.target_velocity_r = vel.norm();
+    target.target_velocity_theta = std::atan2(vel.y(), vel.x());
 
-    command.simple_velocity_target_mode.push_back(target);
+    command.polar_velocity_target_mode.push_back(target);
 
     if (std::hypot(command.current_velocity.x, command.current_velocity.y) > vel.norm()) {
       // 減速中は減速度制限をmax_accelerationに代入

crane_msgs/CMakeLists.txt

@@ -42,7 +42,7 @@ set(msg_files
   "msg/control/LocalCameraMode.msg"
   "msg/control/PositionTargetMode.msg"
   "msg/control/SimpleVelocityTargetMode.msg"
-  "msg/control/VelocityTargetMode.msg"
+  "msg/control/PolarVelocityTargetMode.msg"
 )
 
 set(srv_files

crane_msgs/msg/control/PolarVelocityTargetMode.msg

@@ -0,0 +1,2 @@
+float32 target_velocity_r
+float32 target_velocity_theta

crane_msgs/msg/control/RobotCommand.msg

@@ -40,7 +40,7 @@ uint16 elapsed_time_ms_since_last_vision 0
 uint8 LOCAL_CAMERA_MODE=0
 uint8 POSITION_TARGET_MODE=1
 uint8 SIMPLE_VELOCITY_TARGET_MODE=2
-uint8 VELOCITY_TARGET_MODE=3
+uint8 POLAR_VELOCITY_TARGET_MODE=3
 
 uint8 control_mode
 
@@ -49,4 +49,4 @@ string skill_name
 LocalCameraMode[<=1] local_camera_mode
 PositionTargetMode[<=1] position_target_mode
 SimpleVelocityTargetMode[<=1] simple_velocity_target_mode
-VelocityTargetMode[<=1] velocity_target_mode
+PolarVelocityTargetMode[<=1] polar_velocity_target_mode

crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp

@@ -26,7 +26,7 @@ namespace crane::skills
 DEFINE_SKILL_COMMAND(KickWithChip, Position);
 DEFINE_SKILL_COMMAND(KickStraight, Position);
 DEFINE_SKILL_COMMAND(Dribble, Position);
-DEFINE_SKILL_COMMAND(SetVelocity, SimpleVelocity);
+DEFINE_SKILL_COMMAND(SetVelocity, PolarVelocity);
 DEFINE_SKILL_COMMAND(SetTargetPosition, Position);
 DEFINE_SKILL_COMMAND(SetDribblerTargetPosition, Position);
 DEFINE_SKILL_COMMAND(SetTargetTheta, Position);

crane_sender/include/crane_sender/robot_packet.h

@@ -124,39 +124,27 @@ inline void SimpleVelocityTargetModeArgs_serialize(
 
 typedef struct
 {
-  float target_global_vel[2];
-  float trajectory_global_origin[2];
-  float trajectory_origin_angle;
-  float trajectory_curvature;
-} VelocityTargetWithTrajectoryModeArgs;
+  float target_global_velocity_r;
+  float target_global_velocity_theta;
+} PolarVelocityModeArgs;
 
-inline void VelocityTargetWithTrajectoryModeArgs_init(
-  VelocityTargetWithTrajectoryModeArgs * args, const uint8_t * data)
+inline void PolarVelocityModeArgs_init(PolarVelocityModeArgs * args, const uint8_t * data)
 {
-  args->target_global_vel[0] = convertTwoByteToFloat(data[0], data[1], 32.767);
-  args->target_global_vel[1] = convertTwoByteToFloat(data[2], data[3], 32.767);
-  args->trajectory_global_origin[0] = convertTwoByteToFloat(data[4], data[5], 32.767);
-  args->trajectory_global_origin[1] = convertTwoByteToFloat(data[6], data[7], 32.767);
-  args->trajectory_origin_angle = convertTwoByteToFloat(data[8], data[9], M_PI);
-  args->trajectory_curvature = convertTwoByteToFloat(data[10], data[11], 32.767);
+  args->target_global_velocity_r = convertTwoByteToFloat(data[0], data[1], 32.767);
+  args->target_global_velocity_theta = convertTwoByteToFloat(data[2], data[3], 32.767);
 }
 
-inline void VelocityTargetWithTrajectoryModeArgs_serialize(
-  const VelocityTargetWithTrajectoryModeArgs * args, uint8_t * data)
+inline void PolarVelocityModeArgs_serialize(const PolarVelocityModeArgs * args, uint8_t * data)
 {
-  forward(&data[0], &data[1], args->target_global_vel[0], 32.767);
-  forward(&data[2], &data[3], args->target_global_vel[1], 32.767);
-  forward(&data[4], &data[5], args->trajectory_global_origin[0], 32.767);
-  forward(&data[6], &data[7], args->trajectory_global_origin[1], 32.767);
-  forward(&data[8], &data[9], args->trajectory_origin_angle, M_PI);
-  forward(&data[10], &data[11], args->trajectory_curvature, 32.767);
+  forward(&data[0], &data[1], args->target_global_velocity_r, 32.767);
+  forward(&data[2], &data[3], args->target_global_velocity_theta, 32.767);
 }
 
 typedef enum {
   LOCAL_CAMERA_MODE = 0,
   POSITION_TARGET_MODE = 1,
   SIMPLE_VELOCITY_TARGET_MODE = 2,
-  VELOCITY_TARGET_WITH_TRAJECTORY_MODE = 3,
+  POLAR_VELOCITY_TARGET_MODE = 3,
 } ControlMode;
 
 typedef struct
@@ -186,7 +174,7 @@ typedef struct
     LocalCameraModeArgs local_camera;
     PositionTargetModeArgs position;
     SimpleVelocityTargetModeArgs simple_velocity;
-    VelocityTargetWithTrajectoryModeArgs velocity;
+    PolarVelocityModeArgs polar_velocity;
   } mode_args;
 } RobotCommandV2;
 
@@ -288,9 +276,9 @@ inline void RobotCommandSerializedV2_serialize(
       SimpleVelocityTargetModeArgs_serialize(
         &command->mode_args.simple_velocity, &serialized->data[CONTROL_MODE_ARGS]);
       break;
-    case VELOCITY_TARGET_WITH_TRAJECTORY_MODE:
-      VelocityTargetWithTrajectoryModeArgs_serialize(
-        &command->mode_args.velocity, &serialized->data[CONTROL_MODE_ARGS]);
+    case POLAR_VELOCITY_TARGET_MODE:
+      PolarVelocityModeArgs_serialize(
+        &command->mode_args.polar_velocity, &serialized->data[CONTROL_MODE_ARGS]);
       break;
   }
 }
@@ -345,9 +333,9 @@ inline RobotCommandV2 RobotCommandSerializedV2_deserialize(
       SimpleVelocityTargetModeArgs_init(
         &command.mode_args.simple_velocity, &serialized->data[CONTROL_MODE_ARGS]);
       break;
-    case VELOCITY_TARGET_WITH_TRAJECTORY_MODE:
-      VelocityTargetWithTrajectoryModeArgs_init(
-        &command.mode_args.velocity, &serialized->data[CONTROL_MODE_ARGS]);
+    case POLAR_VELOCITY_TARGET_MODE:
+      PolarVelocityModeArgs_init(
+        &command.mode_args.polar_velocity, &serialized->data[CONTROL_MODE_ARGS]);
       break;
   }
   return command;

crane_sender/include/crane_sender/sender_base.hpp

@@ -112,7 +112,7 @@ class SenderBase : public rclcpp::Node
           break;
         case crane_msgs::msg::RobotCommand::POSITION_TARGET_MODE:
           break;
-        case crane_msgs::msg::RobotCommand::VELOCITY_TARGET_MODE: {
+        case crane_msgs::msg::RobotCommand::POLAR_VELOCITY_TARGET_MODE: {
         } break;
         case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE:
           break;

crane_sender/include/crane_sender/sim_sender.hpp

@@ -168,6 +168,16 @@ class SimSenderComponent : public SenderBase
           cmd.set__veltangent(vx * cos(-theta) - vy * sin(-theta));
           cmd.set__velnormal(vx * sin(-theta) + vy * cos(-theta));
         } break;
+        case crane_msgs::msg::RobotCommand::POLAR_VELOCITY_TARGET_MODE: {
+          double v_r = command.polar_velocity_target_mode.front().target_velocity_r;
+          double current_theta = command.current_pose.theta + omega * delay_s;
+          double velocity_theta =
+            command.polar_velocity_target_mode.front().target_velocity_theta - current_theta;
+          double vx = v_r * cos(velocity_theta);
+          double vy = v_r * sin(velocity_theta);
+          cmd.set__veltangent(vx);
+          cmd.set__velnormal(vy);
+        } break;
         default:
           std::cout << "Invalid control mode" << std::endl;
           break;

crane_sender/src/ibis_sender_node.cpp

@@ -176,16 +176,33 @@ class IbisSenderNode : public SenderBase
           }
         } break;
         case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE: {
-          packet.control_mode = SIMPLE_VELOCITY_TARGET_MODE;
+          // SimpleVelocityはPolarVelocityに変換して送信
+          packet.control_mode = POLAR_VELOCITY_TARGET_MODE;
           if (not command.simple_velocity_target_mode.empty()) {
-            packet.mode_args.simple_velocity.target_global_vel[0] =
-              command.simple_velocity_target_mode.front().target_vx;
-            packet.mode_args.simple_velocity.target_global_vel[1] =
-              command.simple_velocity_target_mode.front().target_vy;
+            if (command.polar_velocity_target_mode.empty()) {
+              command.polar_velocity_target_mode.emplace_back();
+            }
+            packet.mode_args.polar_velocity.target_global_velocity_r = std::hypot(
+              command.simple_velocity_target_mode.front().target_vx,
+              command.simple_velocity_target_mode.front().target_vy);
+            packet.mode_args.polar_velocity.target_global_velocity_theta = atan2(
+              command.simple_velocity_target_mode.front().target_vy,
+              command.simple_velocity_target_mode.front().target_vx);
           } else {
             std::cout << "empty simple_velocity_target_mode" << std::endl;
           }
         } break;
+        case crane_msgs::msg::RobotCommand::POLAR_VELOCITY_TARGET_MODE: {
+          packet.control_mode = POLAR_VELOCITY_TARGET_MODE;
+          if (not command.polar_velocity_target_mode.empty()) {
+            packet.mode_args.polar_velocity.target_global_velocity_r =
+              command.polar_velocity_target_mode.front().target_velocity_r;
+            packet.mode_args.polar_velocity.target_global_velocity_theta =
+              command.polar_velocity_target_mode.front().target_velocity_theta;
+          } else {
+            std::cout << "empty polar_velocity_target_mode" << std::endl;
+          }
+        } break;
         case crane_msgs::msg::RobotCommand::LOCAL_CAMERA_MODE: {
           packet.control_mode = LOCAL_CAMERA_MODE;
           if (not command.local_camera_mode.empty()) {

crane_sender/test/test_robot_packet.cpp

@@ -206,13 +206,9 @@ TEST(RobotPacket, ENcodeDecode)
 
   {
     // VelocityTargetWithTrajectoryModeArgs
-    packet.control_mode = VELOCITY_TARGET_WITH_TRAJECTORY_MODE;
-    packet.mode_args.velocity.target_global_vel[0] = dist_32(gen);
-    packet.mode_args.velocity.target_global_vel[1] = dist_32(gen);
-    packet.mode_args.velocity.trajectory_global_origin[0] = dist_32(gen);
-    packet.mode_args.velocity.trajectory_global_origin[1] = dist_32(gen);
-    packet.mode_args.velocity.trajectory_origin_angle = dist_pi(gen);
-    packet.mode_args.velocity.trajectory_curvature = dist_32(gen);
+    packet.control_mode = POLAR_VELOCITY_TARGET_MODE;
+    packet.mode_args.polar_velocity.target_global_velocity_r = dist_32(gen);
+    packet.mode_args.polar_velocity.target_global_velocity_theta = dist_32(gen);
 
     RobotCommandSerializedV2 serialized_packet;
     RobotCommandSerializedV2_serialize(&serialized_packet, &packet);
@@ -247,23 +243,11 @@ TEST(RobotPacket, ENcodeDecode)
       deserialized_packet.elapsed_time_ms_since_last_vision);
     EXPECT_EQ(packet.control_mode, deserialized_packet.control_mode);
     EXPECT_NEAR(
-      packet.mode_args.velocity.target_global_vel[0],
-      deserialized_packet.mode_args.velocity.target_global_vel[0], MAX_ERROR_32);
+      packet.mode_args.polar_velocity.target_global_velocity_r,
+      deserialized_packet.mode_args.polar_velocity.target_global_velocity_r, MAX_ERROR_32);
     EXPECT_NEAR(
-      packet.mode_args.velocity.target_global_vel[1],
-      deserialized_packet.mode_args.velocity.target_global_vel[1], MAX_ERROR_32);
-    EXPECT_NEAR(
-      packet.mode_args.velocity.trajectory_global_origin[0],
-      deserialized_packet.mode_args.velocity.trajectory_global_origin[0], MAX_ERROR_32);
-    EXPECT_NEAR(
-      packet.mode_args.velocity.trajectory_global_origin[1],
-      deserialized_packet.mode_args.velocity.trajectory_global_origin[1], MAX_ERROR_32);
-    EXPECT_NEAR(
-      packet.mode_args.velocity.trajectory_origin_angle,
-      deserialized_packet.mode_args.velocity.trajectory_origin_angle, MAX_ERROR_PI);
-    EXPECT_NEAR(
-      packet.mode_args.velocity.trajectory_curvature,
-      deserialized_packet.mode_args.velocity.trajectory_curvature, MAX_ERROR_32);
+      packet.mode_args.polar_velocity.target_global_velocity_theta,
+      deserialized_packet.mode_args.polar_velocity.target_global_velocity_theta, MAX_ERROR_32);
   }
 }