Embedded Motion Control 2013 Group 11

Group Members

Name:	Student id:	Email:
Justin Wildschut	0617495	j.a.wildschut@student.tue.nl
Jeroen Zegers	0638172	j.c.zegers@student.tue.nl
Alex Andriën	0652965	a.r.p.andrien@student.tue.nl
Talha Ali Arslan	0867550	talhaaliarslan@gmail.com
Leroy Hazeleger	0651762	l.hazeleger@student.tue.nl

Maze challenge evaluation

The program used to successfully find the exit of a maze was insufficient. The robot entered the first crossing where it encountered a small pocket left and right of the path. Since the pockets left and right where very small, it should go straight since the program neglects pockets which are not useful to explore. Thus the robot continued and wanted to exit the first crossing but it stopped moving when it wanted to exit the crossing. Normally this should not happen. For driving out of the crossing there are three different states where it can be in. Somehow, at first it entered the first state where it follows the right minimum to position itself into the path, after that it changed state (which should not be possible) and this resulted in the fact that it lost its current state and the robot just stopped. Of course in the second trial the same problem should occur, and it did. The day before the maze challenge we added another if() statement which caused this problem and we knew it before the second trial, but we thought that we could not change the code between the two trials.

When the challenge was finished, we did a third trial, of the records, were we erased that extra if() statement. After that, the robot was able to continue, detected both the arrows but at the end it did not go to the exit unfortunately....

Discussion and Possible Solutions

* http://www.youtube.com/watch?v=MNNZ6KVImGg

Program Structure

We designed a program structure to divide the tasks, make the program more transparant and easier to debug. The program is divided in the following nodes, of which the workings are explained below:

The nodes and topics are described in the following document: File:EMC Main.pdf

Smoothing Node

Here, the raw laser data is smoothened using a moving average. This is done by averaging each value from the laserscanner with its neighbours, which leads to data with less noise in it, which is sent to the Averaging node.

The smoothing is done by applying the moving average method in a variable window size. In this method, moving average at every index is calculated by taking the sum of 'window size' number of elements to the left and right (combined) of that point and dividing it by the window size (i.e. if the window size is 21, 10 points to the back, 10 points to the front and the current point is taken into account). We first started with a constant window size, but there were problems. Because the resolution of laser readings is high over small portions at close distances and low over large portions at large distances, we had to adjust the moving average window size accordingly. In these gifs on the upper left and center, you will see what happens if we use a small and large window size at close distances (Look at with small window size how the minima index changes rapidly and how smoothened the data is, and at large window size, look at how the data is smoothened and there not that much rapid change in minima location as with smaller window size, but there is still movement and this will be overcome in the next node 'Averaging Node').

One of the first assumptions was that the laser data below a magnitude of 0.15 m is due to the body of the robot. This is only correct for large distances. In the simulation this holds everywhere but in the real world, we discovered that at close distances first indices of the laser read distances 'between' the actual distance and 0; they look like an interpolation between 0 and those distances and we do not know why this happens; we guess that it may be due to the white shiny surface of the robot case that beams can reflect and be effective at close distances or it may just be a bug of the laser. So; because we depend our smoothing of laser data and determination of local minimum and maximum distances based on these start and finish indices, we should no longer use a dynamic start index and based on real world testing, we should constantly avoid first and last (~approximately) 20 indices which are around 18 in the simulation. We can increase these values based on readings (if a reading below 0.15m still comes after 20th index, we may shift starting index to that value but it should never be below 20). This gif file on the upper right shows the phenomena; where at first the distance to the wall is close, and when the distance increases the problematic readings goes below the line of 0.15 m (black) which was our previous assumption.

The gif pictures below show the whole data and the effect of window size on smoothing. Basically it is seen that with small window size, the smoothened data resembles the original more (not oversmoothened) but at close distances location changes too rapidly and even multiple maximas are found close to each other in some instances. Using a larger window size gives advantage at close distances and flat surfaces, but you can see in the middle gif that data is over smoothened and some corners to the sides are dislocated too much. Below right show a dynamical window result, at distances above a certain distance constand window size is used but between a preferred distance region, window size is proportionally changed between 21 and 101 in this case. Using a very large window size is also not preferable because it increases computation time. Because we use dynamical window, we cannot use a moving array which would solve that problem actually.

How the local minimum values are found is explained in the 'DeterminePosition Node' part.

Averaging Node

