机器人控制系统——LCM

Huang A S, Olson E, Moore D C. LCM: Lightweight communications and marshalling[C]//2010 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2010: 4057-4062.

LCM（Lightweight Communications and Marshalling）是一组用于消息传递和数据编组的库和工具，其基于UDP传输的属性，传输速度较快，其目标是高带宽和低延迟的实时系统。它提供了一种发布/订阅消息传递模型以及带有各种编程语言C++、Java、python等应用程序绑定的自动编组/解组代码生成，LCM通过将消息封装在不同的Channel中进行通信，这点类似于ROS中的Topic。

支持消息类型：

type	Description
int8_t	8-bit signed integer(Byte)
int16_t	16-bit signed integer(short)
int32_t	32-bit signed integer(int)
int64_t	64-bit signed integer(long)
float	32-bit IEEE floating point value
double	64-bit IEEE floating point value
string	UTF-8 string
boolean	true/false logical value
byte	8-bit value

安装

参考Build Instructions

Required packages:

• build-essential
• libglib2.0-dev
• cmake

Optional packages (e.g., for language-specific support or building documentation):

• default-jdk
• python-all-dev
• liblua5.1-dev
• golang
• doxygen
• python-epydoc

$ mkdir build
$ cd build
$ cmake ..
$ make
$ sudo make install

如果没有lcm-spy需要安装java，注意版本

sudo apt-get install openjdk-8-jdk

# Java
lcm_option(
  LCM_ENABLE_JAVA
  "Build Java bindings and utilities"
  JAVA_FOUND Java 1.6)
if(LCM_ENABLE_JAVA)
  add_subdirectory(lcm-java)
  add_custom_target(lcm-spy DEPENDS lcm-spy-alias)
  add_custom_target(lcm-logplayer-gui DEPENDS lcm-logplayer-gui-alias)
endif()

工具

命令	作用
lcm-gen	生成lcm消息
lcm-spy	查看当前数据
lcm-logger	记录log
lcm-logplayer,lcm-logplayer-gui	log回放

