int main()
{
// push waypoints to a routes' map
waypoints.push_back(waypoint(-4.0, -3.0)); // 0
waypoints.push_back(waypoint(-3.0, -1.0)); // 1
waypoints.push_back(waypoint(1.0, -4.0)); // 2
waypoints.push_back(waypoint(3.0, -2.0)); // 3
waypoints.push_back(waypoint(-1.0, 1.0)); // 4
waypoints.push_back(waypoint(4.0, -3.0)); // 5
waypoints.push_back(waypoint(2.0, 1.0)); // 6
waypoints.push_back(waypoint(-2.0, 2.0)); // 7
waypoints.push_back(waypoint(5.0, -1.0)); // 8
waypoints.push_back(waypoint(6.0, 2.0)); // 9
waypoints.push_back(waypoint(-1.0, 4.0)); // 10
waypoints.push_back(waypoint(4.0, 4.0)); // 11
waypoints.push_back(waypoint(8.0, 3.0)); // 12
waypoints.push_back(waypoint(1.0, 6.0)); // 13
waypoints.push_back(waypoint(6.0, 5.0)); // 14
waypoints.push_back(waypoint(2.0, 7.0)); // 15
waypoints.push_back(waypoint(7.0, 7.0)); // 16
waypoints.push_back(waypoint(4.0, 7.0)); // 17
waypoints.push_back(waypoint(6.0, 9.0)); // 18
waypoints.push_back(waypoint(8.0, 9.0)); // 19
obstacles.push_back(obstacle(0.0,5.0,1.0)); // 0
obstacles.push_back(obstacle(3.0,3.0,2.0)); // 1
obstacles.push_back(obstacle(3.5,0.0,1.0)); // 2
obstacles.push_back(obstacle(8.0,7.0,2.5)); // 3
// define exponential traverser type and create an exemplar
typedef ExpTr<node_value,cost_value,expand,less<cost_value> > ExpTr_t;
ExpTr_t j;
cout << "in exp traverser forward search ...\n";
j = ExpTr_t(0);
// set a maximal lag length
j.getexpfunc().setMaxLagLen(5.0); // or 6.0
// add two defined handlers to enable calculation of an expansions number
j.set_handler_on_expand_root(OnExpandRoot);
j.set_handler_on_select_cursor(OnSelectCursor);
// launch A* algorithm (combination of a predefined heuristics with a best - first
// search that is provided by exponential traverser is A*!)
ForwardSearch(j, goal);
ShowPath(j);
// show founded path
cout << "number of expansions = " << num_expansions << '\n';
cout << "...out \n\n";
cout << "Search is completed.\n";
return 0;
}
void sonarCallback(const sensor_msgs::PointCloud::ConstPtr& msg)
{
double pf_Start = ros::Time::now().toSec();
std::cout << "I was called : (ms)" << (pf_Start - lastTimeCalled)*1000 << std::endl ;
lastTimeCalled = pf_Start;
obstacles_positions_current.clear();
for(int i=0; i< msg->points.size(); ++i)
{
Eigen::Vector3d obstacle(msg->points[i].x,msg->points[i].y,0.0);
if(obstacle.norm()<laser_max_distance-0.01)
//if((obstacle.norm()>robot_radius)&&(obstacle.norm()<laser_max_distance-0.01)) // check if measurement is between the laser range and the robot
{
//ROS_INFO_STREAM("INSIDE THE LIMITS:"<<obstacle.norm());
obstacles_positions_current.push_back(obstacle);
}
}
if(!init_flag)
{
init_flag=true;
obstacles_positions_previous=obstacles_positions_current;
return;
}
//ROS_INFO_STREAM("obstacles:" << obstacles_positions.size());
//ROS_INFO("I heard sensor data : [%f, %f , %f]", msg->points[0].x , msg->points[0].y , msg->points[0].z );
computeForceField();
// feedbackMaster();
//feedbackSlave();
obstacles_positions_previous=obstacles_positions_current;
double pf_End = ros::Time::now().toSec();
//std::cout << "Call back took : (ms)" << (pf_End - pf_Start)*1000 << std::endl ;
}
void addObstaclesAndPropositions(oc::PropositionalTriangularDecomposition* decomp)
{
Polygon obstacle(4);
obstacle.pts[0] = Vertex(0.,.9);
obstacle.pts[1] = Vertex(1.1,.9);
obstacle.pts[2] = Vertex(1.1,1.1);
obstacle.pts[3] = Vertex(0.,1.1);
decomp->addHole(obstacle);
Polygon p0(4);
p0.pts[0] = Vertex(.9,.3);
p0.pts[1] = Vertex(1.1,.3);
p0.pts[2] = Vertex(1.1,.5);
p0.pts[3] = Vertex(.9,.5);
decomp->addProposition(p0);
Polygon p1(4);
p1.pts[0] = Vertex(1.5,1.6);
p1.pts[1] = Vertex(1.6,1.6);
p1.pts[2] = Vertex(1.6,1.7);
p1.pts[3] = Vertex(1.5,1.7);
decomp->addProposition(p1);
Polygon p2(4);
p2.pts[0] = Vertex(.2,1.7);
p2.pts[1] = Vertex(.3,1.7);
p2.pts[2] = Vertex(.3,1.8);
p2.pts[3] = Vertex(.2,1.8);
decomp->addProposition(p2);
}
int main(){
int i;
pt debut = 0;
pt fin = _largeur/2 +1;
double* mesh = init_tab_mesh();
obstacle(mesh);
//double bottom_left[2] = { 10, 10 };
//double top_right[2] = { 15, 15 };
//generate_rectangle(bottom_left, top_right, mesh);
double* dist = tab_distance(mesh, debut, fin);
printf("\n");
for(i=0; i<_largeur*_largeur;i++){
if(i%_largeur == 0) printf("\n");
printf("%d\t", (int)dist[i]);
}
printf("\n");
save_dist(dist, "distance.vtk");
pt* chemin = court_chemin_bis(dist, fin);
save_path(dist, fin, chemin, "path.vtk");
printf("\n");
for(i=0; i< dist[fin]; i++)
printf("%d \t %d \n", chemin[i]/_largeur, chemin[i]%_largeur);
free(mesh);
free(dist);
return 0;
}
void step(double& t){
int nx, ny, nz, alarm;
double *A,
*Bx, *By, *Bz,
dt,
*parx, *pary, *parz;
nx = n[0]; ny = n[1]; nz = n[2];
parx = new double[(nz)*(ny)*5];
pary = new double[(nz)*5];
parz = new double[5];
A = new double[5];
Bx = new double[5];
By = new double[5];
Bz = new double[5];
init_aux(parx, pary, parz);
dt = 42;
for(int ix = 2; ix < nx+2; ix++){
for(int iy = 2; iy < ny+2; iy++){
for(int iz = 2; iz < nz+2;iz++){
if(obstacle(ix,iy,iz,0) > 0){
flux(&parx[(iy-2)*nz + (iz-2)], ix, iy, iz, Bx, 'X', dt);
flux(&pary[iz-2], ix, iy, iz, By, 'Y', dt);
flux(parz, ix, iy, iz, Bz, 'Z', dt);
f(ix, iy, iz, A);
for(int i = 0; i < 5; i++){ A[i] = A[i] + dt*(Bx[i]/dif[0][ix-2] + By[i]/dif[1][iy-2] + Bz[i]/dif[2][iz-2]); }
f_inv(ix, iy, iz, A);
}
}
}
}
recalc_dif_p();
t = t + dt;
}
std::vector<Obstacle> XMLExtractor::extractObstacles(std::string gameName, unsigned int level, Hole& hole, Point& ballPosition, float& radius)
{
std::vector<Obstacle> obstacleArray;
pugi::xml_document doc;
std::string fileName = "..\\DataManager\\Courses\\" + gameName + ".xml";
if (doc.load_file(fileName.c_str()))
{
std::string s = "Course" + std::to_string(level);
pugi::xml_node obstacles = doc.child(s.c_str());
for (pugi::xml_node obstacleNode = obstacles.first_child(); obstacleNode; obstacleNode = obstacleNode.next_sibling())
{
std::string obstaclestr = std::string("Obstacle").c_str();
std::string holestr = std::string("Hole").c_str();
std::string ballstr = std::string("Ball").c_str();
if (obstacleNode.name() == obstaclestr)
{
double x = atof(obstacleNode.child_value("X"));
double y = atof(obstacleNode.child_value("Y"));
double width = atof(obstacleNode.child_value("Width"));
double height = atof(obstacleNode.child_value("Height"));
double resistance = atof(obstacleNode.child_value("Resistance"));
double rotation = atof(obstacleNode.child_value("Rotation"));
Obstacle obstacle(x, y, width, height);
obstacle.setAngle(rotation);
obstacle.setSurfaceResistance(resistance);
//Ajouter le nouveau point au vecteur
obstacleArray.emplace_back(obstacle);
}
else if (obstacleNode.name() == holestr)
{
hole.setPosition(atof(obstacleNode.child_value("X")), atof(obstacleNode.child_value("Y")));
hole.setWidth(atof(obstacleNode.child_value("Width")));
hole.setHeight(atof(obstacleNode.child_value("Height")));
}
else if (obstacleNode.name() == ballstr)
{
ballPosition = Point(atof(obstacleNode.child_value("X")), atof(obstacleNode.child_value("Y")));
radius = atof(obstacleNode.child_value("Radius"));
}
}
}
else
{
std::cout << "ERREUR: Le fichier '" + fileName + "' est introuvable. \n";
}
return obstacleArray;
}
void flux(double* par_ax, int ix, int iy, int iz, double* B, int ax, double& ddeltat){
int alarm;
double *par, rho1, u1, p1, ltan1, ltan2, rho2, u2, p2, rtan1, rtan2;
par = new double[12];
//forward
riemparam2(par, ix, iy, iz, ax); riemi(par, &alarm);
if(alarm){
std::cerr<<"function 'riemi' sends an alarm at ("<<ix<<" "<<iy<<" "<<iz<<'\n';}
tanvel(par[10], rtan1, rtan2, ax, ix, iy, iz);
updatetimestep(ax, ix, iy, iz, par[9], par[11], ddeltat);
rho2 = par[6]; u2 = par[7]; p2 = par[8];
//backward
if(obstacle(ix, iy, iz, -ax) == 1){
//tangential velosity components conserve near an obstacle
riemparam2(par, ix, iy, iz, -ax); riemi(par, &alarm);
if(alarm){
std::cerr<<"function 'riemi' sends an alarm at ("<<ix<<" "<<iy<<" "<<iz<<'\n';}
updatetimestep(-ax, ix, iy, iz, par[9], par[11], ddeltat);
par_ax[0] = par[6]; par_ax[1] = par[7]; par_ax[2] = par[8]; par_ax[3] = p[tan1(ax)][ix][iy][iz]; par_ax[4] = p[tan2(ax)][ix][iy][iz];
}
rho1 = par_ax[0]; u1 = par_ax[1]; p1 = par_ax[2]; ltan1 = par_ax[3]; ltan2 = par_ax[4];
//fluxes
B[0] = u2*rho2 - rho1*u1;
switch(ax){
case 'X':
B[1] = rho2*u2*u2 + p2 - (rho1*u1*u1 + p1);
B[2] = rho2*u2*rtan1 - (rho1*u1*ltan1);
B[3] = rho2*u2*rtan2 - (rho1*u1*ltan2);
break;
case 'Y':
B[1] = rho2*u2*rtan1 - (rho1*u1*ltan1);
B[2] = rho2*u2*u2 + p2 - (rho1*u1*u1 + p1);
B[3] = rho2*u2*rtan2 - (rho1*u1*ltan2);
break;
case 'Z':
B[1] = rho2*u2*rtan1 - rho1*u1*ltan1;
B[2] = rho2*u2*rtan2 - rho1*u1*ltan2;
B[3] = rho2*u2*u2 + p2 - (rho1*u1*u1 + p1);
break;
}
B[4] = u2*(rho2*(inenergy(rho2,p2) + 0.5*(u2*u2+rtan1*rtan1+rtan2*rtan2)) + p2) - u1*(rho1*(inenergy(rho1,p1) + 0.5*(u1*u1+ltan1*ltan1+ltan2*ltan2)) + p1);
delete[] par;
//parameters' update
par_ax[0] = rho2; par_ax[1] = u2; par_ax[2] = p2; par_ax[3] = rtan1; par_ax[4] = rtan2;
return;
}
task main()
{
ClearMessage();
float widthOfRoom=perimeterX(); //calculate the width of the room
float distanceY=perimeterY(); //calculate the length of the room
int repitition=reps(widthOfRoom); //calculate how many repititions of the cleaning cycle it needs to do
float distanceX= distX(widthOfRoom); //calculate the distance per strafe on the repititions
bool hit= false;
int counter=0;
while((counter<repitition-3)&&hit!=1)// run while it hasn't hit the bottom, or has not completed all of its repititions
{
hit=drive(1,distanceY-40); //drive forward -20cm for clearance from wall
if(hit) //if it hit something go through obstacle functions
{
float lengthOfObstacle=obstacle(1); //drive around the obstacle
drive(1, distanceY-lengthOfObstacle +10); //drive forward the remainder of the distance
hit=false; //reset hit
}
drive(3, distanceX); //drive right
hit=drive(2, (distanceY+20) ); //drive backwards -20cm for clearance from wall
if(hit) //if it hits something go through obstacle functions
{
wait1Msec(2000); //wait for sensor to rotate
float lengthOfObstacle=obstacle(2); //drive around the obstacle
drive(2, distanceY+lengthOfObstacle); //drive the remainder of the distance
hit=false; //reset hit
}
hit=drive(3, distanceX);//drive right
counter++;
}
motor[motorA]=0;
motor[motorB]=0;
motor[motorC]=0;
}
Map::Map (Game* game)
{
const auto nodes = 20;
_ground.reserve(nodes);
for (auto i = 0; i < nodes; i++)
_ground.push_back(vec2f(
math::random(width()),
math::random(height())));
_obstacles.push_back(
obstacle({
{ 30, 30 },
{ 320, 35 },
{ 310, 300 },
{ 30, 310 } }));
for (auto& b : _obstacles)
b.createBody(game);
}
int main(int argc, char * argv[])
{
std::cout<<"Common LAAS Raster library"<<std::endl;
if (argc < 3) {
std::cerr<<"usage: "<<argv[0]<<" flat.tiff obstacle.tiff region_out.tif"<<std::endl;
return EXIT_FAILURE;
}
gladys::gdal flat(argv[1]);
gladys::gdal obstacle(argv[2]);
gladys::gdal region;
region.copy_meta(flat, clara::region::N_RASTER);
region.bands_name = {"NO_3D_CLASS", "FLAT", "OBSTACLE", "ROUGH", "SLOPE"};
region.bands[clara::region::FLAT] = flat.bands[0];
region.bands[clara::region::OBSTACLE] = obstacle.bands[0];
region.save(argv[3]);
return EXIT_SUCCESS;
}
//return REACHED if position reached, OBSTACLE if stopped because of obstacle A CUSTOM STOP MESSAGE MUST BE SENT, CUSTOM_STOP_MESSAGE if stopped because a custom message was sent
//robot position is updated
//position is reached running for distance in steps <=200 (first the dist%200 than n steps dist/200)
//it sleeps 1s after each step
int go_to_position(pos_t target_pos, int margin, int speed){
int32_t tacho_dist, start_tacho, end_tacho;
int angle, dist;
int i;
if(robot_rank == 0) while(wait);
compute_polar_coord(target_pos, &dist, &angle,margin);
target_pos.x = robot.x + dist*sin(angle*M_PI/180);
target_pos.y = robot.y + dist*cos(angle*M_PI/180);
//start movement, in two steps if bigger than 200
int div = dist/200;
int mod = dist%200;
int step;
for(i = 0; i < div+1 ; i++){
if(i == div) step = mod;
else step = 200;
if(robot_rank == 0 && send_action_flag) send_action(next, angle, step, speed/2);
//vado alla posizione
set_motors_speed(SLOW_SPEED);
turn_absolute_fb(angle, get_cal_location(robot.x,robot.y));
//aggiorno posizione final;
robot.t = angle;//get_gyro_value();
printf("robot.t = %d\n",robot.t);
//save initial tacho counts
start_tacho=get_motorR_position();
set_motors_speed(speed);
run_relative((int)(step*360.0/18));
while(!command_finish()){
if(cancel){
//cancel = 0;
run_relative(0);
end_tacho = get_motorR_position();
tacho_dist = end_tacho-start_tacho;
dist = tacho_dist/20.0;
robot.x = robot.x + dist*sin(robot.t*M_PI/180);
robot.y = robot.y + dist*cos(robot.t*M_PI/180);
printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t);
if(send_action_flag) send_cancel(next, dist);
// Send to ourself an action in order came closer
act_val.dist = cancel - dist;
act_val.angle = robot.t;
act++;
return CUSTOM_STOP_MESSAGE;
}
if(obstacle()){
run_relative(0);
end_tacho = get_motorR_position();
tacho_dist = end_tacho-start_tacho;
dist = tacho_dist/20.0;
printf("step %d dist %d\n",step,dist);
if(step-dist < STOP_DIST*18/360.0){
set_motors_speed(speed);
run_relative((int)((step-dist)*360.0/18));
return REACHED;
}
printf("tacho dist %ld dist %ld\n",tacho_dist,dist);
robot.x = robot.x + dist*sin(robot.t*M_PI/180);
robot.y = robot.y + dist*cos(robot.t*M_PI/180);
printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t);
if(send_action_flag) send_cancel(next,dist);
// HERE WE MUST SEND A CUSTOM MESSAGE STOP TO FOLLOWING ROBOT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// HERE WE MUST SEND A CUSTOM MESSAGE STOP TO FOLLOWING ROBOT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
return OBSTACLE;
}
}
sleep(1);
}
//update pos
robot.x = target_pos.x;
robot.y = target_pos.y;
printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t);
return REACHED;
}
void sched (Info * info) {
//mylogfd (SCHFD, "%d\n", info->state);
// if (!info->video_info->is_valid) return;
// mylogfd(SCHFD, "%d %d\n", IsEdgeValid(info), getEdgeX(info));
// mylogfd(SCHFD, "IsDoorSeen %d IsDoorAllSeen %d IsDoorLeft %d IsDoorRight %d IsDoorMiddle %d DoorMidX %d DoorMidY %d\n",
// IsDoorSeen(info), IsDoorAllSeen(info), IsDoorLeft(info), IsDoorRight(info), IsDoorMiddle(info), getDoorMidx(info), getDoorMidy(info));
/*static int wait_edge = -1;
if (wait_edge >= 0) {
wait_edge ++;
if (wait_edge >= 5) {
mylogfd(SCHFD, "Touch edges->Stop\n");
do_stop(info);
wait_edge = -1;
}
return;
}*/
// mylogfd(SCHFD, "IsDoorSeen:%d IsEdgeValid:%d IsOutSide:%d edgeX %d\n", IsDoorAllSeen(info), IsEdgeValid(info), IsOutSide(info), getEdgeX(info));
estimate_seen_state(info);
// mylogfd(SCHFD, "IsRobotSeen %d RobotX %d RobotY %d\n", IsRobotSeen(info), getRobotX(info), getRobotY(info));
if (info->last_cmd == CMD_FORWARD && info->state != SHOOTING && IsOutSide(info)) {
mylogfd(SCHFD, "Outside\n");
info->last_cmd = CMD_STOP;
do_stop(info);
info->state = OUTSIDE;
return;
}
/* if (info->last_cmd == CMD_FORWARD && IsOutSide(info) && IsEdgeValid(info)) {
if (getEdgeX(info) < 50 && getEdgeX(info) >= 0) {
mylogfd(SCHFD, "Touch edges %d\n", getEdgeX(info));
do_stop(info);
info->state = DONE;
//wait_edge = 0;
return;
}
}*/
/* if (info->last_cmd == CMD_FORWARD && IsRobotSeen(info)) {
if (IsRobotAcc(info)) {
if (getRobotY(info) < 30) {
do_stop(info);
info->state = SEARCH_BALL;
} else if (getRobotY(info) < 60) {
info->destP.x = 0;
info->destP.y = getRobotY(info) + 50;
info->obstacle[0].x = getRobotY(info);
info->obstacle[0].y = - getRobotX(info);
do_obstacle(info);
info->state = OBSTACLE;
}
}
}*/
//mylogfd(-1, "s");
switch (info->state) {
case START: //chang in state machine -- redwalker
start(info);
case FIND_PLACE:
// find_place(info);
break;
case SEARCH_BALL:
search_ball(info);
break;
case SEARCHING_BALL:
searching_ball(info);
break;
case SEARCHING_ADJUST:
searching_adjust(info);
break;
case FORWARDING_BALL:
forwarding_ball(info);
break;
case CATCHING_BALL:
catching_ball(info);
break;
case BALL_CATCHED:
ball_catched(info);
break;
case SHOOT:
shoot(info);
break;
case SHOOTING:
shooting(info);
break;
case MOVING:
moving(info);
break;
case CHANGE_PLACE:
change_place(info);
break;
case FINDING_DIR:
finding_dir(info);
break;
case CHANGING_PLACE:
changing_place(info);
break;
case OBSTACLE:
obstacle(info);
break;
case PROTECT_DOOR:
protect_door(info);
break;
case OUTSIDE:
if (!IsOutSide(info)) {
mylogfd(SCHFD, "Inside restart to search_ball\n");
info->state = SEARCH_BALL;
}
break;
case FACE_DOOR:
face_door(info);
break;
case DONE:
break;
case GAME:
//game(info);
break;
case SHOOTED:
shooted(info);
default :
break;
}
}
void CGameObject::on_matrix_change (const Fmatrix &previous)
{
obstacle().on_move ();
}