In contrary to the Smoothing node, which averages over all the laser data from a single time instance, this node averages the already smoothened data over several time steps. This is necessary because if there is a disturbance in a single timestep (a large dust particle in front of the laser for instance), we don't want the program to detect things that aren't there. The Averaging reduces the effect of such disturbances. The smoothened, averaged data is then sent to the Determine Position node.

In the visuals above, one can see that averaging helps reduce the oscillation of the minimas.

Determine Position Node

Here, the smoothened and averaged data is analysed. The minima and maxima of the laser data are determined, as well as their corresponding indices. This data is sent to the Detect Crossing node.

In this node, every data point from the Averaging node is checked according to its distance and index, relative to the other points within a constant window, where the current point is in the center. Also local maximas or minimas next to each other who have same distances are avoided and only one is taken into account, because from the smoothened and averaged data, there can be points next to or close to each other with same distance value.

Result of this node can be seen in the gif pictures in 'Smoothing Node' and 'Averaging Node' where local minimas are plotted with a circle and a vertical line.

Detect Crossing Node (under construction!!)

As is presented in the program structure above there is a node which detects the crossings in the maze. This node detects at which moment Pico is at the middle of a cross-section and if there is a side-path on its left and/or right and/or a path in front. These different type of cross-sections are detected using the positions of the minima in the laser-data. The positions of these minima are found in the 'Determine Position' node and send via a topic to this 'Detect Crossing' node. The principle of this function is illustrated using the pictures below.

Suppose the robot approaches a T-crossing like in the picture below and that we want the robot to go left.

(Text will be changed)

The left picture above shows the moment at which Pico arrives at the middle of an intersection. If two minima are within the red area's we know that the robot is approximately in the middle of the T-crossing (assuming that the width of the corridor and the side-path are approximately equal). The 'Detect Crossing' node will give a message to the 'Decision Making' node that it is in the center of an intersection and which options there are to take (left, straight or back). If the 'Decision Making' node decides to make a turn left, the robot will turn left untill the minima are in the regions as is depicted in the right picture above, The detect crossing sends a message to the 'Decision Making' node when the minima are in these regions and thus the turn in made.

For other types of intersections, the node will search for minima in different combinations. Each type of intersection has its own configuration of minima.

(Text will be changed)

text.

Decision Making Node

The structure of this node can best be explained using a program flow diagram as depicted below.

Mapper

It is easy to remember which turns robot has made over time and to manipulate its choices by always making a specific turn or balancing them. However the task of remembering where it has come from and what choices remain is rather complex. For this we tried to simplify the problem as much as possible. At every new node, a term is added to the map which is kept as an [some constant][node count] size array. When the robot crosses a node, map is updated on that node, so that when the robot comes back, it knows which direction it has come from, where it can go and cannot go. When a dead end is reached, it reflects back on previous nodes too and as robot makes its way back to the last node where there is an option to go to, nodes are erased from the array. So, in the end, if the robot makes it out of the maze, we have an array with smallest amounts of nodes in it, and with 'dead end' information in them.

Every node is defined as 4 term vector:

[current , right of the current , ahead of the current , left of the current]

It is simply a record of ways in counterclockwise directions. The term current 'Current' is where the robot is and it can either be 3 or 4 (4 means that there is a dead end behind the robot and it is returning). Other terms can be 0,1,2,4. 0 means there is no opening (wall). 1 means it is an option that is not discovered yet. 2 means robot has come from there before. 4 means there is an opening but it has dead end(s).

As an example 'T crossing' is defined as [3101]. Current direction is 3, the one to the right is 1 (there is an opening), ahead is 0 (wall), left is 1 (there is an opening too). As another example, a right corner is defined as [3100] or a left corner [3001] or when the robot drives between walls [3010] (but this is not added to the map other than the start). If there is a 4-way crossing it is [3111].

If the robot crossed a new right corner, it first detects it as [3100] and when it makes the turn and passes it, map updates that term to be [3002]; because now it has become a left turn. Same with if the robot makes a left turn at a 4-way crossing [3111] after the node is passed, that node will be updated as (it turned left so now it is as if it has come from the right), [3211]. Take for instance the node type of [3110] where the robot can continue straight or right and there is no opening on its left. If it makes the right turn, (if it comes back to that node) the wall will be at its front, the option that it did not go before will be on its right and its previous choice will be on its left; [3102]. When the robot gets to dead ends, it updates current 3 as 4 and this effects previous nodes too. As an example at an 4 way crossing in the maze at the very start, we assume for the maze competition that on of them can be the correct one. Initially it will be [3111] and if we are not so lucky, we are going to eliminate all the dead ends and the node will become for example [4241]. 2 is where the start of the maze is (on the right) and this means that if we chose the option straight ahead at first it could have saved us time.

