• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

ROS中那些好用的功能包推荐(1)—— yocs_cmd_vel_mux多路复用速度控制

人工智能 跃动的风 1165次浏览 0个评论
  • 在我们控制一个移动机器人运动时,可能会遇到如下场景:自研的移动机器人在自动导航的过程中突然迷路要撞墙了,一场车祸马上就要发生,这时候,我们就会很希望能够通过无线手柄或者键盘去控制小车紧急停车,让小车改邪归正,迷途知返。想要实现这个功能,就需要用到多路输入复用控制,即把多种速度控制信号收集起来,并按照优先级发给小车,覆盖掉自主导航的速度控制消息。
  • 幸运的是,完善的ROS生态中已经给我们提供了这个名为yocs_cmd_vel_mux的功能包。

一、 yocs_cmd_vel_mux是怎么工作的

  • 我们需要在配置文件中对该功能包接收的话题和优先级进行配置,它可以接收多个话题并将其基于优先级发送,处理流程示意如下:

ROS中那些好用的功能包推荐(1)—— yocs_cmd_vel_mux多路复用速度控制
需要注意的是,只要高优先级话题当前有发布就会覆盖低优先级的话题,因此可以通过让高优先级信息源持续发送速度0的信息来控制移动机器人停下。

1.1 接收的话题

~input/cmd_vel:即为要处理的速度话题,话题格式为(geometry_msgs/Twist),接收的话题名称可以自己配置。

1.2 发布的话题

~output/cmd_vel:即为发送给控制器的速度话题,话题格式为(geometry_msgs/Twist),话题名称可以自己配置。
~active:即为当前激活的输入话题所对应配置的name,话题格式为(std_msgs/String),话题名称在代码中固定为/active。

二、安装与配置过程

2.1 安装

git地址: https://github.com/yujinrobot/yujin_ocs.git
注意需要根据你的ROS版本选择对应的分支:

  • melodic: release/0.8-melodic
  • kinetic: kinetic
    下载后catkin_make编译yocs_cmd_vel_mux功能包,并去param文件夹中参考example.yaml去配置自己的映射。
    给大家一个博主的配置作为参考:
subscribers:
  - name:        "Navigation control"
    topic:       "movebase/cmd_vel/navigation"
    timeout:     0.5
    priority:    1
    short_desc:  "Navigation control"

  - name:        "Remote control"
    topic:       "movebase/cmd_vel/remote"
    timeout:     0.1
    priority:    2

  - name:        "Application control"
    topic:       "movebase/cmd_vel/webapp"
    timeout:     0.3
    priority:    3

  - name:        "Keyboard operation"
    topic:       "movebase/cmd_vel/keyop"
    timeout:     0.1
    priority:    4

  - name:        "Joystick control"
    topic:       "movebase/cmd_vel/joystick"
    timeout:     0.1
    priority:    10
    short_desc:  "Joystick control"

publisher:       "movebase/cmd_vel"

可以看到之前的每组输入都可以几个参数,分别为:
name: 数据源名称
topic: 提供cmd_vel的话题名称
timeout:某话题超过该时间没有新的输入则认为该输入话题不活跃(单位:s)。
priority:优先级,范围为(0-MAX_INT)数字越大优先级越高。
short_dessc:简短描述,可以不填。

最后一行的publisher字段为配置的发布话题。

我们可以把自己的配置文件任意命名,放置于此文件夹中。

2.2 运行

进入到launch文件夹中配置launch文件,参考standalone.launch文件进行配置,注意要修改config_file配置文件的地址。

三、 扒一下代码实现

查看了一下源代码,可以看到这个功能很简单,但是被作者实现的很优美。
首先创建类CmdVelSubscribers,该类中包含根据读入yaml配置文件而构建的一个列表std::vector<std::shared_ptr<CmdVelSubs>> list,以及allowed字段用于记录当前转发的的话题idx号。

CmdVelSubs内容为

