Fix JTC segfault (#164)

* Use a copy of the rt_active_goal to avoid segfault

* Use RealtimeBuffer for thread-safety

Author: matthew-reynolds

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -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));
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -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();
@@ -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) {
@@ -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) {
@@ -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",
@@ -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();
@@ -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;
 }
@@ -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(
@@ -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());
   }
 }