static void level_down() {
carmen_hmap_link_p link;
int i;
printf("\nlevel_down()\n");
link = get_closest_link(CARMEN_HMAP_LINK_ELEVATOR);
if (link == NULL || link->keys[0] == map_zone)
return;
for (i = 0; i < link->degree; i++)
if (link->keys[i] == map_zone)
break;
carmen_map_change_map_zone(hmap.zone_names[link->keys[i-1]]);
carmen_ipc_sleep(1.0);
set_robot_pose(transform_robot_pose(link->points[i], link->points[i-1]));
}
static void hmap_warp_check(carmen_point_t old_pose, carmen_point_t new_pose) {
int i;
double x1, x2, x3, x4, y1, y2, y3, y4;
static int warping = 0;
static char *warp_zone;
static carmen_point_t warp_pose;
static carmen_point_t warp_src;
static carmen_point_t warp_dst;
static carmen_point_t warp_post1;
static carmen_point_t warp_post2;
if (warping) {
if (line_side_test(warp_post1, warp_post2, warp_pose, new_pose) < 0) {
if (carmen_distance(&warp_pose, &new_pose) > 0.5) { //dbug: param?
carmen_map_change_map_zone(warp_zone);
carmen_ipc_sleep(1.0);
set_robot_pose(transform_robot_pose(warp_src, warp_dst));
warping = 0;
}
}
else
warping = 0;
}
else {
for (i = 0; i < hmap.num_links; i++) {
if (hmap.links[i].type == CARMEN_HMAP_LINK_DOOR) {
if (hmap.links[i].keys[0] == map_zone) {
if (segment_intersect(old_pose, new_pose, hmap.links[i].points[0], hmap.links[i].points[1])) {
warping = 1;
warp_zone = hmap.zone_names[hmap.links[i].keys[1]];
warp_pose = old_pose;
x1 = hmap.links[i].points[0].x;
x2 = hmap.links[i].points[1].x;
x3 = hmap.links[i].points[2].x;
x4 = hmap.links[i].points[3].x;
y1 = hmap.links[i].points[0].y;
y2 = hmap.links[i].points[1].y;
y3 = hmap.links[i].points[2].y;
y4 = hmap.links[i].points[3].y;
warp_src.x = (x1 + x2) / 2.0;
warp_src.y = (y1 + y2) / 2.0;
warp_src.theta = atan2(y2-y1, x2-x1);
warp_dst.x = (x3 + x4) / 2.0;
warp_dst.y = (y3 + y4) / 2.0;
warp_dst.theta = atan2(y4-y3, x4-x3);
warp_post1 = hmap.links[i].points[0];
warp_post2 = hmap.links[i].points[1];
}
}
else if (hmap.links[i].keys[1] == map_zone) {
if (segment_intersect(old_pose, new_pose, hmap.links[i].points[2], hmap.links[i].points[3])) {
warping = 1;
warp_zone = hmap.zone_names[hmap.links[i].keys[0]];
warp_pose = old_pose;
x1 = hmap.links[i].points[0].x;
x2 = hmap.links[i].points[1].x;
x3 = hmap.links[i].points[2].x;
x4 = hmap.links[i].points[3].x;
y1 = hmap.links[i].points[0].y;
y2 = hmap.links[i].points[1].y;
y3 = hmap.links[i].points[2].y;
y4 = hmap.links[i].points[3].y;
warp_dst.x = (x1 + x2) / 2.0;
warp_dst.y = (y1 + y2) / 2.0;
warp_dst.theta = atan2(y2-y1, x2-x1);
warp_src.x = (x3 + x4) / 2.0;
warp_src.y = (y3 + y4) / 2.0;
warp_src.theta = atan2(y4-y3, x4-x3);
warp_post1 = hmap.links[i].points[2];
warp_post2 = hmap.links[i].points[3];
}
}
}
}
}
}
double smp::smoothness_cost<typeparams,NUM_DIMENSIONS>
::evaluate_cost_trajectory (state_t *state_initial_in,
trajectory_t *trajectory_in,
state_t *state_final_in) {
double total_time = 0.0;
double initialCost_=0;
//! FOR POSITION CONTROLLER
int cnt;
double old_x,old_y,new_x,new_y,s,sw,vmax, old_theta, new_theta;
vmax=2;
old_x=0;
old_y=0;
new_x=0;
new_y=0;
cnt=0;
/// Initial cost introduced as discount factor to penilize multi steps trajectories
total_time+=initialCost_;
for (typename list<state_t*>::iterator iter=trajectory_in->list_states.begin();
iter!= trajectory_in->list_states.end();iter++){
state_t *state_curr=*iter;
if(cnt==0){
old_x=(*state_curr)[0];
old_y=(*state_curr)[1];
old_theta=(*state_curr)[2];
// Initial value
total_time+=0.1;
}
else{
new_x=(*state_curr)[0];
new_y=(*state_curr)[1];
new_theta=(*state_curr)[2];
/// to compute the social force, for each new state of the tree evaluate the social force as the robot pose was in that state
if(computeSocialforce){
set_robot_pose(new_x,new_y,new_theta);
s=0.5*sqrt((new_x-old_x)*(new_x-old_x)+(new_y-old_y)*(new_y-old_y))+0.25*fabs(1-cos(diff_angle_unwrap(new_theta,old_theta)))+0.25*get_social_force_cost(1);
}
else{
s=0.5*sqrt((new_x-old_x)*(new_x-old_x)+(new_y-old_y)*(new_y-old_y))+0.5*fabs(1-cos(diff_angle_unwrap(new_theta,old_theta)));
}
total_time+=s;
old_y=new_y;
old_x=new_x;
old_theta=new_theta;
}
cnt++;
}
return total_time;
}
