Skip to content

Commit

Permalink
Fix JTC segfault (#164)
Browse files Browse the repository at this point in the history 
* Use a copy of the rt_active_goal to avoid segfault

* Use RealtimeBuffer for thread-safety
  • Loading branch information
@matthew-reynolds
matthew-reynolds authored Apr 8, 2021
1 parent 316e4c1 commit 0a6fe52
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 26 deletions.
3 changes: 2 additions & 1 deletion ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
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
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
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));
}

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

0 comments on commit 0a6fe52

Please sign in to comment.