ROS话题服务动作

节点：

节点可理解为功能或进程，一个机器人有多个功能，因此也就有了多个节点，因此，如果一个机器人的各个功能被分开，也就是一个个的节点，那ROS进行软件的复用就会方便的多。

功能包是ROS中的基本单元，包含src、include等文件夹一个功能包里可以有多个节点，在节点中进行发布和订阅话题；

一个节点就是 ROS 功能包中的一个可执行文件（.cpp、.py可执行源文件）,节点之间可以通过 ROS 客户端库(如roscpp 、 rospy )相互通信。一个机器人控制系统由许多节点组成,这些节点各司其职,如,一个节点控制激光距离传感器,一个节点控制轮子的马达,一个节点执行定位,一个节点执行路径规划,一个节点提供系统的整个视图等。因此executable_name就是说的.cpp/.py源文件名称。

我们的.cpp源文件中的函数init指定的节点名称仅仅在计算图中表明节点之间的信息传递关系，其他的并没有用，有用的是以下两个名字：

1. 节点的executable_name：节点所在.cpp源文件的文件名；

2. 包的名称，即package_name：ROS项目文件夹下src目录中子文件夹的名称。
 

CMakelist.txt文件：

Linux 平台编译 C++ 文件，需要编译器和链接器，其中编译器是将源代码编译成目标代码，链接器是将目标代码链接到可执行文件。若是编译单个文件，用 g++ 即可；若是编译一个 C++ 工程，则需要批处理编译工具，如 make，通过设定规则文件 makefile 调用 g++ 等编译工具进行批量编译。但 makefile 的编写十分复杂，便诞生了 CMake，通过编写简单的 CMakeLists.txt 规则文件，就能自动生成 makefile 文件，并且 CMake 是跨平台的，十分强大。ROS 的编译器便是 CMake，为了更加人性化，ROS 在 CMake 基础上封装了 catkin 命令，用 cmake 命令创建功能包时，会自动生成 CMakeList.txt 文件，已配置了多数编译选项，且包含详细的注释，只需稍作修改便可编译自己的文件。

这个文件直接规定了这个package要依赖哪些package，要编译生成哪些目标，如何编译等等流程。所以 CMakeLists.txt 非常重要，它指定了由源码到目标文件的规则，catkin编译系统在 工作时首先会找到每个package下的 CMakeLists.txt ，然后按照规则来编译构建。

我们细心就会发现有两种CMakelist.txt文件，一个是针对所有package的，一个是针对于某一个特性的package的。这两种不同使用范围的CMake文件回答了：“如何将所有package功能包生成的可执行文件组成我们想要的目标文件——宏观把握“‘以及”如何将某一个特定的package功能包中的源代码生成可执行目标文件——细节掌控“。

CMakelist中添加的  add_executable和  target_link_libraries：

add_executable(demo01_pub src/hello_vscode_c.cpp)

表示”将.cpp源文件映射为demo01_pub相当于给.cpp源文件取了个别名方便链接使用“，告诉它要把hello_vscode_c.cpp编译成src的可执行文件

target_link_libraries(demo01_pub, ${catkin_LIBRARIES})

在CMakeLists.txt文件中，target_link_libraries用于将库文件与目标可执行文件进行链接。这个命令的语法如下：

target_link_libraries(target_name library1 library2 ...)

其中，target_name是你要链接的目标可执行文件的名称，library1 library2 ...是要链接的库文件的名称。

在你提供的代码中，target_link_libraries(demo01_pub, ${catkin_LIBRARIES})的含义如下：

• demo01_pub是目标可执行文件的名称，也就是你要构建的可执行文件的名称。
• ${catkin_LIBRARIES}是一个CMake变量，它包含了要链接的库文件的信息。${catkin_LIBRARIES}是由Catkin构建系统生成的一个变量，包含了所有需要链接到目标可执行文件的库文件。

通过使用target_link_libraries命令并传递${catkin_LIBRARIES}作为参数，你将会把${catkin_LIBRARIES}中列出的库文件链接到demo01_pub可执行文件上，使得在编译和链接过程中可以正确找到并使用这些库文件的函数和功能。

