Skip to content

Commit

Permalink
Reduce mutex scope in Servo thread (#3259)
Browse files Browse the repository at this point in the history 
Fixes occasional main thread starvation caused by repeatedly
creating a lock_guard in the servo loop. Fairness is improved
by removing the thread sleep rate from the lock scope.
  • Loading branch information
@henningkayser
henningkayser authored Jan 22, 2025
1 parent f47ecb5 commit b91e529
Showing 1 changed file with 59 additions and 57 deletions.
116 changes: 59 additions & 57 deletions moveit_ros/moveit_servo/src/servo_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -326,76 +326,78 @@ void ServoNode::servoLoop()
continue;
}

std::lock_guard<std::mutex> 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<std::mutex> 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<int8_t>(servo_->getStatus());
status_msg.message = servo_->getStatusMessage();
status_publisher_->publish(status_msg);
status_msg.code = static_cast<int8_t>(servo_->getStatus());
status_msg.message = servo_->getStatusMessage();
status_publisher_->publish(status_msg);
}

servo_frequency.sleep();
}
Expand Down

0 comments on commit b91e529

Please sign in to comment.