42#include <realtime_tools/thread_priority.hpp>
50 return node_->get_node_base_interface();
56 if (servo_loop_thread_.joinable())
57 servo_loop_thread_.join();
61 : node_{ std::make_shared<
rclcpp::Node>(
"servo_node", options) }
62 , stop_servo_{ false }
63 , servo_paused_{ false }
64 , new_joint_jog_msg_{ false }
65 , new_twist_msg_{ false }
66 , new_pose_msg_{ false }
71 if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority))
73 RCLCPP_INFO_STREAM(node_->get_logger(),
"Enabled SCHED_FIFO and higher thread priority.");
77 RCLCPP_WARN_STREAM(node_->get_logger(),
"Could not enable FIFO RT scheduling policy. Continuing with the default.");
81 if (!realtime_tools::has_realtime_kernel())
83 RCLCPP_WARN_STREAM(node_->get_logger(),
"Realtime kernel is recommended for better performance.");
86 std::shared_ptr<servo::ParamListener> servo_param_listener =
87 std::make_shared<servo::ParamListener>(node_,
"moveit_servo");
91 servo_ = std::make_unique<Servo>(node_, servo_param_listener, planning_scene_monitor_);
93 servo_params_ = servo_->getParams();
96 joint_jog_subscriber_ = node_->create_subscription<control_msgs::msg::JointJog>(
97 servo_params_.joint_command_in_topic, rclcpp::SystemDefaultsQoS(),
98 [
this](
const control_msgs::msg::JointJog::ConstSharedPtr& msg) {
return jointJogCallback(msg); });
101 twist_subscriber_ = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
102 servo_params_.cartesian_command_in_topic, rclcpp::SystemDefaultsQoS(),
103 [
this](
const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) {
return twistCallback(msg); });
106 pose_subscriber_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
107 servo_params_.pose_command_in_topic, rclcpp::SystemDefaultsQoS(),
108 [
this](
const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) {
return poseCallback(msg); });
110 if (servo_params_.command_out_type ==
"trajectory_msgs/JointTrajectory")
112 trajectory_publisher_ = node_->create_publisher<trajectory_msgs::msg::JointTrajectory>(
113 servo_params_.command_out_topic, rclcpp::SystemDefaultsQoS());
115 else if (servo_params_.command_out_type ==
"std_msgs/Float64MultiArray")
117 multi_array_publisher_ = node_->create_publisher<std_msgs::msg::Float64MultiArray>(servo_params_.command_out_topic,
118 rclcpp::SystemDefaultsQoS());
122 node_->create_publisher<moveit_msgs::msg::ServoStatus>(servo_params_.status_topic, rclcpp::SystemDefaultsQoS());
125 switch_command_type_ = node_->create_service<moveit_msgs::srv::ServoCommandType>(
126 "~/switch_command_type", [
this](
const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Request>& request,
127 const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Response>& response) {
128 return switchCommandType(request, response);
132 pause_servo_ = node_->create_service<std_srvs::srv::SetBool>(
133 "~/pause_servo", [
this](
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
134 const std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
135 return pauseServo(request, response);
139 servo_loop_thread_ = std::thread(&ServoNode::servoLoop,
this);
142void ServoNode::pauseServo(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
143 const std::shared_ptr<std_srvs::srv::SetBool::Response>& response)
145 servo_paused_ = request->data;
146 response->success = (servo_paused_ == request->data);
149 servo_->setCollisionChecking(
false);
150 response->message =
"Servoing disabled";
155 last_commanded_state_ = servo_->getCurrentRobotState(
true );
156 servo_->resetSmoothing(last_commanded_state_);
159 joint_cmd_rolling_window_.clear();
162 servo_->setCollisionChecking(
true);
163 response->message =
"Servoing enabled";
167void ServoNode::switchCommandType(
const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Request>& request,
168 const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Response>& response)
170 const bool is_valid = (request->command_type >=
static_cast<int8_t
>(
CommandType::MIN)) &&
174 servo_->setCommandType(
static_cast<CommandType>(request->command_type));
178 RCLCPP_WARN_STREAM(node_->get_logger(),
"Unknown command type " << request->command_type <<
" requested");
180 response->success = (request->command_type ==
static_cast<int8_t
>(servo_->getCommandType()));
183void ServoNode::jointJogCallback(
const control_msgs::msg::JointJog::ConstSharedPtr& msg)
185 latest_joint_jog_ = *msg;
186 new_joint_jog_msg_ =
true;
189void ServoNode::twistCallback(
const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg)
191 latest_twist_ = *msg;
192 new_twist_msg_ =
true;
195void ServoNode::poseCallback(
const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg)
198 new_pose_msg_ =
true;
201std::optional<KinematicState> ServoNode::processJointJogCommand(
const moveit::core::RobotStatePtr& robot_state)
203 std::optional<KinematicState> next_joint_state = std::nullopt;
205 new_twist_msg_ = new_pose_msg_ =
false;
207 const bool command_stale = (node_->now() - latest_joint_jog_.header.stamp) >=
208 rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout);
211 JointJogCommand command{ latest_joint_jog_.joint_names, latest_joint_jog_.velocities };
212 next_joint_state = servo_->getNextJointState(robot_state, command);
216 auto result = servo_->smoothHalt(last_commanded_state_);
217 new_joint_jog_msg_ = !result.first;
218 if (new_joint_jog_msg_)
220 next_joint_state = result.second;
221 RCLCPP_DEBUG_STREAM(node_->get_logger(),
"Joint jog command timed out. Halting to a stop.");
225 return next_joint_state;
228std::optional<KinematicState> ServoNode::processTwistCommand(
const moveit::core::RobotStatePtr& robot_state)
230 std::optional<KinematicState> next_joint_state = std::nullopt;
234 new_joint_jog_msg_ = new_pose_msg_ =
false;
236 const bool command_stale = (node_->now() - latest_twist_.header.stamp) >=
237 rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout);
240 const Eigen::Vector<double, 6> velocities{ latest_twist_.twist.linear.x, latest_twist_.twist.linear.y,
241 latest_twist_.twist.linear.z, latest_twist_.twist.angular.x,
242 latest_twist_.twist.angular.y, latest_twist_.twist.angular.z };
243 const TwistCommand command{ latest_twist_.header.frame_id, velocities };
244 next_joint_state = servo_->getNextJointState(robot_state, command);
248 auto result = servo_->smoothHalt(last_commanded_state_);
249 new_twist_msg_ = !result.first;
252 next_joint_state = result.second;
253 RCLCPP_DEBUG_STREAM(node_->get_logger(),
"Twist command timed out. Halting to a stop.");
257 return next_joint_state;
260std::optional<KinematicState> ServoNode::processPoseCommand(
const moveit::core::RobotStatePtr& robot_state)
262 std::optional<KinematicState> next_joint_state = std::nullopt;
266 new_joint_jog_msg_ = new_twist_msg_ =
false;
268 const bool command_stale = (node_->now() - latest_pose_.header.stamp) >=
269 rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout);
273 next_joint_state = servo_->getNextJointState(robot_state, command);
277 auto result = servo_->smoothHalt(last_commanded_state_);
278 new_pose_msg_ = !result.first;
281 next_joint_state = result.second;
282 RCLCPP_DEBUG_STREAM(node_->get_logger(),
"Pose command timed out. Halting to a stop.");
286 return next_joint_state;
289void ServoNode::servoLoop()
291 moveit_msgs::msg::ServoStatus status_msg;
292 std::optional<KinematicState> next_joint_state = std::nullopt;
293 rclcpp::WallRate servo_frequency(1 / servo_params_.publish_period);
296 const auto servo_node_start = node_->now();
297 while (planning_scene_monitor_->getLastUpdateTime().get_clock_type() != node_->get_clock()->get_clock_type() ||
298 servo_node_start > planning_scene_monitor_->getLastUpdateTime())
300 RCLCPP_INFO(node_->get_logger(),
"Waiting to receive robot state update.");
301 rclcpp::sleep_for(std::chrono::seconds(1));
303 KinematicState current_state = servo_->getCurrentRobotState(
true );
304 last_commanded_state_ = current_state;
306 servo_->resetSmoothing(current_state);
309 moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
311 robot_state->getJointModelGroup(servo_params_.move_group_name);
313 while (rclcpp::ok() && !stop_servo_)
318 servo_->resetSmoothing(current_state);
319 servo_frequency.sleep();
323 const bool use_trajectory = servo_params_.command_out_type ==
"trajectory_msgs/JointTrajectory";
324 const auto cur_time = node_->now();
326 if (use_trajectory && !joint_cmd_rolling_window_.empty() && joint_cmd_rolling_window_.back().time_stamp > cur_time)
328 current_state = joint_cmd_rolling_window_.back();
333 joint_cmd_rolling_window_.clear();
334 current_state = servo_->getCurrentRobotState(
false );
335 current_state.velocities *= 0.0;
339 robot_state->setJointGroupPositions(joint_model_group, current_state.positions);
340 robot_state->setJointGroupVelocities(joint_model_group, current_state.velocities);
342 next_joint_state = std::nullopt;
343 const CommandType expected_type = servo_->getCommandType();
347 next_joint_state = processJointJogCommand(robot_state);
351 next_joint_state = processTwistCommand(robot_state);
355 next_joint_state = processPoseCommand(robot_state);
357 else if (new_joint_jog_msg_ || new_twist_msg_ || new_pose_msg_)
359 new_joint_jog_msg_ = new_twist_msg_ = new_pose_msg_ =
false;
360 RCLCPP_WARN_STREAM(node_->get_logger(),
"Command type has not been set, cannot accept input");
368 auto& next_joint_state_value = next_joint_state.value();
369 updateSlidingWindow(next_joint_state_value, joint_cmd_rolling_window_, servo_params_.max_expected_latency,
373 trajectory_publisher_->publish(msg.value());
380 last_commanded_state_ = next_joint_state.value();
385 updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time);
388 status_msg.code =
static_cast<int8_t
>(servo_->getStatus());
389 status_msg.message = servo_->getStatusMessage();
390 status_publisher_->publish(status_msg);
392 servo_frequency.sleep();
398#include "rclcpp_components/register_node_macro.hpp"
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface()
ServoNode(const rclcpp::NodeOptions &options)
std::optional< trajectory_msgs::msg::JointTrajectory > composeTrajectoryMessage(const servo::Params &servo_params, const std::deque< KinematicState > &joint_cmd_rolling_window)
Create a trajectory message from a rolling window queue of joint state commands. Method optionally re...
std_msgs::msg::Float64MultiArray composeMultiArrayMessage(const servo::Params &servo_params, const KinematicState &joint_state)
Create a Float64MultiArray message from given joint state.
void updateSlidingWindow(KinematicState &next_joint_state, std::deque< KinematicState > &joint_cmd_rolling_window, double max_expected_latency, const rclcpp::Time &cur_time)
Adds a new joint state command to a queue containing commands over a time window. Also modifies the v...
PoseCommand poseFromPoseStamped(const geometry_msgs::msg::PoseStamped &msg)
Convert a PoseStamped message to a Servo Pose.
planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const rclcpp::Node::SharedPtr &node, const servo::Params &servo_params)
Creates the planning scene monitor used by servo.
void setNodeLoggerName(const std::string &name)
Call once after creating a node to initialize logging namespaces.