fix ci errors and pr comments

Author: evan-palmer

auv_control_msgs/action/FollowEndEffectorTrajectory.action

@@ -1,7 +1,16 @@
+# The end effector trajectory to follow.
+#
+# The trajectory header stamp is used to set the trajectory start time. This can be
+# set to zero to indicate that the controller should start following the trajectory
+# now.
 auv_control_msgs/EndEffectorTrajectory trajectory
 
+# The maximum error that the controller is allowed when following the trajectory.
+# When this is set to 0, the tolerance will not be applied during control.
 float64 path_tolerance
 
+# The maximum terminal error that the controller is allowed.
+# When this is set to 0, the tolerance will not affect the success of the action.
 float64 goal_tolerance
 
 ---
@@ -12,10 +21,20 @@ int32 OLD_HEADER_TIMESTAMP = -2
 int32 PATH_TOLERANCE_VIOLATED = -3
 int32 GOAL_TOLERANCE_VIOLATED = -4
 
+# A human-readable error description.
 string error_string
 
 ---
 std_msgs/Header header
+
+# The reference pose for the controller at the current time instance.
+# This is distinct from the sample, which is retrieved at the next time
+# instance.
 geometry_msgs/Pose desired
+
+# The current end effector state.
 geometry_msgs/Pose actual
+
+# The squared Euclidean norm of the geodesic distance between the desired
+# and actual states.
 float64 error

auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg

@@ -1,5 +1,5 @@
 # The sequence of end effector poses.
 geometry_msgs/Pose point
 
-# The duration of the trajectory.
+# The time that this point should be reached, measured from the start of the trajectory.
 builtin_interfaces/Duration time_from_start

end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp

@@ -88,16 +88,16 @@ class EndEffectorTrajectoryController : public controller_interface::ControllerI
   // the end effector trajectories can be set using either the topic or action server
   // the action server is preferred and easier to integrate into state-machines/behavior trees, but can be a bit
   // cumbersome to interface with. on the other hand, the topic interface is easier to use, but doesn't integrate
-  // well with high-level coordination
+  // well with high-level coordinators
   realtime_tools::RealtimeBuffer<Trajectory> rt_trajectory_;
   std::shared_ptr<rclcpp::Subscription<auv_control_msgs::msg::EndEffectorTrajectory>> trajectory_sub_;
 
-  using FollowTrajectoryAction = auv_control_msgs::action::FollowEndEffectorTrajectory;
-  using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<FollowTrajectoryAction>;
+  using FollowTrajectory = auv_control_msgs::action::FollowEndEffectorTrajectory;
+  using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<FollowTrajectory>;
   using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
   using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
 
-  std::shared_ptr<rclcpp_action::Server<FollowTrajectoryAction>> action_server_;
+  std::shared_ptr<rclcpp_action::Server<FollowTrajectory>> action_server_;
   RealtimeGoalHandleBuffer rt_active_goal_;
   realtime_tools::RealtimeBuffer<bool> rt_goal_in_progress_;
   std::shared_ptr<rclcpp::TimerBase> goal_handle_timer_;
@@ -113,6 +113,8 @@ class EndEffectorTrajectoryController : public controller_interface::ControllerI
   end_effector_trajectory_controller::Params params_;
 
   // error tolerances
+  // the default tolerances are extracted from the parameters and applied when the action interface is not used
+  // if the action interface is being used, then the tolerances set in the goal are applied
   double default_path_tolerance_, default_goal_tolerance_;
   realtime_tools::RealtimeBuffer<double> rt_goal_tolerance_, rt_path_tolerance_;
 

end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp

@@ -48,22 +48,23 @@ class Trajectory
   Trajectory(const auv_control_msgs::msg::EndEffectorTrajectory & trajectory, const geometry_msgs::msg::Pose & state);
 
   /// Whether or not the trajectory is empty.
-  auto empty() const -> bool;
+  [[nodiscard]] auto empty() const -> bool;
 
   /// Get the starting time of the trajectory.
-  auto start_time() const -> rclcpp::Time;
+  [[nodiscard]] auto start_time() const -> rclcpp::Time;
 
   /// Get the ending time of the trajectory.
-  auto end_time() const -> rclcpp::Time;
+  [[nodiscard]] auto end_time() const -> rclcpp::Time;
 
   /// Get the first point in the trajectory.
