void ipc_gps_gprmc_handler( carmen_gps_gprmc_message *gps_data)
{
fprintf(stderr, "g");
carmen_logwrite_write_gps_gprmc(gps_data, outfile, carmen_get_time() - logger_starttime);
}
void laser_laser5_handler(carmen_laser_laser_message *laser)
{
fprintf(stderr, "5");
carmen_logwrite_write_laser_laser(laser, 5, outfile,
carmen_get_time() - logger_starttime);
}
void localize_handler(carmen_localize_globalpos_message *msg)
{
fprintf(stderr, "L");
carmen_logwrite_write_localize(msg, outfile, carmen_get_time() -
logger_starttime);
}
void base_odometry_handler(carmen_base_odometry_message *odometry)
{
fprintf(stderr, "O");
carmen_logwrite_write_odometry(odometry, outfile,
carmen_get_time() - logger_starttime);
}
void publish_xsens_global(xsens_global data, /*CmtOutputMode& mode,*/ CmtOutputSettings& settings)
{
IPC_RETURN_TYPE err;
switch (settings & CMT_OUTPUTSETTINGS_ORIENTMODE_MASK) {
case CMT_OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
carmen_xsens_global_quat_message xsens_quat_message;
//Acceleration
xsens_quat_message.m_acc.x = data.acc.x;
xsens_quat_message.m_acc.y = data.acc.y;
xsens_quat_message.m_acc.z = data.acc.z;
//Gyro
xsens_quat_message.m_gyr.x = data.gyr.x;
xsens_quat_message.m_gyr.y = data.gyr.y;
xsens_quat_message.m_gyr.z = data.gyr.z;
//Magnetism
xsens_quat_message.m_mag.x = data.mag.x;
xsens_quat_message.m_mag.y = data.mag.y;
xsens_quat_message.m_mag.z = data.mag.z;
xsens_quat_message.quat_data.m_data[0] = data.quatData.data[0];
xsens_quat_message.quat_data.m_data[1] = data.quatData.data[1];
xsens_quat_message.quat_data.m_data[2] = data.quatData.data[2];
xsens_quat_message.quat_data.m_data[3] = data.quatData.data[3];
//Temperature and Message Count
xsens_quat_message.m_temp = data.temp;
xsens_quat_message.m_count = data.count;
//Timestamp
xsens_quat_message.timestamp = carmen_get_time();
//Host
xsens_quat_message.host = carmen_get_host();
err = IPC_publishData(CARMEN_XSENS_GLOBAL_QUAT_NAME, &xsens_quat_message);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_XSENS_GLOBAL_QUAT_NAME);
break;
case CMT_OUTPUTSETTINGS_ORIENTMODE_EULER:
carmen_xsens_global_euler_message xsens_euler_message;
//Acceleration
xsens_euler_message.m_acc.x = data.acc.x;
xsens_euler_message.m_acc.y = data.acc.y;
xsens_euler_message.m_acc.z = data.acc.z;
//Gyro
xsens_euler_message.m_gyr.x = data.gyr.x;
xsens_euler_message.m_gyr.y = data.gyr.y;
xsens_euler_message.m_gyr.z = data.gyr.z;
//Magnetism
xsens_euler_message.m_mag.x = data.mag.x;
xsens_euler_message.m_mag.y = data.mag.y;
xsens_euler_message.m_mag.z = data.mag.z;
xsens_euler_message.euler_data.m_pitch = data.eulerData.pitch;
xsens_euler_message.euler_data.m_roll = data.eulerData.roll;
xsens_euler_message.euler_data.m_yaw = data.eulerData.yaw;
//Temperature and Message Count
xsens_euler_message.m_temp = data.temp;
xsens_euler_message.m_count = data.count;
//Timestamp
xsens_euler_message.timestamp = carmen_get_time();
//Host
xsens_euler_message.host = carmen_get_host();
err = IPC_publishData(CARMEN_XSENS_GLOBAL_EULER_NAME, &xsens_euler_message);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_XSENS_GLOBAL_EULER_NAME);
break;
case CMT_OUTPUTSETTINGS_ORIENTMODE_MATRIX:
carmen_xsens_global_matrix_message xsens_matrix_message;
//Acceleration
xsens_matrix_message.m_acc.x = data.acc.x;
xsens_matrix_message.m_acc.y = data.acc.y;
xsens_matrix_message.m_acc.z = data.acc.z;
//Gyro
xsens_matrix_message.m_gyr.x = data.gyr.x;
xsens_matrix_message.m_gyr.y = data.gyr.y;
xsens_matrix_message.m_gyr.z = data.gyr.z;
//Magnetism
xsens_matrix_message.m_mag.x = data.mag.x;
xsens_matrix_message.m_mag.y = data.mag.y;
xsens_matrix_message.m_mag.z = data.mag.z;
xsens_matrix_message.matrix_data.m_data[0][0] = data.matrixData.data[0][0];
xsens_matrix_message.matrix_data.m_data[0][1] = data.matrixData.data[0][1];
xsens_matrix_message.matrix_data.m_data[0][2] = data.matrixData.data[0][2];
xsens_matrix_message.matrix_data.m_data[1][0] = data.matrixData.data[1][0];
xsens_matrix_message.matrix_data.m_data[1][1] = data.matrixData.data[1][1];
xsens_matrix_message.matrix_data.m_data[1][2] = data.matrixData.data[1][2];
xsens_matrix_message.matrix_data.m_data[2][0] = data.matrixData.data[2][0];
xsens_matrix_message.matrix_data.m_data[2][1] = data.matrixData.data[2][1];
xsens_matrix_message.matrix_data.m_data[2][2] = data.matrixData.data[2][2];
//Temperature and Message Count
xsens_matrix_message.m_temp = data.temp;
xsens_matrix_message.m_count = data.count;
//Timestamp
xsens_matrix_message.timestamp = carmen_get_time();
xsens_matrix_message.timestamp2 = data.timestamp;
//Host
xsens_matrix_message.host = carmen_get_host();
err = IPC_publishData(CARMEN_XSENS_GLOBAL_MATRIX_NAME, &xsens_matrix_message);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_XSENS_GLOBAL_MATRIX_NAME);
break;
default:
carmen_die("Unknown settings in publish_xsens_global()\n");
break;
}
}
int main(int argc, char **argv)
{
char filename[1024];
char key;
/* initialize connection to IPC network */
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
/* open logfile, check if file overwrites something */
if(argc < 2)
carmen_die("usage: %s <logfile>\n", argv[0]);
strcpy(filename, argv[1]);
outfile = carmen_fopen(filename, "r");
if (outfile != NULL) {
fprintf(stderr, "Overwrite %s? ", filename);
scanf("%c", &key);
if (toupper(key) != 'Y')
exit(-1);
carmen_fclose(outfile);
}
outfile = carmen_fopen(filename, "w");
if(outfile == NULL)
carmen_die("Error: Could not open file %s for writing.\n", filename);
carmen_logwrite_write_header(outfile);
get_logger_params(argc, argv);
if ( !(log_odometry && log_laser && log_robot_laser ) )
carmen_warn("\nWARNING: You are neither logging laser nor odometry messages!\n");
if (log_params)
get_all_params();
register_ipc_messages();
if (log_odometry)
carmen_base_subscribe_odometry_message(NULL, (carmen_handler_t)
base_odometry_handler,
CARMEN_SUBSCRIBE_ALL);
if (log_robot_laser) {
carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)
robot_frontlaser_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)
robot_rearlaser_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_laser) {
carmen_laser_subscribe_laser1_message(NULL, (carmen_handler_t)
laser_laser1_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser2_message(NULL, (carmen_handler_t)
laser_laser2_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser3_message(NULL, (carmen_handler_t)
laser_laser3_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser4_message(NULL, (carmen_handler_t)
laser_laser4_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser5_message(NULL, (carmen_handler_t)
laser_laser5_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_localize) {
carmen_localize_subscribe_globalpos_message(NULL, (carmen_handler_t)
localize_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_simulator) {
carmen_simulator_subscribe_truepos_message(NULL, (carmen_handler_t)
carmen_simulator_truepos_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_gps) {
carmen_gps_subscribe_nmea_message( NULL,
(carmen_handler_t) ipc_gps_gpgga_handler,
CARMEN_SUBSCRIBE_ALL );
carmen_gps_subscribe_nmea_rmc_message( NULL,
(carmen_handler_t) ipc_gps_gprmc_handler,
CARMEN_SUBSCRIBE_ALL );
}
if (log_smart) {
smart_subscribe_status_message( NULL,
(carmen_handler_t) smart_status_handler,
CARMEN_SUBSCRIBE_ALL );
}
if (log_gyro) {
gyro_subscribe_integrated_message( NULL,
(carmen_handler_t) gyro_integrated_handler,
CARMEN_SUBSCRIBE_ALL );
}
signal(SIGINT, shutdown_module);
logger_starttime = carmen_get_time();
carmen_ipc_dispatch();
return 0;
}
void
carmen_conventional_dynamic_program(int goal_x, int goal_y)
{
double *utility_ptr;
int index;
double max_val, min_val;
int num_expanded;
int done;
struct timeval start_time, end_time;
int delta_sec, delta_usec;
static double last_time, cur_time, last_print;
queue state_queue;
state_ptr current_state;
if (costs == NULL)
return;
gettimeofday(&start_time, NULL);
cur_time = carmen_get_time();
if ((int)(cur_time - last_print) > 10) {
carmen_verbose("Time since last DP: %d secs, %d usecs\n", (int)(cur_time - last_time),
((int)((cur_time-last_time)*1e6)) % 1000000);
last_print = cur_time;
}
last_time = cur_time;
if (utility == NULL) {
utility = (double *)calloc(x_size*y_size, sizeof(double));
carmen_test_alloc(utility);
}
utility_ptr = utility;
for (index = 0; index < x_size * y_size; index++)
*(utility_ptr++) = -1;
if (is_out_of_map(goal_x, goal_y))
return;
max_val = -MAXDOUBLE;
min_val = MAXDOUBLE;
done = 0;
state_queue = make_queue();
current_state = carmen_conventional_create_state(goal_x, goal_y, 0);
max_val = MAX_UTILITY;
*(utility_value(goal_x, goal_y)) = max_val;
add_neighbours_to_queue(goal_x, goal_y, state_queue);
num_expanded = 1;
while ((current_state = pop_queue(state_queue)) != NULL) {
num_expanded++;
if (current_state->cost <= *(utility_value(current_state->x, current_state->y)))
add_neighbours_to_queue(current_state->x, current_state->y, state_queue);
if (current_state->cost < min_val)
min_val = current_state->cost;
free(current_state);
}
delete_queue(&state_queue);
gettimeofday(&end_time, NULL);
delta_usec = end_time.tv_usec - start_time.tv_usec;
delta_sec = end_time.tv_sec - start_time.tv_sec;
if (delta_usec < 0) {
delta_sec--;
delta_usec += 1000000;
}
// carmen_warn("Elasped time for dp: %d secs, %d usecs\n", delta_sec, delta_usec);
}
void video_event_handler(int kinect_id, uint8_t* data, uint32_t timestamp __attribute__ ((unused)), int size){
copy_video(kinect_id, data, carmen_get_time(), size);
}
void depth_event_handler(int kinect_id, uint16_t* data, uint32_t timestamp __attribute__ ((unused)), int size){
copy_depth(kinect_id, data, carmen_get_time(), size);
}
static void base_odometry_handler(void)
{
int i;
if (strcmp(robot_host, carmen_robot_latest_odometry.host) == 0)
odometry_count = CARMEN_ROBOT_ESTIMATES_CONVERGE;
else
odometry_count++;
carmen_warn("o");
for(i = 0; i < CARMEN_ROBOT_MAX_READINGS - 1; i++) {
carmen_robot_odometry[i] = carmen_robot_odometry[i + 1];
odometry_local_timestamp[i] = odometry_local_timestamp[i + 1];
}
carmen_robot_odometry[CARMEN_ROBOT_MAX_READINGS - 1] =
carmen_robot_latest_odometry;
odometry_local_timestamp[CARMEN_ROBOT_MAX_READINGS - 1] = carmen_get_time();
carmen_running_average_add(&odometry_average,
odometry_local_timestamp[CARMEN_ROBOT_MAX_READINGS - 1]-
carmen_robot_latest_odometry.timestamp);
if (collision_avoidance) {
if (carmen_robot_bumper_on()) {
fprintf(stderr, "S");
carmen_robot_stop_robot(CARMEN_ROBOT_ALL_STOP);
command_tv = 0;
command_rv = 0;
}
#ifndef COMPILE_WITHOUT_LASER_SUPPORT
if (carmen_robot_latest_odometry.tv > 0 &&
carmen_robot_laser_max_front_velocity() < carmen_robot_latest_odometry.tv &&
command_tv > carmen_robot_laser_max_front_velocity()) {
if (carmen_robot_laser_max_front_velocity() <= 0.0) {
command_tv = 0;
fprintf(stderr, "S");
carmen_robot_stop_robot(CARMEN_ROBOT_ALLOW_ROTATE);
}
else {
command_tv = carmen_robot_laser_max_front_velocity();
carmen_robot_send_base_velocity_command();
}
}
else if (carmen_robot_latest_odometry.tv < 0 &&
carmen_robot_laser_min_rear_velocity() > carmen_robot_latest_odometry.tv &&
command_tv < carmen_robot_laser_min_rear_velocity()) {
if (carmen_robot_laser_min_rear_velocity() >= 0.0) {
fprintf(stderr, "S");
command_tv = 0;
carmen_robot_stop_robot(CARMEN_ROBOT_ALLOW_ROTATE);
}
else {
command_tv = carmen_robot_laser_min_rear_velocity();
carmen_robot_send_base_velocity_command();
}
}
#endif
} // End of if (collision_avoidance)
if (use_sonar)
carmen_robot_correct_sonar_and_publish();
if (use_bumper)
carmen_robot_correct_bumper_and_publish();
#ifndef COMPILE_WITHOUT_LASER_SUPPORT
if (use_laser)
carmen_robot_correct_laser_and_publish();
#endif
}
int carmen_laser_run(void)
{
static int first = 1;
static double last_update;
static double last_alive = 0;
double current_time;
int print_stats;
static int laser1_stalled = 0, laser2_stalled = 0, laser3_stalled = 0,
laser4_stalled = 0, laser5_stalled = 0;;
if(first) {
last_update = carmen_get_time();
first = 0;
}
current_time = carmen_get_time();
print_stats = (current_time - last_update > 1.0);
if(use_laser1) {
sick_handle_laser(&laser1);
if(laser1.new_reading)
publish_laser_message(&laser1, &laser1_config);
laser1_stalled = (current_time - laser1.timestamp > 1.0);
//**
//fprintf(stderr, "time: %.1f",current_time - laser1.timestamp);
//**
if(print_stats)
fprintf(stderr, "L1: %s(%.1f%% full) ", laser1_stalled ?
"STALLED " : " ", (laser1.buffer_position -
laser1.processed_mark) /
(float)LASER_BUFFER_SIZE * 100.0);
}
if(use_laser2) {
sick_handle_laser(&laser2);
if(laser2.new_reading)
publish_laser_message(&laser2, &laser2_config);
laser2_stalled = (current_time - laser2.timestamp > 1.0);
if(print_stats)
fprintf(stderr, "L2: %s(%.1f%% full) ", laser2_stalled ?
"STALLED " : " ", (laser2.buffer_position -
laser2.processed_mark) /
(float)LASER_BUFFER_SIZE * 100.0);
}
if(use_laser3) {
sick_handle_laser(&laser3);
if(laser3.new_reading)
publish_laser_message(&laser3, &laser3_config);
laser3_stalled = (current_time - laser3.timestamp > 1.0);
if(print_stats)
fprintf(stderr, "L3: %s(%.1f%% full) ", laser3_stalled ?
"STALLED " : " ", laser3.buffer_position /
(float)LASER_BUFFER_SIZE * 100.0);
}
if(use_laser4) {
sick_handle_laser(&laser4);
if(laser4.new_reading)
publish_laser_message(&laser4, &laser4_config);
laser4_stalled = (current_time - laser4.timestamp > 1.0);
if(print_stats)
fprintf(stderr, "L4: %s(%.1f%% full) ", laser4_stalled ?
"STALLED " : " ", laser4.buffer_position /
(float)LASER_BUFFER_SIZE * 100.0);
}
if(use_laser5) {
sick_handle_laser(&laser5);
if(laser5.new_reading)
publish_laser_message(&laser5, &laser5_config);
laser5_stalled = (current_time - laser5.timestamp > 1.0);
if(print_stats)
fprintf(stderr, "L5: %s(%.1f%% full) ", laser5_stalled ?
"STALLED " : " ", laser5.buffer_position /
(float)LASER_BUFFER_SIZE * 100.0);
}
if(print_stats) {
fprintf(stderr, "\n");
last_update = current_time;
}
if(current_time - last_alive > 1.0) {
publish_laser_alive(laser1_stalled, laser2_stalled, laser3_stalled,
laser4_stalled, laser5_stalled);
last_alive = current_time;
}
carmen_publish_heartbeat("laser");
return 0;
}
static void velocity_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
void *clientData __attribute__ ((unused)))
{
carmen_robot_velocity_message v;
FORMATTER_PTR formatter;
IPC_RETURN_TYPE err;
formatter = IPC_msgInstanceFormatter(msgRef);
err = IPC_unmarshallData(formatter, callData, &v,
sizeof(carmen_robot_velocity_message));
IPC_freeByteArray(callData);
carmen_test_ipc_return(err, "Could not unmarshall",
IPC_msgInstanceName(msgRef));
time_of_last_command = carmen_get_time();
command_rv = v.rv;
command_tv = v.tv;
following_vector = following_trajectory = 0;
carmen_robot_send_base_velocity_command();
publish_vector_status(0, 0);
IPC_freeDataElements(formatter, &v);
}
static void follow_vector(void)
{
double true_angle_difference, angle_difference, displacement;
double radius;
static void
send_trajectory_to_robot()
{
/*int i; //Uncomment the hole block to print the motion command that it is been sent
for (i = 0; i < NUM_MOTION_COMMANDS_PER_VECTOR; i++)
printf("v = %lf, phi = %lf, t = %lf\n", motion_commands_vector[i].v, motion_commands_vector[i].phi, motion_commands_vector[i].time);*/
carmen_robot_ackerman_publish_motion_command(motion_commands_vector, NUM_MOTION_COMMANDS_PER_VECTOR, carmen_get_time());
}
static void
localize_globalpos_handler(carmen_localize_ackerman_globalpos_message *msg)
{
// if (current_algorithm != CARMEN_BEHAVIOR_SELECTOR_A_STAR)
// return;
IPC_RETURN_TYPE err = IPC_OK;
static int roundValue = 2;
static carmen_ackerman_traj_point_t robot_position;
robot_position.x = round(msg->globalpos.x * roundValue) / roundValue;
robot_position.y = round(msg->globalpos.y * roundValue) / roundValue;
robot_position.theta = round(msg->globalpos.theta * roundValue) / roundValue;
robot_position.v = msg->v;
robot_position.phi = msg->phi;
if (messageControl.carmen_planner_ackerman_update_robot(&robot_position, &robot_config) == 0)
return;
//publica caminho
carmen_planner_status_t status;
messageControl.carmen_planner_ackerman_get_status(&status);
//Mensagem que faz andar
if (status.path.length > 0)
{
carmen_motion_planner_publish_path_message(status.path.points, status.path.length, current_algorithm);
IPC_RETURN_TYPE err;
static int firsttime = 1;
static carmen_navigator_ackerman_astar_goal_list_message goal_list_msg;
if (firsttime)
{
err = IPC_defineMsg(CARMEN_ASTAR_GOAL_LIST_NAME, IPC_VARIABLE_LENGTH, CARMEN_ASTAR_GOAL_LIST_FMT);
carmen_test_ipc_exit(err, "Could not define message", CARMEN_ASTAR_GOAL_LIST_NAME);
goal_list_msg.host = carmen_get_host();
firsttime = 0;
}
goal_list_msg.goal_list = status.path.points;
goal_list_msg.size = status.path.length;
goal_list_msg.timestamp = carmen_get_time();
err = IPC_publishData(CARMEN_ASTAR_GOAL_LIST_NAME, &goal_list_msg);//todo valgrind encontra problema de valor nao inicializado aqui
carmen_test_ipc(err, "Could not publish", CARMEN_ASTAR_GOAL_LIST_NAME);
}
//Mensagem que exibe o caminho na tela
if (status.path.length > 0)
{
static carmen_navigator_ackerman_plan_tree_message plan_tree_msg;
static bool firstTime = true;
if (firstTime == true)
{
err = IPC_defineMsg(CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME, IPC_VARIABLE_LENGTH,
CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_FMT);
carmen_test_ipc_exit(err, "Could not define", CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME);
plan_tree_msg.host = carmen_get_host();
firstTime = false;
}
if (status.path.length > 100)
{ // Ver tipo carmen_navigator_ackerman_plan_tree_message
printf("Error: status.path.length > 100\n");
return;
}
plan_tree_msg.num_path = 1;
memcpy(plan_tree_msg.paths[0], status.path.points, sizeof(carmen_ackerman_traj_point_t) * status.path.length);
plan_tree_msg.path_size[0] = status.path.length;
err = IPC_publishData(CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME, &plan_tree_msg);
carmen_test_ipc(err, "Could not publish", CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME);
}
}
void smart_status_handler(smart_status_message *msg)
{
fprintf(stderr, "S");
carmen_logwrite_write_smart_status(msg, outfile, carmen_get_time() -
logger_starttime);
}
int carmen_localize_get_map(int global, carmen_map_t *map)
{
IPC_RETURN_TYPE err;
carmen_localize_map_query_message msg;
carmen_localize_map_message *response = NULL;
int index;
#ifndef NO_ZLIB
int uncompress_return;
int uncompress_size;
uLongf uncompress_size_result;
unsigned char *uncompressed_data;
#endif
static int initialized = 0;
if (!initialized)
{
err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_QUERY_NAME,
IPC_VARIABLE_LENGTH,
CARMEN_LOCALIZE_MAP_QUERY_FMT);
carmen_test_ipc_exit(err, "Could not define message",
CARMEN_LOCALIZE_MAP_QUERY_NAME);
initialized = 1;
}
msg.map_is_global_likelihood = global;
msg.timestamp = carmen_get_time();
msg.host = carmen_get_host();
err = IPC_queryResponseData(CARMEN_LOCALIZE_MAP_QUERY_NAME, &msg,
(void **)&response, timeout);
carmen_test_ipc(err, "Could not get map", CARMEN_LOCALIZE_MAP_QUERY_NAME);
#ifndef NO_ZLIB
if (response && response->compressed)
{
uncompress_size = response->config.x_size*
response->config.y_size;
uncompressed_data = (unsigned char *)
calloc(uncompress_size, sizeof(float));
carmen_test_alloc(uncompressed_data);
uncompress_size_result = uncompress_size*sizeof(float);
uncompress_return = uncompress((void *)uncompressed_data,
&uncompress_size_result,
(void *)response->data,
response->size);
response->data = uncompressed_data;
response->size = uncompress_size_result;
}
#else
if (response && response->compressed)
{
carmen_warn("Received compressed map from server. This program was\n"
"compiled without zlib support, so this map cannot be\n"
"used. Sorry.\n");
free(response->data);
free(response);
response = NULL;
}
#endif
if (response)
{
if (map)
{
map->config = response->config;
map->complete_map = (float *)response->data;
map->map = (float **)calloc(map->config.x_size, sizeof(float));
carmen_test_alloc(map->map);
for (index = 0; index < map->config.x_size; index++)
map->map[index] = map->complete_map+index*map->config.y_size;
}
else
free(response->data);
free(response);
}
return 0;
}
void gyro_integrated_handler(gyro_integrated_message *msg)
{
fprintf(stderr, "G");
carmen_logwrite_write_gyro_integrated(msg, outfile, carmen_get_time() -
logger_starttime);
}
carmen_test_ipc(err, "Could not publish", CARMEN_FORD_ESCAPE_ERROR_NAME);
}
static void
publish_velocity_message(void *clientData __attribute__ ((unused)), unsigned long currentTime __attribute__ ((unused)), unsigned long scheduledTime __attribute__ ((unused)))
{
// This function publish the low level velocity and steering angle from the car
IPC_RETURN_TYPE err = IPC_OK;
carmen_robot_ackerman_velocity_message robot_ackerman_velocity_message;
static int heartbeat = 0;
robot_ackerman_velocity_message.v = g_XGV_velocity;
robot_ackerman_velocity_message.phi = get_phi_from_curvature(-tan(g_XGV_atan_curvature), ford_escape_hybrid_config);
robot_ackerman_velocity_message.timestamp = carmen_get_time(); // @@ Alberto: era igual a = ford_escape_hybrid_config->XGV_v_and_phi_timestamp;
robot_ackerman_velocity_message.host = carmen_get_host();
err = IPC_publishData(CARMEN_ROBOT_ACKERMAN_VELOCITY_NAME, &robot_ackerman_velocity_message);
carmen_test_ipc(err, "Could not publish ford_escape_hybrid message named carmen_robot_ackerman_velocity_message", CARMEN_ROBOT_ACKERMAN_VELOCITY_NAME);
if ((heartbeat % 10) == 0)
{
publish_car_status();
publish_car_error();
}
heartbeat++;
}
//////////////////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char *argv[]) {
UDPConnectionServer connection(2368);
AcquisitionThread<DataPacket> thread(connection);
Calibration calib;
std::shared_ptr<DataPacket> packet;
IPC_RETURN_TYPE err;
char dump_filename[4096];
carmen_velodyne_ipc_initialize(argc, argv);
carmen_velodyne_read_parameters(argc, argv);
carmen_velodyne_set_spin_rate(spin_rate);
calib_file.open(calib_path);
if (!calib_file.is_open())
carmen_die("\nError: Could not open %s for reading\n", calib_filename);
calib_file >> calib;
calib_file.close();
signal(SIGINT, carmen_velodyne_sigint_handler);
thread.start();
double time;
double start_time = carmen_get_time();
int num_packets = 0;
int start_num_packets = 0;
int num_lost_packets = 0;
int start_num_lost_packets = 0;
while (!quit) {
try {
while (dump_enabled || points_publish) {
packet = thread.getBuffer().dequeue();
++num_packets;
if (dump_enabled) {
if (!dump_file.is_open()) {
char dump_time[1024];
time_t local = packet->getTimestamp();
struct tm* time = localtime(&local);
strftime(dump_time, sizeof(dump_time), "%Y-%m-%d-%H%M%S", time);
sprintf(dump_filename, "%s/%s.bin", dump_path, dump_time);
dump_file.open(dump_filename, std::ios::out | std::ios::binary);
}
if (dump_file.is_open()) {
long dump_filepos = dump_file.tellp();
dump_file << *packet;
carmen_velodyne_publish_packet(id, dump_filename,
dump_filepos, packet->getTimestamp());
}
else {
carmen_warn("\nWarning: Could not open %s for writing\n",
dump_filename);
}
}
if (points_publish) {
VdynePointCloud pointCloud;
Converter::toPointCloud(*packet, calib, pointCloud);
int num_points = pointCloud.getSize();
float x[num_points], y[num_points], z[num_points];
std::vector<VdynePointCloud::Point3D>::const_iterator it;
int i = 0;
for (it = pointCloud.getPointBegin();
it != pointCloud.getPointEnd(); ++it, ++i) {
x[i] = it->mX;
y[i] = it->mY;
z[i] = it->mZ;
}
carmen_velodyne_publish_pointcloud(id, num_points, x, y, z,
packet->getTimestamp());
}
double time = carmen_get_time();
if (time-start_time >= 1.0) {
num_lost_packets = thread.getBuffer().getNumDroppedElements();
float period_num_packets = num_packets-start_num_packets;
float period_num_lost_packets = num_lost_packets-
start_num_lost_packets;
double packet_loss = period_num_lost_packets/
(period_num_packets+period_num_lost_packets);
fprintf(stdout, "\rPacket loss: %6.2f%%", packet_loss*100.0);
fflush(stdout);
start_time = time;
start_num_packets = num_packets;
start_num_lost_packets = num_lost_packets;
}
}
}
catch (IOException& exception) {
// buffer underrun
}
carmen_ipc_sleep(0.0);
}
if (dump_file.is_open())
dump_file.close();
fprintf(stdout, "\n");
return 0;
}
// updates the internal state variables
int carmen_base_direct_update_status(double* packet_timestamp) //
{
// fill in the time stamp and update internal time
double curr_time = carmen_get_time();
//s_delta_time = curr_time - s_time;
s_time = curr_time;
*packet_timestamp = curr_time;
// sonar is not implemented in orc 5 so we don't deal with this
s_left_range = 0.0;
s_right_range = 0.0;
// ir deleted since not being used
// bumpers
s_bumpers[0] = orc_digital_read( s_orc, ORC_BUMPER_PORT0 );
s_bumpers[1] = orc_digital_read( s_orc, ORC_BUMPER_PORT1 );
s_bumpers[2] = orc_digital_read( s_orc, ORC_BUMPER_PORT2 );
s_bumpers[3] = orc_digital_read( s_orc, ORC_BUMPER_PORT3 );
// read encoders
int curr_left_tick = orc_quadphase_read( s_orc, ORC_LEFT_ENCODER_PORT );
int curr_right_tick = orc_quadphase_read( s_orc, ORC_RIGHT_ENCODER_PORT );
int left_delta = compute_delta_ticks_mod( curr_left_tick, s_left_tick ) * ORC_LEFT_REVERSE_ENCODER ;
int right_delta = compute_delta_ticks_mod( curr_right_tick, s_right_tick ) * ORC_RIGHT_REVERSE_ENCODER ;
s_left_displacement = delta_tick_to_metres( left_delta );
s_right_displacement = delta_tick_to_metres( right_delta );
s_left_tick = curr_left_tick;
s_right_tick = curr_right_tick;
// update velocity information
int left_delta_velocity = orc_quadphase_read_velocity_signed( s_orc, ORC_LEFT_ENCODER_PORT );
int right_delta_velocity = orc_quadphase_read_velocity_signed( s_orc, ORC_RIGHT_ENCODER_PORT );
// looking for underflow or overflow
/*
if( left_delta_velocity > SHORT_MAX/2 )
left_delta_velocity = SHORT_MAX - left_delta_velocity;
if( right_delta_velocity > SHORT_MAX/2 )
right_delta_velocity = SHORT_MAX - right_delta_velocity;
if( left_delta_velocity < -1.0 * SHORT_MAX/2 )
left_delta_velocity = SHORT_MAX + left_delta_velocity;
if( right_delta_velocity < -1.0 * SHORT_MAX/2 )
right_delta_velocity = SHORT_MAX + right_delta_velocity;
*/
s_left_velocity = -1.0 * delta_tick_to_metres( left_delta_velocity ) * 60.0 ; //s_left_displacement / s_delta_time;
s_right_velocity = delta_tick_to_metres( right_delta_velocity ) * 60.0 ; //s_right_displacement / s_delta_time;
//printf( " *** ticks: left %d, right %d \n", left_delta, right_delta );
//printf( " *** ticks: left %d, right %d \n", left_delta_velocity , right_delta_velocity );
//printf( " *** vels: left %f, right %f \n", s_left_velocity, s_right_velocity );
// update position information
double displacement = ( s_left_displacement + s_right_displacement )/2;
double rotation = atan2( s_right_displacement - s_left_displacement,
ORC_WHEEL_BASE );
s_x = s_x + displacement*cos( s_theta );
s_y = s_y + displacement*sin( s_theta );
s_theta = carmen_normalize_theta( s_theta + rotation );
return 0;
}
void param_change_handler(carmen_param_variable_change_message *msg)
{
carmen_logwrite_write_param(msg->module_name, msg->variable_name,
msg->value, msg->timestamp,
msg->host, outfile, carmen_get_time());
}
void robot_rearlaser_handler(carmen_robot_laser_message *laser)
{
fprintf(stderr, "R");
carmen_logwrite_write_robot_laser(laser, 2, outfile,
carmen_get_time() - logger_starttime);
}
void base_sonar_handler(carmen_base_sonar_message *sonar)
{
fprintf(stderr, "S");
carmen_logwrite_write_base_sonar(sonar, outfile,
carmen_get_time() - logger_starttime);
}
{
rear_laser_count++;
}
void shutdown_module(int x __attribute__ ((unused)))
{
carmen_ipc_disconnect();
printf("\nDisconnected from laser.\n");
exit(1);
}
void print_statistics(void *clientdata __attribute__ ((unused)),
unsigned long t1 __attribute__ ((unused)),
unsigned long t2 __attribute__ ((unused)))
{
double current_timestamp = carmen_get_time();
fprintf(stderr, "L1 - %.2f Hz L2 - %.2f Hz\n",
front_laser_count / (current_timestamp - first_timestamp),
rear_laser_count / (current_timestamp - first_timestamp));
}
int main(int argc, char **argv)
{
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
signal(SIGINT, shutdown_module);
carmen_laser_subscribe_frontlaser_message(&frontlaser, (carmen_handler_t)
laser_frontlaser_handler,
CARMEN_SUBSCRIBE_LATEST);