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);
}