需要注意的是，在使用target_link_libraries命令前，你需要通过其他方式定义${catkin_LIBRARIES}变量，以确保它包含了正确的库文件信息。

话题通讯：
**第一步：**信息的发布者（talker）和接收者（listener）都必须向节点管理器（ROS Master）注册自己的信息，就跟人员登记一样，节点管理器需要知道你这个节点的地址等信息，为什么需要知道呢，因为ROS的节点在实际机器人设计中，可能都存在于一个计算机之中，也可能分布于不同的计算机，因此需要地址等信息来对节点进行确定；
**第二步：**节点管理器在确定需要通信的两个节点之后就会对两个节点的信息进行匹配；
**第三步：**接收者（listener）向发布者（talker）发送自己的TCP通讯协议以及连接请求；
**第四步：**发布者（talker）确定连接请求，然后将自己的TCP地址发送给接收者（listener）；
**第五步：**两个节点正式建立连接，并以TCP协议进行数据传输。

发布端：

#include<ros/ros.h>
#include"homework_pkg/number1.h"
//homework_pkg是工作空间下的功能包名称，number1是包含了数据类型的写在srv中的文件名
int f(int count) {
    if (count == 1 || count == 2) {
        return 1;
    }
    else {
        return f(count - 2) + f(count - 1);
    }
}
//           argc是个数，  argv是字符串指针
int main(int argc, char** argv) {
    //本地初始化，防止中文乱码
     setlocale(LC_ALL,"");
    //ROS节点初始化，前两个固定，节点名称是“number_publisher”，不可重复
    ros::init(argc, argv, "number_publisher");
    //创建节点句柄，相当于一个重命名NodeHandle=n
    ros::NodeHandle n;
    //创建一个publisher,发布名为/fibonacci的topic，消息类型为std_msgs::Int32,队>列长度为100
    ros::Publisher fibonacci_pub = n.advertise<homework_pkg::number1>("/fibonacci", 100);
    //设置循环频率
    ros::Rate loop_rate(10);
    int count = 1;
  while (ros::ok()) {
        //ros::ok()在遇到ctrl+c会退出
        //初始化geometry_sgs::Twist类型消息
        homework_pkg::number1 msg;
        msg.number = f(count);
        //发布消息
        fibonacci_pub.publish(msg);
        //ROS_INFO相当于打印
        ROS_INFO("%d",msg.number);
        //按循环频率延时
        loop_rate.sleep();
        count++;
    }
    return 0;
}

创建发布对象的参数含义：

//创建发布者对象
    ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",10);
    改名                     发布者    发布的消息类型      话题     消息最多看到几个

spin：

对于spin函数，一旦进入spin函数，它就不会返回了，也不继续往后执行了，相当于它在自己的函数里面死循环了（直到ctrl+c　或者程序终止的时候才退出）。
主要的工作，就是不断的检查回调函数队列里面是否有domsg函数存在，如果有的话，它就会马上去执行domsg函数。如果没有的话，它就会阻塞，不会占用CPU。

Python版：

#!/usr/bin/env python

import rospy
from homework_pkg.msg import number1

def f(count):
    if count == 1 or count == 2:
        return 1
    else:
        return f(count - 2) + f(count - 1)

def main():
    # 初始化ROS节点
    rospy.init_node("number_publisher")

    # 创建一个Publisher，发布名为"/fibonacci"的topic，消息类型为homework_pkg/number1
    fibonacci_pub = rospy.Publisher("/fibonacci", number1, queue_size=100)

    # 设置循环频率
    rate = rospy.Rate(10)

    count = 1

    while not rospy.is_shutdown():
        # 初始化homework_pkg/number1类型的消息
        msg = number1()

        msg.number = f(count)

        # 发布消息
        fibonacci_pub.publish(msg)

        # 打印消息
        rospy.loginfo(str(msg.number))

        # 按循环频率延时
        rate.sleep()

        count += 1

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

 

