sas
Modularised monitoring, logging, and control of robots.
Loading...
Searching...
No Matches
sas_robot_driver_brov2.hpp
Go to the documentation of this file.
1#pragma once
2/*
3# Copyright (c) 2024-2026 Rawan Mohamed Elsayed and Murilo Marques Marinho
4#
5# This file is part of sas_robot_driver_brov2.
6#
7# sas_robot_driver_brov2 is free software: you can redistribute it and/or modify
8# it under the terms of the GNU Lesser General Public License as published by
9# the Free Software Foundation, either version 2.1 of the License, or
10# (at your option) any later version.
11#
12# sas_robot_driver_brov2 is distributed in the hope that it will be useful,
13# but WITHOUT ANY WARRANTY; without even the implied warranty of
14# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15# GNU Lesser General Public License for more details.
16#
17# You should have received a copy of the GNU Lesser General Public License
18# along with sas_robot_driver_brov2. If not, see <https://www.gnu.org/licenses/>.
19#
20# ################################################################
21#
22# Author: Rawan M. Elsayed, email: rawan.elsayed@manchester.ac.uk
23# Co-author: Murilo M. Marinho, email: murilomarinho@ieee.org
24#
25# ################################################################*/
26
27#include <atomic>
28#include <memory>
29#include <sas_core/sas_robot_driver.hpp>
30#include <rclcpp/rclcpp.hpp>
31#include <mavros_msgs/msg/override_rc_in.hpp>
32#include <realtime_tools/realtime_publisher.hpp>
33
38
39namespace sas
40{
41
46{
47 int channel;
50
51 double f_min;
52 double f_max;
53 double min_pwm;
54 double max_pwm;
55 double deadband_pwm;
56
58};
59
64{
65 std::vector<ThrusterConfig> thruster_configs;
66 std::vector<double> fwd_thrust_curve_coeffs;
67 std::vector<double> rev_thrust_curve_coeffs;
68
69 std::string rc_override_topic{"mavros/rc/override"};
70 std::string node_name_prefix{"sas_thruster_node"};
71 std::string param_service_name{"/mavros/param/set_parameters"};
72 std::string sim_batt_voltage_param{"SIM_BATT_VOLTAGE"};
73
74 double v_bus;
75
76 Eigen::MatrixXd B;
77};
78
84class RobotDriverBrov2 : public RobotDriver
85{
86private:
87 RobotDriverBrov2Configuration configuration_;
88
89 //Use the Impl idiom to "hide" internal driver sources
90 class Impl;
91 std::unique_ptr<Impl> impl_;
92
93public:
94 // Prevent copies as usually drivers have threads
95 RobotDriverBrov2(const RobotDriverBrov2&) = delete;
96 RobotDriverBrov2() = delete;
97 ~RobotDriverBrov2();
98
104 RobotDriverBrov2(const RobotDriverBrov2Configuration& configuration,
105 const std::shared_ptr<marinholab::sas::core::ShutdownSignaler>& shutdown_signaler);
106
112 Eigen::VectorXd get_joint_positions() override;
113
118 void set_target_joint_positions(const Eigen::VectorXd& q) override;
119
125 Eigen::VectorXd get_joint_torques() override;
126
132 void set_target_joint_torques(const Eigen::VectorXd& tau) override;
133
135 void connect() override;
137 void disconnect() override;
138
140 void initialize() override;
142 void deinitialize() override;
143};
144} // namespace sas
Definition sas_robot_driver_brov2.cpp:36
void disconnect() override
Release the ROS resources created in connect().
Definition sas_robot_driver_brov2.cpp:362
Eigen::VectorXd get_joint_torques() override
Get the last commanded wrench.
Definition sas_robot_driver_brov2.cpp:99
void initialize() override
Configure the autopilot servo parameters and send neutral commands.
Definition sas_robot_driver_brov2.cpp:231
Eigen::VectorXd get_joint_positions() override
Get the last commanded PWM vector (there is no sensor feedback).
Definition sas_robot_driver_brov2.cpp:75
void deinitialize() override
Return the thrusters to neutral.
Definition sas_robot_driver_brov2.cpp:327
void set_target_joint_torques(const Eigen::VectorXd &tau) override
Command a 6D wrench, which is allocated to the thrusters and sent as PWM.
Definition sas_robot_driver_brov2.cpp:121
void connect() override
Create the ROS node and the RC override publisher.
Definition sas_robot_driver_brov2.cpp:178
void set_target_joint_positions(const Eigen::VectorXd &q) override
Not supported.
Definition sas_robot_driver_brov2.cpp:87
Configuration of the RobotDriverBrov2.
Definition sas_robot_driver_brov2.hpp:64
std::string param_service_name
MAVROS parameter service.
Definition sas_robot_driver_brov2.hpp:71
std::string sim_batt_voltage_param
Simulated battery voltage parameter.
Definition sas_robot_driver_brov2.hpp:72
Eigen::MatrixXd B
Thruster allocation matrix (6 x number of thrusters).
Definition sas_robot_driver_brov2.hpp:76
double v_bus
Bus (battery) voltage.
Definition sas_robot_driver_brov2.hpp:74
std::string rc_override_topic
Topic the RC override is published to.
Definition sas_robot_driver_brov2.hpp:69
std::vector< double > rev_thrust_curve_coeffs
Reverse thrust curve coefficients.
Definition sas_robot_driver_brov2.hpp:67
std::vector< double > fwd_thrust_curve_coeffs
Forward thrust curve coefficients.
Definition sas_robot_driver_brov2.hpp:66
std::vector< ThrusterConfig > thruster_configs
One ThrusterConfig per thruster.
Definition sas_robot_driver_brov2.hpp:65
std::string node_name_prefix
Prefix of the internal node name.
Definition sas_robot_driver_brov2.hpp:70
Configuration of a single thruster.
Definition sas_robot_driver_brov2.hpp:46
double min_pwm
Min allowed PWM.
Definition sas_robot_driver_brov2.hpp:53
std::string servo_function_param
Autopilot parameter name, e.g. "SERVO1_FUNCTION".
Definition sas_robot_driver_brov2.hpp:57
double f_max
Max allowed thruster force.
Definition sas_robot_driver_brov2.hpp:52
double f_min
Min allowed thruster force.
Definition sas_robot_driver_brov2.hpp:51
int servo_function_values
Servo function value (1 is passthrough mode).
Definition sas_robot_driver_brov2.hpp:49
double max_pwm
Max allowed PWM.
Definition sas_robot_driver_brov2.hpp:54
int channel
RC channel index (1-based).
Definition sas_robot_driver_brov2.hpp:47
double deadband_pwm
PWM deadband.
Definition sas_robot_driver_brov2.hpp:55
int neutral_pwm
PWM to send when stopped.
Definition sas_robot_driver_brov2.hpp:48