Chapter3 ROS通信机制----进阶篇

1. 在ROS程序中，频繁的使用了 ros::spin() 和 ros::spinOnce() 两个回旋函数，可以用于处理回调函数。

2.spin与spinonce区别

相同点:二者都用于处理回调函数；

不同点:ros::spin() 是进入了循环执行回调函数，而 ros::spinOnce() 只会执行一次回调函数(没有循环)，在 ros::spin() 后的语句不会执行到，而 ros::spinOnce() 后的语句可以执行。

1.ROS中时间相关的API是极其常用，比如:获取当前时刻、持续时间的设置、执行频率、休眠、定时器...都与时间相关。

2.时刻

Ⅰ.作用：获取时刻，或是设置指定时刻

Ⅱ.实现--获取时刻：

①时间相关的头文件已经在 ros.h 中包含，无须包含其他头文件

②API：ros::Time::now() ;     返回值类型为 ros::Time 类型 值为距离1970/01/01的时间

ros::Time rightnow =  ros::Time::now();

③参考代码Ⅰ

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"helllo_time");
    ros::NodeHandle nh;
    
    //now函数会将当前时刻封装并返回，当前时刻：now被调用执行的时刻
    ros::Time rightnow =  ros::Time::now();
    //将离参考系的时间转换成秒
    ROS_INFO("当前时刻%.2f",rightnow.toSec());
    ROS_INFO("当前时刻%d",rightnow.sec);
    return 0;
}

④运行结果Ⅰ

liuhongwei@liuhongwei-virtual-machine:~/demo03_ws$ rosrun plumbing_apis demo02_apis_time 
[ INFO] [1649842275.104207240]: 当前时刻1649842275.10
[ INFO] [1649842275.105031117]: 当前时刻1649842275

Ⅲ.实现--设置指定时刻：

//设置时间为距离1970/01/01 过去20秒钟 + 2426655纳秒
ros::Time t1(20,2426655);
ROS_INFO("当前时刻%.2f",t1.toSec());

[ INFO] [1649842603.712080612]: 当前时刻20.00

3.持续时间（程序休眠）

Ⅰ.需求：程序中停顿5秒

Ⅱ.步骤：

①创建持续时间对象

②休眠 *** 作

Ⅲ.code

ros::Duration du(4.5);   //有一个持续时间，持续时间封装了4.5秒
du.sleep();

4.时间运算

Ⅰ.需求：已知程序开始运行的时刻和程序运行的时间，求程序运行结束的时刻

Ⅱ.步骤：

①获取开始执行的时刻

②调用duration模拟运行时间

③计算结束时刻

Ⅲ.code

ros::Time begin = ros::Time::now();
ros::Duration du1(5);
ros::Time stop = begin + du1;
ROS_INFO("开始的时刻：%.2f",begin.toSec());
ROS_INFO("结束的时刻：%.2f",stop.toSec());

 Ⅳ.运行

[ INFO] [1649849886.072840085]: 开始的时刻：1649849886.07
[ INFO] [1649849886.072916669]: 结束的时刻：1649849891.07

Ⅴ.code2

ros::Duration du3 = begin - stop ;
ROS_INFO("时刻相减：%.2f",du3.toSec());

Ⅵ.运行

[ INFO] [1649850600.163914520]: 时刻相减：-5.00

Ⅶ.此外，持续时间也可相加，即 du3 du1之间可以运算。

5.设置运行频率

rosRate rate(1);    rate.sleep()  实现程序休眠 

 6.定时器

Ⅰ.需求：ROS 中内置了专门的定时器，可以实现与 ros::Rate 类似的效果

Ⅱ.code：输出结果，每隔一秒输入一行--------

void cb(const ros::TimerEvent & event)
{
    ROS_INFO("----------------------");
}


//需求：每隔一秒钟 后台输出一段文本
ROS_INFO("-----------------------------定时器-----------------------------------");
//时间间隔 回调函数 false(定时器是否一次性，如果为true，一秒钟执行一次回调函数，只执行一次，false每隔一秒执行一次) 
//true 定时器是否自动启动
ros::Timer timer = nh.createTimer(ros::Duration(1),cb);
ros::spin();

