Offboard Control 例程 (ROS C++)

本教程介绍如何使用为 Kerloud Autocar 系列提供的 C++ API 在 ROS 中编写一个offboard控制节点。offboard 例程可作为开发各种任务应用程序的基础。

Caution

例程代码仅适于项目研究，不可直接用于产品。如果您在测试中发现错误，可随时在我们的官方资源库中创建pull请求。

1. 环境设置

推荐环境为使用 Ubuntu 16.04 的 ROS kinetic，尽管本教程也可以在其他 ROS 版本中运行。

现成可用的ROS工作区位于 ~/src/rover_workspace/catkinws_offboard目录下。该工作区包含我们官方资源库中的软件包，在维护的资源库链接如下：

mavros（dev_rover 分支）：https://github.com/cloudkernel-tech/mavros
mavlink（dev_rover 分支）：https://github.com/cloudkernel-tech/mavlink-gdp-release
offboard demo node（dev_rover 分支）：https ://github.com/cloudkernel-tech/offboard_demo

要更新到最新版本，用户可以执行以下命令：

cd ~/src/rover_workspace/catkinws_offboard/mavros
git pull origin
git checkout dev_rover

cd ~/src/rover_workspace/catkinws_offboard/mavlink
git pull origin
git checkout dev_rover

cd ~/src/rover_workspace/catkinws_offboard/offboard_mission
git pull origin
git checkout dev_rover

编译工作区，

cd ~/src/catkinws_offboard
catkin build -j 1 # set j=1 only for raspberry Pi

在树莓派电脑上，此编译过程耗时可能超过十分钟，首次操作时请耐心等待。

可使用如下指令清空工作区：

cd ~/src/rover_workspace/catkinws_offboard/
catkin clean

2.代码说明

例程代码为标准ROS节点形式，用户需熟悉官方ROS教程中发布、订阅节点的编程方法，官方链接为：http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber(c%2B%2B)。

重点指出，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节点文件。

2.1 命令模式

车体可以在多种指令模式下运行：

位置模式：车体被指引前进到指定位置。
速度模式：车体以指定速度前进。
姿态模式：可以使用姿态设定点和所需的电机油门值操作车体，支持前进和后退。
执行器控制模式：车体可以通过直接执行器控制进行操作，包括转向伺服和电机油门输入，支持前进和后退。

2.2 程序说明

与上述指令模式相对应的offboard demo节点任务由四个阶段依次组成。关键在于利用 C++ API 中提供的各种主题或服务。

(1) 订阅、发布、服务的声明

我们通常在主程序的开头声明 ROS 订阅、发布和服务。

要获取状态信息或发送命令，可以轻松使用 mavros 包中的各种主题或服务。例如，要获取机体的本地位置信息，需要订阅mavros/setpoint_position/local 主题。请注意，所用坐标系为ENU（East-North-Up）。Mavros软件包的标准信息可参见：http://wiki.ros.org/mavros。

 /*subscriptions, publications and services*/
 ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 5, state_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);

//publication for local velocity setpoint
ros::Publisher local_vel_pub = nh.advertise<geometry_msgs::Twist>
        ("mavros/setpoint_velocity/cmd_vel", 5);

//publication for attitude setpoint (attitutde & thrust)
ros::Publisher att_sp_pub = nh.advertise<geometry_msgs::PoseStamped>
        ("mavros/setpoint_attitude/attitude", 5);

ros::Publisher thrust_sp_pub = nh.advertise<mavros_msgs::Thrust>
        ("mavros/setpoint_attitude/thrust", 5);

(2)服务指令构建

然后，我们构造必要的命令来调用这些已定义的服务，如下所示，包括解锁/锁定、主模式设置和前进/后退请求。

//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");

(3)位置引导阶段

在位置引导阶段，车体会加载waypoints_xyzy.yaml中定义的航点任务，然后依次通过这些航点。当车体到达某个点时，航点索引会更新。

/* publish local position setpoint for rover maneuvering*/
case ROVER_MISSION_PHASE::POS_PHASE:{

        ROS_INFO_ONCE("Starting position guidance phase");

        geometry_msgs::PoseStamped pose; //pose to be passed to fcu

        if (current_wpindex == 0){
            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);

        local_pos_pub.publish(pose);
        updateWaypointIndex();

    break;
}

(4))速度引导阶段

到达位置引导阶段的所有航路点后，车体将切换到速度引导阶段。速度设定值在 ENU 框架中定义，自动驾驶仪将通过将速度分解为前进速度和横向速度来处理车身框架中的速度设定值。前进速度设定值将用于在 PID 控制规律下计算所需的电机油门，并使用横向分量来推导转向角。

 /* publish local velocity setpoint for rover maneuvering*/
