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