0
点赞
收藏
分享

微信扫一扫

ROS系统 实现客户端Client和服务端Server

服务编程流程:

  • 创建服务器
  • 创建客户端
  • 添加编译选项
  • 运行可执行程序

实现客户端Clien

创建功能包

cd ~/catkin_ws/src

catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

ROS系统 实现客户端Client和服务端Server_python

如何实现一个客户端

  • 初始化ROS节点
  • 创建一个Client实例
  • 发布服务请求数据
  • 等待Server处理之后的应答结果

在刚创建的 learning_service 目录中的src文件夹下创建一个 cpp文件

cd ~/catkin_ws/src/learning_service/src

touch turtle_spawn.cpp

在 turtle_spawn.cpp 文件中写入以下代码。
下面代码实现功能:请求/spawn服务,服务数据类型turtlesim::Spawn

#include <ros/ros.h>
#include <turtlesim/Spawn.h>

int&nbsp;main(int&nbsp;argc,&nbsp;char**&nbsp;argv)
{
&nbsp;&nbsp;&nbsp;&nbsp;// 初始化ROS节点
ros::init(argc,&nbsp;argv,&nbsp;&quot;turtle_spawn&quot;);

&nbsp;&nbsp;&nbsp;&nbsp;// 创建节点句柄
ros::NodeHandle&nbsp;node;

&nbsp;&nbsp;&nbsp;&nbsp;// 发现/spawn服务后,创建一个服务客户端,连接名为/spawn的service
ros::service::waitForService(&quot;/spawn&quot;);
ros::ServiceClient&nbsp;add_turtle&nbsp;=&nbsp;node.serviceClient&lt;turtlesim::Spawn&gt;(&quot;/spawn&quot;);

&nbsp;&nbsp;&nbsp;&nbsp;// 初始化turtlesim::Spawn的请求数据
turtlesim::Spawn&nbsp;srv;
srv.request.x&nbsp;=&nbsp;2.0;
srv.request.y&nbsp;=&nbsp;2.0;
srv.request.name&nbsp;=&nbsp;&quot;turtle2&quot;;

&nbsp;&nbsp;&nbsp;&nbsp;// 请求服务调用
ROS_INFO(&quot;Call&nbsp;service&nbsp;to&nbsp;spwan&nbsp;turtle[x:%0.6f,&nbsp;y:%0.6f,&nbsp;name:%s]&quot;,&nbsp;
&nbsp;srv.request.x,&nbsp;srv.request.y,&nbsp;srv.request.name.c_str());

add_turtle.call(srv);

// 显示服务调用结果
ROS_INFO(&quot;Spwan&nbsp;turtle&nbsp;successfully&nbsp;[name:%s]&quot;,&nbsp;srv.response.name.c_str());

return&nbsp;0;
};

在CMakeLists.txt中添加以下代码:

add_executable(turtle_spawn src/turtle_spawn.cpp)
target_link_libraries(turtle_spawn ${catkin_LIBRARIES})

ROS系统 实现客户端Client和服务端Server_python_02

编译项目

cd ~/catkin_ws

catkin_make

ROS系统 实现客户端Client和服务端Server_客户端_03

设置环境变量,让系统能够找到该工作空间

source devel/setup.bash

启动ROS Master

roscore

启动小海龟仿真器

rosrun turtlesim turtlesim_node

启动自定义的海龟控制节点,会在客户端中生成一个新的小海龟。

rosrun learning_service turtle_spawn

ROS系统 实现客户端Client和服务端Server_回调函数_04

用Python创建客户端步骤:

cd ~/catkin_ws/src/learning_service

创建 scripts 文件夹

mkdir scripts

cd scripts

创建 turtle_spawn.py Python文件

touch turtle_spawn.py

用Python创建客户端代码:
下面代码实现功能: 请求/spawn服务,服务数据类型turtlesim::Spawn

import&nbsp;sys
import&nbsp;rospy
from&nbsp;turtlesim.srv&nbsp;import&nbsp;Spawn