So discovering the maze requires a strategy of choosing between the undiscovered options, but returning from dead ends requires only remembering the nodes and making our way back to a desired node correctly.

Drive Safe Node

The Drive Safe node makes sure that PICO drives safely throughout the whole maze. This means that PICO drives safely between walls so that it does not hit a wall and drives optimally through the pathway, it detects when it is entering a crossing and drives safely and straight to the middle of the crossing, and after PICO made the correct turn it exits the crossing safely and straight. After it exits the crossing, it continues to drive safely between walls.

The Drive Safe node is divided into four states: Normal Driving, Enter Crossing, Exit Crossing and Safety Driving. The Decision Making node determines if the Drive Safe node should be in the Normal Driving state,in the Exit Crossing state or PICO should do nothing. It publishes a Drive message of 1 if PICO should drive safely between walls (Normal Driving state), publishes a Drive message of 2 if PICO exits a crossing (Exit Crossing state) or publishes a Drive message of 0 if PICO should stop moving so that a turn can be made. Within the Drive Safe node, it is determined if PICO is actually between walls (Normal Driving state, Drive message 1), enters a crossing (Enter Crossing state, Drive message 1) or perform a ` special' maneuver to prevent collision (Safety Driving, Drive message 1). To determine the state in which PICO is, the message published by the Determine Position node is used which tells us where the minimum distances to the wall are located (index between 0 - 1081) and provides the distance to that minimum. Since we are interested only in the local minima, i.e. minima below a certain threshold, only the minimum distances below the threshold are considered in the Drive Safe node. Below, the different parts are explained in more detail.

Note that all thresholds are tuned during the real-time experiments

Normal Driving

PICO is in the `Normal Driving' mode if PICO is located between walls. In order for PICO to take the turns correctly, PICO should remain approximately in the middle of the path with its forward position parallel to the walls before it will be in the `Enter Crossing' mode. To determine when PICO is located between walls, we know that there are at least two minima which are approximately 180 degrees apart (approximately since the noise disturbs the Determine Position node). If those two minima are indeed approximately 180 degrees apart PICO is between walls, and if this is not the case (the angle between the two minima differs to much with a certain threshold) PICO enters a crossing. Also if one of the minima becomes to small, PICO goes into the Safety Driving node.

The two minima (index and distance, indicated by the red lines) are used to determine the width of the path (orange line), the position from the middle of the path (difference between the green and blue line, left from the blue line is negative) and the angle between the forward position and the right wall and the left wall (purple lines). Those are depicted in the figure below.

The width of the path is determined by summation of the distances of the two minima.

[math]\displaystyle{ \frac{}{}widthPath = distance_{left} + distance_{right} }[/math]

The position from the middle of the path is determined by dividing the width of the path by 2 and subtracting the distance to the right minimum.

[math]\displaystyle{ posFromMiddle = \frac{widthPath}{2} - distance_{right} }[/math]

Using the minimum angle and the angle increment, the angles of the two minima [math]\displaystyle{ \theta_{left} }[/math] and [math]\displaystyle{ \theta_{right} }[/math] are determined.

What we want is that PICO drives in the middle of the path and if not, it drives optimally to the middle of the path. Therefore we would like PICO to drive to the middle of the path by following a exponential decay towards to middle of the path. The exponent decay is shown by the red line in the figure above.

[math]\displaystyle{ \frac{}{}posFromMiddle(t) = posFromMiddle_{int} * e^{\beta t} }[/math]

Since we can only control the linear and angular velocity, the slope of the exponent in each point is determined

[math]\displaystyle{ \frac{d}{dt}posFromMiddle(t) = \beta * posFromMiddle_{int} * e^{\beta t} }[/math]

and from this exponent the desired netto angle of PICO is determined.

[math]\displaystyle{ \theta_{desired} = sin(\frac{\beta*posFromMiddle}{\sqrt{1+\beta^2*posFromMiddle^2}}) }[/math]

The actual netto angle is determined using the following equation

[math]\displaystyle{ nettoAngle = \frac{\theta_{left} + \theta_{right}}{2} }[/math]

