# MavROS thruster contorl

**URL:** <https://discuss.bluerobotics.com/t/mavros-thruster-contorl/17912>\
**Category:** General Discussion\
**Tags:** ardusub, simulate\
**Created:** [September 6, 2024, 5:56pm UTC](https://discuss.bluerobotics.com/t/mavros-thruster-contorl/17912 "2024-09-06T17:56:29Z")\
**Posts on this page:** 1\
**Page:** 1

<div class="post-metadata">

**Author:** ![Youssefattia\_98](https://sea1.discourse-cdn.com/flex019/user_avatar/discuss.bluerobotics.com/youssefattia_98/32/6650_2.png) [@Youssefattia\_98](https://discuss.bluerobotics.com/u/Youssefattia_98)\
**Post date:** [September 6, 2024, 5:56pm UTC](https://discuss.bluerobotics.com/t/mavros-thruster-contorl/17912/1 "2024-09-06T17:56:29Z")

</div>

Hello everyone, I am trying to control the thrusters as low level using MavROS.

I am running sofware in the loop:  
sim\_vehicle.py -v ArduSub -L RATBeach --model=Vectored\_6DOF --out=udp:0.0.0.0:14550 --console --map

and MavROS as:  
ros2 run mavros mavros\_node --ros-args --params-file /home/usflinux/ros2\_ws/src/x300\_core\_ros2/x300\_ctrl/param/mavros\_config.yaml

where my yamal file is:

# mavros\_param.yaml

mavros:  
ros\_\_parameters: {}

mavros\_router:  
ros\_\_parameters: {}

mavros\_node:  
ros\_\_parameters:  
fcu\_url: udp://0.0.0.0:14550@0.0.0.0:14550 # Match your SITL connection settings  
target\_system\_id: 1  
target\_component\_id: 1  
system\_id: 255  
component\_id: 1  
protocol\_version: “2.0” # Specify MAVLink 2.0 for compatibility  
# rc\_channels\_override\_timeout: -1 # Disable timeout for RC overrides  
# param\_pull\_timeout: 30.0 # Increase timeout to 30 seconds (default might be shorter)  
# param\_pull\_retries: 5 # Increase the number of retries

/\*\*:  
ros\_\_parameters:  
plugin\_denylist: [odometry, distance\_sensor]

I make sure to arm the vechile using:  
ros2 service call /mavros/cmd/arming mavros\_msgs/srv/CommandBool “{value: true}” or using my cpp code

and I am sure it is armed using:  
ros2 topic echo /mavros/state

and then I set full thrust for tesing as:  
ros2 topic pub /mavros/rc/override mavros\_msgs/msg/OverrideRCIn “channels: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]”

However, the vechile do not move and checking check RC:  
ros2 topic echo /mavros/rc/out outputs:  
header:  
stamp:  
sec: 1725645352  
nanosec: 885560912  
frame\_id: ‘’  
channels:

- 1500
- 1500
- 1500
- 1500
- 1500
- 1500
- 1500
- 1500
- 0
- 1500
- 0
- 0
- 0
- 0
- 0
- 0

* * *

is there any missing setp I have. Also check out my ros2 cpp node here:  
#include “x300\_ctrl/ardusub\_bridge\_class.hpp”

const double EARTH\_RADIUS = 6378137.0;  
const double FLATTENING = 1.0 / 298.257223563;  
const double ATMOSPHERIC\_PRESSURE = 101325.0;  
const double WATER\_DENSITY = 1025.0;  
const double GRAVITY = 9.80665;

ArduSubBridge::ArduSubBridge() : Node(“auv\_control\_node”), fluid\_pressure\_(0.0), forces\_desired\_(8) {  
auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).best\_effort().durability\_volatile();

```
// Subscriptions
imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
    "/mavros/imu/data", qos, std::bind(&ArduSubBridge::imu_callback, this, _1));

pressure_sub_ = this->create_subscription<sensor_msgs::msg::FluidPressure>(
    "/mavros/imu/static_pressure", qos, std::bind(&ArduSubBridge::pressure_callback, this, _1));

gps_sub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
    "/mavros/global_position/global", qos, std::bind(&ArduSubBridge::gps_callback, this, _1));

forces_desired_subscription_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
    x300_msgs::topicnames::forces_desired, 1, std::bind(&ArduSubBridge::forces_desired_callback, this, _1));

kcl_state_subscription_ = this->create_subscription<std_msgs::msg::String>(
    x300_msgs::topicnames::kcl_state, 1, std::bind(&ArduSubBridge::kcl_state_callback, this, _1));

// Publisher for thruster control
actuator_control_pub_ = this->create_publisher<mavros_msgs::msg::ActuatorControl>("/mavros/actuator_control", 10);

// Timer for periodic loop execution
timer_ = this->create_wall_timer(100ms, std::bind(&ArduSubBridge::loop, this));

// Arming client initialization
arming_client_ = this->create_client<mavros_msgs::srv::CommandBool>("/mavros/cmd/arming");

// Arm the vehicle upon initialization
auto request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
request->value = true;
auto result = arming_client_->async_send_request(request);

// Default KCL state
KCL_current_state_ = "IDLE";

```

}

void ArduSubBridge::imu\_callback(const sensor\_msgs::msg::Imu::SharedPtr msg) {  
imu\_orientation\_ = Eigen::Quaterniond(msg-\>orientation.w, msg-\>orientation.x, msg-\>orientation.y, msg-\>orientation.z);  
}

void ArduSubBridge::pressure\_callback(const sensor\_msgs::msg::FluidPressure::SharedPtr msg) {  
fluid\_pressure\_ = msg-\>fluid\_pressure;  
}

void ArduSubBridge::gps\_callback(const sensor\_msgs::msg::NavSatFix::SharedPtr msg) {  
gps\_position\_ = Eigen::Vector3d(msg-\>latitude, msg-\>longitude, msg-\>altitude);  
}

void ArduSubBridge::forces\_desired\_callback(const std\_msgs::msg::Float64MultiArray::SharedPtr msg) {  
forces\_desired\_.resize(msg-\>data.size());  
for (size\_t i = 0; i \< msg-\>data.size(); ++i) {  
forces\_desired\_(i) = msg-\>data[i];  
}  
}

void ArduSubBridge::kcl\_state\_callback(const std\_msgs::msg::String::SharedPtr msg) {  
KCL\_current\_state\_ = msg-\>data;  
}

Eigen::Vector3d ArduSubBridge::get\_rpy\_from\_quaternion(const Eigen::Quaterniond& q) {  
return q.toRotationMatrix().eulerAngles(0, 1, 2);  
}

Eigen::Vector3d ArduSubBridge::convert\_gps\_to\_ecef(double lat, double lon, double alt) {  
lat = lat \* M\_PI / 180.0;  
lon = lon \* M\_PI / 180.0;

```
const double a = EARTH_RADIUS;
const double f = FLATTENING;
const double e2 = 2.0 * f - f * f;
const double N = a / sqrt(1.0 - e2 * sin(lat) * sin(lat));

double x = (N + alt) * cos(lat) * cos(lon);
double y = (N + alt) * cos(lat) * sin(lon);
double z = (N * (1.0 - e2) + alt) * sin(lat);

return Eigen::Vector3d(x, y, z);

```

}

double ArduSubBridge::calculate\_depth(double pressure) {  
return (pressure - ATMOSPHERIC\_PRESSURE) / (WATER\_DENSITY \* GRAVITY);  
}

void ArduSubBridge::loop() {  
std::cout \<\< "KCL state: " \<\< KCL\_current\_state\_ \<\< std::endl;

```
if (KCL_current_state_ == "IDLE") {
    forces_desired_.setZero();
} else {
    forces_desired_ = forces_desired_.unaryExpr([](double x) {
        return (x >= 0) ? (x / 70.0) : (x / 60.0);
    });
}

//map forces desired to pwm values 1100 1500 1900
forces_desired_ = forces_desired_.unaryExpr([](double x) {
        return (x >= 0) ? (x * 400 + 1500) : (x * 400 + 1500);
    });

std::cout << "Desired forces: " << forces_desired_.transpose() << std::endl;

// Publish the thruster control commands
mavros_msgs::msg::ActuatorControl control_msg;
control_msg.group_mix = 0; // Group for thruster control
for (int i = 0; i < std::min(8, static_cast<int>(forces_desired_.size())); ++i) {
    control_msg.controls[i] = static_cast<float>(forces_desired_(i));
}

actuator_control_pub_->publish(control_msg);

```

}