[JTC]: Abort goal on deactivate (#1517)

Author: urfeex

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -1000,7 +1000,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
     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 during deactivate transition.");
-    active_goal->setCanceled(action_res);
+    active_goal->setAborted(action_res);
     rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
   }
 

joint_trajectory_controller/test/test_trajectory_actions.cpp

@@ -969,6 +969,91 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_tr
   EXPECT_TRUE(gh_future.get());
 }
 
+TEST_P(TestTrajectoryActionsTestParameterized, deactivate_controller_aborts_action)
+{
+  // deactivate velocity tolerance
+  std::vector<rclcpp::Parameter> params = {
+    rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)};
+  SetUpExecutor(params, false, 1.0, 0.0);
+
+  // We use our own hardware thread here, as we want to make sure the controller is deactivated
+  auto controller_thread = std::thread(
+    [&]()
+    {
+      // controller hardware cycle update loop
+      auto clock = rclcpp::Clock(RCL_STEADY_TIME);
+      auto now_time = clock.now();
+      auto last_time = now_time;
+      rclcpp::Duration wait = rclcpp::Duration::from_seconds(0.5);
+      auto end_time = last_time + wait;
+      while (now_time < end_time)
+      {
+        now_time = now_time + rclcpp::Duration::from_seconds(0.01);
+        traj_controller_->update(now_time, now_time - last_time);
+        last_time = now_time;
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+      }
+      RCLCPP_INFO(node_->get_logger(), "Controller hardware thread finished");
+      traj_controller_->get_node()->deactivate();
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    });
+
+  std::shared_future<typename GoalHandle::SharedPtr> gh_future;
+  // send goal
+  std::vector<double> point_positions{1.0, 2.0, 3.0};
+  {
+    std::vector<JointTrajectoryPoint> points;
+    JointTrajectoryPoint point;
+    point.time_from_start = rclcpp::Duration::from_seconds(2.5);
+    point.positions = point_positions;
+
+    points.push_back(point);
+
+    gh_future = sendActionGoal(points, 1.0, goal_options_);
+  }
+
+  controller_thread.join();
+
+  EXPECT_TRUE(gh_future.get());
+  EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_);
+
+  auto state_ref = traj_controller_->get_state_reference();
+  auto state = traj_controller_->get_state_feedback();
+
+  // run an update
+  updateControllerAsync(rclcpp::Duration::from_seconds(0.01));
+
+  // There will be no active trajectory upon deactivation, so we can't use the expectCommandPoint
+  // method.
+  if (traj_controller_->has_position_command_interface())
+  {
+    EXPECT_NEAR(state_ref.positions.at(0), joint_pos_[0], COMMON_THRESHOLD);
+    EXPECT_NEAR(state_ref.positions.at(1), joint_pos_[1], COMMON_THRESHOLD);
+    EXPECT_NEAR(state_ref.positions.at(2), joint_pos_[2], COMMON_THRESHOLD);
+  }
+
+  if (traj_controller_->has_velocity_command_interface())
+  {
+    EXPECT_EQ(0.0, joint_vel_[0]);
+    EXPECT_EQ(0.0, joint_vel_[1]);
+    EXPECT_EQ(0.0, joint_vel_[2]);
+  }
+
+  if (traj_controller_->has_acceleration_command_interface())
+  {
+    EXPECT_EQ(0.0, joint_acc_[0]);
+    EXPECT_EQ(0.0, joint_acc_[1]);
+    EXPECT_EQ(0.0, joint_acc_[2]);
+  }
+
+  if (traj_controller_->has_effort_command_interface())
+  {
+    EXPECT_EQ(0.0, joint_eff_[0]);
+    EXPECT_EQ(0.0, joint_eff_[1]);
+    EXPECT_EQ(0.0, joint_eff_[2]);
+  }
+}
+
 // position controllers
 INSTANTIATE_TEST_SUITE_P(
   PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized,