Offboard Control 例程 (ROS C++)
本教程涵盖内容为:基于为Kerloud flying rover系列提供的C++ API,如何在ROS中编写用于offboard控制的节点。Offboard例程使飞车在陆地车、多旋翼模式下实现航路点任务。
Caution
📌 例程代码仅适于项目研究,不可直接用于产品。如果您在测试中发现错误,可随时在我们的官方资源库中创建pull请求。
1. 环境设置
本教程推荐的环境为Ubuntu18.04系统下ROS melodic,或者我们发行的树莓派model 3b+的ubuntu 16.04和ROS kinetic。现成可用的ROS工作区位于~/src/catkinws_offboard目录下,该工作区包含我们官方资源库中的软件包,在维护的资源库链接如下:
mavros (dev_flyingrover 分支): https://github.com/cloudkernel-tech/mavros
mavlink (dev_flyingrover 分支): https://github.com/cloudkernel-tech/mavlink-gdp-release
offboard control node (dev_flyingrover 分支): https://github.com/cloudkernel-tech/offboard_demo
用户可通过如下指令更新到最新版本:
cd ~/src/flyingrover_workspace/catkinws_offboard/src/mavros
git pull origin
git checkout dev_flyingrover
cd ~/src/flyingrover_workspace/catkinws_offboard/src/mavlink
git pull origin
git checkout dev_flyingrover
cd ~/src/flyingrover_workspace/catkinws_offboard/offboard_demo
git pull origin
git checkout dev_flyingrover
由于Kerloud产品软件在持续更新,如果用户在上述目录没有找到对应代码,可以通过以下指令创建工作区。
mkdir ~/src/flyingrover_workspace
mkdir ~/src/flyingrover_workspace/catkinws_offboard
cd ~/src/flyingrover_workspace/catkinws_offboard
catkin init
cd src
git clone --recursive https://github.com/cloudkernel-tech/mavros
cd mavros && git checkout dev_flyingrover
cd ..
git clone --recursive https://github.com/cloudkernel-tech/mavlink-gdp-release mavlink
cd mavlink && git checkout dev_flyingrover
cd ..
git clone --recursive https://github.com/cloudkernel-tech/offboard_demo
cd offboard demo && git checkout dev_flyingrover
编译工作区:
cd ~/src/flyingrover_workspace/catkinws_offboard/
catkin build -j 1 # set j=1 only for raspberry Pi
在树莓派电脑上,此编译过程耗时可能超过十分钟,首次操作时请耐心等待。
可使用如下指令清空工作区:
cd ~/src/flyingrover_workspace/catkinws_offboard/
catkin clean
2. 代码讲解
例程代码为标准ROS节点形式,用户需熟悉官方ROS教程中发布、订阅节点的编程方法,官方链接为:http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29。 重点指出,offboard_demo节点包含以下几个文件:
offboard_demo/launch/off_mission.launch: offboard控制节点的默认启动文件。
offboard_demo/launch/waypoints_xyzy.yaml: ENU坐标系下的航路点任务定义文件,包含相应的偏航信息。
offboard_demo/src/off_mission_node.cpp: 用于offboard控制的ROS节点文件。
本例程实现的任务描述如下:飞车将先在陆地车模式下走过几个航路点,到达最后一个航路点后切换为多旋翼模式,然后起飞依次走过之前相同的航路点。为清晰起见,我们在此按顺序讲解off_mission_node中的主要功能:
(1) 订阅、发布、服务的声明
我们通常在主程序开头处声明ROS的订阅、发布、服务。通过mavros包中丰富的话题或服务,可轻松实现信息获取或命令发送。例如,若要获取无人机的本地位置信息,则需订阅mavros/setpoint_position/local话题。请注意,所用坐标系为ENU(East-North-Up)。 Mavros软件包的标准信息可参见:http://wiki.ros.org/mavros。对于飞车专用的其他话题或服务,请参阅API部分。
/*subscriptions, publications and services*/
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 5, state_cb);
//subscription for flying rover state
ros::Subscriber extended_state_sub = nh.subscribe<mavros_msgs::ExtendedState>
("mavros/extended_state", 2, extendedstate_cb);
//subscription for local position
ros::Subscriber local_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
("mavros/local_position/pose", 5, local_pose_cb);
ros::Subscriber rc_input_sub = nh.subscribe<mavros_msgs::RCIn>
("mavros/rc/in", 5, rc_input_cb);
//publication for local position setpoint
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 5);
//service for arm/disarm
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
//service for main mode setting
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
//service for command send
ros::ServiceClient command_long_client = nh.serviceClient<mavros_msgs::CommandLong>
("mavros/cmd/command");
(2) 服务指令构建
接下来,我们将通过如下方式创建必要的命令来调用上文中所定义的服务。
/*service commands*/
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
mavros_msgs::CommandBool disarm_cmd;
disarm_cmd.request.value = false;
//flying rover mode switching commands
mavros_msgs::CommandLong switch_to_mc_cmd;//command to switch to multicopter
switch_to_mc_cmd.request.command = (uint16_t)mavlink::common::MAV_CMD::DO_FLYINGROVER_TRANSITION;
switch_to_mc_cmd.request.confirmation = 0;
switch_to_mc_cmd.request.param1 = (float)mavlink::common::MAV_FLYINGROVER_STATE::MC;
mavros_msgs::CommandLong switch_to_rover_cmd;//command to switch to rover
switch_to_rover_cmd.request.command = (uint16_t)mavlink::common::MAV_CMD::DO_FLYINGROVER_TRANSITION;
switch_to_rover_cmd.request.confirmation = 0;
switch_to_rover_cmd.request.param1 = (float)mavlink::common::MAV_FLYINGROVER_STATE::ROVER;
(3)陆地车、多旋翼模式下的航路点坐标解析
陆地车、多旋翼模式下的航路点坐标信息,会以不同的方式传递到位置设定话题。多旋翼模式下,需要用到3D位置,而且起飞过程需单独处理;陆地车模式下,则可直接将2D位置传递到所需的话题。
if (current_wpindex == 0){
//use yaw measurement during multicopter takeoff (relative height<1) to avoid rotation, and use relative pose
if (_flyingrover_mode == FLYINGROVER_MODE::MULTICOPTER){
//initialize for the 1st waypoint when offboard is triggered
if (!is_tko_inited_flag && current_state.mode=="OFFBOARD"){
//reload waypoint from yaml
initTagetVector(wp_list);
waypoints.at(0).pose.position.x += current_local_pos.pose.position.x; //set with relative position here
waypoints.at(0).pose.position.y += current_local_pos.pose.position.y;
waypoints.at(0).pose.position.z += current_local_pos.pose.position.z;
tf::Quaternion q = tf::createQuaternionFromYaw(current_yaw);//set with current yaw measurement
tf::quaternionTFToMsg(q, waypoints.at(0).pose.orientation);
is_tko_inited_flag = true;
}
//update yaw setpoint after tko is finished
if (is_tko_inited_flag && !is_tko_finished){
if (current_local_pos.pose.position.z >2.0f){ //reset yaw to wp_list value
ROS_INFO("Takeoff finished, reset to waypoint yaw");
XmlRpc::XmlRpcValue data_list(wp_list[0]);
tf::Quaternion q = tf::createQuaternionFromYaw(data_list[3]);
tf::quaternionTFToMsg(q, waypoints.at(0).pose.orientation);
is_tko_finished = true;
}
}
}
else //rover mode, pass relative update, use loaded waypoints
{
waypoints.at(0).pose.position.x += current_local_pos.pose.position.x; //set with relative position here
waypoints.at(0).pose.position.y += current_local_pos.pose.position.y;
waypoints.at(0).pose.position.z += current_local_pos.pose.position.z;
}
pose = waypoints.at(0);
}
else
pose = waypoints.at(current_wpindex);
(4)模式切换
本例程的最后一部分内容是处理飞车的模式切换。当飞车在陆地车模式下到达最后一个航路点时,程序将调用模式转换服务将飞车切换到多旋翼模式。一旦切换成功,航路点索引就会被清零,飞车将会起飞并沿相同的航路点飞行。
/*mode switching or disarm after last waypoint*/
if (current_wpindex == waypoints.size()-1 && _flag_last_wp_reached){
if (_flyingrover_mode == FLYINGROVER_MODE::ROVER){
//request to switch to multicopter mode
if( current_extendedstate.flyingrover_state== mavros_msgs::ExtendedState::FLYINGROVER_STATE_ROVER &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( command_long_client.call(switch_to_mc_cmd) && switch_to_mc_cmd.response.success){
ROS_INFO("Flyingrover multicopter mode cmd activated");
//update mode for next round
_flyingrover_mode = FLYINGROVER_MODE::MULTICOPTER;
current_wpindex = 0;
_flag_last_wp_reached = false;
}
last_request = ros::Time::now();
}
}
else if (_flyingrover_mode == FLYINGROVER_MODE::MULTICOPTER){
//disarm when landed and the vehicle is heading for the last waypoint
if (current_extendedstate.landed_state == mavros_msgs::ExtendedState::LANDED_STATE_LANDING){
if( current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( arming_client.call(disarm_cmd) && arm_cmd.response.success){
ROS_INFO("Vehicle disarmed");
is_tko_inited_flag = false;
is_tko_finished = false;
}
last_request = ros::Time::now();
}
}
}
}
3. 如何进行实测
现在可以将飞车拿到室外进行实测了。为避免意外,用户需严格按照如下流程操作:
将电池及线固定好,确保在飞行中不会发生脱落。
确保所有开关都处于初始位置,且油门通道已拉到最低。
电池上电。
等待GPS锁定:通常2-3分钟后我们可听到提示音,同时飞控上的LED会变为绿色。
确保飞手已就位,且飞车周围没有人。等待几分钟,机载电脑启动后可通过本地Wi-Fi网络进行远程登录,确认 waypoints_xyzy.yaml的航路点任务,然后通过如下指令启动offboard控制单元:
# launch a terminal cd ~/src/flyingrover_workspace/catkinws_offboard source devel/setup.bash roslaunch mavros px4.launch fcu_url:="/dev/ttyPixhawk:921600" # launch a new terminal cd ~/src/flyingrover_workspace/catkinws_offboard source devel/setup.bash roslaunch offboard_flyingrover off_mission.launch
拉低油门摇杆,同时右推偏航摇杆进行机器解锁,将听到一个长音提示。
使用特定的摇杆(例如通道7)切换到offboard模式,飞车将自动启动航路点任务。恭喜飞行成功!