From ea8557df96a68ad1ac8957ff926481e7ed58e486 Mon Sep 17 00:00:00 2001 From: KazuyaOguma Date: Mon, 20 Jan 2025 22:54:14 +0900 Subject: [PATCH 1/8] Update robot state if time since last command exceeds timeout --- moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp | 1 + moveit_ros/moveit_servo/src/servo_node.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp index b825c0664b..67edddcf8d 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp @@ -111,6 +111,7 @@ class ServoNode planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; KinematicState last_commanded_state_; // Used when commands go stale; + rclcpp::Time last_commanded_time_; control_msgs::msg::JointJog latest_joint_jog_; geometry_msgs::msg::TwistStamped latest_twist_; geometry_msgs::msg::PoseStamped latest_pose_; diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 06a2d716d3..ebdad486a9 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -330,6 +330,11 @@ void ServoNode::servoLoop() const bool use_trajectory = servo_params_.command_out_type == "trajectory_msgs/JointTrajectory"; const auto cur_time = node_->now(); + if(cur_time.get_clock_type() == last_commanded_time_.get_clock_type() && + cur_time - last_commanded_time_ > rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout)){ + current_state = servo_->getCurrentRobotState(true /* block for current robot state */); + } + if (use_trajectory && !joint_cmd_rolling_window_.empty() && joint_cmd_rolling_window_.back().time_stamp > cur_time) { current_state = joint_cmd_rolling_window_.back(); @@ -385,6 +390,7 @@ void ServoNode::servoLoop() multi_array_publisher_->publish(composeMultiArrayMessage(servo_->getParams(), next_joint_state.value())); } last_commanded_state_ = next_joint_state.value(); + last_commanded_time_ = node_->now(); } else { From 7b3fe4144d43d4789a23edf4ccfc013a7fd49159 Mon Sep 17 00:00:00 2001 From: KazuyaOguma Date: Fri, 31 Jan 2025 23:38:12 +0900 Subject: [PATCH 2/8] update last_commanded_state_, when the command is staled --- .../moveit_servo/include/moveit_servo/servo_node.hpp | 1 - moveit_ros/moveit_servo/src/servo_node.cpp | 9 +++------ 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp index 67edddcf8d..b825c0664b 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp @@ -111,7 +111,6 @@ class ServoNode planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; KinematicState last_commanded_state_; // Used when commands go stale; - rclcpp::Time last_commanded_time_; control_msgs::msg::JointJog latest_joint_jog_; geometry_msgs::msg::TwistStamped latest_twist_; geometry_msgs::msg::PoseStamped latest_pose_; diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 6534a44668..4799263fcf 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -226,6 +226,7 @@ std::optional ServoNode::processJointJogCommand(const moveit::co next_joint_state = result.second; RCLCPP_DEBUG_STREAM(node_->get_logger(), "Joint jog command timed out. Halting to a stop."); } + last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */); } return next_joint_state; @@ -258,6 +259,7 @@ std::optional ServoNode::processTwistCommand(const moveit::core: next_joint_state = result.second; RCLCPP_DEBUG_STREAM(node_->get_logger(), "Twist command timed out. Halting to a stop."); } + last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */); } return next_joint_state; @@ -287,6 +289,7 @@ std::optional ServoNode::processPoseCommand(const moveit::core:: next_joint_state = result.second; RCLCPP_DEBUG_STREAM(node_->get_logger(), "Pose command timed out. Halting to a stop."); } + last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */); } return next_joint_state; @@ -330,11 +333,6 @@ void ServoNode::servoLoop() const bool use_trajectory = servo_params_.command_out_type == "trajectory_msgs/JointTrajectory"; const auto cur_time = node_->now(); - if(cur_time.get_clock_type() == last_commanded_time_.get_clock_type() && - cur_time - last_commanded_time_ > rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout)){ - current_state = servo_->getCurrentRobotState(true /* block for current robot state */); - } - if (use_trajectory && !joint_cmd_rolling_window_.empty() && joint_cmd_rolling_window_.back().time_stamp > cur_time) { current_state = joint_cmd_rolling_window_.back(); @@ -390,7 +388,6 @@ void ServoNode::servoLoop() multi_array_publisher_->publish(composeMultiArrayMessage(servo_->getParams(), next_joint_state.value())); } last_commanded_state_ = next_joint_state.value(); - last_commanded_time_ = node_->now(); } else { From 7a9736003af69c4e646561cd697c055c0372f67f Mon Sep 17 00:00:00 2001 From: KazuyaOguma Date: Fri, 31 Jan 2025 23:44:19 +0900 Subject: [PATCH 3/8] resolve conflicts --- moveit_ros/moveit_servo/src/servo_node.cpp | 125 +++++++++++---------- 1 file changed, 67 insertions(+), 58 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 4799263fcf..6f961589e4 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -147,6 +147,14 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) void ServoNode::pauseServo(const std::shared_ptr& request, const std::shared_ptr& response) { + if (servo_paused_ == request->data) + { + std::string message = "Requested pause state is already active."; + RCLCPP_INFO(node_->get_logger(), "%s", message.c_str()); + response->success = true; + response->message = message; + return; + } servo_paused_ = request->data; response->success = (servo_paused_ == request->data); if (servo_paused_) @@ -329,77 +337,78 @@ void ServoNode::servoLoop() continue; } - std::lock_guard lock_guard(lock_); - const bool use_trajectory = servo_params_.command_out_type == "trajectory_msgs/JointTrajectory"; - const auto cur_time = node_->now(); + { // scope for mutex-protected operations + std::lock_guard lock_guard(lock_); + const bool use_trajectory = servo_params_.command_out_type == "trajectory_msgs/JointTrajectory"; + const auto cur_time = node_->now(); - if (use_trajectory && !joint_cmd_rolling_window_.empty() && joint_cmd_rolling_window_.back().time_stamp > cur_time) - { - current_state = joint_cmd_rolling_window_.back(); - } - else - { - // if all joint_cmd_rolling_window_ is empty or all commands in it are outdated, use current robot state - joint_cmd_rolling_window_.clear(); - current_state = servo_->getCurrentRobotState(false /* block for current robot state */); - current_state.velocities *= 0.0; - } + if (use_trajectory && !joint_cmd_rolling_window_.empty() && joint_cmd_rolling_window_.back().time_stamp > cur_time) + { + current_state = joint_cmd_rolling_window_.back(); + } + else + { + // if all joint_cmd_rolling_window_ is empty or all commands in it are outdated, use current robot state + joint_cmd_rolling_window_.clear(); + current_state = servo_->getCurrentRobotState(false /* block for current robot state */); + current_state.velocities *= 0.0; + } - // update robot state values - robot_state->setJointGroupPositions(joint_model_group, current_state.positions); - robot_state->setJointGroupVelocities(joint_model_group, current_state.velocities); + // update robot state values + robot_state->setJointGroupPositions(joint_model_group, current_state.positions); + robot_state->setJointGroupVelocities(joint_model_group, current_state.velocities); - next_joint_state = std::nullopt; - const CommandType expected_type = servo_->getCommandType(); + next_joint_state = std::nullopt; + const CommandType expected_type = servo_->getCommandType(); - if (expected_type == CommandType::JOINT_JOG && new_joint_jog_msg_) - { - next_joint_state = processJointJogCommand(robot_state); - } - else if (expected_type == CommandType::TWIST && new_twist_msg_) - { - next_joint_state = processTwistCommand(robot_state); - } - else if (expected_type == CommandType::POSE && new_pose_msg_) - { - next_joint_state = processPoseCommand(robot_state); - } - 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(node_->get_logger(), "Command type has not been set, cannot accept input"); - } + if (expected_type == CommandType::JOINT_JOG && new_joint_jog_msg_) + { + next_joint_state = processJointJogCommand(robot_state); + } + else if (expected_type == CommandType::TWIST && new_twist_msg_) + { + next_joint_state = processTwistCommand(robot_state); + } + else if (expected_type == CommandType::POSE && new_pose_msg_) + { + next_joint_state = processPoseCommand(robot_state); + } + 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(node_->get_logger(), "Command type has not been set, cannot accept input"); + } - if (next_joint_state && (servo_->getStatus() != StatusCode::INVALID) && - (servo_->getStatus() != StatusCode::HALT_FOR_COLLISION)) - { - if (use_trajectory) + if (next_joint_state && (servo_->getStatus() != StatusCode::INVALID) && + (servo_->getStatus() != StatusCode::HALT_FOR_COLLISION)) { - auto& next_joint_state_value = next_joint_state.value(); - updateSlidingWindow(next_joint_state_value, joint_cmd_rolling_window_, servo_params_.max_expected_latency, - cur_time); - if (const auto msg = composeTrajectoryMessage(servo_params_, joint_cmd_rolling_window_)) + if (use_trajectory) + { + auto& next_joint_state_value = next_joint_state.value(); + updateSlidingWindow(next_joint_state_value, joint_cmd_rolling_window_, servo_params_.max_expected_latency, + cur_time); + if (const auto msg = composeTrajectoryMessage(servo_params_, joint_cmd_rolling_window_)) + { + trajectory_publisher_->publish(msg.value()); + } + } + else { - trajectory_publisher_->publish(msg.value()); + multi_array_publisher_->publish(composeMultiArrayMessage(servo_->getParams(), next_joint_state.value())); } + last_commanded_state_ = next_joint_state.value(); } else { - multi_array_publisher_->publish(composeMultiArrayMessage(servo_->getParams(), next_joint_state.value())); + // if no new command was created, use current robot state + updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time); + servo_->resetSmoothing(current_state); } - last_commanded_state_ = next_joint_state.value(); - } - else - { - // if no new command was created, use current robot state - updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time); - servo_->resetSmoothing(current_state); - } - - status_msg.code = static_cast(servo_->getStatus()); - status_msg.message = servo_->getStatusMessage(); - status_publisher_->publish(status_msg); + status_msg.code = static_cast(servo_->getStatus()); + status_msg.message = servo_->getStatusMessage(); + status_publisher_->publish(status_msg); + } servo_frequency.sleep(); } } From f821ad7a0fa92314fa0834b8360bf261e7d62a68 Mon Sep 17 00:00:00 2001 From: KazuyaOguma Date: Fri, 31 Jan 2025 23:46:33 +0900 Subject: [PATCH 4/8] delete space --- moveit_ros/moveit_servo/src/servo_node.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 6f961589e4..a709557f4d 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -148,12 +148,12 @@ void ServoNode::pauseServo(const std::shared_ptr& response) { if (servo_paused_ == request->data) - { - std::string message = "Requested pause state is already active."; - RCLCPP_INFO(node_->get_logger(), "%s", message.c_str()); - response->success = true; - response->message = message; - return; + { + std::string message = "Requested pause state is already active."; + RCLCPP_INFO(node_->get_logger(), "%s", message.c_str()); + response->success = true; + response->message = message; + return; } servo_paused_ = request->data; response->success = (servo_paused_ == request->data); @@ -409,6 +409,7 @@ void ServoNode::servoLoop() status_msg.message = servo_->getStatusMessage(); status_publisher_->publish(status_msg); } + servo_frequency.sleep(); } } From 5482ec333b9f64286c64c69e6f9f20fd0a5b751c Mon Sep 17 00:00:00 2001 From: KazuyaOguma Date: Sun, 2 Feb 2025 18:14:50 +0900 Subject: [PATCH 5/8] chore: add new line --- moveit_ros/moveit_servo/src/servo_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index a709557f4d..d9c863da5d 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -417,4 +417,4 @@ void ServoNode::servoLoop() } // namespace moveit_servo #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::ServoNode) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::ServoNode) From e88d08dd74a4603d1be9b3052552e6144e1f6bad Mon Sep 17 00:00:00 2001 From: KazuyaOguma Date: Wed, 5 Feb 2025 23:00:29 +0900 Subject: [PATCH 6/8] refactor: if next_joint_state is nullopt, update robot state with blocking --- moveit_ros/moveit_servo/src/servo_node.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index d9c863da5d..6b56024d72 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -234,7 +234,6 @@ std::optional ServoNode::processJointJogCommand(const moveit::co next_joint_state = result.second; RCLCPP_DEBUG_STREAM(node_->get_logger(), "Joint jog command timed out. Halting to a stop."); } - last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */); } return next_joint_state; @@ -267,7 +266,6 @@ std::optional ServoNode::processTwistCommand(const moveit::core: next_joint_state = result.second; RCLCPP_DEBUG_STREAM(node_->get_logger(), "Twist command timed out. Halting to a stop."); } - last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */); } return next_joint_state; @@ -297,7 +295,6 @@ std::optional ServoNode::processPoseCommand(const moveit::core:: next_joint_state = result.second; RCLCPP_DEBUG_STREAM(node_->get_logger(), "Pose command timed out. Halting to a stop."); } - last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */); } return next_joint_state; @@ -400,6 +397,10 @@ void ServoNode::servoLoop() } else { + // The next command does not exist = Because the robot is in a stopped state and there is no next target pose, a blocking process is used to fully update the robot's state. + if (!next_joint_state) { + last_commanded_state_ = servo_->getCurrentRobotState(true); + } // if no new command was created, use current robot state updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time); servo_->resetSmoothing(current_state); From 7a1f1b9d2b640880930628b670f0525897befe75 Mon Sep 17 00:00:00 2001 From: KazuyaOguma Date: Fri, 7 Feb 2025 00:25:52 +0900 Subject: [PATCH 7/8] non-blocking update of robot status --- moveit_ros/moveit_servo/src/servo_node.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 6b56024d72..a4ec7e024e 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -38,6 +38,7 @@ * */ +#include #if __has_include() #include #else @@ -397,11 +398,8 @@ void ServoNode::servoLoop() } else { - // The next command does not exist = Because the robot is in a stopped state and there is no next target pose, a blocking process is used to fully update the robot's state. - if (!next_joint_state) { - last_commanded_state_ = servo_->getCurrentRobotState(true); - } // if no new command was created, use current robot state + last_commanded_state_ = current_state = servo_->getCurrentRobotState(false); updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time); servo_->resetSmoothing(current_state); } From babb78a6144b9364f43819b31c31d0d81ab74da4 Mon Sep 17 00:00:00 2001 From: KazuyaOguma Date: Fri, 7 Feb 2025 00:26:54 +0900 Subject: [PATCH 8/8] remove unused include file --- moveit_ros/moveit_servo/src/servo_node.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index a4ec7e024e..7676584d40 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -38,7 +38,6 @@ * */ -#include #if __has_include() #include #else