一、创建功能包
在Linux下,Ros的工作空间路径为:~/src/catkin_ws
,执行如下指令:
cd ~/catkin_ws/src
catkin_create_pkg myServiceSpawn roscpp rospy std_msgs geometry_msgs turtlesim
创建功能包myServiceSpawn。
注意:可以自行命名功能包以及下文的文件名,执行命令后要对应上即可。
二、创建Clint请求
在功能包myServiceSpawn
下,新建scripts
文件夹用来存储Python脚本,随后在该目录下创建turtle_spawn.py
文件
Python实现:
#!usr/bin/env python
#-*- coding: utf-8 -*-
import sys
import rospy
import random
from turtlesim.srv import Spawn
def turtle_spawn(x,y,name):
rospy.init_node("turtle_spawn")
rospy.wait_for_service("/spawn")
try:
add_turtle = rospy.ServiceProxy("/spawn",Spawn)
respone = add_turtle(x,y,0.0,name)
return respone.name
except rospy.ServiceException as e:
rospy.logerr("!ERROR! "+e.__str__())
if __name__=="__main__":
x = 0.2
y = 0.2
for i in range(2,120):
rospy.loginfo("Spawn successlly! the name is %s",turtle_spawn(x,y,"turtle"+str(i)))
x += 1
if x > 10:
y += 1
x = 1
if y > 20:
break
三、执行
首先启动master节点:
roscore
随后启动turtlesim_node:
rosrun turtlesim turtlesim_node
执行python脚本:
python ~/catkin_ws/src/myServiceSpawn/scripts/turtle_spawn.py
四、补充
采用C++也可以实现请求增加新乌龟,在功能包中的src目录下创建turtle_spawn.cpp文件,代码如下:
#include <ros/ros.h>
#include <turtlesim/Spawn.h>
int main(int argc,char **argv)
{
ros::init(argc,argv,"turtle_spawn");
ros::NodeHandle node;
ros::service::waitForService("/spawn");
ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
//init the request
turtlesim::Spawn srv;
srv.request.x = 2.0;
srv.request.y = 2.0;
srv.request.name = "turtle5";
ROS_INFO("Call service to spawn turtle:[x:%0.6f, y:%0.6f, name:%s]",srv.request.x,srv.request.y,srv.request.name.c_str());
add_turtle.call(srv);
ROS_INFO("Spawn successfully! you have named it as %s",srv.response.name.c_str());
return 0;
};
在功能包中的CMakeLists.txt添加:
add_executable(turtle_spawn src/turtle_spawn.cpp)
target_link_libraries(turtle_spawn ${catkin_LIBRARIES})
回到catkin_ws工作空间编译后执行:
cd ~/catkin_ws
catkin_make
rosrun myServiceSpawn turtle_spawn
Comments NOTHING