Check command interface name when setting speed scaling

Felix Exner · Felix Exner · commit 00c753d14e90 · 2024-04-03T21:22:39.000+02:00
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -1658,8 +1658,13 @@ bool JointTrajectoryController::set_scaling_factor(
   {
     if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
     {
-      // TODO(Felix): Use proper adressing here.
-      command_interfaces_.back().set_value(static_cast<double>(req->scaling_factor));
+      for (auto & interface : command_interfaces_)
+      {
+        if (interface.get_name() == params_.speed_scaling_command_interface_name)
+        {
+          interface.set_value(static_cast<double>(req->scaling_factor));
+        }
+      }
     }
   }
   resp->success = true;

Original file line number	Diff line number	Diff line change
`@@ -1658,8 +1658,13 @@ bool JointTrajectoryController::set_scaling_factor(`
`1658`	`1658`	`{`
`1659`	`1659`	`if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)`
`1660`	`1660`	`{`
`1661`		`- // TODO(Felix): Use proper adressing here.`
`1662`		`- command_interfaces_.back().set_value(static_cast<double>(req->scaling_factor));`
	`1661`	`+ for (auto & interface : command_interfaces_)`
	`1662`	`+ {`
	`1663`	`+ if (interface.get_name() == params_.speed_scaling_command_interface_name)`
	`1664`	`+ {`
	`1665`	`+ interface.set_value(static_cast<double>(req->scaling_factor));`
	`1666`	`+ }`
	`1667`	`+ }`
`1663`	`1668`	`}`
`1664`	`1669`	`}`
`1665`	`1670`	`resp->success = true;`