The angular velocity is then calculated is the actual netto angle minus this desired netto angle and multiplied with a constant, resulting in an exponential decay towards the middle of the path.

[math]\displaystyle{ \frac{d}{dt}\theta_{z} = constant*(nettoAngle - nettoAngleDesired) }[/math]

Of course [math]\displaystyle{ \beta }[/math] can be chosen very high so that PICO returns quickly to the middle of the path, however from real-time experiments was observed that PICO has an overshoot due to a certain delay. Therefore [math]\displaystyle{ \beta }[/math] and the constant are tuned so that PICO moves to the middle of the path without oscillating around the middle.

Entering Crossing

When the two minima do not make an angle of approximately 180 degrees and PICO was in the Normal Driving state before, PICO enters a crossing. From the location of the different minima that are observed can be determined what type of crossing it enters.

T-crossing, left turn or right turn

If there is one minimum detected approximately at index 540 (within the threshold of course), we know that there is a wall in front of PICO. The only thing we want is that PICO drives towards this minimum so that drives straight into the crossing and can detect a crossing accurately using the DetectCrossing node. So the angular velocity is adapted so that the minimum will be detected around index 540 the whole time. The figures below show the different crossing when it should go to the minimum at index 540. So we correct the angle of PICO, for instance if the minima is at index 580, we know that PICO is oriented to the left, and we give it an angular velocity to the right.

Left turn or right turn with a straight path

If there is no minimum approximately at index 540, and if there is one minimum detected approximately at index 180 (right), or one minimum detected approximately at index 900 (left) and not both at the same time of course otherwise we are between walls, then we know that PICO enters a crossing where you can turn right or straight and respectively left or straight. Since the width of the path can not be measured in the crossing, the old width of the path is remembered and the same strategy is used as in the Normal Driving state to drive to the middle of the path and to drive straight with respect to the wall (right or left).

Three-way crossing

If the T-crossing, left turn, right turn, left turn or right turn with a straight path do not occur, then PICO enters a three-way crossing. Now we need to use the two minima which are in front of PICO but the number of minima can vary between 2, 3 or 4 minima in the threshold. Also the index of two minima in front of PICO will change when PICO enters the crossing. Therefore, depending on the number of minima in the threshold, the right minima are selected and the angular velocity is changed such that the distances to these minima become equal. So if the left minima of the two is further away than the right minima, then PICO should be oriented to the left and should drive forward.

Exiting Crossing

When PICO is entering a crossing, a Drive message of 1 is published by the Decision Making node. At a certain point, the Detect Crossing node detects a crossing and the Decision Making node publishes a Drive message of 0 and PICO is will make a turn accordingly. During the Drive message of 0, PICO does not drive, and only turns. When the turn is finished, a new Drive message of 2 is published by the Decision Node and PICO is in the Exit Crossing state.

In the Exit Crossing state, the same method as in the Enter Crossing state could have been applied. However the problem with that is that when PICO turns, the width of the new path it will enter can be different from the path it came from and is unknown. Also the position from the middle is unknown and therefore the algorithm as used previously cannot be used. It can only be used if it is guaranteed that the width of the new path is equal to the old width of the path.

When PICO made the turn, three different situations can occur: a left + straight crossing can be seen, a right + straight crossing can be seen, or a three-way crossing can be seen. These are given in the figures below. The Detect Crossing node made sure that PICO stopped in approximately the middle of the intersection. Therefore what we like to have is that PICO moves straight into the new path, but due to the turn it made, the back wheels are orientated perpendicular to the new driving direction. Therefore we need a new control strategy which corrects PICO if it does not move straight into the new path. Depending on the minima it detects, the different situations occur and have different strategies. If a left + straight crossing can be seen when PICO made a left turn, PICO should follow the right wall since at index 180 a minimum will be seen. By keeping the index of this minimum at index 180 PICO can move straight into the new path. If the minimum is detected at for instance index 200, PICO is oriented in the direction of the right wall and the angular velocity should be positive. When there are two minima within the theshold which are 180 degrees across each other, then PICO is in the path ,thus PICO is between walls and a Drive message of 1 is published by the Decision Making node. PICO can continue in the Normal Driving state.

The same strategy is used for the right + straight crossing, which can seen when PICO made a right turn before. A minimum at approximately index 900 can be seen and should stay at index 900.

