ROS——Client Library(含代码实例)

Posted on Edited on In

为了更好的使用ROS编程，ROS提供了Client Library，也就是一个编程库，可以用来建立node，发布消息，调用服务等等。其中有cpp版本的roscpp，python版本的rospy，还有lisp版本的roslisp，但是显然我们用的最多的是cpp与python版本。这里会编写一些简单的通信实例，以得到更好的学习效果。

roscpp

先介绍一些用cpp编写的通信实例，这些过程都很简单，实际上在ros/tutorials上都有更清晰的介绍。

Topic

首先编写一个简单的发布/订阅实例。最基本的，是在workspace中建立package。在建立package时，我们还可以在包名后添加依赖： 

1
catkin_create_pkg topic_demo rospy roscpp std_msgs

建立好package之后新建目录src，msg，分别用来存放源代码与msg格式。我们首先定义消息的格式，这里命名为gps.msg，也就是发送的是位置信息，包含坐标和状态： 

1
2
3
4
# content of gps.msg
float32 x
float32 y
string state

这样我们就定义了一个非常简单的Message。在编译之后，catkin会在devel/include/topic_demo目录下生成一个gps.h文件，这个是我们实际上之后要使用的头文件。

接着，在src中，我们创建talker.cpp，与listener.cpp，分别编写publisher和subscriber的node。实际上这些代码看起来也很容易理解： 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
//talker.cpp
#include<ros/ros.h>
#include<topic_demo/gps.h>

int main(int argc, char **argv)
{
	ros::init(argc,argv,"talker");
	ros::NodeHandle nh;
	topic_demo::gps msg;
	msg.x = 1.0;
	msg.y = 1.0;
	msg.state = "working";
	ros::Publisher pub = nh.advertise<topic_demo::gps>("gps_info",1);
	ros::Rate loop_rate(1.0);
	while(ros::ok())
	{
		msg.x = 1.3*msg.x;
		msg.y = 1.2*msg.y;
		ROS_INFO("talker: GPS x = %f, y = %f",msg.x, msg.y);
		pub.publish(msg);
		loop_rate.sleep();	
	}
        return 0;
}

首先，我们要对ros::init传入argc，argv参数，以及这个Node的名称，并且通过ros::NodeHandle建立句柄，这是建立Node都需要的步骤。接下来定义了一个gps变量，通过句柄的advertise来得到publisher，之后也可以看到，subscriber，server还有client等等都是通过这个handle得到的。从publisher也可以看出topic名称为gps_info。接下来的ros::Rate是一个控制频率的类，上述代码表示频率是1Hz，也就是一秒发送一下。之后的内容也很简单，就是简单发送的过程。上述代码实现了一个一秒发送一次位置信息的Node。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
//content of listener.cpp
#include<ros/ros.h>
#include<topic_demo/gps.h>
#include<std_msgs/Float32.h>
void gpsCallBack(const topic_demo::gps::ConstPtr &msg)
{
std_msgs::Float32 distance;
distance.data = sqrt(msg->x*msg->x + msg->y * msg->y);
ROS_INFO("listener: Distance to origin = %f, state = %s", distance.data, msg->state.c_str());
}
int main(int argc,char **argv)
{
ros::init(argc,argv,"listener");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("gps_info",1,gpsCallBack);
ros::spin();//反复执行,阻塞
//ros::spinOnce();//非阻塞
return 0;
}

对于订阅者，首先需要一个回调函数，也就是收到信息后做的事情，这个被定义在gpsCallBack中。值得注意的是函数ros::spin，只有代码中使用了这个函数，回调函数才会被执行，这个是一定要记住的。ros::spin是一个阻塞函数，也就是当没有收到消息，进程会被阻塞而非继续执行之后的内容。另外一个类似的ros::spinOnce，在收到消息执行一次回调函数后，就会执行之后的内容了。

在做完这些之后，还需要修改CMakeLists.txt，需要修改或添加： 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
find_package(catkin REQUIRED COMPONENTS
  message_generation
  roscpp
  rospy
  std_msgs
)
add_message_files(
   FILES
   gps.msg
)
generate_messages(
   DEPENDENCIES
   std_msgs
)
add_executable(talker src/talker.cpp)
add_dependencies(talker topic_demo_generate_messages_cpp)
target_link_libraries(talker ${catkin_LIBRARIES})

add_executable(listener src/listener.cpp)
add_dependencies(listener topic_demo_generate_messages_cpp)
target_link_libraries(listener ${catkin_LIBRARIES})

同时对package.xml也进行修改： 

1
2
3
4
5
6
7
8
9
10
11
12
13
<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_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>

<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>

最后，回到workspace目录，输入catkin_make编译，使用rosrun pkg_name node_name来运行node。如果找不到package，看是不是没有将package加入环境变量，可以使用source workspace/devel/setup.bash命令来做到这一点。

Service

下面是一个Service的实例，与topic是很类似的。同样建立service_demo的package，添加依赖项。我们编写一个客户端与服务端打招呼的简单实例，将这个服务命名为greeting。在src下有两个源代码：server.cpp，client.cpp。同时记着要编写一个Greeting.srv，也就是service的数据格式： 

