ControlsROS2Wrapper.h

Go to the documentation of this file.

#ifndef CONTROLS_NODES_CONTROLSROS2WRAPPER_H
#define CONTROLS_NODES_CONTROLSROS2WRAPPER_H
 
#include <allocator/ThrustAllocator.h>
#include <utils/Types.h>
#include <utils/concurrentqueue.h>
#include <vehicle/Vehicle.h>
#include <yaml-cpp/yaml.h>
 
#include <atomic>
#include <bb_controls_msgs/action/locomotion.hpp>
#include <bb_controls_msgs/msg/controller_status.hpp>
#include <bb_controls_msgs/msg/thrusters.hpp>
#include <bb_controls_msgs/srv/controller.hpp>
#include <bb_controls_msgs/srv/encircle_traj.hpp>
#include <bb_controls_msgs/srv/spline_traj.hpp>
#include <bb_planner_msgs/srv/get_plan_through_poses.hpp>
#include <bb_sensor_msgs/msg/batteries_with_id.hpp>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <memory>
#include <mutex>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_action/server.hpp>
#include <std_msgs/msg/bool.hpp>
#include <string>
#include <string_view>
#include <thread>
#include <unordered_map>
 
class ControlsROS2Wrapper : public rclcpp::Node {
public:
    explicit ControlsROS2Wrapper(const std::string& name, std::unique_ptr<Vehicle> robot,
                                 std::string body_frame_tf_name = "base_link", std::string world_frame_tf_name = "map");
 
    ~ControlsROS2Wrapper() override;
 
    // Non-copyable and non-movable: mutex, atomic, unique_ptr, and thread members
    ControlsROS2Wrapper(const ControlsROS2Wrapper&)            = delete;
    ControlsROS2Wrapper& operator=(const ControlsROS2Wrapper&) = delete;
    ControlsROS2Wrapper(ControlsROS2Wrapper&&)                 = delete;
    ControlsROS2Wrapper& operator=(ControlsROS2Wrapper&&)      = delete;
 
    using Locomotion           = bb_controls_msgs::action::Locomotion;
    using GoalHandleLocomotion = rclcpp_action::ServerGoalHandle<Locomotion>;
 
    struct Params {
        double ff_gain               = 0.0;
        double fb_gain               = 1.0;
        double buoyancy_gain         = 0.0;
        bool surface_gain_scheduling = false;
        double surface_threshold     = 0.1;
        double axis_gain_x           = 1.0;
        double axis_gain_y           = 1.0;
        double axis_gain_z           = 0.0;
        double axis_gain_roll        = 0.0;
        double axis_gain_pitch       = 0.0;
        double axis_gain_yaw         = 1.0;
        double kp_x                  = 1491.433;
        double ki_x                  = 35.628;
        double kd_x                  = 21.098;
        double kp_y                  = 600.345;
        double ki_y                  = 9.834;
        double kd_y                  = 330.864;
        double kp_z                  = 969.781;
        double ki_z                  = 474.207;
        double kd_z                  = 180.169;
        double kp_roll               = 79.701;
        double ki_roll               = 91.516;
        double kd_roll               = 10.975;
        double kp_pitch              = 117.765;
        double ki_pitch              = 135.223;
        double kd_pitch              = 16.209;
        double kp_yaw                = 150.113;
        double ki_yaw                = 10.038;
        double kd_yaw                = 54.212;
        double kp_x_surface          = 226.595;
        double ki_x_surface          = 4.124;
        double kd_x_surface          = 1643.310;
        double kp_y_surface          = 334.345;
        double ki_y_surface          = 9.834;
        double kd_y_surface          = 1589.864;
        double kp_z_surface          = 969.781;
        double ki_z_surface          = 474.207;
        double kd_z_surface          = 180.169;
        double kp_roll_surface       = 79.701;
        double ki_roll_surface       = 91.516;
        double kd_roll_surface       = 10.975;
        double kp_pitch_surface      = 117.765;
        double ki_pitch_surface      = 135.223;
        double kd_pitch_surface      = 16.209;
        double kp_yaw_surface        = 29.113;
        double ki_yaw_surface        = 1.038;
        double kd_yaw_surface        = 54.212;
        double slow_down_gain        = 0.0;
        double max_xy_vel            = 10.0;
        double max_xy_acc            = 5.0;
        double max_xy_jerk           = 0.8;
        double max_z_vel             = 0.0;
        double max_z_acc             = 0.0;
        double max_z_jerk            = 0.0;
        double max_yaw_vel           = 3.0;
        double max_yaw_acc           = 0.4;
        double max_yaw_jerk          = 0.3;
 
