Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Reduce mutex scope in Servo thread #3259

Merged
merged 1 commit into from
Jan 22, 2025
Merged

Reduce mutex scope in Servo thread #3259

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
116 changes: 59 additions & 57 deletions moveit_ros/moveit_servo/src/servo_node.cpp
View file
Open in desktop
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
Loading