feat: update lcm-lite
This commit is contained in:
anjingyu
parent
e139cb953d
commit
456f438a01
|
|
@ -0,0 +1,106 @@
|
|||
# Byte-compiled / optimized / DLL files
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
*$py.class
|
||||
|
||||
# C extensions
|
||||
*.so
|
||||
|
||||
# Distribution / packaging
|
||||
.Python
|
||||
env/
|
||||
build/
|
||||
develop-eggs/
|
||||
dist/
|
||||
downloads/
|
||||
eggs/
|
||||
.eggs/
|
||||
lib/
|
||||
lib64/
|
||||
parts/
|
||||
sdist/
|
||||
var/
|
||||
*.egg-info/
|
||||
.installed.cfg
|
||||
*.egg
|
||||
|
||||
# PyInstaller
|
||||
# Usually these files are written by a python script from a template
|
||||
# before PyInstaller builds the exe, so as to inject date/other infos into it.
|
||||
*.manifest
|
||||
*.spec
|
||||
|
||||
# Installer logs
|
||||
pip-log.txt
|
||||
pip-delete-this-directory.txt
|
||||
|
||||
# Unit test / coverage reports
|
||||
htmlcov/
|
||||
.tox/
|
||||
.coverage
|
||||
.coverage.*
|
||||
.cache
|
||||
nosetests.xml
|
||||
coverage.xml
|
||||
*,cover
|
||||
.hypothesis/
|
||||
|
||||
# Translations
|
||||
*.mo
|
||||
*.pot
|
||||
|
||||
# Django stuff:
|
||||
*.log
|
||||
local_settings.py
|
||||
|
||||
# Flask stuff:
|
||||
instance/
|
||||
.webassets-cache
|
||||
|
||||
# Scrapy stuff:
|
||||
.scrapy
|
||||
|
||||
# Sphinx documentation
|
||||
docs/_build/
|
||||
|
||||
# PyBuilder
|
||||
target/
|
||||
|
||||
# IPython Notebook
|
||||
.ipynb_checkpoints
|
||||
|
||||
# pyenv
|
||||
.python-version
|
||||
|
||||
# celery beat schedule file
|
||||
celerybeat-schedule
|
||||
|
||||
# dotenv
|
||||
.env
|
||||
|
||||
# virtualenv
|
||||
venv/
|
||||
ENV/
|
||||
|
||||
# Spyder project settings
|
||||
.spyderproject
|
||||
|
||||
# Rope project settings
|
||||
.ropeproject
|
||||
|
||||
tags
|
||||
|
||||
test_data/test.log.*
|
||||
*.swp
|
||||
.directory
|
||||
*.dep
|
||||
*.o
|
||||
.ycm_extra_conf.py
|
||||
.build/*
|
||||
lib/*
|
||||
bin/*
|
||||
test_data/*.tmp
|
||||
test_lcm/*.c
|
||||
test_lcm/*.h
|
||||
test_lcm/*.cpp
|
||||
test_lcm/*.hpp
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
cmake_minimum_required (VERSION 3.15)
|
||||
|
||||
project (lcm)
|
||||
|
||||
set (ADMAKE_WIN_NEED_MT ON)
|
||||
set (ADMAKE_DISABLE_ADDRESS_SANITIZER ON)
|
||||
|
||||
include (CMakeListsPub)
|
||||
set (TARGET_NAME ${CMAKE_PROJECT_NAME})
|
||||
|
||||
include_directories ("${CMAKE_CURRENT_SOURCE_DIR}/include")
|
||||
|
||||
if (NOT DEFINED ADMAKE_BUILD_TEST)
|
||||
#lcm
|
||||
if (WIN32)
|
||||
file (GLOB_RECURSE LCM_SRC
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/source/winporting/*.c"
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/source/winporting/*.cpp"
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/source/lcm/*.c"
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/source/lcm/*.cpp")
|
||||
else ()
|
||||
file (GLOB_RECURSE LCM_SRC
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/source/lcm/*.c"
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/source/lcm/*.cpp")
|
||||
endif ()
|
||||
|
||||
add_library (${TARGET_NAME} STATIC ${LCM_SRC})
|
||||
|
||||
#lcmlite-logger
|
||||
file (GLOB_RECURSE LCMLITE_LOGGER_SRC
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/source/lcmlite-logger/*.c")
|
||||
add_executable (lcm-logger ${LCMLITE_LOGGER_SRC})
|
||||
add_dependencies (lcm-logger ${TARGET_NAME})
|
||||
if (WIN32)
|
||||
target_link_libraries (lcm-logger ${TARGET_NAME} ws2_32)
|
||||
elseif ("${AD_OS}" STREQUAL "qnx7")
|
||||
target_link_libraries (lcm-logger ${TARGET_NAME} socket)
|
||||
elseif (NOT("${AD_OS}" STREQUAL "android"))
|
||||
target_link_libraries (lcm-logger ${TARGET_NAME} pthread)
|
||||
else ()
|
||||
target_link_libraries (lcm-logger ${TARGET_NAME})
|
||||
endif ()
|
||||
|
||||
#lcmlite-logplayer
|
||||
file (GLOB_RECURSE LCMLITE_LOGGER_SRC
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/source/lcmlite-logplayer/*.c")
|
||||
add_executable (lcm-logplayer ${LCMLITE_LOGGER_SRC})
|
||||
add_dependencies (lcm-logplayer ${TARGET_NAME})
|
||||
if (WIN32)
|
||||
target_link_libraries (lcm-logplayer ${TARGET_NAME} ws2_32)
|
||||
elseif ("${AD_OS}" STREQUAL "qnx7")
|
||||
target_link_libraries (lcm-logplayer ${TARGET_NAME} socket)
|
||||
elseif (NOT("${AD_OS}" STREQUAL "android"))
|
||||
target_link_libraries (lcm-logplayer ${TARGET_NAME} pthread)
|
||||
else ()
|
||||
target_link_libraries (lcm-logplayer ${TARGET_NAME})
|
||||
endif ()
|
||||
|
||||
else ()
|
||||
execute_process (COMMAND lcm-gen
|
||||
"-c" "${CMAKE_CURRENT_SOURCE_DIR}/test_lcm/example.lcm"
|
||||
"--c-hpath" "${CMAKE_CURRENT_SOURCE_DIR}/test_lcm"
|
||||
"--c-cpath" "${CMAKE_CURRENT_SOURCE_DIR}/test_lcm")
|
||||
|
||||
execute_process (COMMAND lcm-gen
|
||||
"-x" "${CMAKE_CURRENT_SOURCE_DIR}/test_lcm/example.lcm"
|
||||
"--cpp-hpath" "${CMAKE_CURRENT_SOURCE_DIR}/test_lcm")
|
||||
include (PrecompiledHeader)
|
||||
include_directories ("${CMAKE_CURRENT_SOURCE_DIR}/test" "${CMAKE_CURRENT_SOURCE_DIR}/test_lcm")
|
||||
set (TEST_TARGET_NAME "${TARGET_NAME}_test")
|
||||
link_directories ("${CMAKE_CURRENT_SOURCE_DIR}/lib/${CMAKE_BUILD_TYPE}")
|
||||
append_3rd_modules (gtest)
|
||||
file (GLOB_RECURSE TEST_SRC "${CMAKE_CURRENT_SOURCE_DIR}/test/*.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/test_lcm/*.c")
|
||||
add_executable (${TEST_TARGET_NAME} ${TEST_SRC})
|
||||
target_link_libraries (${TEST_TARGET_NAME} ${TARGET_NAME} ${GTEST_LIB} ${DEPENDENCE_LIB})
|
||||
|
||||
endif ()
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
if (WIN32)
|
||||
set (_REQUIRED_LIBS "${module_lib_name}${SUFFIX_STR};${DEPENDENCE_LIB}")
|
||||
elseif (${AD_OS} STREQUAL "android")
|
||||
set (_REQUIRED_LIBS "${module_lib_name}${SUFFIX_STR}")
|
||||
elseif (${AD_OS} MATCHES "^qnx")
|
||||
set (_REQUIRED_LIBS "${module_lib_name}${SUFFIX_STR};socket;pthread")
|
||||
else ()
|
||||
# Append dependency "glib-2.0" and "pthread" on *UNIX
|
||||
set (_REQUIRED_LIBS "${module_lib_name}${SUFFIX_STR};pthread")
|
||||
endif ()
|
||||
|
||||
set (${MODULE_UPPER}_LIB "${_REQUIRED_LIBS}" CACHE INTERNAL "${MODULE_UPPER}_LIB")
|
||||
|
|
@ -0,0 +1,142 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @defgroup LcmC_lcm_eventlog_t lcm_eventlog_t
|
||||
* @ingroup LcmC
|
||||
* @brief Read and write %LCM log files
|
||||
*
|
||||
* @code
|
||||
* #include <lcm/lcm.h>
|
||||
* @endcode
|
||||
*
|
||||
* Linking: <tt> `pkg-config --libs lcm` </tt>
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef struct _lcm_eventlog_t lcm_eventlog_t;
|
||||
struct _lcm_eventlog_t
|
||||
{
|
||||
/**
|
||||
* The underlying file handle. Made available for debugging.
|
||||
*/
|
||||
FILE *f;
|
||||
|
||||
/**
|
||||
* Internal counter, keeps track of how many events have been written.
|
||||
*/
|
||||
int64_t eventcount;
|
||||
};
|
||||
|
||||
/**
|
||||
* Represents a single event (message) in a log file.
|
||||
*/
|
||||
typedef struct _lcm_eventlog_event_t lcm_eventlog_event_t;
|
||||
struct _lcm_eventlog_event_t
|
||||
{
|
||||
/**
|
||||
* A monotonically increasing number assigned to the message to identify it
|
||||
* in the log file.
|
||||
*/
|
||||
int64_t eventnum;
|
||||
/**
|
||||
* Time that the message was received, in microseconds since the UNIX
|
||||
* epoch
|
||||
*/
|
||||
int64_t timestamp;
|
||||
/**
|
||||
* Length of @c channel, in bytes
|
||||
*/
|
||||
int32_t channellen;
|
||||
/**
|
||||
* Length of @c data, in bytes
|
||||
*/
|
||||
int32_t datalen;
|
||||
|
||||
/**
|
||||
* Channel that the message was received on
|
||||
*/
|
||||
char *channel;
|
||||
/**
|
||||
* Raw byte buffer containing the message payload.
|
||||
*/
|
||||
void *data;
|
||||
};
|
||||
|
||||
/**
|
||||
* Open a log file for reading or writing.
|
||||
*
|
||||
* @param path Log file to open
|
||||
* @param mode "r" (read mode), "w" (write mode), or "a" (append mode)
|
||||
*
|
||||
* @return a newly allocated lcm_eventlog_t, or NULL on failure.
|
||||
*/
|
||||
|
||||
lcm_eventlog_t *lcm_eventlog_create(const char *path, const char *mode);
|
||||
|
||||
/**
|
||||
* Read the next event in the log file. Valid in read mode only. Free the
|
||||
* returned structure with lcm_eventlog_free_event() after use.
|
||||
*
|
||||
* @param eventlog The log file object
|
||||
*
|
||||
* @return the next event in the log file. Returns NULL when the end of the
|
||||
* file has been reached or when invalid data is read.
|
||||
*/
|
||||
|
||||
lcm_eventlog_event_t *lcm_eventlog_read_next_event(lcm_eventlog_t *eventlog);
|
||||
|
||||
/**
|
||||
* Free a structure returned by lcm_eventlog_read_next_event().
|
||||
*
|
||||
* @param event A structure returned by lcm_eventlog_read_next_event()
|
||||
*/
|
||||
|
||||
void lcm_eventlog_free_event(lcm_eventlog_event_t *event);
|
||||
|
||||
/**
|
||||
* Seek (approximately) to a particular timestamp.
|
||||
*
|
||||
* @param eventlog The log file object
|
||||
* @param ts Timestamp of the target event in the log file.
|
||||
*
|
||||
* @return 0 on success, -1 on failure
|
||||
*/
|
||||
|
||||
int lcm_eventlog_seek_to_timestamp(lcm_eventlog_t *eventlog, int64_t ts);
|
||||
|
||||
/**
|
||||
* Write an event into a log file. Valid in write mode only.
|
||||
*
|
||||
* @param eventlog The log file object
|
||||
* @param event The event to write to the file. On return, the eventnum field
|
||||
* will be filled in for you.
|
||||
*
|
||||
* @return 0 on success, -1 on failure.
|
||||
*/
|
||||
|
||||
int lcm_eventlog_write_event(lcm_eventlog_t *eventlog,
|
||||
lcm_eventlog_event_t *event);
|
||||
|
||||
/**
|
||||
* Close a log file and release allocated resources.
|
||||
*
|
||||
* @param eventlog The log file object
|
||||
*/
|
||||
|
||||
void lcm_eventlog_destroy(lcm_eventlog_t *eventlog);
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,82 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#if defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
#include <arpa/inet.h>
|
||||
#elif defined(OS_WINDOWS)
|
||||
#include <winsock2.h>
|
||||
#endif /* OS_LINUX || OS_ANDROID */
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
static inline int fwrite32(FILE *f, int32_t v)
|
||||
{
|
||||
v = htonl(v);
|
||||
|
||||
if (fwrite(&v, 4, 1, f) == 1)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
static inline int fwrite64(FILE *f, int64_t v64)
|
||||
{
|
||||
// See Section 5.8 paragraph 3 of the standard
|
||||
// http://open-std.org/JTC1/SC22/WG21/docs/papers/2015/n4527.pdf
|
||||
// use uint for shifting instead if int
|
||||
int32_t v = ((uint64_t)v64) >> 32;
|
||||
|
||||
if (0 != fwrite32(f, v))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
v = v64 & 0xffffffff;
|
||||
return fwrite32(f, v);
|
||||
}
|
||||
|
||||
static inline int fread32(FILE *f, int32_t *v32)
|
||||
{
|
||||
int32_t v;
|
||||
|
||||
if (fread(&v, 4, 1, f) != 1)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
*v32 = ntohl(v);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline int fread64(FILE *f, int64_t *v64)
|
||||
{
|
||||
int32_t v1, v2;
|
||||
|
||||
if (fread32(f, &v1))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (fread32(f, &v2))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// See Section 5.8 paragraph 3 of the standard
|
||||
// http://open-std.org/JTC1/SC22/WG21/docs/papers/2015/n4527.pdf
|
||||
// use uint for shifting instead if int
|
||||
*v64 = (int64_t)(((uint64_t) v1) << 32) | (((int64_t) v2) & 0xffffffff);
|
||||
return 0;
|
||||
}
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,430 @@
|
|||
// DO NOT EVER INCLUDE THIS HEADER FILE YOURSELF
|
||||
#pragma once
|
||||
|
||||
#include <lcm/lcm-cpp.hpp>
|
||||
|
||||
#ifndef __lcm_cpp_impl_ok__
|
||||
#error "Don't include this file"
|
||||
#endif
|
||||
|
||||
// =============== implementation ===============
|
||||
|
||||
int
|
||||
Subscription::setQueueCapacity(int num_messages)
|
||||
{
|
||||
return lcm_subscription_set_queue_capacity(c_subs, num_messages);
|
||||
}
|
||||
|
||||
template <class MessageType, class ContextClass>
|
||||
class LCMTypedSubscription : public Subscription
|
||||
{
|
||||
friend class LCM;
|
||||
private:
|
||||
ContextClass context;
|
||||
void (*handler)(const ReceiveBuffer *rbuf, const std::string &channel,
|
||||
const MessageType *msg, ContextClass context);
|
||||
static void cb_func(const lcm_recv_buf_t *rbuf, const char *channel,
|
||||
void *user_data)
|
||||
{
|
||||
typedef LCMTypedSubscription<MessageType, ContextClass> SubsClass;
|
||||
SubsClass *subs = static_cast<SubsClass *>(user_data);
|
||||
MessageType msg;
|
||||
int status = msg.decode(rbuf->data, 0, rbuf->data_size);
|
||||
|
||||
if (status < 0)
|
||||
{
|
||||
fprintf(stderr, "error %d decoding %s!!!\n", status,
|
||||
MessageType::getTypeName());
|
||||
return;
|
||||
}
|
||||
|
||||
const ReceiveBuffer rb =
|
||||
{
|
||||
rbuf->data,
|
||||
rbuf->data_size,
|
||||
rbuf->recv_utime
|
||||
};
|
||||
subs->handler(&rb, channel, &msg, subs->context);
|
||||
}
|
||||
};
|
||||
|
||||
template <class ContextClass>
|
||||
class LCMUntypedSubscription : public Subscription
|
||||
{
|
||||
friend class LCM;
|
||||
private:
|
||||
ContextClass context;
|
||||
void (*handler)(const ReceiveBuffer *rbuf, const std::string &channel,
|
||||
ContextClass context);
|
||||
static void cb_func(const lcm_recv_buf_t *rbuf, const char *channel,
|
||||
void *user_data)
|
||||
{
|
||||
typedef LCMUntypedSubscription<ContextClass> SubsClass;
|
||||
SubsClass *subs = static_cast<SubsClass *>(user_data);
|
||||
const ReceiveBuffer rb =
|
||||
{
|
||||
rbuf->data,
|
||||
rbuf->data_size,
|
||||
rbuf->recv_utime
|
||||
};
|
||||
subs->handler(&rb, channel, subs->context);
|
||||
}
|
||||
};
|
||||
|
||||
template <class MessageType, class MessageHandlerClass>
|
||||
class LCMMHSubscription : public Subscription
|
||||
{
|
||||
friend class LCM;
|
||||
private:
|
||||
MessageHandlerClass *handler;
|
||||
void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer *rbuf, const std::string &channel, const MessageType *msg);
|
||||
static void cb_func(const lcm_recv_buf_t *rbuf, const char *channel,
|
||||
void *user_data)
|
||||
{
|
||||
LCMMHSubscription<MessageType, MessageHandlerClass> *subs =
|
||||
static_cast<LCMMHSubscription<MessageType, MessageHandlerClass> *>(user_data);
|
||||
MessageType msg;
|
||||
int status = msg.decode(rbuf->data, 0, rbuf->data_size);
|
||||
|
||||
if (status < 0)
|
||||
{
|
||||
fprintf(stderr, "error %d decoding %s!!!\n", status,
|
||||
MessageType::getTypeName());
|
||||
return;
|
||||
}
|
||||
|
||||
const ReceiveBuffer rb =
|
||||
{
|
||||
rbuf->data,
|
||||
rbuf->data_size,
|
||||
rbuf->recv_utime
|
||||
};
|
||||
std::string chan_str(channel);
|
||||
(subs->handler->*subs->handlerMethod)(&rb, chan_str, &msg);
|
||||
}
|
||||
};
|
||||
|
||||
template<class MessageHandlerClass>
|
||||
class LCMMHUntypedSubscription : public Subscription
|
||||
{
|
||||
friend class LCM;
|
||||
private:
|
||||
MessageHandlerClass *handler;
|
||||
void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer *rbuf, const std::string &channel);
|
||||
static void cb_func(const lcm_recv_buf_t *rbuf, const char *channel, void *user_data)
|
||||
{
|
||||
LCMMHUntypedSubscription<MessageHandlerClass> *subs =
|
||||
static_cast<LCMMHUntypedSubscription<MessageHandlerClass> *>(user_data);
|
||||
const ReceiveBuffer rb =
|
||||
{
|
||||
rbuf->data,
|
||||
rbuf->data_size,
|
||||
rbuf->recv_utime
|
||||
};
|
||||
std::string chan_str(channel);
|
||||
(subs->handler->*subs->handlerMethod)(&rb, chan_str);
|
||||
}
|
||||
};
|
||||
|
||||
inline
|
||||
LCM::LCM(std::string lcm_url):
|
||||
owns_lcm(true)
|
||||
{
|
||||
this->lcm = lcm_create(lcm_url.c_str());
|
||||
}
|
||||
|
||||
inline
|
||||
LCM::LCM(lcm_t *lcm_in):
|
||||
owns_lcm(false)
|
||||
{
|
||||
this->lcm = lcm_in;
|
||||
}
|
||||
|
||||
inline bool
|
||||
LCM::good() const
|
||||
{
|
||||
return this->lcm != NULL;
|
||||
}
|
||||
|
||||
inline
|
||||
LCM::~LCM()
|
||||
{
|
||||
for (size_t i = 0, n = subscriptions.size(); i < n; i++)
|
||||
{
|
||||
delete subscriptions[i];
|
||||
}
|
||||
|
||||
if (this->lcm && this->owns_lcm)
|
||||
{
|
||||
lcm_destroy(this->lcm);
|
||||
}
|
||||
}
|
||||
|
||||
inline int
|
||||
LCM::publish(const std::string &channel, const void *data, unsigned int datalen)
|
||||
{
|
||||
if (!this->lcm)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"LCM instance not initialized. Ignoring call to publish()\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return lcm_publish(this->lcm, channel.c_str(), data, datalen);
|
||||
}
|
||||
|
||||
template<class MessageType>
|
||||
inline int
|
||||
LCM::publish(const std::string &channel, const MessageType *msg)
|
||||
{
|
||||
unsigned int datalen = msg->getEncodedSize();
|
||||
uint8_t *buf = new uint8_t[datalen];
|
||||
msg->encode(buf, 0, datalen);
|
||||
int status = this->publish(channel, buf, datalen);
|
||||
delete[] buf;
|
||||
return status;
|
||||
}
|
||||
|
||||
inline int
|
||||
LCM::unsubscribe(Subscription *subscription)
|
||||
{
|
||||
if (!this->lcm)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"LCM instance not initialized. Ignoring call to unsubscribe()\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::vector<Subscription *>::iterator iter;
|
||||
std::vector<Subscription *>::iterator eiter = subscriptions.end();
|
||||
|
||||
for (iter = subscriptions.begin(); iter != eiter; ++iter)
|
||||
{
|
||||
if (*iter == subscription)
|
||||
{
|
||||
int status = lcm_unsubscribe(lcm, subscription->c_subs);
|
||||
subscriptions.erase(iter);
|
||||
delete subscription;
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
inline int
|
||||
LCM::getFileno()
|
||||
{
|
||||
if (!this->lcm)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"LCM instance not initialized. Ignoring call to fileno()\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return lcm_get_fileno(this->lcm);
|
||||
}
|
||||
|
||||
inline int
|
||||
LCM::handle()
|
||||
{
|
||||
if (!this->lcm)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"LCM instance not initialized. Ignoring call to handle()\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return lcm_handle(this->lcm);
|
||||
}
|
||||
|
||||
inline int
|
||||
LCM::handleTimeout(int timeout_millis)
|
||||
{
|
||||
if (!this->lcm)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"LCM instance not initialized. Ignoring call to handle()\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return lcm_handle_timeout(this->lcm, timeout_millis);
|
||||
}
|
||||
|
||||
template <class MessageType, class MessageHandlerClass>
|
||||
Subscription *
|
||||
LCM::subscribe(const std::string &channel,
|
||||
void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer *rbuf, const std::string &channel, const MessageType *msg),
|
||||
MessageHandlerClass *handler)
|
||||
{
|
||||
if (!this->lcm)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"LCM instance not initialized. Ignoring call to subscribe()\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
LCMMHSubscription<MessageType, MessageHandlerClass> *subs =
|
||||
new LCMMHSubscription<MessageType, MessageHandlerClass>();
|
||||
subs->handler = handler;
|
||||
subs->handlerMethod = handlerMethod;
|
||||
subs->c_subs = lcm_subscribe(this->lcm, channel.c_str(),
|
||||
LCMMHSubscription<MessageType, MessageHandlerClass>::cb_func, subs);
|
||||
subscriptions.push_back(subs);
|
||||
return subs;
|
||||
}
|
||||
|
||||
template <class MessageHandlerClass>
|
||||
Subscription *
|
||||
LCM::subscribe(const std::string &channel,
|
||||
void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer *rbuf, const std::string &channel),
|
||||
MessageHandlerClass *handler)
|
||||
{
|
||||
if (!this->lcm)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"LCM instance not initialized. Ignoring call to subscribe()\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
LCMMHUntypedSubscription<MessageHandlerClass> *subs =
|
||||
new LCMMHUntypedSubscription<MessageHandlerClass>();
|
||||
subs->handler = handler;
|
||||
subs->handlerMethod = handlerMethod;
|
||||
subs->c_subs = lcm_subscribe(this->lcm, channel.c_str(),
|
||||
LCMMHUntypedSubscription<MessageHandlerClass>::cb_func, subs);
|
||||
subscriptions.push_back(subs);
|
||||
return subs;
|
||||
}
|
||||
|
||||
template <class MessageType, class ContextClass>
|
||||
Subscription *
|
||||
LCM::subscribeFunction(const std::string &channel,
|
||||
void (*handler)(const ReceiveBuffer *rbuf,
|
||||
const std::string &channel,
|
||||
const MessageType *msg, ContextClass context),
|
||||
ContextClass context)
|
||||
{
|
||||
if (!this->lcm)
|
||||
{
|
||||
fprintf(stderr, "LCM instance not initialized. Ignoring call to subscribeFunction()\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
typedef LCMTypedSubscription<MessageType, ContextClass> SubsClass;
|
||||
SubsClass *sub = new SubsClass();
|
||||
sub->c_subs = lcm_subscribe(lcm, channel.c_str(), SubsClass::cb_func, sub);
|
||||
sub->handler = handler;
|
||||
sub->context = context;
|
||||
subscriptions.push_back(sub);
|
||||
return sub;
|
||||
}
|
||||
|
||||
template <class ContextClass>
|
||||
Subscription *
|
||||
LCM::subscribeFunction(const std::string &channel,
|
||||
void (*handler)(const ReceiveBuffer *rbuf,
|
||||
const std::string &channel,
|
||||
ContextClass context),
|
||||
ContextClass context)
|
||||
{
|
||||
if (!this->lcm)
|
||||
{
|
||||
fprintf(stderr, "LCM instance not initialized. Ignoring call to subscribeFunction()\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
typedef LCMUntypedSubscription<ContextClass> SubsClass;
|
||||
SubsClass *sub = new SubsClass();
|
||||
sub->c_subs = lcm_subscribe(lcm, channel.c_str(), SubsClass::cb_func, sub);
|
||||
sub->handler = handler;
|
||||
sub->context = context;
|
||||
subscriptions.push_back(sub);
|
||||
return sub;
|
||||
}
|
||||
|
||||
|
||||
lcm_t *
|
||||
LCM::getUnderlyingLCM()
|
||||
{
|
||||
return this->lcm;
|
||||
}
|
||||
|
||||
LogFile::LogFile(const std::string &path, const std::string &mode) :
|
||||
eventlog(lcm_eventlog_create(path.c_str(), mode.c_str())),
|
||||
last_event(NULL)
|
||||
{
|
||||
}
|
||||
|
||||
LogFile::~LogFile()
|
||||
{
|
||||
if (eventlog)
|
||||
{
|
||||
lcm_eventlog_destroy(eventlog);
|
||||
}
|
||||
|
||||
eventlog = NULL;
|
||||
|
||||
if (last_event)
|
||||
{
|
||||
lcm_eventlog_free_event(last_event);
|
||||
}
|
||||
|
||||
last_event = NULL;
|
||||
}
|
||||
|
||||
bool
|
||||
LogFile::good() const
|
||||
{
|
||||
return eventlog != NULL;
|
||||
}
|
||||
|
||||
const LogEvent *
|
||||
LogFile::readNextEvent()
|
||||
{
|
||||
lcm_eventlog_event_t *evt = lcm_eventlog_read_next_event(eventlog);
|
||||
|
||||
if (last_event)
|
||||
{
|
||||
lcm_eventlog_free_event(last_event);
|
||||
}
|
||||
|
||||
last_event = evt;
|
||||
|
||||
if (!evt)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
curEvent.eventnum = evt->eventnum;
|
||||
curEvent.timestamp = evt->timestamp;
|
||||
curEvent.channel.assign(evt->channel, evt->channellen);
|
||||
curEvent.datalen = evt->datalen;
|
||||
curEvent.data = evt->data;
|
||||
return &curEvent;
|
||||
}
|
||||
|
||||
int
|
||||
LogFile::seekToTimestamp(int64_t timestamp)
|
||||
{
|
||||
return lcm_eventlog_seek_to_timestamp(eventlog, timestamp);
|
||||
}
|
||||
|
||||
int
|
||||
LogFile::writeEvent(LogEvent *event)
|
||||
{
|
||||
lcm_eventlog_event_t evt;
|
||||
evt.eventnum = event->eventnum;
|
||||
evt.timestamp = event->timestamp;
|
||||
evt.channellen = (int32_t)event->channel.size();
|
||||
evt.datalen = event->datalen;
|
||||
evt.channel = const_cast<char *>(event->channel.c_str());
|
||||
evt.data = event->data;
|
||||
return lcm_eventlog_write_event(eventlog, &evt);
|
||||
}
|
||||
|
||||
FILE *
|
||||
LogFile::getFilePtr()
|
||||
{
|
||||
return eventlog->f;
|
||||
}
|
||||
|
|
@ -0,0 +1,588 @@
|
|||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cstdio> /* needed for FILE* */
|
||||
#include <lcm/lcm.h>
|
||||
|
||||
namespace lcm {
|
||||
|
||||
/**
|
||||
* @defgroup LcmCpp C++ API Reference
|
||||
*
|
||||
* THe %LCM C++ API provides classes and data structures for communicating with
|
||||
* other %LCM clients, as well as reading and writing %LCM log files. It is a
|
||||
* pure header wrapper around the C API, and has the same linking requirements
|
||||
* as the C API.
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
|
||||
class Subscription;
|
||||
|
||||
struct ReceiveBuffer;
|
||||
|
||||
/**
|
||||
* @brief Core communications class for the C++ API.
|
||||
*
|
||||
* @headerfile lcm/lcm-cpp.hpp
|
||||
*/
|
||||
class LCM
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor.
|
||||
*
|
||||
* Initializes the LCM instance and connects it to the specified LCM
|
||||
* network. See the documentation on lcm_create() in the C API for
|
||||
* details on how lcm_url is formatted.
|
||||
*
|
||||
* @sa lcm_create()
|
||||
*/
|
||||
inline LCM(std::string lcm_url = "");
|
||||
|
||||
/**
|
||||
* @brief Constructor.
|
||||
*
|
||||
* Initializes the c++ LCM instance from an existing C instance.
|
||||
*
|
||||
* @sa lcm_create()
|
||||
*/
|
||||
inline LCM(lcm_t *lcm_in);
|
||||
|
||||
/**
|
||||
* @brief Destructor.
|
||||
*
|
||||
* Disconnects from the LCM network, and destroys all outstanding
|
||||
* Subscription objects.
|
||||
*/
|
||||
inline ~LCM();
|
||||
|
||||
/**
|
||||
* @brief Checks if initialization succeeded during object
|
||||
* construction.
|
||||
*
|
||||
* @return true if initialization succeeded and the instance appears
|
||||
* ready for communication, false if not.
|
||||
*/
|
||||
inline bool good() const;
|
||||
|
||||
/**
|
||||
* @brief Publishes a raw data message.
|
||||
*
|
||||
* @param channel the channel to publish the message on.
|
||||
* @param data data buffer containing the message to publish
|
||||
* @param datalen length of the message, in bytes.
|
||||
*
|
||||
* @return 0 on success, -1 on failure.
|
||||
*/
|
||||
inline int publish(const std::string &channel, const void *data,
|
||||
unsigned int datalen);
|
||||
|
||||
/**
|
||||
* @brief Publishes a message with automatic message encoding.
|
||||
*
|
||||
* This template method is designed for use with C++ classes generated
|
||||
* by lcm-gen.
|
||||
*
|
||||
* @param channel the channel to publish the message on.
|
||||
* @param msg the message to publish.
|
||||
*
|
||||
* @return 0 on success, -1 on failure.
|
||||
*/
|
||||
template<class MessageType>
|
||||
inline int publish(const std::string &channel, const MessageType *msg);
|
||||
|
||||
/**
|
||||
* @brief Returns a file descriptor or socket that can be used with
|
||||
* @c select(), @c poll(), or other event loops for asynchronous
|
||||
* notification of incoming messages.
|
||||
*
|
||||
* This method is useful when integrating LCM into another event loop,
|
||||
* such as the Qt event loop (via QSocketNotifier), the GLib event loop
|
||||
* (via GIOChannel), a custom @c select() @c - or @c poll() @c -based event loop, or any other
|
||||
* event loop that supports file descriptors.
|
||||
*
|
||||
* @todo link to example code.
|
||||
*
|
||||
* @return a non-negative file descriptor on success, or -1 if something
|
||||
* is wrong.
|
||||
* @sa lcm_get_fileno()
|
||||
*/
|
||||
inline int getFileno();
|
||||
|
||||
/**
|
||||
* @brief Waits for and dispatches messages.
|
||||
*
|
||||
* @return 0 on success, -1 if something went wrong.
|
||||
* @sa lcm_handle()
|
||||
*/
|
||||
inline int handle();
|
||||
|
||||
/**
|
||||
* @brief Waits for and dispatches messages, with a timeout.
|
||||
*
|
||||
* New in LCM 1.1.0.
|
||||
*
|
||||
* @return >0 if a message was handled, 0 if the function timed out,
|
||||
* and <0 if an error occured.
|
||||
* @sa lcm_handle_timeout()
|
||||
*/
|
||||
inline int handleTimeout(int timeout_millis);
|
||||
|
||||
/**
|
||||
* @brief Subscribes a callback method of an object to a channel, with
|
||||
* automatic message decoding.
|
||||
*
|
||||
* This method is designed for use with C++ classes generated by
|
||||
* @c lcm-gen @c .
|
||||
*
|
||||
* The callback method will be invoked on the object when a message
|
||||
* arrives on the specified channel. Prior to method invocation, LCM
|
||||
* will attempt to automatically decode the message to the specified
|
||||
* message type @c MessageType @c , which should be a class generated
|
||||
* by @c lcm-gen @c . If message
|
||||
* decoding fails, the callback method is not invoked and an error
|
||||
* message is printed to stderr.
|
||||
*
|
||||
* The callback method is invoked during calls to LCM::handle().
|
||||
* Callback methods are invoked by the same thread that invokes
|
||||
* LCM::handle(), in the order that they were subscribed.
|
||||
*
|
||||
* For example:
|
||||
*
|
||||
* \code
|
||||
* #include <exlcm/example_t.lcm>
|
||||
* #include <lcm/lcm-cpp.hpp>
|
||||
*
|
||||
* class MyMessageHandler {
|
||||
* void onMessage(const lcm::ReceiveBuffer* rbuf, const std::string& channel,
|
||||
* const exlcm::example_t* msg) {
|
||||
* // do something with the message
|
||||
* }
|
||||
* };
|
||||
*
|
||||
* int main(int argc, char** argv) {
|
||||
* lcm::LCM lcm;
|
||||
* MyMessageHandler handler;
|
||||
* lcm.subscribe("CHANNEL", &MyMessageHandler::onMessage, &handler);
|
||||
* while(true)
|
||||
* lcm.handle();
|
||||
* return 0;
|
||||
* }
|
||||
* \endcode
|
||||
*
|
||||
* @param channel The channel to subscribe to. This is treated as a
|
||||
* regular expression implicitly surrounded by '^' and '$'.
|
||||
* @param handlerMethod A class method pointer identifying the callback
|
||||
* method.
|
||||
* @param handler A class instance that the callback method will be
|
||||
* invoked on.
|
||||
*
|
||||
* @return 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, class MessageHandlerClass>
|
||||
Subscription *subscribe(const std::string &channel,
|
||||
void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer *rbuf, const std::string &channel, const MessageType *msg),
|
||||
MessageHandlerClass *handler);
|
||||
|
||||
/**
|
||||
* @brief Subscribe a callback method of an object to a channel,
|
||||
* without automatic message decoding.
|
||||
*
|
||||
* This method is designed for use when automatic message decoding is
|
||||
* not desired.
|
||||
*
|
||||
* The callback method will be invoked on the object when a message
|
||||
* arrives on the specified channel. Callback methods are invoked
|
||||
* during calls to LCM::handle(), by the same thread that calls
|
||||
* LCM::handle(). Callbacks are invoked in the order that they were
|
||||
* subscribed.
|
||||
*
|
||||
* For example:
|
||||
*
|
||||
* \code
|
||||
* #include <lcm/lcm-cpp.hpp>
|
||||
*
|
||||
* class MyMessageHandler {
|
||||
* void onMessage(const lcm::ReceiveBuffer* rbuf, const std::string& channel) {
|
||||
* // do something with the message. Raw message bytes are
|
||||
* // accessible via rbuf->data
|
||||
* }
|
||||
* };
|
||||
*
|
||||
* int main(int argc, char** argv) {
|
||||
* lcm::LCM lcm;
|
||||
* MyMessageHandler handler;
|
||||
* lcm.subscribe("CHANNEL", &MyMessageHandler::onMessage, &handler);
|
||||
* while(true)
|
||||
* lcm.handle();
|
||||
* return 0;
|
||||
* }
|
||||
* \endcode
|
||||
*
|
||||
* @param channel The channel to subscribe to. This is treated as a
|
||||
* regular expression implicitly surrounded by '^' and '$'.
|
||||
* @param handlerMethod A class method pointer identifying the callback
|
||||
* method.
|
||||
* @param handler A class instance that the callback method will be
|
||||
* invoked on.
|
||||
*
|
||||
* @return 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 MessageHandlerClass>
|
||||
Subscription *subscribe(const std::string &channel,
|
||||
void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer *rbuf, const std::string &channel),
|
||||
MessageHandlerClass *handler);
|
||||
|
||||
/**
|
||||
* @brief Subscribe a function callback to a channel, with automatic
|
||||
* message decoding.
|
||||
*
|
||||
* This method is designed for use with static member functions and
|
||||
* C-style functions.
|
||||
*
|
||||
* The callback function will be invoked on the object when a message
|
||||
* arrives on the specified channel. Prior to callback invocation, LCM
|
||||
* will attempt to automatically decode the message to the specified
|
||||
* message type @c MessageType @c , which should be a class generated
|
||||
* by @c lcm-gen @c . If message decoding fails, the callback function
|
||||
* is not invoked and an error message is printed to stderr.
|
||||
*
|
||||
* The callback function is invoked during calls to LCM::handle().
|
||||
* Callbacks are invoked by the same thread that invokes
|
||||
* LCM::handle(), in the order that they were subscribed.
|
||||
*
|
||||
* For example:
|
||||
*
|
||||
* \code
|
||||
* #include <lcm/lcm-cpp.hpp>
|
||||
*
|
||||
* class State {
|
||||
* public:
|
||||
* lcm::LCM lcm;
|
||||
* int usefulVariable;
|
||||
* };
|
||||
*
|
||||
* void onMessage(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const MessageType* msg, State* state) {
|
||||
* // do something with the message.
|
||||
* }
|
||||
*
|
||||
* int main(int argc, char** argv) {
|
||||
* State* state = new State;
|
||||
* state->lcm.subscribe("CHANNEL", onMessage, state);
|
||||
* while(true)
|
||||
* state->lcm.handle();
|
||||
* delete state;
|
||||
* return 0;
|
||||
* }
|
||||
* \endcode
|
||||
*
|
||||
* @param channel The channel to subscribe to. This is treated as a
|
||||
* regular expression implicitly surrounded by '^' and '$'.
|
||||
* @param handler A function pointer identifying the callback
|
||||
* function.
|
||||
* @param context A context variable that will be passed to the
|
||||
* callback function. This can be used to pass state or other
|
||||
* information to the callback function. If not needed, then @c
|
||||
* ContextClass @c can be set to void*, and this argument set to NULL.
|
||||
*
|
||||
* @return 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, class ContextClass>
|
||||
Subscription *subscribeFunction(const std::string &channel,
|
||||
void (*handler)(const ReceiveBuffer *rbuf,
|
||||
const std::string &channel,
|
||||
const MessageType *msg,
|
||||
ContextClass context),
|
||||
ContextClass context);
|
||||
|
||||
/**
|
||||
* @brief Subscribe a function callback to a channel, without automatic
|
||||
* message decoding.
|
||||
*
|
||||
* This method is designed for use when automatic message decoding is
|
||||
* not desired.
|
||||
*
|
||||
* For example:
|
||||
*
|
||||
* \code
|
||||
* #include <lcm/lcm-cpp.hpp>
|
||||
*
|
||||
* void onMessage(const lcm::ReceiveBuffer* rbuf, const std::string& channel, void*) {
|
||||
* // do something with the message. Raw message bytes are
|
||||
* // accessible via rbuf->data
|
||||
* }
|
||||
*
|
||||
* int main(int argc, char** argv) {
|
||||
* LCM::lcm lcm;
|
||||
* lcm.subscribe("CHANNEL", onMessage, NULL);
|
||||
* while(true)
|
||||
* lcm.handle();
|
||||
* return 0;
|
||||
* }
|
||||
* \endcode
|
||||
*
|
||||
* @param channel The channel to subscribe to. This is treated as a
|
||||
* regular expression implicitly surrounded by '^' and '$'.
|
||||
* @param handler A function pointer identifying the callback
|
||||
* function.
|
||||
* @param context A context variable that will be passed to the
|
||||
* callback function. This can be used to pass state or other
|
||||
* information to the callback function. If not needed, then @c
|
||||
* ContextClass @c can be set to void*, and this argument set to NULL.
|
||||
*
|
||||
* @return 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 ContextClass>
|
||||
Subscription *subscribeFunction(const std::string &channel,
|
||||
void (*handler)(const ReceiveBuffer *rbuf,
|
||||
const std::string &channel,
|
||||
ContextClass context),
|
||||
ContextClass context);
|
||||
|
||||
/**
|
||||
* @brief Unsubscribes a message handler.
|
||||
*
|
||||
* After unsubscription, the callback registered by the original call
|
||||
* to subscribe() or subscribeFunction() will no longer be invoked when
|
||||
* messages are received.
|
||||
* The Subscription object is destroyed by this method.
|
||||
*
|
||||
* @param subscription a Subscription object previously returned by a
|
||||
* call to subscribe() or subscribeFunction().
|
||||
*
|
||||
* @return 0 on success, -1 if @p subscription is not a valid
|
||||
* subscription.
|
||||
*/
|
||||
inline int unsubscribe(Subscription *subscription);
|
||||
|
||||
/**
|
||||
* @brief retrives the lcm_t C data structure wrapped by this class.
|
||||
*
|
||||
* This method should be used carefully and sparingly. An example use
|
||||
* case would be extending the subscription mechanism to Boost
|
||||
* Function objects.
|
||||
*
|
||||
* @return the lcm_t instance wrapped by this object.
|
||||
*
|
||||
* @sa lcm_t
|
||||
*/
|
||||
inline lcm_t *getUnderlyingLCM();
|
||||
|
||||
private:
|
||||
lcm_t *lcm;
|
||||
bool owns_lcm;
|
||||
|
||||
std::vector<Subscription *> subscriptions;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Stores the raw bytes and timestamp of a received message.
|
||||
*
|
||||
* @headerfile lcm/lcm-cpp.hpp
|
||||
*/
|
||||
struct ReceiveBuffer
|
||||
{
|
||||
/**
|
||||
* Message payload data, represented as a raw byte buffer.
|
||||
*/
|
||||
void *data;
|
||||
/**
|
||||
* Length of message payload, in bytes.
|
||||
*/
|
||||
uint32_t data_size;
|
||||
/**
|
||||
* Timestamp identifying when the message was received. Specified in
|
||||
* microseconds since the UNIX epoch.
|
||||
*/
|
||||
int64_t recv_utime;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Represents a channel subscription, and can be used to unsubscribe
|
||||
* and set options.
|
||||
*
|
||||
* This class is not meant to be instantiated by the user, and instead is
|
||||
* constructed and returned by a call to LCM::subscribe() or
|
||||
* LCM::subscribeFunction().
|
||||
*
|
||||
* To unsubscribe, pass the instance to LCM::unsubscribe(). Once unsubscribed,
|
||||
* the object is destroyed and can not be used anymore.
|
||||
*
|
||||
* @headerfile lcm/lcm-cpp.hpp
|
||||
*/
|
||||
class Subscription
|
||||
{
|
||||
public:
|
||||
virtual ~Subscription() {}
|
||||
|
||||
/**
|
||||
* @brief Adjusts the maximum number of received messages that can be
|
||||
* queued up for this subscription.
|
||||
*
|
||||
* @param num_messages the maximum queue size, in messages. The
|
||||
* default is 30.
|
||||
*
|
||||
* Setting this to a low number may reduce
|
||||
* overall latency at the expense of dropping more messages.
|
||||
* Conversely, setting this to a high number may drop fewer messages at
|
||||
* the expense of increased latency. A value of 0 indicates no limit,
|
||||
* and should be used very carefully.
|
||||
*
|
||||
*/
|
||||
inline int setQueueCapacity(int num_messages);
|
||||
|
||||
friend class LCM;
|
||||
protected:
|
||||
Subscription() {};
|
||||
/**
|
||||
* The underlying lcm_subscription_t object wrapped by this
|
||||
* subscription.
|
||||
*/
|
||||
lcm_subscription_t *c_subs;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Represents a single event (message) in a log file.
|
||||
*
|
||||
*
|
||||
*
|
||||
* This struct is the C++ counterpart for lcm_eventlog_event_t.
|
||||
*
|
||||
* @sa lcm_eventlog_event_t
|
||||
*
|
||||
* @headerfile lcm/lcm-cpp.hpp
|
||||
*/
|
||||
struct LogEvent
|
||||
{
|
||||
/**
|
||||
* Monotically increasing counter identifying the event number. This field
|
||||
* is managed by LCM, and there should be no need to ever set it manually.
|
||||
*/
|
||||
int64_t eventnum;
|
||||
/**
|
||||
* Timestamp identifying when the event was received. Represented in
|
||||
* microseconds since the UNIX epoch.
|
||||
*/
|
||||
int64_t timestamp;
|
||||
/**
|
||||
* The LCM channel on which the message was received.
|
||||
*/
|
||||
std::string channel;
|
||||
/**
|
||||
* The length of the message payload, in bytes
|
||||
*/
|
||||
int32_t datalen;
|
||||
/**
|
||||
* The message payload.
|
||||
*/
|
||||
void *data;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Read and write %LCM log files.
|
||||
*
|
||||
* This class is the C++ counterpart for lcm_eventlog_t.
|
||||
*
|
||||
* @sa lcm_eventlog_t
|
||||
*
|
||||
* @headerfile lcm/lcm-cpp.hpp
|
||||
*/
|
||||
class LogFile
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor. Opens the specified log file for reading or writing.
|
||||
* @param path the file to open
|
||||
* @param mode "r" (read mode) or "w" (write mode)
|
||||
*
|
||||
* @sa lcm_eventlog_create()
|
||||
*/
|
||||
inline LogFile(const std::string &path, const std::string &mode);
|
||||
|
||||
/**
|
||||
* Destructor. Closes the log file.
|
||||
*/
|
||||
inline ~LogFile();
|
||||
|
||||
/**
|
||||
* @return true if the log file is ready for reading/writing.
|
||||
*/
|
||||
inline bool good() const;
|
||||
|
||||
/**
|
||||
* Reads the next event in the log file. Valid in read mode only.
|
||||
*
|
||||
* The LogFile class manages the memory of the read event. The
|
||||
* returned event is valid until the next call to this method.
|
||||
*
|
||||
* @return the next event, or NULL if the end of the log file has been
|
||||
* reached.
|
||||
*/
|
||||
inline const LogEvent *readNextEvent();
|
||||
|
||||
/**
|
||||
* Seek close to the specified timestamp in the log file. Valid
|
||||
* in read mode only.
|
||||
*
|
||||
* @param timestamp the desired seek point in the log file.
|
||||
*
|
||||
* @return 0 on success, -1 on error.
|
||||
* @sa lcm_eventlog_seek_to_timestamp()
|
||||
*/
|
||||
inline int seekToTimestamp(int64_t timestamp);
|
||||
|
||||
/**
|
||||
* Writes an event to the log file. Valid in write mode only.
|
||||
*
|
||||
* @param event the event to write. The timestamp, channel, datalen,
|
||||
* and data fields should be filled in. The eventnum field will be
|
||||
* automatically filled in.
|
||||
*
|
||||
* @return 0 on success, -1 on error.
|
||||
* @sa lcm_eventlog_write_event()
|
||||
*/
|
||||
inline int writeEvent(LogEvent *event);
|
||||
|
||||
/**
|
||||
* @brief retrives the underlying FILE* wrapped by this class.
|
||||
*
|
||||
* This method should be used carefully and sparingly.
|
||||
* An example use-case is borrowing to tweak the behavior of the I/O.
|
||||
* Calls of interest include fflush(), fileno(), setvbuf(), etc
|
||||
* It is a bad idea to attempt reading or writing on the raw FILE*
|
||||
*
|
||||
* @return the FILE* wrapped by this object.
|
||||
*/
|
||||
inline FILE *getFilePtr();
|
||||
|
||||
private:
|
||||
LogEvent curEvent;
|
||||
lcm_eventlog_t *eventlog;
|
||||
lcm_eventlog_event_t *last_event;
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
#ifndef __lcm_cpp_impl_ok__
|
||||
#define __lcm_cpp_impl_ok__
|
||||
#include <lcm/lcm-cpp-impl.hpp>
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,126 @@
|
|||
#pragma once
|
||||
|
||||
#include <lcm/lcmlite.h>
|
||||
|
||||
#if defined(OS_WINDOWS)
|
||||
#define _WINSOCKAPI_
|
||||
#include <windows.h>
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
#include <pthread.h>
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
#if defined(__cplusplus)
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <lcm/url_parser.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
typedef struct _lcm_t lcm_t;
|
||||
typedef struct _lcm_connect_info_t lcm_connect_info_t;
|
||||
typedef struct _lcm_recv_buf_t lcm_recv_buf_t;
|
||||
typedef struct _lcm_subscription_t lcm_subscription_t;
|
||||
|
||||
#define LCM_TIMEOUT_FOREVER 0
|
||||
|
||||
// Callback to create a handler of a protocol
|
||||
typedef lcm_connect_info_t *(*lcm_protocol_match_t)(const url_parse_result_t *);
|
||||
|
||||
// Callback to receive packages
|
||||
typedef int (*lcm_recv_callback_t)(lcm_t *lcm, int timeout);
|
||||
typedef void (*lcm_msg_handler_t)(const lcm_recv_buf_t *rbuf,
|
||||
const char *channel, void *user_data);
|
||||
typedef void (*lcm_send_callback_t)(const void *_buf, int buf_len,
|
||||
lcm_connect_info_t *user);
|
||||
typedef void (*lcm_protocol_reg_t)();
|
||||
|
||||
// Protocol info
|
||||
typedef struct _lcm_protocol_info_t {
|
||||
// Match url, not NULL matched.
|
||||
lcm_protocol_match_t match;
|
||||
|
||||
// Next protocol
|
||||
struct _lcm_protocol_info_t *next;
|
||||
} lcm_protocol_info_t;
|
||||
|
||||
// Handler info
|
||||
struct _lcm_connect_info_t {
|
||||
// Called when lcm_handle was called
|
||||
lcm_recv_callback_t handler;
|
||||
|
||||
// Callback to send packages
|
||||
lcm_send_callback_t send_callback;
|
||||
|
||||
// Get file number
|
||||
int (*get_file_no)(lcm_connect_info_t *);
|
||||
|
||||
// Destroy
|
||||
void (*destroy)(lcm_connect_info_t *);
|
||||
};
|
||||
|
||||
// Lcm object
|
||||
struct _lcm_t {
|
||||
// lcm
|
||||
lcmlite_t lcm;
|
||||
|
||||
// Handle lock
|
||||
#if defined(OS_WINDOWS)
|
||||
CRITICAL_SECTION lock;
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_mutex_t lock;
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
// Info of protocol
|
||||
lcm_connect_info_t *info;
|
||||
};
|
||||
|
||||
// Subscription
|
||||
struct _lcm_subscription_t {
|
||||
lcmlite_subscription_t light_subscription;
|
||||
lcm_msg_handler_t callback;
|
||||
void *userdata;
|
||||
};
|
||||
|
||||
struct _lcm_recv_buf_t {
|
||||
/**
|
||||
* the data received (raw bytes)
|
||||
*/
|
||||
void *data;
|
||||
/**
|
||||
* the length of the data received (in bytes)
|
||||
*/
|
||||
uint32_t data_size;
|
||||
/**
|
||||
* timestamp (micrseconds since the epoch) at which the message was
|
||||
* received.
|
||||
*/
|
||||
int64_t recv_utime;
|
||||
/**
|
||||
* pointer to the lcm_t struct that owns this buffer
|
||||
*/
|
||||
lcm_t *lcm;
|
||||
};
|
||||
|
||||
// Regist new protocol
|
||||
void lcm_regist_protocol(lcm_protocol_info_t *p_protocol_info);
|
||||
|
||||
// See document of lcm
|
||||
lcm_t *lcm_create(const char *url);
|
||||
void lcm_destroy(lcm_t *lcm);
|
||||
int lcm_publish(lcm_t *lcm, const char *channel, const void *data,
|
||||
unsigned int datalen);
|
||||
lcm_subscription_t *lcm_subscribe(lcm_t *lcm, const char *channel,
|
||||
lcm_msg_handler_t handler, void *userdata);
|
||||
int lcm_subscription_set_queue_capacity(lcm_subscription_t *handler,
|
||||
int num_messages);
|
||||
int lcm_unsubscribe(lcm_t *lcm, lcm_subscription_t *handler);
|
||||
int lcm_handle(lcm_t *lcm);
|
||||
int lcm_handle_timeout(lcm_t *lcm, int timeout_millis);
|
||||
int lcm_get_fileno(lcm_t *lcm);
|
||||
uint64_t lcm_get_timestamp();
|
||||
|
||||
#if defined(__cplusplus)
|
||||
}
|
||||
#endif
|
||||
|
||||
#include <lcm/eventlog.h>
|
||||
|
|
@ -0,0 +1,628 @@
|
|||
#pragma once
|
||||
|
||||
#include <lcm/lcm.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
union float_uint32
|
||||
{
|
||||
float f;
|
||||
uint32_t i;
|
||||
};
|
||||
|
||||
union double_uint64
|
||||
{
|
||||
double f;
|
||||
uint64_t i;
|
||||
};
|
||||
|
||||
typedef struct ___lcm_hash_ptr __lcm_hash_ptr;
|
||||
struct ___lcm_hash_ptr
|
||||
{
|
||||
const __lcm_hash_ptr *parent;
|
||||
int64_t (*v)(void);
|
||||
};
|
||||
|
||||
/**
|
||||
* BOOLEAN
|
||||
*/
|
||||
#define __boolean_hash_recursive __int8_t_hash_recursive
|
||||
#define __boolean_decode_array_cleanup __int8_t_decode_array_cleanup
|
||||
#define __boolean_encoded_array_size __int8_t_encoded_array_size
|
||||
#define __boolean_encode_array __int8_t_encode_array
|
||||
#define __boolean_decode_array __int8_t_decode_array
|
||||
#define __boolean_clone_array __int8_t_clone_array
|
||||
#define boolean_encoded_size int8_t_encoded_size
|
||||
|
||||
/**
|
||||
* BYTE
|
||||
*/
|
||||
#define __byte_hash_recursive(p) 0
|
||||
#define __byte_decode_array_cleanup(p, sz) {}
|
||||
#define byte_encoded_size(p) ( sizeof(int64_t) + sizeof(uint8_t) )
|
||||
|
||||
static inline int __byte_encoded_array_size(const uint8_t *p, int elements)
|
||||
{
|
||||
(void)p;
|
||||
return sizeof(uint8_t) * elements;
|
||||
}
|
||||
|
||||
static inline int __byte_encode_array(void *_buf, int offset, int maxlen, const uint8_t *p, int elements)
|
||||
{
|
||||
if (maxlen < elements)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint8_t *buf = (uint8_t *) _buf;
|
||||
memcpy(&buf[offset], p, elements);
|
||||
return elements;
|
||||
}
|
||||
|
||||
static inline int __byte_decode_array(const void *_buf, int offset, int maxlen, uint8_t *p, int elements)
|
||||
{
|
||||
if (maxlen < elements)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
const uint8_t *buf = (const uint8_t *) _buf;
|
||||
memcpy(p, &buf[offset], elements);
|
||||
return elements;
|
||||
}
|
||||
|
||||
static inline int __byte_clone_array(const uint8_t *p, uint8_t *q, int elements)
|
||||
{
|
||||
memcpy(q, p, elements * sizeof(uint8_t));
|
||||
return 0;
|
||||
}
|
||||
/**
|
||||
* INT8_T
|
||||
*/
|
||||
#define __int8_t_hash_recursive(p) 0
|
||||
#define __int8_t_decode_array_cleanup(p, sz) {}
|
||||
#define int8_t_encoded_size(p) ( sizeof(int64_t) + sizeof(int8_t) )
|
||||
|
||||
static inline int __int8_t_encoded_array_size(const int8_t *p, int elements)
|
||||
{
|
||||
(void)p;
|
||||
return sizeof(int8_t) * elements;
|
||||
}
|
||||
|
||||
static inline int __int8_t_encode_array(void *_buf, int offset, int maxlen, const int8_t *p, int elements)
|
||||
{
|
||||
if (maxlen < elements)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
int8_t *buf = (int8_t *) _buf;
|
||||
memcpy(&buf[offset], p, elements);
|
||||
return elements;
|
||||
}
|
||||
|
||||
static inline int __int8_t_decode_array(const void *_buf, int offset, int maxlen, int8_t *p, int elements)
|
||||
{
|
||||
if (maxlen < elements)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
const int8_t *buf = (const int8_t *) _buf;
|
||||
memcpy(p, &buf[offset], elements);
|
||||
return elements;
|
||||
}
|
||||
|
||||
static inline int __int8_t_clone_array(const int8_t *p, int8_t *q, int elements)
|
||||
{
|
||||
memcpy(q, p, elements * sizeof(int8_t));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* INT16_T
|
||||
*/
|
||||
#define __int16_t_hash_recursive(p) 0
|
||||
#define __int16_t_decode_array_cleanup(p, sz) {}
|
||||
#define int16_t_encoded_size(p) ( sizeof(int64_t) + sizeof(int16_t) )
|
||||
|
||||
static inline int __int16_t_encoded_array_size(const int16_t *p, int elements)
|
||||
{
|
||||
(void)p;
|
||||
return sizeof(int16_t) * elements;
|
||||
}
|
||||
|
||||
static inline int __int16_t_encode_array(void *_buf, int offset, int maxlen, const int16_t *p, int elements)
|
||||
{
|
||||
int total_size = sizeof(int16_t) * elements;
|
||||
uint8_t *buf = (uint8_t *) _buf;
|
||||
int pos = offset;
|
||||
int element;
|
||||
|
||||
if (maxlen < total_size)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// See Section 5.8 paragraph 3 of the standard
|
||||
// http://open-std.org/JTC1/SC22/WG21/docs/papers/2015/n4527.pdf
|
||||
// use uint for shifting instead if int
|
||||
const uint16_t *unsigned_p = (const uint16_t *)p;
|
||||
|
||||
for (element = 0; element < elements; element++)
|
||||
{
|
||||
uint16_t v = unsigned_p[element];
|
||||
buf[pos++] = (v >> 8) & 0xff;
|
||||
buf[pos++] = (v & 0xff);
|
||||
}
|
||||
|
||||
return total_size;
|
||||
}
|
||||
|
||||
static inline int __int16_t_decode_array(const void *_buf, int offset, int maxlen, int16_t *p, int elements)
|
||||
{
|
||||
int total_size = sizeof(int16_t) * elements;
|
||||
const uint8_t *buf = (const uint8_t *) _buf;
|
||||
int pos = offset;
|
||||
int element;
|
||||
|
||||
if (maxlen < total_size)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
for (element = 0; element < elements; element++)
|
||||
{
|
||||
p[element] = (buf[pos] << 8) + buf[pos + 1];
|
||||
pos += 2;
|
||||
}
|
||||
|
||||
return total_size;
|
||||
}
|
||||
|
||||
static inline int __int16_t_clone_array(const int16_t *p, int16_t *q, int elements)
|
||||
{
|
||||
memcpy(q, p, elements * sizeof(int16_t));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* INT32_T
|
||||
*/
|
||||
#define __int32_t_hash_recursive(p) 0
|
||||
#define __int32_t_decode_array_cleanup(p, sz) {}
|
||||
#define int32_t_encoded_size(p) ( sizeof(int64_t) + sizeof(int32_t) )
|
||||
|
||||
static inline int __int32_t_encoded_array_size(const int32_t *p, int elements)
|
||||
{
|
||||
(void)p;
|
||||
return sizeof(int32_t) * elements;
|
||||
}
|
||||
|
||||
static inline int __int32_t_encode_array(void *_buf, int offset, int maxlen, const int32_t *p, int elements)
|
||||
{
|
||||
int total_size = sizeof(int32_t) * elements;
|
||||
uint8_t *buf = (uint8_t *) _buf;
|
||||
int pos = offset;
|
||||
int element;
|
||||
|
||||
if (maxlen < total_size)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// See Section 5.8 paragraph 3 of the standard
|
||||
// http://open-std.org/JTC1/SC22/WG21/docs/papers/2015/n4527.pdf
|
||||
// use uint for shifting instead if int
|
||||
const uint32_t *unsigned_p = (const uint32_t *)p;
|
||||
|
||||
for (element = 0; element < elements; element++)
|
||||
{
|
||||
const uint32_t v = unsigned_p[element];
|
||||
buf[pos++] = (v >> 24) & 0xff;
|
||||
buf[pos++] = (v >> 16) & 0xff;
|
||||
buf[pos++] = (v >> 8) & 0xff;
|
||||
buf[pos++] = (v & 0xff);
|
||||
}
|
||||
|
||||
return total_size;
|
||||
}
|
||||
|
||||
static inline int __int32_t_decode_array(const void *_buf, int offset, int maxlen, int32_t *p, int elements)
|
||||
{
|
||||
int total_size = sizeof(int32_t) * elements;
|
||||
const uint8_t *buf = (const uint8_t *) _buf;
|
||||
int pos = offset;
|
||||
int element;
|
||||
|
||||
if (maxlen < total_size)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// See Section 5.8 paragraph 3 of the standard
|
||||
// http://open-std.org/JTC1/SC22/WG21/docs/papers/2015/n4527.pdf
|
||||
// use uint for shifting instead if int
|
||||
for (element = 0; element < elements; element++)
|
||||
{
|
||||
p[element] = (((uint32_t)buf[pos + 0]) << 24) + (((uint32_t)buf[pos + 1]) << 16) + (((uint32_t)buf[pos + 2]) << 8) + ((uint32_t)buf[pos + 3]);
|
||||
pos += 4;
|
||||
}
|
||||
|
||||
return total_size;
|
||||
}
|
||||
|
||||
static inline int __int32_t_clone_array(const int32_t *p, int32_t *q, int elements)
|
||||
{
|
||||
memcpy(q, p, elements * sizeof(int32_t));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* INT64_T
|
||||
*/
|
||||
#define __int64_t_hash_recursive(p) 0
|
||||
#define __int64_t_decode_array_cleanup(p, sz) {}
|
||||
#define int64_t_encoded_size(p) ( sizeof(int64_t) + sizeof(int64_t) )
|
||||
|
||||
static inline int __int64_t_encoded_array_size(const int64_t *p, int elements)
|
||||
{
|
||||
(void)p;
|
||||
return sizeof(int64_t) * elements;
|
||||
}
|
||||
|
||||
static inline int __int64_t_encode_array(void *_buf, int offset, int maxlen, const int64_t *p, int elements)
|
||||
{
|
||||
int total_size = sizeof(int64_t) * elements;
|
||||
uint8_t *buf = (uint8_t *) _buf;
|
||||
int pos = offset;
|
||||
int element;
|
||||
|
||||
if (maxlen < total_size)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// See Section 5.8 paragraph 3 of the standard
|
||||
// http://open-std.org/JTC1/SC22/WG21/docs/papers/2015/n4527.pdf
|
||||
// use uint for shifting instead if int
|
||||
const uint64_t *unsigned_p = (const uint64_t *)p;
|
||||
|
||||
for (element = 0; element < elements; element++)
|
||||
{
|
||||
const uint64_t v = unsigned_p[element];
|
||||
buf[pos++] = (v >> 56) & 0xff;
|
||||
buf[pos++] = (v >> 48) & 0xff;
|
||||
buf[pos++] = (v >> 40) & 0xff;
|
||||
buf[pos++] = (v >> 32) & 0xff;
|
||||
buf[pos++] = (v >> 24) & 0xff;
|
||||
buf[pos++] = (v >> 16) & 0xff;
|
||||
buf[pos++] = (v >> 8) & 0xff;
|
||||
buf[pos++] = (v & 0xff);
|
||||
}
|
||||
|
||||
return total_size;
|
||||
}
|
||||
|
||||
static inline int __int64_t_decode_array(const void *_buf, int offset, int maxlen, int64_t *p, int elements)
|
||||
{
|
||||
int total_size = sizeof(int64_t) * elements;
|
||||
const uint8_t *buf = (const uint8_t *) _buf;
|
||||
int pos = offset;
|
||||
int element;
|
||||
|
||||
if (maxlen < total_size)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// See Section 5.8 paragraph 3 of the standard
|
||||
// http://open-std.org/JTC1/SC22/WG21/docs/papers/2015/n4527.pdf
|
||||
// use uint for shifting instead if int
|
||||
for (element = 0; element < elements; element++)
|
||||
{
|
||||
uint64_t a = (((uint32_t)buf[pos + 0]) << 24) + (((uint32_t)buf[pos + 1]) << 16) + (((uint32_t)buf[pos + 2]) << 8) + (uint32_t)buf[pos + 3];
|
||||
pos += 4;
|
||||
uint64_t b = (((uint32_t)buf[pos + 0]) << 24) + (((uint32_t)buf[pos + 1]) << 16) + (((uint32_t)buf[pos + 2]) << 8) + (uint32_t)buf[pos + 3];
|
||||
pos += 4;
|
||||
p[element] = (a << 32) + (b & 0xffffffff);
|
||||
}
|
||||
|
||||
return total_size;
|
||||
}
|
||||
|
||||
static inline int __int64_t_clone_array(const int64_t *p, int64_t *q, int elements)
|
||||
{
|
||||
memcpy(q, p, elements * sizeof(int64_t));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* FLOAT
|
||||
*/
|
||||
#define __float_hash_recursive(p) 0
|
||||
#define __float_decode_array_cleanup(p, sz) {}
|
||||
#define float_encoded_size(p) ( sizeof(int64_t) + sizeof(float) )
|
||||
|
||||
static inline int __float_encoded_array_size(const float *p, int elements)
|
||||
{
|
||||
(void)p;
|
||||
return sizeof(float) * elements;
|
||||
}
|
||||
|
||||
static inline int __float_encode_array(void *_buf, int offset, int maxlen, const float *p, int elements)
|
||||
{
|
||||
return __int32_t_encode_array(_buf, offset, maxlen, (const int32_t *) p, elements);
|
||||
}
|
||||
|
||||
static inline int __float_decode_array(const void *_buf, int offset, int maxlen, float *p, int elements)
|
||||
{
|
||||
return __int32_t_decode_array(_buf, offset, maxlen, (int32_t *) p, elements);
|
||||
}
|
||||
|
||||
static inline int __float_clone_array(const float *p, float *q, int elements)
|
||||
{
|
||||
memcpy(q, p, elements * sizeof(float));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* DOUBLE
|
||||
*/
|
||||
#define __double_hash_recursive(p) 0
|
||||
#define __double_decode_array_cleanup(p, sz) {}
|
||||
#define double_encoded_size(p) ( sizeof(int64_t) + sizeof(double) )
|
||||
|
||||
static inline int __double_encoded_array_size(const double *p, int elements)
|
||||
{
|
||||
(void)p;
|
||||
return sizeof(double) * elements;
|
||||
}
|
||||
|
||||
static inline int __double_encode_array(void *_buf, int offset, int maxlen, const double *p, int elements)
|
||||
{
|
||||
return __int64_t_encode_array(_buf, offset, maxlen, (const int64_t *) p, elements);
|
||||
}
|
||||
|
||||
static inline int __double_decode_array(const void *_buf, int offset, int maxlen, double *p, int elements)
|
||||
{
|
||||
return __int64_t_decode_array(_buf, offset, maxlen, (int64_t *) p, elements);
|
||||
}
|
||||
|
||||
static inline int __double_clone_array(const double *p, double *q, int elements)
|
||||
{
|
||||
memcpy(q, p, elements * sizeof(double));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* STRING
|
||||
*/
|
||||
#define __string_hash_recursive(p) 0
|
||||
|
||||
static inline int __string_decode_array_cleanup(char **s, int elements)
|
||||
{
|
||||
int element;
|
||||
|
||||
for (element = 0; element < elements; element++)
|
||||
{
|
||||
free(s[element]);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline int __string_encoded_array_size(char *const *s, int elements)
|
||||
{
|
||||
int size = 0;
|
||||
int element;
|
||||
|
||||
for (element = 0; element < elements; element++)
|
||||
{
|
||||
size += 4 + (int)strlen(s[element]) + 1;
|
||||
}
|
||||
|
||||
return size;
|
||||
}
|
||||
|
||||
static inline int __string_encoded_size(char *const *s)
|
||||
{
|
||||
return sizeof(int64_t) + __string_encoded_array_size(s, 1);
|
||||
}
|
||||
|
||||
static inline int __string_encode_array(void *_buf, int offset, int maxlen, char *const *p, int elements)
|
||||
{
|
||||
int pos = 0, thislen;
|
||||
int element;
|
||||
|
||||
for (element = 0; element < elements; element++)
|
||||
{
|
||||
int32_t length = (int32_t)strlen(p[element]) + 1; // length includes \0
|
||||
thislen = __int32_t_encode_array(_buf, offset + pos, maxlen - pos, &length, 1);
|
||||
|
||||
if (thislen < 0)
|
||||
{
|
||||
return thislen;
|
||||
}
|
||||
else
|
||||
{
|
||||
pos += thislen;
|
||||
}
|
||||
|
||||
thislen = __int8_t_encode_array(_buf, offset + pos, maxlen - pos, (int8_t *) p[element], length);
|
||||
|
||||
if (thislen < 0)
|
||||
{
|
||||
return thislen;
|
||||
}
|
||||
else
|
||||
{
|
||||
pos += thislen;
|
||||
}
|
||||
}
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
static inline int __string_decode_array(const void *_buf, int offset, int maxlen, char **p, int elements)
|
||||
{
|
||||
int pos = 0, thislen;
|
||||
int element;
|
||||
|
||||
for (element = 0; element < elements; element++)
|
||||
{
|
||||
int32_t length;
|
||||
// read length including \0
|
||||
thislen = __int32_t_decode_array(_buf, offset + pos, maxlen - pos, &length, 1);
|
||||
|
||||
if (thislen < 0)
|
||||
{
|
||||
return thislen;
|
||||
}
|
||||
else
|
||||
{
|
||||
pos += thislen;
|
||||
}
|
||||
|
||||
p[element] = (char *) malloc(length);
|
||||
thislen = __int8_t_decode_array(_buf, offset + pos, maxlen - pos, (int8_t *) p[element], length);
|
||||
|
||||
if (thislen < 0)
|
||||
{
|
||||
return thislen;
|
||||
}
|
||||
else
|
||||
{
|
||||
pos += thislen;
|
||||
}
|
||||
}
|
||||
|
||||
return pos;
|
||||
}
|
||||
|
||||
static inline int __string_clone_array(char *const *p, char **q, int elements)
|
||||
{
|
||||
int element;
|
||||
|
||||
for (element = 0; element < elements; element++)
|
||||
{
|
||||
// because strdup is not C99
|
||||
size_t len = strlen(p[element]) + 1;
|
||||
q[element] = (char *) malloc(len);
|
||||
memcpy(q[element], p[element], len);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline void *lcm_malloc(size_t sz)
|
||||
{
|
||||
if (sz)
|
||||
{
|
||||
return malloc(sz);
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/**
|
||||
* Describes the type of a single field in an LCM message.
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
LCM_FIELD_INT8_T,
|
||||
LCM_FIELD_INT16_T,
|
||||
LCM_FIELD_INT32_T,
|
||||
LCM_FIELD_INT64_T,
|
||||
LCM_FIELD_BYTE,
|
||||
LCM_FIELD_FLOAT,
|
||||
LCM_FIELD_DOUBLE,
|
||||
LCM_FIELD_STRING,
|
||||
LCM_FIELD_BOOLEAN,
|
||||
LCM_FIELD_USER_TYPE
|
||||
} lcm_field_type_t;
|
||||
|
||||
#define LCM_TYPE_FIELD_MAX_DIM 50
|
||||
|
||||
/**
|
||||
* Describes a single lcmtype field's datatype and array dimmensions
|
||||
*/
|
||||
typedef struct _lcm_field_t lcm_field_t;
|
||||
struct _lcm_field_t
|
||||
{
|
||||
/**
|
||||
* name of the field
|
||||
*/
|
||||
const char *name;
|
||||
|
||||
/**
|
||||
* datatype of the field
|
||||
**/
|
||||
lcm_field_type_t type;
|
||||
|
||||
/**
|
||||
* datatype of the field (in string format)
|
||||
* this should be the same as in the lcm type decription file
|
||||
*/
|
||||
const char *typestr;
|
||||
|
||||
/**
|
||||
* number of array dimensions
|
||||
* if the field is scalar, num_dim should equal 0
|
||||
*/
|
||||
int num_dim;
|
||||
|
||||
/**
|
||||
* the size of each dimension. Valid on [0:num_dim-1].
|
||||
*/
|
||||
int32_t dim_size[LCM_TYPE_FIELD_MAX_DIM];
|
||||
|
||||
/**
|
||||
* a boolean describing whether the dimension is
|
||||
* variable. Valid on [0:num_dim-1].
|
||||
*/
|
||||
int8_t dim_is_variable[LCM_TYPE_FIELD_MAX_DIM];
|
||||
|
||||
/**
|
||||
* a data pointer to the start of this field
|
||||
*/
|
||||
void *data;
|
||||
};
|
||||
|
||||
|
||||
typedef int (*lcm_encode_t)(void *buf, int offset, int maxlen, const void *p);
|
||||
typedef int (*lcm_decode_t)(const void *buf, int offset, int maxlen, void *p);
|
||||
typedef int (*lcm_decode_cleanup_t)(void *p);
|
||||
typedef int (*lcm_encoded_size_t)(const void *p);
|
||||
typedef int (*lcm_struct_size_t)(void);
|
||||
typedef int (*lcm_num_fields_t)(void);
|
||||
typedef int (*lcm_get_field_t)(const void *p, int i, lcm_field_t *f);
|
||||
typedef int64_t (*lcm_get_hash_t)(void);
|
||||
|
||||
/**
|
||||
* Describes an lcmtype info, enabling introspection
|
||||
*/
|
||||
typedef struct _lcm_type_info_t lcm_type_info_t;
|
||||
struct _lcm_type_info_t
|
||||
{
|
||||
lcm_encode_t encode;
|
||||
lcm_decode_t decode;
|
||||
lcm_decode_cleanup_t decode_cleanup;
|
||||
lcm_encoded_size_t encoded_size;
|
||||
lcm_struct_size_t struct_size;
|
||||
lcm_num_fields_t num_fields;
|
||||
lcm_get_field_t get_field;
|
||||
lcm_get_hash_t get_hash;
|
||||
|
||||
};
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,128 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/** LCM Lite is a minimalist implementation of LCM designed to
|
||||
* minimize external dependencies and memory usage, making it the
|
||||
* easiest way to port LCM to a new platform, particularly
|
||||
* resource-limited embedded platforms.
|
||||
*
|
||||
* There are two main differences between LCM-Lite and the standard
|
||||
* LCM:
|
||||
*
|
||||
* 1) The actual communications code must be provided by the
|
||||
* user. (However, see lcmlite_posix for an example of how to provide
|
||||
* this functionality for most POSIX systems). In short, you call an
|
||||
* lcmlite function whenever an LCM UDP packet arrives, and you
|
||||
* provide a function pointer that will transmit packets (which
|
||||
* LCMLite will call as necessary).
|
||||
*
|
||||
* 2) LCMLite itself does no runtime memory allocation; all memory
|
||||
* structures are allocated either at compile time or by the
|
||||
* caller. This means that the system does not need an implementation
|
||||
* of malloc(), and that issues of memory fragmentation are avoided.
|
||||
*
|
||||
* Most of the memory is allocated with the lcmlite_t object. This can
|
||||
* be too large for many stacks, so it is recommended to declare this
|
||||
* as a static global variable. The memory footprint is determined by
|
||||
* the defines below.
|
||||
**/
|
||||
|
||||
// Disable long packet reception by setting NUM BUFFERS to zero.
|
||||
// Total memory allocated is roughly:
|
||||
//
|
||||
// NUM_BUFFERS*(MAX_PACKET_SIZE + MAX_FRAGMENTS + CHANNEL_LENGTH) + PUBLISH_BUFFER_SIZE
|
||||
//
|
||||
// Note that for full LCM compatibility, CHANNEL_LENGTH must be 256.
|
||||
//
|
||||
#define LCM3_NUM_BUFFERS 4
|
||||
#define LCM3_MAX_PACKET_SIZE (300000)
|
||||
#define LCM3_MAX_FRAGMENTS 256
|
||||
|
||||
#define LCM_MAX_CHANNEL_LENGTH 256
|
||||
|
||||
// LCMLite will allocate a single buffer of the size below for
|
||||
// publishing messages. The LCM3 fragmentation option will be used to
|
||||
// send messages larger than this.
|
||||
#define LCM_PUBLISH_BUFFER_SIZE 8192
|
||||
|
||||
typedef struct lcmlite_subscription lcmlite_subscription_t;
|
||||
typedef struct lcmlite lcmlite_t;
|
||||
|
||||
struct fragment_buffer
|
||||
{
|
||||
int32_t last_fragment_count;
|
||||
|
||||
uint64_t from_addr;
|
||||
uint32_t msg_seq;
|
||||
|
||||
char channel[LCM_MAX_CHANNEL_LENGTH];
|
||||
int fragments_remaining;
|
||||
char buf[LCM3_MAX_PACKET_SIZE];
|
||||
uint8_t frag_received[LCM3_MAX_FRAGMENTS];
|
||||
};
|
||||
|
||||
struct lcmlite_subscription
|
||||
{
|
||||
const char *channel;
|
||||
|
||||
void (*callback)(lcmlite_t *lcm, const char *channel, const void *buf, int buf_len, void *user);
|
||||
void *user;
|
||||
|
||||
// 'next' field is for lcmlite internal use.
|
||||
lcmlite_subscription_t *next;
|
||||
// 'prev' item
|
||||
lcmlite_subscription_t *prev;
|
||||
};
|
||||
|
||||
struct lcmlite
|
||||
{
|
||||
/** Buffers for reassembling multi-fragment messages. **/
|
||||
struct fragment_buffer fragment_buffers[LCM3_NUM_BUFFERS];
|
||||
|
||||
/** every time we receive a fragment, we increment this counter
|
||||
and write the value to the corresponding fragment buffer. This
|
||||
allows us to measure how "stale" a fragment is (how long it's
|
||||
been since we received a fragment for it).
|
||||
**/
|
||||
int32_t last_fragment_count;
|
||||
|
||||
void (*transmit_packet)(const void *_buf, int buf_len, void *user);
|
||||
void *transmit_user;
|
||||
|
||||
uint8_t publish_buffer[LCM_PUBLISH_BUFFER_SIZE];
|
||||
uint32_t msg_seq;
|
||||
|
||||
lcmlite_subscription_t *first_subscription;
|
||||
};
|
||||
|
||||
#if defined(__cplusplus)
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// Caller allocates the lcmlite_t object, which we initialize.
|
||||
int lcmlite_init(lcmlite_t *lcm, void (*transmit_packet)(const void *_buf, int buf_len, void *user), void *transmit_user);
|
||||
|
||||
// The user is responsible for creating and listening on a UDP
|
||||
// multicast socket. When a packet is received, call this function. Do
|
||||
// not call this function from more than one thread at a time. Returns
|
||||
// zero if the packet was successfully handled by LCM, however no
|
||||
// special action is required by the caller when an error occurs.
|
||||
int lcmlite_receive_packet(lcmlite_t *lcm, const void *_buf, int buf_len, uint64_t from_addr);
|
||||
|
||||
// Publish a message. Will call transmit_packet function one or more
|
||||
// times synchronously. Returns 0 on success. This function should not
|
||||
// be called concurrently with itself, but can be called concurrently
|
||||
// with _receive_packet.
|
||||
int lcmlite_publish(lcmlite_t *lcm, const char *channel, const void *_buf, int buf_len);
|
||||
|
||||
// not thread safe WRT lcmlite_receive_packet. Caller allocates
|
||||
// subscription record and initializes its fields. Note that the
|
||||
// "channel" field does not support regular expressions, but ending
|
||||
// with ".*" is supported as a special case.
|
||||
void lcmlite_subscribe(lcmlite_t *lcm, lcmlite_subscription_t *sub);
|
||||
void lcmlite_unsubscribe(lcmlite_t *lcm, lcmlite_subscription_t *sub);
|
||||
|
||||
#if defined(__cplusplus)
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
#pragma once
|
||||
|
||||
#include <lcm/lcm.h>
|
||||
|
||||
#if defined(OS_WINDOWS)
|
||||
#include <Ws2ipdef.h>
|
||||
#include <winsock2.h>
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
#include <arpa/inet.h>
|
||||
#include <netinet/in.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
typedef struct _lcm_udp_connect_info_t {
|
||||
lcm_connect_info_t conn;
|
||||
#if defined(OS_WINDOWS)
|
||||
SOCKET send_s;
|
||||
SOCKET recv_s;
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
int send_s;
|
||||
int recv_s;
|
||||
#endif /* OS_WINDOWS */
|
||||
struct sockaddr_in send_addr;
|
||||
struct sockaddr_in recv_addr;
|
||||
} lcm_udp_connect_info_t;
|
||||
|
||||
void lcm_udpm_regist();
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
typedef struct _url_param_t
|
||||
{
|
||||
char *name;
|
||||
char *value;
|
||||
struct _url_param_t *p_next;
|
||||
} url_param_t;
|
||||
|
||||
typedef struct _url_parse_result_t
|
||||
{
|
||||
char *protocol_name;
|
||||
char *address;
|
||||
url_param_t *params;
|
||||
} url_parse_result_t;
|
||||
|
||||
url_parse_result_t *url_parse(const char *url);
|
||||
void free_url_parse_result(url_parse_result_t *);
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* For communication from `getopt' to the caller.
|
||||
When `getopt' finds an option that takes an argument,
|
||||
the argument value is returned here.
|
||||
Also, when `ordering' is RETURN_IN_ORDER,
|
||||
each non-option ARGV-element is returned here.*/
|
||||
|
||||
extern char *optarg;
|
||||
|
||||
/* Index in ARGV of the next element to be scanned.
|
||||
This is used for communication to and from the caller
|
||||
and for communication between successive calls to `getopt'.
|
||||
|
||||
On entry to `getopt', zero means this is the first call; initialize.
|
||||
|
||||
When `getopt' returns -1, this is the index of the first of the
|
||||
non-option elements that the caller should itself scan.
|
||||
|
||||
Otherwise, `optind' communicates from one call to the next
|
||||
how much of ARGV has been scanned so far.*/
|
||||
|
||||
extern int optind;
|
||||
|
||||
/* Callers store zero here to inhibit the error message `getopt' prints
|
||||
for unrecognized options.*/
|
||||
|
||||
extern int opterr;
|
||||
|
||||
/* Set to an option character which was unrecognized.*/
|
||||
|
||||
extern int optopt;
|
||||
|
||||
/* Describe the long-named options requested by the application.
|
||||
The LONG_OPTIONS argument to getopt_long or getopt_long_only is a vector
|
||||
of `struct option' terminated by an element containing a name which is
|
||||
zero.
|
||||
|
||||
The field `has_arg' is:
|
||||
no_argument(or 0) if the option does not take an argument,
|
||||
required_argument(or 1) if the option requires an argument,
|
||||
optional_argument(or 2) if the option takes an optional argument.
|
||||
|
||||
If the field `flag' is not NULL, it points to a variable that is set
|
||||
to the value given in the field `val' when the option is found, but
|
||||
left unchanged if the option is not found.
|
||||
|
||||
To have a long-named option do something other than set an `int' to
|
||||
a compiled-in constant, such as set a value from `optarg', set the
|
||||
option's `flag' field to zero and its `val' field to a nonzero
|
||||
value (the equivalent single-letter option character, if there is
|
||||
one).For long options that have a zero `flag' field, `getopt'
|
||||
returns the contents of the `val' field.*/
|
||||
|
||||
struct option
|
||||
{
|
||||
#if defined (__STDC__) && __STDC__
|
||||
const char *name;
|
||||
#else
|
||||
char *name;
|
||||
#endif
|
||||
/* has_arg can't be an enum because some compilers complain about
|
||||
type mismatches in all the code that assumes it is an int.*/
|
||||
int has_arg;
|
||||
int *flag;
|
||||
int val;
|
||||
};
|
||||
|
||||
/* Names for the values of the `has_arg' field of `struct option'.*/
|
||||
|
||||
#define no_argument 0
|
||||
#define required_argument 1
|
||||
#define optional_argument 2
|
||||
|
||||
extern int getopt_long(int argc, char *const *argv, const char *shortopts,
|
||||
const struct option *longopts, int *longind);
|
||||
extern int getopt_long_only(int argc, char *const *argv,
|
||||
const char *shortopts,
|
||||
const struct option *longopts, int *longind);
|
||||
|
||||
/* Internal only.Users should not call this directly.*/
|
||||
extern int _getopt_internal(int argc, char *const *argv,
|
||||
const char *shortopts,
|
||||
const struct option *longopts, int *longind,
|
||||
int long_only);
|
||||
extern int
|
||||
getopt(int argc,
|
||||
char *const *argv,
|
||||
const char *optstring);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
#pragma once
|
||||
|
||||
#if defined(OS_WINDOWS)
|
||||
#include <lcm/winporting/getopt.h>
|
||||
#define strdup _strdup
|
||||
#define mode_t int
|
||||
#define PATH_MAX MAX_PATH
|
||||
#define fseeko _fseeki64
|
||||
#define ftello _ftelli64
|
||||
//#define socklen_t int
|
||||
#define in_addr_t in_addr
|
||||
#define SHUT_RDWR SD_BOTH
|
||||
#define HUGE HUGE_VAL
|
||||
#define O_NONBLOCK 0x4000
|
||||
#define F_GETFL 3
|
||||
#define F_SETFL 4
|
||||
|
||||
#if _MSC_VER < 1800
|
||||
#define strtoll _strtoi64
|
||||
#endif
|
||||
|
||||
#if _MSC_VER < 1900
|
||||
#define snprintf _snprintf
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,298 @@
|
|||
#include <stdio.h>
|
||||
#include <sys/types.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#if defined(OS_WINDOWS)
|
||||
#define __STDC_FORMAT_MACROS // Enable integer types
|
||||
#endif
|
||||
#include <stdint.h>
|
||||
|
||||
#include <lcm/ioutils.h>
|
||||
#include <lcm/eventlog.h>
|
||||
#include <lcm/winporting/winporting.h>
|
||||
|
||||
#define MAGIC ((int32_t) 0xEDA1DA01L)
|
||||
|
||||
lcm_eventlog_t *lcm_eventlog_create(const char *path, const char *mode)
|
||||
{
|
||||
assert(!strcmp(mode, "r") || !strcmp(mode, "w") || !strcmp(mode, "a"));
|
||||
|
||||
if (*mode == 'w')
|
||||
{
|
||||
mode = "wb";
|
||||
}
|
||||
else if (*mode == 'r')
|
||||
{
|
||||
mode = "rb";
|
||||
}
|
||||
else if (*mode == 'a')
|
||||
{
|
||||
mode = "ab";
|
||||
}
|
||||
else
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
lcm_eventlog_t *l = (lcm_eventlog_t *) calloc(1, sizeof(lcm_eventlog_t));
|
||||
l->f = fopen(path, mode);
|
||||
|
||||
if (l->f == NULL)
|
||||
{
|
||||
free(l);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
l->eventcount = 0;
|
||||
return l;
|
||||
}
|
||||
|
||||
void lcm_eventlog_destroy(lcm_eventlog_t *l)
|
||||
{
|
||||
fflush(l->f);
|
||||
fclose(l->f);
|
||||
free(l);
|
||||
}
|
||||
|
||||
lcm_eventlog_event_t *lcm_eventlog_read_next_event(lcm_eventlog_t *l)
|
||||
{
|
||||
lcm_eventlog_event_t *le =
|
||||
(lcm_eventlog_event_t *) calloc(1, sizeof(lcm_eventlog_event_t));
|
||||
int32_t magic = 0;
|
||||
int r;
|
||||
|
||||
do
|
||||
{
|
||||
r = fgetc(l->f);
|
||||
|
||||
if (r < 0)
|
||||
{
|
||||
free(le);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
magic = (magic << 8) | r;
|
||||
} while (magic != MAGIC);
|
||||
|
||||
if (0 != fread64(l->f, &le->eventnum) ||
|
||||
0 != fread64(l->f, &le->timestamp) ||
|
||||
0 != fread32(l->f, &le->channellen) ||
|
||||
0 != fread32(l->f, &le->datalen))
|
||||
{
|
||||
free(le);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Sanity check the channel length and data length
|
||||
if (le->channellen <= 0 || le->channellen >= 1000)
|
||||
{
|
||||
fprintf(stderr, "Log event has invalid channel length: %d\n", le->channellen);
|
||||
free(le);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (le->datalen < 0)
|
||||
{
|
||||
fprintf(stderr, "Log event has invalid data length: %d\n", le->datalen);
|
||||
free(le);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
le->channel = (char *) calloc(1, le->channellen + 1);
|
||||
|
||||
if (fread(le->channel, 1, le->channellen, l->f) != (size_t) le->channellen)
|
||||
{
|
||||
free(le->channel);
|
||||
free(le->data);
|
||||
free(le);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
le->data = calloc(1, le->datalen + 1);
|
||||
|
||||
if (fread(le->data, 1, le->datalen, l->f) != (size_t) le->datalen)
|
||||
{
|
||||
free(le->channel);
|
||||
free(le->data);
|
||||
free(le);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Check that there's a valid event or the EOF after this event.
|
||||
int32_t next_magic;
|
||||
|
||||
if (0 == fread32(l->f, &next_magic))
|
||||
{
|
||||
if (next_magic != MAGIC)
|
||||
{
|
||||
fprintf(stderr, "Invalid header after log data\n");
|
||||
free(le->channel);
|
||||
free(le->data);
|
||||
free(le);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
fseeko(l->f, -4, SEEK_CUR);
|
||||
}
|
||||
|
||||
return le;
|
||||
}
|
||||
|
||||
int lcm_eventlog_write_event(lcm_eventlog_t *l, lcm_eventlog_event_t *le)
|
||||
{
|
||||
if (0 != fwrite32(l->f, MAGIC))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
le->eventnum = l->eventcount;
|
||||
|
||||
if (0 != fwrite64(l->f, le->eventnum))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (0 != fwrite64(l->f, le->timestamp))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (0 != fwrite32(l->f, le->channellen))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (0 != fwrite32(l->f, le->datalen))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (le->channellen != fwrite(le->channel, 1, le->channellen, l->f))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (le->datalen != fwrite(le->data, 1, le->datalen, l->f))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
l->eventcount++;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void lcm_eventlog_free_event(lcm_eventlog_event_t *le)
|
||||
{
|
||||
if (le->data)
|
||||
{
|
||||
free(le->data);
|
||||
}
|
||||
|
||||
if (le->channel)
|
||||
{
|
||||
free(le->channel);
|
||||
}
|
||||
|
||||
memset(le, 0, sizeof(lcm_eventlog_event_t));
|
||||
free(le);
|
||||
}
|
||||
|
||||
|
||||
|
||||
static int64_t get_event_time(lcm_eventlog_t *l)
|
||||
{
|
||||
int32_t magic = 0;
|
||||
int r;
|
||||
|
||||
do
|
||||
{
|
||||
r = fgetc(l->f);
|
||||
|
||||
if (r < 0)
|
||||
{
|
||||
goto eof;
|
||||
}
|
||||
|
||||
magic = (magic << 8) | r;
|
||||
} while (magic != MAGIC);
|
||||
|
||||
int64_t event_num;
|
||||
int64_t timestamp;
|
||||
|
||||
if (0 != fread64(l->f, &event_num))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (0 != fread64(l->f, ×tamp))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
fseeko(l->f, -20, SEEK_CUR);
|
||||
l->eventcount = event_num;
|
||||
return timestamp;
|
||||
eof:
|
||||
return -1;
|
||||
}
|
||||
|
||||
int lcm_eventlog_seek_to_timestamp(lcm_eventlog_t *l, int64_t timestamp)
|
||||
{
|
||||
fseeko(l->f, 0, SEEK_END);
|
||||
off_t file_len = (off_t)ftello(l->f);
|
||||
int64_t cur_time;
|
||||
double frac1 = 0; // left bracket
|
||||
double frac2 = 1; // right bracket
|
||||
double prev_frac = -1;
|
||||
double frac; // current position
|
||||
|
||||
while (1)
|
||||
{
|
||||
frac = 0.5 * (frac1 + frac2);
|
||||
off_t offset = (off_t)(frac * file_len);
|
||||
fseeko(l->f, offset, SEEK_SET);
|
||||
cur_time = get_event_time(l);
|
||||
|
||||
if (cur_time < 0)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((frac > frac2) || (frac < frac1) || (frac1 >= frac2))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
double df = frac - prev_frac;
|
||||
|
||||
if (df < 0)
|
||||
{
|
||||
df = -df;
|
||||
}
|
||||
|
||||
if (df < 1e-12)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if (cur_time == timestamp)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if (cur_time < timestamp)
|
||||
{
|
||||
frac1 = frac;
|
||||
}
|
||||
else
|
||||
{
|
||||
frac2 = frac;
|
||||
}
|
||||
|
||||
prev_frac = frac;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,310 @@
|
|||
#include <lcm/lcm.h>
|
||||
#include <lcm/udpm.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#if defined(OS_WINDOWS)
|
||||
#include <winbase.h>
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
#define LCM_DEFAULT_URL "udpm://239.255.76.67:7667?ttl=0"
|
||||
|
||||
// Protocol regist list
|
||||
// All regist function will be called when the first lcm object is created
|
||||
static lcm_protocol_reg_t lcm_protocols_reg_table[] = {lcm_udpm_regist, NULL};
|
||||
|
||||
// Initialize flag
|
||||
static volatile bool g_initialized = false;
|
||||
#if defined(OS_WINDOWS)
|
||||
static volatile LONG g_init_flag_lock = 0;
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
static pthread_rwlock_t g_init_flag_lock = PTHREAD_RWLOCK_INITIALIZER;
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
// Protocol list
|
||||
static lcm_protocol_info_t *g_protocol_infos = NULL;
|
||||
|
||||
// Protocol list lock
|
||||
#if defined(OS_WINDOWS)
|
||||
static SRWLOCK g_protocol_list_lock;
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
static pthread_rwlock_t g_protocol_list_lock = PTHREAD_RWLOCK_INITIALIZER;
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
void lcm_init() {
|
||||
// Check flag
|
||||
#if defined(OS_WINDOWS)
|
||||
while (InterlockedExchange(&g_init_flag_lock, 1) != 0) {
|
||||
Sleep(20);
|
||||
}
|
||||
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_rwlock_wrlock(&g_init_flag_lock);
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
if (g_initialized) {
|
||||
#if defined(OS_WINDOWS)
|
||||
g_init_flag_lock = 0;
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_rwlock_unlock(&g_init_flag_lock);
|
||||
#endif /* OS_WINDOWS */
|
||||
return;
|
||||
}
|
||||
|
||||
#if defined(OS_WINDOWS)
|
||||
InitializeSRWLock(&g_protocol_list_lock);
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
// Regist protocols
|
||||
for (lcm_protocol_reg_t *p = lcm_protocols_reg_table; *p != NULL; p++) {
|
||||
(*p)();
|
||||
}
|
||||
|
||||
g_initialized = true;
|
||||
#if defined(OS_WINDOWS)
|
||||
g_init_flag_lock = 0;
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_rwlock_unlock(&g_init_flag_lock);
|
||||
#endif /* OS_WINDOWS */
|
||||
}
|
||||
|
||||
void lcm_regist_protocol(lcm_protocol_info_t *p_protocol_info) {
|
||||
#if defined(OS_WINDOWS)
|
||||
AcquireSRWLockExclusive(&g_protocol_list_lock);
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_rwlock_wrlock(&g_protocol_list_lock);
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
// Insert item
|
||||
if (g_protocol_infos == NULL) {
|
||||
g_protocol_infos = p_protocol_info;
|
||||
} else {
|
||||
lcm_protocol_info_t *p_info = g_protocol_infos;
|
||||
|
||||
while (p_info->next != NULL) {
|
||||
p_info = p_info->next;
|
||||
}
|
||||
|
||||
p_info->next = p_protocol_info;
|
||||
}
|
||||
|
||||
p_protocol_info->next = NULL;
|
||||
#if defined(OS_WINDOWS)
|
||||
ReleaseSRWLockExclusive(&g_protocol_list_lock);
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_rwlock_unlock(&g_protocol_list_lock);
|
||||
#endif /* OS_WINDOWS */
|
||||
return;
|
||||
}
|
||||
|
||||
lcm_t *lcm_create(const char *url) {
|
||||
if (!g_initialized) {
|
||||
lcm_init();
|
||||
}
|
||||
|
||||
if (url == NULL || url[0] == '\0') {
|
||||
url = LCM_DEFAULT_URL;
|
||||
}
|
||||
|
||||
// Allocate lcm object
|
||||
lcm_t *ret;
|
||||
ret = (lcm_t *)malloc(sizeof(lcm_t));
|
||||
|
||||
if (ret == NULL) {
|
||||
fprintf(stderr, "Failed to allocate memory for lcm_t, %d\n", __LINE__);
|
||||
goto EXCEPT_ALLOC_LCM;
|
||||
}
|
||||
|
||||
// Match protocol
|
||||
#if defined(OS_WINDOWS)
|
||||
AcquireSRWLockShared(&g_protocol_list_lock);
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_rwlock_rdlock(&g_protocol_list_lock);
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
if (g_protocol_infos == NULL) {
|
||||
fprintf(stderr, "Failed to lock the rdlock %d\n", __LINE__);
|
||||
goto EXCEPT_EMPTY_PROTOCOL;
|
||||
}
|
||||
|
||||
lcm_protocol_info_t *p_info = g_protocol_infos;
|
||||
url_parse_result_t *p_url_parse_result = url_parse(url);
|
||||
|
||||
if (p_url_parse_result == NULL) {
|
||||
fprintf(stderr, "Failed to parse the url %d\n", __LINE__);
|
||||
goto EXCEPT_URL_PARSE;
|
||||
}
|
||||
|
||||
while ((ret->info = p_info->match(p_url_parse_result)) == NULL) {
|
||||
p_info = p_info->next;
|
||||
|
||||
if (p_info == NULL) {
|
||||
fprintf(stderr, "Failed to match the url result %d\n", __LINE__);
|
||||
goto EXCEPT_NO_MATCH;
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize lcm
|
||||
#if defined(OS_WINDOWS)
|
||||
InitializeCriticalSection(&(ret->lock));
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_mutex_init(&(ret->lock), NULL);
|
||||
#endif
|
||||
|
||||
if (lcmlite_init(
|
||||
&(ret->lcm),
|
||||
(void (*)(const void *, int, void *))(ret->info->send_callback),
|
||||
ret->info) != 0) {
|
||||
fprintf(stderr, "Failed to lcmlite_init %d\n", __LINE__);
|
||||
goto EXCEPT_INIT_FAILED;
|
||||
}
|
||||
|
||||
free_url_parse_result(p_url_parse_result);
|
||||
#if defined(OS_WINDOWS)
|
||||
ReleaseSRWLockShared(&g_protocol_list_lock);
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_rwlock_unlock(&g_protocol_list_lock);
|
||||
#endif
|
||||
return ret;
|
||||
// Exceptions
|
||||
EXCEPT_INIT_FAILED:
|
||||
ret->info->destroy(ret->info);
|
||||
EXCEPT_NO_MATCH:
|
||||
free_url_parse_result(p_url_parse_result);
|
||||
EXCEPT_URL_PARSE:
|
||||
EXCEPT_EMPTY_PROTOCOL:
|
||||
#if defined(OS_WINDOWS)
|
||||
ReleaseSRWLockShared(&g_protocol_list_lock);
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_rwlock_unlock(&g_protocol_list_lock);
|
||||
#endif /* OS_WINDOWS */
|
||||
free(ret);
|
||||
EXCEPT_ALLOC_LCM:
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void lcm_destroy(lcm_t *lcm) {
|
||||
lcm->info->destroy(lcm->info);
|
||||
|
||||
while (lcm->lcm.first_subscription != NULL) {
|
||||
lcm_unsubscribe(lcm, (lcm_subscription_t *)(lcm->lcm.first_subscription));
|
||||
}
|
||||
|
||||
free(lcm);
|
||||
return;
|
||||
}
|
||||
|
||||
int lcm_publish(lcm_t *lcm, const char *channel, const void *data,
|
||||
unsigned int datalen) {
|
||||
return lcmlite_publish(&(lcm->lcm), channel, data, datalen);
|
||||
}
|
||||
|
||||
void lcmlite_subscription_callback(lcmlite_t *lcmlite, const char *channel,
|
||||
const void *buf, int buf_len, void *user) {
|
||||
// Get timestamp
|
||||
int64_t timestamp = lcm_get_timestamp();
|
||||
// Make buffer
|
||||
lcm_t *lcm = (lcm_t *)lcmlite;
|
||||
lcm_recv_buf_t buffer;
|
||||
buffer.data = (void *)buf;
|
||||
buffer.data_size = buf_len;
|
||||
buffer.lcm = lcm;
|
||||
buffer.recv_utime = timestamp;
|
||||
lcm_subscription_t *p_subscription = (lcm_subscription_t *)user;
|
||||
p_subscription->callback(&buffer, channel, p_subscription->userdata);
|
||||
return;
|
||||
}
|
||||
|
||||
lcm_subscription_t *lcm_subscribe(lcm_t *lcm, const char *channel,
|
||||
lcm_msg_handler_t handler, void *userdata) {
|
||||
// Allocate memory
|
||||
lcm_subscription_t *ret = malloc(sizeof(lcm_subscription_t));
|
||||
|
||||
if (ret == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Fill members
|
||||
ret->callback = handler;
|
||||
ret->userdata = userdata;
|
||||
ret->light_subscription.channel = (const char *)malloc(strlen(channel) + 1);
|
||||
strcpy((char *)(ret->light_subscription.channel), channel);
|
||||
ret->light_subscription.user = ret;
|
||||
ret->light_subscription.callback = lcmlite_subscription_callback;
|
||||
// Append to list
|
||||
#if defined(OS_WINDOWS)
|
||||
EnterCriticalSection(&(lcm->lock));
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_mutex_lock(&(lcm->lock));
|
||||
#endif /* OS_WINDOWS */
|
||||
lcmlite_subscribe(&(lcm->lcm), &(ret->light_subscription));
|
||||
#if defined(OS_WINDOWS)
|
||||
LeaveCriticalSection(&(lcm->lock));
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_mutex_unlock(&(lcm->lock));
|
||||
#endif /* OS_WINDOWS */
|
||||
return ret;
|
||||
}
|
||||
|
||||
int lcm_subscription_set_queue_capacity(lcm_subscription_t *handler,
|
||||
int num_messages) {
|
||||
(void)(handler);
|
||||
(void)(num_messages);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lcm_unsubscribe(lcm_t *lcm, lcm_subscription_t *handler) {
|
||||
#if defined(OS_WINDOWS)
|
||||
EnterCriticalSection(&(lcm->lock));
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_mutex_lock(&(lcm->lock));
|
||||
#endif /* OS_WINDOWS */
|
||||
lcmlite_unsubscribe(&(lcm->lcm), &(handler->light_subscription));
|
||||
#if defined(OS_WINDOWS)
|
||||
LeaveCriticalSection(&(lcm->lock));
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_mutex_unlock(&(lcm->lock));
|
||||
#endif /* OS_WINDOWS */
|
||||
free((char *)(handler->light_subscription.channel));
|
||||
free(handler);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int lcm_handle(lcm_t *lcm) {
|
||||
return lcm_handle_timeout(lcm, LCM_TIMEOUT_FOREVER);
|
||||
}
|
||||
|
||||
int lcm_handle_timeout(lcm_t *lcm, int timeout_millis) {
|
||||
#if defined(OS_WINDOWS)
|
||||
EnterCriticalSection(&(lcm->lock));
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_mutex_lock(&(lcm->lock));
|
||||
#endif /* OS_WINDOWS */
|
||||
int ret = 0;
|
||||
ret = lcm->info->handler(lcm, timeout_millis);
|
||||
#if defined(OS_WINDOWS)
|
||||
LeaveCriticalSection(&(lcm->lock));
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
pthread_mutex_unlock(&(lcm->lock));
|
||||
#endif /* OS_WINDOWS */
|
||||
return ret;
|
||||
}
|
||||
|
||||
int lcm_get_fileno(lcm_t *lcm) { return lcm->info->get_file_no(lcm->info); }
|
||||
|
||||
uint64_t lcm_get_timestamp() {
|
||||
// Get timestamp
|
||||
#if defined(OS_WINDOWS)
|
||||
#define EPOCHFILETIME (116444736000000000UL)
|
||||
FILETIME ft;
|
||||
LARGE_INTEGER li;
|
||||
GetSystemTimeAsFileTime(&ft);
|
||||
li.LowPart = ft.dwLowDateTime;
|
||||
li.HighPart = ft.dwHighDateTime;
|
||||
return (li.QuadPart - EPOCHFILETIME) / 10;
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
struct timeval tv;
|
||||
gettimeofday(&tv, 0);
|
||||
return tv.tv_sec * 1000000 + (int64_t)tv.tv_usec;
|
||||
#endif /* OS_WINDOWS */
|
||||
}
|
||||
|
|
@ -0,0 +1,408 @@
|
|||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <lcm/lcmlite.h>
|
||||
|
||||
#define MAGIC_LCM2 0x4c433032
|
||||
#define MAGIC_LCM3 0x4c433033
|
||||
|
||||
// header can contain a channel plus our usual header.
|
||||
#define MAXIMUM_HEADER_LENGTH 300
|
||||
|
||||
static inline void encode_u32(uint8_t *p, uint32_t v)
|
||||
{
|
||||
// big endian. p[3] gets lowest 8 bits.
|
||||
p[3] = v & 0xff;
|
||||
v >>= 8;
|
||||
p[2] = v & 0xff;
|
||||
v >>= 8;
|
||||
p[1] = v & 0xff;
|
||||
v >>= 8;
|
||||
p[0] = v & 0xff;
|
||||
}
|
||||
|
||||
static inline uint32_t decode_u32(const uint8_t *p)
|
||||
{
|
||||
uint32_t v = 0;
|
||||
// big endian. p[0] gets most significant bits.
|
||||
v |= p[0];
|
||||
v <<= 8;
|
||||
v |= p[1];
|
||||
v <<= 8;
|
||||
v |= p[2];
|
||||
v <<= 8;
|
||||
v |= p[3];
|
||||
return v;
|
||||
}
|
||||
|
||||
static inline void encode_u16(uint8_t *p, uint16_t v)
|
||||
{
|
||||
// big endian. p[1] gets lowest 8 bits.
|
||||
p[1] = v & 0xff;
|
||||
v >>= 8;
|
||||
p[0] = v & 0xff;
|
||||
}
|
||||
|
||||
static inline uint16_t decode_u16(const uint8_t *p)
|
||||
{
|
||||
uint16_t v = 0;
|
||||
// big endian. p[0] gets most significant bits.
|
||||
v |= p[0];
|
||||
v <<= 8;
|
||||
v |= p[1];
|
||||
return v;
|
||||
}
|
||||
|
||||
// Called by lcmlite internally when a packet is decoded. (Provides a
|
||||
// common delivery code path for fragmented and non-fragmented
|
||||
// packets.)
|
||||
static void deliver_packet(lcmlite_t *lcm, const char *channel, const void *buf, int buf_len)
|
||||
{
|
||||
// printf("deliver packet, channel %-30s, size %10d\n", channel, buflen);
|
||||
for (lcmlite_subscription_t *sub = lcm->first_subscription; sub != NULL; sub = sub->next)
|
||||
{
|
||||
int good = 1;
|
||||
|
||||
for (int pos = 0; 1; pos++)
|
||||
{
|
||||
// special case: does the channel have a wildcard-like expression in it?
|
||||
if (sub->channel[pos] == '.' && sub->channel[pos + 1] == '*')
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if (sub->channel[pos] == channel[pos])
|
||||
{
|
||||
// end of both strings? if so, we're done.
|
||||
if (channel[pos] == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// proceed to the next letter
|
||||
continue;
|
||||
}
|
||||
|
||||
// not a match.
|
||||
good = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
if (good)
|
||||
{
|
||||
sub->callback(lcm, channel, buf, buf_len, sub->user);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// The caller allocates permanent storage for LCMLite. This initializes
|
||||
int lcmlite_init(lcmlite_t *lcm, void (*transmit_packet)(const void *_buf, int buf_len, void *user), void *transmit_user)
|
||||
{
|
||||
memset(lcm, 0, sizeof(lcmlite_t));
|
||||
lcm->transmit_packet = transmit_packet;
|
||||
lcm->transmit_user = transmit_user;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/** Call this function whenever an LCM UDP packet is
|
||||
* received. Registered LCM handlers will be called
|
||||
* synchronously. When the function returns, the buffer can be safely
|
||||
* reused. Returns non-zero if the packet was not decoded properly,
|
||||
* but no special action is required by the caller.
|
||||
*
|
||||
* from_addr is opaque, but should uniquely identify the sender's IP
|
||||
* address and port.
|
||||
**/
|
||||
int lcmlite_receive_packet(lcmlite_t *lcm, const void *_buf, int buf_len, uint64_t from_addr)
|
||||
{
|
||||
uint8_t *buf = (uint8_t *) _buf;
|
||||
int buf_pos = 0;
|
||||
|
||||
// not even a header's length
|
||||
if (buf_len < 4)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint32_t magic = decode_u32(&buf[buf_pos]);
|
||||
buf_pos += 4;
|
||||
|
||||
if (magic == MAGIC_LCM2)
|
||||
{
|
||||
uint32_t msg_seq = decode_u32(&buf[buf_pos]);
|
||||
buf_pos += 4;
|
||||
(void) msg_seq; // quiet unused variable warning.
|
||||
// copy out zero-terminated string holding the channel #.
|
||||
char channel[LCM_MAX_CHANNEL_LENGTH];
|
||||
int channel_len = 0;
|
||||
|
||||
while (buf[buf_pos] != 0)
|
||||
{
|
||||
// malformed packet.
|
||||
if (buf_pos >= buf_len || channel_len >= LCM_MAX_CHANNEL_LENGTH)
|
||||
{
|
||||
return -2;
|
||||
}
|
||||
|
||||
channel[channel_len++] = buf[buf_pos++];
|
||||
}
|
||||
|
||||
channel[channel_len] = 0;
|
||||
buf_pos++; // skip the zero.
|
||||
deliver_packet(lcm, channel, &buf[buf_pos], buf_len - buf_pos);
|
||||
}
|
||||
else if (magic == MAGIC_LCM3)
|
||||
{
|
||||
if (LCM3_NUM_BUFFERS == 0)
|
||||
{
|
||||
return -3;
|
||||
}
|
||||
|
||||
uint32_t msg_seq = decode_u32(&buf[buf_pos]);
|
||||
buf_pos += 4;
|
||||
uint32_t msg_size = decode_u32(&buf[buf_pos]);
|
||||
buf_pos += 4;
|
||||
uint32_t fragment_offset = decode_u32(&buf[buf_pos]);
|
||||
buf_pos += 4;
|
||||
uint32_t fragment_id = decode_u16(&buf[buf_pos]);
|
||||
buf_pos += 2;
|
||||
uint32_t fragments_in_msg = decode_u16(&buf[buf_pos]);
|
||||
buf_pos += 2;
|
||||
int payload_len = buf_len - buf_pos;
|
||||
|
||||
// printf("%08x:%08x %d / %d\n", from_addr, msg_seq, fragment_id, fragments_in_msg);
|
||||
|
||||
// validate packet metadata
|
||||
if (msg_size > LCM3_MAX_PACKET_SIZE)
|
||||
{
|
||||
return -4;
|
||||
}
|
||||
|
||||
if (fragments_in_msg > LCM3_MAX_FRAGMENTS)
|
||||
{
|
||||
return -5;
|
||||
}
|
||||
|
||||
if (fragment_id >= fragments_in_msg || fragment_offset + payload_len > msg_size)
|
||||
{
|
||||
return -6;
|
||||
}
|
||||
|
||||
// find the fragment. Use a simple linear search; this is
|
||||
// cheap in comparison to how much work we're spending to
|
||||
// decode the large packet...
|
||||
struct fragment_buffer *fbuf = NULL;
|
||||
|
||||
// try to find a reassembly buffer for this from_addr that's already in progress
|
||||
for (int idx = 0; idx < LCM3_NUM_BUFFERS; idx++)
|
||||
{
|
||||
if (lcm->fragment_buffers[idx].from_addr == from_addr &&
|
||||
lcm->fragment_buffers[idx].msg_seq == msg_seq)
|
||||
{
|
||||
fbuf = &lcm->fragment_buffers[idx];
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (fbuf == NULL)
|
||||
{
|
||||
// didn't find one. Pick a new buffer to use.
|
||||
// Priorities:
|
||||
// 1) an idle (complete) buffer
|
||||
// 2) the incomplete buffer that received a valid fragment the longest time ago.
|
||||
int32_t max_age = -1; // low scores are good.
|
||||
|
||||
for (int idx = 0; idx < LCM3_NUM_BUFFERS; idx++)
|
||||
{
|
||||
if (lcm->fragment_buffers[idx].fragments_remaining == 0)
|
||||
{
|
||||
fbuf = &lcm->fragment_buffers[idx];
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
int32_t age = lcm->last_fragment_count - lcm->fragment_buffers[idx].last_fragment_count;
|
||||
|
||||
if (age > max_age)
|
||||
{
|
||||
fbuf = &lcm->fragment_buffers[idx];
|
||||
max_age = age;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (fbuf == NULL)
|
||||
{
|
||||
return -7; // this should never happen
|
||||
}
|
||||
|
||||
// initialize the fragment buffer
|
||||
for (size_t i = 0; i < fragments_in_msg; i++)
|
||||
{
|
||||
fbuf->frag_received[i] = 0;
|
||||
}
|
||||
|
||||
fbuf->from_addr = from_addr;
|
||||
fbuf->msg_seq = msg_seq;
|
||||
fbuf->fragments_remaining = fragments_in_msg;
|
||||
}
|
||||
|
||||
// now, handle this fragment
|
||||
fbuf->last_fragment_count = lcm->last_fragment_count;
|
||||
lcm->last_fragment_count++;
|
||||
|
||||
if (fragment_id == 0)
|
||||
{
|
||||
// this fragment contains the channel name plus data
|
||||
int channel_len = 0;
|
||||
|
||||
while (buf[buf_pos] != 0)
|
||||
{
|
||||
if (buf_pos >= buf_len || channel_len >= LCM_MAX_CHANNEL_LENGTH)
|
||||
{
|
||||
return -8;
|
||||
}
|
||||
|
||||
fbuf->channel[channel_len++] = buf[buf_pos++];
|
||||
}
|
||||
|
||||
fbuf->channel[channel_len] = 0;
|
||||
buf_pos++; // skip the zero.
|
||||
}
|
||||
|
||||
if (buf_pos < buf_len)
|
||||
{
|
||||
memcpy(&fbuf->buf[fragment_offset], &buf[buf_pos], buf_len - buf_pos);
|
||||
}
|
||||
|
||||
// record reception of this packet
|
||||
if (fbuf->frag_received[fragment_id] == 0)
|
||||
{
|
||||
fbuf->frag_received[fragment_id] = 1;
|
||||
fbuf->fragments_remaining--;
|
||||
|
||||
if (fbuf->fragments_remaining == 0)
|
||||
{
|
||||
deliver_packet(lcm, fbuf->channel, fbuf->buf, msg_size);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void lcmlite_subscribe(lcmlite_t *lcm, lcmlite_subscription_t *sub)
|
||||
{
|
||||
sub->next = lcm->first_subscription;
|
||||
sub->prev = NULL;
|
||||
lcm->first_subscription = sub;
|
||||
|
||||
if (sub->next != NULL)
|
||||
{
|
||||
sub->next->prev = sub;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void lcmlite_unsubscribe(lcmlite_t *lcm, lcmlite_subscription_t *sub)
|
||||
{
|
||||
if (sub->next != NULL)
|
||||
{
|
||||
sub->next->prev = sub->prev;
|
||||
}
|
||||
|
||||
if (sub->prev != NULL)
|
||||
{
|
||||
sub->prev->next = sub->next;
|
||||
}
|
||||
|
||||
if (lcm->first_subscription == sub)
|
||||
{
|
||||
lcm->first_subscription = sub->next;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
int lcmlite_publish(lcmlite_t *lcm, const char *channel, const void *_buf, int buf_len)
|
||||
{
|
||||
if (buf_len < LCM_PUBLISH_BUFFER_SIZE - MAXIMUM_HEADER_LENGTH)
|
||||
{
|
||||
// publish non-fragmented message
|
||||
uint32_t buf_pos = 0;
|
||||
encode_u32(&lcm->publish_buffer[buf_pos], MAGIC_LCM2);
|
||||
buf_pos += 4;
|
||||
encode_u32(&lcm->publish_buffer[buf_pos], lcm->msg_seq);
|
||||
buf_pos += 4;
|
||||
lcm->msg_seq++;
|
||||
|
||||
// copy channel
|
||||
while (*channel != 0)
|
||||
{
|
||||
lcm->publish_buffer[buf_pos++] = *channel;
|
||||
channel++;
|
||||
}
|
||||
|
||||
lcm->publish_buffer[buf_pos++] = 0 ;
|
||||
memcpy(&lcm->publish_buffer[buf_pos], _buf, buf_len);
|
||||
buf_pos += buf_len;
|
||||
lcm->transmit_packet(lcm->publish_buffer, buf_pos, lcm->transmit_user);
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
// send fragmented message
|
||||
uint32_t msg_seq = lcm->msg_seq;
|
||||
lcm->msg_seq++;
|
||||
uint32_t fragment_offset = 0;
|
||||
uint32_t max_fragment_size = LCM_PUBLISH_BUFFER_SIZE - MAXIMUM_HEADER_LENGTH;
|
||||
uint32_t fragment_id = 0;
|
||||
uint32_t fragments_in_msg = (buf_len + max_fragment_size - 1) / max_fragment_size;
|
||||
|
||||
while (fragment_offset < (uint32_t)buf_len)
|
||||
{
|
||||
uint32_t buf_pos = 0;
|
||||
encode_u32(&lcm->publish_buffer[buf_pos], MAGIC_LCM3);
|
||||
buf_pos += 4;
|
||||
encode_u32(&lcm->publish_buffer[buf_pos], msg_seq);
|
||||
buf_pos += 4;
|
||||
encode_u32(&lcm->publish_buffer[buf_pos], buf_len);
|
||||
buf_pos += 4;
|
||||
encode_u32(&lcm->publish_buffer[buf_pos], fragment_offset);
|
||||
buf_pos += 4;
|
||||
encode_u16(&lcm->publish_buffer[buf_pos], (uint16_t)fragment_id);
|
||||
buf_pos += 2;
|
||||
encode_u16(&lcm->publish_buffer[buf_pos], (uint16_t)fragments_in_msg);
|
||||
buf_pos += 2;
|
||||
|
||||
// copy channel
|
||||
if (fragment_id == 0)
|
||||
{
|
||||
while (*channel != 0)
|
||||
{
|
||||
lcm->publish_buffer[buf_pos++] = *channel;
|
||||
channel++;
|
||||
}
|
||||
|
||||
lcm->publish_buffer[buf_pos++] = 0 ;
|
||||
}
|
||||
|
||||
uint32_t this_fragment_size = buf_len - fragment_offset;
|
||||
|
||||
if (this_fragment_size > max_fragment_size)
|
||||
{
|
||||
this_fragment_size = max_fragment_size;
|
||||
}
|
||||
|
||||
memcpy(&lcm->publish_buffer[buf_pos], &((char *) _buf)[fragment_offset], this_fragment_size);
|
||||
buf_pos += this_fragment_size;
|
||||
lcm->transmit_packet(lcm->publish_buffer, buf_pos, lcm->transmit_user);
|
||||
fragment_offset += this_fragment_size;
|
||||
fragment_id++;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,234 @@
|
|||
#include <lcm/lcm.h>
|
||||
#include <lcm/udpm.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#if defined(OS_WINDOWS)
|
||||
#include "lcm/winporting/winporting.h"
|
||||
|
||||
typedef SSIZE_T ssize_t;
|
||||
typedef ssize_t socklen_t;
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
static lcm_udp_connect_info_t *match(const url_parse_result_t *);
|
||||
static void on_send(const void *_buf, int buf_len,
|
||||
lcm_udp_connect_info_t *user);
|
||||
static int do_recv(lcm_t *lcm, int timeout);
|
||||
static void destroy(lcm_udp_connect_info_t *info);
|
||||
static int get_file_no(lcm_udp_connect_info_t *info);
|
||||
|
||||
void lcm_udpm_regist() {
|
||||
static lcm_protocol_info_t info = {(lcm_protocol_match_t)(match), NULL};
|
||||
lcm_regist_protocol(&info);
|
||||
return;
|
||||
}
|
||||
|
||||
lcm_udp_connect_info_t *match(const url_parse_result_t *result) {
|
||||
// Check protocol
|
||||
if (strcmp(result->protocol_name, "udpm") != 0) {
|
||||
fprintf(stderr, "Protocol is mismatch, %s, %d\n", result->protocol_name,
|
||||
__LINE__);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Get ip address
|
||||
char ip_addr[16];
|
||||
memset(ip_addr, 0, 16);
|
||||
const char *ip_end = strstr(result->address, ":");
|
||||
|
||||
if (ip_end == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
strncpy(ip_addr, result->address, ip_end - result->address);
|
||||
// Port
|
||||
unsigned short port = (unsigned short)atoi(ip_end + 1);
|
||||
// Allocate ret
|
||||
lcm_udp_connect_info_t *ret = malloc(sizeof(lcm_udp_connect_info_t));
|
||||
|
||||
if (ret == NULL) {
|
||||
goto EXCEPT_ALLOC_RET;
|
||||
}
|
||||
|
||||
ret->conn.destroy = (void (*)(lcm_connect_info_t *))destroy;
|
||||
ret->conn.handler = do_recv;
|
||||
ret->conn.get_file_no = (int (*)(lcm_connect_info_t *))get_file_no;
|
||||
ret->conn.send_callback = (lcm_send_callback_t)on_send;
|
||||
// Initialize winsock
|
||||
#if defined(OS_WINDOWS)
|
||||
WSADATA wsaData;
|
||||
|
||||
if (WSAStartup(MAKEWORD(1, 1), &wsaData) != 0) {
|
||||
goto EXCEPT_WSASTARTUP;
|
||||
}
|
||||
|
||||
#endif /* OS_WINDOWS */
|
||||
// Address
|
||||
struct in_addr inaddr;
|
||||
#if defined(OS_WINDOWS)
|
||||
inaddr.s_addr = inet_addr(ip_addr);
|
||||
|
||||
if (inaddr.s_addr == INADDR_NONE) {
|
||||
goto EXCEPT_ADDR;
|
||||
}
|
||||
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
|
||||
if (inet_aton(ip_addr, (struct in_addr *)&inaddr) < 0) {
|
||||
fprintf(stderr, "Failed to convert address, %d\n", __LINE__);
|
||||
goto EXCEPT_ADDR;
|
||||
}
|
||||
|
||||
#endif /* OS_WINDOWS */
|
||||
memset(&(ret->recv_addr), 0, sizeof(struct sockaddr_in));
|
||||
ret->recv_addr.sin_family = AF_INET;
|
||||
ret->recv_addr.sin_addr.s_addr = INADDR_ANY;
|
||||
ret->recv_addr.sin_port = htons(port);
|
||||
memset(&(ret->send_addr), 0, sizeof(struct sockaddr_in));
|
||||
ret->send_addr.sin_family = AF_INET;
|
||||
ret->send_addr.sin_addr = inaddr;
|
||||
ret->send_addr.sin_port = htons(port);
|
||||
// Create receive socket
|
||||
ret->recv_s = socket(AF_INET, SOCK_DGRAM, 0);
|
||||
|
||||
if (ret->recv_s < 0) {
|
||||
fprintf(stderr, "Failed to create socket, %d\n", __LINE__);
|
||||
goto EXCEPT_RECV_SOCKET;
|
||||
}
|
||||
|
||||
// Reuse address
|
||||
int opt = 1;
|
||||
|
||||
if (setsockopt(ret->recv_s, SOL_SOCKET, SO_REUSEADDR, (const char *)&opt,
|
||||
sizeof(opt)) < 0) {
|
||||
fprintf(stderr, "Failed to set option: SOREUSEADDR, %d\n", __LINE__);
|
||||
goto EXCEPT_RECV_SOCKET_REUSE;
|
||||
}
|
||||
|
||||
// Join the multicast group
|
||||
struct ip_mreq mreq;
|
||||
mreq.imr_multiaddr = inaddr;
|
||||
mreq.imr_interface.s_addr = INADDR_ANY;
|
||||
|
||||
if (setsockopt(ret->recv_s, IPPROTO_IP, IP_ADD_MEMBERSHIP,
|
||||
(const char *)(&mreq), sizeof(mreq)) < 0) {
|
||||
fprintf(stderr, "Failed to join the multicast group, %d\n", __LINE__);
|
||||
goto EXCEPT_RECV_JOIN_MULTICAST;
|
||||
}
|
||||
|
||||
// Bind receive socket
|
||||
if (bind(ret->recv_s, (struct sockaddr *)&(ret->recv_addr),
|
||||
sizeof(ret->recv_addr)) < 0) {
|
||||
fprintf(stderr, "Failed to bind the receive socket, %d\n", __LINE__);
|
||||
goto EXCEPT_RECV_BIND;
|
||||
}
|
||||
|
||||
// Options
|
||||
for (url_param_t *p_param = result->params; p_param != NULL;
|
||||
p_param = p_param->p_next) {
|
||||
if (strcmp(p_param->name, "ttl") == 0) {
|
||||
uint8_t ttl = (uint8_t)atoi(p_param->value);
|
||||
|
||||
if (setsockopt(ret->recv_s, IPPROTO_IP, IP_MULTICAST_TTL,
|
||||
(const char *)(&ttl), sizeof(ttl)) < 0) {
|
||||
fprintf(stderr, "Failed to set the option: IP_MULTICAST_TTL, %d\n",
|
||||
__LINE__);
|
||||
goto EXCEPT_RECV_OPTION;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Create send socket
|
||||
ret->send_s = socket(AF_INET, SOCK_DGRAM, 0);
|
||||
|
||||
if (ret->send_s < 0) {
|
||||
fprintf(stderr, "Failed to create send socket, %d\n", __LINE__);
|
||||
goto EXCEPT_SEND_SOCKET;
|
||||
}
|
||||
|
||||
return ret;
|
||||
EXCEPT_SEND_SOCKET:
|
||||
EXCEPT_RECV_OPTION:
|
||||
EXCEPT_RECV_BIND:
|
||||
EXCEPT_RECV_JOIN_MULTICAST:
|
||||
EXCEPT_RECV_SOCKET_REUSE:
|
||||
#if defined(OS_WINDOWS)
|
||||
closesocket(ret->recv_s);
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
close(ret->recv_s);
|
||||
#endif /* OS_WINDOWS */
|
||||
EXCEPT_RECV_SOCKET:
|
||||
EXCEPT_ADDR:
|
||||
#if defined(OS_WINDOWS)
|
||||
WSACleanup();
|
||||
EXCEPT_WSASTARTUP:
|
||||
#endif /* OS_WINDOWS */
|
||||
free(ret);
|
||||
EXCEPT_ALLOC_RET:
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void on_send(const void *_buf, int buf_len, lcm_udp_connect_info_t *info) {
|
||||
sendto(info->send_s, _buf, buf_len, 0, (struct sockaddr *)&info->send_addr,
|
||||
sizeof(info->send_addr));
|
||||
}
|
||||
|
||||
int do_recv(lcm_t *lcm, int timeout_millisec) {
|
||||
lcm_udp_connect_info_t *info = (lcm_udp_connect_info_t *)(lcm->info);
|
||||
// Timeout
|
||||
struct timeval tv;
|
||||
|
||||
if (timeout_millisec == LCM_TIMEOUT_FOREVER) {
|
||||
memset(&tv, 0, sizeof(tv));
|
||||
} else {
|
||||
tv.tv_sec = timeout_millisec / 1000;
|
||||
tv.tv_usec = timeout_millisec % 1000 * 1000;
|
||||
}
|
||||
|
||||
if (setsockopt(info->recv_s, SOL_SOCKET, SO_RCVTIMEO, (const char *)(&tv),
|
||||
sizeof(tv)) < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Receive
|
||||
char buf[65536];
|
||||
struct sockaddr_in from_addr;
|
||||
socklen_t from_addr_sz = sizeof(from_addr);
|
||||
#if defined(OS_WINDOWS)
|
||||
ssize_t buf_len =
|
||||
recvfrom(info->recv_s, buf, sizeof(buf), 0, (struct sockaddr *)&from_addr,
|
||||
(int *)(&from_addr_sz));
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
ssize_t buf_len = recvfrom(info->recv_s, buf, sizeof(buf), 0,
|
||||
(struct sockaddr *)&from_addr, &from_addr_sz);
|
||||
#endif /* OS_WINDOWS */
|
||||
|
||||
if (buf_len < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
int res = lcmlite_receive_packet(&(lcm->lcm), buf, (int)buf_len,
|
||||
from_addr.sin_addr.s_addr |
|
||||
((uint64_t)from_addr.sin_port << 32));
|
||||
|
||||
if (res < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int get_file_no(lcm_udp_connect_info_t *info) { return (int)(info->recv_s); }
|
||||
|
||||
void destroy(lcm_udp_connect_info_t *info) {
|
||||
#if defined(OS_WINDOWS)
|
||||
closesocket(info->recv_s);
|
||||
closesocket(info->send_s);
|
||||
WSACleanup();
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
close(info->recv_s);
|
||||
close(info->send_s);
|
||||
#endif /* OS_WINDOWS */
|
||||
free(info);
|
||||
return;
|
||||
}
|
||||
|
|
@ -0,0 +1,210 @@
|
|||
#include <lcm/url_parser.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
static bool get_protocol_name(url_parse_result_t *out,
|
||||
const char **p)
|
||||
{
|
||||
//Get protocol
|
||||
const char *p_end = strstr(*p, "://");;
|
||||
|
||||
if (p_end == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
size_t protocol_len = (size_t)(p_end - *p + 1);
|
||||
//Copy protocol name
|
||||
out->protocol_name = malloc(protocol_len);
|
||||
|
||||
if (out->protocol_name == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
strncpy(out->protocol_name, *p, protocol_len - 1);
|
||||
out->protocol_name[protocol_len - 1] = '\0';
|
||||
*p = p_end + 3;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool get_address(url_parse_result_t *out,
|
||||
const char **p)
|
||||
{
|
||||
//Get address
|
||||
const char *p_end = *p;
|
||||
|
||||
while (*p_end != '\0' && *p_end != '?')
|
||||
{
|
||||
p_end++;
|
||||
}
|
||||
|
||||
//Copy address
|
||||
size_t addr_len = (size_t)(p_end - *p + 1);
|
||||
out->address = malloc(addr_len);
|
||||
|
||||
if (out->address == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
strncpy(out->address, *p, addr_len - 1);
|
||||
out->address[addr_len - 1] = '\0';
|
||||
*p = p_end;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool get_params(url_parse_result_t *out,
|
||||
const char **p)
|
||||
{
|
||||
out->params = NULL;
|
||||
|
||||
if (**p == '\0')
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
(*p)++;
|
||||
const char *begin = *p;
|
||||
|
||||
//Parse arguments
|
||||
while (*begin != '\0')
|
||||
{
|
||||
//Name
|
||||
const char *name_end = strstr(begin, "=");
|
||||
|
||||
if (name_end == NULL)
|
||||
{
|
||||
goto EXCEPT;
|
||||
}
|
||||
|
||||
size_t name_len = name_end - begin + 1;
|
||||
//Value
|
||||
const char *value_begin = name_end + 1;
|
||||
const char *value_end = strstr(value_begin, "&");
|
||||
|
||||
if (value_end == NULL)
|
||||
{
|
||||
value_end = value_begin + strlen(value_begin);
|
||||
}
|
||||
|
||||
size_t value_len = value_end - value_begin + 1;
|
||||
//Allocate param
|
||||
url_param_t *p_new = malloc(sizeof(url_param_t));
|
||||
|
||||
if (p_new == NULL)
|
||||
{
|
||||
goto EXCEPT;
|
||||
}
|
||||
|
||||
//Name
|
||||
p_new->name = malloc(name_len);
|
||||
|
||||
if (p_new->name == NULL)
|
||||
{
|
||||
free(p_new);
|
||||
goto EXCEPT;
|
||||
}
|
||||
|
||||
//Value
|
||||
p_new->value = malloc(value_len);
|
||||
|
||||
if (p_new->value == NULL)
|
||||
{
|
||||
free(p_new->name);
|
||||
free(p_new);
|
||||
goto EXCEPT;
|
||||
}
|
||||
|
||||
//Copy name
|
||||
strncpy(p_new->name, begin, name_len - 1);
|
||||
p_new->name[name_len - 1] = '\0';
|
||||
//Copy value
|
||||
strncpy(p_new->value, value_begin, value_len - 1);
|
||||
p_new->value[value_len - 1] = '\0';
|
||||
//Insert
|
||||
p_new->p_next = out->params;
|
||||
out->params = p_new;
|
||||
begin = value_end;
|
||||
|
||||
if (*begin != '\0')
|
||||
{
|
||||
begin++;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
EXCEPT:
|
||||
|
||||
while (out->params != NULL)
|
||||
{
|
||||
url_param_t *next = out->params->p_next;
|
||||
free(out->params->name);
|
||||
free(out->params->value);
|
||||
free(out->params);
|
||||
out->params = next;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
url_parse_result_t *url_parse(const char *url)
|
||||
{
|
||||
//Allocate memory
|
||||
url_parse_result_t *ret = malloc(sizeof(url_parse_result_t));
|
||||
|
||||
if (ret == NULL)
|
||||
{
|
||||
goto EXCEPT_MALLOC_RET;
|
||||
}
|
||||
|
||||
memset(ret, 0, sizeof(url_parse_result_t));
|
||||
const char *begin = url;
|
||||
|
||||
//Get protocol name
|
||||
if (!get_protocol_name(ret, &begin))
|
||||
{
|
||||
goto EXCEPT_GET_PROTOCOL;
|
||||
}
|
||||
|
||||
//Get address
|
||||
if (!get_address(ret, &begin))
|
||||
{
|
||||
goto EXCEPT_GET_ADDRESS;
|
||||
}
|
||||
|
||||
//Get params
|
||||
if (!get_params(ret, &begin))
|
||||
{
|
||||
goto EXCEPT_GET_PARAMS;
|
||||
}
|
||||
|
||||
return ret;
|
||||
EXCEPT_GET_PARAMS:
|
||||
free(ret->address);
|
||||
EXCEPT_GET_ADDRESS:
|
||||
free(ret->protocol_name);
|
||||
EXCEPT_GET_PROTOCOL:
|
||||
free(ret);
|
||||
EXCEPT_MALLOC_RET:
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void free_url_parse_result(url_parse_result_t *result)
|
||||
{
|
||||
while (result->params != NULL)
|
||||
{
|
||||
url_param_t *next = result->params->p_next;
|
||||
free(result->params->name);
|
||||
free(result->params->value);
|
||||
free(result->params);
|
||||
result->params = next;
|
||||
}
|
||||
|
||||
free(result->address);
|
||||
free(result->protocol_name);
|
||||
free(result);
|
||||
return;
|
||||
}
|
||||
|
|
@ -0,0 +1,163 @@
|
|||
#include <lcm/lcm.h>
|
||||
#include <lcm/winporting/winporting.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#if defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
#include <getopt.h>
|
||||
#include <unistd.h>
|
||||
#endif /* OS_LINUX || OS_ANDROID */
|
||||
|
||||
static void usage(const char *name) {
|
||||
printf(
|
||||
"Usage:\n"
|
||||
" %s [OPTIONS] [FILE]\n"
|
||||
"\n"
|
||||
" LCM message logging utility. Subscribes to all channels on an LCM\n"
|
||||
" network, and records all messages received on that network to\n"
|
||||
" FILE. If FILE is not specified, then a filename is automatically\n"
|
||||
" chosen.\n"
|
||||
"\n"
|
||||
"Options:\n"
|
||||
" -c, --channel=CHAN Channel string to pass to "
|
||||
"lcm_subscribe.\n"
|
||||
" (default: \".*\")\n"
|
||||
" -h, --help Shows this help text and exits\n"
|
||||
" -l, --lcm-url=URL Log messages on the specified LCM URL\n"
|
||||
" -q, --quiet Suppress normal output and only report "
|
||||
"errors.\n"
|
||||
" -a, --append Append events to the given log file.\n"
|
||||
"\n"
|
||||
"",
|
||||
name);
|
||||
return;
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
lcm_eventlog_t *log;
|
||||
bool quiet;
|
||||
} recv_info_t;
|
||||
|
||||
static void on_recv(const lcm_recv_buf_t *rbuf, const char *channel,
|
||||
recv_info_t *info) {
|
||||
lcm_eventlog_event_t event;
|
||||
event.channel = (char *)channel;
|
||||
event.channellen = (int32_t)strlen(channel);
|
||||
event.data = rbuf->data;
|
||||
event.datalen = rbuf->data_size;
|
||||
event.timestamp = rbuf->recv_utime;
|
||||
event.eventnum = 0;
|
||||
|
||||
if (!info->quiet) {
|
||||
printf("Received\n");
|
||||
}
|
||||
|
||||
lcm_eventlog_write_event(info->log, &event);
|
||||
fflush(info->log->f);
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
// Default options
|
||||
const char *log_path = NULL;
|
||||
const char *channel = ".*";
|
||||
const char *url = NULL;
|
||||
bool quiet = false;
|
||||
bool append = false;
|
||||
// Get options
|
||||
const char *opt_str = "c:hl:qa";
|
||||
struct option long_opts[] = {{"channel", 1, 0, 'c'}, {"help", 0, 0, 'h'},
|
||||
{"lcm-url", 1, 0, 'l'}, {"quiet", 0, 0, 'q'},
|
||||
{"append", 0, 0, 'a'}, {NULL, 0, 0, 0}};
|
||||
int opt;
|
||||
|
||||
while ((opt = getopt_long(argc, argv, opt_str, long_opts, NULL)) != -1) {
|
||||
switch (opt) {
|
||||
case 'c':
|
||||
channel = optarg;
|
||||
break;
|
||||
|
||||
case 'h':
|
||||
usage(argv[0]);
|
||||
return 0;
|
||||
|
||||
case 'l':
|
||||
url = optarg;
|
||||
break;
|
||||
|
||||
case 'q':
|
||||
quiet = true;
|
||||
break;
|
||||
|
||||
case 'a':
|
||||
append = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
usage(argv[0]);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
char namebuf[18];
|
||||
|
||||
if (optind == argc) {
|
||||
time_t t = time(NULL);
|
||||
struct tm *cur_tm = localtime(&t);
|
||||
sprintf(namebuf, "lcmlog-%d-%.2d-%.2d", cur_tm->tm_year + 1900,
|
||||
cur_tm->tm_mon + 1, cur_tm->tm_mday);
|
||||
log_path = namebuf;
|
||||
} else if (optind == argc - 1) {
|
||||
log_path = argv[optind];
|
||||
} else if (optind < argc - 1) {
|
||||
usage(argv[0]);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Open event log
|
||||
lcm_eventlog_t *log_file;
|
||||
|
||||
if (append) {
|
||||
log_file = lcm_eventlog_create(log_path, "a");
|
||||
} else {
|
||||
log_file = lcm_eventlog_create(log_path, "w");
|
||||
}
|
||||
|
||||
if (log_file == NULL) {
|
||||
fprintf(stderr, "Failed to open log file \"%s\".\n", log_path);
|
||||
goto EXCEPT_OPEN_LOG_FILE;
|
||||
}
|
||||
|
||||
// Create lcm
|
||||
lcm_t *lcm = lcm_create(url);
|
||||
|
||||
if (lcm == NULL) {
|
||||
fprintf(stderr, "Failed to create lcm object, url = \"%s\".\n", url == NULL ? "NULL" : url);
|
||||
goto EXCEPT_CREATE_LCM;
|
||||
}
|
||||
|
||||
// Bind lcm
|
||||
recv_info_t info;
|
||||
info.log = log_file;
|
||||
info.quiet = quiet;
|
||||
|
||||
if (lcm_subscribe(lcm, channel, (lcm_msg_handler_t)on_recv, &info) == NULL) {
|
||||
fprintf(stderr, "lcm_subscribe() failed!\n");
|
||||
goto EXCEPT_BIND_LCM;
|
||||
}
|
||||
|
||||
// Main loop
|
||||
while (1) {
|
||||
lcm_handle(lcm);
|
||||
}
|
||||
|
||||
lcm_destroy(lcm);
|
||||
lcm_eventlog_destroy(log_file);
|
||||
return 0;
|
||||
EXCEPT_BIND_LCM:
|
||||
lcm_destroy(lcm);
|
||||
EXCEPT_CREATE_LCM:
|
||||
lcm_eventlog_destroy(log_file);
|
||||
EXCEPT_OPEN_LOG_FILE:
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -0,0 +1,138 @@
|
|||
#include <lcm/lcm.h>
|
||||
#include <lcm/winporting/winporting.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#if defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
#include <getopt.h>
|
||||
#include <unistd.h>
|
||||
#endif /* OS_LINUX || OS_ANDROID */
|
||||
|
||||
static void usage(const char *name) {
|
||||
printf(
|
||||
"Usage:\n"
|
||||
" %s [OPTIONS] FILE\n"
|
||||
"\n"
|
||||
" Reads packets from an LCM log file and publishes them to LCM.\n"
|
||||
"\n"
|
||||
"Options:\n"
|
||||
" -s, --speed=NUM Playback speed multiplier. Default is "
|
||||
"1.0.\n"
|
||||
" -v, --verbose Print information about each packet.\n"
|
||||
" -h, --help Shows this help text and exits\n"
|
||||
" -l, --lcm-url=URL Log messages on the specified LCM URL\n"
|
||||
"\n"
|
||||
"",
|
||||
name);
|
||||
return;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
// Default options
|
||||
float speed = 1.0;
|
||||
const char *url = NULL;
|
||||
const char *log_path;
|
||||
bool verbose = false;
|
||||
// Get options
|
||||
const char *opt_str = "s:vhl:";
|
||||
struct option long_opts[] = {{"speed", 1, 0, 's'},
|
||||
{"verbose", 0, 0, 'v'},
|
||||
{"help", 0, 0, 'h'},
|
||||
{"lcm-url", 1, 0, 'l'},
|
||||
{NULL, 0, 0, 0}};
|
||||
int opt;
|
||||
|
||||
while ((opt = getopt_long(argc, argv, opt_str, long_opts, NULL)) != -1) {
|
||||
switch (opt) {
|
||||
case 's':
|
||||
speed = (float)atof(optarg);
|
||||
break;
|
||||
|
||||
case 'v':
|
||||
verbose = true;
|
||||
break;
|
||||
|
||||
case 'h':
|
||||
usage(argv[0]);
|
||||
return 0;
|
||||
|
||||
case 'l':
|
||||
url = optarg;
|
||||
break;
|
||||
|
||||
default:
|
||||
usage(argv[0]);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (optind == argc - 1) {
|
||||
log_path = argv[optind];
|
||||
} else {
|
||||
usage(argv[0]);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Open log file
|
||||
int ret;
|
||||
lcm_eventlog_t *log_file = lcm_eventlog_create(log_path, "r");
|
||||
|
||||
if (log_file == NULL) {
|
||||
fprintf(stderr, "Failed to open log file \"%s\".\n", log_path);
|
||||
ret = -1;
|
||||
goto EXCEPT_OPEN_LOG_FILE;
|
||||
}
|
||||
|
||||
// Create lcm
|
||||
lcm_t *lcm = lcm_create(url);
|
||||
|
||||
if (lcm == NULL) {
|
||||
fprintf(stderr, "Failed to create lcm object, url = \"%s\".\n", url == NULL ? "NULL" : url);
|
||||
ret = -1;
|
||||
goto EXCEPT_CREATE_LCM;
|
||||
}
|
||||
|
||||
// Send log
|
||||
lcm_eventlog_event_t *event;
|
||||
int64_t prev_timestamp = 0;
|
||||
int64_t prev_time = 0;
|
||||
printf("Using playback speed %f.\n", speed);
|
||||
|
||||
while ((event = lcm_eventlog_read_next_event(log_file)) != NULL) {
|
||||
if (prev_timestamp != 0) {
|
||||
int64_t send_time =
|
||||
prev_time + (int64_t)((event->timestamp - prev_timestamp) / speed);
|
||||
#if defined(OS_WINDOWS)
|
||||
Sleep((DWORD)((send_time - lcm_get_timestamp()) / 1000));
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
int64_t timestamp = lcm_get_timestamp();
|
||||
|
||||
if (timestamp < send_time) {
|
||||
usleep(send_time - timestamp);
|
||||
}
|
||||
|
||||
#endif /* OS_WINDOWS */
|
||||
}
|
||||
|
||||
lcm_publish(lcm, event->channel, event->data, event->datalen);
|
||||
|
||||
if (verbose) {
|
||||
printf("Timestamp : %llu, channel : %s, size : %u.\n",
|
||||
(unsigned long long)(event->timestamp), event->channel,
|
||||
event->datalen);
|
||||
}
|
||||
|
||||
prev_time = lcm_get_timestamp();
|
||||
prev_timestamp = event->timestamp;
|
||||
lcm_eventlog_free_event(event);
|
||||
}
|
||||
|
||||
ret = 0;
|
||||
lcm_destroy(lcm);
|
||||
EXCEPT_CREATE_LCM:
|
||||
lcm_eventlog_destroy(log_file);
|
||||
EXCEPT_OPEN_LOG_FILE:
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -0,0 +1,871 @@
|
|||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <windows.h>
|
||||
|
||||
#define getpid() GetCurrentProcessId()
|
||||
|
||||
#ifndef _
|
||||
/* This is for other GNU distributions with internationalized messages.
|
||||
When compiling libc, the _ macro is predefined. */
|
||||
#ifdef NEVER_HAVE_LIBINTL_H
|
||||
#include <libintl.h>
|
||||
#define _(msgid) gettext (msgid)
|
||||
#else
|
||||
#define _(msgid) (msgid)
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/* This version of `getopt' appears to the caller like standard Unix `getopt'
|
||||
but it behaves differently for the user, since it allows the user
|
||||
to intersperse the options with the other arguments.
|
||||
|
||||
As `getopt' works, it permutes the elements of ARGV so that,
|
||||
when it is done, all the options precede everything else. Thus
|
||||
all application programs are extended to handle flexible argument order.
|
||||
|
||||
Setting the environment variable POSIXLY_CORRECT disables permutation.
|
||||
Then the behavior is completely standard.
|
||||
|
||||
GNU application programs can use a third alternative mode in which
|
||||
they can distinguish the relative order of options and other arguments. */
|
||||
|
||||
#include <lcm/winporting/getopt.h>
|
||||
|
||||
/* For communication from `getopt' to the caller.
|
||||
When `getopt' finds an option that takes an argument,
|
||||
the argument value is returned here.
|
||||
Also, when `ordering' is RETURN_IN_ORDER,
|
||||
each non-option ARGV-element is returned here. */
|
||||
|
||||
char *optarg = NULL;
|
||||
|
||||
/* Index in ARGV of the next element to be scanned.
|
||||
This is used for communication to and from the caller
|
||||
and for communication between successive calls to `getopt'.
|
||||
|
||||
On entry to `getopt', zero means this is the first call; initialize.
|
||||
|
||||
When `getopt' returns -1, this is the index of the first of the
|
||||
non-option elements that the caller should itself scan.
|
||||
|
||||
Otherwise, `optind' communicates from one call to the next
|
||||
how much of ARGV has been scanned so far. */
|
||||
|
||||
/* 1003.2 says this must be 1 before any call. */
|
||||
int optind = 1;
|
||||
|
||||
/* Formerly, initialization of getopt depended on optind==0, which
|
||||
causes problems with re-calling getopt as programs generally don't
|
||||
know that. */
|
||||
|
||||
int __getopt_initialized = 0;
|
||||
|
||||
/* The next char to be scanned in the option-element
|
||||
in which the last option character we returned was found.
|
||||
This allows us to pick up the scan where we left off.
|
||||
|
||||
If this is zero, or a null string, it means resume the scan
|
||||
by advancing to the next ARGV-element. */
|
||||
|
||||
static char *nextchar;
|
||||
|
||||
/* Callers store zero here to inhibit the error message
|
||||
for unrecognized options. */
|
||||
|
||||
int opterr = 1;
|
||||
|
||||
/* Set to an option character which was unrecognized.
|
||||
This must be initialized on some systems to avoid linking in the
|
||||
system's own getopt implementation. */
|
||||
|
||||
int optopt = '?';
|
||||
|
||||
/* Describe how to deal with options that follow non-option ARGV-elements.
|
||||
|
||||
If the caller did not specify anything,
|
||||
the default is REQUIRE_ORDER if the environment variable
|
||||
POSIXLY_CORRECT is defined, PERMUTE otherwise.
|
||||
|
||||
REQUIRE_ORDER means don't recognize them as options;
|
||||
stop option processing when the first non-option is seen.
|
||||
This is what Unix does.
|
||||
This mode of operation is selected by either setting the environment
|
||||
variable POSIXLY_CORRECT, or using `+' as the first character
|
||||
of the list of option characters.
|
||||
|
||||
PERMUTE is the default. We permute the contents of ARGV as we scan,
|
||||
so that eventually all the non-options are at the end. This allows options
|
||||
to be given in any order, even with programs that were not written to
|
||||
expect this.
|
||||
|
||||
RETURN_IN_ORDER is an option available to programs that were written
|
||||
to expect options and other ARGV-elements in any order and that care about
|
||||
the ordering of the two. We describe each non-option ARGV-element
|
||||
as if it were the argument of an option with character code 1.
|
||||
Using `-' as the first character of the list of option characters
|
||||
selects this mode of operation.
|
||||
|
||||
The special argument `--' forces an end of option-scanning regardless
|
||||
of the value of `ordering'. In the case of RETURN_IN_ORDER, only
|
||||
`--' can cause `getopt' to return -1 with `optind' != ARGC. */
|
||||
|
||||
static enum
|
||||
{
|
||||
REQUIRE_ORDER, PERMUTE, RETURN_IN_ORDER
|
||||
} ordering;
|
||||
|
||||
/* Value of POSIXLY_CORRECT environment variable. */
|
||||
static char *posixly_correct;
|
||||
|
||||
/* Avoid depending on library functions or files
|
||||
whose names are inconsistent. */
|
||||
|
||||
char *getenv();
|
||||
|
||||
static char *
|
||||
my_index(const char *str, int chr)
|
||||
{
|
||||
while (*str)
|
||||
{
|
||||
if (*str == chr)
|
||||
{
|
||||
return (char *) str;
|
||||
}
|
||||
|
||||
str++;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Handle permutation of arguments. */
|
||||
|
||||
/* Describe the part of ARGV that contains non-options that have
|
||||
been skipped. `first_nonopt' is the index in ARGV of the first of them;
|
||||
`last_nonopt' is the index after the last of them. */
|
||||
|
||||
static int first_nonopt;
|
||||
static int last_nonopt;
|
||||
|
||||
|
||||
# define SWAP_FLAGS(ch1, ch2)
|
||||
|
||||
/* Exchange two adjacent subsequences of ARGV.
|
||||
One subsequence is elements [first_nonopt,last_nonopt)
|
||||
which contains all the non-options that have been skipped so far.
|
||||
The other is elements [last_nonopt,optind), which contains all
|
||||
the options processed since those non-options were skipped.
|
||||
|
||||
`first_nonopt' and `last_nonopt' are relocated so that they describe
|
||||
the new indices of the non-options in ARGV after they are moved. */
|
||||
|
||||
|
||||
|
||||
static void
|
||||
exchange(char **argv)
|
||||
|
||||
{
|
||||
int bottom = first_nonopt;
|
||||
int middle = last_nonopt;
|
||||
int top = optind;
|
||||
char *tem;
|
||||
|
||||
/* Exchange the shorter segment with the far end of the longer segment.
|
||||
That puts the shorter segment into the right place.
|
||||
It leaves the longer segment in the right place overall,
|
||||
but it consists of two parts that need to be swapped next. */
|
||||
|
||||
while (top > middle && middle > bottom)
|
||||
{
|
||||
if (top - middle > middle - bottom)
|
||||
{
|
||||
/* Bottom segment is the short one. */
|
||||
int len = middle - bottom;
|
||||
register int i;
|
||||
|
||||
/* Swap it with the top part of the top segment. */
|
||||
for (i = 0; i < len; i++)
|
||||
{
|
||||
tem = argv[bottom + i];
|
||||
argv[bottom + i] = argv[top - (middle - bottom) + i];
|
||||
argv[top - (middle - bottom) + i] = tem;
|
||||
SWAP_FLAGS(bottom + i, top - (middle - bottom) + i);
|
||||
}
|
||||
|
||||
/* Exclude the moved bottom segment from further swapping. */
|
||||
top -= len;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Top segment is the short one. */
|
||||
int len = top - middle;
|
||||
register int i;
|
||||
|
||||
/* Swap it with the bottom part of the bottom segment. */
|
||||
for (i = 0; i < len; i++)
|
||||
{
|
||||
tem = argv[bottom + i];
|
||||
argv[bottom + i] = argv[middle + i];
|
||||
argv[middle + i] = tem;
|
||||
SWAP_FLAGS(bottom + i, middle + i);
|
||||
}
|
||||
|
||||
/* Exclude the moved top segment from further swapping. */
|
||||
bottom += len;
|
||||
}
|
||||
}
|
||||
|
||||
/* Update records for the slots the non-options now occupy. */
|
||||
first_nonopt += (optind - last_nonopt);
|
||||
last_nonopt = optind;
|
||||
}
|
||||
|
||||
/* Initialize the internal data when the first call is made. */
|
||||
|
||||
static const char *
|
||||
_getopt_initialize(int argc,
|
||||
char *const *argv,
|
||||
const char *optstring)
|
||||
{
|
||||
/* Start processing options with ARGV-element 1 (since ARGV-element 0
|
||||
is the program name); the sequence of previously skipped
|
||||
non-option ARGV-elements is empty. */
|
||||
first_nonopt = last_nonopt = optind;
|
||||
nextchar = NULL;
|
||||
|
||||
// posixly_correct = getenv ("POSIXLY_CORRECT");
|
||||
|
||||
/* Determine how to handle the ordering of options and nonoptions. */
|
||||
|
||||
if (optstring[0] == '-')
|
||||
{
|
||||
ordering = RETURN_IN_ORDER;
|
||||
++optstring;
|
||||
}
|
||||
else if (optstring[0] == '+')
|
||||
{
|
||||
ordering = REQUIRE_ORDER;
|
||||
++optstring;
|
||||
}
|
||||
//else if (posixly_correct != NULL)
|
||||
// ordering = REQUIRE_ORDER;
|
||||
else
|
||||
{
|
||||
ordering = PERMUTE;
|
||||
}
|
||||
|
||||
return optstring;
|
||||
(void)(argc);
|
||||
(void)(argv);
|
||||
}
|
||||
|
||||
/* Scan elements of ARGV (whose length is ARGC) for option characters
|
||||
given in OPTSTRING.
|
||||
|
||||
If an element of ARGV starts with '-', and is not exactly "-" or "--",
|
||||
then it is an option element. The characters of this element
|
||||
(aside from the initial '-') are option characters. If `getopt'
|
||||
is called repeatedly, it returns successively each of the option characters
|
||||
from each of the option elements.
|
||||
|
||||
If `getopt' finds another option character, it returns that character,
|
||||
updating `optind' and `nextchar' so that the next call to `getopt' can
|
||||
resume the scan with the following option character or ARGV-element.
|
||||
|
||||
If there are no more option characters, `getopt' returns -1.
|
||||
Then `optind' is the index in ARGV of the first ARGV-element
|
||||
that is not an option. (The ARGV-elements have been permuted
|
||||
so that those that are not options now come last.)
|
||||
|
||||
OPTSTRING is a string containing the legitimate option characters.
|
||||
If an option character is seen that is not listed in OPTSTRING,
|
||||
return '?' after printing an error message. If you set `opterr' to
|
||||
zero, the error message is suppressed but we still return '?'.
|
||||
|
||||
If a char in OPTSTRING is followed by a colon, that means it wants an arg,
|
||||
so the following text in the same ARGV-element, or the text of the following
|
||||
ARGV-element, is returned in `optarg'. Two colons mean an option that
|
||||
wants an optional arg; if there is text in the current ARGV-element,
|
||||
it is returned in `optarg', otherwise `optarg' is set to zero.
|
||||
|
||||
If OPTSTRING starts with `-' or `+', it requests different methods of
|
||||
handling the non-option ARGV-elements.
|
||||
See the comments about RETURN_IN_ORDER and REQUIRE_ORDER, above.
|
||||
|
||||
Long-named options begin with `--' instead of `-'.
|
||||
Their names may be abbreviated as long as the abbreviation is unique
|
||||
or is an exact match for some defined option. If they have an
|
||||
argument, it follows the option name in the same ARGV-element, separated
|
||||
from the option name by a `=', or else the in next ARGV-element.
|
||||
When `getopt' finds a long-named option, it returns 0 if that option's
|
||||
`flag' field is nonzero, the value of the option's `val' field
|
||||
if the `flag' field is zero.
|
||||
|
||||
The elements of ARGV aren't really const, because we permute them.
|
||||
But we pretend they're const in the prototype to be compatible
|
||||
with other systems.
|
||||
|
||||
LONGOPTS is a vector of `struct option' terminated by an
|
||||
element containing a name which is zero.
|
||||
|
||||
LONGIND returns the index in LONGOPT of the long-named option found.
|
||||
It is only valid when a long-named option has been found by the most
|
||||
recent call.
|
||||
|
||||
If LONG_ONLY is nonzero, '-' as well as '--' can introduce
|
||||
long-named options. */
|
||||
|
||||
int
|
||||
_getopt_internal(int argc,
|
||||
char *const *argv,
|
||||
const char *optstring,
|
||||
const struct option *longopts,
|
||||
int *longind,
|
||||
int long_only)
|
||||
{
|
||||
optarg = NULL;
|
||||
|
||||
if (optind == 0 || !__getopt_initialized)
|
||||
{
|
||||
if (optind == 0)
|
||||
{
|
||||
optind = 1; /* Don't scan ARGV[0], the program name. */
|
||||
}
|
||||
|
||||
optstring = _getopt_initialize(argc, argv, optstring);
|
||||
__getopt_initialized = 1;
|
||||
}
|
||||
|
||||
/* Test whether ARGV[optind] points to a non-option argument.
|
||||
Either it does not have option syntax, or there is an environment flag
|
||||
from the shell indicating it is not an option. The later information
|
||||
is only used when the used in the GNU libc. */
|
||||
#define NONOPTION_P (argv[optind][0] != '-' || argv[optind][1] == '\0')
|
||||
|
||||
if (nextchar == NULL || *nextchar == '\0')
|
||||
{
|
||||
/* Advance to the next ARGV-element. */
|
||||
|
||||
/* Give FIRST_NONOPT & LAST_NONOPT rational values if OPTIND has been
|
||||
moved back by the user (who may also have changed the arguments). */
|
||||
if (last_nonopt > optind)
|
||||
{
|
||||
last_nonopt = optind;
|
||||
}
|
||||
|
||||
if (first_nonopt > optind)
|
||||
{
|
||||
first_nonopt = optind;
|
||||
}
|
||||
|
||||
if (ordering == PERMUTE)
|
||||
{
|
||||
/* If we have just processed some options following some non-options,
|
||||
exchange them so that the options come first. */
|
||||
if (first_nonopt != last_nonopt && last_nonopt != optind)
|
||||
{
|
||||
exchange((char **) argv);
|
||||
}
|
||||
else if (last_nonopt != optind)
|
||||
{
|
||||
first_nonopt = optind;
|
||||
}
|
||||
|
||||
/* Skip any additional non-options
|
||||
and extend the range of non-options previously skipped. */
|
||||
|
||||
while (optind < argc && NONOPTION_P)
|
||||
{
|
||||
optind++;
|
||||
}
|
||||
|
||||
last_nonopt = optind;
|
||||
}
|
||||
|
||||
/* The special ARGV-element `--' means premature end of options.
|
||||
Skip it like a null option,
|
||||
then exchange with previous non-options as if it were an option,
|
||||
then skip everything else like a non-option. */
|
||||
|
||||
if (optind != argc && !strcmp(argv[optind], "--"))
|
||||
{
|
||||
optind++;
|
||||
|
||||
if (first_nonopt != last_nonopt && last_nonopt != optind)
|
||||
{
|
||||
exchange((char **) argv);
|
||||
}
|
||||
else if (first_nonopt == last_nonopt)
|
||||
{
|
||||
first_nonopt = optind;
|
||||
}
|
||||
|
||||
last_nonopt = argc;
|
||||
optind = argc;
|
||||
}
|
||||
|
||||
/* If we have done all the ARGV-elements, stop the scan
|
||||
and back over any non-options that we skipped and permuted. */
|
||||
|
||||
if (optind == argc)
|
||||
{
|
||||
/* Set the next-arg-index to point at the non-options
|
||||
that we previously skipped, so the caller will digest them. */
|
||||
if (first_nonopt != last_nonopt)
|
||||
{
|
||||
optind = first_nonopt;
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* If we have come to a non-option and did not permute it,
|
||||
either stop the scan or describe it to the caller and pass it by. */
|
||||
|
||||
if (NONOPTION_P)
|
||||
{
|
||||
if (ordering == REQUIRE_ORDER)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
optarg = argv[optind++];
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* We have found another option-ARGV-element.
|
||||
Skip the initial punctuation. */
|
||||
nextchar = (argv[optind] + 1
|
||||
+ (longopts != NULL && argv[optind][1] == '-'));
|
||||
}
|
||||
|
||||
/* Decode the current option-ARGV-element. */
|
||||
|
||||
/* Check whether the ARGV-element is a long option.
|
||||
|
||||
If long_only and the ARGV-element has the form "-f", where f is
|
||||
a valid short option, don't consider it an abbreviated form of
|
||||
a long option that starts with f. Otherwise there would be no
|
||||
way to give the -f short option.
|
||||
|
||||
On the other hand, if there's a long option "fubar" and
|
||||
the ARGV-element is "-fu", do consider that an abbreviation of
|
||||
the long option, just like "--fu", and not "-f" with arg "u".
|
||||
|
||||
This distinction seems to be the most useful approach. */
|
||||
|
||||
if (longopts != NULL
|
||||
&& (argv[optind][1] == '-'
|
||||
|| (long_only
|
||||
&& (argv[optind][2]
|
||||
|| !my_index(optstring, argv[optind][1])))))
|
||||
{
|
||||
char *nameend;
|
||||
const struct option *p;
|
||||
const struct option *pfound = NULL;
|
||||
int exact = 0;
|
||||
int ambig = 0;
|
||||
int indfound = -1;
|
||||
int option_index;
|
||||
|
||||
for (nameend = nextchar; *nameend && *nameend != '='; nameend++)
|
||||
/* Do nothing. */ ;
|
||||
|
||||
/* Test all long options for either exact match
|
||||
or abbreviated matches. */
|
||||
for (p = longopts, option_index = 0; p->name; p++, option_index++)
|
||||
if (!strncmp(p->name, nextchar, nameend - nextchar))
|
||||
{
|
||||
if ((unsigned int)(nameend - nextchar)
|
||||
== (unsigned int) strlen(p->name))
|
||||
{
|
||||
/* Exact match found. */
|
||||
pfound = p;
|
||||
indfound = option_index;
|
||||
exact = 1;
|
||||
break;
|
||||
}
|
||||
else if (pfound == NULL)
|
||||
{
|
||||
/* First nonexact match found. */
|
||||
pfound = p;
|
||||
indfound = option_index;
|
||||
}
|
||||
else
|
||||
/* Second or later nonexact match found. */
|
||||
{
|
||||
ambig = 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (ambig && !exact)
|
||||
{
|
||||
if (opterr)
|
||||
fprintf(stderr, _("%s: option `%s' is ambiguous\n"),
|
||||
argv[0], argv[optind]);
|
||||
|
||||
nextchar += strlen(nextchar);
|
||||
optind++;
|
||||
optopt = 0;
|
||||
return '?';
|
||||
}
|
||||
|
||||
if (pfound != NULL)
|
||||
{
|
||||
option_index = indfound;
|
||||
optind++;
|
||||
|
||||
if (*nameend)
|
||||
{
|
||||
/* Don't test has_arg with >, because some C compilers don't
|
||||
allow it to be used on enums. */
|
||||
if (pfound->has_arg)
|
||||
{
|
||||
optarg = nameend + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (opterr)
|
||||
if (argv[optind - 1][1] == '-')
|
||||
/* --option */
|
||||
fprintf(stderr,
|
||||
_
|
||||
("%s: option `--%s' doesn't allow an argument\n"),
|
||||
argv[0], pfound->name);
|
||||
else
|
||||
/* +option or -option */
|
||||
fprintf(stderr,
|
||||
_
|
||||
("%s: option `%c%s' doesn't allow an argument\n"),
|
||||
argv[0], argv[optind - 1][0], pfound->name);
|
||||
|
||||
nextchar += strlen(nextchar);
|
||||
optopt = pfound->val;
|
||||
return '?';
|
||||
}
|
||||
}
|
||||
else if (pfound->has_arg == 1)
|
||||
{
|
||||
if (optind < argc)
|
||||
{
|
||||
optarg = argv[optind++];
|
||||
}
|
||||
else
|
||||
{
|
||||
if (opterr)
|
||||
fprintf(stderr,
|
||||
_("%s: option `%s' requires an argument\n"),
|
||||
argv[0], argv[optind - 1]);
|
||||
|
||||
nextchar += strlen(nextchar);
|
||||
optopt = pfound->val;
|
||||
return optstring[0] == ':' ? ':' : '?';
|
||||
}
|
||||
}
|
||||
|
||||
nextchar += strlen(nextchar);
|
||||
|
||||
if (longind != NULL)
|
||||
{
|
||||
*longind = option_index;
|
||||
}
|
||||
|
||||
if (pfound->flag)
|
||||
{
|
||||
*(pfound->flag) = pfound->val;
|
||||
return 0;
|
||||
}
|
||||
|
||||
return pfound->val;
|
||||
}
|
||||
|
||||
/* Can't find it as a long option. If this is not getopt_long_only,
|
||||
or the option starts with '--' or is not a valid short
|
||||
option, then it's an error.
|
||||
Otherwise interpret it as a short option. */
|
||||
if (!long_only || argv[optind][1] == '-'
|
||||
|| my_index(optstring, *nextchar) == NULL)
|
||||
{
|
||||
if (opterr)
|
||||
{
|
||||
if (argv[optind][1] == '-')
|
||||
/* --option */
|
||||
fprintf(stderr, _("%s: unrecognized option `--%s'\n"),
|
||||
argv[0], nextchar);
|
||||
else
|
||||
/* +option or -option */
|
||||
fprintf(stderr, _("%s: unrecognized option `%c%s'\n"),
|
||||
argv[0], argv[optind][0], nextchar);
|
||||
}
|
||||
|
||||
nextchar = (char *) "";
|
||||
optind++;
|
||||
optopt = 0;
|
||||
return '?';
|
||||
}
|
||||
}
|
||||
|
||||
/* Look at and handle the next short option-character. */
|
||||
{
|
||||
char c = *nextchar++;
|
||||
char *temp = my_index(optstring, c);
|
||||
|
||||
/* Increment `optind' when we start to process its last character. */
|
||||
if (*nextchar == '\0')
|
||||
{
|
||||
++optind;
|
||||
}
|
||||
|
||||
if (temp == NULL || c == ':')
|
||||
{
|
||||
if (opterr)
|
||||
{
|
||||
if (posixly_correct)
|
||||
/* 1003.2 specifies the format of this message. */
|
||||
{
|
||||
fprintf(stderr, _("%s: illegal option -- %c\n"), argv[0], c);
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(stderr, _("%s: invalid option -- %c\n"), argv[0], c);
|
||||
}
|
||||
}
|
||||
|
||||
optopt = c;
|
||||
return '?';
|
||||
}
|
||||
|
||||
/* Convenience. Treat POSIX -W foo same as long option --foo */
|
||||
if (temp[0] == 'W' && temp[1] == ';')
|
||||
{
|
||||
char *nameend;
|
||||
const struct option *p;
|
||||
const struct option *pfound = NULL;
|
||||
int exact = 0;
|
||||
int ambig = 0;
|
||||
int indfound = 0;
|
||||
int option_index;
|
||||
|
||||
/* This is an option that requires an argument. */
|
||||
if (*nextchar != '\0')
|
||||
{
|
||||
optarg = nextchar;
|
||||
/* If we end this ARGV-element by taking the rest as an arg,
|
||||
we must advance to the next element now. */
|
||||
optind++;
|
||||
}
|
||||
else if (optind == argc)
|
||||
{
|
||||
if (opterr)
|
||||
{
|
||||
/* 1003.2 specifies the format of this message. */
|
||||
fprintf(stderr, _("%s: option requires an argument -- %c\n"),
|
||||
argv[0], c);
|
||||
}
|
||||
|
||||
optopt = c;
|
||||
|
||||
if (optstring[0] == ':')
|
||||
{
|
||||
c = ':';
|
||||
}
|
||||
else
|
||||
{
|
||||
c = '?';
|
||||
}
|
||||
|
||||
return c;
|
||||
}
|
||||
else
|
||||
/* We already incremented `optind' once;
|
||||
increment it again when taking next ARGV-elt as argument. */
|
||||
{
|
||||
optarg = argv[optind++];
|
||||
}
|
||||
|
||||
/* optarg is now the argument, see if it's in the
|
||||
table of longopts. */
|
||||
|
||||
for (nextchar = nameend = optarg; *nameend && *nameend != '=';
|
||||
nameend++)
|
||||
/* Do nothing. */ ;
|
||||
|
||||
/* Test all long options for either exact match
|
||||
or abbreviated matches. */
|
||||
for (p = longopts, option_index = 0; p->name; p++, option_index++)
|
||||
if (!strncmp(p->name, nextchar, nameend - nextchar))
|
||||
{
|
||||
if ((unsigned int)(nameend - nextchar) == strlen(p->name))
|
||||
{
|
||||
/* Exact match found. */
|
||||
pfound = p;
|
||||
indfound = option_index;
|
||||
exact = 1;
|
||||
break;
|
||||
}
|
||||
else if (pfound == NULL)
|
||||
{
|
||||
/* First nonexact match found. */
|
||||
pfound = p;
|
||||
indfound = option_index;
|
||||
}
|
||||
else
|
||||
/* Second or later nonexact match found. */
|
||||
{
|
||||
ambig = 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (ambig && !exact)
|
||||
{
|
||||
if (opterr)
|
||||
fprintf(stderr, _("%s: option `-W %s' is ambiguous\n"),
|
||||
argv[0], argv[optind]);
|
||||
|
||||
nextchar += strlen(nextchar);
|
||||
optind++;
|
||||
return '?';
|
||||
}
|
||||
|
||||
if (pfound != NULL)
|
||||
{
|
||||
option_index = indfound;
|
||||
|
||||
if (*nameend)
|
||||
{
|
||||
/* Don't test has_arg with >, because some C compilers don't
|
||||
allow it to be used on enums. */
|
||||
if (pfound->has_arg)
|
||||
{
|
||||
optarg = nameend + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (opterr)
|
||||
fprintf(stderr, _("\
|
||||
%s: option `-W %s' doesn't allow an argument\n"), argv[0], pfound->name);
|
||||
|
||||
nextchar += strlen(nextchar);
|
||||
return '?';
|
||||
}
|
||||
}
|
||||
else if (pfound->has_arg == 1)
|
||||
{
|
||||
if (optind < argc)
|
||||
{
|
||||
optarg = argv[optind++];
|
||||
}
|
||||
else
|
||||
{
|
||||
if (opterr)
|
||||
fprintf(stderr,
|
||||
_("%s: option `%s' requires an argument\n"),
|
||||
argv[0], argv[optind - 1]);
|
||||
|
||||
nextchar += strlen(nextchar);
|
||||
return optstring[0] == ':' ? ':' : '?';
|
||||
}
|
||||
}
|
||||
|
||||
nextchar += strlen(nextchar);
|
||||
|
||||
if (longind != NULL)
|
||||
{
|
||||
*longind = option_index;
|
||||
}
|
||||
|
||||
if (pfound->flag)
|
||||
{
|
||||
*(pfound->flag) = pfound->val;
|
||||
return 0;
|
||||
}
|
||||
|
||||
return pfound->val;
|
||||
}
|
||||
|
||||
nextchar = NULL;
|
||||
return 'W'; /* Let the application handle it. */
|
||||
}
|
||||
|
||||
if (temp[1] == ':')
|
||||
{
|
||||
if (temp[2] == ':')
|
||||
{
|
||||
/* This is an option that accepts an argument optionally. */
|
||||
if (*nextchar != '\0')
|
||||
{
|
||||
optarg = nextchar;
|
||||
optind++;
|
||||
}
|
||||
else
|
||||
{
|
||||
optarg = NULL;
|
||||
}
|
||||
|
||||
nextchar = NULL;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* This is an option that requires an argument. */
|
||||
if (*nextchar != '\0')
|
||||
{
|
||||
optarg = nextchar;
|
||||
/* If we end this ARGV-element by taking the rest as an arg,
|
||||
we must advance to the next element now. */
|
||||
optind++;
|
||||
}
|
||||
else if (optind == argc)
|
||||
{
|
||||
if (opterr)
|
||||
{
|
||||
/* 1003.2 specifies the format of this message. */
|
||||
fprintf(stderr,
|
||||
_("%s: option requires an argument -- %c\n"),
|
||||
argv[0], c);
|
||||
}
|
||||
|
||||
optopt = c;
|
||||
|
||||
if (optstring[0] == ':')
|
||||
{
|
||||
c = ':';
|
||||
}
|
||||
else
|
||||
{
|
||||
c = '?';
|
||||
}
|
||||
}
|
||||
else
|
||||
/* We already incremented `optind' once;
|
||||
increment it again when taking next ARGV-elt as argument. */
|
||||
{
|
||||
optarg = argv[optind++];
|
||||
}
|
||||
|
||||
nextchar = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
return c;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
getopt(int argc,
|
||||
char *const *argv,
|
||||
const char *optstring)
|
||||
{
|
||||
return _getopt_internal(argc, argv, optstring,
|
||||
(const struct option *) 0, (int *) 0, 0);
|
||||
}
|
||||
|
||||
int
|
||||
getopt_long(int argc,
|
||||
char *const *argv,
|
||||
const char *optstring,
|
||||
const struct option *longopts,
|
||||
int *longind)
|
||||
{
|
||||
return _getopt_internal(argc, argv, optstring,
|
||||
longopts, longind, 0);
|
||||
}
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
struct example_t
|
||||
{
|
||||
int64_t a;
|
||||
int32_t b;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
#include "stdafx.h"
|
||||
#include <lcm/lcm.h>
|
||||
extern "C" {
|
||||
#include <example_t.h>
|
||||
}
|
||||
|
||||
#if defined(OS_WINDOWS)
|
||||
#include <windows.h>
|
||||
|
||||
#define sleep(n) Sleep((n)*1000)
|
||||
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
#endif /* OS_LINUX || OS_ANDROID || OS_QNX7 */
|
||||
|
||||
static bool recvFlag = false;
|
||||
|
||||
static void my_handler(const lcm_recv_buf_t *rbuf, const char *channel,
|
||||
const example_t *msg, void *user) {
|
||||
if (msg->a == 12 && msg->b == 56) {
|
||||
recvFlag = true;
|
||||
}
|
||||
|
||||
printf("a=%lld,b=%d\n", (long long)msg->a, msg->b);
|
||||
(void)(user);
|
||||
(void)(channel);
|
||||
(void)(rbuf);
|
||||
}
|
||||
|
||||
TEST(lcm_lite, c_api) {
|
||||
lcm_t *lcm = lcm_create(NULL);
|
||||
ASSERT_NE(lcm, (void *)NULL);
|
||||
example_t example = {12, 56};
|
||||
|
||||
for (int i = 0; i < 5; i++) {
|
||||
example_t_publish(lcm, "test", &example);
|
||||
printf("send\n");
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
example_t_subscribe(lcm, "test", my_handler, NULL);
|
||||
|
||||
for (int i = 0; i < 5; i++) {
|
||||
EXPECT_EQ(lcm_handle(lcm), 0);
|
||||
}
|
||||
|
||||
ASSERT_TRUE(recvFlag);
|
||||
lcm_destroy(lcm);
|
||||
}
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
#include "stdafx.h"
|
||||
#include <example_t.hpp>
|
||||
#include <lcm/lcm-cpp.hpp>
|
||||
|
||||
#if defined(OS_WINDOWS)
|
||||
#include <windows.h>
|
||||
|
||||
#define sleep(n) Sleep((n)*1000)
|
||||
|
||||
#elif defined(OS_LINUX) || defined(OS_ANDROID) || defined(OS_QNX7)
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
#endif /* OS_LINUX || OS_ANDROID || OS_QNX7 */
|
||||
|
||||
class Handler {
|
||||
public:
|
||||
bool m_ok;
|
||||
|
||||
public:
|
||||
Handler() : m_ok(false) {}
|
||||
~Handler() {}
|
||||
|
||||
void handleMessage(const lcm::ReceiveBuffer *rbuf, const std::string &chan,
|
||||
const example_t *msg) {
|
||||
if (msg->a == 12 && msg->b == 56) {
|
||||
m_ok = true;
|
||||
}
|
||||
|
||||
printf("a=%lld,b=%d\n", (long long)msg->a, msg->b);
|
||||
(void)(chan);
|
||||
(void)(rbuf);
|
||||
}
|
||||
};
|
||||
|
||||
TEST(lcm_lite, cpp_api) {
|
||||
lcm::LCM lcm;
|
||||
ASSERT_TRUE(lcm.good());
|
||||
example_t data;
|
||||
data.a = 12;
|
||||
data.b = 56;
|
||||
|
||||
for (int i = 0; i < 5; i++) {
|
||||
lcm.publish("test", &data);
|
||||
printf("send\n");
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
Handler handlerObject;
|
||||
lcm.subscribe("test", &Handler::handleMessage, &handlerObject);
|
||||
|
||||
for (int i = 0; i < 5; i++) {
|
||||
EXPECT_EQ(lcm.handle(), 0);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
#include "stdafx.h"
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdio.h>
|
||||
#include <gtest.h>
|
||||
#include <stdlib.h>
|
||||
Loading…
Reference in New Issue