        // Adaptive buoyancy bias estimation
        bool buoyancy_adaptive              = false;
        double buoyancy_adaptive_max_scale  = 5.0;
        double buoyancy_adaptive_clamp_pct  = 0.15;
        double buoyancy_adaptive_pos_thresh = 0.15;
        double buoyancy_adaptive_vel_thresh = 0.05;
 
        // System dynamics parameters
        double mass            = 36.10;
        double buoyancy_factor = 1.0;
        double i_xx            = 0.7699579;
        double i_yy            = 0.7699579;
        double i_zz            = 1.38624;
        double i_xy            = 0.0;
        double i_xz            = 0.0;
        double i_yz            = 0.0;
        double ma_x            = 12.19;
        double ma_y            = 17.44;
        double ma_z            = 26.47;
        double ma_roll         = 0.15;
        double ma_pitch        = 0.26;
        double ma_yaw          = 0.19;
        double dl_x            = 3.8;
        double dl_y            = 4.4;
        double dl_z            = 34.8;
        double dl_roll         = 0.131;
        double dl_pitch        = 0.201;
        double dl_yaw          = 0.077;
        double dnl_x           = 38.7;
        double dnl_y           = 54.6;
        double dnl_z           = 78.9;
        double dnl_roll        = 0.342;
        double dnl_pitch       = 0.465;
        double dnl_yaw         = 0.451;
        double rbb_x           = 0.0;
        double rbb_y           = 0.0;
        double rbb_z           = -0.05593619792;
        double rgb_x           = 0.0;
        double rgb_y           = 0.0;
        double rgb_z           = 0.0;
        double cg_offset_x     = 0.0;
        double cg_offset_y     = 0.0;
        double cg_offset_z     = 0.0;
    };
 
