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。

支持消息类型:

安装

参考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
    

API

  • 发送消息

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

Publishes a raw data message.

  • 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.

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

Publishes a message with automatic message encoding.

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.lc1.

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使用