TEST(LCM_C, InvalidCreation) {
lcm_t* lcm = lcm_create("udpm://asdf");
EXPECT_EQ(NULL, lcm);
lcm = lcm_create("udpm://0.0.0.0");
EXPECT_EQ(NULL, lcm);
lcm = lcm_create("udpm://239.255.1.1:65536");
EXPECT_EQ(NULL, lcm);
}
int main(int argc, char** argv)
{
lcm_t* lcm = lcm_create(NULL);
srand(0);
int num_messages = 10000;
for(int i=0; i<num_messages; i++) {
int data_sz;
// if(i < num_messages / 2) {
// data_sz = rand() % 1000000;
// } else {
data_sz = 1200;
// }
char* data = (char*) calloc(1, data_sz);
snprintf(data, data_sz, "%d", i);
lcm_publish(lcm, "BUFTEST", data, 80);
printf("transmitted msg # %5d size %d\n", i, data_sz);
g_usleep(1000);
free(data);
}
return 0;
}
int main(int argc, char *argv[]) {
var_getted=0;
angle_pitch = 0;
angle_roll = 0;
angle_yaw = 0;
pthread_t p_acithread;
fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
struct termios port_settings; // structure to store the port settings in
cfsetispeed(&port_settings, B57600); // set baud rates
port_settings.c_cflag = B57600 | CS8 | CREAD | CLOCAL;
port_settings.c_iflag = IGNPAR;
port_settings.c_oflag = 0;
port_settings.c_lflag = 0;
tcsetattr(fd, TCSANOW, &port_settings); // apply the settings to the port
lcm = lcm_create(NULL);
if(!lcm) return 1;
aciInit();
aciSetSendDataCallback(&transmit);
aciSetVarListUpdateFinishedCallback(&varListUpdateFinished);
aciSetEngineRate(100, 10);
pthread_create( &p_acithread, NULL, aciThread, NULL);
aciGetDeviceVariablesList();
pthread_join( p_acithread, NULL);
lcm_destroy(lcm);
}
int main(){
setvbuf (stdout, (char *) NULL, _IONBF, 0);
state_t *state = calloc (1, sizeof *state);
state->pose = (pose_xyt_t*)calloc(1,sizeof(pose_xyt_t));
state->scan_pose = (scanpose_xyt_t*)calloc(1,sizeof(scanpose_xyt_t));
memset(&state->t_filt_mvag,0,sizeof(mvag_filter_t));
state->t_filt_mvag.window = 0;
state->lcm = lcm_create (NULL);
pose_xyt_t_subscribe (state->lcm,
"BOTLAB_ODOMETRY",
pose_xyt_handler, state);
rplidar_laser_t_subscribe (state->lcm,
"RPLIDAR_LASER",
rplidar_laser_handler, state);
while (1){
lcm_handle (state->lcm);
}
}
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
const int16_T *cmode = (const int16_T*) ssGetInputPortSignal(S,0);
const int16_T *count = (const int16_T*) ssGetInputPortSignal(S,1);
const real_T *traject = (const real_T*) ssGetInputPortSignal(S,2);
int i;
lcm_t *lcm = lcm_create("udpm://239.255.76.67:7667?ttl=25");
if(!lcm)
printf("NOTLCM");
if(traject[0]==0)
goto END;
if(!lcm)
goto END;
fau_coordinate_t my_data;
my_data.mode = *cmode;
my_data.count = *count;
for(i = 0; i < 3; i++){
my_data.traject[i*3] = traject[i];
my_data.traject[i*3+1] = traject[i+3];
my_data.traject[i*3+2] = traject[i+6];
}
fau_coordinate_t_publish(lcm,"goto",&my_data);
END:
lcm_destroy(lcm);
}
int main(int argc, char ** argv)
{
double xyz0[3] = { -20, -20, 0 };
double xyz1[3] = { 20, 20, 5 };
double mpp[3] = { .2, .2, .2 };
occ_map::FloatVoxelMap fvm(xyz0, xyz1, mpp, 0);
double ixyz[3];
for (ixyz[2] = -5; ixyz[2] < 10; ixyz[2]+=.2) {
for (ixyz[1] = -5; ixyz[1] < 10; ixyz[1]+=.2) {
for (ixyz[0] = .5; ixyz[0] < 1; ixyz[0]+=.2) {
fvm.writeValue(ixyz,0.99);
}
}
}
double xyzO[3] = { 0, 0, 0 };
double xyzR[3] = { 0, 5, 2 };
for (double x = -5; x < 5; x += .5) {
xyzR[0] = x;
fvm.raytrace(xyzO, xyzR, 1, .3);
}
#ifndef NO_LCM
const occ_map_voxel_map_t * msg = fvm.get_voxel_map_t(0);
lcm_t * lcm = lcm_create(NULL);
occ_map_voxel_map_t_publish(lcm, "VOXEL_MAP",msg);
#endif
}
int main()
{
lcm = lcm_create(NULL);
channels_t tx_msg;
channels_t_subscribe(lcm, "CHANNELS_.*_RX", handler_channels_lcm, rxdata);
for(;;) {
lcm_handle(lcm);
//do transformation of incoming data here
for(int i=0; i<8; i++){
txdata[i] = rxdata[i]-500;
}
//make tx msg
tx_msg.utime = utime_now();
tx_msg.num_channels = 8;
for(int i=0; i < tx_msg.num_channels; i++){
tx_msg.channels[i] = txdata[i];
}
//pblish transmint msg
channels_t_publish(lcm, "CHANNELS_1_TX", &tx_msg);
}
lcm_destroy(lcm);
return 0;
}
/**
* Creates and initializes an LCM userdata.
*
* Optionally takes one argument, a string, containing the LCM provider. If no
* provider is supplied, the LCM userdata is created using the environment
* variable LCM_DEFAULT_URL if it is defined, or the default
* "udpm://239.255.76.67:7667".
*
* @see lcm_create
*
* @pre The Lua arguments on the stack:
* A string for the provider.
*
* @post The Lua return values on the stack:
* The new LCM userdata.
*
* @param L The Lua state.
* @return The number of return values on the Lua stack.
*
* @throws Lua error if LCM userdata cannot be created.
*/
static int impl_lcm_new(lua_State * L) {
/* we expect 1 argument */
lua_settop(L, 1);
/* check for a string containing the provider */
const char * provider = NULL;
if(!lua_isnil(L, 1)) {
/* if there was no argument, the stack will have nil */
provider = luaL_checkstring(L, 1);
}
/* open the lcm connection */
lcm_t * lc = lcm_create(provider);
/* check for failure */
if(!lc) {
lua_pushstring(L, "error lcm create");
lua_error(L);
}
/* create lcm userdata */
impl_lcm_userdata_t * lcmu = impl_lcm_newuserdata(L);
lcmu->lcm = lc;
/* create a subscription table */
impl_lcm_createsubscriptiontable(L, -1);
/* return lcm userdata, which is on top of the stack */
return 1;
}
int main(int argc, char ** argv){
double xyz0[2] = {-20, -20};
double xyz1[2] = {20, 20};
double mpp = .2;
occ_map::FloatPixelMap fvm(xyz0,xyz1,mpp,0);
double xy[2];
for (xy[1] = -5; xy[1] < 10; xy[1] += 0.2) {
for (xy[0] = -5; xy[0] < 5; xy[0] += 0.2) {
fvm.writeValue (xy, 0.99);
}
}
double xy0[2] = {0, 0};
double xyR[2] = {0, 5};
for (double x = -5; x < 5; x += 0.5) {
xyR[0] = x;
fvm.rayTrace (xy0, xyR, 1, 0.3);
}
#ifndef NO_LCM
const occ_map_pixel_map_t *msg = fvm.get_pixel_map_t (0);
lcm_t * lcm = lcm_create (NULL);
occ_map_pixel_map_t_publish (lcm, "PIXEL_MAP", msg);
#endif
}
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
if (nrhs < 1) {
mexPrintf("Usage: publishLCMLog(lcm_log)\n");
return;
}
if (!lcm) lcm = lcm_create(NULL);
if (!lcm) mexErrMsgTxt("failed to create lcm node");
char* channel;
mxArray* data;
int channel_field_number = mxGetFieldNumber(prhs[0], "channel"),
data_field_number = mxGetFieldNumber(prhs[0], "data");
if (channel_field_number < 0 || data_field_number < 0)
mexErrMsgTxt(
"publishLCMLog failed: input must be a structure with fields 'channel' "
"and 'data'");
for (size_t i = 0; i < mxGetNumberOfElements(prhs[0]); i++) {
channel =
mxArrayToString(mxGetFieldByNumber(prhs[0], i, channel_field_number));
data = mxGetFieldByNumber(prhs[0], i, data_field_number);
lcm_publish(lcm, channel, mxGetData(data), mxGetNumberOfElements(data));
mxFree(channel);
}
}
int main()
{
lcm_t * lcm = lcm_create(NULL);
BotParam * param = bot_param_new_from_server(lcm, 1);
if (param == NULL) {
fprintf(stderr, "could not get params!\n");
exit(1);
}
bot_param_add_update_subscriber(param,param_update_handler,lcm);
char * s;
int ret = bot_param_write_to_string(param, &s);
fprintf(stderr, "%s", s);
free(s);
// double foo = bot_param_get_double_or_fail(param, "foo");
// double bar = bot_param_get_double_or_fail(param, "bar");
// printf("foo=%f, bar = %f\n", foo, bar);
bot_param_write(param, stderr);
while (1) {
lcm_handle(lcm);
char * key = "coordinate_frames.body.history";
fprintf(stderr, "%s = %d\n", key, bot_param_get_int_or_fail(param, key));
}
return 0;
}
int
main(int argc, char ** argv)
{
state_t* state = calloc(1, sizeof(state_t));
state->lcm = lcm_create(NULL);
if(!state->lcm)
return 1;
// subscribe to messages
exlcm_example_t_subscribe(state->lcm, "EXAMPLE", &on_example_msg, state);
// attach LCM to the GLib event loop
state->mainloop = g_main_loop_new(NULL, TRUE);
GIOChannel* ioc = g_io_channel_unix_new(lcm_get_fileno(state->lcm));
guint sid = g_io_add_watch(ioc, G_IO_IN, (GIOFunc)on_lcm, state);
// add a periodic timer to call a function every 1000 ms.
g_timeout_add(1000, on_timeout, state);
// start the GLib event loop running
g_main_loop_run(state->mainloop);
lcm_destroy(state->lcm);
free(state);
return 0;
}
lcm_t *
bot_lcm_get_global(const char *provider)
{
g_static_mutex_lock (&lcm_glib_sources_mutex);
if(!global_lcm)
global_lcm = lcm_create(provider);
g_static_mutex_unlock (&lcm_glib_sources_mutex);
return global_lcm;
}
virtual void SetUp() {
// start up lcm
lcm_ = lcm_create ("udpm://239.255.76.67:7667?ttl=0");
param_ = bot_param_new_from_server(lcm_, 0);
converter_ = new ServoConverter(param_);
}
int main(int argc, char *argv[]) {
var_getted = 0;
cmd_ready = 0;
cmmd_msg_get = 0;
lcm = lcm_create(NULL);
if(!lcm){
printf("lcm initialize failed!\n");
return 1;
}
pthread_t p_acithread, p_subscribeDMCCmmdThread, p_sendOutDMCcmmdThread, p_publishVarThread, p_fetchDataThread;
fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
struct termios port_settings; // structure to store the port settings in
cfsetispeed(&port_settings, B57600); // set baud rates
port_settings.c_cflag = B57600 | CS8 | CREAD | CLOCAL;
port_settings.c_iflag = IGNPAR;
port_settings.c_oflag = 0;
port_settings.c_lflag = 0;
tcsetattr(fd, TCSANOW, &port_settings); // apply the settings to the port
aciInit();
aciSetSendDataCallback(&transmit);
aciSetVarListUpdateFinishedCallback(&varListUpdateFinished);
aciSetCmdListUpdateFinishedCallback(&cmdListUpdateFinished);
aciSetEngineRate(100, 10);
//start running acithread
pthread_create(&p_acithread, NULL, aciThread, NULL);
aciGetDeviceVariablesList();
printf("Waiting for variable list.. \n");
while(!var_getted) usleep(1000);
// start fetching data and publish via LCM
pthread_create(&p_fetchDataThread, NULL, fetchDataThread, NULL);
aciGetDeviceCommandsList();
printf("Waiting for command list...\n");
while(!cmd_ready) usleep(1000);
pthread_create(&p_subscribeDMCCmmdThread, NULL, subscribeDMCCmmdThread, NULL);
pthread_create(&p_sendOutDMCcmmdThread, NULL, sendOutDMCcmmdThread, NULL);
pthread_join(p_acithread, NULL);
pthread_join(p_fetchDataThread, NULL);
pthread_join(p_subscribeDMCCmmdThread, NULL);
pthread_join(p_sendOutDMCcmmdThread, NULL);
}
int main(int argc, char *argv[]) {
pthread_t p_viconThread, p_sendCmmdThread, p_publishCmmdThread, p_fetchDataThread;
pthread_t p_acithread;
fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
struct termios port_settings; // structure to store the port settings in
cfsetispeed(&port_settings, B57600); // set baud rates
port_settings.c_cflag = B57600 | CS8 | CREAD | CLOCAL;
port_settings.c_iflag = IGNPAR;
port_settings.c_oflag = 0;
port_settings.c_lflag = 0;
tcsetattr(fd, TCSANOW, &port_settings); // apply the settings to the port
aciInit();
aciSetSendDataCallback(&transmit);
aciSetVarListUpdateFinishedCallback(&varListUpdateFinished);
aciSetCmdListUpdateFinishedCallback(&cmdListUpdateFinished);
aciSetParamListUpdateFinishedCallback(¶mListUpdateFinished);
aciSetEngineRate(100, 10);
lcm = lcm_create(NULL);
if(!lcm)
return 1;
pthread_create(&p_acithread, NULL, aciThread, NULL);
aciGetDeviceVariablesList();
while(!var_getted) usleep(1000);
aciGetDeviceCommandsList();
printf("Waiting for command list...\n");
while(!cmd_ready) usleep(1000);
printf("starting motors!\n");
//turn on motors()
startMotors();
usleep(1000000);
//start getting data from Vicon
pthread_create(&p_viconThread, NULL, viconStateThread, NULL);
pthread_create(&p_sendCmmdThread, NULL, sendCmmdThread, NULL);
// pthread_create(&p_fetchDataThread, NULL, fetchDataThread, NULL);
pthread_create(&p_publishCmmdThread, NULL, publishCmmdThread, NULL);
pthread_join(p_viconThread, NULL);
pthread_join(p_sendCmmdThread, NULL);
// pthread_join(p_fetchDataThread, NULL);
pthread_join(p_publishCmmdThread, NULL);
pthread_join(p_acithread, NULL);
}
/*
LCM processing (top-level loop) -- Only use to talk with BLOCKS
*/
void *lcm_thread_loop(void *data){
printf("lcm running");
lcm = lcm_create(NULL);
channels_t_subscribe(lcm, "CHANNELS_1_RX", channels_handler, lcm);
waypoint_trigger_t_subscribe(lcm, "WP_TRIGGER", wp_trigger_handler, lcm);
while(1){
lcm_handle(lcm);
usleep(10);
}
lcm_destroy(lcm);
return 0;
}
virtual void SetUp() {
// start up lcm
lcm_ = lcm_create ("udpm://239.255.76.67:7667?ttl=0");
param_ = bot_param_new_from_server(lcm_, 0);
bot_frames_ = bot_frames_new(lcm_, param_);
bot_frames_get_trans(bot_frames_, "local", "opencvFrame", &global_to_camera_trans_);
bot_frames_get_trans(bot_frames_, "opencvFrame", "local", &camera_to_global_trans_);
}
int
main(int argc, char ** argv)
{
lcm_t * lcm = lcm_create(NULL);
if(!lcm)
return 1;
exlcm_example_t_subscribe(lcm, "EXAMPLE", &my_handler, NULL);
while(1)
lcm_handle(lcm);
lcm_destroy(lcm);
return 0;
}
int setup_lcm(const char* IP_adress)
/*****************************************************************************
* Input :
* Output :
* Function :
******************************************************************************/
{
lcm = lcm_create(IP_adress); // allocates and initializes an instance of lcm_t, specific IP
//lcm = lcm_create(NULL); // use just on local network
if(!lcm)
{
return 1; // creating fail
}
return 0; // succes
}
void *
diff_drive_thread (void *arg)
{
lcm_t *lcm = lcm_create (NULL);
uint64_t utime_start;
while(1) {
utime_start = utime_now ();
pthread_mutex_lock (&msg_mutex);
maebot_motor_command_t_publish (lcm, "MAEBOT_MOTOR_COMMAND", &msg);
pthread_mutex_unlock (&msg_mutex);
usleep (CMD_PRD - (utime_now() - utime_start));
}
return NULL;
}
int main(int argc, char* argv[]) {
time(&rawtime);
info = localtime(&rawtime);
strftime(state_fname, 50, "../data/state/state_%d%H%M%S.txt", info);
printf("%s\n",state_fname);
lcm_t* lcm = lcm_create(NULL);
state_t_subscribe(lcm, "state", state_handler, NULL);
while(1) {
lcm_handle(lcm);
}
lcm_destroy(lcm);
return 0;
}
int main(int argc,char** argv)
{
strcpy(lowbatcmd, "mplayer ");
strcat(lowbatcmd, argv[0]);
strcat(lowbatcmd, "-battery-low-sound.wav < /dev/null &");
printf("This program requires mplayer. If you don't have it, sudo apt-get install mplayer\n");
char *lcm_in = NULL;
if (argc!=2) {
usage();
exit(0);
}
lcm_in = argv[1];
lcm_out = argv[2];
lcm = lcm_create ("udpm://239.255.76.67:7667?ttl=0");
if (!lcm)
{
fprintf(stderr, "lcm_create for recieve failed. Quitting.\n");
return 1;
}
battery_status_sub = lcmt_battery_status_subscribe (lcm, lcm_in, &lcm_battery_status_handler, NULL);
struct timeval thisTime;
signal(SIGINT,sighandler);
printf("Listening to LCM: %s\n", lcm_in, lcm_out);
while (!stop)
{
// read the LCM channel
lcm_handle (lcm);
}
return 0;
}
int main(int argc, char* argv[]) {
time(&rawtime);
info = localtime(&rawtime);
strftime(accel_fname, 50, "../data/accel_%d%H%M%S.txt", info);
lcm_t* lcm = lcm_create(NULL);
accel_t_subscribe(lcm, "acceleration_wrong", accel_handler, NULL);
while(1) {
lcm_handle(lcm);
}
lcm_destroy(lcm);
return 0;
}
int
main (int argc, char *argv[])
{
getopt_t *gopt = getopt_create ();
getopt_add_bool (gopt, 'h', "help", 0, "Show this help screen");
getopt_add_string (gopt, 'd', "device", "/dev/ttyUSB0", "Device name");
getopt_add_int (gopt, 'b', "baud", "1000000", "Device baud rate");
getopt_add_int (gopt, 'n', "num_servos", "6", "Number of servos");
getopt_add_string (gopt, '\0', "status-channel", "ARM_STATUS2", "LCM status channel");
getopt_add_string (gopt, '\0', "command-channel", "ARM_COMMAND2", "LCM command channel");
if (!getopt_parse (gopt, argc, argv, 1) || getopt_get_bool (gopt, "help")) {
getopt_do_usage (gopt);
exit (-1);
}
arm_state_t *arm_state = arm_state_create (getopt_get_string (gopt, "device"),
getopt_get_int (gopt, "baud"),
getopt_get_int (gopt, "num_servos"));
// LCM Initialization
arm_state->lcm = lcm_create (NULL);
arm_state->command_channel = getopt_get_string (gopt, "command-channel");
arm_state->status_channel = getopt_get_string (gopt, "status-channel");
if (!arm_state->lcm)
return -1;
dynamixel_command_list_t_subscribe (arm_state->lcm,
arm_state->command_channel,
command_handler,
arm_state);
pthread_create (&arm_state->status_thread, NULL, status_loop, arm_state);
pthread_create (&arm_state->driver_thread, NULL, driver_loop, arm_state);
// Probably not needed, given how this operates
pthread_join (arm_state->status_thread, NULL);
pthread_join (arm_state->driver_thread, NULL);
// Cleanup
arm_state_destroy (arm_state);
getopt_destroy (gopt);
}
void * process_handler(void *user) {
static int64_t lastTime = 0;
lcm_t *lcmP = lcm_create(NULL);
if (!lcmP) exit (EXIT_FAILURE);
maebot_shared_state_t * state = user;
maebot_sensor_data_t * data = &state->sensorData;
maebot_processed_sensor_data_t * procData = &state->processedSensorData;
pthread_mutex_lock(&sensor_data_mutex);
if(data->utime != lastTime) {
maebot_processed_sensor_data_t_publish(lcmP,
"MAEBOT_PROCESSED_SENSOR_DATA", procData);
lastTime = data->utime;
}
pthread_mutex_unlock(&sensor_data_mutex);
lcm_destroy(lcmP);
return NULL;
}
static int
pylcm_initobj (PyObject *self, PyObject *args, PyObject *kwargs)
{
dbg(DBG_PYTHON, "%s %p\n", __FUNCTION__, self);
PyLCMObject *lcm_obj = (PyLCMObject *)self;
char *url = NULL;
if (!PyArg_ParseTuple (args, "|s", &url))
return -1;
lcm_obj->lcm = lcm_create (url);
if (! lcm_obj->lcm) {
PyErr_SetString (PyExc_RuntimeError, "Couldn't create LCM");
return -1;
}
lcm_obj->saved_thread_state = NULL;
return 0;
}
int
main (int argc, char *argv[])
{
// so that redirected stdout won't be insanely buffered.
setvbuf (stdout, (char *) NULL, _IONBF, 0);
lcm_t *lcm = lcm_create (NULL);
if (!lcm)
return EXIT_FAILURE;
maebot_sensor_data_t_subscribe (lcm,
"MAEBOT_SENSOR_DATA",
sensor_data_handler,
NULL);
while (1)
lcm_handle (lcm);
return EXIT_SUCCESS;
}
void *
diff_drive_thread (void *arg)
{
lcm_t *lcm = lcm_create (NULL);
uint64_t utime_start;
while(1) {
utime_start = utime_now ();
pthread_mutex_lock (&msg_mutex);
{
msg.utime = utime_now ();
maebot_diff_drive_t_publish (lcm, "MAEBOT_DIFF_DRIVE", &msg);
}
pthread_mutex_unlock (&msg_mutex);
usleep (CMD_PRD - (utime_now() - utime_start));
}
return NULL;
}
int main(int argc, char* argv[])
{
UNUSED(argc);
UNUSED(argv);
lcm_t * lcm;
lcm = lcm_create(NULL);
bot_lcmgl_t* lcmgl = bot_lcmgl_init(lcm, "lcmgl-example");
// bot_lcmgl_color3f(lcmgl, 0, 0, 1); // blue
double xyz[3] = {0.0, 0.0, 0.0};
bot_lcmgl_sphere(lcmgl, xyz, .1, 36, 36);
bot_lcmgl_switch_buffer(lcmgl);
bot_lcmgl_destroy(lcmgl);
return 0;
}