    // clang-format off
    static inline const std::unordered_map<std::string_view, double Params::*> DOUBLE_PARAM_MAP = {
        {"ff_gain", &Params::ff_gain},
        {"fb_gain", &Params::fb_gain},
        {"buoyancy_gain", &Params::buoyancy_gain},
        {"surface_threshold", &Params::surface_threshold},
        {"axis_gain_x", &Params::axis_gain_x},
        {"axis_gain_y", &Params::axis_gain_y},
        {"axis_gain_z", &Params::axis_gain_z},
        {"axis_gain_roll", &Params::axis_gain_roll},
        {"axis_gain_pitch", &Params::axis_gain_pitch},
        {"axis_gain_yaw", &Params::axis_gain_yaw},
        {"kp_x", &Params::kp_x},
        {"ki_x", &Params::ki_x},
        {"kd_x", &Params::kd_x},
        {"kp_y", &Params::kp_y},
        {"ki_y", &Params::ki_y},
        {"kd_y", &Params::kd_y},
        {"kp_z", &Params::kp_z},
        {"ki_z", &Params::ki_z},
        {"kd_z", &Params::kd_z},
        {"kp_roll", &Params::kp_roll},
        {"ki_roll", &Params::ki_roll},
        {"kd_roll", &Params::kd_roll},
        {"kp_pitch", &Params::kp_pitch},
        {"ki_pitch", &Params::ki_pitch},
        {"kd_pitch", &Params::kd_pitch},
        {"kp_yaw", &Params::kp_yaw},
        {"ki_yaw", &Params::ki_yaw},
        {"kd_yaw", &Params::kd_yaw},
        {"kp_x_surface", &Params::kp_x_surface},
        {"ki_x_surface", &Params::ki_x_surface},
        {"kd_x_surface", &Params::kd_x_surface},
        {"kp_y_surface", &Params::kp_y_surface},
        {"ki_y_surface", &Params::ki_y_surface},
        {"kd_y_surface", &Params::kd_y_surface},
        {"kp_z_surface", &Params::kp_z_surface},
        {"ki_z_surface", &Params::ki_z_surface},
        {"kd_z_surface", &Params::kd_z_surface},
        {"kp_roll_surface", &Params::kp_roll_surface},
        {"ki_roll_surface", &Params::ki_roll_surface},
        {"kd_roll_surface", &Params::kd_roll_surface},
        {"kp_pitch_surface", &Params::kp_pitch_surface},
        {"ki_pitch_surface", &Params::ki_pitch_surface},
        {"kd_pitch_surface", &Params::kd_pitch_surface},
        {"kp_yaw_surface", &Params::kp_yaw_surface},
        {"ki_yaw_surface", &Params::ki_yaw_surface},
        {"kd_yaw_surface", &Params::kd_yaw_surface},
        {"buoyancy_adaptive_max_scale", &Params::buoyancy_adaptive_max_scale},
        {"buoyancy_adaptive_clamp_pct", &Params::buoyancy_adaptive_clamp_pct},
        {"buoyancy_adaptive_pos_thresh", &Params::buoyancy_adaptive_pos_thresh},
        {"buoyancy_adaptive_vel_thresh", &Params::buoyancy_adaptive_vel_thresh},
        {"slow_down_gain", &Params::slow_down_gain},
        {"max_xy_vel", &Params::max_xy_vel},
        {"max_xy_acc", &Params::max_xy_acc},
        {"max_xy_jerk", &Params::max_xy_jerk},
        {"max_z_vel", &Params::max_z_vel},
        {"max_z_acc", &Params::max_z_acc},
        {"max_z_jerk", &Params::max_z_jerk},
        {"max_yaw_vel", &Params::max_yaw_vel},
        {"max_yaw_acc", &Params::max_yaw_acc},
        {"max_yaw_jerk", &Params::max_yaw_jerk},
        {"mass", &Params::mass},
        {"buoyancy_factor", &Params::buoyancy_factor},
        {"i_xx", &Params::i_xx},
        {"i_yy", &Params::i_yy},
        {"i_zz", &Params::i_zz},
        {"i_xy", &Params::i_xy},
        {"i_xz", &Params::i_xz},
        {"i_yz", &Params::i_yz},
        {"ma_x", &Params::ma_x},
        {"ma_y", &Params::ma_y},
        {"ma_z", &Params::ma_z},
        {"ma_roll", &Params::ma_roll},
        {"ma_pitch", &Params::ma_pitch},
        {"ma_yaw", &Params::ma_yaw},
        {"dl_x", &Params::dl_x},
        {"dl_y", &Params::dl_y},
        {"dl_z", &Params::dl_z},
        {"dl_roll", &Params::dl_roll},
        {"dl_pitch", &Params::dl_pitch},
        {"dl_yaw", &Params::dl_yaw},
        {"dnl_x", &Params::dnl_x},
        {"dnl_y", &Params::dnl_y},
        {"dnl_z", &Params::dnl_z},
        {"dnl_roll", &Params::dnl_roll},
        {"dnl_pitch", &Params::dnl_pitch},
        {"dnl_yaw", &Params::dnl_yaw},
        {"rbb_x", &Params::rbb_x},
        {"rbb_y", &Params::rbb_y},
        {"rbb_z", &Params::rbb_z},
        {"rgb_x", &Params::rgb_x},
        {"rgb_y", &Params::rgb_y},
        {"rgb_z", &Params::rgb_z},
        {"cg_offset_x", &Params::cg_offset_x},
        {"cg_offset_y", &Params::cg_offset_y},
        {"cg_offset_z", &Params::cg_offset_z},
    };
 
    static inline const std::unordered_map<std::string_view, bool Params::*> BOOL_PARAM_MAP = {
        {"surface_gain_scheduling", &Params::surface_gain_scheduling},
        {"buoyancy_adaptive", &Params::buoyancy_adaptive},
    };
    // clang-format on
 
protected:
    static constexpr int CONTROL_LOOP_RATE = 20;
 
