Embedded Motion Control 2012 Group 1

Group Members

Name:	Student id:	Email:
Rein Appeldoorn	0657407	r.p.w.appeldoorn@student.tue.nl
Jeroen Graafmans	0657171	j.a.j.graafmans@student.tue.nl
Bart Peeters	0748906	b.p.j.j.peeters@student.tue.nl
Ton Peters	0662103	t.m.c.peters@student.tue.nl
Scott van Venrooij	0658912	s.j.p.v.venrooij@student.tue.nl

Tutor: Jos Elfring

Week 1

• Installed all required software on our computers :)
• Everybody gained some knowledge about Unix, ROS and C++.

Week 2

http://www.youtube.com/watch?v=Cmh8dMrmgN0

• Everybody gained more knowledge about Unix, ROS and C++.
• Looked at Gazebo, Rviz, Turtlebot_simulator, PR2_simulator, Jazz_robot_simulator
• Managed to create 2D map with Jazz Robot Simulator.
• Made presentation of Chapter 1: Introduction, Link to the presentation: http://cstwiki.wtb.tue.nl/images/Powerpoint_Chapter_1.pdf

Week 3

Optimized map

• Thought about an efficient algorithm to solve the 'maze'-problem. Depth-First-Search is proposed as a candidate algorithm.
• Started with arrow recognition using the OpenCV package.
• Started with map-processing to build up a tree of the maze which can be used by the decision algorithm.
• Optimized mapping algorithm and did some processing on the map. See Figure "Optimized map"
• tarted with navigation node for autonomous navigation of the jazz

Tasks for Week 4:

• Process map
• Add markers at crossings
• Define paths between these markers for navigation

Week 4

Corner detection

• Arrows can be detected by camera with use of the OpenCV package.
• Robot can detect its position in a corridor with use of its laser data and can drive autonomous.
• Corner detection; extracted out of the occupancy-grid. See Figure "Corner detection".

Week 5

• Map-processing is cancelled, since the used package (hector_mapping) creates incorrect maps.
• Now working on processing the laserdata to let the Jazz drive "autonomously"
• Writing an algorithm by using depth-first-search, this algorithm obtains the kind of crossing, the position of the Robot and the angle of the Robot and sends the new direction to the Robot

Week 6

http://youtu.be/_g9S24xXes4

http://www.youtube.com/watch?v=KSjsA6BR-Bs&feature=channel&list=UL

• Did tests with the real Jazz Robot; some issues were find concerning the update-rate; perhaps a new navigation strategy is needed.
• New navigation strategy implemented. Still a couple of bugs that have to be fixed; this is for next week.
• Created a visualization-class to visualize the node-structure for better perception of the algorithm.
• Finished DFS algorithm; algorithm works now if there are no loops.

Tasks for Week 7:

• Debug navigation node; fix issue with no 'corridor' between 'corners'.
• Debug algorithm; try to find some errors.
• Corridor competition
• Test on real robot

Week 7

Navigation node debugged, fixed issue with no corridor between corners. Original maze without loops solved: http://www.youtube.com/watch?v=eQblXU57aeU&feature=youtu.be. Figure "Original map solved" also shows the how the original given maze looks in Rviz when it is solved the robot. Big maze solving also possible: http://www.youtube.com/watch?v=HEO7ETBTaDs&feature=youtu.be. The big maze solved can also be seen in Figure "Big maze solved".

Original maze solved

Big maze solved

Week 8

• Some adaptions on code with new simulator; result: http://www.youtube.com/watch?v=tgHOjsxDUjk&feature=plcp

Navigation

The navigation node is used to steer the robot correctly in the maze. This navigating is done with use of the laser data. The laser data is first set into a certain variable. On the basis of this variable the robot can be navigated through the maze. The navigation is divided into 6 cases.

The first case is the case in which the robot is driving trough a corridor. In this case the robot is kept in the middle of the corridor by checking and adjusting its angle with respect to the walls of the corridors. While driving trough it is constantly checked whether a corner of any type or a dead end can be recognized. Whenever a corner is recognized, the navigation switches to case 2.

In case 2 the recognized corner or dead end is labeled as one of seven possible corner-types. This corner-type can be a corner left or right, one of three possible T-junctions, a cross-over or a dead end. These corner-types all get a number to identify them easily:
0. Dead end
1. Corner right
2. Corner left
3. T-junction middle (means the robot approaches for the middle)
4. T-junction left (means the robot approaches for the left)
5. T-junction right (means the robot approaches for the right)
6. Cross-over
This corner-type is now published together with the coordinates of the center of this corner-type. The coordinates of the center of the corner-type, also called the coordinates of the node, can be retrieved by processing the coordinates retrieved from the map topic, published by the hector_mapping node. Now the algorithm node processes this corner-type and at its turn publish the direction the robot should go. If the corner-type is 0 (dead end), the navigation node will still publish this type and the coordinates of the node, but then the navigation directly switches to case 6. Once the direction in which the robot should go, is received, the navigation switches to case 3.

