Merge branch 'master' into pid_controller_feedforward_1270_1

Author: Juliaj

diff_drive_controller/doc/userdoc.rst

@@ -20,6 +20,7 @@ Other features
    + Odometry publishing
    + Task-space velocity, acceleration and jerk limits
    + Automatic stop after command time-out
+   + Chainable Controller
 
 
 Description of controller's interfaces
@@ -28,7 +29,13 @@ Description of controller's interfaces
 References
 ,,,,,,,,,,,,,,,,,,
 
-(the controller is not yet implemented as chainable controller)
+When controller is in chained mode, it exposes the following references which can be commanded by the preceding controller:
+
+- ``<controller_name>/linear/velocity``      double, in m/s
+- ``<controller_name>/angular/velocity``     double, in rad/s
+
+Together, these represent the body twist (which in unchained-mode would be obtained from ~/cmd_vel).
+The ``<controller_name>`` is commonly set to ``diff_drive_controller``.
 
 Feedback
 ,,,,,,,,,,,,,,

doc/writing_new_controller.rst

@@ -141,4 +141,7 @@ That's it! Enjoy writing great controllers!
 Useful External References
 ---------------------------
 
-- `Templates and scripts for generating controllers shell <https://rtw.stoglrobotics.de/master/use-cases/ros2_control/setup_controller.html>`_
+- `Templates and scripts for generating controllers shell <https://rtw.b-robotized.com/master/use-cases/ros2_control/setup_controller.html>`_
+
+
+  .. NOTE:: The script is currently only recommended to use with Humble, not compatible with the API from Jazzy and onwards.

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,