#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <std_msgs/Bool.h>
#include <cmath>
// 全局变量
ros::Publisher pos_raw_pub;
mavros_msgs::State current_state;
geometry_msgs::PoseStamped current_pose;
bool land_command_received = false;
geometry_msgs::Point target_position;
// 状态、位置、降落指令回调
void state_cb(const mavros_msgs::State::ConstPtr& msg) {
current_state = *msg;
}
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
current_pose = *msg;
}
void landCommandCallback(const std_msgs::Bool::ConstPtr& msg) {
land_command_received = msg->data;
}
void targetPointCallback(const geometry_msgs::Point::ConstPtr& msg) {
target_position = *msg; // 更新目标点
}
// 构造 PositionTarget 消息
mavros_msgs::PositionTarget makePositionTarget(
float x, float y, float z,
float vx, float vy, float vz,
float yaw,
uint16_t type_mask)
{
mavros_msgs::PositionTarget target;
target.header.stamp = ros::Time::now();
target.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
target.type_mask = type_mask;
target.position.x = x;
target.position.y = y;
target.position.z = z;
target.velocity.x = vx;
target.velocity.y = vy;
target.velocity.z = vz;
target.yaw = yaw;
return target;
}
// 判断是否到达目标
bool reached_target(double x, double y, double z, double threshold) {
double dx = current_pose.pose.position.x - x;
double dy = current_pose.pose.position.y - y;
double dz = current_pose.pose.position.z - z;
return (std::sqrt(dx*dx + dy*dy + dz*dz) < threshold);
}
int main(int argc, char **argv) {
ros::init(argc, argv, "offboard_node");
ros::NodeHandle nh;
// 订阅器
ros::Subscriber state_sub = nh.subscribe("mavros/state", 10, state_cb);
ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 100, pose_cb);
ros::Subscriber land_cmd_sub = nh.subscribe("land_cmd", 1, landCommandCallback);
ros::Subscriber target_cmd_sub = nh.subscribe("target_point", 1, targetPointCallback);
// 发布器
pos_raw_pub = nh.advertise<mavros_msgs::PositionTarget>(
"mavros/setpoint_raw/local", 100);
// 服务端
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>(
"mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>(
"mavros/set_mode");
ros::Rate rate(100.0);
ros::Rate rate_1(50.0);
// 等待连接
while (ros::ok() && !current_state.connected) {
ros::spinOnce();
rate.sleep();
}
// 起飞前发送一些 setpoint（惯例）
for (int i = 0; i < 100; ++i) {
pos_raw_pub.publish(makePositionTarget(
0, 0, 1.0,
0, 0, 0,
0,
mavros_msgs::PositionTarget::IGNORE_VX |
mavros_msgs::PositionTarget::IGNORE_VY |
mavros_msgs::PositionTarget::IGNORE_VZ |
mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW_RATE));
ros::spinOnce();
rate.sleep();
}
// 设置模式并解锁
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
// 等待进入 OFFBOARD 模式并解锁
while (ros::ok() && !current_state.armed) {
if (current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(1.0))) {
if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
ROS_INFO("Offboard mode enabled");
}
last_request = ros::Time::now();
} else if (!current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(1.0))) {
if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
// 上升速度控制起飞
pos_raw_pub.publish(makePositionTarget(
0, 0, 1.0,
0, 0, 0, // 向上速度
0,
mavros_msgs::PositionTarget::IGNORE_VX |
mavros_msgs::PositionTarget::IGNORE_VY |
mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW_RATE));
ros::spinOnce();
rate.sleep();
}
ROS_INFO("Takeoff in progress...");
// === 位置误差控制飞到目标点 ===
double target_x = 0.0, target_y = 0.0, target_z = 1.0;
target_position.x = target_x; target_position.y = target_y; target_position.z = target_z;
while (ros::ok() && !reached_target(target_x, target_y, target_z, 0.1)) {
pos_raw_pub.publish(makePositionTarget(
target_x, target_y, target_z,
0, 0, 0.3,
0,
mavros_msgs::PositionTarget::IGNORE_VX |
mavros_msgs::PositionTarget::IGNORE_VY |
mavros_msgs::PositionTarget::IGNORE_VZ |
mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW_RATE));
ros::spinOnce();
rate.sleep();
}
ROS_INFO("Reached target. Hovering...");
bool has_landed = false;
while (ros::ok() && !has_landed) {
// 继续 hover
/* pos_raw_pub.publish(makePositionTarget(
target_x, target_y, target_z,
0, 0, 0,
0,
mavros_msgs::PositionTarget::IGNORE_VX |
mavros_msgs::PositionTarget::IGNORE_VY |
mavros_msgs::PositionTarget::IGNORE_VZ |
mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW_RATE));
*/
if(reached_target(target_position.x, target_position.y, target_position.z, 0.1))
{
pos_raw_pub.publish(makePositionTarget(
target_position.x, target_position.y, target_position.z,
0, 0, 0,
0,
mavros_msgs::PositionTarget::IGNORE_VX |
mavros_msgs::PositionTarget::IGNORE_VY |
mavros_msgs::PositionTarget::IGNORE_VZ |
mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW_RATE));
}
else
{
pos_raw_pub.publish(makePositionTarget(
target_position.x, target_position.y, target_position.z,
2.0, 0, 0,
0,
mavros_msgs::PositionTarget::IGNORE_VX |
mavros_msgs::PositionTarget::IGNORE_VY |
mavros_msgs::PositionTarget::IGNORE_VZ |
mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY |
mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW_RATE));
}
// 收到降落指令
if (land_command_received) {
mavros_msgs::SetMode land_mode;
land_mode.request.custom_mode = "AUTO.LAND";
if (set_mode_client.call(land_mode) && land_mode.response.mode_sent) {
ROS_INFO("AUTO.LAND mode set");
}
// 等待自动降落并 disarm（监听 current_state.armed）
ros::Time land_start = ros::Time::now();
while (ros::ok()) {
ros::spinOnce();
// 降落过程中不再发送 setpoint，避免干扰
if (!current_state.armed) {
ROS_INFO("Landing complete. Vehicle disarmed.");
break;
}
// 可选：设置超时
if ((ros::Time::now() - land_start).toSec() > 20.0) {
ROS_WARN("Landing timeout. Attempting manual disarm...");
mavros_msgs::CommandBool disarm_cmd;
disarm_cmd.request.value = false;
if (arming_client.call(disarm_cmd) && disarm_cmd.response.success) {
ROS_INFO("Manual disarm succeeded.");
} else {
ROS_WARN("Manual disarm failed.");
}
break;
}
rate.sleep();
}
has_landed = true;
}
ros::spinOnce();
rate_1.sleep();
}
return 0;
}
请详细解释上述代码
最新发布
ROS中ros::spinOnce()的妙用
本文介绍在ROS系统中，ros::spinOnce()指令的实用性和优势。该指令能有效控制主题的发布频率，降低功耗，尤其适用于摄像头等高CPU占用设备，实现按需发布订阅，保持高性能的同时节省资源。
扫一扫
1331
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
