ros服务模型编程通信

测试两个数相加
1、定义服务请求与应答的方式

进入工作空间，定义srv文件

cd comm
mkdir srv
gedit AddTwoInts.srv

输入以下代码：

int64 a
int64 b
---
int64 sum

需要在package.xml中添加功能包依赖。

cd ~/comm_ws/src/comm
gedit package.xml

输入以下代码：

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

修改CMakeLists.txt
gedit CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  actionlib_msgs
  actionlib
)
## Generate services in the 'srv' folder
add_service_files(
   FILES AddTwoInts.srv
#   Service1.srv
#   Service2.srv
)

## Generate actions in the 'action' folder
 add_action_files(
   FILES 
   turtleMove.action
)

## Generate added messages and services with any dependencies listed here
generate_messages(
   DEPENDENCIES
   std_msgs
   actionlib_msgs
 )
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES comm
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
 CATKIN_DEPENDS roscpp rospy std_msgs
  message_runtime
)
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

touch fa1srv.cpp
gedit fa1srv.cpp

/*  
　　　此程序通过通过动作编程实现由client发布一个目标位置
　　　然后控制Turtle运动到目标位置的过程
 */
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "comm/turtleMoveAction.h"
#include <turtlesim/Pose.h> 
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
 
typedef actionlib::SimpleActionServer<comm::turtleMoveAction> Server;
 
struct Myturtle
{
    float x;
    float y;
    float theta;
}turtle_original_pose,turtle_target_pose;
 
ros::Publisher turtle_vel;
 
void posecallback(const turtlesim::PoseConstPtr& msg) 
{ 
  ROS_INFO("turtle1_position:(%f,%f,%f)",msg->x,msg->y,msg->theta);
  turtle_original_pose.x=msg->x; 
  turtle_original_pose.y=msg->y;
  turtle_original_pose.theta=msg->theta;
 }
 
// 收到action的goal后调用该回调函数
void execute(const comm::turtleMoveGoalConstPtr& goal, Server* as)
{
    comm::turtleMoveFeedback feedback;
 
    ROS_INFO("TurtleMove is working.");
    turtle_target_pose.x=goal->turtle_target_x;
    turtle_target_pose.y=goal->turtle_target_y; 
    turtle_target_pose.theta=goal->turtle_target_theta;
    
    geometry_msgs::Twist vel_msgs;
    float break_flag;
    
    while(1)
    {  
        ros::Rate r(10);
        
        vel_msgs.angular.z = 4.0 * (atan2(turtle_target_pose.y-turtle_original_pose.y,
                                   turtle_target_pose.x-turtle_original_pose.x)-turtle_original_pose.theta);
        vel_msgs.linear.x = 0.5 * sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +
                                      pow(turtle_target_pose.y-turtle_original_pose.y, 2)); 
        break_flag=sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +
                                        pow(turtle_target_pose.y-turtle_original_pose.y, 2));
        turtle_vel.publish(vel_msgs);
 
        feedback.present_turtle_x=turtle_original_pose.x;
        feedback.present_turtle_y=turtle_original_pose.y;
        feedback.present_turtle_theta=turtle_original_pose.theta;
        as->publishFeedback(feedback);
        ROS_INFO("break_flag=%f",break_flag);
        if(break_flag<0.1) break;
        r.sleep();
    }
        // 当action完成后，向客户端返回结果
        ROS_INFO("TurtleMove is finished.");
        as->setSucceeded();
}
 
int main(int argc, char** argv)
{
    ros::init(argc, argv, "turtleMove");
    ros::NodeHandle n,turtle_node;
    ros::Subscriber sub = turtle_node.subscribe("turtle1/pose",10,&posecallback); //订阅小乌龟的位置信息
    turtle_vel = turtle_node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);//发布控制小乌龟运动的速度
    // 定义一个服务器
        Server server(n, "turtleMove", boost::bind(&execute, _1, &server), false);
        // 服务器开始运行
        server.start();
        ROS_INFO("server has started.");
    ros::spin();
 
    return 0;
}

touch jie1srv.cpp
gedit jie1srv.cpp

#include <actionlib/client/simple_action_client.h>
#include "comm/turtleMoveAction.h"
#include <turtlesim/Pose.h> 
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h> 
typedef actionlib::SimpleActionClient<comm::turtleMoveAction> Client;
struct Myturtle
{
    float x;
    float y;
    float theta;
}turtle_present_pose;
 
// 当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
        const comm::turtleMoveResultConstPtr& result)
{
    ROS_INFO("Yay! The turtleMove is finished!");
    ros::shutdown();
}
 
// 当action激活后会调用该回调函数一次
void activeCb()
{
    ROS_INFO("Goal just went active");
}
 
// 收到feedback后调用该回调函数
void feedbackCb(const comm::turtleMoveFeedbackConstPtr& feedback)
{
    ROS_INFO(" present_pose : %f  %f  %f", feedback->present_turtle_x,
                   feedback->present_turtle_y,feedback->present_turtle_theta);
}
 
int main(int argc, char** argv)
{
    ros::init(argc, argv, "turtleMoveClient");
 
    // 定义一个客户端
    Client client("turtleMove", true);
 
    // 等待服务器端
    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");
 
    // 创建一个action的goal
    comm::turtleMoveGoal goal;
    goal.turtle_target_x = 1;
    goal.turtle_target_y = 1;
    goal.turtle_target_theta = 0;
 
    // 发送action的goal给服务器端，并且设置回调函数
    client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);
 
    ros::spin();
 
    return 0;
}

5、将目录切换到工作区目录，并执行catkin_make运行命令：
cd ~/comm_ws
catkin_make
6、新开一个终端启动ros
roscore
7、在原来的终端
cd ~/comm_ws
source ./devel/setup.bash
此处是发布者的
8、在新开一个终端
cd ~/comm_ws
source ./devel/setup.bash此处是订阅者
9、分别在两个终端下
rosrun comm fa1srv
rosrun comm client 2 4
运行两个结点即可。