Merge branch 'main' into dp-quaternion-control

Author: ppakr

auv_setup/config/robots/orca.yaml

@@ -123,7 +123,7 @@
       twist: "twist"
       operation_mode: "operation_mode"
       killswitch: "killswitch"
-      aruco_board_pose_camera: "aruco_board_pose_camera"
+      reference_pose: "reference_pose"
       waypoint: "waypoint"
       waypoint_list: "waypoint_list"
       guidance:

guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp

@@ -78,6 +78,8 @@ class ReferenceFilterNode : public rclcpp::Node {
 
     Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in, uint8_t mode);
 
+    void publish_hold_reference();
+
     vortex_msgs::msg::ReferenceFilter fill_reference_msg();
 
     rclcpp_action::Server<

guidance/reference_filter_dp/src/reference_filter_ros.cpp

@@ -1,5 +1,6 @@
 #include "reference_filter_dp/reference_filter_ros.hpp"
 #include <spdlog/spdlog.h>
+#include <mutex>
 #include <rclcpp_components/register_node_macro.hpp>
 #include <string_view>
 #include <vortex/utils/math.hpp>
@@ -34,20 +35,20 @@ void ReferenceFilterNode::set_subscribers_and_publisher() {
     this->declare_parameter<std::string>("topics.pose");
     this->declare_parameter<std::string>("topics.twist");
     this->declare_parameter<std::string>("topics.guidance.dp");
-    this->declare_parameter<std::string>("topics.aruco_board_pose_camera");
+    this->declare_parameter<std::string>("topics.reference_pose");
 
     std::string pose_topic = this->get_parameter("topics.pose").as_string();
     std::string twist_topic = this->get_parameter("topics.twist").as_string();
     std::string guidance_topic =
         this->get_parameter("topics.guidance.dp").as_string();
-    std::string aruco_board_pose_camera_topic =
-        this->get_parameter("topics.aruco_board_pose_camera").as_string();
+    std::string reference_pose_topic =
+        this->get_parameter("topics.reference_pose").as_string();
 
     auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1);
     reference_pub_ = this->create_publisher<vortex_msgs::msg::ReferenceFilter>(
         guidance_topic, qos_sensor_data);
     reference_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
-        aruco_board_pose_camera_topic, qos_sensor_data,
+        reference_pose_topic, qos_sensor_data,
         std::bind(&ReferenceFilterNode::reference_callback, this,
                   std::placeholders::_1));
     pose_sub_ = this->create_subscription<