For the three-way crossing the same strategy as in the Enter Crossing state is used: A minimum at approximately index 360 and a minimum at approximately index 720 can be seen, and those distances are kept equal by adjusting the angular velocity. Those minima will change index, but rather than looking for the minima at those indexes, it is checked what the indexes are of those two minima.

Safety Driving

When PICO is too close to a wall, PICO should stop driving forward to prevent collision. Thus if the minimum distance is closer then 30 cm, then PICO stops driving forward and backs up till the minimum distance is 45 cm. After that, PICO can continue to drive safe; in drive safe the orientation of PICO will be corrected and therefore PICO will not come close to a wall.

Camera Node

In order to reduce the time required to finish the maze, we use the arrows that are placed around the maze to guide us towards the exit. To recognize the arrows we use the camera mounted on Pico, but the raw footage (displayed below) needs to be processed in order for Pico to detect arrows.

Initial Cropping

We start of by cropping the initial RGB image, because we do not need to detect an arrow in the whole image, only in the center. This reduces the calculation time as well as the disturbances from the environment. The cropped image is showed below.

Template Matching

Then we use Template Matching to give a good first estimate of where the arrow is. We use both a right and left arrow template to run over the image, where the following formula returns a value for the similarity between the template and the actual image at every pixel:

[math]\displaystyle{ R(x,y) = \sum_{x',y'} (T(x',y')-I(x+x',y+y'))^2 }[/math]

The pixel with the lowest value gives us the best estimate of where the arrow is. We then crop the image even further, it is now the size of the template. We also already have an idea if the arrow is towards the right or left, since the template matching function of the correct arrow will have returned the lowest value. The cropped image is displayed below.

Thresholding

Next, the small image (which still has RGB values), is converted to the HSV color space. We then perform thresholding for the following values (HSV(165-179,240-255,100-175) and HSV(0-20,240-255,100-175)), in order to seperate the red in the (RGB) image from everything else. The resulting binary image is white where the original RGB image was red and black for any other color. Note: we use two seperate thresholds to create two binary image, which we subsequently combine, because red is exactly at the end of the spectrum in the HSV values in OpenCV. This means that red can have the value of H = 0, as well as H = 179, which can be seen in the picture to the right of the HSV Cylinder. The seperate binary images as well as the combined one are displayed below (we use a right arrow because this one demonstrates the use of two thresholds the best).

HSV Cylinder

/* Template:Pp-template */ /* this is a block */ .thumbinner { display: flex; flex-direction: column; }

.trow { display: flex; flex-direction: row; clear: left; flex-wrap: wrap; width: 100%; box-sizing: border-box; }

/* this is a cell */ .tsingle { margin: 1px; float: left; }

.theader { clear: both; font-weight: bold; text-align: center; align-self: center; background-color: transparent; width: 100%; }

.thumbcaption { text-align: left; background-color: transparent; }

.thumbcaption-center { text-align: center; background-color: transparent; }

.text-align-left { text-align: left; }

.text-align-right { text-align: right; }

.text-align-center { text-align: center; }

@media all and ( max-width: 720px ) { .thumbinner { /* not !important usage can be removed when these are no longer inline styles inside Template:Multiple image and media query using min-width is introduced */ width: 100% !important; box-sizing: border-box; max-width: none !important; align-items: center; } .trow { justify-content: center; } .tsingle { /* not !important usage can be removed when these are no longer inline styles inside Template:Multiple image and media query using min-width is introduced */ float: none !important; max-width: 100% !important; box-sizing: border-box; text-align: center; } .thumbcaption { text-align: center; } }

Dilating and Eroding (Closing)

Because this binary image is still very noisy and the arrow is not completely filled, we use two iterations of dilating followed by two iterations of eroding the image. Dilation consists of convoluting an image with a certain kernel of any shape or size. The kernel has an anchor point, which usually is in its center. As the kernel is run over the image, the maximum pixel value overlapped by the kernel is determined, and the pixel of the image in the anchor position of the kernel gets that maximum value. This makes the bright areas in the image become larger, which in our case is the arrow. Erosion is very similar, only that instead of the maximum pixel value, we take the minimal pixal value above. This makes the dark areas bigger, thus reducing the size of the arrow. We use a rectangular kernel of size 4 for the dilation as well as the eroding. The process of dilation followed by eroding is also called closing an image. The result is an arrow with its gaps filled and the rough edges smoothened out, as displayed below.

Edge Detection

