Refactor/remove reference action feedback (#702)

* remove guidance action feedback

* remove reference feedback from waypoint manager action

* colcon ignore docking

Author: jorgenfj

guidance/los_guidance/src/los_guidance_ros.cpp

@@ -182,8 +182,6 @@ void LOSGuidanceNode::execute(
 
     adaptive_los_guidance_->update_angles(last_point_, next_point_);
 
-    auto feedback =
-        std::make_shared<vortex_msgs::action::LOSGuidance::Feedback>();
     auto result = std::make_shared<vortex_msgs::action::LOSGuidance::Result>();
 
     rclcpp::Rate loop_rate(1000.0 / time_step_.count());
@@ -213,23 +211,22 @@ void LOSGuidanceNode::execute(
 
         adaptive_los_guidance_->update_adaptive_estimates(errors);
 
-        vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference();
-
-        feedback->feedback = reference_msg;
-
-        goal_handle->publish_feedback(feedback);
-        reference_pub_->publish(reference_msg);
-
         if ((eta_ - next_point_).as_vector().norm() < goal_reached_tol_) {
             result->success = true;
             goal_handle->succeed(result);
             u_desired_ = 0.0;
-            vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference();
-            reference_pub_->publish(reference_msg);
+            auto final_reference_msg =
+                std::make_unique<vortex_msgs::msg::LOSGuidance>(
+                    fill_los_reference());
+            reference_pub_->publish(std::move(final_reference_msg));
             spdlog::info("Goal reached");
             return;
         }
 
+        auto reference_msg = std::make_unique<vortex_msgs::msg::LOSGuidance>(
+            fill_los_reference());
+        reference_pub_->publish(std::move(reference_msg));
+
         loop_rate.sleep();
     }
 }

guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp

@@ -170,8 +170,6 @@ void ReferenceFilterNode::execute(
 
     follower_->start(pose, twist, wp, convergence_threshold);
 
-    auto feedback = std::make_shared<
-        vortex_msgs::action::ReferenceFilterWaypoint::Feedback>();
     auto result = std::make_shared<
         vortex_msgs::action::ReferenceFilterWaypoint::Result>();
 
@@ -205,24 +203,22 @@ void ReferenceFilterNode::execute(
         if (target_reached) {
             follower_->snap_state_to_reference();
 
-            vortex_msgs::msg::ReferenceFilter final_reference_msg =
-                fill_reference_msg(follower_->state());
+            auto final_reference_msg =
+                std::make_unique<vortex_msgs::msg::ReferenceFilter>(
+                    fill_reference_msg(follower_->state()));
 
-            feedback->reference = final_reference_msg;
-            goal_handle->publish_feedback(feedback);
-            reference_pub_->publish(final_reference_msg);
+            reference_pub_->publish(std::move(final_reference_msg));
 
             result->success = true;
             goal_handle->succeed(result);
             spdlog::info("Goal reached");
             return;
         }
 
-        vortex_msgs::msg::ReferenceFilter reference_msg =
-            fill_reference_msg(filter_state);
-        reference_pub_->publish(reference_msg);
-        feedback->reference = reference_msg;
-        goal_handle->publish_feedback(feedback);
+        auto reference_msg =
+            std::make_unique<vortex_msgs::msg::ReferenceFilter>(
+                fill_reference_msg(filter_state));
+        reference_pub_->publish(std::move(reference_msg));
         loop_rate.sleep();
     }
     if (!rclcpp::ok() && goal_handle->is_active()) {

mission/FSM/docking/COLCON_IGNORE

@@ -4,7 +4,6 @@
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <vector>
 #include <vortex_msgs/action/reference_filter_waypoint.hpp>
@@ -105,8 +104,6 @@ class WaypointManagerNode : public rclcpp::Node {
     bool persistent_action_mode_active_{false};
     bool priority_mode_active_{false};
 
-    ReferenceFilterAction::Feedback latest_ref_feedback_;
-    bool has_reference_pose_{false};
     bool is_cancel_in_progress_{false};
 
     std::uint64_t mission_id_ = 0;

mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp

@@ -2,7 +2,6 @@
 #include <spdlog/spdlog.h>
 #include <cmath>
 #include <rclcpp_components/register_node_macro.hpp>
-#include <vortex/utils/ros/ros_conversions.hpp>
 
 namespace vortex::mission {
 
@@ -87,11 +86,6 @@ WaypointManagerNode::construct_result(bool success) const {
     auto result =
         std::make_shared<vortex_msgs::action::WaypointManager_Result>();
     result->success = success;
-    result->pose_valid = has_reference_pose_;
-    if (has_reference_pose_) {
-        result->final_pose = vortex::utils::ros_conversions::to_pose_msg(
-            latest_ref_feedback_.reference);
-    }
     return result;
 }
 
@@ -100,7 +94,6 @@ void WaypointManagerNode::cleanup_mission_state() {
     current_index_ = 0;
     persistent_action_mode_active_ = false;
     priority_mode_active_ = false;
-    has_reference_pose_ = false;
 
     if (active_reference_filter_goal_) {
         reference_filter_client_->async_cancel_goal(
@@ -122,6 +115,12 @@ void WaypointManagerNode::send_next_reference_filter_goal() {
         return;
     }
 
+    if (active_action_goal_ && active_action_goal_->is_active()) {
+        auto wm_fb = std::make_shared<WaypointManager::Feedback>();
+        wm_fb->current_waypoint = waypoints_[current_index_];
+        active_action_goal_->publish_feedback(wm_fb);
+    }
+
     ReferenceFilterAction::Goal rf_goal;
     rf_goal.waypoint = waypoints_[current_index_];
     rf_goal.convergence_threshold = convergence_threshold_;
@@ -153,7 +152,6 @@ rclcpp_action::GoalResponse WaypointManagerNode::handle_waypoint_goal(
     current_index_ = 0;
     persistent_action_mode_active_ = goal->persistent;
     priority_mode_active_ = false;
-    has_reference_pose_ = false;
     convergence_threshold_ = goal->convergence_threshold;
 
     if (waypoints_.empty() && !persistent_action_mode_active_) {
@@ -212,7 +210,6 @@ void WaypointManagerNode::handle_send_waypoints_service_request(
         mission_id_++;
         waypoints_ = request->waypoints;
         current_index_ = 0;
-        has_reference_pose_ = false;
 
         if (active_reference_filter_goal_) {
             reference_filter_client_->async_cancel_goal(
@@ -269,31 +266,6 @@ void WaypointManagerNode::send_reference_filter_goal(
             }
         };
 
-    options.feedback_callback =
-        [this, this_mission](
-            ReferenceFilterGoalHandle::SharedPtr,
-            const std::shared_ptr<const ReferenceFilterAction::Feedback> fb) {
-            if (this_mission != mission_id_) {
-                return;
-            }
-
-            latest_ref_feedback_ = *fb;
-            has_reference_pose_ = true;
-
-            if (!active_action_goal_ || !active_action_goal_->is_active())
-                return;
-
-            geometry_msgs::msg::Pose robot_pose =
-                vortex::utils::ros_conversions::to_pose_msg(fb->reference);
-
-            if (current_index_ < waypoints_.size()) {
-                auto wm_fb = std::make_shared<WaypointManager::Feedback>();
-                wm_fb->current_pose = robot_pose;
-                wm_fb->current_waypoint = waypoints_[current_index_];
-                active_action_goal_->publish_feedback(wm_fb);
-            }
-        };
-
     options
         .result_callback = [this, this_mission](
                                const ReferenceFilterGoalHandle::WrappedResult&