【Mavros解析 】02 ROS服务以及在mavros中体现

• 标题 ： MAVROS教程—offboard模式下自主飞行
• 起源： 根据阿木实验室讲解课程整理的学习笔记，推荐大家学习无人机的去学习这个课程，俗话说的好：问渠那得清如许，唯有源头活水来 -
  
• MAVROS教程—offboard模式下自主飞行：https://bbs.amovlab.com/plugin.php?id=zhanmishu_video:video&mod=video&cid=11

简介

service是节点之间互相通信的另一种方式，
services允许节点发送一个请求并接收一个回应。
一个无人机进入 offboard 模式，并自动解锁飞行 2 米高的例子

实现步骤

以下代码已经进行了详细的注释，其中我们可以看到使用 ros 服务分为 3 个步骤，

步骤 1：连接服务

ros::ServiceClient arming_client = 
nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");

serviceClient 函数也是一个模板函数，模板参数为要连接的服务的服务参数类型，实际形参为服务名称。

该函数如果执行成功则返回一个服务对象 ros::ServiceClient

步骤 2：实例化服务参数：

mavros_msgs::CommandBool arm_cmd;

然后对参数进行赋值

arm_cmd.request.value = true;

步骤 3：请求服务

arming_client.call(arm_cmd)

请求的时候记得把服务参数传递进去，如果该函数返回 true 表示执行请求服务成功，但并不代表执行服务成功

• 我们还应该判断服务执行后返回的值，这里我们的例子为解锁那么，解锁服务返回的状态存储在 arm_cmd.response.success 中。
• 每个不同的服务的返回状态存储的变量都不一样，具体要看文档。
  注意，在 ros 中所有服务参数都在 xxx.request 中，所有返回状态都在 xxx.response

Demo

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
	//订阅 mavros/state 的回调函数
	mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
	 current_state = *msg;
}
int main(int argc, char **argv)
{
	//初始化
	 ros::init(argc, argv, "offb_node");
	 ros::NodeHandle nh;
	//订阅 mavros/state
	 ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
	 ("mavros/state", 10, state_cb);
//公告 mavros/setpoint_position/local
 ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
 ("mavros/setpoint_position/local", 10);
//连接服务 mavros/cmd/arming
 	ros::ServiceClient arming_client = 		nh.serviceClient<mavros_msgs::CommandBool>
 ("mavros/cmd/arming");
//连接服务 mavros/set_mode
	 ros::ServiceClient set_mode_client = 			nh.serviceClient<mavros_msgs::SetMode>
	 ("mavros/set_mode");
 //初始化循环频率对象
 ros::Rate rate(20.0);
//一直等待 current_state.connected 为 true，表示我们已经得到了 mavros/state 消息，飞控
已经成功连接 mavros
 while(ros::ok() && current_state.connected){
 ros::spinOnce();//执行消息回调
 rate.sleep();//睡眠保证频率
 }



//实例化一个 geometry_msgs::PoseStamped 类，用于发布，并把 Z 轴赋予 2 米
	 geometry_msgs::PoseStamped pose;
	 pose.pose.position.x = 0;
	 pose.pose.position.y = 0;
	 pose.pose.position.z = 2;
// 实 例 化 mavros_msgs::SetMode 类 ， offb_set_mode.request.custom_mode 赋 值 为
	"OFFBOARD"，表示我们要进入 OFFBOARD 模式
		//apm 固件也许为“GUILDED”
		 mavros_msgs::SetMode offb_set_mode;
	 offb_set_mode.request.custom_mode = "OFFBOARD";
//实例化 mavros_msgs::CommandBool，arm_cmd.request.value = true;表示我们要解锁，
=false 为加锁
 	mavros_msgs::CommandBool arm_cmd;
	 arm_cmd.request.value = true;
//得到当前系统时间
	 ros::Time last_request = ros::Time::now();
	 while(ros::ok()){
	 if( current_state.mode != "OFFBOARD" &&
	 (ros::Time::now() - last_request > ros::Duration(5.0))){
 //如果模式还不为 offboard，并且距离上一次执行进入 offboard 命令已经过去了 5
秒，进入这里
	 if( set_mode_client.call(offb_set_mode) &&
		 offb_set_mode.response.mode_sent){
		 ROS_INFO("Offboard enabled");//如果请求服务成功且服务返回执行成功
打印消息
 }
	 last_request = ros::Time::now();//更新最后一次请求服务时间
	 } else {
 //如果已经处于 offboard 模式进入这里
	 if( !current_state.armed &&
 (ros::Time::now() - last_request > ros::Duration(5.0))){
//如果还未解锁，并且距离上次此请求解锁已经过去 5 秒，进入这里
 if( arming_client.call(arm_cmd) &&
 arm_cmd.response.success){//如果请求服务成功且服务返回执行成
功打印消息
 ROS_INFO("Vehicle armed");
 }
 last_request = ros::Time::now();//更新最后一次请求解锁时间阿木实验室(AMOVLAB) 
 wwww.amovauto.com
3
 }
 }
 local_pos_pub.publish(pose);//发布位置消息
 ros::spinOnce();
 rate.sleep();
 }
 return 0;
}