1
2
3
4
5
#content of Greeting.srv
string name
int32 age
---
string feedback

经过编译后，上述文件会在devel/include/service_demo/目录下生成Greeting.h，GreetingRequest.h，GreetingResponse.h三个头文件。不过Greeting.h中就include了后面二者，因此无需重复include。

下面分别是server.cpp与client.cpp的内容。 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
//content of server.cpp
#include<ros/ros.h>
#include<service_demo/Greeting.h>

bool handleFuntion(service_demo::Greeting::Request &req, service_demo::Greeting::Response &res)
{
    ROS_INFO("Request from %s, age %d", req.name.c_str(), req.age);
    res.feedback="Hi, "+req.name+".";
    return true;
}
int main(int argc,char **argv)
{
ros::init(argc,argv,"greeting_server");
ros::NodeHandle nh;
ros::ServiceServer service = nh.advertiseService("greeting",handleFuntion);
ros::spin();
return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
//content of client.cpp
#include<ros/ros.h>
#include<service_demo/Greeting.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "greeting_client");
    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<service_demo::Greeting>("greeting");
    service_demo::Greeting srv;
    srv.request.name="wlsdzyzl";
    srv.request.age = 23;
    if(client.call(srv))
    {
        ROS_INFO("Feedback from server :%s", srv.response.feedback.c_str());
    }
    else
    {
        ROS_ERROR("Failed to call service.");
        return 1;
    }
    return 0;
}

最后，对CMakeLists.txt与package.xml做类似的修改，实际上修改是很容易的，因为生成package时候给了模板和注释内容，仔细阅读取消对应注释，修改名称即可。

rospy

python的写法与cpp还是有很大不同的，但是一般来说python的写法更简单。下面我们将用python写两个功能与上述cpp实例一样的实例。

Topic

对于*.msg文件的内容我们无需改动。在新建package（topic_demo_py）之后，新建目录scripts，用来存放python脚本。新建pytalker.py与pylistener.py两个文件，内容分别如下： 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
# content of pytalker.py
#!/usr/bin/env python
import rospy
import math
from topic_demo_py.msg import gps

def talker():

    pub = rospy.Publisher('gps_info',gps,queue_size=1)
    rospy.init_node('pytalker',anonymous=True)
    rate = rospy.Rate(1)
    x=1.2
    y=1.3
    state = "working"
    while not rospy.is_shutdown():
        rospy.loginfo("talker x = %f, y = %f",x,y)
        pub.publish(gps(x,y,state))
        x = math.pow(x,2)
        y = math.pow(y,2)
        rate.sleep()
if __name__ == '__main__':
    talker()

这里的代码也是很容易理解的，基本看字面就能知道意思，值得一提的是anonymous参数，这确保了结点名称的唯一性，通过对前面的NAME后添加随机数字。而第一行的注释： 

1
#!/usr/bin/env python

是每个python编写的ros脚本之前都需要加的，表明这个脚本按照python执行。下面是pylistener.py的内容： 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
# content of pylistener.py
#!/usr/bin/env python
import rospy
import math
from topic_demo_py.msg import gps

def callback(gps):
    distance = math.sqrt(math.pow(gps.x,2)+math.pow(gps.y,2))
    rospy.loginfo("distance = %f, state = %s",distance,gps.state)

def listener():
    rospy.init_node('pylistener')
    rospy.Subscriber('gps_info',gps,callback)
    rospy.spin()
if __name__ == '__main__':
    listener()

最后还需要注意的是，尽管python是脚本应当可以直接执行，但是观察上面代码我们发现一行from topic_demo_py.msg import gps，这个是需要message_generation生成的，因此我们依然需要对CMakeLists.txt（不需要生成可执行文件的部分）与package.xml进行改动，用来生成python需要的包。

最后，在进行catkin_make与source workspace/devel/setup.bash之后，就可以通过下面的命令来启动结点了： 

1
rosrun pkg_name pyscript.py

这时候如果你是ubuntu，可能发现执行不成功，因为我们编写的python脚本没有可执行的权限，需要使用下面的命令增加权限： 

1
chmod +x pyscript.py

接下来应该就正常了。

Service

对于Service来说，基本上也是一致的。除了编译后会生成多个需要引入的包，为了方便我们使用*来全部引入，下面就只贴出代码，其他的不过多介绍了。 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
# content of pyserver.py
#!/usr/bin/env python

import rospy
from service_demo_py.srv import *

def handle_function(req):
    rospy.loginfo("name: %s, age: %d",req.name,req.age)
    return GreetingResponse("Hi, "+req.name)
def server():
    rospy.init_node("pyservice")
    srv = rospy.Service("greeting",Greeting,handle_function)
    rospy.spin()
if __name__ =="__main__":
    server()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
# content of pyclient.py
#!/usr/bin/env python

import rospy
from service_demo_py.srv import *

def client():
    rospy.init_node("pyclient")
    #rospy.wait_for_service("greeting")
    name = 'wlsdzyzl'
    age = 23
    req = GreetingRequest(name,age)
    cl = rospy.ServiceProxy("greeting",Greeting)
    resp= cl(req)
    rospy.loginfo(resp.feedback)
if __name__ == "__main__":
    client()