[add] add wait_for_reaching_initial_values parameter

include/topic_based_ros2_control/topic_based_system.hpp

@@ -73,6 +73,7 @@ class TopicBasedSystem : public hardware_interface::SystemInterface
   bool sum_wrapped_joint_states_{ false };
   bool initial_states_as_initial_cmd_{ false };
   bool ready_to_send_cmds_{ false };
+  bool initial_cmd_reached_{ false };
 
   /// Use standard interfaces for joints because they are relevant for dynamic behavior
   std::array<std::string, 4> standard_interfaces_ = { hardware_interface::HW_IF_POSITION,
@@ -93,6 +94,7 @@ class TopicBasedSystem : public hardware_interface::SystemInterface
   /// The size of this vector is (standard_interfaces_.size() x nr_joints)
   std::vector<std::vector<double>> joint_commands_;
   std::vector<std::vector<double>> joint_states_;
+  std::vector<std::vector<double>> initial_joint_commands_;
 
   // If the difference between the current joint state and joint command is less than this value,
   // the joint command will not be published.

src/topic_based_system.cpp

@@ -78,6 +78,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
   }
 
   // Initial command values
+  initial_joint_commands_ = joint_commands_;
   for (auto i = 0u; i < info_.joints.size(); i++)
   {
     const auto& component = info_.joints[i];
@@ -91,7 +92,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
         // Check the initial_value param is used
         if (!interface.initial_value.empty())
         {
-          joint_commands_[index][i] = std::stod(interface.initial_value);
+          initial_joint_commands_[index][i] = std::stod(interface.initial_value);
         }
       }
     }
@@ -164,6 +165,10 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
     initial_states_as_initial_cmd_ = true;
     ready_to_send_cmds_ = false;
   }
+  if (get_hardware_parameter("wait_for_reaching_initial_values", "false") == "false")
+  {
+    initial_cmd_reached_ = true;
+  }
 
   return CallbackReturn::SUCCESS;
 }
@@ -288,6 +293,22 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
   {
     return hardware_interface::return_type::ERROR;
   }
+
+  if (initial_cmd_reached_ == false)
+  {
+    const auto diff = std::transform_reduce(
+        joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(),
+        initial_joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0,
+        [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus<double>{});
+    if (diff < trigger_joint_command_threshold_)
+    {
+      initial_cmd_reached_ = true;
+    }
+    else
+    {
+      joint_commands_ = initial_joint_commands_;
+    }
+  }
   // To avoid spamming TopicBased's joint command topic we check the difference between the joint states and
   // the current joint commands, if it's smaller than a threshold we don't publish it.
   const auto diff = std::transform_reduce(