void navigator_stop_moving(void)
{
if (!carmen_navigator_stop())
carmen_verbose("Said stop\n");
else
carmen_verbose("Could not say stop\n");
}
void navigator_start_moving(void)
{
if (!carmen_navigator_go())
carmen_verbose("Said go!\n");
else
carmen_verbose("could not say go!\n");
}
void carmen_joystick_control(carmen_joystick_type *joystick, double max_tv,
double max_rv, double *tv, double *rv,
int *tilt, int *fire)
{
if (joystick->initialized == 0)
return;
float range;
range = (float)(CEREBELLUM_TILT_MIN - CEREBELLUM_TILT_MAX);
if( range < 0) range *= -1.0;
*tv = joystick->axes[1] / 32767.0 * max_tv;
*rv = -1 * joystick->axes[0] / 32767.0 * max_rv;
*tilt =(int)( (joystick->axes[3]/ 32767.0 +1.0)/2.0 * range + (float) CEREBELLUM_TILT_MAX);
*fire = joystick->buttons[0];
carmen_verbose("axes[0]: %d axes[1]: %d tv: %f rv: %f\n",
joystick->axes[0], joystick->axes[1], *tv, *rv);
// printf("axes[0]: %d axes[1]: %d axes[2]: %d axes[3]: %d\n",
// joystick->axes[0], joystick->axes[1],joystick->axes[2],joystick->axes[3]);
// printf("buttons[0]: %d buttons[1]: %d buttons[2]: %d\n",
// joystick->buttons[0], joystick->buttons[1],joystick->buttons[2]);
//printf("calc tilt: %d\n", *tilt);
//printf("calc fire: %d\n", *fire);
}
static void
shutdown_grid_mapping_view(int x)
{
if(x == SIGINT) {
carmen_verbose("Disconnecting Grid Mapping View Service.\n");
exit(1);
}
}
void shutdown_xsens(int x)
{
if(x == SIGINT) {
carmen_verbose("Disconnecting from IPC network.\n");
shutDownXsens(cmt3);
delete packet;
exit(1);
}
}
static void
shutdown_traffic_light_view(int x)
{
/* release memory */
cvReleaseImage(&right_image);
/* exit module */
if (x == SIGINT)
{
carmen_verbose("Disconnecting Traffic Light View Service.\n");
carmen_ipc_disconnect();
exit(1);
}
}
void navigator_update_robot(carmen_world_point_p robot)
{
carmen_point_t std = {0.2, 0.2, carmen_degrees_to_radians(4.0)};
if (robot == NULL) {
carmen_localize_initialize_uniform_command();
} else {
carmen_verbose("Set robot position to %d %d %f\n",
carmen_round(robot->pose.x),
carmen_round(robot->pose.y),
carmen_radians_to_degrees(robot->pose.theta));
carmen_localize_initialize_gaussian_command(robot->pose, std);
}
}
static void
shutdown_v_disparitity_view(int x)
{
/* release memory */
cvReleaseImage(&right_image);
cvReleaseImage(&v_disparity_map);
cvReleaseImage(&road_profile_image);
/* exit module */
if(x == SIGINT)
{
carmen_verbose("Disconnecting V-Disparity View Service.\n");
exit(1);
}
}
static void write_pixbuf_as_png(GdkPixbuf *pixbuf, char *user_filename)
{
char basefilename[100] = "video";
char filename[100];
GError *error;
static int image_count = 0;
if (user_filename == NULL)
sprintf(filename, "%s%04d.png", basefilename, image_count);
else
snprintf(filename, 100, "%s", user_filename);
carmen_verbose("Saving image to %s... ", filename);
error = NULL;
gdk_pixbuf_save(pixbuf, filename, "png", &error, NULL);
image_count++;
}
void navigator_status_handler(carmen_navigator_status_message *msg)
{
carmen_traj_point_t new_robot;
carmen_world_point_t new_goal;
carmen_verbose("Got Status message: Robot %.1f %.1f %.2f Goal: %.0f %.0f\n",
msg->robot.x, msg->robot.y, msg->robot.theta,
msg->goal.x, msg->goal.y);
last_navigator_status = msg->timestamp;
new_robot = msg->robot;
new_goal.map = map;
if (!is_graphics_up)
return;
if (msg->goal_set) {
new_goal.pose = msg->goal;
navigator_graphics_update_display(&new_robot, &new_goal, msg->autonomous);
} else
navigator_graphics_update_display(&new_robot, NULL, msg->autonomous);
}
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 carmen_conventional_build_costs(carmen_robot_config_t *robot_conf,
carmen_map_point_t *robot_posn,
carmen_navigator_config_t *navigator_conf)
{
int x_index = 0, y_index = 0;
double value;
double *cost_ptr;
float *map_ptr;
double resolution;
int index;
int x, y;
int x_start, y_start;
int x_end, y_end;
double robot_distance;
carmen_verbose("Building costs...");
if (x_size != carmen_planner_map->config.x_size ||
y_size != carmen_planner_map->config.y_size) {
free(costs);
costs = NULL;
free(utility);
utility = NULL;
}
x_size = carmen_planner_map->config.x_size;
y_size = carmen_planner_map->config.y_size;
if (costs == NULL) {
costs = (double *)calloc(x_size*y_size, sizeof(double));
carmen_test_alloc(costs);
for (index = 0; index < x_size*y_size; index++)
costs[index] = carmen_planner_map->complete_map[index];
}
resolution = carmen_planner_map->config.resolution;
if (robot_posn && navigator_conf) {
x_start = robot_posn->x - navigator_conf->map_update_radius/
robot_posn->map->config.resolution;
x_start = carmen_clamp(0, x_start, x_size);
y_start = robot_posn->y - navigator_conf->map_update_radius/
robot_posn->map->config.resolution;
y_start = carmen_clamp(0, y_start, y_size);
x_end = robot_posn->x + navigator_conf->map_update_radius/
robot_posn->map->config.resolution;
x_end = carmen_clamp(0, x_end, x_size);
y_end = robot_posn->y + navigator_conf->map_update_radius/
robot_posn->map->config.resolution;
y_end = carmen_clamp(0, y_end, y_size);
cost_ptr = costs+x_start*y_size;
map_ptr = carmen_planner_map->complete_map+x_start*y_size;
for (x_index = x_start; x_index < x_end; x_index++) {
for (y_index = y_start; y_index < y_end; y_index++)
cost_ptr[y_index] = map_ptr[y_index];
cost_ptr += y_size;
map_ptr += y_size;
}
} else {
x_start = 0;
y_start = 0;
x_end = x_size;
y_end = y_size;
for (index = 0; index < x_size*y_size; index++)
costs[index] = carmen_planner_map->complete_map[index];
}
/* Initialize cost function to match map, where empty cells have
MIN_COST cost and filled cells have MAX_UTILITY cost */
for (x_index = x_start; x_index < x_end; x_index++) {
cost_ptr = costs+x_index*y_size+y_start;
for (y_index = y_start; y_index < y_end; y_index++) {
value = *(cost_ptr);
if (value >= 0 && value < MIN_COST)
value = MIN_COST;
else
value = MAX_UTILITY;
*(cost_ptr++) = value;
}
}
/* Loop through cost map, starting at top left, updating cost of cell
as max of itself and most expensive neighbour less the cost
downgrade. */
for (x_index = x_start; x_index < x_end; x_index++) {
cost_ptr = costs+x_index*y_size+y_start;
for (y_index = y_start; y_index < y_end; y_index++, cost_ptr++) {
if (x_index < 1 || x_index >= x_size-1 || y_index < 1 ||
y_index >= y_size-1)
continue;
for (index = 0; index < NUM_ACTIONS; index++) {
x = x_index + carmen_planner_x_offset[index];
y = y_index + carmen_planner_y_offset[index];
value = *(costs+x*y_size+y) - resolution*MAX_UTILITY;
if (value > *cost_ptr)
*cost_ptr = value;
}
}
}
/* Loop through cost map again, starting at bottom right, updating cost of
cell as max of itself and most expensive neighbour less the cost
downgrade. */
for (x_index = x_end-1; x_index >= x_start; x_index--) {
cost_ptr = costs+x_index*y_size+y_end-1;
for (y_index = y_end-1; y_index >= y_start; y_index--, cost_ptr--) {
if (x_index < 1 || x_index >= x_size-1 || y_index < 1 ||
y_index >= y_size-1)
continue;
for (index = 0; index < NUM_ACTIONS; index++) {
x = x_index + carmen_planner_x_offset[index];
y = y_index + carmen_planner_y_offset[index];
value = *(costs+x*y_size+y) - resolution*MAX_UTILITY;
if (value > *cost_ptr)
*cost_ptr = value;
}
}
}
robot_distance = robot_conf->width/2*MAX_UTILITY;
/* Clamp any cell that's closer than the robot width to be
impassable. Also, rescale cost function so that the max cost
is 0.5 */
for (x_index = x_start; x_index < x_end; x_index++) {
cost_ptr = costs+x_index*y_size+y_start;
for (y_index = y_start; y_index < y_end; y_index++) {
value = *(cost_ptr);
if (value < MAX_UTILITY - robot_distance) {
value = value / (2*MAX_UTILITY);
if (value < MIN_COST)
value = MIN_COST;
} else
value = 1.0;
*(cost_ptr++) = value;
}
}
if (robot_posn != NULL) {
x_index = robot_posn->x / carmen_planner_map->config.resolution;
y_index = robot_posn->y / carmen_planner_map->config.resolution;
if (x_index >= 0 && x_index < x_size && y_index >= 0 && y_index < y_size)
*(costs+x_index*y_size+y_index) = MIN_COST;
}
carmen_verbose("done\n");
}
static void
set_param(char *lvalue, char *rvalue, int param_level)
{
int param_index;
char module[255], variable[255];
int num_items;
param_index = lookup_name(lvalue);
if (param_index == -1)
{
num_items = sscanf(lvalue, "%[^_]_%s", module, variable);
if (num_items != 2) {
carmen_warn("Ill-formed parameter name %s%s%s. Could not find "
"module and variable name.\n"
"Not setting this parameter.\n", carmen_red_code,
lvalue, carmen_normal_code);
return;
}
check_param_space();
param_index = num_params;
num_params++;
param_list[param_index].lvalue = (char *)calloc
(strlen(lvalue)+1, sizeof(char));
carmen_test_alloc(param_list[param_index].lvalue);
strcpy(param_list[param_index].lvalue, lvalue);
param_list[param_index].module_name = (char *)calloc
(strlen(module)+1, sizeof(char));
carmen_test_alloc(param_list[param_index].module_name);
strcpy(param_list[param_index].module_name, module);
if (lookup_module(module) == -1)
add_module(module);
param_list[param_index].variable_name = (char *)calloc
(strlen(variable)+1, sizeof(char));
carmen_test_alloc(param_list[param_index].variable_name);
strcpy(param_list[param_index].variable_name, variable);
if (param_level == PARAM_LEVEL_NOCHANGE)
param_level = PARAM_LEVEL_BASIC;
}
else
{
free(param_list[param_index].rvalue);
}
param_list[param_index].rvalue = (char *)calloc
(strlen(rvalue)+1, sizeof(char));
carmen_test_alloc(param_list[param_index].rvalue);
strcpy(param_list[param_index].rvalue, rvalue);
if (param_level != PARAM_LEVEL_NOCHANGE)
param_list[param_index].expert = param_level;
carmen_verbose("Added %s %s%s: %s = %s \n",
param_list[param_index].module_name,
param_list[param_index].variable_name,
param_list[param_index].expert ? " (expert)" : "",
param_list[param_index].lvalue,
param_list[param_index].rvalue);
publish_new_param(param_index);
}
void navigator_set_goal(double x, double y)
{
carmen_verbose("Set goal to %.1f %.1f\n", x, y);
carmen_navigator_set_goal(x, y);
}
void navigator_set_goal(double x, double y, double theta)
{
carmen_verbose("Set goal to %.1f %.1f\n", x, y);
carmen_navigator_ackerman_set_goal(x, y, theta);
IPC_listen(50);
}