Corner-type 1, 2, 3

Corner-type 4, 5, 6

In case 3 the robot drives to the middle of the corner-type. To get to the middle of a corner the robot uses different points with respect to which it gets to the middle. If the corner is of a type in which the robot would drive into a wall if it would not make the corner (type 1, 2 and 3) it uses the distance to the wall to know when to stop driving forward. If this distance is half the distance of a corridor, the robot has reached the middle. If the corner is of type 1 (corner right), the robot uses its laser data on the left, to keep the right distance from the walls it is driving parallel to, while driving to the middle of the corner. With a corner of type 2 (corner left) it does the same, only with the laser data on the right. A type 3 corner (T-junction middle) is the only corner-type in which the robot does not have any points with respect to which it can stay in the middle, while driving forward. In Figure “Corner-type 1, 2, 3” the corner-types 1, 2, 3 can be seen, with in green the laser data or points with respect to which the robot drives to the middle of the corner.

If the corner is of type 4, 5 or 6 the robot uses the distance to the 90 degrees corners to know when to stop driving forward. When the y-distance (distance in the way the robot is driving) is half the distance of a corridor, it has reached the middle. With corners of type 4 and 5 the robot uses its right or left laser data the same way as it uses it when driving to the middle of a corner of type 1 and 2, to stay in the middle while driving forward. With a corner of type 6 the robot uses the x-distance to the 90 degrees corners to stay in the middle while driving forward. When the robot has reached the middle, the navigation switches to case 4. In Figure “Corner-type 4, 5, 6” the corner-types 4, 5, 6 can be seen, with in green the laser data or points with respect to which the robot drives to the middle of the corner.

In case 4 the robot makes a turn or does nothing. This is dependant of the direction in which the robot has to go. If the algorithm node publishes as direction to go straight ahead, the robot will not do anything in case 4 and will immediately switch to case 5. On the other hand, if the algorithm publishes to go to the left or right, the robot will make a turn of +(1/2)π radials or -(1/2)π radials respectively. After the turn is completed, the navigation switches to case 5.

In case 5 the robot starts driving forward again until it reaches the corridor. When the corridor is reached, the navigation switches back to case 1.

Case 6 is the case in which the navigation goes, when the robot is driving towards a dead end. In this case only direction is possible and that is backwards. So after driving to center of the dead end, which is at half the distance of a corridor of the dead end, the robot will make a turn of π radials. After the turn is completed, the navigation switches back to case 1.

Algorithm

The algorithm node is to choose where the robot has to go, when it is arriving at a node. The algorithm gets the corner-type and the coordinates of the node over a topic published by the navigation node. When the robot arrives at a node, which is new and where the robot has not been jet, it processes this node. This processing means a number-id is given to the node and the coordinates, previous and free directions of this node are saved. When the robot is arriving at a node, which has as corner-type the T-junction type where the robot arrives in the middle and can go to the right or the left, the algorithm will first check with the camera whether an arrow can be found on the wall or not. If an arrow is visible, the algorithm returns that the robot has to go in the direction of the arrow. If no arrow is found the algorithm as visible in the flowchart in Figure "Flowchart algorithm" is used to choose in which direction the robot has to go.

Flowchart algorithm

First the algorithm checks whether the boolean ‘loop_flag’ is true or not. This boolean indicates whether the robot is following a path on a loop inside the maze, or not. This path leads the robot from the node on which it was discovered that the robot was in a loop to the closest node on which a direction is not discovered jet. So if this variable is true (flowchart goes to the purple box 9) the algorithm returns that the robot has to keep following the path that has been set. How this works exactly is visualized in Figure "Driving along a set path".

Driving along a set path

If the boolean ‘loop_flag’ is not true, which means the robot is not in a loop, the algorithm checks whether the boolean ‘explorer’ is true or not. This boolean indicates whether the robot is exploring a part of the maze which is not discovered jet or it is driving back in a corridor it already discovered. If this boolean is false the algorithm checks whether the node at which it is arriving is part of a loop, indicated by the boolean ‘node_loop_flag’. If this is the case (flowchart goes to light-blue box 6) the robot is back at a loop it already discovered, so the boolean ‘loop_flag’ is set to true, the closest node with a free direction is found and a path towards this node is set. Now the algorithm returns that the robot has to start driving along this set path. Figure "Driving backwards, arriving at node loop" visualizes how this works exactly.

Driving back, arriving at node

If the boolean ‘node_loop_flag’ is not true, it means the robot is arriving at a node which is already discovered but is not a part of loop. This node can still have a direction free, the robot can discover, indicated by the boolean ‘direction_free’. If this is the case ( flowchart goes to the blue box 7), the ‘explorer’ is set to true and the algorithm outputs that the robot has to go into the free direction. This is visualized in Figure "Driving back, arriving at node with free direction". If the node at which the robot is arriving does not have a free direction left, so ‘direction_free’ is false, the algorithm returns that the robot has to go to the previous node of the node it is arriving at (flowchart goes to the dark blue box 8). This is visualized in Figure "Driving back, arriving at node with no free direction".

