Merge pull request #56 from ibis-ssl/add-1012

Add 1012

camera.bash

@@ -0,0 +1,10 @@
+device_id = 0
+v4l2-ctl -d $device_id -c exposure_dynamic_framerate=0
+v4l2-ctl -d $device_id -c auto_exposure=1
+v4l2-ctl -d $device_id -c focus_automatic_continuous=0
+v4l2-ctl -d $device_id -c focus_absolute=0
+
+
+v4l2-ctl -d $device_id -c exposure_time_absolute=120
+v4l2-ctl -d $device_id -c brightness=120
+v4l2-ctl -d $device_id -c sharpness=400

consai_ros2/robocup_ssl_comm/src/grsim_component.cpp

@@ -35,12 +35,8 @@ GrSim::GrSim(const rclcpp::NodeOptions & options) : Node("grsim", options)
     create_subscription<Commands>("commands", 10, std::bind(&GrSim::callback_commands, this, _1));
   sub_replacement = create_subscription<Replacement>(
     "replacement", 10, std::bind(&GrSim::callback_replacement, this, _1));
-
-  // timer_ = create_wall_timer(1s, std::bind(&GrSim::on_timer, this));
 }
 
-void GrSim::on_timer() { RCLCPP_INFO(get_logger(), "Hello World!"); }
-
 void GrSim::callback_commands(const Commands::SharedPtr msg)
 {
   grSim_Commands * commands = new grSim_Commands();

crane_bringup/launch/play_switcher.launch.py

@@ -112,7 +112,9 @@ def generate_launch_description():
         parameters=[
             {
                 "enable_rvo": False,
-                "non_rvo_gain": 2.15,
+                # "non_rvo_gain": 2.15,
+                "non_rvo_p_gain": 1.0,
+                "non_rvo_d_gain": 1.0,
             }
         ],
     )
@@ -182,7 +184,7 @@ def generate_launch_description():
     real_sender = Node(
         package="crane_sender",
         executable="real_sender_node",
-        # output="screen",
+        output="screen",
         parameters=[{}],
     )
 

crane_local_planner/include/crane_local_planner/local_planner.hpp

@@ -21,6 +21,38 @@ namespace crane
 {
 class LocalPlannerComponent : public rclcpp::Node
 {
+  class PIDController
+  {
+  public:
+    PIDController() = default;
+
+    void setGain(float kp, float ki, float kd)
+    {
+      this->kp = kp;
+      this->ki = ki;
+      this->kd = kd;
+      error_prev = 0.0f;
+    }
+
+    float update(float error, float dt)
+    {
+      float p = kp * error;
+      float i = ki * (error + error_prev) * dt / 2.0f;
+      float d = kd * (error - error_prev) / dt;
+      error_prev = error;
+      return p + i + d;
+    }
+
+  private:
+    float kp;
+
+    float ki;
+
+    float kd;
+
+    float error_prev;
+  };
+
 public:
   COMPOSITION_PUBLIC
   explicit LocalPlannerComponent(const rclcpp::NodeOptions & options)
@@ -51,8 +83,21 @@ class LocalPlannerComponent : public rclcpp::Node
     RVO_TRAPEZOIDAL_MAX_SPEED = get_parameter("rvo_trapezoidal_max_speed").as_double();
     declare_parameter("non_rvo_max_vel", NON_RVO_MAX_VEL);
     NON_RVO_MAX_VEL = get_parameter("non_rvo_max_vel").as_double();
-    declare_parameter("non_rvo_gain", NON_RVO_GAIN);
-    NON_RVO_GAIN = get_parameter("non_rvo_gain").as_double();
+
+    declare_parameter("non_rvo_p_gain", NON_RVO_P_GAIN);
+    NON_RVO_P_GAIN = get_parameter("non_rvo_p_gain").as_double();
+    declare_parameter("non_rvo_i_gain", NON_RVO_I_GAIN);
+    NON_RVO_I_GAIN = get_parameter("non_rvo_i_gain").as_double();
+    declare_parameter("non_rvo_d_gain", NON_RVO_D_GAIN);
+    NON_RVO_D_GAIN = get_parameter("non_rvo_d_gain").as_double();
+
+    for (auto & controller : vx_controllers) {
+      controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN);
+    }
+
+    for (auto & controller : vy_controllers) {
+      controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN);
+    }
 
     rvo_sim = std::make_unique<RVO::RVOSimulator>(
       RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST,
@@ -94,6 +139,9 @@ class LocalPlannerComponent : public rclcpp::Node
 
   bool enable_rvo;
 
+  std::array<PIDController, 20> vx_controllers;
+  std::array<PIDController, 20> vy_controllers;
+
   float RVO_TIME_STEP = 1.0 / 60.0f;
   float RVO_NEIGHBOR_DIST = 2.0f;
   int RVO_MAX_NEIGHBORS = 5;
@@ -107,7 +155,9 @@ class LocalPlannerComponent : public rclcpp::Node
   float RVO_TRAPEZOIDAL_MAX_SPEED = 4.0;
 
   double NON_RVO_MAX_VEL = 4.0;
-  double NON_RVO_GAIN = 4.0;
+  double NON_RVO_P_GAIN = 4.0;
+  double NON_RVO_I_GAIN = 0.0;
+  double NON_RVO_D_GAIN = 0.0;
 };
 
 }  // namespace crane