@@ -111,11 +112,13 @@ void ReferenceFilterNode::reference_callback(
 
 void ReferenceFilterNode::pose_callback(
     const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
+    std::lock_guard<std::mutex> lock(mutex_);
     current_pose_ = *msg;
 }
 
 void ReferenceFilterNode::twist_callback(
     const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) {
+    std::lock_guard<std::mutex> lock(mutex_);
     current_twist_ = *msg;
 }
 
@@ -268,6 +271,41 @@ Eigen::Vector6d ReferenceFilterNode::apply_mode_logic(
     return r_out;
 }
 
+void ReferenceFilterNode::publish_hold_reference() {
+    if (!reference_pub_) {
+        return;
+    }
+
+    const auto& p = current_pose_.pose.pose.position;
+    const auto& o = current_pose_.pose.pose.orientation;
+
+    Eigen::Quaterniond q(o.w, o.x, o.y, o.z);
+    if (q.norm() < 1e-9) {
+        q = Eigen::Quaterniond::Identity();
+    } else {
+        q.normalize();
+    }
+
+    Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q);
+
+    vortex_msgs::msg::ReferenceFilter hold_msg;
+    hold_msg.x = p.x;
+    hold_msg.y = p.y;
+    hold_msg.z = p.z;
+    hold_msg.roll = vortex::utils::math::ssa(euler_angles(0));
+    hold_msg.pitch = vortex::utils::math::ssa(euler_angles(1));
+    hold_msg.yaw = vortex::utils::math::ssa(euler_angles(2));
+
+    hold_msg.x_dot = 0.0;
+    hold_msg.y_dot = 0.0;
+    hold_msg.z_dot = 0.0;
+    hold_msg.roll_dot = 0.0;
+    hold_msg.pitch_dot = 0.0;
+    hold_msg.yaw_dot = 0.0;
+
+    reference_pub_->publish(hold_msg);
+}
+
 void ReferenceFilterNode::execute(
     const std::shared_ptr<rclcpp_action::ServerGoalHandle<
         vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle) {
@@ -307,16 +345,21 @@ void ReferenceFilterNode::execute(
         {
             std::lock_guard<std::mutex> lock(mutex_);
             if (goal_handle->get_goal_id() == preempted_goal_id_) {
+                publish_hold_reference();
                 result->success = false;
                 goal_handle->abort(result);
                 return;
             }
         }
-        if (goal_handle->is_canceling()) {
-            result->success = false;
-            goal_handle->canceled(result);
-            spdlog::info("Goal canceled");
-            return;
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            if (goal_handle->is_canceling()) {
+                publish_hold_reference();
+                result->success = false;
+                goal_handle->canceled(result);
+                spdlog::info("Goal canceled");
+                return;
+            }
         }
         Eigen::Vector18d x_dot = reference_filter_->calculate_x_dot(x_, r_);
         x_ += x_dot * time_step_.count() / 1000.0;

mission/FSM/docking/COLCON_IGNORE

@@ -1,8 +1,6 @@
 #ifndef DOCKING__DOCKING_HPP_
 #define DOCKING__DOCKING_HPP_
 
-#include <chrono>
-#include <iostream>
 #include <memory>
 #include <string>
 
@@ -38,112 +36,108 @@ using ReturnHomeAction = vortex_msgs::action::ReferenceFilterWaypoint;
 class FindDockingStationState
     : public yasmin_ros::ActionState<docking_fsm::FindDockingAction> {
    public:
-    FindDockingStationState(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+    explicit FindDockingStationState(
+        std::shared_ptr<yasmin::Blackboard> blackboard);
 
     docking_fsm::FindDockingAction::Goal create_goal_handler(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+        std::shared_ptr<yasmin::Blackboard> blackboard);
 
     std::string response_handler(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<yasmin::Blackboard> blackboard,
         docking_fsm::FindDockingAction::Result::SharedPtr response);
 
     void print_feedback(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<yasmin::Blackboard> blackboard,
         std::shared_ptr<const docking_fsm::FindDockingAction::Feedback>
             feedback);
 };
 
 class ApproachDockingStationState
     : public yasmin_ros::ActionState<docking_fsm::ApproachDockingAction> {
    public:
-    ApproachDockingStationState(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+    explicit ApproachDockingStationState(
+        std::shared_ptr<yasmin::Blackboard> blackboard);
 
     docking_fsm::ApproachDockingAction::Goal create_goal_handler(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+        std::shared_ptr<yasmin::Blackboard> blackboard);
 
     std::string response_handler(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<yasmin::Blackboard> blackboard,
         docking_fsm::ApproachDockingAction::Result::SharedPtr response);
 
     void print_feedback(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<yasmin::Blackboard> blackboard,
         std::shared_ptr<const docking_fsm::ApproachDockingAction::Feedback>
             feedback);
 };
 
 class GoAboveDockingStationState
     : public yasmin_ros::ActionState<docking_fsm::GoAboveDockingAction> {
    public:
-    GoAboveDockingStationState(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+    explicit GoAboveDockingStationState(
+        std::shared_ptr<yasmin::Blackboard> blackboard);
 
     docking_fsm::GoAboveDockingAction::Goal create_goal_handler(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+        std::shared_ptr<yasmin::Blackboard> blackboard);
 
     std::string response_handler(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<yasmin::Blackboard> blackboard,
         docking_fsm::GoAboveDockingAction::Result::SharedPtr response);
 
     void print_feedback(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<yasmin::Blackboard> blackboard,
         std::shared_ptr<const docking_fsm::GoAboveDockingAction::Feedback>
             feedback);
 };
 
-std::string DockedState(
-    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+std::string DockedState(std::shared_ptr<yasmin::Blackboard> blackboard);
 
 class ReturnHomeState
     : public yasmin_ros::ActionState<docking_fsm::ReturnHomeAction> {
    public:
-    explicit ReturnHomeState(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+    explicit ReturnHomeState(std::shared_ptr<yasmin::Blackboard> blackboard);
 
     docking_fsm::ReturnHomeAction::Goal create_goal_handler(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+        std::shared_ptr<yasmin::Blackboard> blackboard);
 
     std::string response_handler(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<yasmin::Blackboard> blackboard,
         docking_fsm::ReturnHomeAction::Result::SharedPtr response);
 
     void print_feedback(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<yasmin::Blackboard> blackboard,
         std::shared_ptr<const docking_fsm::ReturnHomeAction::Feedback>
             feedback);
 };
 
-std::string AbortState(
-    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+std::string AbortState(std::shared_ptr<yasmin::Blackboard> blackboard);
 
-std::string ErrorState(
-    std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+std::string ErrorState(std::shared_ptr<yasmin::Blackboard> blackboard);
 
 class ConvergeDockingStationState
     : public yasmin_ros::ActionState<docking_fsm::ConvergeDockingAction> {
    public:
-    ConvergeDockingStationState(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+    explicit ConvergeDockingStationState(
+        std::shared_ptr<yasmin::Blackboard> blackboard);
 
     docking_fsm::ConvergeDockingAction::Goal create_goal_handler(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+        std::shared_ptr<yasmin::Blackboard> blackboard);
 
     std::string response_handler(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<yasmin::Blackboard> blackboard,
         docking_fsm::ConvergeDockingAction::Result::SharedPtr response);
 
     void print_feedback(
-        std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
+        std::shared_ptr<yasmin::Blackboard> blackboard,
         std::shared_ptr<const docking_fsm::ConvergeDockingAction::Feedback>
             feedback);
 };
 
 std::shared_ptr<yasmin::StateMachine> create_state_machines();
 
 void add_states(std::shared_ptr<yasmin::StateMachine> sm,
-                std::shared_ptr<yasmin::blackboard::Blackboard> blackboard);
+                std::shared_ptr<yasmin::Blackboard> blackboard);
 
-auto initialize_blackboard();
+std::shared_ptr<yasmin::Blackboard> initialize_blackboard();
 
 #endif  // DOCKING__DOCKING_HPP_