Driving back, arriving at node with free direction

Driving back, arriving at node without free direction

If ‘explorer’ is true, the next thing the algorithm checks is whether the node, the robot is arriving at, is a new node or not, indicated by boolean ‘new_node’. If the node is a new node, first the node is processed. After this, it is checked whether the node has as corner-type a dead end or not. If it is a dead end (flowchart goes to the orange box 2), so boolean ‘dead_end’ is true, ‘explorer’ is set to false and the algorithm returns that the robot has to go to the previous node of this node. This visualized in Figure "Arriving at new node, which is a dead end". If the node the robot is arriving at is not a dead end (flowchart goes to the red box 1), at least one direction is free at the node. If more directions are free always the most right option is chosen. The algorithm now returns that the robot has go to into the chosen, or in some cases only, free direction. This visualized in Figure "Arriving at new node, with free direction".

Arriving at new node, which is a dead end

Arriving at new node, with free direction

Now going back to the boolean ‘new_node’ in the flowchart. If the node the robot is arriving at is not a new node, the algorithm will check whether this node is part of an already discovered and “flagged” loop or not, by checking the boolean ‘node_loop_flag’. If this boolean is true, it means the robot is arriving at a node which is shared by two adjacent loops. On the other hand, if the boolean is false, the robot is arriving at a node which it already has discovered, but is not already part of a loop, so this is a single loop. To clarify this, first have a look at the case that there is only one loop, so ‘node_loop_flag’ is false. Now the algorithm will first check whether the robot has made this loop in a clockwise or counterclockwise direction. This is important, because, since at all the times the most right option is chosen at a node, any free direction of a node in a counterclockwise loop would lead to the inside of the loop . Beacuse the exit of the maze is always at the outside, these directions can be ignored. So the algorithm checks the direction of the loop, indicated by the boolean ‘loop_clockwise’. If this boolean is true, the direction of the loop is clockwise. In this case (flowchart goes to the dark green box 5), ‘loop_flag’ is set to true, ‘explorer’ is set to false and the loop is initialized. Initializing the loop means that at each node, which is part of the loop, the boolean ‘node_loop_flag’ is set to true, indicating it is part of the loop. This way a node which is part of the loop can be recognized as being part of a loop, whenever the robot gets back to this node. After this the closest node with a free direction is searched for and a path towards this node is set. Now the algorithm returns that the robot has to start driving along this set path. How this works exactly is visualized in Figure "Arriving at known node, clockwise loop".

Arriving at known node, clockwise loop

If ‘loop_clockwise’ is false, the direction of the loop is counterclockwise (flowchart goes to the light green box 4) and as told in this case no node part of this loop now has a value in finding the exit of the maze except for the node the robot is arriving at. This node can still have one free direction, which leads outside the loop. So now it is first checked whether the node, the robot is arriving at, still has a free direction. If this is the case the algorithm returns that the robot has to go into this free direction. This is visualized in Figure "Arriving at known node, counterclockwise loop, direction free". If this is not the case, the only way to go for the robot is to the previous node of the node it is arriving at, leaving the counterclockwise behind. So the algorithm sets ‘explorer’ to false and returns that the robot has to go the previous node of the node it is arriving at. This is visualized in Figure "Arriving at known node, counterclockwise loop, no direction free".

Arriving at known node, counterclockwise loop, direction free

Arriving at known node, counterclockwise loop, no direction free

Now there is still the case left, in which the robot is arriving at a node it already discovered, but which is already part of another loop. So it is arriving at a node which is shared by two adjacent loops. In this case (flowchart goes to the yellow box 3) it is first checked whether the new found loop has a clockwise or a counterclockwise direction. If the new loop has a clockwise direction ‘loop_flag’ is set to true, ‘explorer’ is set to false and the loop is initialized. Initializing means in this case that both the new loop and already discovered loop are being merged into one larger loop and at all nodes, which are part of this merged loop, the boolean ‘node_loop_flag’ is set to true. After this the closest node with a free direction is searched for in merged loop and a path towards this node is set. Now the algorithm returns that the robot has to start driving along this set path. How this works exactly is visualized in Figure "Arriving at known node of clockwise loop". If the new loop has a counterclockwise direction, first the new loop has to be initialized clockwise. This means that the nodes in the new loop are set in a clockwise order. So the node at which the robot is just arriving now becomes the previous node of the node from which the robot just came. After this the same procedure can be followed as when the new loop is clockwise initially, because now the new counterclockwise loop is made a new clockwise loop. The working of this is visualized in Figure "Arriving at known node of counterclockwise loop".

Arriving at known node of clockwise loop

Arriving at known node of counterclockwise loop