Merge remote-tracking branch 'origin/develop' into fix/robot_receiver

# Conflicts:
#	crane_robot_receiver/src/robot_receiver_node.cpp

Author: HansRobo

crane_local_planner/include/crane_local_planner/simple_planner.hpp

@@ -44,6 +44,9 @@ class SimplePlanner
     for (auto & command : commands.robot_commands) {
       if ((not command.target_x.empty()) && (not command.target_y.empty())) {
         auto robot = world_model->getOurRobot(command.robot_id);
+        command.current_pose.x = robot->pose.pos.x();
+        command.current_pose.y = robot->pose.pos.y();
+        command.current_pose.theta = robot->pose.theta;
         Point target;
         target << command.target_x.front(), command.target_y.front();
 

crane_robot_receiver/src/robot_receiver_node.cpp

@@ -4,16 +4,6 @@
 // license that can be found in the LICENSE file or at
 // https://opensource.org/licenses/MIT.
 
-//#include <arpa/inet.h>
-//#include <netinet/in.h>
-//#include <stdio.h>
-//#include <stdlib.h>
-//#include <string.h>
-//#include <sys/socket.h>
-//#include <sys/types.h>
-//#include <time.h>
-
-//#include <atomic>
 #include <boost/asio.hpp>
 #include <boost/thread.hpp>
 #include <crane_msgs/msg/robot_feedback.hpp>
@@ -33,7 +23,7 @@ struct RobotInterfaceConfig
 auto makeConfig(uint8_t id) -> RobotInterfaceConfig
 {
   RobotInterfaceConfig config;
-  config.ip = "224.5.20.10" + std::to_string(id);
+  config.ip = "224.5.20." + std::to_string(id + 100);
   config.port = 50100 + id;
   return config;
 }
@@ -293,40 +283,5 @@ int main(int argc, char * argv[])
   exe.add_node(node->get_node_base_interface());
   exe.spin();
   rclcpp::shutdown();
-  //    try {
-  //      boost::asio::io_service io_service;
-  //
-  //      // ローカルホストの指定したポートでUDPソケットを開く
-  //      udp::socket socket(io_service);
-  //      auto listen_address = boost::asio::ip::address::from_string("224.5.20.100");
-  //      boost::asio::ip::udp::endpoint listen_endpoint(listen_address, 50100);
-  //
-  //      socket.open(listen_endpoint.protocol());
-  //
-  //      if (socket.is_open()) {
-  //        std::cout << "socket opened" << std::endl;
-  //      } else {
-  //        std::cout << "socket not opened" << std::endl;
-  //      }
-  //      socket.set_option(boost::asio::ip::udp::socket::reuse_address(true));
-  //      socket.bind(listen_endpoint);
-  //
-  //      // Join the multicast group.
-  //      socket.set_option(boost::asio::ip::multicast::join_group(listen_address));
-  //
-  //
-  //      std::cout << "joined multicast group" << std::endl;
-  //
-  //      for (;;) {  // 無限ループでパケットを待ち受ける
-  //        char data[1024];
-  //        udp::endpoint sender_endpoint;
-  //        size_t length = socket.receive_from(boost::asio::buffer(data), sender_endpoint);
-  //
-  //        std::cout << "Received packet: " << std::string(data, length) << std::endl;
-  //      }
-  //    } catch (std::exception & e) {
-  //      std::cerr << "Exception: " << e.what() << "\n";
-  //    }
-
   return 0;
 }

crane_sender/include/crane_sender/sim_sender.hpp

@@ -35,6 +35,10 @@ class SimSenderComponent : public SenderBase
 
   void sendCommands(const crane_msgs::msg::RobotCommands & msg) override
   {
+    if (checkNan(msg)) {
+      return;
+    }
+
     const double MAX_KICK_SPEED = 8.0;  // m/s
     robocup_ssl_msgs::msg::Commands commands;
     commands.isteamyellow = msg.is_yellow;
@@ -75,6 +79,34 @@ class SimSenderComponent : public SenderBase
     pub_commands->publish(commands);
   }
 
+  bool checkNan(const crane_msgs::msg::RobotCommands & msg)
+  {
+    bool is_nan = false;
+    for (const auto & command : msg.robot_commands) {
+      if (std::isnan(command.target_velocity.x)) {
+        std::cout << "id: " << command.robot_id << " target_velocity.x is nan" << std::endl;
+        is_nan = true;
+      }
+      if (std::isnan(command.target_velocity.y)) {
+        std::cout << "id: " << command.robot_id << "target_velocity.y is nan" << std::endl;
+        is_nan = true;
+      }
+      if (std::isnan(command.target_velocity.theta)) {
+        std::cout << "id: " << command.robot_id << "target_velocity.theta is nan" << std::endl;
+        is_nan = true;
+      }
+      if (std::isnan(command.kick_power)) {
+        std::cout << "id: " << command.robot_id << "kick_power is nan" << std::endl;
+        is_nan = true;
+      }
+      if (std::isnan(command.dribble_power)) {
+        std::cout << "id: " << command.robot_id << "dribble_power is nan" << std::endl;
+        is_nan = true;
+      }
+    }
+    return is_nan;
+  }
+
   //  void send_replacement(const consai2r2_msgs::msg::Replacements::SharedPtr msg) const
   //  {
   //

crane_sender/src/real_sender_node.cpp

@@ -33,7 +33,7 @@ int check;
 int sock;
 struct sockaddr_in addr;
 
-const char * opt = "enx00e04c696e2b";
+const char * opt = "enp4s0";
 
 namespace crane
 {
@@ -132,8 +132,13 @@ class RealSenderNode : public SenderBase
 
       // Vision角度
       // -pi ~ pi -> 0 ~ 32767 ~ 65534
+      packet.VISION_GLOBAL_X = command.current_pose.x;
+      packet.VISION_GLOBAL_Y = command.current_pose.y;
       packet.VISION_GLOBAL_THETA = command.current_pose.theta;
 
+      packet.BALL_GLOBAL_X = command.current_ball_x;
+      packet.BALL_GLOBAL_Y = command.current_ball_y;
+
       // 目標座標
       packet.TARGET_GLOBAL_X = [&]() -> float {
         if (not command.target_x.empty()) {

docs/vision.md

@@ -60,6 +60,12 @@ Thread0の「ImageCapture/CaptureControl」の「start capture」
 - Number of Line Segments
 - Number of Arcs
 
+※ロボットがオフセットしているように感じたら...
+- 「Global/Robot Detection/BlueTeam」などからチームを確認
+- 「Global/Robot Detection/Teams/ER-Force」などからロボットの高さを調整
+ロボットの高さをゼロにするとオフセットがなくなることがある
+
+
 ## 色の設定
 
 右側の「Auto Color Calibration」タブを使って設定する

scripts/camera.bash

@@ -3,13 +3,13 @@ device_id=0
 # manual: 1
 v4l2-ctl -d $device_id -c exposure_auto=1
 #v4l2-ctl -d $device_id -c exposure_time_absolute=120
-v4l2-ctl -d $device_id -c exposure_absolute=170
+v4l2-ctl -d $device_id -c exposure_absolute=300
 
 #v4l2-ctl -d $device_id -c focus_automatic_continuous=0
 v4l2-ctl -d $device_id -c focus_auto=0
 v4l2-ctl -d $device_id -c focus_absolute=0
 
 
 
-v4l2-ctl -d $device_id -c brightness=120
+v4l2-ctl -d $device_id -c brightness=170
 v4l2-ctl -d $device_id -c sharpness=400

scripts/measure_laytency.py

@@ -0,0 +1,57 @@
+import math
+import time
+from rclpy.node import Node
+import rclpy
+from robocup_ssl_msgs.msg import TrackedFrame, RobotId
+from crane_msgs.msg import RobotCommands
+
+class MeasureLatency(Node):
+    def __init__(self):
+        super().__init__('measure_latency')
+        self.detection_sub = self.create_subscription(TrackedFrame, '/detection_tracked', self.detection_callback, 10)
+        self.command_sub = self.create_subscription(RobotCommands, '/robot_commands',self.command_callback, 10)
+        self.robot_id = 0
+        self.move_start_time = None
+        self.pre_vel = 0.0
+        self.VEL_THRESHOLD = 0.1
+        # self.is_waiting_for_moving = False
+
+    def detection_callback(self, msg: TrackedFrame):
+        # print("detection_callback")
+        if self.move_start_time is not None:
+            robot = None
+            for r in msg.robots:
+                if r.robot_id.id == self.robot_id and r.robot_id.team_color == RobotId.TEAM_COLOR_BLUE:
+                    if len(r.vel) > 0:
+                        vel_2d = r.vel[0]
+                        vel = math.sqrt(vel_2d.x ** 2 + vel_2d.y ** 2)
+                        if vel > self.VEL_THRESHOLD:
+                            self.get_logger().info('start detected! latency: {}'.format((time.time() - self.move_start_time) * 1000))
+                            self.move_start_time = None
+                            break
+
+
+    def command_callback(self, msg):
+        # print("command_callback")
+        robot = None
+        for command in msg.robot_commands:
+            if command.robot_id == self.robot_id:
+                robot = command
+                break
+        if robot is None:
+            return
+        # get target velocity size
+        vel = math.sqrt(robot.target_velocity.x ** 2 + robot.target_velocity.y ** 2)
+        # print(vel)
+        if self.pre_vel < self.VEL_THRESHOLD and vel > self.VEL_THRESHOLD and self.move_start_time is None:
+            print("start moving")
+            self.move_start_time = time.time()
+        self.pre_vel = vel
+
+
+if __name__ == '__main__':
+    rclpy.init()
+    node = MeasureLatency()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()

scripts/network.bash

@@ -1,6 +1,6 @@
 robot_wifi_device="enp4s0"
 # Visionパケットをロボットに送信しない
-sudo iptables -A OUTPUT -o $robot_wifi_device -d 224.5.23.2 -j DROP
+sudo iptables -A FORWARD -o $robot_wifi_device -d 224.5.23.2 -j DROP
 # Refereeパケットをロボットに送信しない
-sudo iptables -A OUTPUT -o $robot_wifi_device -d 224.5.23.1 -j DROP
+sudo iptables -A FORWARD -o $robot_wifi_device -d 224.5.23.1 -j DROP
 

session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp

@@ -130,8 +130,10 @@ class GoaliePlanner : public PlannerBase
           [[fallthrough]];
         case crane_msgs::msg::PlaySituation::THEIR_PENALTY_START:
           inplay(target, false);
+          break;
         default:
           inplay(target, true);
+          break;
       }
 
       control_targets.emplace_back(target.getMsg());