.. _tutorial_offboard_cplus_zh:
Offboard Control 渚嬬▼ (ROS C++)
====================================
鏈暀绋嬩粙缁嶅浣曚娇鐢ㄤ负 Kerloud Autocar 绯诲垪鎻愪緵鐨� C++ API 鍦� ROS 涓紪鍐欎竴涓猳ffboard鎺у埗鑺傜偣銆俹ffboard 渚嬬▼鍙綔涓哄紑鍙戝悇绉嶄换鍔″簲鐢ㄧ▼搴忕殑鍩虹銆�
.. caution::
渚嬬▼浠ｇ爜浠呴€備簬椤圭洰鐮旂┒锛屼笉鍙洿鎺ョ敤浜庝骇鍝併€傚鏋滄偍鍦ㄦ祴璇曚腑鍙戠幇閿欒锛屽彲闅忔椂鍦ㄦ垜浠殑瀹樻柟璧勬簮搴撲腑鍒涘缓pull璇锋眰銆�
1. 鐜璁剧疆
-------------
鎺ㄨ崘鐜涓轰娇鐢� Ubuntu 16.04 鐨� ROS kinetic锛屽敖绠℃湰鏁欑▼涔熷彲浠ュ湪鍏朵粬 ROS 鐗堟湰涓繍琛屻€�
鐜版垚鍙敤鐨凴OS宸ヤ綔鍖轰綅浜� ~/src/rover_workspace/catkinws_offboard鐩綍 涓嬨€� 璇ュ伐浣滃尯鍖呭惈鎴戜滑瀹樻柟璧勬簮搴撲腑鐨勮蒋浠跺寘锛屽湪缁存姢鐨勮祫婧愬簱閾炬帴濡備笅锛�
- mavros锛坉ev_rover 鍒嗘敮锛夛細https://github.com/cloudkernel-tech/mavros
- mavlink锛坉ev_rover 鍒嗘敮锛夛細https://github.com/cloudkernel-tech/mavlink-gdp-release
- offboard demo node锛坉ev_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.浠ｇ爜璇存槑
--------------
渚嬬▼浠ｇ爜涓烘爣鍑哛OS鑺傜偣褰㈠紡锛岀敤鎴烽渶鐔熸倝瀹樻柟ROS鏁欑▼涓彂甯冦€佽闃呰妭鐐圭殑缂栫▼鏂规硶锛屽畼鏂归摼鎺ヤ负锛歨ttp://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber(c%2B%2B)銆�
閲嶇偣鎸囧嚭锛宱ffboard demo鑺傜偣鍖呭惈鍑犱釜鏂囦欢锛�
- offboard_demo/launch/off_mission.launch锛氶粯璁ゅ惎鍔ㄦ枃浠讹紝鐢ㄤ簬鍚姩offboard鎺у埗鑺傜偣銆�
- offboard_demo/launch/waypoints_xyzy.yaml锛� ENU鍧愭爣绯讳笅鐨勮埅璺偣浠诲姟瀹氫箟鏂囦欢锛屽寘鍚浉搴旂殑鍋忚埅淇℃伅銆�
- offboard_demo/src/off_mission_node.cpp锛� 鐢ㄤ簬offboard鎺у埗鐨凴OS鑺傜偣鏂囦欢銆�
2.1 鍛戒护妯″紡
^^^^^^^^^^^^^^
杞︿綋鍙互鍦ㄥ绉嶆寚浠ゆā寮忎笅杩愯锛�
- **浣嶇疆妯″紡**锛氳溅浣撹鎸囧紩鍓嶈繘鍒版寚瀹氫綅缃€�
- **閫熷害妯″紡**锛氳溅浣撲互鎸囧畾閫熷害鍓嶈繘銆�
- **濮挎€佹ā寮�**锛氬彲浠ヤ娇鐢ㄥЭ鎬佽瀹氱偣鍜屾墍闇€鐨勭數鏈烘补闂ㄥ€兼搷浣滆溅浣擄紝鏀寔鍓嶈繘鍜屽悗閫€銆�
- **鎵ц鍣ㄦ帶鍒舵ā寮�**锛氳溅浣撳彲浠ラ€氳繃鐩存帴鎵ц鍣ㄦ帶鍒惰繘琛屾搷浣滐紝鍖呮嫭杞悜浼烘湇鍜岀數鏈烘补闂ㄨ緭鍏ワ紝鏀寔鍓嶈繘鍜屽悗閫€銆�
2.2 绋嬪簭璇存槑
^^^^^^^^^^^^^^
涓庝笂杩版寚浠ゆā寮忕浉瀵瑰簲鐨刼ffboard demo鑺傜偣浠诲姟鐢卞洓涓樁娈典緷娆＄粍鎴愩€傚叧閿湪浜庡埄鐢� C++ API 涓彁渚涚殑鍚勭涓婚鎴栨湇鍔°€�
**(1) 璁㈤槄銆佸彂甯冦€佹湇鍔＄殑澹版槑**
鎴戜滑閫氬父鍦ㄤ富绋嬪簭鐨勫紑澶村０鏄� ROS 璁㈤槄銆佸彂甯冨拰鏈嶅姟銆�
瑕佽幏鍙栫姸鎬佷俊鎭垨鍙戦€佸懡浠わ紝鍙互杞绘澗浣跨敤 mavros 鍖呬腑鐨勫悇绉嶄富棰樻垨鏈嶅姟銆備緥濡傦紝瑕佽幏鍙栨満浣撶殑鏈湴浣嶇疆淇℃伅锛岄渶瑕佽闃卪avros/setpoint_position/local 涓婚銆� 璇锋敞鎰忥紝鎵€鐢ㄥ潗鏍囩郴涓篍NU锛圗ast-North-Up锛夈€侻avros杞欢鍖呯殑鏍囧噯淇℃伅鍙弬瑙侊細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)浣嶇疆寮曞闃舵**
鍦ㄤ綅缃紩瀵奸樁娈碉紝杞︿綋浼氬姞杞絯aypoints_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 涓昏鐢ㄤ簬瀹ゅ唴搴旂敤锛屽洜姝ゅ湪瀹炶返姝ffboard鎺у埗鏁欑▼涔嬪墠搴斿厛鍚姩瀹氫綅妯″潡銆備緥濡傦紝鎴戜滑蹇呴』涓� Autocar Laser 鍚姩婵€鍏夊畾浣嶇畻娉曘€傜劧鍚庯紝鐢ㄦ埛灏卞彲浠ユ寜鐓у涓嬪懡浠ゆ潵閬垮厤鎰忓琛屼负锛�
* 纭繚閬ユ帶鍣ㄧ殑鎵€鏈夊紑鍏抽兘澶勪簬鍒濆浣嶇疆锛屽苟涓旀补闂ㄦ媺鑷充綆浣嶃€�
* 鐢垫睜涓婄數寮€鏈恒€�
* 鏈鸿浇鐢佃剳鍚姩杩囩▼闇€绛夊緟鍑犲垎閽燂紝閫氳繃鏈湴Wi-Fi杩滅▼鐧诲綍锛岀‘璁� waypoints_xyzy.yaml 涓畾涔夌殑鑸偣浠诲姟銆�
* 鍚姩瀹氫綅妯″潡锛堝laser slam锛夛紝骞堕€氳繃rostopic 鍛戒护楠岃瘉浣嶇疆杈撳嚭銆�
* 杩愯濡備笅鎸囦护浠ュ惎鍔╫ffboard鍗曞厓锛�
::
# 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 妯″紡锛岀劧鍚庢満鍣ㄥ皢鑷姩寮€濮嬩换鍔°€� 杩愯鎴愬姛 !