.. _tutorial_offboard_cplus: Offboard Control in ROS using C++ API ============================================= This tutorial covers how to write an offboard control node in ROS with the provided C++ API for the Kerloud Autocar series. The offboard example can serve as a basis to develop applications for various tasks. .. caution:: The tutorial code is for research project only, and cannot be deployed directly on a product. Feel free to create a pull request in our official repositories if you find a bug during test. 1. Environment Setup -------------------------- The recommended environment is ROS kinetic with Ubuntu 16.04, although this tutorial can run in other ROS versions. The ready-to-use ROS workspace is under the directory ~/src/rover_workspace/catkinws_offboard. The workspace contains the follow packages maintained in our official repositories listed below: * mavros (dev_rover branch): https://github.com/cloudkernel-tech/mavros * mavlink (dev_rover branch): https://github.com/cloudkernel-tech/mavlink-gdp-release * offboard demo node (dev_rover branch): https://github.com/cloudkernel-tech/offboard_demo To update to the latest version, users can conduct the following commands: :: 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 To build the workspace, :: cd ~/src/catkinws_offboard catkin build -j 1 # set j=1 only for raspberry Pi It can take up to 10 minutes for the build process to finish in a raspberry Pi computer, so be patient for the first time. To clean the workspace, use the command: :: cd ~/src/rover_workspace/catkinws_offboard/ catkin clean 2. Code Explanation ---------------------------- The code example is in a standard ROS node form, and users have to familiarize with the official ROS tutorial on writing publisher and subscriber (http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29). To highlight, the offboard demo node contains several files: * offboard_demo/launch/off_mission.launch: a default launch file to launch the offboard control node. * offboard_demo/launch/waypoints_xyzy.yaml: waypoint mission definition file with ENU coordinates and corresponding yaw values. * offboard_demo/src/off_mission_node.cpp: the ROS node file for offboard control 2.1 Commanding Modes ^^^^^^^^^^^^^^^^^^^^^^^ The rover can be operated in several commanding modes: * **Position Mode**: The rover can be guided towards a position setpoint in forward driving. * **Velocity Mode**: The rover can follow a velocity setpoint in forward driving. * **Attitude Mode**: The rover can be operated with an attitude setpoint and a desired motor throttle value. Both forward and backward driving are supported. * **Actuator Control Mode**: The rover can be operated with direct actuator controls including steering servo and motor throttle inputs. Both forward and backward driving are supported. 2.2 Program Clarification ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ The mission for the offboard demo node consists of four phases corresponding to above commanding modes in sequence. The key is to utilize various topics or services provided in the C++ API. **(1) Declarations of subscriptions, publications and services** We usually declare ROS subscriptions, publications and services at the beginning of a main program. To obtain the state information or send commands, various topics or services from the mavros package can be easily utilized. For example, to obtain the local position information for the UAV, the mavros/setpoint_position/local topic should be subscribed. Kindly note that the coordinates are defined in the ENU (East-North-Up) frame. The standard information for the mavros package can be referred in http://wiki.ros.org/mavros. :: /*subscriptions, publications and services*/ ros::Subscriber state_sub = nh.subscribe ("mavros/state", 5, state_cb); //subscription for local position ros::Subscriber local_pose_sub = nh.subscribe ("mavros/local_position/pose", 5, local_pose_cb); ros::Subscriber rc_input_sub = nh.subscribe ("mavros/rc/in", 5, rc_input_cb); //publication for local position setpoint ros::Publisher local_pos_pub = nh.advertise ("mavros/setpoint_position/local", 5); //publication for local velocity setpoint ros::Publisher local_vel_pub = nh.advertise ("mavros/setpoint_velocity/cmd_vel", 5); //publication for attitude setpoint (attitutde & thrust) ros::Publisher att_sp_pub = nh.advertise ("mavros/setpoint_attitude/attitude", 5); ros::Publisher thrust_sp_pub = nh.advertise ("mavros/setpoint_attitude/thrust", 5); **(2) Service command construction** We then construct necessary commands to call those defined services as below, including arm/disarm, main mode setting, and forward/backward driving request. :: //service for arm/disarm ros::ServiceClient arming_client = nh.serviceClient ("mavros/cmd/arming"); //service for main mode setting ros::ServiceClient set_mode_client = nh.serviceClient ("mavros/set_mode"); //service for command send ros::ServiceClient command_long_client = nh.serviceClient ("mavros/cmd/command"); **(3) Position guidance phase** In the position guidance phase, the rover will load the waypoint mission defined in waypoints_xyzy.yaml, and then go through these waypoints sequentially. Waypoint index is updated when the rover reaches a certain point. :: /* 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) Velocity guidance phase** After all waypoints in the position guidance phases are reached, the rover will switch to the velocity guidance phase. The velocity setpoint is defined in the ENU frame, and the autopilot will handle the velocity setpoint in the body frame by decomposing the velocity into a forward speed and a lateral speed. The forward speed setpoint will be used to calculate the desired motor throttle under a PID control law, and the lateral component is employed to derive the steering angle. :: /* 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) Attitude control phase** In the attitude control phase, users can construct a attitude setpoint with a desired yaw angle, and provide a throttle value. The autopilot will handle the attitude tracking with steering servo actuation. :: /* 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) Actuator control phase** In this phase, users can directly control the low level actuators (steering servo and motor) to achieve more sophisticated tasks like path planning, trajectory tracking. :: /* 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(); } } 3. How to Run for Real Tests ------------------------------- Now it's time to roll out for real tests. Since the Autocar is mainly for indoor applications, it's a prerequisite to launch localization modules before this offboard control tutorial. For instance, we have to launch the laser localization algorithm for the Autocar Laser. Users then can follow orders below to avoid unexpected behaviors: * Make sure that all switches in the transmitter are in their initial position and the throttle channel is lowered down. * Power on with the battery. * Wait a few minutes for the onboard computer to boot, log into it remotely via a local wifi network, confirm the waypoint mission defined in waypoints_xyzy.yaml. * Launch the localization modules (such as laser slam) first, and validate the position output via rostopic command. * Run commands below to start the offboard control modules: :: # 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 * Arm the vehicle by lowering down the throttle and move the yaw stick to the right, then we will hear a long beep from the machine. * Switch to the OFFBOARD mode with the specified stick (e.g. channel 7), then the machine will start the mission autonomously. Cheers!