Improve scaling exchange with hardware

Author: Felix Exner

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -116,12 +116,17 @@ JointTrajectoryController::command_interface_configuration() const
       conf.names.push_back(joint_name + "/" + interface_type);
     }
   }
+  if (!params_.speed_scaling_command_interface_name.empty())
+  {
+    conf.names.push_back(params_.speed_scaling_command_interface_name);
+  }
   return conf;
 }
 
 controller_interface::InterfaceConfiguration
 JointTrajectoryController::state_interface_configuration() const
 {
+  const auto logger = get_node()->get_logger();
   controller_interface::InterfaceConfiguration conf;
   conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
   conf.names.reserve(dof_ * params_.state_interfaces.size());
@@ -132,33 +137,36 @@ JointTrajectoryController::state_interface_configuration() const
       conf.names.push_back(joint_name + "/" + interface_type);
     }
   }
-  if (params_.exchange_scaling_factor_with_hardware)
+  if (!params_.speed_scaling_state_interface_name.empty())
   {
-    conf.names.push_back(params_.speed_scaling_interface_name);
+    RCLCPP_INFO(
+      logger, "Using scaling state from the hardware from interface %s.",
+      params_.speed_scaling_state_interface_name.c_str());
+    conf.names.push_back(params_.speed_scaling_state_interface_name);
   }
   return conf;
 }
 
 controller_interface::return_type JointTrajectoryController::update(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  if (params_.exchange_scaling_factor_with_hardware)
+  if (params_.speed_scaling_state_interface_name.empty())
+  {
+    scaling_factor_ = *(scaling_factor_rt_buff_.readFromRT());
+  }
+  else
   {
-    if (state_interfaces_.back().get_name() == params_.speed_scaling_interface_name)
+    if (state_interfaces_.back().get_name() == params_.speed_scaling_state_interface_name)
     {
       scaling_factor_ = state_interfaces_.back().get_value();
     }
     else
     {
       RCLCPP_ERROR(
         get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.",
-        params_.speed_scaling_interface_name.c_str());
+        params_.speed_scaling_state_interface_name.c_str());
     }
   }
-  else
-  {
-    scaling_factor_ = *(scaling_factor_rt_buff_.readFromRT());
-  }
 
   if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
@@ -494,7 +502,8 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto
   auto interface_has_values = [](const auto & joint_interface)
   {
     return std::find_if(
-             joint_interface.begin(), joint_interface.end(), [](const auto & interface)
+             joint_interface.begin(), joint_interface.end(),
+             [](const auto & interface)
              { return std::isnan(interface.get().get_value()); }) == joint_interface.end();
   };
 
@@ -564,7 +573,8 @@ bool JointTrajectoryController::read_commands_from_command_interfaces(
   auto interface_has_values = [](const auto & joint_interface)
   {
     return std::find_if(
-             joint_interface.begin(), joint_interface.end(), [](const auto & interface)
+             joint_interface.begin(), joint_interface.end(),
+             [](const auto & interface)
              { return std::isnan(interface.get().get_value()); }) == joint_interface.end();
   };
 
@@ -908,6 +918,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
                               std::placeholders::_1, std::placeholders::_2));
 
   // set scaling factor to low value default
+  RCLCPP_INFO(
+    logger, "Setting initial scaling factor to %2f", params_.scaling_factor_initial_default);
   scaling_factor_rt_buff_.writeFromNonRT(params_.scaling_factor_initial_default);
 
   return CallbackReturn::SUCCESS;
@@ -1624,7 +1636,26 @@ bool JointTrajectoryController::set_scaling_factor(
   }
 
   RCLCPP_INFO(get_node()->get_logger(), "New scaling factor will be %f", req->scaling_factor);
-  scaling_factor_rt_buff_.writeFromNonRT(req->scaling_factor);
+  if (params_.speed_scaling_command_interface_name.empty())
+  {
+    if (!params_.speed_scaling_state_interface_name.empty())
+    {
+      RCLCPP_WARN(
+        get_node()->get_logger(),
+        "Setting the scaling factor while only one-way communication with the hardware is setup. "
+        "This will likely get overwritten by the hardware again. If available, please also setup "
+        "the speed_scaling_command_interface_name");
+    }
+    scaling_factor_rt_buff_.writeFromNonRT(req->scaling_factor);
+  }
+  else
+  {
+    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));
+    }
+  }
   resp->success = true;
   return true;
 }

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -39,21 +39,22 @@ joint_trajectory_controller:
       "joint_trajectory_controller::state_interface_type_combinations": null,
     }
   }
-  exchange_scaling_factor_with_hardware: {
-    type: bool,
-    default_value: false,
-    description: "Flag that mark is hardware supports setting and reading of scaling factor.\n
-      If set to 'true' corresponding command and state interfaces are created."
-  }
   scaling_factor_initial_default: {
     type: double,
     default_value: 1.0,
     description: "The initial value of the scaling factor if not exchange with hardware takes place."
   }
-  speed_scaling_interface_name: {
+  speed_scaling_state_interface_name: {
     type: string,
-    default_value: "speed_scaling/speed_scaling_factor",
-    description: "Fully qualified name of the speed scaling interface name"
+    default_value: "",
+    read_only: true,
+    description: "Fully qualified name of the speed scaling state interface name"
+  }
+  speed_scaling_command_interface_name: {
+    type: string,
+    default_value: "",
+    read_only: true,
+    description: "Command interface name used for setting the speed scaling factor on the hardware."
   }
   allow_partial_joints_goal: {
     type: bool,