Skip to content

Commit

Permalink
Make communication thread-safe
Browse files Browse the repository at this point in the history
  • Loading branch information
@fmauch
fmauch committed Oct 10, 2024
1 parent 3e758b1 commit d96b096
Show file tree
Hide file tree
Showing 2 changed files with 147 additions and 63 deletions.
20 changes: 20 additions & 0 deletions ur_controllers/include/ur_controllers/force_mode_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,8 @@
#pragma once
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <realtime_tools/realtime_buffer.h>
#include <array>
#include <memory>

#include <controller_interface/controller_interface.hpp>
Expand Down Expand Up @@ -85,6 +87,17 @@ enum StateInterfaces
INITIALIZED_FLAG = 0u,
};

struct ForceModeParameters
{
std::array<double, 6> task_frame;
std::array<double, 6> selection_vec;
std::array<double, 6> limits;
geometry_msgs::msg::WrenchStamped wrench;
double type;
double damping_factor;
double gain_scaling;
};

class ForceModeController : public controller_interface::ControllerInterface
{
public:
Expand All @@ -102,6 +115,8 @@ class ForceModeController : public controller_interface::ControllerInterface

CallbackReturn on_init() override;

CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override;

private:
bool setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req,
ur_msgs::srv::SetForceMode::Response::SharedPtr resp);
Expand All @@ -114,6 +129,11 @@ class ForceModeController : public controller_interface::ControllerInterface
std::shared_ptr<force_mode_controller::ParamListener> param_listener_;
force_mode_controller::Params params_;

realtime_tools::RealtimeBuffer<ForceModeParameters> force_mode_params_buffer_;
std::atomic<bool> force_mode_active_;
std::atomic<bool> change_requested_;
std::atomic<double> async_state_;

static constexpr double ASYNC_WAITING = 2.0;
/**
* @brief wait until a command interface isn't in state ASYNC_WAITING anymore or until the parameter maximum_retries
Expand Down
190 changes: 127 additions & 63 deletions ur_controllers/src/force_mode_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
*/
//----------------------------------------------------------------------

#include <limits>
#include <rclcpp/logging.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