-  auto start_point() const -> std::optional<geometry_msgs::msg::Pose>;
+  [[nodiscard]] auto start_point() const -> std::optional<geometry_msgs::msg::Pose>;
 
   /// Get the last point in the trajectory.
-  auto end_point() const -> std::optional<geometry_msgs::msg::Pose>;
+  [[nodiscard]] auto end_point() const -> std::optional<geometry_msgs::msg::Pose>;
 
   /// Sample a point in the trajectory at the given time.
-  auto sample(const rclcpp::Time & sample_time) const -> std::expected<geometry_msgs::msg::Pose, SampleError>;
+  [[nodiscard]] auto sample(const rclcpp::Time & sample_time) const
+    -> std::expected<geometry_msgs::msg::Pose, SampleError>;
 
   /// Reset the initial end effector state and trajectory start time.
   auto reset_initial_state(const geometry_msgs::msg::Pose & state) -> void;

end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp

@@ -107,6 +107,7 @@ auto EndEffectorTrajectoryController::validate_trajectory(
     trajectory.header.stamp = get_node()->now();
   }
 
+  // NOLINTNEXTLINE(readability-use-anyofallof)
   for (const auto [p1, p2] : std::views::zip(trajectory.points, trajectory.points | std::views::drop(1))) {
     const rclcpp::Duration p1_start = p1.time_from_start;
     const rclcpp::Duration p2_start = p2.time_from_start;
@@ -160,8 +161,8 @@ auto EndEffectorTrajectoryController::on_configure(const rclcpp_lifecycle::State
     });
 
   auto handle_goal =