Ⅲ.定时器其他参数

①参数3，默认false，改成true，执行结果：只输出一行-------------

void cb(const ros::TimerEvent & event)
{
    ROS_INFO("----------------------");
}


//需求：每隔一秒钟 后台输出一段文本
ROS_INFO("-----------------------------定时器-----------------------------------");
//时间间隔 回调函数 false(定时器是否一次性，如果为true，一秒钟执行一次回调函数，只执行一次，false每隔一秒执行一次) 
//true 定时器是否自动启动
ros::Timer timer = nh.createTimer(ros::Duration(1),cb,true);
ros::spin();

②参数4，是否自动启动，默认true，改成false，则定时器不自动启动，无输出结果

void cb(const ros::TimerEvent & event)
{
    ROS_INFO("----------------------");
}


//需求：每隔一秒钟 后台输出一段文本
ROS_INFO("-----------------------------定时器-----------------------------------");
//时间间隔 回调函数 false(定时器是否一次性，如果为true，一秒钟执行一次回调函数，只执行一次，false每隔一秒执行一次) 
//true 定时器是否自动启动
ros::Timer timer = nh.createTimer(ros::Duration(1),cb,true,false);
ros::spin();

如何启动？调用 timer.start();  手动启动

1.节点退出的情况

• 节点接收到了关闭信息，比如常用的 ctrl + c 快捷键就是关闭节点的信号；
• 同名节点启动，导致现有节点退出；
• 程序中的其他部分调用了节点关闭相关的API(C++中是ros::shutdown()，python中是rospy.signal_shutdown())

2.ROS日志的五个级别

• DEBUG(调试):只在调试时使用，此类消息不会输出到控制台；
• INFO(信息):标准消息，一般用于说明系统内正在执行的 *** 作；
• WARN(警告):提醒一些异常情况，但程序仍然可以执行；
• ERROR(错误):提示错误信息，此类错误会影响程序运行；
• FATAL(严重错误):此类错误将阻止节点继续运行。
  

3.code

ROS_DEBUG("hello,DEBUG"); //不会输出
ROS_INFO("hello,INFO"); //默认白色字体
ROS_WARN("Hello,WARN"); //默认黄色字体
ROS_ERROR("hello,ERROR");//默认红色字体
ROS_FATAL("hello,FATAL");//默认红色字体

1.参数用途

Ⅹ.案例所用代码

#! /usr/bin/env python
#encoding:utf-8
import rospy
from std_msgs.msg import String

if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("lhw")
    #3.实例化 发布者 对象
    pub = rospy.Publisher("car",String,queue_size=10)
    #4.组织被发布的数据，并编写逻辑发布数据
    msg = String()  #创建 msg 对象
    msg_front = "hello 你好"
    count = 0  #计数器 
    # 设置循环频率
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():

        #拼接字符串
        msg.data = msg_front + str(count)

        pub.publish(msg)
        rate.sleep()
        rospy.loginfo("写出的数据:%s",msg.data)
        count += 1

Ⅰ.各个参数解释

参数① name：设置节点名称

参数②argv：默认值为None，封装节点调用时传递的参数

参数③anonymous：默认值为False，可以为节点生成随机名称，解决节点名称重名问题

Ⅱ.使用--argv

有了argv，可以按照ROS指定的语法格式传参，ROS可以解析并加以使用

liuhongwei@liuhongwei-virtual-machine:~/demo03_ws$ rosrun plumbing_apis demo01pis_pub _a:=10000

这时ROS会解析命令行，并且转化为参数服务器里面的变量

 Ⅲ.使用--anonymous

如果一个节点多次使用，则可使用此参数

    rospy.init_node("lhw",anonymous=True)

1.发布者对象Publisher的参数

Ⅰ.latch参数，默认值为false。