crane_local_planner/src/crane_local_planner.cpp

@@ -221,9 +221,10 @@ void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCo
 
         // 速度に変換する
         Velocity vel;
-        vel << command.target_x.front() - command.current_pose.x,
-          command.target_y.front() - command.current_pose.y;
-        vel *= NON_RVO_GAIN;
+        vel << vx_controllers[command.robot_id].update(
+          command.target_x.front() - command.current_pose.x, 1.f / 30.f),
+          vy_controllers[command.robot_id].update(
+            command.target_y.front() - command.current_pose.y, 1.f / 30.f);
         vel += vel.normalized() * command.local_planner_config.terminal_velocity;
         if (vel.norm() > max_vel) {
           vel = vel.normalized() * max_vel;
@@ -232,17 +233,19 @@ void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCo
         command.target_velocity.x = vel.x();
         command.target_velocity.y = vel.y();
 
-        double MAX_THETA_DIFF = max_omega / 30.0f;
-        // 1フレームで変化するthetaの量が大きすぎると急に回転するので制限する
-        if (not command.target_theta.empty()) {
-          double theta_diff =
-            getAngleDiff(command.target_theta.front(), command.current_pose.theta);
-          if (std::fabs(theta_diff) > MAX_THETA_DIFF) {
-            theta_diff = std::copysign(MAX_THETA_DIFF, theta_diff);
-          }
-
-          command.target_theta.front() = command.current_pose.theta + theta_diff;
-        }
+        //　2023/11/12 出力の目標角度制限をしたらVisionの遅れと相まってロボットが角度方向に発振したのでコメントアウトする
+        // そしてこの過ちを再びおかさぬようここに残しておく． R.I.P.
+        //        double MAX_THETA_DIFF = max_omega / 30.0f;
+        //        // 1フレームで変化するthetaの量が大きすぎると急に回転するので制限する
+        //        if (not command.target_theta.empty()) {
+        //          double theta_diff =
+        //            getAngleDiff(command.target_theta.front(), command.current_pose.theta);
+        //          if (std::fabs(theta_diff) > MAX_THETA_DIFF) {
+        //            theta_diff = std::copysign(MAX_THETA_DIFF, theta_diff);
+        //          }
+        //
+        //          command.target_theta.front() = command.current_pose.theta + theta_diff;
+        //        }
 
         command.current_ball_x = world_model->ball.pos.x();
         command.current_ball_y = world_model->ball.pos.y();

crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp

@@ -78,7 +78,7 @@ class TurnAroundPoint : public SkillBase<>
 
   double max_turn_omega = M_PI_4;
 
-  double max_velocity = 0.1;
+  double max_velocity = 0.5;
 };
 }  // namespace crane
 #endif  // CRANE_ROBOT_SKILLS__TURN_AROUND_POINT_HPP_