def&nbsp;turtle_spawn():
# ROS节点初始化
&nbsp;&nbsp;&nbsp;&nbsp;rospy.init_node(&#39;turtle_spawn')

# 发现/spawn服务后,创建一个服务客户端,连接名为/spawn的service
&nbsp;&nbsp;&nbsp;&nbsp;rospy.wait_for_service(&#39;/spawn')
&nbsp;&nbsp;&nbsp;&nbsp;try:
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;add_turtle&nbsp;=&nbsp;rospy.ServiceProxy(&#39;/spawn', Spawn)

# 请求服务调用,输入请求数据
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;response&nbsp;=&nbsp;add_turtle(2.0,&nbsp;2.0,&nbsp;0.0,&nbsp;&quot;turtle2&quot;)
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;return&nbsp;response.name
&nbsp;&nbsp;&nbsp;&nbsp;except&nbsp;rospy.ServiceException,&nbsp;e:
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;print&nbsp;&quot;Service&nbsp;call&nbsp;failed:&nbsp;%s&quot;%e

if&nbsp;__name__&nbsp;==&nbsp;&quot;__main__&quot;:
#服务调用并显示调用结果
&nbsp;&nbsp;&nbsp;&nbsp;print&nbsp;&quot;Spwan&nbsp;turtle&nbsp;successfully&nbsp;[name:%s]&quot;&nbsp;%(turtle_spawn())

ROS系统 实现客户端Client和服务端Server_回调函数_05

Python代码不需要重新编译,可以直接执行。

rosrun learning_service turtle_spawn.py

ROS系统 实现客户端Client和服务端Server_客户端_06

ROS系统 实现客户端Client和服务端Server_回调函数_07

实现服务端Server

如何实现一个服务器

  • 初始化ROS节点
  • 创建Server实例
  • 循环等待服务请求,进入回调函数
  • 在回调函数中完成服务功能的处理,并反馈应答数据。

在创建的 learning_service 目录中的src文件夹下创建一个 turtle_command_server.cpp文件

cd ~/catkin_ws/src/learning_service/src

touch turtle_command_server.cpp

在 turtle_command_server.cpp 文件中写入以下代码。
下面代码实现功能:执行/turtle_command服务,服务数据类型std_srvs/Trigger

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>

ros::Publisher&nbsp;turtle_vel_pub;
bool&nbsp;pubCommand&nbsp;=&nbsp;false;

// service回调函数,输入参数req,输出参数res
bool&nbsp;commandCallback(std_srvs::Trigger::Request&nbsp;&nbsp;&amp;req,
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; std_srvs::Trigger::Response&nbsp;&amp;res)
{
pubCommand&nbsp;=&nbsp;!pubCommand;

&nbsp;&nbsp;&nbsp;&nbsp;// 显示请求数据
&nbsp;&nbsp;&nbsp;&nbsp;ROS_INFO(&quot;Publish&nbsp;turtle&nbsp;velocity&nbsp;command&nbsp;[%s]&quot;,&nbsp;pubCommand==true?&quot;Yes&quot;:&quot;No&quot;);

// 设置反馈数据
res.success&nbsp;=&nbsp;true;
res.message&nbsp;=&nbsp;&quot;Change&nbsp;turtle&nbsp;command&nbsp;state!&quot;;

&nbsp;&nbsp;&nbsp;&nbsp;return&nbsp;true;
}

int&nbsp;main(int&nbsp;argc,&nbsp;char&nbsp;**argv)
{
&nbsp;&nbsp;&nbsp;&nbsp;// ROS节点初始化
&nbsp;&nbsp;&nbsp;&nbsp;ros::init(argc,&nbsp;argv,&nbsp;&quot;turtle_command_server&quot;);

&nbsp;&nbsp;&nbsp;&nbsp;// 创建节点句柄
&nbsp;&nbsp;&nbsp;&nbsp;ros::NodeHandle&nbsp;n;

&nbsp;&nbsp;&nbsp;&nbsp;// 创建一个名为/turtle_command的server,注册回调函数commandCallback
&nbsp;&nbsp;&nbsp;&nbsp;ros::ServiceServer&nbsp;command_service&nbsp;=&nbsp;n.advertiseService(&quot;/turtle_command&quot;,&nbsp;commandCallback);

// 创建一个Publisher,发布名为/turtle1/cmd_vel的topic,消息类型为geometry_msgs::Twist,队列长度10
turtle_vel_pub&nbsp;=&nbsp;n.advertise&lt;geometry_msgs::Twist&gt;(&quot;/turtle1/cmd_vel&quot;,&nbsp;10);

&nbsp;&nbsp;&nbsp;&nbsp;// 循环等待回调函数
&nbsp;&nbsp;&nbsp;&nbsp;ROS_INFO(&quot;Ready&nbsp;to&nbsp;receive&nbsp;turtle&nbsp;command.&quot;);

// 设置循环的频率
ros::Rate&nbsp;loop_rate(10);

while(ros::ok())
{
// 查看一次回调函数队列
&nbsp;&nbsp;&nbsp;&nbsp; ros::spinOnce();

// 如果标志为true,则发布速度指令
if(pubCommand)
{
geometry_msgs::Twist&nbsp;vel_msg;
vel_msg.linear.x&nbsp;=&nbsp;0.5;
vel_msg.angular.z&nbsp;=&nbsp;0.2;
turtle_vel_pub.publish(vel_msg);
}

//按照循环频率延时
&nbsp;&nbsp;&nbsp;&nbsp;loop_rate.sleep();
}

&nbsp;&nbsp;&nbsp;&nbsp;return&nbsp;0;
}

注:
可以通过以下指令 显示服务类型的所有信息

rossrv show 头文件名

例如:

rossrv show std_srvs/Trigger

ROS系统 实现客户端Client和服务端Server_python_08

在CMakeLists.txt中添加以下代码:

add_executable(turtle_command_server src/turtle_command_server.cpp)
target_link_libraries(turtle_command_server ${catkin_LIBRARIES})

ROS系统 实现客户端Client和服务端Server_python_09
编译项目

cd ~/catkin_ws

catkin_make

ROS系统 实现客户端Client和服务端Server_回调函数_10

设置环境变量,让系统能够找到该工作空间

source devel/setup.bash

启动ROS Master

roscore

启动小海龟仿真器

rosrun turtlesim turtlesim_node

启动自定义的海龟控制节点,会在客户端中生成一个新的小海龟。

rosrun learning_service turtle_command_server

ROS系统 实现客户端Client和服务端Server_回调函数_11

以下指令相当于海龟的开关,发送一次,海龟就会启动,再次发送时,就会让海龟停止运动。

rosservice call /turtle_command "{}"

ROS系统 实现客户端Client和服务端Server_python_12
ROS系统 实现客户端Client和服务端Server_回调函数_13

用Python创建服务端步骤:

cd ~/catkin_ws/src/learning_service

创建 scripts 文件夹

mkdir scripts

cd scripts

创建 turtle_command_server.py Python文件

touch turtle_command_server.py

用Python创建客户端代码:
下面代码实现功能: 执行/turtle_command服务,服务数据类型std_srvs/Trigger

import&nbsp;rospy
import&nbsp;thread,time
from&nbsp;geometry_msgs.msg&nbsp;import&nbsp;Twist
from&nbsp;std_srvs.srv&nbsp;import&nbsp;Trigger,&nbsp;TriggerResponse

pubCommand&nbsp;=&nbsp;False;
turtle_vel_pub&nbsp;=&nbsp;rospy.Publisher(&#39;/turtle1/cmd_vel', Twist, queue_size=10)

def&nbsp;command_thread():
while&nbsp;True:
if&nbsp;pubCommand:
vel_msg&nbsp;=&nbsp;Twist()
vel_msg.linear.x&nbsp;=&nbsp;0.5
vel_msg.angular.z&nbsp;=&nbsp;0.2
turtle_vel_pub.publish(vel_msg)

time.sleep(0.1)

def&nbsp;commandCallback(req):
global&nbsp;pubCommand
pubCommand&nbsp;=&nbsp;bool(1-pubCommand)

# 显示请求数据
rospy.loginfo(&quot;Publish&nbsp;turtle&nbsp;velocity&nbsp;command![%d]&quot;,&nbsp;pubCommand)

# 反馈数据
return&nbsp;TriggerResponse(1,&nbsp;&quot;Change&nbsp;turtle&nbsp;command&nbsp;state!&quot;)

def&nbsp;turtle_command_server():
# ROS节点初始化
&nbsp;&nbsp;&nbsp;&nbsp;rospy.init_node(&#39;turtle_command_server')

# 创建一个名为/turtle_command的server,注册回调函数commandCallback
&nbsp;&nbsp;&nbsp;&nbsp;s&nbsp;=&nbsp;rospy.Service(&#39;/turtle_command', Trigger, commandCallback)

# 循环等待回调函数
&nbsp;&nbsp;&nbsp;&nbsp;print&nbsp;&quot;Ready&nbsp;to&nbsp;receive&nbsp;turtle&nbsp;command.&quot;

&nbsp;&nbsp;&nbsp;&nbsp;thread.start_new_thread(command_thread,&nbsp;())
&nbsp;&nbsp;&nbsp;&nbsp;rospy.spin()

if&nbsp;__name__&nbsp;==&nbsp;&quot;__main__&quot;:
&nbsp;&nbsp;&nbsp;&nbsp;turtle_command_server()

ROS系统 实现客户端Client和服务端Server_回调函数_14

启动ROS Master

roscore

启动小海龟仿真器

rosrun turtlesim turtlesim_node

Python代码不需要重新编译,可以直接执行。

rosrun learning_service turtle_command_server.py

以下指令相当于海龟的开关,发送一次,海龟就会启动,再次发送时,就会让海龟停止运动。

rosservice call /turtle_command "{}"

ROS系统 实现客户端Client和服务端Server_python_15

举报

相关推荐

0 条评论