I want to interface Ros2 with Ardupilot. Now the 2 ways I have fount out are 1) Using DDS for communication where no need to use mavros 2) Using Mavros2.
The issue is Mavros is evolved and has a lot of topics to play with, and using directly dds doesnt has a lot of functionality and also interface. Now, what do you think i should do?? Also, i wasnt able to see any documentation for using Ardupilot with Mavros2. The official documentation mentions more about Ros and Mavros.
You should use ArduCopter 4.6.0-dev and DDS
Thanks for the reply, by any chance do you know how to publish velocity to the /cmd_vel topic published by ardupilot. Right now i am subscribing to topic and publishing some velocity to it, but its not working. Also if you have any template that would really help.
Thanks Already!
This is code i was using:
#include <rclcpp/rclcpp.hpp>
#include <ardupilot_msgs/srv/arm_motors.hpp>
#include <ardupilot_msgs/srv/mode_switch.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <chrono>
#include <thread>
#include <memory>
using namespace std::chrono_literals;
class DroneController : public rclcpp::Node
{
public:
DroneController() : Node("drone_controller"), manual_control_(false), move_forward_(false)
{
// Initialize subscribers
cmd_vel_subscription_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
"/ap/cmd_vel", 10, std::bind(&DroneController::cmd_vel_callback, this, std::placeholders::_1));
odom_subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
"/ap/pose/filtered", 10, std::bind(&DroneController::odom_callback, this, std::placeholders::_1));
RCLCPP_INFO(this->get_logger(), "DroneController initialized");
}
void arm()
{
auto client = this->create_client<ardupilot_msgs::srv::ArmMotors>("/ap/arm_motors");
if (!client->wait_for_service(5s)) {
RCLCPP_ERROR(this->get_logger(), "Arm service not available");
return;
}
auto request = std::make_shared<ardupilot_msgs::srv::ArmMotors::Request>();
request->arm = true;
RCLCPP_INFO(this->get_logger(), "Sending arm command");
auto result = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_INFO(this->get_logger(), "Arm command result: %s", result.get()->result ? "Success" : "Failed");
} else {
RCLCPP_ERROR(this->get_logger(), "Failed to call arm service");
}
}
void set_guided_mode()
{
auto client = this->create_client<ardupilot_msgs::srv::ModeSwitch>("/ap/mode_switch");
if (!client->wait_for_service(5s)) {
RCLCPP_ERROR(this->get_logger(), "Mode switch service not available");
return;
}
auto request = std::make_shared<ardupilot_msgs::srv::ModeSwitch::Request>();
request->mode = 4; // Assuming 4 is GUIDED mode
RCLCPP_INFO(this->get_logger(), "Sending set GUIDED mode command");
auto result = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_INFO(this->get_logger(), "Set GUIDED mode result: %s", result.get()->status ? "Success" : "Failed");
} else {
RCLCPP_ERROR(this->get_logger(), "Failed to call set mode service");
}
}
void start_moving_forward()
{
manual_control_ = true;
move_forward_ = true;
RCLCPP_INFO(this->get_logger(), "Starting to move drone forward");
}
void stop_moving()
{
move_forward_ = false;
manual_control_ = false;
RCLCPP_INFO(this->get_logger(), "Stopped drone movement");
}
private:
void cmd_vel_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
{
if (manual_control_) {
// Modify the incoming message
if (move_forward_) {
msg->twist.linear.x = 0.5; // Move forward at 0.5 m/s
} else {
msg->twist.linear.x = 0.0; // Stop moving
}
msg->twist.linear.y = 0.0;
msg->twist.linear.z = 0.0;
msg->twist.angular.x = 0.0;
msg->twist.angular.y = 0.0;
msg->twist.angular.z = 0.0;
// Publish the modified message back to the same topic
cmd_vel_subscription_->publish(*msg);
RCLCPP_INFO(this->get_logger(), "Published velocity command: linear.x = %.2f, linear.y = %.2f, linear.z = %.2f",
msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z);
} else {
RCLCPP_INFO(this->get_logger(), "Received cmd_vel: linear.x = %.2f, linear.y = %.2f, linear.z = %.2f",
msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z);
}
}
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
RCLCPP_INFO(this->get_logger(), "Drone position: x=%.2f, y=%.2f, z=%.2f",
msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
}
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_subscription_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscription_;
bool manual_control_;
bool move_forward_;
};
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<DroneController>();
node->arm();
node->set_guided_mode();
std::this_thread::sleep_for(2s); // Wait for mode change to take effect
node->start_moving_forward();
std::this_thread::sleep_for(10s); // Move forward for 10 seconds
node->stop_moving();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
This is my previous journey, not sure is it of any help to you. Nothing great, just sharing my hiccups hopefully other can benefit.
Thanks! btw were you able to successfully publish message on topics published by ardupilot such as /ap/cmd_vel or /ap/pose/filtered. I was able to arm the drone as well as change the mode to guided, but i am not able to move the drone with my ros2 script.
Back then, didn’t have the bandwidth to explore further.
Preempt to do other tasks. Also some warnings and error from motivating me to go further.
ohh okay.
@amilcarlucas your help will really mean a lot.
Post Questions on the ArduPilot Discord channel. There is a ros section.