声明：本文转载，自己进行微微改动方便自己观看，有需要可以看原作者点击这里跳转
一、话题、服务模式编程
1.1 创建工作空间
输入以下三条命令：
mkdir -p ~/comm_ws/src
cd ~/comm_ws/src
catkin_init_workspace
编译工作空间，输入以下命令：
cd ..
catkin_make
在bash中注册工作区
echo $ROS_PACKAGE_PATH
输出工作空间路径代表成功
1.1.1 创建ROS工程包
在工作空间中使用catkin_create_pkg命令去创建一个叫comm(通信)的包，这个包依靠std_msgs、roscpp、rospy。
catkin_create_pkg comm std_msgs rospy roscpp
1.1.2 在工作区编译工程包
输入以下命令：
cd ..
catkin_make
1.2 话题模式编程
1.2.1 创建通信的收、发节点
进入工程包目录
cd ~/comm_ws/src/comm
因为我们已经编译过这个工程包了，所以会在comm文件夹下看到CmakeList.txt、package.xml文件和include、src这两个目录。
可以用ls命令查看：
进入src子目录
cd src
1.2.1.1 在src目录中创建一个发布节点
首先创建一个名为talker.cpp的文件
touch talker.cpp
gedit talker.cpp
并输入以下代码：
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
int main(int argc, char **argv)
{/*** The ros::init() function needs to see argc and argv so that it can perform* any ROS arguments and name remapping that were provided at the command line. For programmatic* remappings you can use a different version of init() which takes remappings* directly, but for most command-line programs, passing argc and argv is the easiest* way to do it. The third argument to init() is the name of the node.** You must call one of the versions of ros::init() before using any other* part of the ROS system.*/ros::init(argc, argv, "talker");/*** NodeHandle is the main access point to communications with the ROS system.* The first NodeHandle constructed will fully initialize this node, and the last* NodeHandle destructed will close down the node.*/ros::NodeHandle n;/*** The advertise() function is how you tell ROS that you want to* publish on a given topic name. This invokes a call to the ROS* master node, which keeps a registry of who is publishing and who* is subscribing. After this advertise() call is made, the master* node will notify anyone who is trying to subscribe to this topic name,* and they will in turn negotiate a peer-to-peer connection with this* node. advertise() returns a Publisher object which allows you to* publish messages on that topic through a call to publish(). Once* all copies of the returned Publisher object are destroyed, the topic* will be automatically unadvertised.** The second parameter to advertise() is the size of the message queue* used for publishing messages. If messages are published more quickly* than we can send them, the number here specifies how many messages to* buffer up before throwing some away.*/ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);ros::Rate loop_rate(10);/*** A count of how many messages we have sent. This is used to create* a unique string for each message.*/int count = 0;while (ros::ok()){/*** This is a message object. You stuff it with data, and then publish it.*/std_msgs::String msg;std::stringstream ss;ss << "hello world " << count;msg.data = ss.str();ROS_INFO("%s", msg.data.c_str());/*** The publish() function is how you send messages. The parameter* is the message object. The type of this object must agree with the type* given as a template parameter to the advertise<>() call, as was done* in the constructor above.*/chatter_pub.publish(msg);ros::spinOnce();loop_rate.sleep();++count;}return 0;
}
点击右上角save保存
1.2.1.2 在src目录中创建一个订阅节点
首先创建一个listener.cpp文件
touch listener.cpp
gedit listener.cpp
打开文件并输入以下代码：
#include "ros/ros.h"
#include "std_msgs/String.h"/*** This tutorial demonstrates simple receipt of messages over the ROS system.*/
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{ROS_INFO("I heard: [%s]", msg->data.c_str());
}int main(int argc, char **argv)
{/*** The ros::init() function needs to see argc and argv so that it can perform* any ROS arguments and name remapping that were provided at the command line. For programmatic* remappings you can use a different version of init() which takes remappings* directly, but for most command-line programs, passing argc and argv is the easiest* way to do it. The third argument to init() is the name of the node.** You must call one of the versions of ros::init() before using any other* part of the ROS system.*/ros::init(argc, argv, "listener");/*** NodeHandle is the main access point to communications with the ROS system.* The first NodeHandle constructed will fully initialize this node, and the last* NodeHandle destructed will close down the node.*/ros::NodeHandle n;/*** The subscribe() call is how you tell ROS that you want to receive messages* on a given topic. This invokes a call to the ROS* master node, which keeps a registry of who is publishing and who* is subscribing. Messages are passed to a callback function, here* called chatterCallback. subscribe() returns a Subscriber object that you* must hold on to until you want to unsubscribe. When all copies of the Subscriber* object go out of scope, this callback will automatically be unsubscribed from* this topic.** The second parameter to the subscribe() function is the size of the message* queue. If messages are arriving faster than they are being processed, this* is the number of messages that will be buffered up before beginning to throw* away the oldest ones.*/ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);/*** ros::spin() will enter a loop, pumping callbacks. With this version, all* callbacks will be called from within this thread (the main one). ros::spin()* will exit when Ctrl-C is pressed, or the node is shutdown by the master.*/ros::spin();return 0;
}
点击save保存
1.2.2 编辑Cmakelist.txt文件（注意：是comm项目包下的CMakelist文件）
gedit CMakeList.txt
在文件末尾输入以下代码：
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker comm_generate_messages_cpp)
add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener comm_generate_messages_cpp)
1.2.3 将目录切换到工作区目录，并执行catkin_make运行命令：
cd ~/comm_ws
catkin_make
1.2.4 测试程序正确性
新开一个终端，启动ROS核心程序
roscore
在原终端注册程序
cd ~/comm_ws
source ./devel/setup.bash
（暂时不要运行这句命令）运行talker节点
rosrun comm talker
新建一个终端运行listener节点（暂时不要运行最后一句命令）
cd ~/comm_ws
source ./devel/setup.bash
rosrun comm listener
准备完成后，先在第一个终端运行talker程序，再在新终端运行listener程序，效果如下：
这表示订阅节点成功接收到了发送节点的信息
可通过CTRL+C结束程序
1.3 服务模式编程
1.3.1 定义服务请求与应答的方式
进入工作空间，定义srv文件
cd comm
mkdir srv
vim AddTwoInts.srv
输入以下代码：
int64 a
int64 b
---
int64 sum
在package.xml中添加功能包依赖
cd ~/comm_ws/src/comm
gedit package.xml
输入以下代码：
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
修改CMakeLists.txt
gedit CMakeLists.txt
1.3.2 代码编写
进入comm的src子目录
touch server.cpp
touch client.cpp
gedit server.cpp
输入以下代码
#include<ros/ros.h>
#include"comm/AddTwoInts.h"
//service回调函数，输入参数req，输出参数res
bool add(comm::AddTwoInts::Request &req,comm::AddTwoInts::Response &res)
{//将输入的参数中的请求数据相加，结果放到应答变量中res.sum=req.a+req.b;ROS_INFO("request: x=%1d,y=%1d",(long int)req.a,(long int)req.b);ROS_INFO("sending back response:[%1d]",(long int)res.sum);return true;
}
int main(int argc,char **argv)
{//ROS节点初始化ros::init(argc,argv,"add_two_ints_server");//创建节点句柄ros::NodeHandle n;//创建一个名为add_two_ints的server,注册回调函数add()ros::ServiceServer service=n.advertiseService("add_two_ints",add);//循环等待回调函数ROS_INFO("Ready to add two ints.");ros::spin();return 0;
}
gedit client.cpp
输入以下代码：
#include<cstdlib>
#include<ros/ros.h>
#include"comm/AddTwoInts.h"
int main(int argc,char **argv)
{//ROS节点初始化ros::init(argc,argv,"add_two_ints_client");//从终端命令行获取两个加数if(argc!=3){ROS_INFO("usage:add_two_ints_client X Y");return 1;}//创建节点句柄ros::NodeHandle n;//创建一个client，请求add_two_ints_service//service消息类型是learning_communication::AddTwoIntsros::ServiceClient client=n.serviceClient<comm::AddTwoInts>("add_two_ints");//创建learning_communication::AddTwoInts类型的service消息comm::AddTwoInts srv;srv.request.a=atoll(argv[1]);srv.request.b=atoll(argv[2]);//发布service请求，等待加法运算的应答请求if(client.call(srv)){ROS_INFO("sum: %1d",(long int)srv.response.sum);}else{ROS_INFO("Failed to call service add_two_ints");return 1;}return 0;
}
设置CMakeLists.txt文件
add_executable(server src/server.cpp)
target_link_libraries(server ${catkin_LIBRARIES})
add_dependencies(server ${PROJECT_NAME}_gencpp)
add_executable(client src/client.cpp)
target_link_libraries(client ${catkin_LIBRARIES})
add_dependencies(client ${PROJECT_NAME}_gencpp)
编译
catkin_make
```运行程序：
新开一个终端：```powershell
roscore
原终端：
source devel/setup.bash
rosrun comm server
再新开一个终端
source devel/setup.bash
rosrun comm client
在客户端输入：
rosrun client server 整数 整数
运行成功！
二、ROS编程，使小海龟移动到指定位置
进入到工程包的src子目录下
cd ~/comm_ws/src/comm/src
2.1 新建服务文件turtleMove.cpp
touch turtleMove.cpp
/* 此程序通过通过动作编程实现由client发布一个目标位置然后控制Turtle运动到目标位置的过程*/
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "comm/turtleMoveAction.h"
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>typedef actionlib::SimpleActionServer<comm::turtleMoveAction> Server;struct Myturtle
{float x;float y;float theta;
}turtle_original_pose,turtle_target_pose;ros::Publisher turtle_vel;void posecallback(const turtlesim::PoseConstPtr& msg)
{ ROS_INFO("turtle1_position:(%f,%f,%f)",msg->x,msg->y,msg->theta);turtle_original_pose.x=msg->x; turtle_original_pose.y=msg->y;turtle_original_pose.theta=msg->theta;}// 收到action的goal后调用该回调函数
void execute(const comm::turtleMoveGoalConstPtr& goal, Server* as)
{comm::turtleMoveFeedback feedback;ROS_INFO("TurtleMove is working.");turtle_target_pose.x=goal->turtle_target_x;turtle_target_pose.y=goal->turtle_target_y; turtle_target_pose.theta=goal->turtle_target_theta;geometry_msgs::Twist vel_msgs;float break_flag;while(1){ ros::Rate r(10);vel_msgs.angular.z = 4.0 * (atan2(turtle_target_pose.y-turtle_original_pose.y,turtle_target_pose.x-turtle_original_pose.x)-turtle_original_pose.theta);vel_msgs.linear.x = 0.5 * sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +pow(turtle_target_pose.y-turtle_original_pose.y, 2)); break_flag=sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +pow(turtle_target_pose.y-turtle_original_pose.y, 2));turtle_vel.publish(vel_msgs);feedback.present_turtle_x=turtle_original_pose.x;feedback.present_turtle_y=turtle_original_pose.y;feedback.present_turtle_theta=turtle_original_pose.theta;as->publishFeedback(feedback);ROS_INFO("break_flag=%f",break_flag);if(break_flag<0.1) break;r.sleep();}// 当action完成后，向客户端返回结果ROS_INFO("TurtleMove is finished.");as->setSucceeded();
}int main(int argc, char** argv)
{ros::init(argc, argv, "turtleMove");ros::NodeHandle n,turtle_node;ros::Subscriber sub = turtle_node.subscribe("turtle1/pose",10,&posecallback); //订阅小乌龟的位置信息turtle_vel = turtle_node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);//发布控制小乌龟运动的速度// 定义一个服务器Server server(n, "turtleMove", boost::bind(&execute, _1, &server), false);// 服务器开始运行server.start();ROS_INFO("server has started.");ros::spin();return 0;
}
2.2 创建小乌龟“发布目标位置文件”turtleMoveClient.cpp
输入以下命令：
touch turtleMoveClient.cpp
打开此文件并输入以下代码
gedit turtleMoveClient.cpp
#include <actionlib/client/simple_action_client.h>
#include "comm/turtleMoveAction.h"
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
typedef actionlib::SimpleActionClient<comm::turtleMoveAction> Client;
struct Myturtle
{float x;float y;float theta;
}turtle_present_pose;// 当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,const comm::turtleMoveResultConstPtr& result)
{ROS_INFO("Yay! The turtleMove is finished!");ros::shutdown();
}// 当action激活后会调用该回调函数一次
void activeCb()
{ROS_INFO("Goal just went active");
}// 收到feedback后调用该回调函数
void feedbackCb(const comm::turtleMoveFeedbackConstPtr& feedback)
{ROS_INFO(" present_pose : %f %f %f", feedback->present_turtle_x,feedback->present_turtle_y,feedback->present_turtle_theta);
}int main(int argc, char** argv)
{ros::init(argc, argv, "turtleMoveClient");// 定义一个客户端Client client("turtleMove", true);// 等待服务器端ROS_INFO("Waiting for action server to start.");client.waitForServer();ROS_INFO("Action server started, sending goal.");// 创建一个action的goalcomm::turtleMoveGoal goal;goal.turtle_target_x = 1;goal.turtle_target_y = 1;goal.turtle_target_theta = 0;// 发送action的goal给服务器端，并且设置回调函数client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);ros::spin();return 0;
}
2.3 在功能包目录下创建action文件夹
输入以下命令:
cd ..
mkdir action
进入action文件夹并创建turtleMove.action文件
cd action
touch turtleMove.action
打开此文件并输入以下代码
action是动作编程里的处理代码，相当于servlet，一个用户的请求发过来，交个action处理，处理完了返回需要返回的界面，其实就是一个自己写的类，让这个类处理，这个类里有很多方法，交给你指定的方法处理
2.4 修改CMakeLists.txt文件内容
进入工程包目录
cd ..
打开CMakeLists.txt文件
sudo gedit CMakeLists.txt
在文件末尾输入以下代码
add_executable(turtleMoveClient src/turtleMoveClient.cpp)
target_link_libraries(turtleMoveClient ${catkin_LIBRARIES})
add_dependencies(turtleMoveClient ${PROJECT_NAME}_gencpp)
add_executable(turtleMove src/turtleMove.cpp)
target_link_libraries(turtleMove ${catkin_LIBRARIES})
add_dependencies(turtleMove ${PROJECT_NAME}_gencpp)
在该文件中找到find_package函数方法，修改为如下：
find_package(catkin REQUIRED COMPONENTSroscpprospystd_msgsmessage_generationactionlib_msgsactionlib
)
继续在该文件中找到add_action_files函数一项，默认是用#注释掉了的，这里我们找到后修改为如下代码：
add_action_files(FILESturtleMove.action)
注意：以上turtleMove.action为我们在action文件夹下面创建的文件名字turtleMove.action
继续在该文件中找到generate_messages函数一项，默认也是#注释，这里修改为如下代码：
generate_messages(DEPENDENCIESstd_msgsactionlib_msgs)
找到catkin_package函数一项，修改为如下代码：
# INCLUDE_DIRS include
# LIBRARIES comm
# CATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_libCATKIN_DEPENDS roscpp rospy std_msgsmessage_runtime
)
保存关闭文件
2.5 修改package.xml文件内容
gedit package.xml
将文件中build_depend一栏、替换为如下代码：
<buildtool_depend>catkin</buildtool_depend><build_depend>roscpp</build_depend><build_depend>rospy</build_depend><build_depend>std_msgs</build_depend><build_depend>message_generation</build_depend><build_depend>actionlib</build_depend><build_depend>actionlib_msgs</build_depend><build_export_depend>roscpp</build_export_depend><build_export_depend>rospy</build_export_depend><build_export_depend>std_msgs</build_export_depend>
将文件中exec_depend一栏、替换为如下代码：
<exec_depend>roscpp</exec_depend><exec_depend>rospy</exec_depend><exec_depend>std_msgs</exec_depend><exec_depend>message_runtime</exec_depend><exec_depend>actionlib</exec_depend><exec_depend>actionlib_msgs</exec_depend>
保存关闭
2.6 运行程序进行测试
2.6.1 进入工作空间
cd ~/comm_ws
catkin_make
编译成功
注册程序
source ./devel/setup.bash
不要关闭此终端（终端1），输入以下命令但先不要运行
rosrun comm turtleMove
2.6.2 新建一个终端，启动ros核心程序
roscore
2.6.3 再新建一终端（终端3），运行小海龟
rosrun turtlesim turtlesim_node
2.6.4 再新建一个终端(终端4)运行目标位置程序代码：
cd ~/comm_ws
source ./devel/setup.bash
输入以下命令，但先不要运行
rosrun comm turtleMoveClient
2.6.5 开始测试
终端1回车、终端4回车，出现如下画面，则我们动作编程的实验完美成功
终端4在实时更新小乌龟的位置，而运行窗口则显示小乌龟的移动路径
