diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index 36c6ee86ec..2c59b68a8d 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -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. diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index a5e84109ce..62a5baf08c 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -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 @@ -90,7 +91,7 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr 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); } } @@ -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; } @@ -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() diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index fefe6fff19..7e29c3ecae 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -156,11 +156,13 @@ void ServoNode::pauseServo(const std::shared_ptrsuccess = (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"; } @@ -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(servo_->getStatus());