Allow option to initialise robot in any starting state

include/topic_based_ros2_control/topic_based_system.hpp

@@ -71,6 +71,8 @@ class TopicBasedSystem : public hardware_interface::SystemInterface
   rclcpp::Node::SharedPtr node_;
   sensor_msgs::msg::JointState latest_joint_state_;
   bool sum_wrapped_joint_states_{ false };
+  bool initial_states_as_initial_cmd_{ false };
+  bool ready_to_send_cmds_{ 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,

src/topic_based_system.cpp

@@ -96,6 +96,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
       }
     }
   }
+  ready_to_send_cmds_ = true;
 
   // Search for mimic joints
   for (auto i = 0u; i < info_.joints.size(); ++i)
@@ -156,6 +157,11 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
   {
     sum_wrapped_joint_states_ = true;
   }
+  if (get_hardware_parameter("use_initial_states_as_initial_commands", "false") == "true")
+  {
+    initial_states_as_initial_cmd_ = true;
+    ready_to_send_cmds_ = false;
+  }
 
   return CallbackReturn::SUCCESS;
 }
@@ -244,6 +250,18 @@ hardware_interface::return_type TopicBasedSystem::read(const rclcpp::Time& /*tim
     }
   }
 
+  if (!ready_to_send_cmds_ && initial_states_as_initial_cmd_)
+  {
+    for (std::size_t i = 0; i < joint_states_.size(); ++i)
+    {
+      for (std::size_t j = 0; j < joint_states_[i].size(); ++j)
+      {
+        joint_commands_[i][j] = joint_states_[i][j];
+      }
+    }
+    ready_to_send_cmds_ = true;
+  }
+
   return hardware_interface::return_type::OK;
 }
 
@@ -264,6 +282,10 @@ bool TopicBasedSystem::getInterface(const std::string& name, const std::string&
 
 hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
 {
+  if (!ready_to_send_cmds_)
+  {
+    return hardware_interface::return_type::ERROR;
+  }
   // 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(