void carmen_localize_initialize_gaussian_command(carmen_point_t mean, carmen_point_t std) { static carmen_localize_initialize_message init; static int first = 1; IPC_RETURN_TYPE err; if(first) { err = IPC_defineMsg(CARMEN_LOCALIZE_INITIALIZE_NAME, IPC_VARIABLE_LENGTH, CARMEN_LOCALIZE_INITIALIZE_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_LOCALIZE_INITIALIZE_NAME); first = 0; } init.timestamp = carmen_get_time(); init.host = carmen_get_host(); init.distribution = CARMEN_INITIALIZE_GAUSSIAN; init.num_modes = 1; init.mean = &mean; init.std = &std; err = IPC_publishData(CARMEN_LOCALIZE_INITIALIZE_NAME, &init); carmen_test_ipc(err, "Could not publish", CARMEN_LOCALIZE_INITIALIZE_NAME); }
void carmen_robot_follow_trajectory(carmen_traj_point_t *trajectory, int trajectory_length, carmen_traj_point_t *robot) { IPC_RETURN_TYPE err; static carmen_robot_follow_trajectory_message msg; err = IPC_defineMsg(CARMEN_ROBOT_FOLLOW_TRAJECTORY_NAME, IPC_VARIABLE_LENGTH, CARMEN_ROBOT_FOLLOW_TRAJECTORY_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_ROBOT_FOLLOW_TRAJECTORY_NAME); msg.trajectory = trajectory; msg.trajectory_length = trajectory_length; msg.robot_position = *robot; msg.timestamp = carmen_get_time(); msg.host = carmen_get_host(); err = IPC_publishData(CARMEN_ROBOT_FOLLOW_TRAJECTORY_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_ROBOT_FOLLOW_TRAJECTORY_NAME); }
void publish_navigator_ackerman_plan_message(carmen_ackerman_traj_point_t *points_vector, int size) { carmen_navigator_ackerman_plan_message msg; static int first_time = 1; IPC_RETURN_TYPE err; msg.path = points_vector; msg.path_length = size; msg.host = carmen_get_host(); msg.timestamp = carmen_get_time(); if (first_time) { err = IPC_defineMsg(CARMEN_NAVIGATOR_ACKERMAN_PLAN_NAME, IPC_VARIABLE_LENGTH, CARMEN_NAVIGATOR_ACKERMAN_PLAN_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_NAVIGATOR_ACKERMAN_PLAN_NAME); first_time = 0; } err = IPC_publishData(CARMEN_NAVIGATOR_ACKERMAN_PLAN_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_NAVIGATOR_ACKERMAN_PLAN_NAME); }
int carmen_navigator_set_goal_triplet(carmen_point_p goal) { IPC_RETURN_TYPE err = IPC_OK; carmen_navigator_set_goal_triplet_message msg; static int initialized = 0; if (!initialized) { err = IPC_defineMsg(CARMEN_NAVIGATOR_SET_GOAL_TRIPLET_NAME, IPC_VARIABLE_LENGTH, CARMEN_NAVIGATOR_SET_GOAL_TRIPLET_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_NAVIGATOR_SET_GOAL_TRIPLET_NAME); initialized = 1; } msg.goal = *goal; msg.timestamp = carmen_get_time(); msg.host = carmen_get_host(); err = IPC_publishData(CARMEN_NAVIGATOR_SET_GOAL_TRIPLET_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_NAVIGATOR_SET_GOAL_TRIPLET_NAME); return 0; }
static void publish_carmen_base_ackerman_odometry_message(double timestamp) { IPC_RETURN_TYPE err = IPC_OK; static carmen_base_ackerman_odometry_message odometry; static int first = 1; if (first) { odometry.host = carmen_get_host(); odometry.x = 0; odometry.y = 0; odometry.theta = 0; odometry.v = odometry.phi = 0; first = 0; } odometry.x = car_config->odom_pose.x; odometry.y = car_config->odom_pose.y; odometry.theta = car_config->odom_pose.theta; odometry.v = car_config->v; odometry.phi = car_config->phi; odometry.timestamp = timestamp; err = IPC_publishData(CARMEN_BASE_ACKERMAN_ODOMETRY_NAME, &odometry); carmen_test_ipc(err, "Could not publish base_odometry_message", CARMEN_BASE_ACKERMAN_ODOMETRY_NAME); }
static void carmen_motion_planner_publish_path_message( carmen_ackerman_traj_point_t *path, int size, int algorithm) { IPC_RETURN_TYPE err; static int firsttime = 1; static carmen_motion_planner_path_message msg; if(firsttime) { IPC_RETURN_TYPE err; err = IPC_defineMsg(CARMEN_MOTION_PLANNER_PATH_NAME, IPC_VARIABLE_LENGTH, CARMEN_MOTION_PLANNER_PATH_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_MOTION_PLANNER_PATH_NAME); msg.host = carmen_get_host(); firsttime = 0; } msg.timestamp = carmen_get_time(); msg.path = path; msg.path_size = size; msg.algorithm = algorithm; err = IPC_publishData(CARMEN_MOTION_PLANNER_PATH_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_MOTION_PLANNER_PATH_NAME); }
void carmen_localize_initialize_uniform_command(void) { static carmen_localize_initialize_message init; static int first = 1; IPC_RETURN_TYPE err; if(first) { err = IPC_defineMsg(CARMEN_LOCALIZE_INITIALIZE_NAME, IPC_VARIABLE_LENGTH, CARMEN_LOCALIZE_INITIALIZE_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_LOCALIZE_INITIALIZE_NAME); first = 0; } init.timestamp = carmen_get_time(); init.host = carmen_get_host(); init.distribution = CARMEN_INITIALIZE_UNIFORM; init.num_modes = 0; init.mean = NULL; init.std = NULL; err = IPC_publishData(CARMEN_LOCALIZE_INITIALIZE_NAME, &init); carmen_test_ipc(err, "Could not publish", CARMEN_LOCALIZE_INITIALIZE_NAME); }
void localize_ackerman_velodyne_publish_frontlaser(double timestamp, double *laser_ranges) { IPC_RETURN_TYPE err = IPC_OK; localize_ackerman_velodyne_create_frontlaser_message(timestamp, laser_ranges); err = IPC_publishData(CARMEN_LASER_FRONTLASER_NAME, &flaser_message); carmen_test_ipc(err, "Could not publish laser_frontlaser_message", CARMEN_LASER_FRONTLASER_NAME); }
static void publish_vector_status(double distance, double angle) { static carmen_robot_vector_status_message msg; int err; msg.host = carmen_get_host(); msg.timestamp = carmen_get_time(); msg.vector_distance = distance; msg.vector_angle = angle; err = IPC_publishData(CARMEN_ROBOT_VECTOR_STATUS_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_ROBOT_VECTOR_STATUS_NAME); }
void publish_info_message() { IPC_RETURN_TYPE err; carmen_playback_info_message playback_info_message; playback_info_message.message_number = current_position; playback_info_message.message_timestamp = playback_timestamp; playback_info_message.message_timestamp_difference = playback_starttime + playback_timestamp; playback_info_message.playback_speed = playback_speed; err = IPC_publishData (CARMEN_PLAYBACK_INFO_MESSAGE_NAME, &playback_info_message); carmen_test_ipc (err, "Could not publish", CARMEN_PLAYBACK_INFO_MESSAGE_NAME); timestamp_last_message_published = playback_timestamp; }
static void publish_car_status() { IPC_RETURN_TYPE err = IPC_OK; carmen_ford_escape_status_message msg; msg.g_XGV_velocity = g_XGV_velocity; msg.g_XGV_atan_curvature = g_XGV_atan_curvature; msg.g_XGV_brakes = g_XGV_brakes; msg.g_XGV_gear = g_XGV_gear; msg.g_XGV_headlights_status = g_XGV_headlights_status; msg.g_XGV_horn_status = g_XGV_horn_status; msg.g_XGV_right_front_wheel_speed = g_XGV_right_front_wheel_speed; msg.g_XGV_left_front_wheel_speed = g_XGV_left_front_wheel_speed; msg.g_XGV_right_rear_wheel_speed = g_XGV_right_rear_wheel_speed; msg.g_XGV_left_rear_wheel_speed = g_XGV_left_rear_wheel_speed; msg.g_XGV_main_fuel_supply = g_XGV_main_fuel_supply; msg.g_XGV_main_propulsion = g_XGV_main_propulsion; msg.g_XGV_parking_brake = g_XGV_parking_brake; msg.g_XGV_steering = g_XGV_steering; msg.g_XGV_throttle = g_XGV_throttle; msg.g_XGV_turn_signal = g_XGV_turn_signal; // g_XGV_component_status bit Interpretation F: disengaged, T: engaged (see page 62 of ByWire XGV User Manual, Version 1.5) // 0 (16) Manual override // 1 (17) SafeStop pause relay F: run, T: pause // 2 (18) SafeStop stop relay F: run, T: stop // 3-4 (19-20) SafeStop link status 7 (23) Partial mode override: speed 0: no link, 1: bypass, 2: link good // 5 (21) External E-Stop button F: run, T: stop // 6 (22) Partial mode override: F: computer control, T: manual // steering control // F: computer control, T: manual // control // 8 (24) Door/Liftgate pause F: run, T: pause // 9 (25) Error-caused pause* F: run, T: pause // 10 (26) Emergency manual override* F: disengaged, T: engaged // 11 (27) Steering needs initialization* F: initialized, T: uninitialized // Steering initialization waiting F: initialized, T: user must init // on user input* steering manually or press OK msg.g_XGV_component_status = g_XGV_component_status; // printf("g_XGV_component_status = 0x%x\n", g_XGV_component_status); msg.timestamp = carmen_get_time(); msg.host = carmen_get_host(); err = IPC_publishData(CARMEN_FORD_ESCAPE_STATUS_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_FORD_ESCAPE_STATUS_NAME); }
static void respond_all_doors_msg(MSG_INSTANCE msgRef) { static carmen_dot_all_doors_msg msg; static int first = 1; IPC_RETURN_TYPE err; int i, n; if (first) { first = 0; msg.doors = NULL; strcpy(msg.host, carmen_get_tenchar_host_name()); } for (n = i = 0; i < num_filters; i++) if (filters[i].type == CARMEN_DOT_DOOR) n++; msg.num_doors = n; if (n == 0) msg.doors = NULL; else { msg.doors = (carmen_dot_door_p) realloc(msg.doors, n*sizeof(carmen_dot_door_t)); carmen_test_alloc(msg.doors); for (n = i = 0; i < num_filters; i++) { if (filters[i].type == CARMEN_DOT_DOOR) { msg.doors[n].id = filters[i].id; msg.doors[n].x = filters[i].door_filter.x; msg.doors[n].y = filters[i].door_filter.y; msg.doors[n].theta = filters[i].door_filter.t; msg.doors[n].vx = filters[i].door_filter.px; msg.doors[n].vy = filters[i].door_filter.py; msg.doors[n].vxy = filters[i].door_filter.pxy; msg.doors[n].vtheta = filters[i].door_filter.pt; n++; } } } msg.timestamp = carmen_get_time_ms(); err = IPC_respondData(msgRef, CARMEN_DOT_ALL_DOORS_MSG_NAME, &msg); carmen_test_ipc(err, "Could not respond", CARMEN_DOT_ALL_DOORS_MSG_NAME); }
void carmen_sound_set_volume(int volume) { IPC_RETURN_TYPE err = IPC_OK; carmen_sound_volume_msg msg; msg.timestamp = carmen_get_time_ms(); strcpy(msg.host, carmen_get_tenchar_host_name()); msg.volume = volume; err = IPC_defineMsg(CARMEN_SOUND_VOLUME_MSG_NAME, IPC_VARIABLE_LENGTH, CARMEN_SOUND_VOLUME_MSG_FMT); carmen_test_ipc_exit(err, "Could not define", CARMEN_SOUND_VOLUME_MSG_NAME); err = IPC_publishData(CARMEN_SOUND_VOLUME_MSG_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_SOUND_VOLUME_MSG_NAME); }
void carmen_sound_speech_synth(char *text) { IPC_RETURN_TYPE err = IPC_OK; carmen_sound_speech_synth_msg msg; msg.timestamp = carmen_get_time_ms(); strcpy(msg.host, carmen_get_tenchar_host_name()); msg.text = text; err = IPC_defineMsg(CARMEN_SOUND_SPEECH_SYNTH_MSG_NAME, IPC_VARIABLE_LENGTH, CARMEN_SOUND_SPEECH_SYNTH_MSG_FMT); carmen_test_ipc_exit(err, "Could not define", CARMEN_SOUND_SPEECH_SYNTH_MSG_NAME); err = IPC_publishData(CARMEN_SOUND_SPEECH_SYNTH_MSG_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_SOUND_SPEECH_SYNTH_MSG_NAME); }
void carmen_robot_move_along_vector(double distance, double theta) { IPC_RETURN_TYPE err; static carmen_robot_vector_move_message msg; err = IPC_defineMsg(CARMEN_ROBOT_VECTOR_MOVE_NAME, IPC_VARIABLE_LENGTH, CARMEN_ROBOT_VECTOR_MOVE_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_ROBOT_VECTOR_MOVE_NAME); msg.distance = distance; msg.theta = theta; msg.timestamp = carmen_get_time(); msg.host = carmen_get_host(); err = IPC_publishData(CARMEN_ROBOT_VECTOR_MOVE_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_ROBOT_VECTOR_MOVE_NAME); }
static void publish_vector_status(double distance, double angle) { static carmen_robot_vector_status_message msg; static int first = 1; int err; if (first) { strcpy(msg.host, carmen_get_tenchar_host_name()); first = 0; } msg.timestamp = carmen_get_time_ms(); msg.vector_distance = distance; msg.vector_angle = angle; err = IPC_publishData(CARMEN_ROBOT_VECTOR_STATUS_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_ROBOT_VECTOR_STATUS_NAME); }
static void respond_all_people_msg(MSG_INSTANCE msgRef) { static carmen_dot_all_people_msg msg; static int first = 1; IPC_RETURN_TYPE err; int i, n; if (first) { first = 0; msg.people = NULL; strcpy(msg.host, carmen_get_tenchar_host_name()); } for (n = i = 0; i < num_filters; i++) if (filters[i].type == CARMEN_DOT_PERSON) n++; msg.num_people = n; if (n == 0) msg.people = NULL; else { msg.people = (carmen_dot_person_p) realloc(msg.people, n*sizeof(carmen_dot_person_t)); carmen_test_alloc(msg.people); for (n = i = 0; i < num_filters; i++) { if (filters[i].type == CARMEN_DOT_PERSON) { msg.people[n].id = filters[i].id; msg.people[n].x = filters[i].person_filter.x; msg.people[n].y = filters[i].person_filter.y; msg.people[n].vx = filters[i].person_filter.px; msg.people[n].vy = filters[i].person_filter.py; msg.people[n].vxy = filters[i].person_filter.pxy; n++; } } } msg.timestamp = carmen_get_time_ms(); err = IPC_respondData(msgRef, CARMEN_DOT_ALL_PEOPLE_MSG_NAME, &msg); carmen_test_ipc(err, "Could not respond", CARMEN_DOT_ALL_PEOPLE_MSG_NAME); }
void carmen_robot_velocity_command(double tv, double rv) { IPC_RETURN_TYPE err; static carmen_robot_velocity_message v; err = IPC_defineMsg(CARMEN_ROBOT_VELOCITY_NAME, IPC_VARIABLE_LENGTH, CARMEN_ROBOT_VELOCITY_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_ROBOT_VELOCITY_NAME); v.tv = tv; v.rv = rv; v.host = carmen_get_host(); v.timestamp = carmen_get_time(); err = IPC_publishData(CARMEN_ROBOT_VELOCITY_NAME, &v); carmen_test_ipc(err, "Could not publish", CARMEN_ROBOT_VELOCITY_NAME); }
static void publish_car_error() { IPC_RETURN_TYPE err = IPC_OK; carmen_ford_escape_error_message msg; int i; msg.num_errors = g_XGV_num_errors; msg.error = calloc(msg.num_errors,sizeof(int)); for (i = 0; i < msg.num_errors; i++) { msg.error[i] = g_XGV_error[i]; } msg.timestamp = carmen_get_time(); msg.host = carmen_get_host(); err = IPC_publishData(CARMEN_FORD_ESCAPE_ERROR_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_FORD_ESCAPE_ERROR_NAME); }
int carmen_navigator_set_goal_place(char *placename) { IPC_RETURN_TYPE err = IPC_OK; carmen_navigator_placename_message msg; carmen_navigator_return_code_message *return_msg; int return_code; static int initialized = 0; if (!initialized) { err = IPC_defineMsg(CARMEN_NAVIGATOR_SET_GOAL_PLACE_NAME, IPC_VARIABLE_LENGTH, CARMEN_NAVIGATOR_SET_GOAL_PLACE_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_NAVIGATOR_SET_GOAL_PLACE_NAME); initialized = 1; } msg.placename = placename; msg.timestamp = carmen_get_time(); msg.host = carmen_get_host(); err = IPC_queryResponseData(CARMEN_NAVIGATOR_SET_GOAL_PLACE_NAME, &msg, (void **)&return_msg, timeout); carmen_test_ipc(err, "Could not set goal by place", CARMEN_NAVIGATOR_SET_GOAL_PLACE_NAME); if (err == IPC_OK) { if (return_msg->code) { carmen_warn("%s", return_msg->error); free(return_msg->error); } return_code = return_msg->code; free(return_msg); } else return_code = err; return return_code; }
void carmen_robot_send_base_velocity_command(void) { IPC_RETURN_TYPE err; static carmen_base_velocity_message v; v.host = carmen_get_host(); if (collision_avoidance) { #ifndef COMPILE_WITHOUT_LASER_SUPPORT if (use_laser) { command_tv = carmen_clamp(carmen_robot_laser_min_rear_velocity(), command_tv, carmen_robot_laser_max_front_velocity()); } #endif if (use_sonar) command_tv = carmen_clamp(carmen_robot_sonar_min_rear_velocity(), command_tv, carmen_robot_sonar_max_front_velocity()); if (use_bumper && carmen_robot_bumper_on()) { command_tv = 0; command_rv = 0; } } if (!carmen_robot_config.allow_rear_motion && command_tv < 0) command_tv = 0.0; v.tv = carmen_clamp(-carmen_robot_config.max_t_vel, command_tv, carmen_robot_config.max_t_vel); v.rv = carmen_clamp(-carmen_robot_config.max_r_vel, command_rv, carmen_robot_config.max_r_vel); v.timestamp = carmen_get_time(); err = IPC_publishData(CARMEN_BASE_VELOCITY_NAME, &v); carmen_test_ipc(err, "Could not publish", CARMEN_BASE_VELOCITY_NAME); }
void carmen_localize_initialize_placename_command(char *placename) { static carmen_localize_initialize_placename_message init; static int first = 1; IPC_RETURN_TYPE err; if(first) { err = IPC_defineMsg(CARMEN_LOCALIZE_INITIALIZE_PLACENAME_NAME, IPC_VARIABLE_LENGTH, CARMEN_LOCALIZE_INITIALIZE_PLACENAME_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_LOCALIZE_INITIALIZE_PLACENAME_NAME); first = 0; } init.timestamp = carmen_get_time(); init.host = carmen_get_host(); init.placename = placename; err = IPC_publishData(CARMEN_LOCALIZE_INITIALIZE_PLACENAME_NAME, &init); carmen_test_ipc(err, "Could not publish", CARMEN_LOCALIZE_INITIALIZE_PLACENAME_NAME); }
void carmen_robot_send_base_binary_command(unsigned char *data, int length) { IPC_RETURN_TYPE err; static carmen_base_binary_data_message msg; err = IPC_defineMsg(CARMEN_BASE_BINARY_COMMAND_NAME, IPC_VARIABLE_LENGTH, CARMEN_BASE_BINARY_COMMAND_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_BASE_BINARY_COMMAND_NAME); msg.data = data; msg.size = length; msg.timestamp = carmen_get_time(); msg.host = carmen_get_host(); err = IPC_publishData(CARMEN_BASE_BINARY_COMMAND_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_BASE_BINARY_COMMAND_NAME); }
void carmen_logger_comment(char *text) { IPC_RETURN_TYPE err; static carmen_logger_comment_message msg; static int first = 1; if(first) { msg.host = carmen_get_host(); err = IPC_defineMsg(CARMEN_LOGGER_COMMENT_NAME, IPC_VARIABLE_LENGTH, CARMEN_LOGGER_COMMENT_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_LOGGER_COMMENT_NAME); first = 0; } msg.text = text; msg.timestamp = carmen_get_time(); err = IPC_publishData(CARMEN_LOGGER_COMMENT_NAME, &msg); carmen_test_ipc(err, "Could not publish", CARMEN_LOGGER_COMMENT_NAME); }
int carmen_navigator_go(void) { IPC_RETURN_TYPE err; carmen_navigator_go_message *msg; static int initialized = 0; if (!initialized) { err = IPC_defineMsg(CARMEN_NAVIGATOR_GO_NAME, IPC_VARIABLE_LENGTH, CARMEN_DEFAULT_MESSAGE_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_NAVIGATOR_GO_NAME); initialized = 1; } msg = carmen_default_message_create(); err = IPC_publishData(CARMEN_NAVIGATOR_GO_NAME, msg); carmen_test_ipc(err, "Could not publish", CARMEN_NAVIGATOR_GO_NAME); return 0; }
void carmen_playback_command(int command, int argument, float speed) { IPC_RETURN_TYPE err; carmen_playback_command_message playback_msg; static int initialized = 0; if (!initialized) { err = IPC_defineMsg(CARMEN_PLAYBACK_COMMAND_NAME, IPC_VARIABLE_LENGTH, CARMEN_PLAYBACK_COMMAND_FMT); carmen_test_ipc_exit(err, "Could not define message", CARMEN_PLAYBACK_COMMAND_NAME); } playback_msg.cmd = command; playback_msg.arg = argument; playback_msg.speed = speed; err = IPC_publishData(CARMEN_PLAYBACK_COMMAND_NAME, &playback_msg); carmen_test_ipc(err, "Could not publish", CARMEN_PLAYBACK_COMMAND_NAME); }
void carmen_robot_send_base_velocity_command(void) { IPC_RETURN_TYPE err; char *host; static carmen_base_velocity_message v; static int first = 1; if(first) { host = carmen_get_tenchar_host_name(); strcpy(v.host, host); first = 0; } v.tv = carmen_clamp(-carmen_robot_config.max_t_vel, command_tv, carmen_robot_config.max_t_vel); v.rv = carmen_clamp(-carmen_robot_config.max_r_vel, command_rv, carmen_robot_config.max_r_vel); v.timestamp = carmen_get_time_ms(); err = IPC_publishData(CARMEN_BASE_VELOCITY_NAME, &v); carmen_test_ipc(err, "Could not publish", CARMEN_BASE_VELOCITY_NAME); }
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); } }
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; }