ROS多线程

ROS1

roscpp内部支持调用多线程, 有两个:

ros::MultiThreadedSpinner

ros::MultiThreadedSpinner是阻塞微调, 类似于ros::spin(), 你可以在它的构造函数中指定线程数量, 但如果不指定或者设为0, 它会根据你的CPU内核数创建线程.

ros::MultiThreadedSpinner spinner(4); // Use 4 threads
spinner.spin(); // spin() will not return until the node has been shutdown

ros::AsyncSpinner

一个更有用的线程spinner是AsyncSpinner. 与阻塞的spin()不同, 它有start()和stop()调用, 并且在销毁时自动停止

ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
ros::waitForShutdown();

注意：

[FATAL] SingleThreadedSpinner: Attempt to spin a callback queue from two spinners, one of them being single-threaded. You might want to use a MultiThreadedSpinner instead.

这是因为有两个有两个spinners处理同一个callback queue，解决的方法就是设置不同的ros::CallbackQueue
callback queue ros wiki

ROS2

rcl

ROS2 Client Interfaces (Client Libraries)

执行器Executors

一个executor使用底层操作系统的一个或多个线程 to invoke the callbacks of subscriptions, timers, service servers, action servers, etc.

执行器类型

• rclcpp::executors::SingleThreadedExecutor
  

  SingleThreadedExecutor只允许串行处理。

• rclcpp::executors::MultiThreadedExecutor
  

  MultiThreadedExecutor创建可配置数量的线程以允许并行处理多个消息或事件

• rclcpp::executors::StaticSingleThreadedExecutor
  

  StaticSingleThreadedExecutor从订阅、定时器、服务、动作服务等方面优化扫描节点结构的runtime costs。它只在添加节点时执行一次扫描，而其他两类执行器则需要定期执行扫描以应对可能的变化。因此，StaticSingleThreadedExecutor只被用于 初始化时就已经创建了所有subscriptions,timers等 的节点。

上面三种执行器可以被用于多个nodes通过add_node()添加每个node

rclcpp::Node::SharedPtr node1 = ...
rclcpp::Node::SharedPtr node2 = ...
rclcpp::Node::SharedPtr node3 = ...

rclcpp::executors::StaticSingleThreadedExecutor executor;
executor.add_node(node1);
executor.add_node(node2);
executor.add_node(node2);
executor.spin();

不同执行器设置不同的优先级的案例：https://github.com/ros2/examples/tree/master/rclcpp/executors/cbg_executor

其中有关线程调度的知识请参考附录

回调Callback

ROS2中，回调是指一个函数，其调度和执行是由executor处理

• subscription callbacks (receiving and handling data from a topic),其
• timer callbacks,
• service callbacks (for executing service requests in a server),
• different callbacks in action servers and clients,
• done-callbacks of Futures.

回调组Callback_Group

callback_group回调组用于将回调函数打包分组，供executor调度执行。

ROS2提供了两种不同类型的回调组来控制回调的执行：

• Mutually Exclusive Callback Group(互斥回调组)
  rclcpp::CallbackGroupType::MutuallyExclusive
• Reentrant Callback Group(可重入回调组)
  rclcpp::CallbackGroupType::Reentrant

这些回调组以不同的方式限制其回调函数的执行

• 互斥回调组可防止其回调被并行执行 - 本质上使其就像组中的回调由 SingleThreadedExecutor 执行一样。
• 可重入回调组允许执行者以它认为合适的任何方式安排和执行组的回调，没有限制。这意味着，除了不同的回调彼此并行运行之外，同一回调的不同实例也可以同时执行。
• 属于不同回调组（任何类型）的回调始终可以彼此并行执行。

回调组的创建由node的create_callback_group成员函数执行

create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive)

如果用户在创建订阅、定时器等时没有指定任何回调组，则该实体将被分配到节点的默认回调组。默认回调组是互斥回调组，可以通过以下API查询

• rclcpp: NodeBaseInterface::get_default_callback_group()

• rclpy: Node.default_callback_group

rclcpp 中的 Executor 基类也有函数add_callback_group(..)，它允许将回调组分配给不同的 Executor。通过使用操作系统调度程序配置底层线程，特定回调可以优先于其他回调。

附录

线程调度

thread线程类

成员函数