订阅端：

#include<ros/ros.h>
#include"homework_pkg/number1.h"
void poseCallback(const homework_pkg::number1::ConstPtr& msg)
{
    //将接受到的消息打印出来
    ROS_INFO("%d", msg->number);
}
int main(int argc, char** argv)
{
 
    //初始化ROS节点
    ros::init(argc, argv, "number_subscriber");
    //创建节点句柄
    ros::NodeHandle n;
    //创建一个Subscriber,订阅名为/fibonacci的topic,注册回调函数poseCallback
    //在接收到“fibonacci”的话题消息后就会执行回调函数
    ros::Subscriber number_sub = n.subscribe("/fibonacci", 1000, poseCallback);
    //循环等待回调函数
    ros::spin();
    return 0;
}

python版：

#!/usr/bin/env python

import rospy
from homework_pkg.msg import number1

def poseCallback(msg):
    # 将接收到的消息打印出来
    rospy.loginfo(str(msg.number))

def main():
    # 初始化ROS节点
    rospy.init_node("number_subscriber")

    # 创建一个Subscriber，订阅名为"/fibonacci"的topic，消息类型为homework_pkg/number1，注册回调函数poseCallback
    # 在接收到"/fibonacci"话题的消息后就会执行poseCallback回调函数
    rospy.Subscriber("/fibonacci", number1, poseCallback)

    # 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

服务：

 有两种方式可以调用服务：

1 rosservice call +服务名 +参数：
ros::ServiceServer service = n.advertiseService("add_two_ints", add);
advertiseService函数的第一个参数就是你的服务名称
advertiseService函数的第二个参数决定了你的服务类型（经二决定了你的服务需要传入什么参数（request），传出什么参数(response)）,这是由第二个参数（一个函数）操作的结构体的类型（具体的结构体详情在头文件中查看）决定的。

可以看到，用这种方式调用服务，返回的东西也非常简介，不会有[INFO]一类的东西，只会返回一个xx：xxx一类的东西（以例程为例：xx是结构体AddTwoInit中的AddTwoIntsResponse_结构体中的成员sum）

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;
}

服务名称：add_two_ints 由主函数中advertiseService函数给出。

服务类型：rosservice type+服务名称 来查看，结果是头文件中定义的结构体的类型，这个结构体必须包含另外两个结构体：request和response。这两个结构体的成员决定了这个服务类型具体输入什么参数（request成员的数据类型）输出什么参数（response成员的数据类型），可以用rossrv show+服务类型查看。

2、直接rosrun+包含这个服务的节点：

因为创建一个服务之前必须创建一个节点，所以想运行这个服务只需要先运行这个节点就可以。

作业服务端：

#include "ros/ros.h"
#include "hw2/homework.h"

// 回调函数：bool 返回值由于标志是否处理成功
//  hw2::homework::Request是之前创建的srv文件中生成的头文件
bool Callback(hw2::homework::Request& requ,
    hw2::homework::Response& resp)
{
    //1处理请求
    int num1 = requ.num1;
    int num2 = requ.num2;
    ROS_INFO("服务器接收到的请求数据为:num1 = %d, num2 = %d", num1, num2);
    //2组织响应
    int sum = num1 + num2;
    resp.sum = sum;
    ROS_INFO("两个数的和：sum = %d", sum);
    return true;
}    //此处，两个整数被相加，和 已经存储在了响应中。然后记录一些有关请求和响应的信息到日志中。完成后，服务返回true。

int main(int argc, char **argv)
{
     setlocale(LC_ALL,"");
    // 初始化 ROS 节点
    ros::init(argc, argv, "homework_Server");
    // 创建 ROS 句柄
    ros::NodeHandle n;
    // 服务对象被创建，并在ros中宣告，设置server函数，该函数将会在ros系统中生成一个service
    ros::ServiceServer server = n.advertiseService<hw2::homework::Request,hw2::homework::Response>("ttt", Callback);
 //                                                              service的名称  该service功能执行的函数，供相应的client节点调用
    ROS_INFO("服务启动....");
    //     回调函数处理请求并产生响应
     ros::spin();
     return 0;
    //    
  }