头文件

除了ROS常用的头文件 外，
还有话题消息服务的头文件
CommandBool司令部
SetMode 设置模式
State 状态

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

订阅回调函数

订阅 mavros/state 的回调函数

一时间有点看不懂…

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
	 current_state = *msg;
}

主函数初始化

可以看到初始化的是node这个节点
节点句柄命名为 nh

//初始化
	ros::init(argc, argv, "offb_node");
	ros::NodeHandle nh;

订阅mavrso 状态

//订阅 mavros/state

 ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
 ("mavros/state", 10, state_cb);

公告

//公告 mavros/setpoint_position/local

 ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
 ("mavros/setpoint_position/local", 10);

连接服务

//连接服务 mavros/cmd/arming

 	ros::ServiceClient arming_client = 		nh.serviceClient<mavros_msgs::CommandBool>
 ("mavros/cmd/arming");

//连接服务 mavros/set_mode

 ros::ServiceClient set_mode_client = 
  nh.serviceClient<mavros_msgs::SetMode>
 ("mavros/set_mode");

初始化循环频率对象

//初始化循环频率对象

 ros::Rate rate(20.0);

等待飞控连接成功反馈

//一直等待 current_state.connected 为 true，表示我们已经得到了 mavros/state 消息，飞控
已经成功连接 mavros

 while(ros::ok() && current_state.connected){
 ros::spinOnce();//执行消息回调
 rate.sleep();//睡眠保证频率
 }

发布类 Z轴赋予2米

//实例化一个 geometry_msgs::PoseStamped 类，用于发布，并把 Z 轴赋予 2 米

```python
 geometry_msgs::PoseStamped pose;
 pose.pose.position.x = 0;
 pose.pose.position.y = 0;
 pose.pose.position.z = 2;

进入OFFOARD 模式

// 实 例 化 mavros_msgs::SetMode 类 ， offb_set_mode.request.custom_mode 赋 值 为
“OFFBOARD”，表示我们要进入 OFFBOARD 模式
//apm 固件也许为“GUILDED”

 mavros_msgs::SetMode offb_set_mode;
 offb_set_mode.request.custom_mode = "OFFBOARD";

解锁

//实例化 mavros_msgs::CommandBool，arm_cmd.request.value = true;
表示我们要解锁，
=false 为加锁

mavros_msgs::CommandBool arm_cmd;
 arm_cmd.request.value = true;

采集时间

//得到当前系统时间

 ros::Time last_request = ros::Time::now();
 while(ros::ok()){
 if( current_state.mode != "OFFBOARD" &&
 (ros::Time::now() - last_request > ros::Duration(5.0))){

超过5秒没有进入模式则重新请求服务

//如果模式还不为 offboard，并且距离上一次执行进入 offboard 命令已经过去了 5秒，进入这里

	 if( set_mode_client.call(offb_set_mode) &&
		 offb_set_mode.response.mode_sent){
		 ROS_INFO("Offboard enabled");
		 //如果请求服务成功且服务返回执行成功打印消息
		 }
	 last_request = ros::Time::now();//更新最后一次请求服务时间
	 } else {
 			//如果已经处于 offboard 模式进入这里
	 if( !current_state.armed &&
 (ros::Time::now() - last_request > ros::Duration(5.0))){
//如果还未解锁，并且距离上次此请求解锁已经过去 5 秒，进入这里
 if( arming_client.call(arm_cmd) &&
 arm_cmd.response.success){//如果请求服务成功且服务返回执行成
功打印消息

 ROS_INFO("Vehicle armed");}
														 
 last_request = ros::Time::now();//更新最后一次请求解锁时间 } }

发布位置信息

 local_pos_pub.publish(pose);//发布位置消息

消息回调保持频率

 ros::spinOnce(); 
 rate.sleep();