case ROVER_MISSION_PHASE::VEL_PHASE:{

        ROS_INFO_ONCE("Starting velocity guidance phase");

        //The local velocity setpoint is defined in the ENU frame, and will be converted to body frame in the autopilot for maneuvering
        geometry_msgs::Twist  vel_cmd;
        vel_cmd.linear.x = 0.25f;
        vel_cmd.linear.y = 0.25f;
        vel_cmd.linear.z = 0.0f;

        local_vel_pub.publish(vel_cmd);

        //phase transition after a certain time
        if ( ros::Time::now() - _phase_entry_timestamp > ros::Duration(6.0)){
            _mission_phase = ROVER_MISSION_PHASE::ATTITUDE_PHASE;
            _phase_entry_timestamp = ros::Time::now();
        }

        break;

}

(5)姿态控制阶段

在姿态控制阶段，用户可以构建具有所需偏航角的姿态设定点，并提供油门值。自动驾驶仪将通过转向舵机驱动来处理姿态跟踪。

 /* publish attitude setpoint for rover maneuvering*/
case ROVER_MISSION_PHASE::ATTITUDE_PHASE:{
        ROS_INFO_ONCE("Starting attitude maneuvering phase");

        //we need to construct both attitude and thrust setpoints
        //we construct desired attitude from yaw setpoint
        geometry_msgs::PoseStamped pose;

        tf::Quaternion q = tf::createQuaternionFromYaw(M_PI/2.0f); //yaw unit: radius

        tf::quaternionTFToMsg(q, pose.pose.orientation);

        att_sp_pub.publish(pose);

        mavros_msgs::Thrust thrust_sp;
        thrust_sp.thrust = 0.3f;
        thrust_sp_pub.publish(thrust_sp);

        //phase transition after a certain time
        if ( ros::Time::now() - _phase_entry_timestamp > ros::Duration(6.0)){
            _mission_phase = ROVER_MISSION_PHASE::ACT_CONTROL_PHASE;
            _phase_entry_timestamp = ros::Time::now();
        }

    break;
}

(6)执行器控制阶段

在这个阶段，用户可以直接控制底层执行器（转向舵机和电机）来完成更复杂的任务，如路径规划、轨迹跟踪。

 /* publish direct actuator control for rover maneuvering*/
case ROVER_MISSION_PHASE::ACT_CONTROL_PHASE:{
        ROS_INFO_ONCE("Starting direct actuator control phase");

        mavros_msgs::ActuatorControl act_control;

        act_control.group_mix = 0;
        act_control.flag_rover_mode = 1;//enable direct actuator control in rover
        act_control.controls[mavros_msgs::ActuatorControl::ROVER_YAW_CHANNEL_CONTROL_INDEX] = 0.4f;
        act_control.controls[mavros_msgs::ActuatorControl::ROVER_THROTTLE_CHANNEL_CONTROL_INDEX] = 0.3f;

        act_controls_pub.publish(act_control);

        //we switch to backward driving after 5s
        if ( ros::Time::now() - _phase_entry_timestamp > ros::Duration(5.0)){

            if (flag_forward_driving && (ros::Time::now() - last_request > ros::Duration(1.0))){

                if( command_long_client.call(set_backward_driving_cmd) && set_backward_driving_cmd.response.success){

                    ROS_INFO("Driving state switching cmd sent");

                    flag_forward_driving = false;

                }

                last_request = ros::Time::now();

            }


        }

2.3 如何实测

现在是时候进行真机实测了。由于 Autocar 主要用于室内应用，因此在实践此offboard控制教程之前应先启动定位模块。例如，我们必须为 Autocar Laser 启动激光定位算法。然后，用户就可以按照如下命令来避免意外行为：

确保遥控器的所有开关都处于初始位置，并且油门拉至低位。
电池上电开机。
机载电脑启动过程需等待几分钟，通过本地Wi-Fi远程登录，确认 waypoints_xyzy.yaml 中定义的航点任务。
启动定位模块（如laser slam），并通过rostopic 命令验证位置输出。
运行如下指令以启动offboard单元：

# launch a terminal
cd ~/src/rover_workspace/catkinws_offboard
source devel/setup.bash
roslaunch mavros px4.launch fcu_url:="/dev/ttyPixhawk:921600"

# launch a new terminal
cd ~/src/rover_workspace/catkinws_offboard
source devel/setup.bash
roslaunch off_mission off_mission.launch

通过拉低油门并右推偏航杆实现车体解锁，然后我们会听到机器发出长长的哔哔声。
使用指定的摇杆（例如通道 7）切换到 OFFBOARD 模式，然后机器将自动开始任务。运行成功 !