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

为了更好的使用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_makesource 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()

# 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()