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

Fix JTC segfault #164

Merged
merged 2 commits into from
Apr 8, 2021
Merged

Fix JTC segfault #164

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
93d2ce5
Use a copy of the rt_active_goal to avoid segfault
matthew-reynolds Apr 5, 2021
01d68f1
Use RealtimeBuffer for thread-safety
matthew-reynolds Apr 5, 2021
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
3 changes: 2 additions & 1 deletion ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,10 +149,11 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
using FollowJTrajAction = control_msgs::action::FollowJointTrajectory;
using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<FollowJTrajAction>;
using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;

rclcpp_action::Server<FollowJTrajAction>::SharedPtr action_server_;
bool allow_partial_joints_goal_ = false;
RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any.
RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any.
rclcpp::TimerBase::SharedPtr goal_handle_timer_;
rclcpp::Duration action_monitor_period_ = rclcpp::Duration(RCUTILS_MS_TO_NS(50));

Expand Down
54 changes: 29 additions & 25 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,8 @@ JointTrajectoryController::update()
}
}

if (rt_active_goal_) {
const auto active_goal = *rt_active_goal_.readFromRT();
if (active_goal) {
// send feedback
auto feedback = std::make_shared<FollowJTrajAction::Feedback>();
feedback->header.stamp = node_->now();
Expand All @@ -195,7 +196,7 @@ JointTrajectoryController::update()
feedback->actual = state_current;
feedback->desired = state_desired;
feedback->error = state_error;
rt_active_goal_->setFeedback(feedback);
active_goal->setFeedback(feedback);

// check abort
if (abort || outside_goal_tolerance) {
Expand All @@ -208,17 +209,18 @@ JointTrajectoryController::update()
RCLCPP_WARN(node_->get_logger(), "Aborted due to goal tolerance violation");
result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
}
rt_active_goal_->setAborted(result);
rt_active_goal_.reset();
}
active_goal->setAborted(result);
// TODO(matthew-reynolds): Need a lock-free write here
destogl marked this conversation as resolved.
Show resolved Hide resolved
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());

// check goal tolerance
if (!before_last_point) {
// check goal tolerance
} else if (!before_last_point) {
if (!outside_goal_tolerance) {
auto res = std::make_shared<FollowJTrajAction::Result>();
res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
rt_active_goal_->setSucceeded(res);
rt_active_goal_.reset();
active_goal->setSucceeded(res);
// TODO(matthew-reynolds): Need a lock-free write here
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());

RCLCPP_INFO(node_->get_logger(), "Goal reached, success!");
} else if (default_tolerances_.goal_time_tolerance != 0.0) {
Expand All @@ -230,8 +232,9 @@ JointTrajectoryController::update()
if (difference > default_tolerances_.goal_time_tolerance) {
auto result = std::make_shared<FollowJTrajAction::Result>();
result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
rt_active_goal_->setAborted(result);
rt_active_goal_.reset();
active_goal->setAborted(result);
// TODO(matthew-reynolds): Need a lock-free write here
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
RCLCPP_WARN(
node_->get_logger(),
"Aborted due goal_time_tolerance exceeding by %f seconds",
Expand Down Expand Up @@ -554,7 +557,8 @@ rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback(
RCLCPP_INFO(node_->get_logger(), "Got request to cancel goal");

// Check that cancel request refers to currently active goal (if any)
if (rt_active_goal_ && rt_active_goal_->gh_ == goal_handle) {
const auto active_goal = *rt_active_goal_.readFromNonRT();
if (active_goal && active_goal->gh_ == goal_handle) {
// Controller uptime
// Enter hold current position mode
set_hold_position();
Expand All @@ -565,10 +569,8 @@ rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback(

// Mark the current goal as canceled
auto action_res = std::make_shared<FollowJTrajAction::Result>();
rt_active_goal_->setCanceled(action_res);

// Reset current goal
rt_active_goal_.reset();
active_goal->setCanceled(action_res);
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
}
return rclcpp_action::CancelResponse::ACCEPT;
}
Expand All @@ -583,17 +585,18 @@ void JointTrajectoryController::feedback_setup_callback(
goal_handle->get_goal()->trajectory);

add_new_trajectory_msg(traj_msg);

RealtimeGoalHandlePtr rt_goal = std::make_shared<RealtimeGoalHandle>(goal_handle);
rt_goal->preallocated_feedback_->joint_names = joint_names_;
rt_active_goal_ = rt_goal;
rt_active_goal_->execute();
}

// Update the active goal
RealtimeGoalHandlePtr rt_goal = std::make_shared<RealtimeGoalHandle>(goal_handle);
rt_goal->preallocated_feedback_->joint_names = joint_names_;
rt_goal->execute();
rt_active_goal_.writeFromNonRT(rt_goal);

// Setup goal status checking timer
goal_handle_timer_ = node_->create_wall_timer(
action_monitor_period_.to_chrono<std::chrono::seconds>(),
std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_));
std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal));
}
Comment on lines +590 to 600
Copy link
Member Author

@matthew-reynolds matthew-reynolds Apr 5, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this needs further work, probably in a follow-up PR.

In both the old and new code, I worry that we might end up in a condition where we're overwriting the goal_handle_timer_ while the previous rt_active_goal_ has pending operations, and thus dropping those operations. I think we should call goal_handle_timer_::execute_callback() or rt_active_goal_.readFromNonRT()->runNonRealtime() before creating the new timer.

Similarly, I think we could probably end up in a race condition where something is resetting the rt_active_goal_ right after this function writes the new rt_goal, and we could end up losing the goal handle. This would involve a little more work to solve, for example only resetting the rt_active_goal_ if new_data_available_ == false. That variable is private, but something along those lines.

Would appreciate your thoughts so we can consider appropriate follow-up PRs. This PR leaves the behaviour unchanged from before, and I haven't yet run into either of those cases, so I think we're ok to leave it to a future task.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This write-up above is a perfect start for the description of a follow-up issue ;)

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@destogl beat me to it! See #166


void JointTrajectoryController::fill_partial_goal(
Expand Down Expand Up @@ -782,12 +785,13 @@ void JointTrajectoryController::add_new_trajectory_msg(

void JointTrajectoryController::preempt_active_goal()
{
if (rt_active_goal_) {
const auto active_goal = *rt_active_goal_.readFromNonRT();
if (active_goal) {
auto action_res = std::make_shared<FollowJTrajAction::Result>();
action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL);
action_res->set__error_string("Current goal cancelled due to new incoming action.");
rt_active_goal_->setCanceled(action_res);
rt_active_goal_.reset();
active_goal->setCanceled(action_res);
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
}
}

Expand Down