static void
app_destroy(app_t *self)
{
delete self->ptr;
lcm_destroy(self->lcm);
free(self);
}
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;
}
/* 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[]) {
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(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;
}
inline
LCM::~LCM() {
for(int i=0, n=subscriptions.size(); i<n; i++) {
delete subscriptions[i];
}
if(this->lcm && this->owns_lcm) {
lcm_destroy(this->lcm);
}
}
void sighandler(int dum)
{
printf("\n\nclosing... ");
lcm_destroy (lcm_);
printf("done.\n");
exit(0);
}
static void
arm_state_destroy (arm_state_t *arm_state)
{
for (int i = 0; i < arm_state->num_servos; i++) {
arm_state->servos[i]->destroy (arm_state->servos[i]);
}
arm_state->bus->destroy (arm_state->bus);
free (arm_state->servos);
lcm_destroy (arm_state->lcm);
free (arm_state);
}
static void
pylcm_dealloc (PyLCMObject *lcm_obj)
{
dbg(DBG_PYTHON, "pylcm_dealloc\n");
if (lcm_obj->lcm) {
lcm_destroy (lcm_obj->lcm);
lcm_obj->lcm = NULL;
}
Py_DECREF (lcm_obj->all_handlers);
Py_TYPE (lcm_obj)->tp_free ((PyObject*)lcm_obj);
}
/*
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;
}
void sighandler(int dum)
{
printf("\nClosing... ");
lcmt_battery_status_unsubscribe (lcm, battery_status_sub);
lcm_destroy (lcm);
printf("done.\n");
exit(0);
}
// ========================== main
int
main(int argc, char ** argv)
{
srand(time(NULL));
g_lcm = lcm_create(NULL);
if(!g_lcm)
{
info("Unable to initialize LCM\n");
return 1;
}
if(!do_echo_test())
goto failed;
if(!do_lcmtest_primitives_t_test())
goto failed;
if(!do_lcmtest_primitives_list_t_test())
goto failed;
if(!do_lcmtest_node_t_test())
goto failed;
if(!do_lcmtest_multidim_array_t_test())
goto failed;
if(!do_lcmtest2_cross_package_t_test())
goto failed;
info("All tests passed.\n");
lcm_destroy(g_lcm);
return 0;
failed:
info("failed\n");
lcm_destroy(g_lcm);
return 1;
}
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;
}
void sighandler(int dum)
{
stop=1;
fprintf(stderr,"Closing.\n");
if (handle_in) {
snd_rawmidi_drain(handle_in);
snd_rawmidi_close(handle_in);
}
lcm_destroy (lcm);
exit(0);
}
void * thread1Run()
/*****************************************************************************
* Input :
* Output :
* Function :
******************************************************************************/
{
exlcm_sync_t_subscribe(lcm, SYNC_CHANNEL, &my_handler, NULL);
while(1)
lcm_handle(lcm);
lcm_destroy(lcm);
}
/**
* Cleans up the LCM userdata. This is the __gc metamethod of the LCM userdata.
* This method is called automatically by the Lua garbage collector.
*
* Automatically unsubscribes all channels.
*
* @see lcm_destroy
*
* @pre The Lua arguments on the stack:
* A LCM userdata (self).
*
* @post The Lua return values on the stack:
* Nothing.
*
* @param L The Lua state.
* @return The number of return values on the Lua stack.
*/
static int impl_lcm_gc(lua_State * L) {
/* we expect 1 argument */
lua_settop(L, 1);
/* get the lcm userdata */
impl_lcm_userdata_t * lcmu = impl_lcm_checkuserdata(L, 1);
/* check if this userdata was ever created */
if(!lcmu->lcm) {
return 0;
}
/* get subscription table */
impl_lcm_getsubscriptiontable(L, 1);
/* subscription table traversal */
lua_pushnil(L); /* first key */
while(lua_next(L, 2) != 0) {
/* get the subscription userdata */
lua_pushstring(L, "userdata");
lua_rawget(L, -2);
impl_sub_userdata_t * subu =
(impl_sub_userdata_t *) lua_touserdata(L, -1);
/* unsubscribe */
if(lcm_unsubscribe(lcmu->lcm, subu->subscription) != 0) {
lua_pushstring(L, "error lcm unsubscribe");
lua_error(L);
}
/* pop the userdata and subscription table entry */
lua_pop(L, 2);
}
/* free lcm */
lcm_destroy(lcmu->lcm);
/* clear out the subscription table */
impl_lcm_createsubscriptiontable(L, 1);
return 0;
}
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[]) {
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;
}
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;
}
int main(int argc, char ** argv)
/*****************************************************************************
* Input :
* Output :
* Function :
******************************************************************************/
{
exlcm_sync_t my_data;
setup(); // setup LCM
while(1)
{
my_data = construct_msg(BEACON_NODE,ALL_NODES,BEACONSENDER,(double)get_localrealtime().tv_sec + (double)(get_localrealtime().tv_usec / (double)1000000) ); // define send struct
exlcm_sync_t_publish(lcm, SYNC_CHANNEL, &my_data); // publish the struct
printf("--------MESSAGE PUBLISH:-------\n NUMBER OF PACKETS = %lld\n TIMESTAMP IN SEC = %f\n"
, my_data.number_of_packets, my_data.timestamp);
sleep(5);
}
lcm_destroy(lcm); // terminate the allocated memory for lcm
return 0;
};
int
main(int argc, char ** argv)
{
lcm_t * lcm;
lcm = lcm_create(NULL);
if(!lcm)
return 1;
exlcm_example_t_subscription_t * sub =
exlcm_example_t_subscribe(lcm, "EXAMPLE", &my_handler, NULL);
while(1) {
// setup the LCM file descriptor for waiting.
int lcm_fd = lcm_get_fileno(lcm);
fd_set fds;
FD_ZERO(&fds);
FD_SET(lcm_fd, &fds);
// wait a limited amount of time for an incoming message
struct timeval timeout = {
1, // seconds
0 // microseconds
};
int status = select(lcm_fd + 1, &fds, 0, 0, &timeout);
if(0 == status) {
// no messages
printf("waiting for message\n");
} else if(FD_ISSET(lcm_fd, &fds)) {
// LCM has events ready to be processed.
lcm_handle(lcm);
}
}
exlcm_example_t_unsubscribe(lcm, sub);
lcm_destroy(lcm);
return 0;
}
int main(int argc, char* argv[]) {
time(&rawtime);
info = localtime(&rawtime);
strftime(vicon_state_fname, 50, "../data/vicon_state_%d%H%M%S.txt", info);
strftime(accel_fname, 50, "../data/accel_%d%H%M%S.txt", info);
strftime(cmmd_fname, 50, "../data/cmmd_%d%H%M%S.txt", info);
lcm_t* lcm = lcm_create(NULL);
vicon_state_t_subscribe(lcm, "vicon_state", vicon_state_handler, NULL);
accel_t_subscribe(lcm, "acceleration", accel_handler, NULL);
command_t_subscribe(lcm, "command_msg", cmmd_handler, NULL);
while(1) {
lcm_handle(lcm);
}
lcm_destroy(lcm);
return 0;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "lcmtoros");
// Handling Program options
static GOptionEntry entries[] =
{
{ "lcmurl", 'l', 0, G_OPTION_ARG_STRING, &lcmurl, "LCM Url to connect to", "udpm://" },
{ "verbose", 'v', 0, G_OPTION_ARG_NONE, &verbose, "Be verbose", NULL },
{ NULL, 0, 0, G_OPTION_ARG_NONE, NULL, NULL, 0 }
};
GError *error = NULL;
GOptionContext *context;
context = g_option_context_new ("- translate MAVLink messages from LCM to ROS (Topic: mavlink)");
g_option_context_add_main_entries (context, entries, NULL);
//g_option_context_add_group (context, NULL);
if (!g_option_context_parse (context, &argc, &argv, &error))
{
g_print ("Option parsing failed: %s\n", error->message);
exit (1);
}
ros::NodeHandle mavlink_nh;
mavlink_pub = mavlink_nh.advertise<lcm_mavlink_ros::Mavlink> ("/fromMAVLINK", 1000);
ros::NodeHandle attitude_nh;
attitude_pub = attitude_nh.advertise<sensor_msgs::Imu>("/fromMAVLINK/Imu", 1000);
/**
* Connect to LCM Channel and register for MAVLink messages
*/
lcm_t* lcm = lcm_create(lcmurl.c_str());
if (!lcm) {
return 1;
}
mavlink_message_t_subscription_t * comm_sub = mavlink_message_t_subscribe(
lcm, "MAVLINK", &mavlink_handler, NULL);
while (ros::ok())
lcm_handle(lcm);
/*
// Initialize LCM receiver thread
GThread* lcm_thread;
GError* err;
if (!g_thread_supported()) {
g_thread_init( NULL);
// Only initialize g thread if not already done
}
if ((lcm_thread = g_thread_create((GThreadFunc) lcm_wait, (void *) lcm,
TRUE, &err)) == NULL) {
printf("Thread create failed: %s!!\n", err->message);
g_error_free ( err);
}*/
//ros::Rate loop_rate(1);
/**
* Now just wait until the process is terminated...
*/
//while (ros::ok()) {
// loop_rate.sleep();
//}
mavlink_message_t_unsubscribe (lcm, comm_sub);
lcm_destroy (lcm);
// if (verbose) printf("Trying GThread Join");
// g_thread_join(lcm_thread);
return 0;
}
lcm_t *
lcm_create (const char *url)
{
if (!g_thread_supported ()) g_thread_init (NULL);
#ifdef WIN32
WSADATA wsd;
int status = WSAStartup ( MAKEWORD ( 2, 0 ), &wsd );
if ( status )
{
return NULL;
}
#endif
char * provider_str = NULL;
char * network = NULL;
GHashTable * args = g_hash_table_new_full (g_str_hash, g_str_equal,
free, free);
GPtrArray * providers = g_ptr_array_new ();
lcm_t *lcm = NULL;
// initialize the list of providers
lcm_udpm_provider_init (providers);
lcm_logprov_provider_init (providers);
lcm_tcpq_provider_init (providers);
if (providers->len == 0) {
fprintf (stderr, "Error: no LCM providers found\n");
goto fail;
}
if (!url || !strlen(url))
url = getenv ("LCM_DEFAULT_URL");
if (!url || !strlen(url))
url = LCM_DEFAULT_URL;
if (0 != lcm_parse_url (url, &provider_str, &network, args)) {
fprintf (stderr, "%s:%d -- invalid URL [%s]\n",
__FILE__, __LINE__, url);
goto fail;
}
lcm_provider_info_t * info = NULL;
/* Find a matching provider */
for (unsigned int i = 0; i < providers->len; i++) {
lcm_provider_info_t * pinfo = (lcm_provider_info_t *) g_ptr_array_index (providers, i);
if (!strcmp (pinfo->name, provider_str)) {
info = pinfo;
break;
}
}
if (!info) {
fprintf (stderr, "Error: LCM provider \"%s\" not found\n",
provider_str);
g_ptr_array_free (providers, TRUE);
free (provider_str);
free (network);
g_hash_table_destroy (args);
return NULL;
}
lcm = (lcm_t *) calloc (1, sizeof (lcm_t));
lcm->vtable = info->vtable;
lcm->handlers_all = g_ptr_array_new();
lcm->handlers_map = g_hash_table_new (g_str_hash, g_str_equal);
g_static_rec_mutex_init (&lcm->mutex);
g_static_rec_mutex_init (&lcm->handle_mutex);
lcm->provider = info->vtable->create (lcm, network, args);
lcm->in_handle = 0;
free (provider_str);
free (network);
g_ptr_array_free (providers, TRUE);
g_hash_table_destroy (args);
if (!lcm->provider) {
lcm_destroy (lcm);
return NULL;
}
lcm->default_max_num_queued_messages = 30;
return lcm;
fail:
free (provider_str);
free (network);
if (args)
g_hash_table_destroy (args);
if (providers)
g_ptr_array_free (providers, TRUE);
// if (lcm)
// lcm_destroy (lcm);
return NULL;
}
int main(int argc, char **argv)
{
logplayer_t l;
double speed = 1.0;
int c;
char *expression = NULL;
struct option long_opts[] = {
{"help", no_argument, 0, 'h'}, {"speed", required_argument, 0, 's'},
{"lcm-url", required_argument, 0, 'l'}, {"verbose", no_argument, 0, 'v'},
{"regexp", required_argument, 0, 'e'}, {0, 0, 0, 0},
};
char *lcmurl = NULL;
memset(&l, 0, sizeof(logplayer_t));
while ((c = getopt_long(argc, argv, "hp:s:ve:l:", long_opts, 0)) >= 0) {
switch (c) {
case 's':
speed = strtod(optarg, NULL);
break;
case 'l':
free(lcmurl);
lcmurl = strdup(optarg);
break;
case 'v':
l.verbose = 1;
break;
case 'e':
expression = strdup(optarg);
break;
case 'h':
default:
usage(argv[0]);
return 1;
};
}
if (optind != argc - 1) {
usage(argv[0]);
return 1;
}
char *file = argv[optind];
printf("Using playback speed %f\n", speed);
if (!expression)
expression = strdup(".*");
#ifndef WIN32
char url_in[strlen(file) + 64];
#else
char url_in[2048];
#endif
sprintf(url_in, "file://%s?speed=%f", argv[optind], speed);
l.lcm_in = lcm_create(url_in);
if (!l.lcm_in) {
fprintf(stderr, "Error: Failed to open %s\n", file);
free(expression);
return 1;
}
l.lcm_out = lcm_create(lcmurl);
free(lcmurl);
if (!l.lcm_out) {
fprintf(stderr, "Error: Failed to create LCM\n");
free(expression);
return 1;
}
lcm_subscribe(l.lcm_in, expression, handler, &l);
while (!lcm_handle(l.lcm_in)) {
// Do nothing
}
lcm_destroy(l.lcm_in);
lcm_destroy(l.lcm_out);
free(expression);
}
virtual void TearDown() {
lcm_destroy(lcm_);
// todo: delete param_;
}
void cleanup(void) { lcm_destroy(lcm); }
int main(int argc,char** argv)
{
int i;
int err;
int thru=0;
int verbose = 0;
char *device_in = NULL;
char *lcm_out = NULL;
int fd_in = -1,fd_out = -1;
snd_rawmidi_t *handle_out = 0;
if (argc!=3) {
usage();
exit(0);
}
device_in = argv[1];
lcm_out = argv[2];
lcm = lcm_create ("udpm://239.255.76.67:7667?ttl=0");
if (!lcm)
return 1;
struct timeval thisTime;
fprintf(stderr,"Using: \n");
fprintf(stderr,"Input: ");
fprintf(stderr,"device %s\n",device_in);
fprintf(stderr,"Output: ");
fprintf(stderr,"LCM channel %s\n", lcm_out);
fprintf(stderr,"Read midi in\n");
fprintf(stderr,"Press ctrl-c to stop\n");
fprintf(stderr,"Broadcasting LCM: %s\n", lcm_out);
if (device_in) {
err = snd_rawmidi_open(&handle_in,NULL,device_in,0);
if (err) {
fprintf(stderr,"snd_rawmidi_open %s failed: %d\n",device_in,err);
return -1;
}
}
signal(SIGINT,sighandler);
int num = 0;
int val[10];
int sysExclusive = 0;
if (handle_in) {
unsigned char ch;
while (!stop) {
snd_rawmidi_read(handle_in,&ch,1);
// check to make sure this isn't a system exclusive message (begins with 0xF0. Ends with a 0xF7)
if (ch == 0xF0)
{
sysExclusive = 1;
//fprintf(stderr,"sys exclusive\n");
}
if (sysExclusive == 0)
{
val[num] = (int)ch;
if (num >= 2)
{
//fprintf(stderr,"read %02x --> %02x --> %d\n", val[0], val[1], val[2]);
num = 0;
// send LCM message
lcmt_midi msg;
gettimeofday(&thisTime, NULL);
msg.timestamp = (thisTime.tv_sec * 1000.0) + (float)thisTime.tv_usec/1000.0 + 0.5;
msg.event[0] = val[0];
msg.event[1] = val[1];
msg.event[2] = val[2];
lcmt_midi_publish (lcm, lcm_out, &msg);
} else {
num ++;
}
//fprintf(stderr,"read %02x\n", ch);
}
if (ch == 0xF7)
{
sysExclusive = 0;
}
}
}
fprintf(stderr,"Closing.\n");
if (handle_in) {
snd_rawmidi_drain(handle_in);
snd_rawmidi_close(handle_in);
}
lcm_destroy (lcm);
return 0;
}
virtual void TearDown() {
bot_lcmgl_destroy(lcmgl_);
lcm_destroy(lcm_);
// todo: delete param_;
}
int main(int argc, char *argv[])
{
uint8_t i;
for(i = 0; i < argc; ++i)
{
if((strcmp(argv[i], "-h") == 0) || (strcmp(argv[i], "--help") == 0))
{
fprintf(stdout, "%s\n", USAGE);
exit(0);
}
if(strcmp(argv[i], "-v") == 0)
{
verbose = true;
fprintf(stdout, "Running in verbose mode\n");
}
if(strcmp(argv[i], "-l") == 0)
{
loopback_mode = true;
fprintf(stdout, "Running in loopback mode\n");
}
}
(void) signal(SIGINT, interrupt);
(void) signal(SIGTERM, interrupt);
lcm = lcm_create(NULL);
while(lcm == NULL)
{
fprintf(stderr, "LCM failed to initialize. Reattempting....\n");
usleep(1000000);
lcm = lcm_create(NULL);
}
// Create Serial Device for XBee
char *xbee_dev_name = "/dev/XBee";
for(i = 0; i < argc; ++i)
{
if(strcmp(argv[i], "-x") == 0)
{
if(i + 1 < argc)
xbee_dev_name = argv[i+1];
break;
}
}
xbee = serial_create(xbee_dev_name, B9600);
if(xbee == NULL)
fprintf(stderr, "XBee device does not exist at %s\n", xbee_dev_name);
else
fprintf(stdout, "XBee device successfully opened at 9600 baud on %s\n", xbee_dev_name);
// Create Serial Device for USB
char *usb_dev_name = "/dev/stack";
for(i = 0; i < argc; ++i)
{
if(strcmp(argv[i], "-u") == 0)
{
if(i + 1 < argc)
usb_dev_name = argv[i+1];
break;
}
}
usb = serial_create(usb_dev_name, B115200);
if(usb == NULL)
fprintf(stderr, "Usb device does not exist at %s\n", usb_dev_name);
else
fprintf(stdout, "Usb device successfully opened at 115200 baud on %s\n", usb_dev_name);
// If no open comm ports and not in loopback mode, quit.
if(!xbee && !usb && !loopback_mode)
{
fprintf(stderr, "Failed to open any comms devices. Exiting...\n");
lcm_destroy(lcm);
exit(1);
}
// Create comms devices
if(xbee)
{
xbee_comms = comms_create(1000, 1000, 1, publish_xbee);
comms_subscribe(xbee_comms, CHANNEL_KILL, handler_kill, NULL);
comms_subscribe(xbee_comms, CHANNEL_CHANNELS, handler_channels, NULL);
comms_subscribe(xbee_comms, CHANNEL_CFG_USB_SN, handler_cfg_usb_serial_num, NULL);
comms_subscribe(xbee_comms, CHANNEL_CFG_DATA_FREQUENCY, handler_cfg_data_frequency, NULL);
pthread_create(&xbee_thread, NULL, xbee_run, NULL);
}
// Create comms devices
if(usb)
{
usb_comms = comms_create(1000, 1000, 1, publish_usb);
comms_subscribe(usb_comms, CHANNEL_KILL, handler_kill, NULL);
comms_subscribe(usb_comms, CHANNEL_CHANNELS, handler_channels, NULL);
comms_subscribe(usb_comms, CHANNEL_CFG_USB_SN, handler_cfg_usb_serial_num, NULL);
comms_subscribe(usb_comms, CHANNEL_CFG_DATA_FREQUENCY, handler_cfg_data_frequency, NULL);
pthread_create(&usb_thread, NULL, usb_run, NULL);
}
kill_t_subscription_t *kill_subs =
kill_t_subscribe(lcm, "KILL_.*_TX", handler_kill_lcm, NULL);
channels_t_subscription_t *channels_subs =
channels_t_subscribe(lcm, "CHANNELS_.*_TX", handler_channels_lcm, NULL);
cfg_data_frequency_t_subscription_t *data_freq_subs =
cfg_data_frequency_t_subscribe(lcm, "CFG_DATA_FREQUENCY_.*_TX",
handler_cfg_data_frequency_lcm, NULL);
cfg_usb_serial_num_t_subscription_t *usb_serial_num_subs =
cfg_usb_serial_num_t_subscribe(lcm, "CFG_USB_SERIAL_NUM_.*_TX",
handler_cfg_usb_serial_num_lcm, NULL);
while(!done)
{
// Set up timeout on lcm_handle to prevent blocking at program exit
int lcm_fd = lcm_get_fileno(lcm);
fd_set fds;
FD_ZERO(&fds);
FD_SET(lcm_fd, &fds);
struct timeval timeout = { 0, 100000 }; // wait 100ms
int status = select(lcm_fd + 1, &fds, NULL, NULL, &timeout);
if(done) break;
if(status != 0 && FD_ISSET(lcm_fd, &fds)) {
if ( lcm_handle(lcm) ) {
fprintf( stderr, "LCM handle error, aborting\n" );
done = true;
}
}
}
fprintf(stdout, "Lcm driver destroyed.\n");
kill_t_unsubscribe(lcm, kill_subs);
channels_t_unsubscribe(lcm, channels_subs);
cfg_data_frequency_t_unsubscribe(lcm, data_freq_subs);
cfg_usb_serial_num_t_unsubscribe(lcm, usb_serial_num_subs);
if(xbee)
{
pthread_cancel(xbee_thread);
comms_destroy(xbee_comms);
serial_destroy(xbee);
fprintf(stdout, "Xbee driver destroyed.\n");
}
if(usb)
{
pthread_cancel(usb_thread);
comms_destroy(usb_comms);
serial_destroy(usb);
fprintf(stdout, "Usb driver destroyed.\n");
}
lcm_destroy(lcm);
return 0;
}
