void
MessageControl::plan()
{
int goal_x, goal_y;
goal_x = carmen_round((this->requested_goal->x - carmen_planner_map->config.x_origin) / carmen_planner_map->config.resolution);
goal_y = carmen_round((this->requested_goal->y - carmen_planner_map->config.y_origin) / carmen_planner_map->config.resolution);
carmen_conventional_dynamic_program(goal_x , goal_y);
}
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 carmen_inline void world_to_screen(carmen_world_point_t *wp, carmen_point_t *p,
GtkMapViewer *map_view)
{
carmen_point_t mp;
carmen_world_to_map_real(wp, &mp);
mp.y = map_view->internal_map->config.y_size - mp.y;
p->x = mp.x * map_view->rescale_size;
p->y = mp.y * map_view->rescale_size;
p->x -= map_view->x_scroll_adj->value;
p->y -= map_view->y_scroll_adj->value;
p->x = carmen_round(p->x);
p->y = carmen_round(p->y);
}
void interpret_params(sick_laser_p laser, char *dev, char *type, double res, char *rem, double fov)
{
strcpy(laser->settings.device_name, dev);
if(strcmp(type, "LMS") == 0)
laser->settings.type = LMS;
else if(strcmp(type, "PLS") == 0)
laser->settings.type = PLS;
if (fabs(fov-M_PI) < 0.1 || fabs(fov-100.0/180.0*M_PI) < 0.1)
carmen_die("The parameter laser_laserX_fov in the ini file must\nbe specified in degrees not in radians!\n");
laser->settings.angle_range = carmen_round(fov);
if ( laser->settings.angle_range != 180 &&
laser->settings.angle_range != 100 )
carmen_die("The laser driver only provides 180 deg and 100 deg field of view!\n");
if(res == 0.25) {
laser->settings.angle_resolution = RES_0_25_DEGREE;
laser->settings.angle_range = 100;
}
else if(res == 0.5)
laser->settings.angle_resolution = RES_0_50_DEGREE;
else
laser->settings.angle_resolution = RES_1_00_DEGREE;
/* remission values - start */
if(strcmp(rem, "direct") == 0) {
laser->settings.use_remission = 1;
laser->settings.range_dist = SICK_REMISSION_DIRECT;
}
else if(strcmp(rem, "normalized") == 0) {
laser->settings.use_remission = 1;
laser->settings.range_dist = SICK_REMISSION_NORM;
}
else if(strcmp(rem, "no") == 0) {
laser->settings.use_remission = 0;
}
else if(strcmp(rem, "off") == 0) {
laser->settings.use_remission = 0;
carmen_warn("Warning: please set the value of the parameter \nlaser_use_remission to \"no\" and do not use \"off\".\nAssuming \"no\" for now.\n\n");
}
else carmen_die("ERROR: Parameter laser_use_remission for laser %d has invalid value: %s\nPossible values are: direct, normalized and off.\n", laser->settings.laser_num, rem);
/* remission values - stop */
check_parameter_settings(laser);
}
void
init_message_laser_params(int laser_id, double accuracy, double fov, double resolution, double maxrange)
{
fov_in_degrees = fov;
message_laser.id = laser_id;
message_laser.num_remissions=0;
message_laser.config.remission_mode = REMISSION_NONE;
message_laser.config.laser_type = LASER_EMULATED_USING_KINECT;
message_laser.config.fov = carmen_degrees_to_radians(fov);
message_laser.config.start_angle = -0.5 * message_laser.config.fov;
message_laser.config.angular_resolution = carmen_degrees_to_radians(resolution);
message_laser.num_readings=1 + carmen_round(message_laser.config.fov / message_laser.config.angular_resolution);
message_laser.config.maximum_range = maxrange;
message_laser.config.accuracy = accuracy;
message_laser.host = carmen_get_host();
message_laser.range = laser_ranges;
}
void
carmen_global_update_progress_bar(int count, int size)
{
char buffer[20];
int percent = carmen_round(count*10.0/size);
if (percent > 10)
{
carmen_warn("\nThere seems to be a bug in your call to %s, \n"
"because you called it with count > size. count truncated to size.\n",
__FUNCTION__);
percent = 10;
}
if (percent > current_percent)
{
memset(buffer, '#', percent*sizeof(char));
buffer[percent] = '\0';
fprintf(stderr, "%s[%dD", buffer, percent);
current_percent = percent;
}
}
static void
fill_laser_config_data(localize_ackerman_velodyne_laser_config_t *lasercfg)
{
lasercfg->num_lasers = 1 + carmen_round(lasercfg->fov / lasercfg->angular_resolution);
lasercfg->start_angle = -0.5 * lasercfg->fov;
/* give a warning if it is not a standard configuration */
if (fabs(lasercfg->fov - M_PI) > 1e-6 &&
fabs(lasercfg->fov - 100.0/180.0 * M_PI) > 1e-6 &&
fabs(lasercfg->fov - 90.0/180.0 * M_PI) > 1e-6)
carmen_warn("Warning: You are not using a standard SICK configuration (fov=%.4f deg)\n",
carmen_radians_to_degrees(lasercfg->fov));
if (fabs(lasercfg->angular_resolution - carmen_degrees_to_radians(1.0)) > 1e-6 &&
fabs(lasercfg->angular_resolution - carmen_degrees_to_radians(0.5)) > 1e-6 &&
fabs(lasercfg->angular_resolution - carmen_degrees_to_radians(0.25)) > 1e-6)
carmen_warn("Warning: You are not using a standard SICK configuration (res=%.4f deg)\n",
carmen_radians_to_degrees(lasercfg->angular_resolution));
}
// returns 1 if point is in map
static int map_filter(double x, double y, double r) {
carmen_world_point_t wp;
carmen_map_point_t mp;
double d;
int md, i, j;
wp.map = &static_map;
wp.pose.x = x;
wp.pose.y = y;
carmen_world_to_map(&wp, &mp);
d = (map_diff_threshold + r*map_diff_threshold_scalar)/static_map.config.resolution;
md = carmen_round(d);
for (i = mp.x-md; i <= mp.x+md; i++)
for (j = mp.y-md; j <= mp.y+md; j++)
if (is_in_map(i, j) && dist(i-mp.x, j-mp.y) <= d &&
exp(localize_map.gprob[i][j]) >= map_occupied_threshold)
return 1;
return 0;
}
// ---- BASE MOVEMENT COMMANDS ---- //
void carmen_base_command_velocity(double desired_velocity,
double current_velocity,
int WHICH_MOTOR)
{
double desired_angular_velocity, current_angular_velocity;
double pTerm = 0.0, dTerm = 0.0, iTerm = 0.0;
double velErrorPrev, angular_velocity_error_prev;
double angular_velocity_error = 0.0;
int current_pwm;
int command_pwm;
int ignore_d_term;
/*
printf( "motor %d desired velocity %f, current velocity %f\n",
WHICH_MOTOR, desired_velocity, current_velocity);
*/
// get the values for the appropriate motor
if (WHICH_MOTOR == ORC_LEFT_MOTOR) {
velErrorPrev = s_left_error_prev;
current_pwm = s_left_pwm;
ignore_d_term = s_ignore_left_d_term;
} else {
velErrorPrev = s_right_error_prev;
current_pwm = s_right_pwm;
ignore_d_term = s_ignore_right_d_term;
}
// convert to angular velocity
desired_angular_velocity = desired_velocity / (ORC_WHEEL_DIAMETER/2.0);
current_angular_velocity = current_velocity / (ORC_WHEEL_DIAMETER/2.0);
angular_velocity_error_prev = velErrorPrev / (ORC_WHEEL_DIAMETER/2.0);
// if the angular velocity is greater than min, do PID
if (fabs(desired_angular_velocity) > .001) {
// set the angular velocity between bounds
if (desired_angular_velocity > ORC_MAX_ANGULAR_VEL)
desired_angular_velocity = (double)ORC_MAX_ANGULAR_VEL;
if (desired_angular_velocity < -ORC_MAX_ANGULAR_VEL)
desired_angular_velocity = -1.0 * (double)ORC_MAX_ANGULAR_VEL;
// compute the PID terms
angular_velocity_error = (desired_angular_velocity - current_angular_velocity);
pTerm = angular_velocity_error * (double)ORC_P_GAIN;
iTerm = angular_velocity_error_prev * (double)ORC_I_GAIN;
if (!ignore_d_term)
dTerm = (angular_velocity_error - angular_velocity_error_prev ) * (double)ORC_D_GAIN;
else {
dTerm = 0;
ignore_d_term = 0;
}
// change current pwm accordingly -- ?? -- sign?
double correction = pTerm + iTerm + dTerm;
command_pwm = current_pwm + carmen_round( correction );
/*
printf( " command pwm %d, current %d, correction %f, rounded correction %d \n"
, command_pwm , current_pwm, correction, carmen_round( correction ) );
*/
// make sure that the command pwm is within bounds
// note: should this be the only check vs. also bounding the ang vel??
if (abs(command_pwm) > ORC_MAX_PWM)
command_pwm = (command_pwm > 0 ? ORC_MAX_PWM : -ORC_MAX_PWM);
// update values for the appropriate motor
if (WHICH_MOTOR == ORC_LEFT_MOTOR) {
s_left_error_prev = angular_velocity_error * (ORC_WHEEL_DIAMETER/2.0);
s_ignore_left_d_term = ignore_d_term;
s_left_pwm = command_pwm;
} else {
s_right_error_prev = angular_velocity_error * (ORC_WHEEL_DIAMETER/2.0);
s_ignore_right_d_term = ignore_d_term;
s_right_pwm = command_pwm;
}
// debug outputs
//printf("Setting motor %d: current pwm: %d command pwm %d angular error= %f p=%f i %f d %f\n",
//WHICH_MOTOR, current_pwm, command_pwm , angular_velocity_error,
//pTerm, iTerm, dTerm);
/*
printf(" with PID terms p %f, i %f, d %f \n",
pTerm, iTerm, dTerm );
printf(" to correct: des_ang_vel %f, act_ang_vel %f, error %f \n",
desired_angular_velocity, current_angular_velocity, angular_velocity_error );
*/
} else {
command_pwm = 0;
}
// set the pwm
printf("|M%d to %d|\n", WHICH_MOTOR, command_pwm );
orc_motor_set_signed( s_orc, WHICH_MOTOR, command_pwm );
}
void
map_modify_update(carmen_robot_laser_message *laser_msg,
carmen_navigator_config_t *config,
carmen_world_point_p world_point,
carmen_map_p true_map,
carmen_map_p modify_map)
{
int index;
double angle, separation;
int laser_x, laser_y;
double dist;
int increment;
carmen_map_point_t map_point;
int count;
int maxrange_beam;
if (!config->map_update_freespace && !config->map_update_obstacles)
return;
if (true_map == NULL || modify_map == NULL)
{
carmen_warn("%s called with NULL map argument.\n", __FUNCTION__);
return;
}
if (laser_scan == NULL) {
laser_scan = (grid_cell_p *)calloc(LASER_HISTORY_LENGTH, sizeof(grid_cell_p));
carmen_test_alloc(laser_scan);
scan_size = (int *)calloc(LASER_HISTORY_LENGTH, sizeof(int));
carmen_test_alloc(scan_size);
max_scan_size = (int *)calloc(LASER_HISTORY_LENGTH, sizeof(int));
carmen_test_alloc(max_scan_size);
}
update_existing_data(true_map, modify_map);
if (laser_msg->num_readings < config->num_lasers_to_use) {
increment = 1;
separation = laser_msg->config.angular_resolution;
}
else {
increment = laser_msg->num_readings / config->num_lasers_to_use;
if (increment != 1)
separation = carmen_normalize_theta(laser_msg->config.fov / (config->num_lasers_to_use+1));
else
separation = laser_msg->config.angular_resolution;
}
angle = carmen_normalize_theta(world_point->pose.theta +
laser_msg->config.start_angle);
carmen_world_to_map(world_point, &map_point);
count = 0;
for (index = 0; index < laser_msg->num_readings; index += increment) {
if (laser_msg->range[index] < laser_msg->config.maximum_range )
maxrange_beam = 0;
else
maxrange_beam = 1;
if (config->map_update_freespace) {
dist = laser_msg->range[index] - modify_map->config.resolution;
if (dist < 0)
dist = 0;
if (dist > config->map_update_radius)
dist = config->map_update_radius;
laser_x = carmen_round(map_point.x + (cos(angle)*dist)/
modify_map->config.resolution);
laser_y = carmen_round(map_point.y + (sin(angle)*dist)/
modify_map->config.resolution);
trace_laser(map_point.x, map_point.y, laser_x, laser_y, true_map, modify_map);
}
if (config->map_update_obstacles) {
// add obstacle only if it is not a maxrange reading!
if (!maxrange_beam) {
dist = laser_msg->range[index];
laser_x = carmen_round(map_point.x + (cos(angle)*dist)/
modify_map->config.resolution);
laser_y = carmen_round(map_point.y + (sin(angle)*dist)/
modify_map->config.resolution);
if (is_in_map(laser_x, laser_y, modify_map) &&
dist < config->map_update_radius &&
dist < (laser_msg->config.maximum_range - 2*modify_map->config.resolution)) {
count++;
add_filled_point(laser_x, laser_y, true_map, modify_map);
}
}
}
angle += separation;
}
}