如果为true，发布方的最后一条信息会被保存且新的订阅者链接到发布方时会将最后一条信息发送给订阅者

Ⅱ.code：

#! /usr/bin/env python
#encoding:utf-8
import rospy
from std_msgs.msg import String

if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("lhw",anonymous=True)
    #3.实例化 发布者 对象
    pub = rospy.Publisher("car",String,queue_size=10,latch=True)
    #4.组织被发布的数据，并编写逻辑发布数据
    msg = String()  #创建 msg 对象
    msg_front = "hello 你好"
    count = 0  #计数器 
    # 设置循环频率
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        count += 1
        #拼接字符串
        if count <= 10:
            msg.data = msg_front + str(count)
            pub.publish(msg)
            rate.sleep()
            rospy.loginfo("写出的数据:%s",msg.data)
            

1.获取当前时间

Ⅰ.code1

    time_now =  rospy.Time.now()
    rospy.loginfo("当前时刻：%.2f",time_now.to_sec())
    rospy.loginfo("当前时刻：%d",time_now.secs)

Ⅱ.演示

liuhongwei@liuhongwei-virtual-machine:~/demo03_ws$ rosrun plumbing_apisemo02_apis_time.py
[INFO] [1649854741.179435]: 当前时刻：1649854741.18
[INFO] [1649854741.181424]: 当前时刻：1649854741

Ⅲ.code2

    #将时间封装成time对象，此时距离时间原点距离100.5s + 多少纳秒
    time1 = rospy.Time(100.5) 
    time2 = rospy.Time(100,3255555)
    rospy.loginfo("指定时间1 ：%.2f",time1.to_sec())
    rospy.loginfo("指定时间2 ：%.2f",time2.to_sec())

Ⅳ.演示

[INFO] [1649855025.249242]: 指定时间1 ：100.50
[INFO] [1649855025.249948]: 指定时间2 ：100.00

2.程序中停顿5秒

    #让程序执行中停顿五秒钟
    #1.封装一个持续时间对象
    rospy.loginfo("休眠前------------------------------------------")
    #5秒 +  x纳秒，也可以只用一个秒参数
    du = rospy.Duration(5,0)
    #2.再将持续时间休眠
    rospy.sleep(du)
    rospy.loginfo("休眠后------------------------------------------")

3.获取程序开始的时刻，且已知持续运行的时间，计算程序结束的时间

    #需求：获取程序开始执行的时刻，且已知的运行时间，计算程序结束的时间
    #1.先获取一个时刻
    t1 = rospy.Time.now()

    #2.设置一个持续时间
    du1 = rospy.Duration(5)

    #3.将t2 =t1 + du1
    t2 = t1 + du1
    rospy.loginfo("开始时刻：%.2f",t1.to_sec())
    rospy.loginfo("结束时刻：%.2f",t2.to_sec())

4.创建定时器，实现类似于 ros::Rate 的功能 

Ⅰ.设置运行频率

rate = rospy.Rate(0.5)
while not rospy.is_shutdown():
    rate.sleep()
    rospy.info("-------------")

Ⅱ.定时器

①code

def domsg(event):
    rospy.loginfo("-----------------------------")

    #需求：创建一个定时器，功能类似于rospy.Rate的功能
    #函数参数
    #1.持续时间 类型为rospy.Duration类型
    #2.回调函数
    #3.是否一次性 默认值false
    #4.
    timer = rospy.Timer(rospy.Duration(2),domsg,False)
    rospy.spin()

②运行结果：每隔两秒钟打印一行-------

[ INFO] [1649899413.898624981]: ----------------------
[ INFO] [1649899414.899210587]: ----------------------
[ INFO] [1649899415.899310332]: ----------------------
[ INFO] [1649899416.898880850]: ----------------------
[ INFO] [1649899417.899059914]: ----------------------
[ INFO] [1649899418.898944819]: ----------------------
[ INFO] [1649899419.898597456]: ----------------------

③若第三个参数为true，则只会执行一行--------------

