Skip to content

Commit

Permalink
Reduce mutex scope in Servo thread
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 committed Jan 22, 2025
1 parent e11cabb commit 59f5424
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 59f5424

Please sign in to comment.