ROS-消息、服务

01.Creating a ROS msg and srv (C++)

1.1 createing a msg

msg/Num.msg

string first_name
string last_name
uint8 age
uint32 score

package.xml

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS  
   roscpp
   rospy
   std_msgs
   message_generation  ## message_generation  必须有
)
...
catkin_package(
  CATKIN_DEPENDS message_runtime ...  ## message_runtime 必须有
)
## 编译的时候，添加消息文件
add_message_files(
  FILES   Num.msg
)
## 生成消息
generate_messages(
  DEPENDENCIES
  std_msgs
)

1.2 createing a srv

srv/AddTwoInts.srv

int64 A
int64 B
---
int64 Sum

package.xml

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS  
   roscpp
   rospy
   std_msgs
   message_generation  ## message_generation  必须有
)
...
catkin_package(
  CATKIN_DEPENDS message_runtime ...  ## message_runtime 必须有
)
## 编译的时候，添加服务文件
add_service_files(
  FILES  AddTwoInts.srv
)

注意：
服务和消息添加成功后，就可以通过rosmsg list 或者rosservice list查看到添加的消息和服务，不需要执行catkin_make

1.3 编译

可以看到编译成功后，会在devel/include/package目录中，生成如下头文件
服务包含三个：服务、服务请求、服务应答；
消息包含一个；

02.Writing a Service Node(C++)

service节点用于向外提供服务，一直运行

代码重点说明：

1. 创建一个服务: ros::ServiceServer service = n.advertiseService("add_two_ints", add); 注意第一个参数是服务的名称
2. 回调函数：bool add(beginner_tutorials::AddTwoInts::Request &req,beginner_tutorials::AddTwoInts::Response &res)
   回调函数接收在service file中定义的请求和应答，返回一个bool值，成功返回ture，表示服务正常；
3. 需要把前面创建的服务的头文件包含进来。

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"

bool add(beginner_tutorials::AddTwoInts::Request  &req,
         beginner_tutorials::AddTwoInts::Response &res)
{
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("sending back response: [%ld]", (long int)res.sum);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("add_two_ints", add);
  ROS_INFO("Ready to add two ints.");
  ros::spin();

  return 0;
}

03.Writing a Client Node(C++)

代码重点说明：

1. 创建一个客户端: ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");注意在创建client的时候，需要指定服务的类型<beginner_tutorials::AddTwoInts>和服务名称"add_two_ints"
2. 创建一个服务对象，beginner_tutorials::AddTwoInts srv;用来向server传递服务请求；
3. 服务对象的req部分由客户根据需要补充：比如srv.request.a=XXX，当req全部确定后，调用服务，获取结果client.call(srv)
4. 使用服务返回的结果：srv.response.sum
5. 如果client.call(srv)的时候，没有启动服务，就会出现如下报错。所以waitForServer()还是有点作用的，可以不报错。
   

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
#include <cstdlib>

int main(int argc, char **argv)
{
  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;
  // ros::service::waitForService("add_two_ints");
  // 为了保证在client调用之前，服务是启动的，可以先执行waitForService
  ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
  beginner_tutorials::AddTwoInts srv;
  srv.request.a = atoll(argv[1]);
  srv.request.b = atoll(argv[2]);
  if (client.call(srv))
  {
    ROS_INFO("Sum: %ld", (long int)srv.response.sum);
  }
  else
  {
    ROS_ERROR("Failed to call service add_two_ints");
    return 1;
  }
  return 0;
}

• 总结

   ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
   // 创建一个service client
   beginner_tutorials::AddTwoInts srv;
   // 创建一个服务消息对象：srv
  srv.request.a = atoll(argv[1]);
  srv.request.b = atoll(argv[2]);
  // 为消息的request进行赋值
  client.call(srv);
  // 调用服务，成功后，应答存在srv.response中

04.调用服务的另外一种方式(C++)

通过ros::service::call("服务名称", 服务请求, 服务应答)进行调用

// #include "std_srvs/SetBool.h" 说明，此处用到的srv，系统提供的
  std_srvs::SetBool srv;  // 创建一个服务对象
  srv.request.data = 0;	// 为服务对象的请求赋值
  while(!ros::service::call("service_server_fortest", srv.request, srv.response)){  
// call 成功后，自动存在srv.response中
        ROS_WARN("Request for map failed; trying again...");
        ros::Duration d(0.5);
        d.sleep();
  }

  ROS_INFO("service call success");
  cout << (srv.response.success?"success=1":"success=0")  <<endl;
  cout << srv.response.message<<endl;

05.(python调用服务) rospy call Service

5.1 （例子）调用turtlesim 的spawn服务

import rospy
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse
 
 
if __name__ == "__main__":
    #初始化ros节点
    rospy.init_node("set_turtle_p")
 
    #创建客户端对象
    client = rospy.ServiceProxy("/spawn", Spawn)
 
    client.wait_for_service()
    # 组织请求数据
    request = SpawnRequest()
    request.x = 2.0
    request.y = 2.0
    request.theta = -1.57
    request.name = "my_turtle_p"
 
    try:
        response = client.call(request)
        # 6.处理响应
        rospy.loginfo("乌龟创建成功!，叫:%s",response.name)
    except expression as identifier:

5.2 （例子）调用gazebo的get_model_state服务获取模型的位置

#!/usr/bin/env python3  
import rospy
import math
import tf
import tf2_ros
import geometry_msgs.msg
from gazebo_msgs.srv import GetModelState, GetModelStateRequest,GetModelStateResponse

if __name__ == '__main__':
    rospy.init_node('tf_pose_node')    
# 创建ropy中的服务客户端：参数1，服务名称；参数2：服务类型
    get_model_serviceClient = rospy.ServiceProxy("/gazebo/get_model_state",GetModelState)
    get_model_serviceClient.wait_for_service()
# 等待服务的服务端启动正常

# 创建服务的请求对象，并给请求对象赋值
    request = GetModelStateRequest()
    request.model_name = "turtlebot3"
    request.relative_entity_name = "world"    
    try:
        rospy.loginfo("call /gazebo/get_model_state ... ")
# resp = XXXclient.call(req)  调用服务
        response = get_model_serviceClient.call(request)                
        rospy.loginfo("call /gazebo/get_model_state success;\t position.x and y: ")
        rospy.loginfo(response.pose.position.x)
        rospy.loginfo(response.pose.position.y)
    except:
        rospy.logwarn("call /gazebo/get_model_state failed ")