~    

作业客户端： 

#include "ros/ros.h"
#include "hw2/homework.h"
int main(int argc, char **argv)
{
    setlocale(LC_ALL,"");
    // 初始化 ROS 节点
    ros::init(argc, argv, "hwt2");
    // 创建 ROS 句柄
    ros::NodeHandle n;
    // 创建 客户端 对象

    ros::ServiceClient client = n.serviceClient<hw2::homework>("ttt");
    // 提交请求并处理响应
    hw2::homework a;
    //提交请求
    a.request.num1 = 3;
    a.request.num2 = 9;
    //阻塞进程，直到得到“ttt”传回的消息才继续往下走
    ros::service::waitForService("ttt");
    bool flag = client.call(a);

    while (flag)
    {
        ROS_INFO(" 响应成功！两个数的和为：%d", a.response.sum);
    }
    else
    {
        ROS_INFO("响应失败");
    }

    ros::spin();
    return 0;

}

python版·：

服务端

#!/usr/bin/env python

import rospy
from hw2.srv import homework, homeworkResponse

def callback(req):
    num1 = req.num1
    num2 = req.num2
    rospy.loginfo("服务器接收到的请求数据为: num1 = %d, num2 = %d", num1, num2)
    
    sum = num1 + num2
    resp = homeworkResponse()
    resp.sum = sum
    rospy.loginfo("两个数的和：sum = %d", sum)
    
    return resp

def homework_server():
    rospy.init_node('homework_Server')
    rospy.loginfo("服务启动....")
    service = rospy.Service("ttt", homework, callback)
    rospy.spin()

if __name__ == '__main__':
    homework_server()

客户端：

#!/usr/bin/env python

import rospy
from hw2.srv import homework

def homework_client():
    rospy.init_node('hw2')
    rospy.wait_for_service('ttt')
    client = rospy.ServiceProxy('ttt', homework)
    req = homeworkRequest()
    req.num1 = 3
    req.num2 = 9

    try:
        resp = client(req)
        rospy.loginfo("响应成功！两个数的和为：%d", resp.sum)
    except rospy.ServiceException as e:
        rospy.loginfo("响应失败")

if __name__ == '__main__':
    homework_client()

动作

action 也是一种类似于 service 的服务端/客户端问答通信机制，不一样的地方是action还带有一个反馈机制，可以周期性的反馈任务的实施进度，而且可以在任务实施过程中，中止运行。
client和server之间通过actionlib定义的“action protocol”进行通讯。这种通讯协议是基于ROS的消息机制实现的，为用户提供了client和server的接口，接口如下图所示：

                                    

client向server端发布任务目标以及在必要的时候取消任务，server会向client发布当前的状态、实时的反馈和最终的任务结果。

goal：任务目标
cancel：请求取消任务
status：通知client当前的状态
feedback：周期反馈任务运行的监控数据
result：向client发送任务的执行结果，这个topic只会发布一次。
动作机制是异步的，运行服务端和客户端采用无阻塞的编程方式。在机器人应用中，执行一个时长不定，目标引导的新任务是很常见的，如 goto_position，clean_the_house。在任何情况下，当需要执行一个任务时，action 可能都是最佳选择。虽然动作需要写更多的代码，但却比服务更强大，扩展性更好。因此，每当你想使用服务时，都应当考虑一下，是否可以替换为动作。

自定义 action

自定义 action
动作定义文件中包含了动作的目标，结果，和反馈的消息格式。通常放在 package 的 action 文件夹下，文件扩展名为.action。
示例：定义一个定时器动作：Timer.action。这个定时器会进行倒计时，并在倒计时停止时发出结果信号告诉我们总的计数时长，倒计时过程中会定期反馈剩余时间。
 

# This is an action definition file, which has three parts: the goal, the
# result, and the feedback.

