Skip to content

Commit

Permalink
[SJTC] Make scaling interface optional
Browse files Browse the repository at this point in the history
This way, the controller can be used on systems, where no scaling
interface is available (e.g. GZ). The upstream version in ros2_controllers
will have the same behavior.
  • Loading branch information
fmauch committed Oct 12, 2024
1 parent 912f9ac commit 8a1dd35
Show file tree
Hide file tree
Showing 2 changed files with 29 additions and 7 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,8 @@
#ifndef UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
#define UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_

#include <optional>
#include <memory>
#include "angles/angles.h"
#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
#include "joint_trajectory_controller/trajectory.hpp"
Expand Down Expand Up @@ -73,9 +75,12 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join
};

private:
double scaling_factor_{};
double scaling_factor_{ 1.0 };
realtime_tools::RealtimeBuffer<TimeData> time_data_;

std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> scaling_state_interface_ =
std::nullopt;

std::shared_ptr<scaled_joint_trajectory_controller::ParamListener> scaled_param_listener_;
scaled_joint_trajectory_controller::Params scaled_params_;
};
Expand Down
29 changes: 23 additions & 6 deletions ur_controllers/src/scaled_joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,14 @@ controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::st
{
controller_interface::InterfaceConfiguration conf;
conf = JointTrajectoryController::state_interface_configuration();
conf.names.push_back(scaled_params_.speed_scaling_interface_name);

if (!scaled_params_.speed_scaling_interface_name.empty()) {
RCLCPP_INFO(get_node()->get_logger(), "Using scaling state from the hardware from interface %s.",
scaled_params_.speed_scaling_interface_name.c_str());
conf.names.push_back(scaled_params_.speed_scaling_interface_name);
} else {
RCLCPP_INFO(get_node()->get_logger(), "No scaling interface set. This controller will not use speed scaling.");
}

return conf;
}
Expand All @@ -70,17 +77,27 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat
time_data.period = rclcpp::Duration::from_nanoseconds(0);
time_data.uptime = get_node()->now();
time_data_.initRT(time_data);

// Set scaling interfaces
if (!scaled_params_.speed_scaling_interface_name.empty()) {
auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), [&](auto& interface) {
return (interface.get_name() == scaled_params_.speed_scaling_interface_name);
});
if (it != state_interfaces_.end()) {
scaling_state_interface_ = *it;
} else {
RCLCPP_ERROR(get_node()->get_logger(), "Did not find speed scaling interface in state interfaces.");
}
}

return JointTrajectoryController::on_activate(state);
}

controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time,
const rclcpp::Duration& period)
{
if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_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.",
scaled_params_.speed_scaling_interface_name.c_str());
if (scaling_state_interface_.has_value()) {
scaling_factor_ = scaling_state_interface_->get().get_value();
}

if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
Expand Down

0 comments on commit 8a1dd35

Please sign in to comment.