Now that we have a clear binary image of the arrow we are driving towards, we first calculate its center by checking the pixel that is on the far left and the pixel that is on the far right which are both still white. We will use this later on.

Summary Arrow Detection

EPIC MOVIE HERE

Possible Improvements

- Use smoothing on original image to reduce noise. (If your goals is to have a binarized image with smooth edges then, if you have the original, it is better to use something like a gaussian blur with cvSmooth() on that before performing the binarization.) (http://stackoverflow.com/questions/5154282/dilate-erode-modify-kernel-option)

- blur before edge detection

- different dilating/eroding, maybe circle for dilating and rectangle for eroding?

- different kernel size for closing, blurring, edge detection and everything.

Sources

- http://en.wikipedia.org/wiki/HSL_and_HSV - http://docs.opencv.org/doc/tutorials/imgproc/erosion_dilatation/erosion_dilatation.html - http://docs.opencv.org/doc/tutorials/imgproc/opening_closing_hats/opening_closing_hats.html - http://stackoverflow.com/questions/5154282/dilate-erode-modify-kernel-option

Filtering and Processing Laser Data

20 Sept 2013

By applying a set of filters, laser data is processed to give information which are easier to interpret for path planning. By detecting certain features of the environment, we now deal with smaller amount of data which are transferred by custom messages that can be modified.

At first, the filter process was created as a node and it got more complex as new stuff were added. So; for ease of programming,reading and debugging the code, we decided to split up some parts. Then it became much simpler, more fixes were done because it was easier to notice them now and the overall functionality of the processes increased. It was discussed that one down-side of creating more and simpler nodes is the possible lag between the nodes. So, we decided we are going to check and measure nodes' communication, how well they are synced and we are also not going to over-simplify and create too many nodes. But we are positive that separating the needed tasks may reduce the reaction time if the nodes are synced well; by using functions and options like ros::spin(),ros::spinOnce(),ros::Duration().sleep()(to save CPU) and message queues for publishing and subscribing.

After visualizing the environment as we wanted to; we now focus more on tasks of the robot as some of the tasks have to be carried out at all times (e.g. obstacle avoidance) and others have to be completed upon requests (e.g. change in trajectory due to decision making).

Laser data visualized

18 Sept 2013

The data from the laser can be interpreted as points given in a polar coordinate system, these points can be transformed to a cartesian coordinate system. This visualisation helps to create a strategy for detecting walls and corners.

ROS Nodes and Topics

17 Sept 2013

With ROS, one can use nodes and topics to create simpler and easy-to-handle structures. Usage of nodes is very useful when multiple tasks have to be done simultaneously. For this project, a 'Master' node, a 'Movement' node and a 'Vision' node can be created to perform given tasks as solving a maze and recognizing signs, which have to be done simultaneously and which are at times independent of each other. For the corridor competition, the task is simple; so the use of multiple nodes and especially the 'Vision' node will not be necessary. However, for the second competition goal of which will be solving the maze, these nodes can be implemented to perform recognition of signs and to solve the maze. Here is one of the first drafts of the nodal structure of the program:

Useful Things

Hardware info

Technical info about the laserscanner can be found in this document. Mostly to satisfy your curiosity, not specifically needed for programming. Hokuyo Laser Scanner

Creating a different map

Using GIMP Image Editor

1. Download GIMP Image Editor from the Software Center
2. Open the existing map ("maze.pgm") in '/home/s080482/ros/emc/general/gazebo_map_spawner/maps' with GIMP
3. Edit the map how you want it to be. Scroll all the way down in the 'Brushes' box in the bottom right of GIMP. Then there is a square on the second last row on the far right, called ' square (5x5)(5 x 5). (make sure you have this one and not one with 'blur'. Also select the 'Pencil Tool Hard Edge Painting Using a Brush' in the top left box.
4. Now just delete blocks and at add them were you want. Zooming in helps. You can switch color on the top left as well.
5. Also if you look closely, you can see a grey dot in the bottom center. I think this to mark the point where the robot spawns. I suggest we leave it where it is and edit the maze around it.
6. Save the map in the same folder but with a different name.
7. Go to the folder '/home/s080482/ros/emc/general/gazebo_map_spawner/scripts' and open spawn_maze in a text editor.
8. Change the name of the map in needs to load, i.e. change the OWN_MAP in the text below to your own map name.
rosrun gazebo_map_spawner map_spawner `rospack find gazebo_map_spawner`/maps/OWN_MAP.pgm
9. Now simply spawn the maze in exactly the same way as you did before, so with exactly the same commands!

Using Gazebo Model Builder

1. Open Gazebo
2. Go to 'Edit', 'Model Builder'.
3. You will see a 2-D image and 3-D world under it. Choose 'Add Wall' and start drawing walls on the 2-D image.
4. Check the locations and distances from the 3-D world.
5. Hit 'Done', save it to a file 'example.sdf'. Close 'Model Builder'. The model should spawn in the world.
6. Go to 'File', 'Save As'; again save as 'example.sdf'.

7. To load, go to the save folder and hit command:

gazebo example.sdf

Teleop Controller for Pico

To be able to move the robot around manually and easily is very useful when testing simple things in the algorithm. Here you can find the teleop controller for PR2:

• http://wiki.ros.org/pr2_simulator/Tutorials/Teleop%20PR2%20arm%20in%20simulation

And here is the revised version of the teleop controller for our robot pico:

 
/*
This code lets you move the jazz robot using your keyboard. 
This is obtained from a sample code for pr2 from this link:
http://wiki.ros.org/pr2_simulator/Tutorials/Teleop%20PR2%20arm%20in%20simulation
It basically goes into a loop, where the terminal is set for user-input kind of operation
and everytime a button is pressed, velocities are published as the cmd_vel topic for pico.

--emc11

*/


#include <termios.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

#define KEYCODE_A 0x61
#define KEYCODE_D 0x64
#define KEYCODE_S 0x73
#define KEYCODE_W 0x77
#define KEYCODE_Q 0x71
#define KEYCODE_E 0x65


class teleop_pico_keyboard
{
	private:
	geometry_msgs::Twist cmd_msg;
	ros::NodeHandle n;
  	ros::Publisher cmd_pub;
  	

	public:

	void init(){
 		cmd_pub = n.advertise<geometry_msgs::Twist>("/pico/cmd_vel", 1000);
		cmd_msg.linear.x=0.0;
    		cmd_msg.linear.y=0.0;
    		cmd_msg.linear.z=0.0;
   		cmd_msg.angular.x=0.0;
   		cmd_msg.angular.y=0.0;
   		cmd_msg.angular.z=0.0;
		cmd_pub.publish(cmd_msg);
	};

	~teleop_pico_keyboard()   { };
	
	void kloop();
};

int kfd = 0;
struct termios cooked, raw;

void quit(int sig)
{
  tcsetattr(kfd, TCSANOW, &cooked);
  exit(0);
}



int main(int argc, char **argv){

	ros::init(argc, argv, "jazz_teleop_t");

	teleop_pico_keyboard tpk;

	tpk.init();	

        signal(SIGINT,quit);

	tpk.kloop();
	
	return 0;
}

void teleop_pico_keyboard::kloop()
{
  char c;
  bool dirty=false;
  ros::Rate loop_rate(5);
  // get the console in raw mode
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);

  puts("Reading from keyboard");
  puts("---------------------------");
  puts("Use 'WS' to forward/back");
  puts("Use 'AD' to rotate left/right");
  //puts("Use 'QE' to ...");

  while(ros::ok())
  { 
    
    // get the next event from the keyboard
    if(read(kfd, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }
   
    switch(c)
    {
    
    case KEYCODE_W:
      cmd_msg.linear.x = 1.0;
      cmd_msg.angular.z = 0.0;
      dirty = true;
      break;
    case KEYCODE_S:
      cmd_msg.linear.x = -0.5;
      cmd_msg.angular.z = 0.0;
      dirty = true;
      break;
    case KEYCODE_A:
      cmd_msg.angular.z = 0.5;
      cmd_msg.linear.x = 0.0;
      dirty = true;
      break;
    case KEYCODE_D:
      cmd_msg.angular.z = -0.5;
      cmd_msg.linear.x = 0.0;
      dirty = true;
      break;
// You can use these for different tasks
//    case KEYCODE_Q:
//      dirty = true;
//      break;
//    case KEYCODE_E:
//      dirty = true;
//      break;

    }


    if (dirty == true)
    {
      cmd_pub.publish(cmd_msg);
    }

  }
  
}
                                              

Just create a ros package dependent on roscpp and geometry_msgs, compile and run it in a seperate terminal. As an example; by pressing 'W' in the terminal, you will move the robot forward in the simulator.