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[])
{
// so that redirected stdout won't be insanely buffered.
setvbuf (stdout, (char *) NULL, _IONBF, 0);
lcm_t *lcm = lcm_create (NULL);
if(!lcm)
return 1;
printf ("utime,\t\tleft_ticks,\tright_ticks\n");
maebot_motor_feedback_t_subscribe (lcm,
"MAEBOT_MOTOR_FEEDBACK",
motor_feedback_handler,
NULL);
while (1)
lcm_handle (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[])
{
if (argc < 2) {
fprintf(stderr, "usage: %s <full vicon channel name> [frame name to update]\n", argv[0]);
exit(1);
}
app_t * app = (app_t *) calloc(1, sizeof(app_t));
app->vicon_channel = strdup(argv[1]);
app->body_frame_name = strdup("body");
if (argc > 2) {
app->body_frame_name = strdup(argv[2]);
}
app->lcm = lcm_create(NULL);
app->param = bot_param_new_from_server(app->lcm, 0);
app->frames = bot_frames_new(app->lcm, app->param);
vicon_body_t_subscribe(app->lcm, app->vicon_channel, vicon_handler, (void *) app);
while (true) {
int ret = lcm_handle(app->lcm);
}
}
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;
}
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);
}
void * lcm_handler (void *lcm) {
for(;;) lcm_handle(lcm);
}
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;
}
int
main (int argc, char *argv[])
{
// so that redirected stdout won't be insanely buffered.
setvbuf (stdout, (char *) NULL, _IONBF, 0);
state_t *state = calloc (1, sizeof *state);
state->meters_per_tick = METERS_PER_TICK; // IMPLEMENT ME
state->alpha = ALPHA;
state->beta = BETA;
// file for gyro integration test
//state->fp = fopen("gyro-yaw-integration.txt","w");
state->fp = fopen("yaw.txt","w");
// Tests for the position estimate
/*
update_position(state,4456,4456);
update_position(state,0,1208);
update_position(state,4456,4456);
while(1);
*/
// Used for testing the gyro bias
/*
for(int i=0; i<100; i++){
state->yaw_cal_array[i] = 15*i;
}
state->gyro_bias = find_gyro_bias(state);
printf("Gyro Bias: %llu\n", state->gyro_bias);
*/
//printf("getting options\n");
state->gopt = getopt_create ();
getopt_add_bool (state->gopt, 'h', "help", 0, "Show help");
getopt_add_bool (state->gopt, 'g', "use-gyro", 0, "Use gyro for heading instead of wheel encoders");
getopt_add_string (state->gopt, '\0', "odometry-channel", "BOTLAB_ODOMETRY", "LCM channel name");
getopt_add_string (state->gopt, '\0', "feedback-channel", "MAEBOT_MOTOR_FEEDBACK", "LCM channel name");
getopt_add_string (state->gopt, '\0', "sensor-channel", "MAEBOT_SENSOR_DATA", "LCM channel name");
getopt_add_double (state->gopt, '\0', "alpha", ALPHA_STRING, "Longitudinal covariance scaling factor");
getopt_add_double (state->gopt, '\0', "beta", BETA_STRING, "Lateral side-slip covariance scaling factor");
getopt_add_double (state->gopt, '\0', "gyro-rms", GYRO_RMS_STRING, "Gyro RMS deg/s");
if (!getopt_parse (state->gopt, argc, argv, 1) || getopt_get_bool (state->gopt, "help")) {
printf ("Usage: %s [--url=CAMERAURL] [other options]\n\n", argv[0]);
getopt_do_usage (state->gopt);
exit (EXIT_FAILURE);
}
state->use_gyro = getopt_get_bool (state->gopt, "use-gyro");
state->odometry_channel = getopt_get_string (state->gopt, "odometry-channel");
state->feedback_channel = getopt_get_string (state->gopt, "feedback-channel");
state->sensor_channel = getopt_get_string (state->gopt, "sensor-channel");
state->alpha = getopt_get_double (state->gopt, "alpha");
state->beta = getopt_get_double (state->gopt, "beta");
state->gyro_rms = getopt_get_double (state->gopt, "gyro-rms") * DTOR;
state->yaw_calibrated = 0;
state->yaw = 0;
state->yaw_old = 0;
//printf("subscribing to channels\n");
// initialize LCM
state->lcm = lcm_create (NULL);
maebot_motor_feedback_t_subscribe (state->lcm, state->feedback_channel,
motor_feedback_handler, state);
maebot_sensor_data_t_subscribe (state->lcm, state->sensor_channel,
sensor_data_handler, state);
printf ("ticks per meter: %f\n", 1.0/state->meters_per_tick);
while (1){
lcm_handle (state->lcm);
}
}
int main(int argc,char** argv) {
std::string mono_channel = "stereo-mono";
ConciseArgs parser(argc, argv);
parser.add(log_directory, "d", "log-directory", "Directory to read video files from");
parser.add(video_directory, "v", "video-directory", "Use this to force a video directory path in case the automatic detection via the more general log-directory option does not work for you.");
parser.add(mono_channel, "m", "stereo-mono-channel", "LCM channel to receive stereo-mono messages on.");
parser.add(image_channel, "i", "image-channel", "LCM channel to publish images on to.");
parser.add(frame_offset, "o", "frame-offset", "Offset frames this amount (to correct for sync issues.");
parser.parse();
if (log_directory == "" && video_directory == "") {
std::cerr << "You must specify either a log-directory or a video-directory." << std::endl;
return 1;
}
lcm_ = lcm_create ("udpm://239.255.76.67:7667?ttl=0");
if (!lcm_) {
fprintf(stderr, "lcm_create for recieve failed. Quitting.\n");
return 1;
}
// control-c handler
signal(SIGINT,sighandler);
std::string dir;
// ensure the directory has a trailing slash
if (log_directory != "") {
if (log_directory.at(log_directory.length() - 1) != boost::filesystem::path::preferred_separator) {
log_directory = log_directory + boost::filesystem::path::preferred_separator;
}
if (!boost::filesystem::exists(log_directory)) {
std::cerr << "Error: failed to find directory for video files: " << log_directory << std::endl;
return false;
}
dir = log_directory;
}
if (video_directory != "") {
if (video_directory.at(video_directory.length() - 1) != boost::filesystem::path::preferred_separator) {
video_directory = video_directory + boost::filesystem::path::preferred_separator;
}
if (!boost::filesystem::exists(video_directory)) {
std::cerr << "Error: failed to find directory for video files: " << video_directory << std::endl;
return false;
}
dir = video_directory;
}
// check to make sure the directory exists:
stereo_sub = lcmt_stereo_subscribe(lcm_, mono_channel.c_str(), &mono_handler, NULL);
cpu_info_sub1 = lcmt_cpu_info_subscribe(lcm_, "cpu-info-odroid-cam1", &cpu_info_handler, NULL);
cpu_info_sub2 = lcmt_cpu_info_subscribe(lcm_, "cpu-info-odroid-cam2", &cpu_info_handler, NULL);
printf("LCM:\n\t%s\n\t%s\nVideo directory: %s\n", mono_channel.c_str(), image_channel.c_str(), dir.c_str());
if (frame_offset != 0) {
std::cerr << "WARNING: frame offset: " << frame_offset << std::endl;
}
while (true) {
// read the LCM channel
lcm_handle(lcm_);
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mavconn_asctec_bridge");
GError* error = NULL;
GOptionContext* context = g_option_context_new("- translate messages between Asctec (ROS) and MAVCONN (LCM)");
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);
}
nh = new ros::NodeHandle;
ros::Subscriber poseStampedSub = nh->subscribe("fcu/current_pose", 10, poseStampedCallback);
ros::Publisher waypointPub = nh->advertise<asctec_hl_comm::WaypointActionGoal>("fcu/waypoint/goal", 10);
// check for changed parameters on parameter server
ros::Timer paramCheckTimer = nh->createTimer(ros::Duration(2.0), paramCheckCallback);
/**
* Connect to LCM Channel and register for MAVLink messages ->
*/
lcm = lcm_create(lcmurl.c_str());
if (!lcm)
{
exit(EXIT_FAILURE);
}
mavlink_message_t_subscription_t* mavlinkSub =
mavlink_message_t_subscribe(lcm, "MAVLINK", &mavlinkHandler, &waypointPub);
// start thread(s) to listen for ROS messages
ros::AsyncSpinner spinner(1);
spinner.start();
// listen for LCM messages
int lcm_fd = lcm_get_fileno(lcm);
// wait a limited amount of time for an incoming LCM message
struct timeval timeout = {
1, // seconds
0 // microseconds
};
while (ros::ok())
{
fd_set fds;
FD_ZERO(&fds);
FD_SET(lcm_fd, &fds);
int status = select(lcm_fd + 1, &fds, 0, 0, &timeout);
if (status != 0 && FD_ISSET(lcm_fd, &fds) && ros::isShuttingDown())
{
// LCM has events ready to be processed.
lcm_handle(lcm);
}
}
delete nh;
mavlink_message_t_unsubscribe(lcm, mavlinkSub);
lcm_destroy(lcm);
return 0;
}
static int
lcm_message_ready (GIOChannel *source, GIOCondition cond, lcm_t *lcm)
{
lcm_handle (lcm);
return TRUE;
}