Expand All @@ -42,6 +43,8 @@ namespace ur_controllers
{
controller_interface::CallbackReturn ForceModeController::on_init()
{
RCLCPP_WARN(get_node()->get_logger(), "The ForceModeController is considered a beta feature. Its interface might "
"change in the future.");
try {
// Create the parameter listener and get the parameters
param_listener_ = std::make_shared<force_mode_controller::ParamListener>(get_node());
Expand Down Expand Up @@ -107,14 +110,6 @@ controller_interface::InterfaceConfiguration ur_controllers::ForceModeController
return config;
}

controller_interface::return_type ur_controllers::ForceModeController::update(const rclcpp::Time& /*time*/,
const rclcpp::Duration& /*period*/)
{
// Publish state of force_mode?

return controller_interface::return_type::OK;
}

controller_interface::CallbackReturn
ur_controllers::ForceModeController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
{
Expand All @@ -134,19 +129,6 @@ ur_controllers::ForceModeController::on_configure(const rclcpp_lifecycle::State&
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_node()->get_clock());
tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);

return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn
ur_controllers::ForceModeController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
{
RCLCPP_WARN(get_node()->get_logger(), "The ForceModeController is considered a beta feature. Its interface might "
"change in the future.");
while (state_interfaces_[StateInterfaces::INITIALIZED_FLAG].get_value() == 0.0) {
RCLCPP_INFO(get_node()->get_logger(), "Waiting for system interface to initialize...");
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}

// Create the service server that will be used to start force mode
try {
set_force_mode_srv_ = get_node()->create_service<ur_msgs::srv::SetForceMode>(
Expand All @@ -155,6 +137,16 @@ ur_controllers::ForceModeController::on_activate(const rclcpp_lifecycle::State&
} catch (...) {
return LifecycleNodeInterface::CallbackReturn::ERROR;
}

return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn
ur_controllers::ForceModeController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
{
change_requested_ = false;
force_mode_active_ = false;
async_state_ = std::numeric_limits<double>::quiet_NaN();
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

Expand All @@ -171,82 +163,154 @@ ur_controllers::ForceModeController::on_deactivate(const rclcpp_lifecycle::State
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn
ur_controllers::ForceModeController::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/)
{
set_force_mode_srv_.reset();
return CallbackReturn::SUCCESS;
}

controller_interface::return_type ur_controllers::ForceModeController::update(const rclcpp::Time& /*time*/,
const rclcpp::Duration& /*period*/)
{
async_state_ = command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value();

// Publish state of force_mode?
if (change_requested_) {
if (force_mode_active_) {
const auto force_mode_parameters = force_mode_params_buffer_.readFromRT();
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(force_mode_parameters->task_frame[0]);
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(force_mode_parameters->task_frame[1]);
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(force_mode_parameters->task_frame[2]);
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(force_mode_parameters->task_frame[3]);
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(force_mode_parameters->task_frame[4]);
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(force_mode_parameters->task_frame[5]);

command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value(
force_mode_parameters->selection_vec[0]);
command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value(
force_mode_parameters->selection_vec[1]);
command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value(
force_mode_parameters->selection_vec[2]);
command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value(
force_mode_parameters->selection_vec[3]);
command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value(
force_mode_parameters->selection_vec[4]);
command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value(
force_mode_parameters->selection_vec[5]);

command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(
force_mode_parameters->wrench.wrench.force.x);
command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(
force_mode_parameters->wrench.wrench.force.y);
command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(
force_mode_parameters->wrench.wrench.force.z);
command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(
force_mode_parameters->wrench.wrench.torque.x);
command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(
force_mode_parameters->wrench.wrench.torque.y);
command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(
force_mode_parameters->wrench.wrench.torque.z);

command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(force_mode_parameters->limits[0]);
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(force_mode_parameters->limits[1]);
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(force_mode_parameters->limits[2]);
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(force_mode_parameters->limits[3]);
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(force_mode_parameters->limits[4]);
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(force_mode_parameters->limits[5]);

command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(force_mode_parameters->type);
command_interfaces_[CommandInterfaces::FORCE_MODE_DAMPING].set_value(force_mode_parameters->damping_factor);
command_interfaces_[CommandInterfaces::FORCE_MODE_GAIN_SCALING].set_value(force_mode_parameters->gain_scaling);

// Signal that we are waiting for confirmation that force mode is activated
command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
async_state_ = ASYNC_WAITING;
} else {
// TODO(fexner): Disable
}
change_requested_ = false;
}

return controller_interface::return_type::OK;
}

bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req,
ur_msgs::srv::SetForceMode::Response::SharedPtr resp)
{
// reset success flag
command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);

ForceModeParameters force_mode_parameters;

// transform task frame into base
const std::string tf_prefix = params_.tf_prefix;
try {
auto task_frame_transformed = tf_buffer_->transform(req->task_frame, tf_prefix + "base");
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(task_frame_transformed.pose.position.x);
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(task_frame_transformed.pose.position.y);
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(task_frame_transformed.pose.position.z);

force_mode_parameters.task_frame[0] = task_frame_transformed.pose.position.x;
force_mode_parameters.task_frame[1] = task_frame_transformed.pose.position.y;
force_mode_parameters.task_frame[2] = task_frame_transformed.pose.position.z;

tf2::Quaternion quat_tf;
tf2::convert(task_frame_transformed.pose.orientation, quat_tf);
tf2::Matrix3x3 rot_mat(quat_tf);
std::vector<double> rot(3);
rot_mat.getRPY(rot[0], rot[1], rot[2]);
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(rot[0]);
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(rot[1]);
command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(rot[2]);
rot_mat.getRPY(force_mode_parameters.task_frame[3], force_mode_parameters.task_frame[4],
force_mode_parameters.task_frame[5]);
} catch (const tf2::TransformException& ex) {
RCLCPP_ERROR(get_node()->get_logger(), "Could not transform %s to robot base: %s",
req->task_frame.header.frame_id.c_str(), ex.what());
return false;
}

/* The selection vector dictates which axes the robot should be compliant along and around */
command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value(req->selection_vector_x);
command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value(req->selection_vector_y);
command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value(req->selection_vector_z);
command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value(req->selection_vector_rx);
command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value(req->selection_vector_ry);
command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value(req->selection_vector_rz);

/* The wrench parameters dictate the amount of force/torque the robot will apply to its environment. The robot will
* move along/around compliant axes to match the specified force/torque. Has no effect for non-compliant axes. */
command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(req->wrench.wrench.force.x);
command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(req->wrench.wrench.force.y);
command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(req->wrench.wrench.force.z);
command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(req->wrench.wrench.torque.x);
command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(req->wrench.wrench.torque.y);
command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(req->wrench.wrench.torque.z);
// The selection vector dictates which axes the robot should be compliant along and around
force_mode_parameters.selection_vec[0] = req->selection_vector_x;
force_mode_parameters.selection_vec[1] = req->selection_vector_y;
force_mode_parameters.selection_vec[2] = req->selection_vector_z;
force_mode_parameters.selection_vec[3] = req->selection_vector_rx;
force_mode_parameters.selection_vec[4] = req->selection_vector_ry;
force_mode_parameters.selection_vec[5] = req->selection_vector_rz;

// The wrench parameters dictate the amount of force/torque the robot will apply to its environment. The robot will
// move along/around compliant axes to match the specified force/torque. Has no effect for non-compliant axes.
force_mode_parameters.wrench = req->wrench;

/* The limits specifies the maximum allowed speed along/around compliant axes. For non-compliant axes this value is
* the maximum allowed deviation between actual tcp position and the one that has been programmed. */
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(req->limits[0]);
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(req->limits[1]);
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(req->limits[2]);
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(req->limits[3]);
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(req->limits[4]);
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(req->limits[5]);

/* The type decides how the robot interprets the force frame (the one defined in task_frame). See ur_script manual for
* explanation, under force_mode. */
command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(unsigned(req->type));

/* The damping factor decides how fast the robot decelarates if no force is present. 0 means no deceleration, 1 means
* quick deceleration*/
command_interfaces_[CommandInterfaces::FORCE_MODE_DAMPING].set_value(req->damping_factor);
force_mode_parameters.limits[0] = req->limits[0];
force_mode_parameters.limits[1] = req->limits[1];
force_mode_parameters.limits[2] = req->limits[2];
force_mode_parameters.limits[3] = req->limits[3];
force_mode_parameters.limits[4] = req->limits[4];
force_mode_parameters.limits[5] = req->limits[5];

/* The type decides how the robot interprets the force frame (the one defined in task_frame). See ur_script manual
* for explanation, under force_mode. */
force_mode_parameters.type = static_cast<double>(req->type);

/* The damping factor decides how fast the robot decelarates if no force is present. 0 means no deceleration, 1
* means quick deceleration*/
force_mode_parameters.damping_factor = req->damping_factor;
/*The gain scaling factor scales the force mode gain. A value larger than 1 may make force mode unstable. */
command_interfaces_[CommandInterfaces::FORCE_MODE_GAIN_SCALING].set_value(req->gain_scaling);
force_mode_parameters.gain_scaling = req->gain_scaling;

force_mode_params_buffer_.writeFromNonRT(force_mode_parameters);
force_mode_active_ = true;
change_requested_ = true;

RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be set.");
const auto maximum_retries = params_.check_io_successful_retries;
int retries = 0;
while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
std::this_thread::sleep_for(std::chrono::milliseconds(50));
while (async_state_ == ASYNC_WAITING || change_requested_) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
retries++;

if (retries > maximum_retries) {
resp->success = false;
}
}

resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value());
resp->success = async_state_ == 1.0;

if (resp->success) {
RCLCPP_INFO(get_node()->get_logger(), "Force mode has been set successfully.");
Expand Down

0 comments on commit d96b096

Please sign in to comment.