[fix] Tested with Isaac Sim and confirmed that it was an unnecessary …

…process, so it was removed.Tested with Isaac Sim and removed as it was an unnecessary process.
PickNikRobotics · Oct 31, 2024 · 197f424 · 197f424
1 parent 9131012
commit 197f424
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 33 deletions.
diff --git a/include/topic_based_ros2_control/topic_based_system.hpp b/include/topic_based_ros2_control/topic_based_system.hpp
@@ -71,7 +71,6 @@ 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_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,
@@ -92,7 +91,6 @@ 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.

diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp
@@ -78,7 +78,6 @@ 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];
@@ -94,8 +93,6 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
         {
           joint_states_[index][i] = std::stod(interface.initial_value);
           joint_commands_[index][i] = std::stod(interface.initial_value);
-
-          initial_joint_commands_[index][i] = std::stod(interface.initial_value);
         }
       }
     }
@@ -162,10 +159,6 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
   {
     sum_wrapped_joint_states_ = true;
   }
-  if (get_hardware_parameter("wait_for_reaching_initial_values", "false") == "false")
-  {
-    initial_cmd_reached_ = true;
-  }
 
   return CallbackReturn::SUCCESS;
 }
@@ -274,37 +267,18 @@ bool TopicBasedSystem::getInterface(const std::string& name, const std::string&
 
 hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
 {
-  std::vector<std::vector<double>> target_joint_commands;
-  if (initial_cmd_reached_ == false)
-  {
-    double abs_sum = std::accumulate(joint_commands_[POSITION_INTERFACE_INDEX].begin(),
-                                     joint_commands_[POSITION_INTERFACE_INDEX].end(), 0,
-                                     [](double sum, double val) { return sum + std::abs(val); });
-    if (abs_sum >= trigger_joint_command_threshold_)
-    {
-      initial_cmd_reached_ = true;
-    }
-  }
-  if (initial_cmd_reached_ == false)
-  {
-    target_joint_commands = initial_joint_commands_;
-  }
-  else
-  {
-    target_joint_commands = 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(
       joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(),
-      target_joint_commands[POSITION_INTERFACE_INDEX].cbegin(), 0.0,
+      joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0,
       [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus<double>{});
 
   bool exist_velocity_command = false;
   static bool exist_velocity_command_old = false;  // Use old state to publish zero velocities
   for (std::size_t i = 0; i < info_.joints.size(); ++i)
   {
-    if (fabs(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_)
+    if (fabs(joint_commands_[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_)
     {
       exist_velocity_command = true;
     }
@@ -330,7 +304,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
         if (interface.name == hardware_interface::HW_IF_POSITION)
         {
           joint_state.name.push_back(info_.joints[i].name);
-          joint_state.position.push_back(target_joint_commands[POSITION_INTERFACE_INDEX][i]);
+          joint_state.position.push_back(joint_commands_[POSITION_INTERFACE_INDEX][i]);
         }
       }
     }
@@ -371,7 +345,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
         if (interface.name == hardware_interface::HW_IF_VELOCITY)
         {
           joint_state.name.push_back(info_.joints[i].name);
-          joint_state.velocity.push_back(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]);
+          joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]);
         }
       }
     }
@@ -412,7 +386,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
         if (interface.name == hardware_interface::HW_IF_EFFORT)
         {
           joint_state.name.push_back(info_.joints[i].name);
-          joint_state.effort.push_back(target_joint_commands[EFFORT_INTERFACE_INDEX][i]);
+          joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]);
         }
       }
     }