API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Oct 18th, 2018
125
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 11.77 KB | None | 0 0
raw download clone embed print report
  1. /*
  2. Implements a robot that coordinates with other using a
  3. circular region.
  4. Read Marcolino & Chaimowicz 2009 ICRA paper for more information
  5. */
  6.  
  7. #include "wiseRobot.h"
  8.  
  9. #include "../common/regionMethods.cpp"
  10. #include "../common/commonMethods.cpp"
  11.  
  12. //Function to send warnings between robots. In other words, send the control messages
  13. void WiseRobot::sendWarning()
  14. {
  15. double msg[SIZE_MSG];
  16.  
  17. if (m_state != I_DONT_CARE && pho() > congestionOkDist)
  18. {
  19. if (m_state == GOING)
  20. msg[MSG_POS_TYPE] = WATCH_OUT;
  21. else if (m_state == WAIT || m_state == WAIT_A_LOT)
  22. msg[MSG_POS_TYPE] = STOP;
  23.  
  24. msg[MSG_POS_WAYPOINT_X] = destineX;
  25. msg[MSG_POS_WAYPOINT_Y] = destineY;
  26. msg[MSG_POS_MY_X] = m_x;
  27. msg[MSG_POS_MY_Y] = m_y;
  28. msg[MSG_POS_MY_ID]= (double) m_id;
  29.  
  30. if ((msg[MSG_POS_TYPE] == WATCH_OUT && pho() < congestionDangerDist) || msg[MSG_POS_TYPE] == STOP) // I will send a WATCH_OUT only if I am near my waypoint
  31. {
  32. connection.sendMsg(msg,SIZE_MSG);
  33. #ifdef MESSAGESLOG
  34. for (int i = 0; i < SIZE_MSG; i++){
  35. messagesLog << msg[i] << " ";
  36. }
  37. messagesLog << endl;
  38. #endif
  39. }
  40. }
  41. }
  42.  
  43. //Constructor. pool is the message pool to send and receive msgs
  44. WiseRobot::WiseRobot(Pool_t *pool):connection(pool){}
  45.  
  46. //Initialize all robot data (pose, connection, velocity, etc.)
  47. void WiseRobot::init(int id, int numRobots, double prob, double region, string path)
  48. {
  49. std::ostringstream strs;
  50. strs << numRobots;
  51. std::string numRobotsStr = strs.str();
  52.  
  53. m_id = id;
  54. m_name = "robot" + intToStr(id);
  55.  
  56. Pose pose = pos->GetPose();
  57. m_x = pose.x;
  58. m_y = pose.y;
  59. m_th = pose.a;
  60.  
  61. numIterations = numIterationsReachGoal = 0;
  62.  
  63. connection.init_connection(path);
  64.  
  65. m_state = GOING;
  66. pos->SetColor(GOING_COLOR);
  67.  
  68. #ifdef GENERAL_LOG
  69. log.open(("logs/"+numRobotsStr+"/"+m_name).c_str());
  70. #endif
  71.  
  72. m_prob = prob;
  73.  
  74. init_position_data();
  75.  
  76. init_laser();
  77.  
  78. finished = false;
  79. #ifdef MESSAGESLOG
  80. messagesLog.open(("messages/"+m_name).c_str());
  81. #endif
  82.  
  83. messageCount = 0;
  84. inFrontMisses = 0;
  85. stalls = 0;
  86. alreadyStalled = false;
  87.  
  88. congestionDangerDist = region;
  89. }
  90.  
  91.  
  92. /*
  93. Alter the values of waitX and waitY so that
  94. the robot goes to a a point outside de danger region
  95. (simbolized by o in the method's name)
  96. */
  97. void WiseRobot::goToRegion_o(){
  98. const double alpha = (PI/180.0)*(90 - ALFA/2);
  99. const double gama = (PI/180.0)*(90 + ALFA/2);
  100. const double deltaX = m_x - destineX;
  101. const double deltaY = m_y - destineY;
  102. // deltaX = (deltaX == 0)?0.001:deltaX;
  103. double r = congestionOkDist + 0.01;
  104. // double A = (deltaY*deltaY)/(deltaX*deltaX) + 1;
  105. // double x = (m_x < destineX)?
  106. // destineX - r/sqrt(A) :
  107. // destineX + r/sqrt(A) ;
  108. // double y = (m_y < destineY)?
  109. // destineY - r*sqrt(1-1/A):
  110. // destineY + r*sqrt(1-1/A);
  111. double x = destineX + r * (deltaX/hypot(deltaX,deltaY));
  112. double y = destineY + r * (deltaY/hypot(deltaX,deltaY));
  113. //if (inRegion_X(x,y)){
  114. waitX = x;
  115. waitY = y;
  116. //}
  117. /*else{
  118. waitX = x;
  119. if ( ((y > destineY) && (x > destineX)) ||
  120. ((y <= destineY) && (x <= destineX)) )
  121. {
  122. waitY = destineY + tan(alpha)*(x-destineX);
  123. }
  124. else
  125. {
  126. waitY = destineY + tan(gama)*(x-destineX);
  127. }
  128. }*/
  129. }
  130.  
  131.  
  132. //Finish robot, freeing some variables and closing files
  133. void WiseRobot::finish()
  134. {
  135. //cout << "Destroyed " << m_name <<"!" << endl;
  136. #ifdef GENERAL_LOG
  137. log.close();
  138. #endif
  139. #ifdef MESSAGESLOG
  140. messagesLog.close();
  141. cout << m_name << ": message created!" << endl;
  142. #endif
  143. }
  144.  
  145. //Alter the values of fx and fy, adding repulsion force.
  146. void WiseRobot::obstaclesRepulsionForces(double &fx, double &fy)
  147. {
  148. double Kobs = (m_state != GOING_OUT && m_state != I_DONT_CARE && m_state != GOING)?Ke:Ki; // Weight of the obstacle repulsion forces
  149. double distance;
  150. double dx = 0;
  151. double dy = 0;
  152. bool found = false;
  153. double fx_ = 0 ,fy_ = 0;
  154. const std::vector<meters_t>& scan = laser->GetRanges();
  155. uint32_t sample_count = scan.size();
  156.  
  157. for(uint32_t i = 0; i < sample_count; i++)
  158. {
  159. distance = scan[i];
  160. double influence = INFLUENCE;
  161. if (distance <= influence)
  162. {
  163. dx = distance*cos(m_th + getBearing(i));
  164. dy = distance*sin(m_th + getBearing(i));
  165.  
  166. if (dx < limit && dx >= 0)
  167. dx = limit;
  168. if (dx > -limit && dx < 0)
  169. dx = -limit;
  170.  
  171. if (dy < limit && dy >= 0)
  172. dy = limit;
  173. if (dy > -limit && dy < 0)
  174. dy = -limit;
  175.  
  176. double _fx = -Kobs*(1.0/distance - 1.0/(influence*2))*(1.0/pow((double)distance,2))*(dx/distance);
  177. double _fy = -Kobs*(1.0/distance - 1.0/(influence*2))*(1.0/pow((double)distance,2))*(dy/distance);
  178.  
  179. fx += _fx;
  180. fy += _fy;
  181.  
  182. fx_ += _fx;
  183. fy_ += _fy;
  184.  
  185. found = true;
  186.  
  187. }
  188. }
  189. #ifdef DEBUG_FORCES
  190. fv.setRepulsiveForces(fx_,fy_);
  191. #endif
  192.  
  193. if (messageCount == 0)
  194. {
  195. if (found)
  196. {
  197. sendWarning();
  198. }
  199. }
  200.  
  201. messageCount = (messageCount + 1) % MSG_CYCLES;
  202. }
  203.  
  204. //Implements the main loop of robot.
  205. //Also contain robot controller and
  206. //probabilistic finite state machine codes
  207. void WiseRobot::walk(){
  208. double fx = 0;
  209. double fy = 0;
  210. double norm = 0;
  211. double r;
  212. double angTarget;
  213. double linAccel;
  214. double rotAccel;
  215.  
  216. numIterations++;
  217. //how many times robot stall?
  218. if (pos->Stalled()){
  219. if (!alreadyStalled){
  220. stalls++;
  221. alreadyStalled = true;
  222. }
  223. }
  224. else{
  225. alreadyStalled = false;
  226. }
  227. Pose pose = pos->GetPose();
  228. m_x = pose.x;
  229. m_y = pose.y;
  230. m_th = pose.a;
  231.  
  232. // Test if WAIT robot should change to I_DONT_CARE if pushed inside the Free region. It is NOT in the official results yet
  233. /*if (pho() < congestionOkDist)
  234. {
  235. m_state = I_DONT_CARE;
  236. pos->SetColor(I_DONT_CARE_COLOR);
  237. }*/
  238.  
  239. //if (pho() < congestionOkDist && (m_state == WAIT || m_state == WAIT_A_LOT))
  240. // goToRegion_o();
  241.  
  242. //if (pho() < waypointDist && (m_state == I_DONT_CARE || m_state == GOING))
  243. if (pho() < waypointDist)
  244. {
  245. finished = true;
  246. currentWaypoint = 1 + (rand() % NUMBER_OF_WAYPOINTS);
  247.  
  248. //if (m_state == I_DONT_CARE){
  249. m_state = GOING;
  250. //}
  251. pos->SetColor(GOING_OUT_COLOR);
  252.  
  253. numIterationsReachGoal = numIterations;
  254. numIterations = 0;
  255. }
  256.  
  257. if (finished && (distance(m_x,m_y,waypoints[0][0],waypoints[0][1]) >= DISTANT_RADIUS)){
  258. log << numIterations << endl;
  259. log.close();
  260. pos->SetColor(END_COLOR);
  261. finish();
  262. finished = false;
  263. connection.finish(m_id,numIterationsReachGoal,numIterations,stalls);
  264. }
  265.  
  266. #ifdef CHECK_DEAD_ROBOTS
  267. if (numIterations > DEAD_ITERATIONS)
  268. {
  269. connection.finish(m_id,numIterations,0,stalls);
  270. pos->SetColor(END_COLOR);
  271. finish();
  272. }
  273. #endif
  274.  
  275. destineX = waypoints[currentWaypoint][0];
  276. destineY = waypoints[currentWaypoint][1];
  277.  
  278. iteration = (iteration + 1) % PROB_CYCLES;
  279.  
  280. if (m_state == WAIT && iteration == 0)
  281. {
  282. r = ( (double)rand() / ((double)(RAND_MAX)+(double)(1)) );
  283. if (r < m_prob)
  284. if (m_state == WAIT){
  285. m_state = I_DONT_CARE;
  286. pos->SetColor(I_DONT_CARE_COLOR);
  287. }
  288. }
  289.  
  290. if (m_state == GOING || m_state == I_DONT_CARE)
  291. {
  292. fx = (destineX - m_x);
  293. fy = (destineY - m_y);
  294.  
  295. norm = sqrt(pow(fx,2) + pow(fy,2));
  296.  
  297. fx = Ka*fx/norm;
  298. fy = Ka*fy/norm;
  299. }
  300. else if (m_state == WAIT || m_state == WAIT_A_LOT )
  301. {
  302. fx = (waitX - m_x);
  303. fy = (waitY - m_y);
  304.  
  305. if (abs(fx) > 0.01 || abs(fy) > 0.01)
  306. {
  307. norm = sqrt(pow(fx,2) + pow(fy,2));
  308.  
  309. if (pho() < congestionOkDist)
  310. {
  311. fx = Ka*fx/norm;
  312. fy = Ka*fy/norm;
  313. }
  314. else
  315. {
  316. fx = 0.1*Ka*fx/norm;
  317. fy = 0.1*Ka*fy/norm;
  318. }
  319. }
  320. /*else{
  321. fx = fy = 0;
  322. }*/
  323. }
  324. #ifdef DEBUG_FORCES
  325. fv.setAttractiveForces(fx,fy);
  326. #endif
  327.  
  328. obstaclesRepulsionForces(fx, fy);
  329.  
  330. saturation(fx, fy, maxForce);
  331. #ifdef DEBUG_FORCES
  332. fv.setResultantForces(fx,fy);
  333. #endif
  334.  
  335. communicate();
  336.  
  337. if (fx == 0 && fy == 0) // Change to inequalities?
  338. angTarget = m_th;
  339. else
  340. angTarget = atan2(fy, fx);
  341.  
  342. linAccel = Kl*(fx*cos(m_th) + fy*sin(m_th));
  343. rotAccel = Kr*(angDiff(angTarget, m_th));
  344.  
  345. linAccel += -Kdp*linSpeed;
  346. rotAccel += -Kdp*rotSpeed;
  347.  
  348. linSpeed = linSpeed + linAccel*(TIME_STEP);
  349. rotSpeed = rotSpeed + rotAccel*(TIME_STEP);
  350.  
  351. this->pos->SetXSpeed(linSpeed);
  352. this->pos->SetTurnSpeed(rotSpeed);
  353.  
  354. #ifdef GENERAL_LOG
  355. //log << m_x << " " << m_y << " " << waitX << " " << waitY << " " << destineX << " " << destineY << " " << m_state << endl;
  356. #endif
  357. }
  358.  
  359.  
  360. //Receive messages from robots in the proximity and changes state depending on its type
  361. //and relative position
  362. void WiseRobot::communicate()
  363. {
  364. double ang;
  365. bool inFront;
  366.  
  367. inFront = false;
  368.  
  369. double msg[SIZE_MSG];
  370. //aqui eh que simula a distancia da comunicacao
  371. bool msgReceived = connection.receiveMsg(m_x,m_y, m_id, msg, SIZE_MSG);
  372.  
  373.  
  374. if (msgReceived && distWaypoint(msg) < sameWaypointOffset && pho() > congestionOkDist )
  375. {
  376. if (msg[MSG_POS_TYPE] == WATCH_OUT)
  377. {
  378. ang = (180/3.14)*calcAngTarget(m_x, m_y, msg[MSG_POS_MY_X], msg[MSG_POS_MY_Y]);
  379.  
  380. if (pho() < congestionDangerDist && m_state == GOING && !(ang < -5 && ang > -175))
  381. {
  382. m_state = WAIT;
  383. pos->SetColor(WAIT_COLOR);
  384.  
  385. waitX = m_x;
  386. waitY = m_y;
  387. }
  388. }
  389. else if (msg[MSG_POS_TYPE] == STOP)
  390. {
  391. ang = (180/3.14)*calcAngTarget(m_x, m_y, msg[MSG_POS_MY_X], msg[MSG_POS_MY_Y]);
  392.  
  393. if (ang > 45 && ang < 135)
  394. {
  395. if (m_state == GOING)
  396. {
  397. m_state = WAIT_A_LOT;
  398. pos->SetColor(WAIT_A_LOT_COLOR);
  399. waitX = m_x;
  400. waitY = m_y;
  401. }
  402. inFront = true;
  403. inFrontMisses = 0;
  404. }
  405. else if (pho() < congestionDangerDist && m_state == GOING && !(ang < -5 && ang > -175)) // **Conferir se é necessário
  406. {
  407. m_state = WAIT;
  408. pos->SetColor(WAIT_COLOR);
  409. waitX = m_x;
  410. waitY = m_y;
  411. }
  412. }
  413. }
  414.  
  415. /*if (m_state == WAIT && pho() > congestionDangerDist){
  416. m_state = WAIT_A_LOT;
  417. pos->SetColor(WAIT_A_LOT_COLOR);
  418. }*/
  419.  
  420. if (m_state == WAIT_A_LOT && !inFront){
  421. inFrontMisses++;
  422. }
  423.  
  424. if (inFrontMisses > MAX_MISSES && m_state == WAIT_A_LOT)
  425. {
  426. m_state = GOING;
  427. pos->SetColor(GOING_COLOR);
  428.  
  429. inFrontMisses = 0;
  430. }
  431.  
  432. // Missing in previous experiments... :(
  433. if (m_state == WAIT_A_LOT && pho() <= congestionDangerDist){
  434. m_state = WAIT;
  435. pos->SetColor(WAIT_COLOR);
  436. }
  437. }
  438.  
  439. Pool_t pool;
  440.  
  441. //Pointer to a new robot.
  442. //Every call of this library will create a new robot
  443. //using this pointer.
  444. WiseRobot* robot;
  445.  
  446. extern "C" int Init(Model *mod, CtrlArgs *args){
  447. //printf("Vou criar o robo!\n");
  448. robot = new WiseRobot(&pool);
  449. vector<string> tokens;
  450. Tokenize(args->worldfile, tokens);
  451.  
  452. robot->pos = (ModelPosition*)mod;
  453. robot->pos->AddCallback(Model::CB_UPDATE, (model_callback_t)PositionUpdate, robot );
  454. robot->laser = (ModelRanger*)mod->GetChild( "ranger:1" );
  455.  
  456. robot->laser->Subscribe(); // starts the laser updates
  457. robot->pos->Subscribe(); // starts the position updates
  458.  
  459. //printf("Vou criar o robo!\n");
  460.  
  461. robot->init(atoi(tokens[1].c_str()), atoi(tokens[2].c_str()), atof(tokens[3].c_str()), atof(tokens[4].c_str()), tokens[5]);
  462. #ifdef DEBUG_FORCES
  463. robot->pos->AddVisualizer( &robot->fv, true );
  464. #endif
  465. return 0;
  466. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!