    static constexpr double MIN_DEPTH_CLAMP = 0.00;
 
    static constexpr double POSITION_JUMP_THRESHOLD = 0.5;
 
    std::once_flag control_loop_started_;
 
    const std::string vehicle_frame_id_;
 
    const std::string world_frame_id_;
 
    std::unique_ptr<Vehicle> auv_;
 
    // ROS subscribers
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr world_sub_;
    rclcpp::Subscription<bb_sensor_msgs::msg::BatteriesWithId>::SharedPtr battery_sub_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr teleop_force_sub_;
 
    // ROS publishers
    rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr pub_controller_force_;
    rclcpp::Publisher<bb_controls_msgs::msg::ControllerStatus>::SharedPtr pub_status_;
    rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pub_traj_viz_;
    rclcpp::Publisher<bb_controls_msgs::msg::Thrusters>::SharedPtr pub_thruster_commands_;
    rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr pub_allocator_force_;
    rclcpp::Publisher<bb_controls_msgs::msg::ThrusterForces>::SharedPtr pub_thruster_forces_;
 
    rclcpp::TimerBase::SharedPtr control_loop_timer_;
    rclcpp::TimerBase::SharedPtr debug_publish_timer_;
 
    moodycamel::ConcurrentQueue<DebugMessage> debug_queue_;
 
    rclcpp::Service<bb_controls_msgs::srv::Controller>::SharedPtr controller_srv_;
    rclcpp::Service<bb_controls_msgs::srv::SplineTraj>::SharedPtr spline_traj_srv_;
    rclcpp::Service<bb_controls_msgs::srv::EncircleTraj>::SharedPtr encircle_traj_srv_;
 
    rclcpp_action::Server<Locomotion>::SharedPtr poly_action_server_;
    std::mutex action_goal_mutex_;
    std::shared_ptr<GoalHandleLocomotion> current_action_goal_;
 
    std::atomic<bool> shutdown_requested_{false};
 
    std::thread action_execute_thread_;
 
    OnSetParametersCallbackHandle::SharedPtr callback_handle_;
 
    Params config_;
 
    double prev_time_ = 0.0;
 
    rclcpp::CallbackGroup::SharedPtr odom_cb_group_, control_loop_cb_group_, services_cb_group_, actions_cb_group_,
        aux_cb_group_;
 
    void control_loop();
 
    void publish_controller_status(const Vehicle::ControllerStatus& status);
 
    void debug_publish_loop();
 
    rcl_interfaces::msg::SetParametersResult parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
 
    void declare_controller_params();
 
    void load_params_from_server();
 
    void apply_config_to_vehicle();
 
    void sub_callback_world_position(const nav_msgs::msg::Odometry::ConstSharedPtr& m);
 
    void sub_callback_battery_voltage(const bb_sensor_msgs::msg::BatteriesWithId::ConstSharedPtr& msg);
 
    void sub_callback_teleop_force(const geometry_msgs::msg::Twist::ConstSharedPtr& msg);
 
    void srv_callback_controller(const std::shared_ptr<bb_controls_msgs::srv::Controller::Request> req,
                                 std::shared_ptr<bb_controls_msgs::srv::Controller::Response> res);
 
    void srv_callback_trajectory_xyz(const std::shared_ptr<bb_controls_msgs::srv::SplineTraj::Request> req,
                                     std::shared_ptr<bb_controls_msgs::srv::SplineTraj::Response> res);
 
    void srv_callback_encircle_traj(const std::shared_ptr<bb_controls_msgs::srv::EncircleTraj::Request> req,
                                    std::shared_ptr<bb_controls_msgs::srv::EncircleTraj::Response> res);
 
    rclcpp_action::GoalResponse poly_handle_goal(const rclcpp_action::GoalUUID& uuid,
                                                 std::shared_ptr<const Locomotion::Goal> goal);
 
    rclcpp_action::CancelResponse poly_handle_cancel(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
 
    void poly_handle_accepted(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
 
    void poly_execute_action(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
};
 
#endif  // CONTROLS_NODES_CONTROLSROS2WRAPPER_H