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 <controls/controls_parameters.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 <thread>
 
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>;
 
protected:
    static constexpr int CONTROL_LOOP_RATE = 20;
 
    static constexpr double MIN_DEPTH_CLAMP = 0.00;
 
    static constexpr double MAX_VEL = 1.0;
 
    static constexpr double TWIST_LPF_RESET_DT = 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_;
 
    std::shared_ptr<controls::ParamListener> param_listener_;
 
    controls::Params config_;
 
    double prev_time_ = 0.0;
 
    Array6d twist_lpf_tau_ = Array6d::Zero();
 
    Vector6d twist_lpf_state_ = Vector6d::Zero();
 
    bool twist_lpf_init_ = false;
 
    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();
 
    void refresh_params_if_changed();
 
    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