-    [this](const rclcpp_action::GoalUUID & /*uuid*/, std::shared_ptr<const FollowTrajectoryAction::Goal> goal) {
-      RCLCPP_INFO(logger_, "Received new trajectory goal");  // NOLINT
+    [this](const rclcpp_action::GoalUUID & /*uuid*/, std::shared_ptr<const FollowTrajectory::Goal> goal) {  // NOLINT
+      RCLCPP_INFO(logger_, "Received new trajectory goal");                                                 // NOLINT
       if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
         RCLCPP_ERROR(logger_, "Can't accept new action goals. Controller is not running.");  // NOLINT
         return rclcpp_action::GoalResponse::REJECT;
@@ -178,12 +179,12 @@ auto EndEffectorTrajectoryController::on_configure(const rclcpp_lifecycle::State
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     };
 
-  auto handle_cancel = [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowTrajectoryAction>> gh) {
-    RCLCPP_INFO(logger_, "Received cancel action goal");  // NOLINT
+  auto handle_cancel = [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowTrajectory>> gh) {  // NOLINT
+    RCLCPP_INFO(logger_, "Received cancel action goal");                                                      // NOLINT
     const auto active_goal = *rt_active_goal_.readFromNonRT();
     if (active_goal && active_goal->gh_ == gh) {
       RCLCPP_INFO(logger_, "Canceling active goal");  // NOLINT
-      auto action_result = std::make_shared<FollowTrajectoryAction::Result>();
+      auto action_result = std::make_shared<FollowTrajectory::Result>();
       active_goal->setCanceled(action_result);
       rt_holding_position_.writeFromNonRT(true);
       rt_first_sample_.writeFromNonRT(true);
@@ -192,14 +193,14 @@ auto EndEffectorTrajectoryController::on_configure(const rclcpp_lifecycle::State
     return rclcpp_action::CancelResponse::ACCEPT;
   };
 
-  auto handle_accepted = [this](std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowTrajectoryAction>> gh) {
-    RCLCPP_INFO(logger_, "Received accepted action goal");  // NOLINT
+  auto handle_accepted = [this](std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowTrajectory>> gh) {  // NOLINT
+    RCLCPP_INFO(logger_, "Received accepted action goal");                                                // NOLINT
     rt_goal_in_progress_.writeFromNonRT(true);
     const auto active_goal = *rt_active_goal_.readFromNonRT();
     if (active_goal) {
       RCLCPP_INFO(logger_, "Canceling active goal");  // NOLINT
-      auto action_result = std::make_shared<FollowTrajectoryAction::Result>();
-      action_result->error_code = FollowTrajectoryAction::Result::INVALID_GOAL;
+      auto action_result = std::make_shared<FollowTrajectory::Result>();
+      action_result->error_code = FollowTrajectory::Result::INVALID_GOAL;
       action_result->error_string = "Current goal cancelled by a new incoming action.";
       active_goal->setCanceled(action_result);
       rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
@@ -208,7 +209,7 @@ auto EndEffectorTrajectoryController::on_configure(const rclcpp_lifecycle::State
     rt_goal_tolerance_.writeFromNonRT(gh->get_goal()->goal_tolerance);
     rt_path_tolerance_.writeFromNonRT(gh->get_goal()->path_tolerance);
 
-    RealtimeGoalHandlePtr rt_gh = std::make_shared<RealtimeGoalHandle>(gh);
+    const RealtimeGoalHandlePtr rt_gh = std::make_shared<RealtimeGoalHandle>(gh);
     rt_gh->execute();
     rt_active_goal_.writeFromNonRT(rt_gh);
 
@@ -392,7 +393,7 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc
 
       case SampleError::SAMPLE_TIME_AFTER_END: {
         const double goal_tolerance = *rt_goal_tolerance_.readFromRT();
-        double goal_error = geodesic_error(trajectory->end_point().value(), end_effector_state);
+        const double goal_error = geodesic_error(trajectory->end_point().value(), end_effector_state);
         RCLCPP_INFO(logger_, "Trajectory sample time is after trajectory end time.");  // NOLINT
         if (goal_tolerance > 0.0) {
           if (goal_error > goal_tolerance) {
@@ -416,7 +417,7 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc
 
   if (active_goal) {
     // write feedback to the action server
-    auto feedback = std::make_shared<FollowTrajectoryAction::Feedback>();
+    auto feedback = std::make_shared<FollowTrajectory::Feedback>();
     feedback->header.stamp = time;
     feedback->desired = reference_state;
     feedback->actual = end_effector_state;
@@ -425,20 +426,20 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc
 
     // check terminal conditions
     if (goal_tolerance_exceeded) {
-      auto action_result = std::make_shared<FollowTrajectoryAction::Result>();
-      action_result->error_code = FollowTrajectoryAction::Result::PATH_TOLERANCE_VIOLATED;
+      auto action_result = std::make_shared<FollowTrajectory::Result>();
+      action_result->error_code = FollowTrajectory::Result::PATH_TOLERANCE_VIOLATED;
       action_result->error_string = "Trajectory execution aborted. Goal tolerance exceeded.";
       active_goal->setAborted(action_result);
       rt_holding_position_.writeFromNonRT(true);
     } else if (trajectory_suceeded) {
-      auto action_result = std::make_shared<FollowTrajectoryAction::Result>();
-      action_result->error_code = FollowTrajectoryAction::Result::SUCCESSFUL;
+      auto action_result = std::make_shared<FollowTrajectory::Result>();
+      action_result->error_code = FollowTrajectory::Result::SUCCESSFUL;
       action_result->error_string = "Trajectory execution completed successfully!";
       active_goal->setSucceeded(action_result);
       rt_holding_position_.writeFromNonRT(true);
     } else if (path_tolerance_exceeded) {
-      auto action_result = std::make_shared<FollowTrajectoryAction::Result>();
-      action_result->error_code = FollowTrajectoryAction::Result::PATH_TOLERANCE_VIOLATED;
+      auto action_result = std::make_shared<FollowTrajectory::Result>();
+      action_result->error_code = FollowTrajectory::Result::PATH_TOLERANCE_VIOLATED;
       action_result->error_string = "Trajectory execution aborted. Path tolerance exceeded.";
       active_goal->setAborted(action_result);
       rt_holding_position_.writeFromNonRT(true);

end_effector_trajectory_controller/src/trajectory.cpp

@@ -96,7 +96,7 @@ auto interpolate(
 Trajectory::Trajectory(
   const auv_control_msgs::msg::EndEffectorTrajectory & trajectory,
   const geometry_msgs::msg::Pose & state)
-: points_(std::move(trajectory)),
+: points_(trajectory),
   initial_time_(static_cast<rclcpp::Time>(trajectory.header.stamp)),
   initial_state_(state)
 {