crane_sender/package.xml

@@ -15,6 +15,7 @@
   <depend>robocup_ssl_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>crane_msg_wrappers</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>crane_lint_common</test_depend>

crane_sender/src/real_sender_node.cpp

@@ -18,6 +18,7 @@
 #include <netinet/udp.h>
 
 #include <class_loader/visibility_control.hpp>
+#include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/robot_commands.hpp>
 #include <iostream>
 #include <memory>
@@ -39,9 +40,13 @@ class RealSenderNode : public SenderBase
 {
 private:
   int debug_id;
+
   std::shared_ptr<rclcpp::ParameterEventHandler> parameter_subscriber;
+
   std::shared_ptr<rclcpp::ParameterCallbackHandle> parameter_callback_handle;
 
+  WorldModelWrapper::SharedPtr world_model;
+
 public:
   CLASS_LOADER_PUBLIC
   explicit RealSenderNode(const rclcpp::NodeOptions & options) : SenderBase("real_sender", options)
@@ -58,10 +63,16 @@ class RealSenderNode : public SenderBase
         }
       });
 
+    world_model = std::make_shared<WorldModelWrapper>(*this);
+
     std::cout << "start" << std::endl;
   }
+
   void sendCommands(const crane_msgs::msg::RobotCommands & msg) override
   {
+    if (not world_model->hasUpdated()) {
+      return;
+    }
     uint8_t send_packet[32] = {};
 
     constexpr double MAX_VEL_SURGE = 7.0;  // m/s
@@ -168,20 +179,6 @@ class RealSenderNode : public SenderBase
         }
       }();
 
-      // キーパーEN
-      // 0 or 1
-      uint8_t keeper_EN = command.local_goalie_enable;
-
-      // Vision位置
-      //  -32.767 ~ 0 ~ 32.767 -> 0 ~ 32767 ~ 65534
-      // meter -> mili meter
-      auto [vision_x_low, vision_x_high] = to_two_byte(command.current_pose.x, 32.767);
-      auto [vision_y_low, vision_y_high] = to_two_byte(command.current_pose.y, 32.767);
-
-      //ボール座標
-      auto [ball_x_low, ball_x_high] = to_two_byte(command.current_ball_x, 32.767);
-      auto [ball_y_low, ball_y_high] = to_two_byte(command.current_ball_y, 32.767);
-
       // 目標座標
       float target_x = 0.f;
       float target_y = 0.f;
@@ -196,6 +193,33 @@ class RealSenderNode : public SenderBase
       } else {
         enable_local_feedback = false;
       }
+
+      enable_local_feedback = false;
+
+      std::vector<uint8_t> available_ids = world_model->ours.getAvailableRobotIds();
+      bool is_id_available =
+        std::count(available_ids.begin(), available_ids.end(), command.robot_id) == 1;
+      std::cout << "id( " << command.robot_id << " ) is available: " << is_id_available
+                << std::endl;
+      // キーパーEN
+      // 0 or 1
+
+      uint8_t keeper_EN = command.local_goalie_enable;
+      uint8_t local_flags = 0x00;
+      local_flags |= (is_id_available << 0);
+      local_flags |= (enable_local_feedback << 2);
+      local_flags |= (keeper_EN << 4);
+
+      // Vision位置
+      //  -32.767 ~ 0 ~ 32.767 -> 0 ~ 32767 ~ 65534
+      // meter -> mili meter
+      auto [vision_x_low, vision_x_high] = to_two_byte(command.current_pose.x, 32.767);
+      auto [vision_y_low, vision_y_high] = to_two_byte(command.current_pose.y, 32.767);
+
+      //ボール座標
+      auto [ball_x_low, ball_x_high] = to_two_byte(command.current_ball_x, 32.767);
+      auto [ball_y_low, ball_y_high] = to_two_byte(command.current_ball_y, 32.767);
+
       auto [target_x_low, target_x_high] = to_two_byte(target_x, 32.767);
       auto [target_y_low, target_y_high] = to_two_byte(target_y, 32.767);
 