④回调函数的参数是 Event 类型的，其中有一个方法是 current_real 方法

    rospy.loginfo("调用回调函数的时刻：%.2f",event.current_real.to_sec())

1.节点判断函数

Ⅰ.isshutdown

返回的是bool值，如果节点没被关闭，返回true，反之返回false，可以用来检测节点的关闭状态

Ⅱ. 手动关闭节点 rospy.signal_shutdown("关闭节点") 可以手动关闭节点，参数为字符串是提示输出的信息。

Ⅲ.节点关闭之后执行回调函数：rospy.on_shutdown(回调函数名字)

2.日志函数

rospy.logdebug("++")  ---- 不会输出 

rospy.loginfo("++")  ---- 默认白色字体

rospy.logwarn("++")  ---- 默认黄色字体

rospy.logerr("++")  ---- 默认红色字体

rospy.logfatal("++")  ---- 默认红色字体

本节主要介绍ROS的C++实现中，如何使用头文件和源文件方式封装代码，具体内容如下：

1.设置头文件，可执行文件作为源文件

2.分别设置头文件，源文件和可执行文件

在ROS中关于头文件的使用，核心内容在于CMakeList.txt文件的篇日志，不同的封装方式，配置上也有差异

Ⅰ.需求：设计头文件，可执行文件本身作为源文件

Ⅱ.流程：编写头文件；编写可执行文件；编辑配置文件并执行 

完整流程：

Ⅰ.创建功能包：plumbing_head ，依赖于roscpp rospy std_msgs

Ⅱ.创建头文件：在功能包下的include 下有个和功能包重名的目录，建立头文件hello.h

Ⅲ.声明头文件保护及头文件实现

#ifndef _HELLO_H
#define _HELLO_H
/*
声明namespace ，namespace下面声明一个类，类下声明一个函数run
*/
namespace hello_ns
{
    class Myhello
    {
        public:
            void run();
    };
}

#endif

Ⅳ.可执行文件实现

①配置includePath路径，在c_cpp_properties.json文件中配置头文件的路径

/home/liuhongwei/demo03_ws/src/plumbing_head/include

加入includePath中，加入结果如下

      "includePath": [
        "/opt/ros/melodic/include/**",
        "/home/liuhongwei/demo01_ws/src/helloworld1/include/**",
        "/usr/include/**",
        "/home/liuhongwei/demo03_ws/devel/include/**",
        "/home/liuhongwei/demo03_ws/src/plumbing_head/include/**"
      ],

②源文件实现

#include "ros/ros.h"
#include "plumbing_head/hello.h"

namespace hello_ns
{
    void Myhello::run()
    {
        ROS_INFO("run 函数执行...............");
    }
}

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"hello_head");
    hello_ns::Myhello mehello;
    mehello.run();
    return 0;
}

③配置 CMakeList.txt  

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
 include
  ${catkin_INCLUDE_DIRS}
)

④编译执行

Ⅰ.需求：设计头文件和源文件，在可执行文件中包含头文件

Ⅱ.流程：编写头文件；编写源文件；编写可执行文件；配置配置文件并执行

Ⅰ.创建功能包 plumbing_head_src，依赖为roscpp rospy std_msgs

Ⅱ.创建头文件 hello.h

#ifndef _HELLO_H
#define _HELLO_H


namespace hello_ns
{
    class Myhello
    {
        public:
            void run();
    };
}

#endif

Ⅲ.创建源文件 hello.cpp

#include "plumbing_head_src/hello.h"
#include "ros/ros.h"
namespace hello_ns
{
    void Myhello::run()
    {
        ROS_INFO("源文件中的run函数................");
    }
}

Ⅳ.修改c_cpp_properties.json下面的includPath文件（同上）

Ⅳ.创建可执行文件 use_hello.cpp

#include "ros/ros.h"
#include "plumbing_head_src/hello.h"

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"hello_src");
    hello_ns::Myhello my;
    my.run();
    return 0;
}