# 1. the goal, to be sent by the client
# The amount of time we want to wait
duration time_to_wait
---
# 2: the result, to be sent by the server upon completion
# How much time we waited
duration time_elapsed
# How many updates we provided along the way
uint32 updates_sent
---
# 3: the feedback, to be sent periodically by the server during execution.
# The amount of time that has elapsed from the start
duration time_elapsed
# The amount of time remaining until we're done
duration time_remaining

在CMakeLists.txt文件中添加如下的编译规则：

find_package(catkin REQUIRED actionlib_msgs)
add_action_files(DIRECTORY action FILES Timer.action)
generate_messages(DEPENDENCIES actionlib_msgs std_msgs)
catkin_package(CATKIN_DEPENDS actionlib_msgs)

还需要在功能包的package.xml中添加：

<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<run_depend>actionlib</run_depend> 

编译完成后会在 devel 文件夹产生一系列的.msg文件:
TimerAction.msg
TimerActionGoal.msg
TimerActionResult.msg
TimerActionFeedback.msg
TimerGoal.msg
TimerResult.msg
TimerFeedback.msg
 

题目：使用 ROS 动作(Action)机制实现目标请求、进度与完成结果的反馈。

要求: 编写代码实现 ROS 中动作请求与进度反馈，创建服务端，注册 Action，客户端发送action 请求检测 40 个零件， 服务端接收后，每隔 1s 测一个零件 (每检测一个打印一次)，:实时给客户端返回检测进度(客户端打印进度百分比)，并在检测完毕时告知客户端目标完成。如，服务端实时打印: 检测 1个零件 检测2 个零件 ...检测 40 个零件 检测完成客户端实时打印: 2.5% 5% ... 100% 检测完成

Python版：

服务端：

#!/usr/bin/env python

import rospy
import actionlib
from std_msgs.msg import Float32
from my_robot_msgs.msg import CheckPartsAction, CheckPartsResult

class CheckPartsServer:
    def __init__(self):
        self.parts_total = 40  # 零件总数
        self.parts_checked = 0  # 已检测的零件数
        self.progress_pub = rospy.Publisher('check_parts_progress', Float32, queue_size=10)

        # 创建动作服务器
        self.server = actionlib.SimpleActionServer('check_parts', CheckPartsAction, self.execute_callback, False)
        self.server.start()

    def execute_callback(self, goal):
        rate = rospy.Rate(1)  # 每秒钟检测一个零件
        result = CheckPartsResult()

        while self.parts_checked < self.parts_total:
            # 更新进度
            progress = float(self.parts_checked) / float(self.parts_total) * 100
            self.progress_pub.publish(progress)

            # 打印检测信息
            rospy.loginfo("检测 %d 个零件", self.parts_checked)
            self.parts_checked += 1
            rate.sleep()

        # 检测完成，发送结果并设置目标为已达成
        self.server.set_succeeded(result, "检测完成")

if __name__ == '__main__':
    rospy.init_node('check_parts_server')
    server = CheckPartsServer()
    rospy.spin()

客户端：

#!/usr/bin/env python

import rospy
import actionlib
from std_msgs.msg import Float32
from my_robot_msgs.msg import CheckPartsAction

class CheckPartsClient:
    def __init__(self):
        self.progress = 0.0

        # 创建动作客户端
        self.client = actionlib.SimpleActionClient('check_parts', CheckPartsAction)
        self.client.wait_for_server()

        # 订阅进度消息
        rospy.Subscriber('check_parts_progress', Float32, self.progress_callback)

    def progress_callback(self, msg):
        self.progress = msg.data

    def send_goal_and_wait(self):
        goal = CheckPartsGoal()
        self.client.send_goal(goal)

        while not self.client.wait_for_result(rospy.Duration.from_sec(1)):
            # 显示进度
            rospy.loginfo("检测进度: %.2f%%", self.progress)

        # 显示最终结果
        rospy.loginfo("检测完成")

if __name__ == '__main__':
    rospy.init_node('check_parts_client')
    client = CheckPartsClient()
    client.send_goal_and_wait()