xsens_xyz_handler * create_xsens_xyz_handler(int argc, char **argv, carmen_fused_odometry_parameters *parameters) { fused_odometry_parameters = parameters; if (xsens_sensor_vector == NULL) xsens_sensor_vector = (sensor_vector_xsens_xyz **) calloc(XSENS_SENSOR_VECTOR_SIZE, sizeof(sensor_vector_xsens_xyz *)); memset(&xsens_handler, 0, sizeof(xsens_handler)); initialize_carmen_parameters(&xsens_handler, argc, argv); initialize_transformations(&xsens_handler); subscribe_messages(); return (&xsens_handler); }
carmen_pose_3D_t get_velodyne_pose_in_relation_to_car_helper(int argc, char** argv) { static int initialized = 0; if(initialized == 0) { initialize_carmen_parameters(argc, argv); initialize_transformations(); initialized = 1; } tf::StampedTransform car_to_velodyne; transformer.lookupTransform(car_tf_name, velodyne_tf_name, tf::Time(0), car_to_velodyne); carmen_pose_3D_t velodyne_pose = tf_transform_to_carmen_pose_3D(car_to_velodyne); return velodyne_pose; }