Fixed velocity controller (#450)

Signed-off-by: Alejandro Hernández Cordero <[email protected]>
ros-controls · Jan 7, 2025 · f9b4e7f · f9b4e7f
1 parent 6df12df
commit f9b4e7f
Showing 1 changed file with 3 additions and 13 deletions.
diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp
@@ -649,19 +649,9 @@ hardware_interface::return_type GazeboSimSystem::write(
     }
 
     if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) {
-      if (!this->dataPtr->ecm->Component<sim::components::JointVelocityCmd>(
-          this->dataPtr->joints_[i].sim_joint))
-      {
-        this->dataPtr->ecm->CreateComponent(
-          this->dataPtr->joints_[i].sim_joint,
-          sim::components::JointVelocityCmd({0}));
-      } else {
-        const auto jointVelCmd =
-          this->dataPtr->ecm->Component<sim::components::JointVelocityCmd>(
-          this->dataPtr->joints_[i].sim_joint);
-        *jointVelCmd = sim::components::JointVelocityCmd(
-          {this->dataPtr->joints_[i].joint_velocity_cmd});
-      }
+      this->dataPtr->ecm->SetComponentData<sim::components::JointVelocityCmd>(
+        this->dataPtr->joints_[i].sim_joint,
+        {this->dataPtr->joints_[i].joint_velocity_cmd});
     } else if (this->dataPtr->joints_[i].joint_control_method & POSITION) {
       // Get error in position
       double error;