From 7178c8233a5900dd14890952e220add21128b276 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Sun, 4 Aug 2024 19:32:50 +0900
Subject: [PATCH] =?UTF-8?q?latency=5Fms=E3=82=92launch=E3=81=A7=E8=A8=AD?=
 =?UTF-8?q?=E5=AE=9A=E3=81=A7=E3=81=8D=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB?=
 =?UTF-8?q?=E3=81=99=E3=82=8B=20(#511)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 crane_bringup/launch/crane.launch.xml             | 2 ++
 crane_sender/include/crane_sender/sender_base.hpp | 3 +++
 crane_sender/src/ibis_sender_node.cpp             | 2 +-
 3 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/crane_bringup/launch/crane.launch.xml b/crane_bringup/launch/crane.launch.xml
index b62b92107..fc5998ddf 100644
--- a/crane_bringup/launch/crane.launch.xml
+++ b/crane_bringup/launch/crane.launch.xml
@@ -55,6 +55,7 @@
   <group if="$(var original_grsim)">
     <node pkg="crane_sender" exec="sim_sender_node" output="screen">
       <param name="no_movement" value="false"/>
+      <param name="latency_ms" value="0.0"/>
       <param name="k_gain" value="2.0"/>
       <param name="i_gain" value="0.0"/>
       <param name="d_gain" value="0.1"/>
@@ -68,6 +69,7 @@
   <group unless="$(var original_grsim)">
     <node pkg="crane_sender" exec="ibis_sender_node" output="screen">
       <param name="no_movement" value="false"/>
+      <param name="latency_ms" value="0.0"/>
       <param name="theta_kp" value="3.5"/>
       <param name="theta_ki" value="0.0"/>
       <param name="theta_kd" value="0.5"/>
diff --git a/crane_sender/include/crane_sender/sender_base.hpp b/crane_sender/include/crane_sender/sender_base.hpp
index bfdaa0f09..4e194d656 100644
--- a/crane_sender/include/crane_sender/sender_base.hpp
+++ b/crane_sender/include/crane_sender/sender_base.hpp
@@ -29,6 +29,9 @@ class SenderBase : public rclcpp::Node
     declare_parameter<double>("delay_s", 0.0);
     get_parameter("delay_s", delay_s);
 
+    declare_parameter<double>("latency_ms", 0.0);
+    get_parameter("latency_ms", current_latency_ms);
+
     // the parameters of the PID controller
     declare_parameter<float>("theta_kp", 4.0);
     declare_parameter<float>("theta_ki", 0.0);
diff --git a/crane_sender/src/ibis_sender_node.cpp b/crane_sender/src/ibis_sender_node.cpp
index c1162bf06..32f5cca31 100644
--- a/crane_sender/src/ibis_sender_node.cpp
+++ b/crane_sender/src/ibis_sender_node.cpp
@@ -157,7 +157,7 @@ class IbisSenderNode : public SenderBase
         std::vector<uint8_t> available_ids = world_model->ours.getAvailableRobotIds();
         return std::count(available_ids.begin(), available_ids.end(), command.robot_id) == 1;
       }();
-      packet.latency_time_ms = 100;  // TODO(Hans): ちゃんと計測する
+      packet.latency_time_ms = current_latency_ms;  // TODO(Hans): ちゃんと計測する
       packet.target_global_theta = command.target_theta;
       packet.kick_power = std::clamp(command.kick_power, 0.f, 1.f);
       packet.dribble_power = std::clamp(command.dribble_power, 0.f, 1.f);