@@ -217,7 +241,7 @@ class RealSenderNode : public SenderBase
       send_packet[8] = static_cast<uint8_t>(target_theta_low);
       send_packet[9] = static_cast<uint8_t>(kick_power_send);
       send_packet[10] = static_cast<uint8_t>(dribble_power_send);
-      send_packet[11] = static_cast<uint8_t>(keeper_EN);
+      send_packet[11] = static_cast<uint8_t>(local_flags);
       send_packet[12] = static_cast<uint8_t>(ball_x_high);
       send_packet[13] = static_cast<uint8_t>(ball_x_low);
       send_packet[14] = static_cast<uint8_t>(ball_y_high);
@@ -230,7 +254,6 @@ class RealSenderNode : public SenderBase
       send_packet[21] = static_cast<uint8_t>(target_x_low);
       send_packet[22] = static_cast<uint8_t>(target_y_high);
       send_packet[23] = static_cast<uint8_t>(target_y_low);
-      send_packet[24] = static_cast<uint8_t>(enable_local_feedback);
 
       if (command.robot_id == debug_id) {
         printf(

docs/grSim.md

@@ -0,0 +1,3 @@
+# grSim
+
+

docs/network.md

@@ -0,0 +1,32 @@
+# ネットワーク設定
+
+## ROS関連
+
+https://autowarefoundation.github.io/autoware-documentation/pr-347/installation/additional-settings-for-developers/#network-settings-for-ros-2
+
+### ローカルホストでマルチキャスト
+```bash
+sudo ip link set multicast on lo
+```
+
+## インターネット接続とロボット接続の共存
+
+ロボットのアドレスに対して静的ルーティングを設定する
+
+`/etc/netplan/01-network-manager-all.yaml`
+```yaml
+network:
+  version: 2
+  renderer: networkd
+  ethernets:
+    enp3s0:
+      addresses:
+      - 192.168.1.2/24
+      gateway4: 192.168.1.1
+      routes:
+      - to: 192.168.2.0/24
+        via: 192.168.1.1
+      - to: 192.168.3.0/24
+        via: 192.168.1.1
+
+```

session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_with_skill_planner.hpp

@@ -92,6 +92,8 @@ class BallPlacementWithSkillPlanner : public PlannerBase
         state = BallPlacementState::GET_BALL_CONTACT;
         turn_around_point = nullptr;
       }
+      command.setMaxVelocity(0.5);
+      command.setTerminalVelocity(0.5);
     } else if (state == BallPlacementState::GET_BALL_CONTACT) {
       if (get_ball_contact->run(command) == SkillBase<>::Status::SUCCESS) {
         move_with_ball->target_pose.pos =

session/crane_session_controller/config/defense.yaml

@@ -0,0 +1,18 @@
+---
+events:
+  - event: "HALT"
+    session: "HALT"
+  - event: "STOP"
+    session: "formation"
+  - event: "THEIR_KICKOFF_PREPARATION"
+    session: "formation"
+  - event: "THEIR_KICKOFF_START"
+    session: "formation"
+  - event: "OUR_KICKOFF_PREPARATION"
+    session: "formation"
+  - event: "OUR_KICKOFF_START"
+    session: "SIMPLE_ATTACK"
+  - event: "INPLAY"
+    session: "SIMPLE_ATTACK"
+  - event: "OUR_BALL_PLACEMENT"
+    session: "ball_placement_with_skill"