• get_id()：获取线程ID，返回一个类型为std::thread::id的对象。
• joinable()：检查线程是否可被join。检查thread对象是否标识一个活动(active)的可行线程。缺省构造的thread对象、已经完成join的thread对象、已经detach的thread对象都不是joinable。
• join()：调用该函数会阻塞当前线程。阻塞调用者(caller)所在的线程直至被join的std::thread对象标识的线程执行结束。
• detach()：将当前线程对象所代表的执行实例与该线程对象分离，使得线程的执行可以单独进行。一旦线程执行完毕，它所分配的资源将会被释放。
• native_handle()：该函数返回与std::thread具体实现相关的线程句柄，类型是native_handle_type，其是连接thread类和操作系统SDK API之间的桥梁，如在Linux g++(libstdc++)里，native_handle_type其实就是pthread里面的pthread_t类型，当thread类的功能不能满足我们的要求的时候(比如改变某个线程的优先级)，可以通过thread类实例的native_handle()返回值作为参数来调用相关的pthread函数达到目的。
• swap()：交换两个线程对象所代表的底层句柄
• hardware_concurrency()：静态成员函数，返回当前计算机最大的硬件并发线程数目。基本上可以视为处理器的核心数目。

有时候我们需要在线程执行代码里面对当前调用者线程进行操作，针对这种情况，C++11里面专门定义了一个命名空间this_thread，此命名空间也声明在<thread>头文件中，其中包括get_id()函数用来获取当前调用者线程的ID；yield()函数可以用来将调用者线程跳出运行状态，重新交给操作系统进行调度，即当前线程放弃执行，操作系统调度另一线程继续执行；sleep_until()函数是将线程休眠至某个指定的时刻(time point),该线程才被重新唤醒；sleep_for()函数是将线程休眠某个指定的时间片(time span)，该线程才被重新唤醒，不过由于线程调度等原因，实际休眠实际可能比sleep_duration所表示的时间片更长。

pthread

线程创建

int pthread_create(
    pthread_t *restrict tidp,
	const pthread_attr_t *restrict attr,
	void *(*start_rtn)(void *),
	void *restrict arg);

返回值：成功返回0；否则返回错误编号

• tidp：新创建的线程ID会被设置成tidp指向的内存单元。
• attr：用于定制各种不能的线程属性，默认为NULL
• start_rtn：新创建的线程从start_rtn函数的地址开始运行，该函数只有一个void类型的指针参数即arg，如果start_rtn需要多个参数，可以将参数放入一个结构中，然后将结构的地址作为arg传入。
• arg：参数指针

注：restrict是c99标准引入的，它只可以用于限定和约束指针，并表明指针是访问一个数据对象的唯一且初始的方式.即它告诉编译器，所有修改该指针所指向内存中内容的操作都必须通过该指针来修改,而不能通过其它途径(其它变量或指针)来修改;这样做的好处是,能帮助编译器进行更好的优化代码,生成更有效率的汇编代码.

互斥量

int pthread_mutex_init(pthread_mutex_t *restrict mutex,const pthread_mutexattr_t *restrict attr); // 默认 attr = NULL
int pthread_mutex_destory(pthread_mutex_t *mutex);// 返回值:成功返回0,否则返回错编号
int pthread_mutex_lock(pthread_mutex_t *mutex);
int pthread_mutex_unlock(pthread_mutex_t *mutex);// 返回值:成功返回0,否则返回错误编号
int pthread_mutex_trylock(pthread_mutex_t *mutex);// 非阻塞版本，如果mutex参数所指定的互斥锁已经被锁定的话，调用pthread_mutex_trylock函数不会阻塞当前线程，而是立即返回一个值来描述互斥锁的状况。

线程属性

pthread_t pthread_self(void); // 返回调用线程的线程ID

线程属性结构如下：

typedef struct
{
    int detachstate; //线程的分离状态
    int schedpolicy; //线程调度策略
    struct sched_param schedparam; //线程的调度参数
    int inheritsched; //线程的继承性
    int scope; //线程的作用域
    size_t guardsize; //线程栈末尾的警戒缓冲区大小
    int stackaddr_set;
    void * stackaddr; //线程栈的位置
    size_t stacksize; //线程栈的大小
}pthread_attr_t;

调度

调度策略

int pthread_attr_getschedpolicy(const pthread_attr_t *, int * policy);
int pthread_attr_setschedpolicy(pthread_attr_*, int policy);

参数：

• attr 线程属性变量
• policy 调度策略
  • SCHED_FIFO ：先进先出
  • SCHED_RR ：轮转法
  • SCHED_OTHER ：其他方法

返回：

​ 若成功返回0，若失败返回-1。

调度参数

int pthread_attr_getschedparam(const pthread_attr_t * attr,struct
sched_param * param);
int pthread_attr_setschedparam(pthread_attr_t * attr,const struct
sched_param * param);

参数：

• attr ：线程变量属性

• param ：sched_parm 结构体

  struct sched_param
  {
  	int sched_priority; //参数的本质就是优先级,大的权值对应高的优先级!
  };
  

  系统支持的最大和最小的优先级值可以用函数：

  int sched_get_priority_max( int policy );
  int sched_get_priority_min( int policy );
  

返回：

​ 若成功返回0，若失败返回-1。