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