/mavros/setpoint_velocity/ c++ 怎么使用

/mavros/setpoint_velocity/是ROS中与Mavros飞控通信的一个节点，通过该节点可以向飞控发送速度控制指令。下面是使用C++编写/mavros/setpoint_velocity/节点的基本步骤：

1. 导入必要的头文件

#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/GlobalPositionTarget.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>

2. 定义全局变量和ROS回调函数

// 定义全局变量
mavros_msgs::State current_state;
geometry_msgs::TwistStamped vel_cmd;

// 定义回调函数
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
    current_state = *msg;
}

void vel_cmd_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    vel_cmd = *msg;
}

3. 初始化节点和订阅话题

int main(int argc, char **argv)
{
    ros::init(argc, argv, "setpoint_velocity_node");
    ros::NodeHandle nh;

    // 订阅当前飞控状态和速度指令
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
    ros::Subscriber vel_cmd_sub = nh.subscribe<geometry_msgs::TwistStamped>("mavros/setpoint_velocity/cmd_vel", 10, vel_cmd_cb);

    // 创建发布器
    ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>("mavros/setpoint_velocity/cmd_vel", 10);

    // 设置循环频率
    ros::Rate rate(20.0);
}

4. 控制飞行器

在ROS的循环中，根据当前状态和速度指令控制飞行器。

while (ros::ok())
{
    // 判断当前状态是否可以控制飞行器
    if (current_state.mode == "OFFBOARD" && current_state.armed)
    {
        // 发布速度指令
        vel_pub.publish(vel_cmd);
    }

    // 循环等待
    ros::spinOnce();
    rate.sleep();
}

完整的/mavros/setpoint_velocity/节点代码如下：

#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/GlobalPositionTarget.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>

// 定义全局变量
mavros_msgs::State current_state;
geometry_msgs::TwistStamped vel_cmd;

// 定义回调函数
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
    current_state = *msg;
}

void vel_cmd_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    vel_cmd = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "setpoint_velocity_node");
    ros::NodeHandle nh;

    // 订阅当前飞控状态和速度指令
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
    ros::Subscriber vel_cmd_sub = nh.subscribe<geometry_msgs::TwistStamped>("mavros/setpoint_velocity/cmd_vel", 10, vel_cmd_cb);

    // 创建发布器
    ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>("mavros/setpoint_velocity/cmd_vel", 10);

    // 设置循环频率
    ros::Rate rate(20.0);

    while (ros::ok())
    {
        // 判断当前状态是否可以控制飞行器
        if (current_state.mode == "OFFBOARD" && current_state.armed)
        {
            // 发布速度指令
            vel_pub.publish(vel_cmd);
        }

        // 循环等待
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}