• lcm-gen

  根据定义的lcm消息生成对应的（c,c++,java,python,lua,c#,go）文件，建议通过脚本处理，参考：

  #!/bin/bash
  
  GREEN='\033[0;32m'
  NC='\033[0m' # No Color
  
  echo -e "${GREEN} Starting LCM type generation...${NC}"
  
  cd ../lcm-types
  
  # Clean
  rm */*.jar
  rm */*.java
  rm */*.hpp
  rm */*.class
  rm */*.py
  rm */*.pyc
  
  # Make
  lcm-gen -jxp *.lcm # 生成java,c++,python
  cp /usr/local/share/java/lcm.jar . # 获取lcm.jar
  javac -cp lcm.jar */*.java    # 生成class
  jar cf my_types.jar */*.class # 消息打包，用于lcm-spy解析
  
  # 整理不同格式消息
  mkdir -p java
  mv my_types.jar java
  mv lcm.jar java
  mkdir -p cpp
  mv *.hpp cpp
  mkdir -p python
  mv *.py python
  
  FILES=$(ls */*.class)
  echo ${FILES} > file_list.txt
  
  echo -e "${GREEN} Done with LCM type generation${NC}"
  

• lcm-spy

  可视化消息，注意需要设置CLASSPATH才能解析消息否则为Undecodable

  #!/bin/bash
  DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
  cd ${DIR}/../lcm-types/java
  export CLASSPATH=${DIR}/../lcm-types/java/my_types.jar
  pwd
  lcm-spy
  

网络配置

本地通信

在本地进行通信时，若未联网初始化会失败，需要配置本地multicast。

sudo ifconfig lo multicast
sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev lo

多机通信

UDP Multicast Setup

• 设置Multicast端口

  # 查看网络状态
  ifconfig
  
  # eth0为设置的Multicast网口,多机通讯时选取通讯网口
  # 239.255.76.67为Multicast地址
  sudo ifconfig eth0 multicast
  sudo route add -net 239.255.76.67 netmask 255.255.255.255 dev eth0 # 添加LCM默认地址为239.255.76.67:7667
  
  # 查看是否添加成功
  route -n
  

• 设置LCM Multicast地址

  这里为默认地址，若维持默认不需要这步操作

  export LCM_DEFAULT_URL=udpm://239.255.76.67:7667?ttl=1
  

API

初始化LCM

lcm::LCM::LCM(std::string lcm_url = "")	

根据设置的lcm_url初始化，默认为udpm://239.255.76.67:7667

支持的链接种类

• udpm

  udpm://
      UDP Multicast provider
      network can be of the form "multicast_address:port".  Either the
      multicast address or the port may be ommitted for the default.
  
      options:
          recv_buf_size = N
              size of the kernel UDP receive buffer to request.  Defaults to
              operating system defaults
  
          ttl = N
              time to live of transmitted packets.  Default 0
  
      examples:
          "udpm://239.255.76.67:7667"
              Default initialization string
  
          "udpm://239.255.76.67:7667?ttl=1"
              Sets the multicast TTL to 1 so that packets published will enter
              the local network.
  

• file

  file://
      LCM Log file-based provider
      network should be the path to the log file
  
      Events are read from or written to the log file.  In read mode, events
      are generated from the log file in real-time, or at the rate specified
      by the speed option.  In write mode, events published to the LCM instance
      will be written to the log file in real-time.
  
      options:
          speed = N
              Scale factor controlling the playback speed of the log file.
              Defaults to 1.  If less than or equal to zero, then the events
              in the log file are played back as fast as possible.  Events are
              never skipped in read mode, so actual playback speed may be slower
              than requested, depending on the handlers.
  
          mode = r | w
              Specifies the log file mode.  Defaults to 'r'
  
          start_timestamp = USEC
              Seeks to USEC microseconds in the logfile, where USEC is given in
              microseconds since 00:00:00 UTC on 1 January 1970.  If USEC is
              before the first event, then playback begins at the start of the
              log file.  If it is after the last event, calls to lcm_handle will
              return -1.
  
      examples:
          "file:///home/albert/path/to/logfile"
              Loads the file "/home/albert/path/to/logfile" as an LCM event
              source.
  
          "file:///home/albert/path/to/logfile?speed=4"
              Loads the file "/home/albert/path/to/logfile" as an LCM event
              source.  Events are played back at 4x speed.
  

• memq

  memq://
     Memory queue test provider
  
     This provider is primarily useful for testing, especially unit testing code
     that uses LCM.  It provides a pub/sub interface that is private to the LCM
     instance, and does _not_ provide any interprocess communication, or even
     inter-instance communication.  Use this provider to implement unit tests
     that require deterministic and predictable behavior that is independent of
     a system's network configuration.
  
     example:
         "memq://"
             This is the only valid way to instantiate this provider.
  

发送消息

Publishes a raw data message

int lcm::LCM::publish(const std::string & channel,
                      const void * 	data,
                      unsigned int 	datalen )	

• Parameters

  channelthe channel to publish the message on.

  datadata buffer containing the message to publish.

  datalen length of the message, in bytes.

• Returns

  0 on success, -1 on failure.

Publishes a message with automatic message encoding

template<class MessageType >
int lcm::LCM::publish(const std::string & channel,
                      const MessageType * msg )	

This template method is designed for use with C++ classes generated by lcm-gen.

• Parameters

  channel the channel to publish the message on.

  msg the message to publish.

• Returns

  0 on success, -1 on failure.

订阅

通过类构造回调函数

template<class MessageType , class MessageHandlerClass >
Subscription * lcm::LCM::subscribe(const std::string & channel,
                    			   void(MessageHandlerClass::*)
                                   		(const ReceiveBuffer *rbuf, 
                                         const std::string &channel, 
                                         const MessageType *msg) handlerMethod,
                    			   MessageHandlerClass * handler)	

• Parameters

  channel The channel to subscribe to. This is treated as a regular expression implicitly surrounded by ‘^’ and ‘$’.

  handlerMethod A class method pointer identifying the callback method.

  handler A class instance that the callback method will be invoked on.

• Returns

  a Subscription object that can be used to adjust the subscription and unsubscribe. The Subscription object is managed by the LCM class, and is automatically destroyed when its LCM instance is destroyed.

直接设置回调函数

template<class MessageType >
Subscription * lcm::LCM::subscribe(const std::string & 	channel,
									LCM::HandlerFunction< MessageType > handler)	

• Parameters

  channel The channel to subscribe to. This is treated as a regular expression implicitly surrounded by ‘^’ and ‘$’.

  handler A handler function, for example a lambda.

• Returns

  a Subscription object that can be used to adjust the subscription and unsubscribe. The Subscription object is managed by the LCM class, and is automatically destroyed when its LCM instance is destroyed.

应用

C++

1. 创建lcm消息example_t.lcm

   struct example_t
   {
       int64_t  timestamp;
       double   position[3];
       double   orientation[4]; 
       int32_t  num_ranges;
       int16_t  ranges[num_ranges];
       string   name;
       boolean  enabled;
   }
   

2. 生成C++对应格式

   若需要通过lcm-spy可视化消息需要生成jar

   lcm-gen -x example_t.lcm
   

3. 创建发布器

   lcm发布的数据需要全部填充，不能空否则会报错

   #include <lcm/lcm-cpp.hpp>
   
   #include "example_t.hpp"
   
   int main(int argc, char ** argv)
   {
       lcm::LCM lcm;
       if(!lcm.good())
           return 1;
   
       example_t my_data;
       my_data.timestamp = 0;
   
       my_data.position[0] = 1;
       my_data.position[1] = 2;
       my_data.position[2] = 3;
   
       my_data.orientation[0] = 1;
       my_data.orientation[1] = 0;
       my_data.orientation[2] = 0;
       my_data.orientation[3] = 0;
   
       my_data.num_ranges = 15;
       my_data.ranges.resize(my_data.num_ranges);
       for(int i = 0; i < my_data.num_ranges; i++)
           my_data.ranges[i] = i;
   
       my_data.name = "example string";
       my_data.enabled = true;
   
       lcm.publish("EXAMPLE", &my_data);
   
       return 0;
   }
   

4. 创建订阅器

   #include <stdio.h>
   
   #include <lcm/lcm-cpp.hpp>
   #include "example_t.hpp"
   
   class Handler 
   {
       public:
           ~Handler() {}
   
           void handleMessage(const lcm::ReceiveBuffer* rbuf,
                   const std::string& chan, 
                   const example_t* msg)
           {
               (void)rbuf;
           	(void)chan;
               
               int i;
               printf("Received message on channel \"%s\":\n", chan.c_str());
               printf("  timestamp   = %lld\n", (long long)msg->timestamp);
               printf("  position    = (%f, %f, %f)\n",
                       msg->position[0], msg->position[1], msg->position[2]);
               printf("  orientation = (%f, %f, %f, %f)\n",
                       msg->orientation[0], msg->orientation[1], 
                       msg->orientation[2], msg->orientation[3]);
               printf("  ranges:");
               for(i = 0; i < msg->num_ranges; i++)
                   printf(" %d", msg->ranges[i]);
               printf("\n");
               printf("  name        = '%s'\n", msg->name.c_str());
               printf("  enabled     = %d\n", msg->enabled);
           }
   };
   
   int main(int argc, char** argv)
   {
       lcm::LCM lcm;
   
       if(!lcm.good())
           return 1;
   
       Handler handlerObject;
       lcm.subscribe("EXAMPLE", &Handler::handleMessage, &handlerObject);
   
       // Waits for and dispatches messages.
       // 0 on success, -1 if something went wrong
       // 使用handle会一直wait下去，后面程序不会执行，可以使用handleTimeout
       while(0 == lcm.handle());
   
       return 0;
   }
   

5. 编译

   include_directories("${PROJECT_SOURCE_DIR}/lcm_types") 
   include_directories("/usr/local/include/lcm/")   # lcm includes
   
   add_executable(publish_node 
      ./src/publish_node.cpp
   )
   target_link_libraries(publish_node lcm)
   
   add_executable(subscribe_node
      ./src/subscribe_node.cpp
   )
   target_link_libraries(subscribe_node lcm)
   

ROS

LCM基本用法与ROS的发布器-订阅器模式基本一致，因此在接收消息时会冲突（spin和handle），两者都会等待而无法执行下去，可改用（ spinOnce和handleTimeout）定时检测或多线程。

#include <ros/ros.h>
#include <std_msgs/Int16.h>

#include "cpp/example_t.hpp"
#include <lcm/lcm-cpp.hpp>

lcm::LCM LCM;

class MessageHandler
{
public:
    MessageHandler() {}

    ~MessageHandler() {}

    void handleMessage(const lcm::ReceiveBuffer *rbuf,
                       const std::string &chan,
                       const example_t *msg)
    {
        int i;
        printf("Received message on channel \"%s\":\n", chan.c_str());
        printf("  timestamp   = %lld\n", (long long)msg->timestamp);
        printf("  position    = (%f, %f, %f)\n",
               msg->position[0], msg->position[1], msg->position[2]);
        printf("  orientation = (%f, %f, %f, %f)\n",
               msg->orientation[0], msg->orientation[1],
               msg->orientation[2], msg->orientation[3]);
        printf("  ranges:");
        for (i = 0; i < msg->num_ranges; i++)
            printf(" %d", msg->ranges[i]);
        printf("\n");
        printf("  name        = '%s'\n", msg->name.c_str());
        printf("  enabled     = %d\n", msg->enabled);
    }
};

void lcm_sub_cb(const std_msgs::Int16::ConstPtr msg)
{
    example_t my_data;
    my_data.timestamp = msg->data;

    my_data.position[0] = 1;
    my_data.position[1] = 2;
    my_data.position[2] = 3;

    my_data.orientation[0] = 1;
    my_data.orientation[1] = 0;
    my_data.orientation[2] = 0;
    my_data.orientation[3] = 0;

    my_data.num_ranges = 15;
    my_data.ranges.resize(my_data.num_ranges);
    for (int i = 0; i < my_data.num_ranges; i++)
        my_data.ranges[i] = i;

    my_data.name = "example string";
    LCM.publish("EXAMPLE", &my_data);
}

int main(int argc, char **argv)
{
    // init ros
    ros::init(argc, argv, "lcm_node");
    ros::NodeHandle nh;
    ros::Subscriber lcm_sub =
        nh.subscribe<std_msgs::Int16>("/lcm_msg", 10, lcm_sub_cb);

    // init lcm
    if (!LCM.good())
        return 1;
    MessageHandler handlerObject;
    LCM.subscribe("EXAMPLE", &MessageHandler::handleMessage, &handlerObject);

    // run
    while (ros::ok())
    {
        LCM.handleTimeout(1);//单位ms
        ros::spinOnce();
    }
    return 0;
}

参考

自动驾驶消息传输机制LCM的安装与使用

ROS学习（四）ROS系统下LCM通信

lcm-proj

LCM C++ API

LCM使用