latency_msをlaunchで設定できるようにする (#511)

ibis-ssl · Aug 4, 2024 · 7178c82 · 7178c82
1 parent 96c4e6e
commit 7178c82
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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);