class CmdVelSubs
  {
  public:
    unsigned int           idx;          /**< Index; assigned according to the order on YAML file */
    std::string            name;         /**< Descriptive name; must be unique to this subscriber */
    std::string            topic;        /**< The name of the topic */
    ros::Subscriber        subs;         /**< The subscriber itself */
    ros::Timer             timer;        /**< No incoming messages timeout */
    double                 timeout;      /**< Timer's timeout, in seconds  */
    unsigned int           priority;     /**< UNIQUE integer from 0 (lowest priority) to MAX_INT */
    std::string            short_desc;   /**< Short description (optional) */
    bool                   active;       /**< Whether this source is active */

    CmdVelSubs(unsigned int idx) : idx(idx), active(false) { };
    ~CmdVelSubs() { }

    /** Fill attributes with a YAML node content */
    void operator << (const YAML::Node& node);
  };

每一组输入话题都配有唯一的idx,并记录了所有我们在的yaml中配置的信息,另外还多了两个字段,分别为timer计时器,用于记录输入时间,以及active记录当前是否激活的布尔量。

程序初始化时,添加各个话题的接收函数,并给每个话题都初始化了定时器,并初始化了一个全局定时器common_timer,用于对话题未接收超时的情况进行处理。

在接收到速度输入信息后触发回调函数:

void CmdVelMuxNodelet::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg, unsigned int idx)
{
  // Reset general timer
  common_timer.stop();
  common_timer.start();

  // Reset timer for this source
  cmd_vel_subs[idx]->timer.stop();
  cmd_vel_subs[idx]->timer.start();

  cmd_vel_subs[idx]->active = true;   // 设置idx对应话题为激活

  //如果当前没有被转发的话题
  //或者当前被转发的话题就是idx对应话题
  //或者idx对应话题的优先级高于当前被转发的话题
  //则设置当前转发话题为idx对应话题
  if ((cmd_vel_subs.allowed == VACANT) ||
      (cmd_vel_subs.allowed == idx)    ||
      (cmd_vel_subs[idx]->priority > cmd_vel_subs[cmd_vel_subs.allowed]->priority))
  {
    if (cmd_vel_subs.allowed != idx)
    {
      cmd_vel_subs.allowed = idx;

      // Notify the world that a new cmd_vel source took the control
      std_msgs::StringPtr acv_msg(new std_msgs::String);
      acv_msg->data = cmd_vel_subs[idx]->name;
      active_subscriber.publish(acv_msg);
    }

    output_topic_pub.publish(msg);
  }
}

重置当前对应的计时器以及全局计时器,并将该话题设置为激活, 如果当前没有被转发的话题、或者当前被转发的话题就是idx对应话题、或者idx对应话题的优先级高于当前被转发的话题,则设置当前转发话题为idx对应话题。如果发生了话题变更则发送一次/active消息。

void CmdVelMuxNodelet::timerCallback(const ros::TimerEvent& event, unsigned int idx)
{
  if (cmd_vel_subs.allowed == idx || (idx == GLOBAL_TIMER && cmd_vel_subs.allowed != VACANT))
  {

    // No cmd_vel messages timeout happened to currently active source, so...
    cmd_vel_subs.allowed = VACANT;

    // ...notify the world that nobody is publishing on cmd_vel; its vacant
    std_msgs::StringPtr acv_msg(new std_msgs::String);
    acv_msg->data = "idle";
    active_subscriber.publish(acv_msg);
  }

  if (idx != GLOBAL_TIMER)
    cmd_vel_subs[idx]->active = false;
}
  • 如果触发了时间回调函数说明当前话题已经超时没有接收到,则需要把当前的转发话题设置为空,并将对应输入话题标记为未激活。
  • 值得一提的是,yocs_cmd_vel_mux包采用了nodelet插件的方式编写,nodelet包可以实现在同一个进程内同时运行多种算法,且算法之间通过shared_ptr通信实现零拷贝,从而减少多个node通过rostcp通信带来的延时问题。不过这种优势只有在我们的输入话题也采用同样的nodelet方式编写才能体现出来。


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS中那些好用的功能包推荐(1)—— yocs_cmd_vel_mux多路复用速度控制
喜欢 (0)

您必须 登录 才能发表评论!

加载中……