Skip to content

Commit

Permalink
Add support for changing smoothing plugin and planning group
Browse files Browse the repository at this point in the history
  • Loading branch information
@ibrahiminfinite
ibrahiminfinite committed Sep 4, 2023
1 parent 50f8bad commit 405ccf3
Show file tree
Hide file tree
Showing 3 changed files with 92 additions and 71 deletions.
2 changes: 1 addition & 1 deletion moveit_ros/moveit_servo/include/moveit_servo/servo.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ class Servo
/**
* \brief Create and initialize the smoothing plugin to be used by servo.
*/
void setSmoothingPlugin();
void setSmoothingPlugin(const servo::Params& servo_params);

/**
* \brief Apply halting logic to specified joints.
Expand Down
101 changes: 60 additions & 41 deletions moveit_ros/moveit_servo/src/servo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ namespace
const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo");
constexpr double ROBOT_STATE_WAIT_TIME = 5.0; // seconds
constexpr double STOPPED_VELOCITY_EPS = 1e-4;
constexpr auto ROS_LOG_THROTTLE_PERIOD = std::chrono::milliseconds(3000).count();
} // namespace

namespace moveit_servo
Expand Down Expand Up @@ -90,7 +91,7 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::P
}

// Load the smoothing plugin
setSmoothingPlugin();
setSmoothingPlugin(servo_params_);

// Create the collision checker and start collision checking.
collision_monitor_ =
Expand All @@ -106,30 +107,29 @@ Servo::~Servo()
setCollisionChecking(false);
}

void Servo::setSmoothingPlugin()
void Servo::setSmoothingPlugin(const servo::Params& servo_params)
{
// Load the smoothing plugin
RCLCPP_INFO_STREAM(LOGGER, "Loading smoothing plugin: " << servo_params.smoothing_filter_plugin_name);
try
{
pluginlib::ClassLoader<online_signal_smoothing::SmoothingBaseClass> smoothing_loader(
"moveit_core", "online_signal_smoothing::SmoothingBaseClass");
smoother_ = smoothing_loader.createUniqueInstance(servo_params_.smoothing_filter_plugin_name);
smoother_ = smoothing_loader.createUniqueInstance(servo_params.smoothing_filter_plugin_name);
}
catch (pluginlib::PluginlibException& ex)
{
RCLCPP_ERROR(LOGGER, "Exception while loading the smoothing plugin '%s': '%s'",
servo_params_.smoothing_filter_plugin_name.c_str(), ex.what());
std::exit(EXIT_FAILURE);
servo_params.smoothing_filter_plugin_name.c_str(), ex.what());
}

// Initialize the smoothing plugin
moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
const int num_joints =
robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size();
robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size();
if (!smoother_->initialize(node_, planning_scene_monitor_->getRobotModel(), num_joints))
{
RCLCPP_ERROR(LOGGER, "Smoothing plugin could not be initialized");
std::exit(EXIT_FAILURE);
}
}

Expand All @@ -141,47 +141,61 @@ void Servo::setCollisionChecking(const bool check_collision)
bool Servo::validateParams(const servo::Params& servo_params)
{
bool params_valid = true;
auto clock = *node_->get_clock();

auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name);
if (joint_model_group == nullptr)
if (getStatus() == StatusCode::PAUSED)
{
RCLCPP_ERROR_STREAM(LOGGER, "Invalid move group name: `" << servo_params.move_group_name << '`');
params_valid = false;
if (servo_params.move_group_name != servo_params_.move_group_name)
{
moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
const moveit::core::JointModelGroup* joint_model_group =
robot_state->getJointModelGroup(servo_params.move_group_name);

if (joint_model_group == nullptr)
{
params_valid = false;
RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD,
"Move group `" << servo_params.move_group_name << "` not found.");
}
}
}

if (servo_params.hard_stop_singularity_threshold <= servo_params.lower_singularity_threshold)
{
RCLCPP_ERROR(LOGGER, "Parameter 'hard_stop_singularity_threshold' "
"should be greater than 'lower_singularity_threshold.' "
"Check the parameters YAML file used to launch this node.");
RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD,
"Parameter 'hard_stop_singularity_threshold' "
"should be greater than 'lower_singularity_threshold.' "
"Check the parameters YAML file used to launch this node.");
params_valid = false;
}

if (!servo_params.publish_joint_positions && !servo_params.publish_joint_velocities &&
!servo_params.publish_joint_accelerations)
{
RCLCPP_ERROR(LOGGER, "At least one of publish_joint_positions / "
"publish_joint_velocities / "
"publish_joint_accelerations must be true. "
"Check the parameters YAML file used to launch this node.");
RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD,
"At least one of publish_joint_positions / "
"publish_joint_velocities / "
"publish_joint_accelerations must be true. "
"Check the parameters YAML file used to launch this node.");
params_valid = false;
}

if ((servo_params.command_out_type == "std_msgs/Float64MultiArray") && servo_params.publish_joint_positions &&
servo_params.publish_joint_velocities)
{
RCLCPP_ERROR(LOGGER, "When publishing a std_msgs/Float64MultiArray, "
"you must select positions OR velocities."
"Check the parameters YAML file used to launch this node.");
RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD,
"When publishing a std_msgs/Float64MultiArray, "
"you must select positions OR velocities."
"Check the parameters YAML file used to launch this node.");
params_valid = false;
}

if (servo_params.scene_collision_proximity_threshold < servo_params.self_collision_proximity_threshold)
{
RCLCPP_ERROR(LOGGER, "Parameter 'self_collision_proximity_threshold' should probably be less "
"than or equal to 'scene_collision_proximity_threshold'."
"Check the parameters YAML file used to launch this node.");
RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD,
"Parameter 'self_collision_proximity_threshold' should probably be less "
"than or equal to 'scene_collision_proximity_threshold'."
"Check the parameters YAML file used to launch this node.");
params_valid = false;
}

Expand All @@ -190,35 +204,40 @@ bool Servo::validateParams(const servo::Params& servo_params)

bool Servo::updateParams()
{
bool params_updated = false;
bool params_valid = true;

if (servo_param_listener_->is_old(servo_params_))
{
auto params = servo_param_listener_->get_params();

const bool params_valid = validateParams(params);
if (params_valid)
params_valid = validateParams(params);

const bool move_group_updated = (params.move_group_name != servo_params_.move_group_name);
const bool smoothing_plugin_updated =
(params.smoothing_filter_plugin_name != servo_params_.smoothing_filter_plugin_name);

if ((move_group_updated || smoothing_plugin_updated) && getStatus() != StatusCode::PAUSED)
{
if (params.override_velocity_scaling_factor != servo_params_.override_velocity_scaling_factor)
{
RCLCPP_INFO_STREAM(LOGGER, "override_velocity_scaling_factor changed to : "
<< std::to_string(params.override_velocity_scaling_factor));
}
if (params.move_group_name != servo_params_.move_group_name)
{
RCLCPP_INFO_STREAM(LOGGER, "Move group changed from " << servo_params_.move_group_name << " to "
<< params.move_group_name);
}
params_valid = false;
RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD,
"Updating move group or smoothing plugin without pausing Servo is not allowed");
}
else if ((move_group_updated || smoothing_plugin_updated) && getStatus() == StatusCode::PAUSED && params_valid)
{
setSmoothingPlugin(params);
}

if (params_valid)
{
servo_params_ = params;
params_updated = true;
}
else
{
RCLCPP_WARN_STREAM(LOGGER, "Parameters will not be updated.");
RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD,
"Parameters will not be updated.");
}
}
return params_updated;
return params_valid;
}

servo::Params& Servo::getParams()
Expand Down
60 changes: 31 additions & 29 deletions moveit_ros/moveit_servo/src/servo_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -156,11 +156,13 @@ void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request
response->success = (servo_paused_ == request->data);
if (servo_paused_)
{
servo_->setStatus(StatusCode::PAUSED);
servo_->setCollisionChecking(false);
response->message = "Servoing disabled";
}
else
{
servo_->setStatus(StatusCode::NO_WARNING);
servo_->setCollisionChecking(true);
response->message = "Servoing enabled";
}
Expand Down Expand Up @@ -300,41 +302,41 @@ void ServoNode::servoLoop()
servo_->updateParams();

// Skip processing commands if servoing is disabled.
if (servo_paused_)
continue;

next_joint_state = std::nullopt;
const CommandType expected_type = servo_->getCommandType();

if (expected_type == CommandType::JOINT_JOG && new_joint_jog_msg_)
if (!servo_paused_)
{
next_joint_state = processJointJogCommand();
}
else if (expected_type == CommandType::TWIST && new_twist_msg_)
{
next_joint_state = processTwistCommand();
}
else if (expected_type == CommandType::POSE && new_pose_msg_)
{
next_joint_state = processPoseCommand();
}
else if (new_joint_jog_msg_ || new_twist_msg_ || new_pose_msg_)
{
new_joint_jog_msg_ = new_twist_msg_ = new_pose_msg_ = false;
RCLCPP_WARN_STREAM(LOGGER, "Command type has not been set, cannot accept input");
}
next_joint_state = std::nullopt;
const CommandType expected_type = servo_->getCommandType();

if (next_joint_state && (servo_->getStatus() != StatusCode::INVALID))
{
if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory")
if (expected_type == CommandType::JOINT_JOG && new_joint_jog_msg_)
{
next_joint_state = processJointJogCommand();
}
else if (expected_type == CommandType::TWIST && new_twist_msg_)
{
next_joint_state = processTwistCommand();
}
else if (expected_type == CommandType::POSE && new_pose_msg_)
{
next_joint_state = processPoseCommand();
}
else if (new_joint_jog_msg_ || new_twist_msg_ || new_pose_msg_)
{
trajectory_publisher_->publish(composeTrajectoryMessage(servo_->getParams(), next_joint_state.value()));
new_joint_jog_msg_ = new_twist_msg_ = new_pose_msg_ = false;
RCLCPP_WARN_STREAM(LOGGER, "Command type has not been set, cannot accept input");
}
else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray")

if (next_joint_state && (servo_->getStatus() != StatusCode::INVALID))
{
multi_array_publisher_->publish(composeMultiArrayMessage(servo_->getParams(), next_joint_state.value()));
if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory")
{
trajectory_publisher_->publish(composeTrajectoryMessage(servo_->getParams(), next_joint_state.value()));
}
else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray")
{
multi_array_publisher_->publish(composeMultiArrayMessage(servo_->getParams(), next_joint_state.value()));
}
last_commanded_state_ = next_joint_state.value();
}
last_commanded_state_ = next_joint_state.value();
}

status_msg.code = static_cast<int8_t>(servo_->getStatus());
Expand Down

0 comments on commit 405ccf3

Please sign in to comment.