機(jī)器人操作系統(tǒng)ROS-典型功能代碼詳解

機(jī)器人操作系統(tǒng)ROS:典型功能代碼詳解李寶全目錄TOC目錄1ROSAPI介紹3Demo3//talker.cpp3//listener.cpp4//add_two_ints_server.cpp4//add_two_int_client.cpp5#CmakeList.txt5ROSARIA81//RosAria.cpp8#CmakeList.txt192//rosaria_teleop_key.cpp193//rosaria_teleop_code.cpp21#CmakeList.txt22Images25//image_converter.cpp//包c(diǎn)v_bridge僅有的一個(gè)程序25//hud_gui_simplified.cpp26//usb_cam.h27//usb_cam.cpp28//usb_cam_node.cpp44#usb_cam/CmakeList.txt47NiTE,Tf的發(fā)布48//tf2_ros/transform_broadcaster.h系統(tǒng)48//tf2_ros/transform_broadcaster.cpp系統(tǒng)48//tf/transform_broadcaster.h49//tf/transform_broadcaster.cpp50//tf2_ros/transform_listener.h50//tf/transform_listener.h51//openni_tracker.cpp節(jié)點(diǎn)54#CmakeList.txt57Tf的接收,并以控制P3AT做示范58//TfListnerAria.cpp58#CmakeList.txt59語音發(fā)布60//sound_play.h60//SoundDemo.cpp62#CmakeList.txt63#test/CmakeList.txt64語言接收,顯示/再發(fā)聲64//VoiceBridgeNode.cpp64#CMakeList.txt64//RespondVoice.cpp65#test/CMakeList.txt66ROS_OpenTLD-GUI66//tld_gui_node.cpp66//base_frame.hpp66//base_frame.cpp68//ui_baseFrame.h72//base_frame_graphics_view.hpp75//base_frame_graphics_view.cpp76ROS_OpenTLD-tracker79//tld_node.cpp79//main.hpp80//main.cpp82ROS_OpenTLD-launch87//ros_tld_gui.launch87//ros_tld_tracker.launch87ROS_OpenTLD-msg88//BoundingBox.h88//Target.h91ROS_OpenTLD-git-opentld/main94ROS數(shù)據(jù)類型95//std_msgs/Float3295//std_msgs/Char95//std_msgs/String95//std_msgs/Header95//sensor_msgs/Image95//sensor_msgs/Image_encodings??96//sound_play/SoundRequest96//geometry_msgs/Twist96//geometry_msgs/TransformStamped96ROS系統(tǒng)程序文件96//fill_image.h96//cv_bridge.h系統(tǒng)97//cv_bridge.cpp101Linux系統(tǒng)程序文件101/usr/include/linux/videodev2.h101ROSAPI介紹關(guān)于publisher1.ros::Publisherchatter_pub=n.advertise<std_msgs::String>("chatter",1000);//創(chuàng)立了一個(gè)發(fā)布者2.image_pub_=it_.advertise("/image_converter/output_video",1);3.image_transport::CameraPublisherimage_pub_;image_pub_=it.advertiseCamera("image_raw",1);image_pub_.publish((sensor_msgs::Image)img_,(sensor_msgs::CameraInfoPtr)*ci);關(guān)于advertise的緩沖區(qū)數(shù)量,對于攝像頭而言,緩沖1幅圖像即可,試想假設(shè)緩沖1000的話,當(dāng)接收圖像時(shí)接收的是歷史圖像.(需要驗(yàn)證???)但是對于第一行的緩沖區(qū)為1000的BaseDemo，listener還是從最新發(fā)布的信息開始讀取啊？？？關(guān)于subscribe,訂閱者要有一個(gè)回調(diào)函數(shù),例如:image_sub_=it_.subscribe("/camera/image_raw",1,&ImageConverter::imageCb,this);//關(guān)聯(lián)了回調(diào)函數(shù)//ImageConverter::是指該類中的一個(gè)函數(shù)ros::Subscribersub=n.subscribe("chatter",1000,chatterCallback);//創(chuàng)立一個(gè)訂閱者//chatterCallback只是外面的與main平級的一個(gè)函數(shù)代碼相關(guān)說明:程序中含有中文注釋時(shí)，也能編譯成功。也能運(yùn)行！安裝的hydro，只有。h文件，cpp文件沒有Demo//talker.cpp#include"ros/ros.h"http://ROS的頭文件#include"std_msgs/String.h"http://ROS自定義的數(shù)據(jù)類型String所在的頭文件#include<sstream>//字符串的輸入輸出流intmain(intargc,char**argv){ ros::init(argc,argv,"talker");//在名字空間”ros”中的函數(shù)init();//初始化ROS,是確切說明節(jié)點(diǎn)名字的地方,節(jié)點(diǎn)的名字必須唯一,//它允許ROS通過命令行重新命名, ros::NodeHandlen;//創(chuàng)立了該節(jié)點(diǎn)的一個(gè)句柄 ros::Publisherchatter_pub=n.advertise<std_msgs::String>("chatter",1000);//創(chuàng)立了一個(gè)發(fā)布者//尖括號里為發(fā)布的數(shù)據(jù)類型,即消息Message //圓括號中”chatter”為發(fā)布的話題的名稱,即Topic//這就使得主機(jī)告訴了所有訂閱了”chatter”話題的節(jié)點(diǎn),我們將在這個(gè)話題上發(fā)布數(shù)據(jù)//用到了std_msgs,對應(yīng)于在創(chuàng)立包時(shí)參加“std_msgs”一項(xiàng)//////////////////////////////第二個(gè)參數(shù)是發(fā)布隊(duì)列的大小,它的作用是緩沖.當(dāng)我們發(fā)布消息很快的時(shí)候,它將能緩沖1000條信息.//NodeHandle::advertise()將會(huì)返回ros::Publisher對象//也可以寫成ros::Publisherchatter_pub;chatter_pub=n.advertise<...>... ros::Rateloop_rate(10);//一個(gè)ros::Rate對象允許user制定循環(huán)的頻率//它將會(huì)記錄從上次調(diào)用Rate::sleep()到現(xiàn)在為止的時(shí)間,并且休眠正確的時(shí)間intcount=0;while(ros::ok())//默認(rèn)情況下,roscpp將會(huì)安裝一個(gè)SIGINT監(jiān)聽,它使當(dāng)Ctrl-C按下時(shí),ros::ok()將會(huì)返回0. { std_msgs::Stringmsg;std::stringstreamss;ss<<"helloworld"<<count;msg.data=ss.str(); ROS_INFO("%s",msg.data.c_str());//函數(shù)ROS_INFO()將相關(guān)數(shù)據(jù)輸出到屏幕上//輸出結(jié)果為:helloworld1;helloworld2;helloworld3;……chatter_pub.publish(msg);//將該消息,即字符串“hellowordxxx”,發(fā)布到話題“chatter”上 ros::spinOnce();//等待消息發(fā)布完成loop_rate.sleep();//休眠一下,使程序滿足前面所設(shè)置的10Hz的要求 ++count; }return0;}//listener.cpp#include"ros/ros.h"#include"std_msgs/String.h"voidchatterCallback(conststd_msgs::String::ConstPtr&msg)//該回調(diào)函數(shù)的輸入?yún)?shù)為std_msgs::Stringmsg,與發(fā)布者中的數(shù)據(jù)類型保持一致///////////////////是否可以不用ConstPtr,因?yàn)閠alker中只是Stringmsg的數(shù)據(jù)結(jié)構(gòu),是不用也可以不用&//是不是但凡接受者,都用ConstPtr??{ ROS_INFO("Iheard:[%s]",msg->data.c_str());//將msg中的信息輸出到屏幕上 //輸出結(jié)果為:Iheard:[helloworld1];Iheard:[helloworld2];Iheard:[helloworld3];…}intmain(intargc,char**argv){ ros::init(argc,argv,"listener"); ros::NodeHandlen; ros::Subscribersub=n.subscribe("chatter",1000,chatterCallback);//創(chuàng)立一個(gè)訂閱者//第一個(gè)參數(shù)為Topic的名稱“chatter” //第三個(gè)參數(shù)為回調(diào)函數(shù)的名稱//每當(dāng)在話題上接受到數(shù)據(jù),即調(diào)用該回調(diào)函數(shù)一次 ros::spin();//等到該節(jié)點(diǎn)被關(guān)閉時(shí),spin()才返回//類似于cvWaitKey()return0;}//add_two_ints_server.cpp#include"ros/ros.h"#include"beginner_tutorials/AddTwoInts.h"booladd(beginner_tutorials::AddTwoInts::Request&req, beginner_tutorials::AddTwoInts::Response&res){res.sum=req.a+req.b; ROS_INFO("request:x=%ld,y=%ld",(longint)req.a,(longint)req.b); ROS_INFO("sendingbackresponse:[%ld]",(longint)res.sum);returntrue;}intmain(intargc,char**argv){ ros::init(argc,argv,"add_two_ints_server"); ros::NodeHandlen; ros::ServiceServerservice=n.advertiseService("add_two_ints",add); ROS_INFO("Readytoaddtwoints."); ros::spin();//程序一直停留在這,等待客戶端的請求.客戶端請求一次,就運(yùn)行一次add()//等到該節(jié)點(diǎn)被關(guān)閉時(shí),spin()才返回return0;}//add_two_int_client.cpp#include"ros/ros.h"#include"beginner_tutorials/AddTwoInts.h"#include<cstdlib>intmain(intargc,char**argv){ ros::init(argc,argv,"add_two_ints_client");if(argc!=3) { ROS_INFO("usage:add_two_ints_clientXY");return1; } ros::NodeHandlen;//符號::是指名字空間下的一個(gè)數(shù)據(jù)類型 ros::ServiceClientclient=n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");//尖括號里面的,是指消息的類型.//Topic話題:圓括號里面的是Topic的名字(節(jié)點(diǎn)可以在一個(gè)話題上發(fā)布消息，同樣也可以訂閱一個(gè)話題來接收消息)//Message消息:在一個(gè)話題上，發(fā)布或訂閱時(shí)所使用的ROS的數(shù)據(jù)類型. beginner_tutorials::AddTwoIntssrv;srv.request.a=atoll(argv[1]);srv.request.b=atoll(argv[2]);//request的類型為Request,其下有兩個(gè)量:a,b//atoll功能:把字符串轉(zhuǎn)換成長長整型數(shù)if(client.call(srv)) { ROS_INFO("Sum:%ld",(longint)srv.response.sum); }else { ROS_ERROR("Failedtocallserviceadd_two_ints");return1; }return0;}#CmakeList.txtcmake_minimum_required(VERSION2.8.3)project(beginner_tutorials)##Findcatkinmacrosandlibraries##ifCOMPONENTSlistlikefind_package(catkinREQUIREDCOMPONENTSxyz)##isused,alsofindothercatkinpackagesfind_package(catkinREQUIREDCOMPONENTSroscpprospystd_msgsmessage_generation)##SystemdependenciesarefoundwithCMake'sconventions#find_package(BoostREQUIREDCOMPONENTSsystem)##Uncommentthisifthepackagehasasetup.py.Thismacroensures##modulesandglobalscriptsdeclaredthereingetinstalled##See:///doc/api/catkin/html/user_guide/setup_dot_py.html#catkin_python_setup()##################################################DeclareROSmessages,servicesandactions####################################################Todeclareandbuildmessages,servicesoractionsfromwithinthis##package,followthesesteps:##*LetMSG_DEP_SETbethesetofpackageswhosemessagetypesyouusein##yourmessages/services/actions(e.g.std_msgs,actionlib_msgs,...).##*Inthefilepackage.xml:##*addabuild_dependandarun_dependtagforeachpackageinMSG_DEP_SET##*IfMSG_DEP_SETisn'temptythefollowingdependenciesmighthavebeen##pulledintransitivelybutcanbedeclaredforcertaintynonetheless:##*addabuild_dependtagfor"message_generation"##*addarun_dependtagfor"message_runtime"##*Inthisfile(CMakeLists.txt):##*add"message_generation"andeverypackageinMSG_DEP_SETto##find_package(catkinREQUIREDCOMPONENTS...)##*add"message_runtime"andeverypackageinMSG_DEP_SETto##catkin_package(CATKIN_DEPENDS...)##*uncommenttheadd_*_filessectionsbelowasneeded##andlistevery.msg/.srv/.actionfiletobeprocessed##*uncommentthegenerate_messagesentrybelow##*addeverypackageinMSG_DEP_SETtogenerate_messages(DEPENDENCIES...)##Generatemessagesinthe'msg'folderadd_message_files(FILESNum.msg)##Generateservicesinthe'srv'folderadd_service_files(FILESAddTwoInts.srv)##Generateactionsinthe'action'folder#add_action_files(#FILES#Action1.action#Action2.action#)##Generateaddedmessagesandserviceswithanydependencieslistedheregenerate_messages(DEPENDENCIESstd_msgs)#####################################catkinspecificconfiguration#######################################Thecatkin_packagemacrogeneratescmakeconfigfilesforyourpackage##Declarethingstobepassedtodependentprojects##INCLUDE_DIRS:uncommentthisifyoupackagecontainsheaderfiles##LIBRARIES:librariesyoucreateinthisprojectthatdependentprojectsalsoneed##CATKIN_DEPENDS:catkin_packagesdependentprojectsalsoneed##DEPENDS:systemdependenciesofthisprojectthatdependentprojectsalsoneedcatkin_package(CATKIN_DEPENDSmessage_runtime#INCLUDE_DIRSinclude#LIBRARIESbeginner_tutorials#CATKIN_DEPENDSroscpprospystd_msgs//#DEPENDSsystem_lib)#############Build###############Specifyadditionallocationsofheaderfiles##Yourpackagelocationsshouldbelistedbeforeotherlocations#include_directories(include)include_directories(${catkin_INCLUDE_DIRS})#add_executable(talkersrc/talker.cpp)#target_link_libraries(talker${catkin_LIBRARIES})#add_dependencies(talkerbeginner_tutorials_generate_messages_cpp)#add_executable(listenersrc/listener.cpp)#target_link_libraries(listener${catkin_LIBRARIES})#add_dependencies(listenerbeginner_tutorials_generate_messages_cpp)add_executable(add_two_ints_serversrc/add_two_ints_server.cpp)#可執(zhí)行程序目標(biāo)target_link_libraries(add_two_ints_server${catkin_LIBRARIES})#設(shè)置單個(gè)目標(biāo)需要鏈接的庫add_dependencies(add_two_ints_serverbeginner_tutorials_gencpp)#目標(biāo)add_two_ints_server依賴于beginner_tutorials_gencppadd_executable(add_two_ints_clientsrc/add_two_ints_client.cpp)target_link_libraries(add_two_ints_client${catkin_LIBRARIES})add_dependencies(add_two_ints_clientbeginner_tutorials_gencpp)##Declareacpplibrary#add_library(beginner_tutorials#src/${PROJECT_NAME}/beginner_tutorials.cpp#)#庫目標(biāo)##Declareacppexecutable#add_executable(beginner_tutorials_nodesrc/beginner_tutorials_node.cpp)##Addcmaketargetdependenciesoftheexecutable/library##asanexample,messageheadersmayneedtobegeneratedbeforenodes#add_dependencies(beginner_tutorials_nodebeginner_tutorials_generate_messages_cpp)##Specifylibrariestolinkalibraryorexecutabletargetagainst#target_link_libraries(beginner_tutorials_node#${catkin_LIBRARIES}#)###############Install################allinstalltargetsshouldusecatkinDESTINATIONvariables#See:///doc/api/catkin/html/adv_user_guide/variables.html##Markexecutablescripts(Pythonetc.)forinstallation##incontrasttosetup.py,youcanchoosethedestination#install(PROGRAMS#scripts/my_python_script#DESTINATION${CATKIN_PACKAGE_BIN_DESTINATION}#)##Markexecutablesand/orlibrariesforinstallation#install(TARGETSbeginner_tutorialsbeginner_tutorials_node#ARCHIVEDESTINATION${CATKIN_PACKAGE_LIB_DESTINATION}#LIBRARYDESTINATION${CATKIN_PACKAGE_LIB_DESTINATION}#RUNTIMEDESTINATION${CATKIN_PACKAGE_BIN_DESTINATION}#)##Markcppheaderfilesforinstallation#install(DIRECTORYinclude/${PROJECT_NAME}/#DESTINATION${CATKIN_PACKAGE_INCLUDE_DESTINATION}#FILES_MATCHINGPATTERN"*.h"#PATTERN".svn"EXCLUDE#)##Markotherfilesforinstallation(e.g.launchandbagfiles,etc.)#install(FILES##myfile1##myfile2#DESTINATION${CATKIN_PACKAGE_SHARE_DESTINATION}#)###############Testing#################Addgtestbasedcpptesttargetandlinklibraries#catkin_add_gtest(${PROJECT_NAME}-testtest/test_beginner_tutorials.cpp)#if(TARGET${PROJECT_NAME}-test)#target_link_libraries(${PROJECT_NAME}-test${PROJECT_NAME})#endif()##Addfolderstoberunbypythonnosetests#catkin_add_nosetests(test)ROSARIA1//RosAria.cpp#include<stdio.h>#include<math.h>//既然有math頭文件,可以去CmakeList.txt中看看是怎么添加鏈接庫的???#ifdefADEPT_PKG#include<Aria.h>//去CmakeList.txt中看看是怎么添加鏈接庫的???#else#include<Aria/Aria.h>#endif#include"ros/ros.h"#include"geometry_msgs/Twist.h"#include"geometry_msgs/Pose.h"#include"geometry_msgs/PoseStamped.h"#include<sensor_msgs/PointCloud.h>//forsonardata#include"nav_msgs/Odometry.h"#include"rosaria/BumperState.h"#include"tf/tf.h"#include"tf/transform_listener.h"http://fortf::getPrefixParam#include<tf/transform_broadcaster.h>#include"tf/transform_datatypes.h"#include<dynamic_reconfigure/server.h>#include<rosaria/RosAriaConfig.h>#include"std_msgs/Float64.h"#include"std_msgs/Float32.h"#include"std_msgs/Int8.h"#include"std_msgs/Bool.h"#include"std_srvs/Empty.h"#include<sstream>//NodethatinterfacesbetweenROSandmobilerobotbasefeaturesviaARIAlibrary.////information,tutorialsanddocumentation.classRosAriaNode{public:RosAriaNode(ros::NodeHandlen);virtual~RosAriaNode();public:intSetup();voidcmdvel_cb(constgeometry_msgs::TwistConstPtr&);//對于傳入?yún)?shù),為何與該函數(shù)的定義不一樣呢???//voidcmd_enable_motors_cb();//voidcmd_disable_motors_cb();voidspin();voidpublish();//用于輸出位姿??voidsonarConnectCb();voiddynamic_reconfigureCB(rosaria::RosAriaConfig&config,uint32_tlevel);voidreadParameters();protected: ros::NodeHandlen; ros::Publisherpose_pub;//輸出姿態(tài)相關(guān) ros::Publisherbumpers_pub; ros::Publishersonar_pub; ros::Publishervoltage_pub; ros::Publisherrecharge_state_pub; std_msgs::Int8recharge_state; ros::Publisherstate_of_charge_pub; ros::Publishermotors_state_pub; std_msgs::Boolmotors_state;boolpublished_motors_state; ros::Subscribercmdvel_sub; ros::ServiceServerenable_srv; ros::ServiceServerdisable_srv;boolenable_motors_cb(std_srvs::Empty::Request&request,std_srvs::Empty::Response&response);booldisable_motors_cb(std_srvs::Empty::Request&request,std_srvs::Empty::Response&response); ros::Timeveltime;std::stringserial_port;intserial_baud;ArRobotConnector*conn;ArRobot*robot; nav_msgs::Odometryposition; rosaria::BumperStatebumpers;ArPosepos;ArFunctorC<RosAriaNode>myPublishCB;//ArRobot::ChargeStatebatteryCharge;//forodom->base_linktransform tf::TransformBroadcasterodom_broadcaster; geometry_msgs::TransformStampedodom_trans;//forresolvingtfnames.std::stringtf_prefix;std::stringframe_id_odom;std::stringframe_id_base_link;std::stringframe_id_bumper;std::stringframe_id_sonar;//Sonarsupportbooluse_sonar;//enableandpublishsonars//DebugAriabooldebug_aria;std::stringaria_log_filename;//RobotParametersintTicksMM,DriftFactor,RevCount;//OdometryCalibrationSettings//dynamic_reconfigure dynamic_reconfigure::Server<rosaria::RosAriaConfig>*dynamic_reconfigure_server;};voidRosAriaNode::readParameters(){//RobotParametersrobot->lock(); ros::NodeHandlen_("~");if(n_.hasParam("TicksMM")) {n_.getParam("TicksMM",TicksMM); ROS_INFO("SettingTicksMMfromROSParameter:%d",TicksMM);robot->comInt(93,TicksMM); }else {TicksMM=robot->getOrigRobotConfig()->getTicksMM();n_.setParam("TicksMM",TicksMM); ROS_INFO("SettingTicksMMfromrobotEEPROM:%d",TicksMM); }if(n_.hasParam("DriftFactor")) {n_.getParam("DriftFactor",DriftFactor); ROS_INFO("SettingDriftFactorfromROSParameter:%d",DriftFactor);robot->comInt(89,DriftFactor); }else {DriftFactor=robot->getOrigRobotConfig()->getDriftFactor();n_.setParam("DriftFactor",DriftFactor); ROS_INFO("SettingDriftFactorfromrobotEEPROM:%d",DriftFactor); }if(n_.hasParam("RevCount")) {n_.getParam("RevCount",RevCount); ROS_INFO("SettingRevCountfromROSParameter:%d",RevCount);robot->comInt(88,RevCount); }else {RevCount=robot->getOrigRobotConfig()->getRevCount();n_.setParam("RevCount",RevCount); ROS_INFO("SettingRevCountfromrobotEEPROM:%d",RevCount); }robot->unlock();}voidRosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig&config,uint32_tlevel){////OdometrySettings//robot->lock();if(TicksMM!=config.TicksMMandconfig.TicksMM>0) { ROS_INFO("SettingTicksMMfromDynamicReconfigure:%d->%d",TicksMM,config.TicksMM);TicksMM=config.TicksMM;robot->comInt(93,TicksMM); }if(DriftFactor!=config.DriftFactor) { ROS_INFO("SettingDriftFactorfromDynamicReconfigure:%d->%d",DriftFactor,config.DriftFactor);DriftFactor=config.DriftFactor;robot->comInt(89,DriftFactor); }if(RevCount!=config.RevCountandconfig.RevCount>0) { ROS_INFO("SettingRevCountfromDynamicReconfigure:%d->%d",RevCount,config.RevCount);RevCount=config.RevCount;robot->comInt(88,RevCount); }////AccelerationParameters//intvalue;value=config.trans_accel*1000;if(value!=robot->getTransAccel()andvalue>0) { ROS_INFO("SettingTransAccelfromDynamicReconfigure:%d",value);robot->setTransAccel(value); }value=config.trans_decel*1000;if(value!=robot->getTransDecel()andvalue>0) { ROS_INFO("SettingTransDecelfromDynamicReconfigure:%d",value);robot->setTransDecel(value); }value=config.lat_accel*1000;if(value!=robot->getLatAccel()andvalue>0) { ROS_INFO("SettingLatAccelfromDynamicReconfigure:%d",value);if(robot->getAbsoluteMaxLatAccel()>0)robot->setLatAccel(value); }value=config.lat_decel*1000;if(value!=robot->getLatDecel()andvalue>0) { ROS_INFO("SettingLatDecelfromDynamicReconfigure:%d",value);if(robot->getAbsoluteMaxLatDecel()>0)robot->setLatDecel(value); }value=config.rot_accel*180/M_PI;if(value!=robot->getRotAccel()andvalue>0) { ROS_INFO("SettingRotAccelfromDynamicReconfigure:%d",value);robot->setRotAccel(value); }value=config.rot_decel*180/M_PI;if(value!=robot->getRotDecel()andvalue>0) { ROS_INFO("SettingRotDecelfromDynamicReconfigure:%d",value);robot->setRotDecel(value); }robot->unlock();}voidRosAriaNode::sonarConnectCb(){robot->lock();if(sonar_pub.getNumSubscribers()==0) {robot->disableSonar();use_sonar=false; }else {robot->enableSonar();use_sonar=true; }robot->unlock();}RosAriaNode::RosAriaNode(ros::NodeHandlenh):myPublishCB(this,&RosAriaNode::publish),serial_port(""),serial_baud(0),use_sonar(false)//整合了publish()函數(shù)??//不使用聲納{//readinconfigoptionsn=nh;//!!!port!!!n.param("port",serial_port,std::string("/dev/ttyUSB0"));//給NodeHandle參加了一個(gè)名為”port”的標(biāo)簽?? ROS_INFO("RosAria:usingport:[%s]",serial_port.c_str());n.param("baud",serial_baud,0);if(serial_baud!=0) ROS_INFO("RosAria:usingserialportbaudrate%d",serial_baud);//handledebuggingmoreelegantlyn.param("debug_aria",debug_aria,false);//defaultnottodebugn.param("aria_log_filename",aria_log_filename,std::string("Aria.log"));//Figureoutwhatframe_id'stouse.ifatf_prefixparamisspecified,//itwillbeaddedtothebeginningoftheframe_ids.////e.g.rosrun..._tf_prefix:=MyRobot(orequivalentlyusing<param>sin//roslaunchfiles)//willresultintheframe_idsbeingsetto/MyRobot/odometc,//ratherthan/odom.ThisisusefulforMultiRobotSystems.//SeeROSWikiforfurtherdetails.tf_prefix=tf::getPrefixParam(n);frame_id_odom=tf::resolve(tf_prefix,"odom");frame_id_base_link=tf::resolve(tf_prefix,"base_link");frame_id_bumper=tf::resolve(tf_prefix,"bumpers_frame");frame_id_sonar=tf::resolve(tf_prefix,"sonar_frame");//advertiseservicesfordatatopics//secondargumenttoadvertise()isqueuesize.//otherargmuments(optional)arecallbacks,oraboolean"latch"flag(whethertosendcurrentdatatonew//subscriberswhentheysubscribe).//Seeros::NodeHandleAPIdocs.pose_pub=n.advertise<nav_msgs::Odometry>("pose",1000);//用于輸出機(jī)器人姿態(tài)//話題名稱為”pose”bumpers_pub=n.advertise<rosaria::BumperState>("bumper_state",1000);sonar_pub=n.advertise<sensor_msgs::PointCloud>("sonar",50, boost::bind(&RosAriaNode::sonarConnectCb,this), boost::bind(&RosAriaNode::sonarConnectCb,this));voltage_pub=n.advertise<std_msgs::Float64>("battery_voltage",1000);recharge_state_pub=n.advertise<std_msgs::Int8>("battery_recharge_state",5,true/*latch*/);recharge_state.data=-2;state_of_charge_pub=n.advertise<std_msgs::Float32>("battery_state_of_charge",100);motors_state_pub=n.advertise<std_msgs::Bool>("motors_state",5,true/*latch*/);motors_state.data=false;published_motors_state=false;//subscribetoservices//話題名稱為”cmd_vel”//將Topic"cmd_vel"與Subscribe:cmdvel_sub以及函數(shù)cmdvel_cb對應(yīng)上//是不是但凡接受者,都用ConstPtr??cmdvel_sub=n.subscribe("cmd_vel",1,(boost::function<void(constgeometry_msgs::TwistConstPtr&)>)boost::bind(&RosAriaNode::cmdvel_cb,this,_1));//綁定函數(shù)??//實(shí)現(xiàn)的功能:其他節(jié)點(diǎn)發(fā)布到話題”cmd_vel”上的數(shù)據(jù)類型geometry_msgs::Twist,就會(huì)傳到到函數(shù)RosAriaNode::cmdvel_cb(constgeometry_msgs::TwistConstPtr&msg)中,并執(zhí)行該函數(shù)(即控制機(jī)器人的速度)//advertiseenable/disableservicesenable_srv=n.advertiseService("enable_motors",&RosAriaNode::enable_motors_cb,this);disable_srv=n.advertiseService("disable_motors",&RosAriaNode::disable_motors_cb,this);veltime=ros::Time::now();}RosAriaNode::~RosAriaNode(){//disablemotorsandsonar.robot->disableMotors();robot->disableSonar();robot->stopRunning();robot->waitForRunExit();Aria::shutdown();}intRosAriaNode::Setup(){//Note,variousobjectsareallocatedherewhichareneverdeleted(freed),sinceSetup()isonlysupposedtobe//calledonceperinstance,andtheseobjectsneedtopersistuntiltheprocessterminates.robot=newArRobot();ArArgumentBuilder*args=newArArgumentBuilder();//neverfreedArArgumentParser*argparser=newArArgumentParser(args);//Warningneverfreedargparser->loadDefaultArguments();//addsanyargumentsgivenin/etc/Aria.args.Usefulonrobotswithunusualserialportorbaudrate(e.g.pioneerlx)//Nowaddanyparametersgivenviarosparams(seeRosAriaNodeconstructor)://ifserialportparametercontainsa':'character,theninterpretitashostname:tcpport//forwirelessserialconnection.Otherwise,interpretitasaserialportname.size_tcolon_pos=serial_port.find(":");if(colon_pos!=std::string::npos) {args->add("-remoteHost");//passrobot'shostname/IPaddresstoAriaargs->add(serial_port.substr(0,colon_pos).c_str());args->add("-remoteRobotTcpPort");//passrobot'sTCPporttoAriaargs->add(serial_port.substr(colon_pos+1).c_str()); }else {args->add("-robotPort");//passrobot'sserialporttoAriaargs->add(serial_port.c_str()); }//ifabaudratewasspecifiedinbaudparameterif(serial_baud!=0) {args->add("-robotBaud");chartmp[100];snprintf(tmp,100,"%d",serial_baud);args->add(tmp); }if(debug_aria) {//turnonallARIAdebuggingargs->add("-robotLogPacketsReceived");//logreceivedpacketsargs->add("-robotLogPacketsSent");//logsentpacketsargs->add("-robotLogVelocitiesReceived");//logreceivedvelocitiesargs->add("-robotLogMovementSent");args->add("-robotLogMovementReceived");ArLog::init(ArLog::File,ArLog::Verbose,aria_log_filename.c_str(),true); }//Connecttotherobot//嗯,新版本的Aria就得這樣連接conn=newArRobotConnector(argparser,robot);//warningneverfreedif(!conn->connectRobot()){ ROS_ERROR("RosAria:ARIAcouldnotconnecttorobot!(Check~portparameteriscorrect,andpermissionsonportdevice.)");return1; }//causesARIAtoloadvariousrobot-specifichardwareparametersfromtherobotparameterfilein/usr/local/Aria/paramsif(!Aria::parseArgs()) { ROS_ERROR("RosAria:ARIAerrorparsingARIAstartupparameters!");return1; }readParameters();//Startdynamic_reconfigureserverdynamic_reconfigure_server=newdynamic_reconfigure::Server<rosaria::RosAriaConfig>;//SetupParameterMinimums rosaria::RosAriaConfigdynConf_min;dynConf_min.trans_accel=robot->getAbsoluteMaxTransAccel()/1000;dynConf_min.trans_decel=robot->getAbsoluteMaxTransDecel()/1000;//TODO:Fixrqtdynamic_reconfigureguitohandleemptyintervals//Untilthen,setunitlengthinterval.dynConf_min.lat_accel=((robot->getAbsoluteMaxLatAccel()>0.0)?robot->getAbsoluteMaxLatAccel():0.1)/1000;dynConf_min.lat_decel=((robot->getAbsoluteMaxLatDecel()>0.0)?robot->getAbsoluteMaxLatDecel():0.1)/1000;dynConf_min.rot_accel=robot->getAbsoluteMaxRotAccel()*M_PI/180;dynConf_min.rot_decel=robot->getAbsoluteMaxRotDecel()*M_PI/180;//I'msettingtheseupperboundsrelitivlyarbitrarily,feelfreetoincreasethem.dynConf_min.TicksMM=10;dynConf_min.DriftFactor=-200;dynConf_min.RevCount=-32760;dynamic_reconfigure_server->setConfigMin(dynConf_min); rosaria::RosAriaConfigdynConf_max;dynConf_max.trans_accel=robot->getAbsoluteMaxTransAccel()/1000;dynConf_max.trans_decel=robot->getAbsoluteMaxTransDecel()/1000;//TODO:Fixrqtdynamic_reconfigureguitohandleemptyintervals//Untilthen,setunitlengthinterval.dynConf_max.lat_accel=((robot->getAbsoluteMaxLatAccel()>0.0)?robot->getAbsoluteMaxLatAccel():0.1)/1000;dynConf_max.lat_decel=((robot->getAbsoluteMaxLatDecel()>0.0)?robot->getAbsoluteMaxLatDecel():0.1)/1000;dynConf_max.rot_accel=robot->getAbsoluteMaxRotAccel()*M_PI/180;dynConf_max.rot_decel=robot->getAbsoluteMaxRotDecel()*M_PI/180;//I'msettingtheseupperboundsrelitivlyarbitrarily,feelfreetoincreasethem.dynConf_max.TicksMM=200;dynConf_max.DriftFactor=200;dynConf_max.RevCount=32760;dynamic_reconfigure_server->setConfigMax(dynConf_max); rosaria::RosAriaConfigdynConf_default;dynConf_default.trans_accel=robot->getTransAccel()/1000;dynConf_default.trans_decel=robot->getTransDecel()/1000;dynConf_default.lat_accel=robot->getLatAccel()/1000;dynConf_default.lat_decel=robot->getLatDecel()/1000;dynConf_default.rot_accel=robot->getRotAccel()*M_PI/180;dynConf_default.rot_decel=robot->getRotDecel()*M_PI/180;dynConf_default.TicksMM=TicksMM;dynConf_default.DriftFactor=DriftFactor;dynConf_default.RevCount=RevCount;dynamic_reconfigure_server->setConfigDefault(dynConf_max);dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB,this,_1,_2));//Enablethemotorsrobot->enableMotors();//disablesonarsonstartuprobot->disableSonar();//callbackwillbecalledbyArRobotbackgroundprocessingthreadforeverySIPdatapacketreceivedfromrobotrobot->addSensorInterpTask("ROSPublishingTask",100,&myPublishCB);//Initializebumperswithrobotnumberofbumpersbumpers.front_bumpers.resize(robot->getNumFrontBumpers());bumpers.rear_bumpers.resize(robot->getNumRearBumpers());//RunArRobotbackgroundprocessingthreadrobot->runAsync(true);return0;}voidRosAriaNode::spin(){ ros::spin();}voidRosAriaNode::publish(){//Note,thisiscalledviaSensorInterpTaskcallback(myPublishCB,named"ROSPublishingTask").ArRobotobject'robot'sholudnotbelockedorunlocked.pos=robot->getPose(); tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180),tf::Vector3(pos.getX()/1000,pos.getY()/1000,0)),position.pose.pose);//Ariareturnsposeinmm.position.twist.twist.linear.x=robot->getVel()/1000;//Ariareturnsvelocityinmm/s.position.twist.twist.linear.y=robot->getLatVel()/1000.0;position.twist.twist.angular.z=robot->getRotVel()*M_PI/180;position.header.frame_id=frame_id_odom;position.child_frame_id=frame_id_base_link;position.header.stamp=ros::Time::now();pose_pub.publish(position); ROS_DEBUG("RosAria:publish:(time%f)posex:%f,y:%f,angle:%f;linearvelx:%f,y: