【ROS】—— ROS通信机制进阶（七）

前言

本系列将依托赵虚左老师的ROS课程，写下自己的一些心得与笔记。
课程链接:https://www.bilibili.com/video/BV1Ci4y1L7ZZ
讲义链接:http://www.autolabor.com.cn/book/ROSTutorials/index.html
文章可能存在疏漏的地方，恳请大家指出。

1. 常用API(C++)

官网对API的说明http://wiki.ros.org/APIs

1.1 初始化

ros::init(argc,argv,"talker_turtle");

参数

参数	功用
argc	封装实参个数（n+1）,多了一个程序文件自身
argv	封装参数的数组（字符串数组）
name	节点名称，需要保证其唯一性，不允许包含命名空间
options	节点启动选项，被封装进了ros::init_options

使用
argc与argv的使用：
若按照ROS中的特定格式传入参数，ROS就可以加以使用，可以达到设置全局参数、节点重命名等功能。

options的使用：
节点的名称需要保证唯一性：同一个节点不能重复启动。当有重名的节点启动时，之前的节点会被关闭。
但是若需要将一个节点多次启动且运行正常，该如何操作？
使用下列代码可以解决

    ros::init(argc,argv,"talker_turtle",ros::init_options::AnonymousName);

同时运行同一个节点

ros::init_options::AnonymousName会使得节点后面跟一个随机数,从而达到不重名的目的.

1.2 话题与服务相关对象

在 roscpp 中，话题和服务的相关对象一般由 NodeHandle 创建。

    ros::Publisher pub = nh.advertise<publisher::person>("chatter_person",10);

作用
创建发布者对象

模板
被发布的消息的类型

参数

参数	功用
话题名称	话题名称
队列长度	队列长度
latch(可选)	文档解释如下 * \param latch (optional) If true, the last message published on this topic will be saved and sent to new subscribers when they connect 若设置为true,会保存发布方的最后一条消息,并且新的订阅对象连接到发布方时,发布方会将这条消息发送给订阅者

使用
latch 设置为true的作用:
以静态地图发布为例,
方案1:可以使用固定频率发送地图数据,但是效率低
方案2:可以将地图发布对象的latch设置为true,并且只发布一次,每当订阅者连接时,将地图数据发送给订阅者(只发送一次),这样提高了数据的发送效率.

1.3 回旋函数(重点)

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

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

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

1.4 时间

1.4.1 获取当前时刻and指定时刻

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

#include "ros/ros.h"
/*
    需求: 演示时间相关操作(获取当前时刻and指定时刻)
    实现:
        1.准备(头文件/节点初始化/节点句柄)
        2.获取当前时刻
        3.设置指定时刻
*/

int main(int argc, char  *argv[])
{
    //1.准备(头文件/节点初始化/节点句柄)
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"get_time");
    ros::NodeHandle nh;  //必须创建句柄，否则时间没有初始化，导致后续API调用失败.(因为NodeHandle会初始化时间操作)

    //2.获取当前时刻
    //now 函数会将当前时刻封装并返回
    //当前时刻:now被执行的那一刻
    //参考系: 1970年1月1日 00:00:00
    ros::Time right_now = ros::Time::now(); 
    ROS_INFO("当前时刻:%.2f",right_now.toSec()); //获取距离 1970年01月01日 00:00:00 的秒数
    ROS_INFO("当期时刻:%d",right_now.sec);
    //3.设置指定时刻
    return 0;
}

   //3.设置指定时刻
    ros::Time t1(20,123456789);
    ROS_INFO("当前时刻:%.2f",t1.toSec());
    return 0;

1.4.2 持续时间

ROS_INFO("当前时刻:%.2f",ros::Time::now().toSec());
ros::Duration du(10);//持续10秒钟,参数是double类型的，以秒为单位
du.sleep();//按照指定的持续时间休眠
ROS_INFO("持续时间:%.2f",du.toSec());//将持续时间换算成秒
ROS_INFO("当前时刻:%.2f",ros::Time::now().toSec());

1.4.3 时间运算

ROS_INFO("时间运算");
ros::Time now = ros::Time::now();
ros::Duration du1(10);
ros::Duration du2(20);
ROS_INFO("当前时刻:%.2f",now.toSec());
//1.time 与 duration 运算
ros::Time after_now = now + du1;
ros::Time before_now = now - du1;
ROS_INFO("当前时刻之后:%.2f",after_now.toSec());
ROS_INFO("当前时刻之前:%.2f",before_now.toSec());

//2.duration 之间相互运算
ros::Duration du3 = du1 + du2;
ros::Duration du4 = du1 - du2;
ROS_INFO("du3 = %.2f",du3.toSec());
ROS_INFO("du4 = %.2f",du4.toSec());
//PS: time 与 time 不可以运算
// ros::Time nn = now + before_now;//异常

注意:
时刻与时刻之间不能进行相加运算,但可以进行相减.

1.4.4 设置运行频率

# 设置执行频率
rate = rospy.Rate(0.5)
while not rospy.is_shutdown():
    rate.sleep() #休眠
    rospy.loginfo("+++++++++++++++")

1.4.5 定时器

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

ros::NodeHandle nh;//必须创建句柄，否则时间没有初始化，导致后续API调用失败

 // ROS 定时器
 /**
* \brief 创建一个定时器，按照指定频率调用回调函数。
*
* \param period 时间间隔
* \param callback 回调函数
* \param oneshot 如果设置为 true,只执行一次回调函数，设置为 false,就循环执行。
* \param autostart 如果为true，返回已经启动的定时器,设置为 false，需要手动启动。
*/
 //Timer createTimer(Duration period, const TimerCallback& callback, bool oneshot = false,
 //                bool autostart = true) const;

 // ros::Timer timer = nh.createTimer(ros::Duration(0.5),doSomeThing);
 ros::Timer timer = nh.createTimer(ros::Duration(0.5),doSomeThing,true);//只执行一次

 // ros::Timer timer = nh.createTimer(ros::Duration(0.5),doSomeThing,false,false);//需要手动启动
 // timer.start();
 ros::spin(); //必须 spin

ROS的定时器主要由createTimer()函数进行下面对相关参数进行说明.

参数	功用
period	时间间隔
callback	回调函数,.函数体封装函数
oneshot	如果设置为 true,只执行一次回调函数，设置为 false,就循环执行。
autostart	如果为true，返回已经启动的定时器,设置为 false，需要手动启动。
定时器的回调函数:

void doSomeThing(const ros::TimerEvent &event){
    ROS_INFO("-------------");
    ROS_INFO("event:%s",std::to_string(event.current_real.toSec()).c_str());
}

1.5 其他函数

在发布实现时，一般会循环发布消息，循环的判断条件一般由节点状态来控制，C++中可以通过 ros::ok() 来判断节点状态是否正常，而 python 中则通过 rospy.is_shutdown() 来实现判断，导致节点退出的原因主要有如下几种:

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

另外，日志相关的函数也是极其常用的，在ROS中日志被划分成如下级别:

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

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

2. 常用API(python)

2.1 初始化

主要介绍name argv anonymous三个参数

rospy.init_node(name, argv=None, anonymous=False, log_level=None, disable_rostime=False, disable_rosout=False, disable_signals=False, xmlrpc_port=0, tcpros_port=0):

参数

参数	功用
name	设置节点节点名称，需要保证其唯一性，不允许包含命名空间
argv	封装节点调用时传递的参数
anonymous	可以为节点名称生成随机后缀,可以解决重名问题

使用
argc使用:
可以按照ROS中指定的语法格式传参,ROS可以解析并加以使用

调用格式如下

 rosrun publisher api_time_p.py _A:=1000

_A:=1000:解析A,并将其加入参数服务器中.
下面是未加入A时的参数列表

下面是加入A时的参数列表

anonymous使用:
与C++的方式类似

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

2.2 话题与服务相关对象

主要介绍latch,作用和C++中差不多

class Publisher(Topic):
    """
    在ROS master注册为相关话题的发布方
    """

    def __init__(self, name, data_class, subscriber_listener=None, tcp_nodelay=False, latch=False, headers=None, queue_size=None):
        """
        Constructor
        @param name: 话题名称 
        @type  name: str
        @param data_class: 消息类型

        @param latch: 如果为 true,该话题发布的最后一条消息将被保存，并且后期当有订阅者连接时会将该消息发送给订阅者
        @type  latch: bool

        @param queue_size: 等待发送给订阅者的最大消息数量
        @type  queue_size: int

        """

使用
latch 设置为true的作用:
以静态地图发布为例,
方案1:可以使用固定频率发送地图数据,但是效率低
方案2:可以将地图发布对象的latch设置为true,并且只发布一次,每当订阅者连接时,将地图数据发送给订阅者(只发送一次),这样提高了数据的发送效率.

2.3 回旋函数

def spin():
    """
    进入循环处理回调 
    """

2.4 时间

2.4.1 获取时刻，或是设置指定时刻

# 获取当前时刻
right_now = rospy.Time.now()
rospy.loginfo("当前时刻:%.2f",right_now.to_sec())
rospy.loginfo("当前时刻:%.2f",right_now.to_nsec())
# 自定义时刻
some_time1 = rospy.Time(1234.567891011)
some_time2 = rospy.Time(1234,567891011)
rospy.loginfo("设置时刻1:%.2f",some_time1.to_sec())
rospy.loginfo("设置时刻2:%.2f",some_time2.to_sec())

# 从时间创建对象
# some_time3 = rospy.Time.from_seconds(543.21)
some_time3 = rospy.Time.from_sec(543.21) # from_sec 替换了 from_seconds
rospy.loginfo("设置时刻3:%.2f",some_time3.to_sec())

2.4.2 持续时间

# 持续时间相关API
rospy.loginfo("持续时间测试开始.....")
du = rospy.Duration(3.3)
rospy.loginfo("du1 持续时间:%.2f",du.to_sec())
rospy.sleep(du) #休眠函数
rospy.loginfo("持续时间测试结束.....")

2.4.3 持续时间与时刻运算

rospy.loginfo("时间运算")
now = rospy.Time.now()
du1 = rospy.Duration(10)
du2 = rospy.Duration(20)
rospy.loginfo("当前时刻:%.2f",now.to_sec())
before_now = now - du1
after_now = now + du1
dd = du1 + du2
# now = now + now #非法
rospy.loginfo("之前时刻:%.2f",before_now.to_sec())
rospy.loginfo("之后时刻:%.2f",after_now.to_sec())
rospy.loginfo("持续时间相加:%.2f",dd.to_sec())

2.4.4 设置运行频率

# 设置执行频率
rate = rospy.Rate(0.5)
while not rospy.is_shutdown():
    rate.sleep() #休眠
    rospy.loginfo("+++++++++++++++")

2.4.5 定时器

#定时器设置
"""    
def __init__(self, period, callback, oneshot=False, reset=False):
    Constructor.
    @param period: 回调函数的时间间隔
    @type  period: rospy.Duration
    @param callback: 回调函数
    @type  callback: function taking rospy.TimerEvent
    @param oneshot: 设置为True，就只执行一次，否则循环执行
    @type  oneshot: bool
    @param reset: if True, timer is reset when rostime moved backward. [default: False]
    @type  reset: bool
"""
rospy.Timer(rospy.Duration(1),doMsg)
# rospy.Timer(rospy.Duration(1),doMsg,True) # 只执行一次
rospy.spin()

2.5 其他函数

2.5.1 节点关闭

while not rospy.is_shutdown():
        count += 1
        #拼接字符串
        if count <= 10:
            msg.data = msg_front + str(count)
            #发布数据
            pub.publish(msg)
            
            rospy.loginfo("写出的数据:%s",msg.data)
        else:
            rospy.signal_shutdown("关闭节点")
    rate.sleep()

2.5.2 日志函数

rospy.logdebug("hello,debug")  #不会输出
rospy.loginfo("hello,info")  #默认白色字体
rospy.logwarn("hello,warn")  #默认黄色字体
rospy.logerr("hello,error")  #默认红色字体
rospy.logfatal("hello,fatal") #默认红色字体

python以及C++剩余部分的内容还请参考讲义http://www.autolabor.com.cn/book/ROSTutorials/index.html

3. ROS中的头文件与源文件

这一部分需要有一定的C++基础
对于头文件好源文件,可以参考这篇博客浅谈头文件(.h)和源文件(.cpp)的区别
在一般的大文件中,头文件主要进行声明,源文件用来定义,可执行文件则使用源文件中定义好的内容.

3.1 自定义头文件调用

需求: 设计头文件，可执行文件本身作为源文件。

流程:

• 编写头文件；
• 编写可执行文件(同时也是源文件)；
• 编辑配置文件并执行。

在功能包下的 include/功能包名 目录下新建头文件: hello.h，示例内容如下:

#ifndef _HELLO_H
#define _HELLO_H

namespace hello_ns{

class HelloPub {

public:
    void run();
};

}

#endif

注意:

在 VScode 中，为了后续包含头文件时不抛出异常，请配置 .vscode 下 c_cpp_properties.json 的 includepath属性.配置方法与以前一致.

在 src 目录下新建文件:hello.cpp，示例内容如下:

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

namespace hello_ns {

void HelloPub::run(){
    ROS_INFO("自定义头文件的使用....");
}

}

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

配置CMakeLists.txt文件，头文件相关配置如下:

include_directories(
include
  ${catkin_INCLUDE_DIRS}
)

可执行配置文件配置方式与之前一致:

add_executable(hello src/hello.cpp)

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

target_link_libraries(hello
  ${catkin_LIBRARIES}
)

注意:若产生以下报错,更改配置文件中hello的名称(可能和之前的程序重复了)

3.2 自定义源文件调用

需求: 设计头文件与源文件，在可执行文件中包含头文件。

流程:

• 编写头文件；
• 编写源文件；
• 编写可执行文件；
• 编辑配置文件并执行。

头文件设置于 3.2.1 类似，在功能包下的 include/功能包名 目录下新建头文件: haha.h，示例内容如下:

#ifndef _HAHA_H
#define _HAHA_H

namespace hello_ns {

class My {

public:
    void run();

};

}

#endif

在 src 目录下新建文件:haha.cpp，示例内容如下:

#include "test_head_src/haha.h"
#include "ros/ros.h"

namespace hello_ns{

void My::run(){
    ROS_INFO("hello,head and src ...");
}

}

在 src 目录下新建文件: use_head.cpp，示例内容如下:

#include "ros/ros.h"
#include "test_head_src/haha.h"

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

头文件与源文件相关配置:

include_directories(
include
  ${catkin_INCLUDE_DIRS}
)

## 声明C++库
add_library(head
  include/test_head_src/haha.h
  src/haha.cpp
)

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

target_link_libraries(head
  ${catkin_LIBRARIES}
)

可执行文件配置:

add_executable(use_head src/use_head.cpp)

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

#此处需要添加之前设置的 head 库
target_link_libraries(use_head
  head
  ${catkin_LIBRARIES}
)

4. Python模块导入

需求: 首先新建一个Python文件A，再创建Python文件UseA，在UseA中导入A并调用A的实现。

实现:

• 新建两个Python文件，使用 import 实现导入关系；
• 添加可执行权限、编辑配置文件并执行UseA。

文件A实现(包含一个变量):

#! /usr/bin/env python
num = 1000

文件B核心实现:

import os
import sys

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

import tools

....
....
    rospy.loginfo("num = %d",tools.num)

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

# import os
# import sys

# path = os.path.abspath(".")
# # 核心
# sys.path.insert(0,path + "/src/hello_head/scripts")

import tools

if __name__ == "__main__":
    rospy.init_node("head_tool")
    pub = rospy.Publisher("python_t",String,queue_size=100)
    rospy.loginfo("启动程序")
    rospy.loginfo("num = %d",tools.num)
    #rospy.loginfo("num = %d",tools.num)
    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

melodic 版本应该可以正常进行,不会报错no module named tools

报错原因: rosrun 运行时,参考的路径是工作空间的路径,但在工作空间下无法查找依赖的模块
解决: 可以声明python的环境变量,当依赖某个模块时,先去指定的环境变量中查找依赖.

这种方式也是正常进行