int main(int argc, char **argv)
{
double angular_velocity_covariance, angular_velocity_stdev;
double linear_acceleration_covariance, linear_acceleration_stdev;
double orientation_covariance, orientation_stdev;
std::string frame_id;
// Initializing ROS
ros::init(argc, argv, "imu");
ros::NodeHandle nh("~");
imu_message = nh.advertise<sensor_msgs::Imu>("data", 10);
att_message = nh.advertise<sensor_msgs::Imu>("att", 10);
mag_message = nh.advertise<geometry_msgs::Vector3Stamped>("mag", 10);
acc_message = nh.advertise<geometry_msgs::Vector3Stamped>("acc", 10);
// Getting Parameters
nh.param<std::string>("frame_id", frame_id, "imu");
imu_data.header.frame_id = frame_id;
mag_data.header.frame_id = frame_id;
nh.param("linear_acceleration_stdev", linear_acceleration_stdev, 0.0);
nh.param("orientation_stdev", orientation_stdev, 0.0);
nh.param("angular_velocity_stdev", angular_velocity_stdev, 0.0);
angular_velocity_covariance = pow(angular_velocity_stdev,2);
orientation_covariance = pow(orientation_stdev,2);
linear_acceleration_covariance = pow(linear_acceleration_stdev,2);
// Fill IMU Message with covariances
imu_data.linear_acceleration_covariance[0] = linear_acceleration_covariance;
imu_data.linear_acceleration_covariance[4] = linear_acceleration_covariance;
imu_data.linear_acceleration_covariance[8] = linear_acceleration_covariance;
imu_data.angular_velocity_covariance[0] = angular_velocity_covariance;
imu_data.angular_velocity_covariance[4] = angular_velocity_covariance;
imu_data.angular_velocity_covariance[8] = angular_velocity_covariance;
imu_data.orientation_covariance[0] = orientation_covariance;
imu_data.orientation_covariance[4] = orientation_covariance;
imu_data.orientation_covariance[8] = orientation_covariance;
// Initializing Ivy
IvyInit ("imu_parser", "'Imu Parser' is READY!", 0, 0, 0, 0);
IvyStart("127.255.255.255");
TimerRepeatAfter (TIMER_LOOP, 500, ROSCallback, 0);
// Binding Messages
IvyBindMsg(ATTCallback, 0, "BOOZ2_AHRS_EULER(.*)");
IvyBindMsg(GYROCallback, 0, "IMU_GYRO_SCALED(.*)");
IvyBindMsg(ACCELCallback, 0, "IMU_ACCEL_SCALED(.*)");
IvyBindMsg(MAGCallback, 0, "IMU_MAG_SCALED(.*)");
ROS_INFO("IMU: Starting Aquisition Loop");
IvyMainLoop();
return 0;
}
/// Main function
int main(int argc, char **argv) {
// default values for options
const char
*defaultbus = "127.255.255.255:2010",
*bus = defaultbus,
*defaultdevice = "/dev/ttyUSB1";
device = defaultdevice;
long delay = 1000;
// parse options
char c;
while ((c = getopt (argc, argv, "hab:d:i:s:")) != EOF) {
switch (c) {
case 'h':
print_usage(argc, argv);
exit(EXIT_SUCCESS);
break;
case 'a':
want_alive_msg = TRUE;
break;
case 'b':
bus = optarg;
break;
case 'd':
device = optarg;
break;
case 'i':
ac_id = atoi(optarg);
break;
case 's':
delay = atoi(optarg)*1000;
break;
case '?':
if (optopt == 'a' || optopt == 'b' || optopt == 'd' || optopt == 's')
fprintf (stderr, "Option -%c requires an argument.\n", optopt);
else if (isprint (optopt))
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
else
fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
print_usage(argc, argv);
exit(EXIT_FAILURE);
default:
abort ();
}
}
// make Ctrl-C stop the main loop and clean up properly
signal(SIGINT, sigint_handler);
bzero (packet, PACKET_LENGTH);
open_port(device);
// setup Ivy communication
IvyInit("davis2ivy", "READY", 0, 0, 0, 0);
IvyStart(bus);
// create timer
tid = TimerRepeatAfter (0, delay, handle_timer, 0);
#if IVYMINOR_VERSION == 8
IvyMainLoop (NULL,NULL);
#else
IvyMainLoop ();
#endif
return 0;
}
/** Main function.
*/
int main(int argc, char **argv) {
// default values for options
const char
*defaultbus = "127.255.255.255:2010",
*bus = defaultbus,
*defaultdevice = "/dev/rfcomm0";
device = defaultdevice;
long delay = 5000;
// parse options
char c;
while ((c = getopt (argc, argv, "hamb:d:i:s:")) != EOF) {
switch (c) {
case 'h':
print_usage(argc, argv);
exit(EXIT_SUCCESS);
break;
case 'a':
want_alive_msg = TRUE;
break;
case 'b':
bus = optarg;
break;
case 'd':
device = optarg;
break;
case 'i':
ac_id = atoi(optarg);
break;
case 'm':
metric_input = TRUE;
break;
case 's':
delay = atoi(optarg)*1000;
if (delay < 5000) {
fprintf(stderr,"kestrel2ivy: Warning: Setting the sampling period less than 5 seconds\n"
"may result in poor sampling performance!\n");
}
break;
case '?':
if (optopt == 'a' || optopt == 'b' || optopt == 'd' || optopt == 's')
fprintf (stderr, "Option -%c requires an argument.\n", optopt);
else if (isprint (optopt))
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
else
fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
print_usage(argc, argv);
exit(EXIT_FAILURE);
default:
abort ();
}
}
// make Ctrl-C stop the main loop and clean up properly
signal(SIGINT, sigint_handler);
// zero out and initialize buffers
int bufferSize = BUF_LENGTH;
cbInit(&cb, bufferSize);
bzero(packet, PACKET_LENGTH);
msgState = UNINIT;
open_port(device);
// setup Ivy communication
IvyInit("kestrel2ivy", "READY", 0, 0, 0, 0);
IvyStart(bus);
// create timer (Ivy)
tid = TimerRepeatAfter (0, delay, handle_timer, 0);
/* main loop */
#if IVYMINOR_VERSION == 8
IvyMainLoop (NULL,NULL);
#else
IvyMainLoop ();
#endif
return 0;
}
value ivy_timerRepeatafter(value nb_ticks,value delay, value closure_name)
{
value * closure = caml_named_value(String_val(closure_name));
TimerId id = TimerRepeatAfter(Int_val(nb_ticks), Int_val(delay), timer_cb, (void*)closure);
return Val_int(id);
}
