ROS Node 之间通信打断操作实例

前言

公司开发小工具,对文件夹下点云PCD文件进行读取和相应操作,目标功能:

  1. 读取文件夹下PCD文件,按照文件名进行排序;
  2. 通过Qt开发UI界面,界面包括操作按钮:
    • continue: 循环播放PCD文件并发布
    • nextpre: 后一帧或前一帧PCD文件
    • save index: 保存当前帧PCD文件名到.txt文件
  3. continue操作正在进行时,点击其余按钮,实现打断停止功能

分析

  1. 将UI界面和实际后台操作分开进行多线程操作,否则在进行continue过程中时,无法通过外部改变判断条件进行打断;
  2. ROS的一个Node默认为是一个进程,所以采用double Node实现多线程;
  3. ROS的单个Node可以同时实现subscribepublish多个消息。本文假设UI界面为Node 1,包括:读取PCD文件,对点击操作进行反应并发送按钮消息到后端;后台实现为Node 2,包括:按钮消息的实现代码。

  4. UI界面
    Screenshot from 2018-03-07 10:23:37.jpg

  5. Node 2关键代码
    由于ROSNode之间特殊的通信机制,如果将条件判断机制放在Node 2的子函数中,那么Node 2在接收Node 1的消息时,如果continue操作正在进行,则必须当continue执行完毕之后再收到Node 1的消息。所以,必须将判断条件房子ROS的Master部分,通过MasterNode 1当前消息进行反应,可实时打断Node 2正在进行的continue操作,马上进行当前消息的操作。
    以下是Node 2中的main函数的ROS循环部分代码:

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    enum recv_sign { none_sign = 0, stop_sign = 1, continue_sign = 2, load_sign = 3 };
    while(ros::ok())
    {
    if( g_con_signal == continue_sign )
    {
    if ( g_cur_index < g_pcd_filelist.size() - 1 )
    {
    publishPCD();
    g_cur_index++;
    }
    else
    g_cur_index = 0;
    }
    else if ( g_con_signal == stop_sign )
    {
    if ( g_pcd_info == "pre_pcd_signal" )
    {
    if ( g_cur_index > 0 )
    {
    g_cur_index--;
    publishPCD();
    }
    else
    std::cerr << "Reach 1st file!!" << std::endl;
    }
    else if ( g_pcd_info == "next_pcd_signal" )
    {
    if ( g_cur_index < g_pcd_filelist.size() - 1 )
    g_cur_index ++;
    else
    g_cur_index = 0;
    publishPCD();
    }
    else if ( g_pcd_info == "save_index" )
    {
    g_outfile << g_pcd_filelist[g_cur_index] << std::endl;
    }
    g_con_signal = none_sign;
    }
    else if ( g_con_signal == load_sign )
    {
    g_file_root_path = g_pcd_info;
    std::cout << g_file_root_path << std::endl;
    read_filelists( g_file_root_path + "/", g_pcd_filelist, ".pcd" );
    for (int i = 0; i < g_pcd_filelist.size(); ++i)
    {
    std::cout << g_pcd_filelist[i] << std::endl;
    }
    g_con_signal = none_sign;
    }
    ros::spinOnce();
    }

代码说明:

  • Node 1同时发送2个std_msgs::Stringg_con_signal用于控制是否执行循环条件;g_pcd_info用于在不执行continue操作时进行细分操作划分,包括:save index操作的文件路径和loadPCD文件时文件路径。
  • g_con_signal可以取4个值:enum recv_sign { none_sign = 0, stop_sign = 1, continue_sign = 2, load_sign = 3 };,分别对应不同操作,其中none_sign用于执行除continue操作之外的跳出当前循环,达到只需执行一次的目的,防止陷入死循环(无线循环)。
  • ros::spinOnce()用于刷新ROS执行条件,每次进入while(ros::ok())循环时,就会内部条件进行判断。

后续:
Node 2当前帧文件名返回给Node 1用于显示于UI界面功能尚待加入。

以上。