Ⅴ.配置Cmakelist&头文件的配置

①修改 include_directories

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
 include
  ${catkin_INCLUDE_DIRS}
)

②配置 C++库  头文件；源文件位置

## Declare a C++ library
 add_library(head_src
  include/plumbing_head_src/hello.h
   src/hello.cpp
 )

③修改两处

 add_dependencies(head_src ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

 target_link_libraries(head_src
   ${catkin_LIBRARIES}
 )

Ⅴ.配置Cmakelist&可执行文件的配置

①修改add_executable

 add_executable(use_hello src/use_hello.cpp)

②修改 add_dependencies

 add_dependencies(use_hello ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

③修改 target_link_libraries

由于use_hello要依赖于head_src 做如下修改

 target_link_libraries(use_hello
  head_src
   ${catkin_LIBRARIES}
 )

Ⅵ.编译执行

1.需求：

首先建立一个python文件A，再创建python文件 useA ，在useA中导入A并调用A的实现

2.rosrun的路径是什么？

[INFO] [1649919184.762048]: 执行时参考路径：/home/liuhongwei/demo03_ws

path = os.path.abspath(".")
rospy.loginfo("执行时参考路径：%s",path)

3.由上述基础后

Ⅰ.我们在同一scripts目录下建立两文件，demo01.py 和 tools.py

#! /usr/bin/env python
#encoding:utf-8

#1.导包 
import rospy
from std_msgs.msg import String
import os
#import tools
if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("lhw")


    #3.实例化 发布者 对象
    pub = rospy.Publisher("car",String,queue_size=10)
    #4.组织被发布的数据，并编写逻辑发布数据
    msg = String()  #创建 msg 对象
    msg_front = "hello 你好"
    count = 0  #计数器 
    # 设置循环频率
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():

        #拼接字符串
        msg.data = msg_front + str(count)

        pub.publish(msg)
        rate.sleep()
        rospy.loginfo("写出的数据:%s",msg.data)
        count += 1

#! /usr/bin/env python
#coding=utf-8

lhwnb = 100

Ⅱ.机器在处理好配置文件及赋予可执行权限后，import tools会抛出异常，

No Module named "tools"

异常原因是找不到tools模块？为什么呢？因为环境变量出问题了，python执行过程中，如果导入了某个模块，会在当前目录下看是否包含，rosrun的当前目录不是scripts目录，因此找不到。

Ⅲ.如何解决①：可以声明python的环境变量，当依赖某个模块时，先去指定的环境变量中的环境中查找依赖（不推荐）

import sys
#import tools
if __name__ == "__main__":

    #设置临时环境变量
    sys.path.insert(0,"/home/liuhongwei/demo03_ws/src/plumbing_pub_sub/scripts")

Ⅳ.如何解决②

动态获取路径：当前工作空间的路径 + 内部路径

①当前工作空间的绝对路径:rosrun的路径 即到..../demo03_ws/的路径，可以用os.path.absolute获取

②内部路径：鼠标右击获取scripts下的路  src/plumbing_pub_sub/scripts

    path = os.path.abspath(".")
    sys.path.insert(0, path + "src/plumbing_pub_sub/scripts")

③完整代码实现

#! /usr/bin/env python
#encoding:utf-8



#1.导包 
import rospy
from std_msgs.msg import String
import os
import sys
#import tools
if __name__ == "__main__":

    path = os.path.abspath(".")
    sys.path.insert(0, path + "src/plumbing_pub_sub/scripts")

    rospy.init_node("lhw")


    #3.实例化 发布者 对象
    pub = rospy.Publisher("car",String,queue_size=10)
    #4.组织被发布的数据，并编写逻辑发布数据
    msg = String()  #创建 msg 对象
    msg_front = "hello 你好"
    count = 0  #计数器 
    # 设置循环频率
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():

        #拼接字符串
        msg.data = msg_front + str(count)

        pub.publish(msg)
        rate.sleep()
        rospy.loginfo("写出的数据:%s",msg.data)
        count += 1