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;
}