Embedded Motion Control 2014 Group 7

Group members

Name	Email	ID number
Maurice van de Ven	m dot l dot j dot v dot d dot ven @ student dot tue dot nl	0742664
Pim Duijsens	p dot j dot h dot duijsens @ student dot tue dot nl	0718473
Glenn Roumen	g dot h dot j dot roumen @ student dot tue dot nl	0753028
Wesley Peijnenburg	w dot peijnenburg @ student dot tue dot nl	0790842
Roel Offermans	r dot r dot offermans @ student dot tue dot nl	0656441

Planning

Week	Deadline	General plan	Groupmember	Specific task
4	16-05-2014	Complete Corridor Challenge
- - - -	- - - - -	- - - - -	- - - - -	- - - - -
5	19-05-2014	Research Maze Strategies and define modules
	19-05-2014 before 10:45 h.		Roel	Research possibilities for wall and corner detection with OpenCV
	19-05-2014 before 10:45 h.		Glenn	Research possibilities for wall and corner detection with OpenCV
	19-05-2014 before 10:45 h.		Wesley	Research possibilities for arrow detection with OpenCV
	19-05-2014 before 10:45 h.		Maurice	Research possibilities for path planning and path tracker/driver
	19-05-2014 before 10:45 h.		Pim	Research possibilities for path planning and path tracker/driver
- - - -	- - - - -	- - - - -	- - - - -	- - - - -
6	26-05-2014	Research functionality of ROS Packages: Gmapping and move_base
	28-05-2014	Decide: use of packages or write complete new code
	30-05-2014	Conceptual version of all modules are finished
- - - -	- - - - -	- - - - -	- - - - -	- - - - -
7	09-06-2014	Development of modules:	Cutting corners	Glenn
	t.b.d.		Publishing and listening to arrow	Wesley/Maurice
	t.b.d.		Exploration	(Pim/Glenn)
	09-06-2014		OpenCV - local rectangles solver	Wesley/Maurice
	09-06-2014		(OpenCV -) floodfill solver	Roel
- - - -	- - - - -	- - - - -	- - - - -	- - - - -
8	13-06-2014	Integration of all nodes
- - - -	- - - - -	- - - - -	- - - - -	- - - - -
9	20-06-2014	Final presentation of the Maze Strategy and some last improvements
- - - -	- - - - -	- - - - -	- - - - -	- - - - -
10	27-06-2014	Maze Competition

Corridor strategy

The provided safe_drive file is extended to a file which solves the corridor challenge for a left and right corridor. The strategy, stated below, is implemented and tested. From these tests, it can be concluded that the strategy works properly. Everything is designed and implemented in one single node. The disadvantages of this approach are that it is hard to maintain, hard to extend and teamwork becomes hard. Therefore, it is desired to use different nodes for the maze challenge.

Maze strategy

A choice is made to analyze two different approaches to solve the maze challenge. The maze can be solved by using different nodes and design each node from scratch, or by using existing ros packages. If the first method will be chosen, the different nodes and flow chart can be used as stated below.

The choice is made to start with using existing ros packages. This is because it is known that ros packages work relatively well for different applications and we thought some packages would be very useful for our case. First, different ros packages are analyzed. The packages GMapping in combination with Move_base are studied and implemented. Since, GMapping and Move_base only covers the mapping, localization and navigation of the pico, a goal planner has to be implemented. The node for the goal planner, the node for the arrow recognition and the integration of both nodes are created from scratch. An alternative for the goal planner is the package Exploration. A more detailed description of the packages and approaches are stated in the next paragraph. The strategy is presented in the diagram below.

GMapping & Move_base

GMapping link

GMapping is being investigated as SLAM algorithm. The pico can, with this simultaneous localization and mapping (SLAM), make a map of an unknown environment and localize itself inside this map. The strategy is to combine the map and the position of the pico with a navigation stack and a goal planner. GMapping subscribes on /pico/laser and /tf, and publishes on /tf and /map.

In order to run GMapping correctly, a launch file gmap.launch is setup which initializes the correct settings. In this launch file the parameter base_frame is set as /pico/base_link, map_frame as /map and odom_frame as /pico/odom. Also, scan is remapped to /pico/laser. GMapping uses a transform from /map to /pico/odom and vice versa and from /pico/odom to /Pico/base_link and vice versa. To see how GMapping works, a GIF is presented below. The black pixels represent an obstacle, the light grey the known area and the dark grey the unknown area.

Move_base link

As navigation stack, Move_base is used. Move_base provides an action that will attempt to reach a given goal with a mobile base. It makes use of a local and global planner in order to solve a global navigation task. Move_base also maintains a costmap for the global planner and a costmap for the local planner. These costmaps are used to accomplish navigation tasks.

A choice is made to use the base_local_planner inside the move_base stack. From incoming sensor data, angular and linear velocities of the pico are chosen properly by the local planner to traverse the segment of the global path. Trajectory Rollout is used as the local planner. For the global planner, navfn is chosen. This makes use of Dijkstra's algorithm which is explained in the Navigation lecture by J.J.M. Lunenburg.

To launch the move_base stack, again a launch file is created which will initialize the correct settings. The move_base.launch file consist of the files listed below. It also performs a remap from /cmd_vel to /pico/cmd_vel.

costmap_common_params.yaml
global_costmap_params.yaml
local_costmap_params.yaml

base_local_planner_params.yaml

The contents of these files are:

costmap_common_params.yaml:	global_costmap_params.yaml:	local_costmap_params.yaml:
obstacle_range: 2.5 raytrace_range: 3.0 inflation_radius: 0.35 robot_radius: 0.30 observation_sources: laser_scan_sensor laser_scan_sensor: {sensor_frame: pico/laser, data_type: LaserScan, topic: /pico/laser, marking: true, clearing: true}	global_costmap: global_frame: /map robot_base_frame: /pico/base_link update_frequency: 5.0 static_map: true	local_costmap: global_frame: /pico/odom robot_base_frame: /pico/base_link update_frequency: 5.0 publish_frequency: 2.0 static_map: false rolling_window: true width: 3.0 height: 3.0 resolution: 0.05

base_local_planner_params.yaml

TrajectoryPlannerROS:
  acc_lim_x: 2.5
  acc_lim_y: 2.5
  acc_lim_theta: 3.2  
  max_vel_x: 0.2
  min_vel_x: 0.0
  max_vel_theta: 1.0
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0

  escape_vel: -0.2  
  holomonic_robot: true
  y_vels: [-0.2, -0.0, 0.0, 0.2]
  yaw_goal_tolerance: 0.05
  xy_goal_tolerance: 0.10
  latch_xy_goal_tolerance: false

  sim_time: 2.0
  sim_granularity: 0.005
  angular_sim_granularity: 0.025

  vx_samples: 5
  vtheta_samples: 10
 
  meter_scoring: true
  pdist_scale: 2.0
  gdist_scale: 1.0
  occdist_scale: 0.05
  heading_lookahead: 0.325
  heading_scoring: false
  heading_scoring_timestep: 0.8
  dwa: false
  publish_cost_grid_pc: false
  global_frame_id: /pico/odom

When a goal is given in Cartesian coordinates, Move_base navigates and publishes on /pico/cmd_vel to reach that goal, if it is possible. This is presented in the figures stated below. Move_base creates an inflated radius around the pico and an inflated radius around the obstacle pixels from GMapping. When the radii are set to the correct values, the pico will not hit any obstacles according to Move_base. When it is not possible for the pico to reach the given goal, Move_base goes into a recovery mode until the goal becomes reachable due to new map data obtained from the recovery or it is given a different goal.

An issue with Move_base is, that it will not move to its first goal when given by the goal planner. After a given time, the pico will go to a recovery mode and starts moving to its goal. The recovery mode is not desired in the beginning because of placing an arc in OpenCv, as will be explained in the next paragraph. Therefore the pico will be initialized by a velocity command in the x direction. The map will update again and pico drives to its first goal.

While GMapping and move_base are running, a tf tree can be made to see how the different frames are linked. For the chosen settings of GMapping and Move_base, this results in the following scheme:

Exploration

Explore_stage

An existing package which works as a goal planner, SLAM and navigation stack, is the explore_stage package. This package is an algorithm that interacts with GMapping to create a map to find exits and tries to navigate by launching a node in Move_base. Which is useful in maze solving case. In order to start this package, again a launch file is made. The same local and global costmap settings have been used as in the previous packages.

The following problems occur: when using RVIZ, it can be seen that a path is planned to an exit, but most of the time the pico seems stuck. Also, when defining a new goal manually by RVIZ, it repeats saying that a node is added on the coordinates: X.X and X.X but the pico does nothing. It also repeats the following warning: Entropy has not been updated. Loop closure opportunities may be ignored.

Within the given time limit, it was not possible to make the Explore_stage package work as desired. The choice is made to continue with GMapping and Move_base in addition with a goal_planner and arrow recognition created from scratch.

Goal planning - OpenCV

The goal planner makes use of different messages and data types. The most important messages and data types which form the basis of the goal planner algorithm are given below.

Datatype	Purpose
Odometry.msg	Odometry data from pico
OccupancyGrid.msg	Map data stored in a vector
MapMetaData.msg	Map information such as resolution and position of origin
LaserScan.msg	Laser data
MoveBaseActionFeedback
MoveBaseAction	Publish goal to move_base

Strategy

The strategy of the goal planner is chosen to be frontier based. The idea is that the destination goals of the pico are set around the lines between known and unknown area. This can be seen as an explorer which tells pico to explore all unknown areas. This is done with OpenCV by subscribing on the /Map topic on which a matrix/image of a map is published by GMapping. With some tools of the OpenCV library, this image of a map is processed in order to publish Cartesian coordinates to Move_base as a goal. Since, GMapping updates the map in time, new goals are set every time a map is updated. When multiple goals are given, the closest goal to pico is chosen as the primary goal. The choice of the correct primary goal will change when an arrow is detected. This will be explained in the next paragraph.

An issue occurs when there are a few laser beams passing through a hole in the wall or just around a corner. It is possible that a goal is set at the frontier of these few laser beams. Since it is not possible for pico to reach these goals, pico will go to its recovery mode. To solve this problem, a floodfill algorithm is used. Unknown area will be inflated and the area starting from pico will be floodfilled. The remaining area is seen as the known area. This way, unreachable areas will be filtered. This can be seen in the pictures given below. The pink area in the right figure is the floodfilled area.

Another issue is, that its starting point is an opening and will be seen as an frontier. It is therefore possible that the goal planner sets its goal to the start and tries to reach the unknown area. This problem is solved by adding an arc behind the starting position of pico in the OpenCV images which close the entrance. Therefore no goal will be set there.

Also an issue occurs when there is a frontier just before a wall which is not detected by the laser data yet. The goal will be set just before the wall and the pico will hit it when it reaches this goal. This problem is solved by inflating the unknown area. The goal will be set with an offset of the frontier. This problem was also solved by the fact that GMapping updates its map. The pico will see the wall when it moves towards the goal and a new goal will be set. BHence, when it takes too much time to update the map and process the data, this map updating would not solve the problem.

The data of a map is processed as followed:

1: - The laser data is converted to an image which can be processed with OpenCV. The laser data is provided as a vector in which the walls have a value of 100, the known area have a value of 1 and the unknown area have a value of 0. In the generated picture the walls are colored black, the known area white and the unknown area grey.

2: All black pixels are inflated with by a circle of a certain radius depending on the inflation radius chosen in the move_base. The color of the known area is changed to red.

3: All pixels of the unknown area (gray) which neighbor known area (red) are inflated by a square. This inflation is implemented to filter noise and remove areas that are unreachable. Next all boundaries between known and unknown area are colored green.

4: A binary is created for the green pixels only. This binary input is used to detect lines. The line detection is done by a hough line transform. The midpoint of each line will be set a provisional goal.

5: Each goal will be evaluated if it is reachable for Pico. To check this, a floodfill (pink) with Pico as starting point is used. The entry of the maze is closed with an ellipse so no goals will be found at the entry of the maze. All goals lying in the floodfilled area are considered as valid goal targets. Goals which do not lie in the flooded area are discarded.

6: At this point multiple valid goals are available but only one goal can be used as a goal to drive to. First the closest goal in front of pico is given priority. If there are no valid goals in front of pico, the closest goal behind pico will be chosen.

The image below shows the map processing for goal planning in several steps.

Recovery mode

If no valid goals are found, no path can be planned to a goal. Therefore pico will not move and no new goals will be planned. In this situation pico is stuck and a recovery is needed. If pico does not move for a certain time, pico will get in recovery mode. Pico will rotate at his current position. Rotating causes gmapping to update the map. From the updated map new valid goals can be found.

Initialization

At the startup, the goal planner does not recieve enough information to plan a valid goal point. Information such as the map and pico's orientation are not received yet. In order to get all the necessary data, pico first needs to move. Therefore an initialization is integrated. In this integration pico will move forward for 0.5m. By moving forward all the necessary information will be received by the goal planner and the normal algorithm as described above will provide pico with new goal points.

Arrow recognition - OpenCV

Strategy

The arrow detection node will publish alongside the goal planner node. The idea is to process the image of the camera in the following way: the image of the camera is converted from red-green-blue (RGB) to hue-saturation-value (HSV) (picture 1). A double for-loop is used to convert every red-tinted pixel to black. A binary (two-colour) image is created based on the black and non-black colours (picture 2). This image is then blurred and an edge detection algorithm gives an output of the edges that it can find (picture 3).

The list of edges is the basis for the arrow recognition, all lines are defined from left to right (so: x1 < x2). Lines are defined as follows: Vec4i line[x1,y1,x2,y2]. The lines are filtered, and the ones that remain are lines with a slope of +/-30 with a +/- 5 degrees margin. All lines with a negative slope are compared with all lines that have a positive slope. Both the +30 [deg] and the -30 [deg] line have a beginning and end point. If the left (begin) points are close enough to each other (3 pixels), the output is "LEFT ARROW", and vice versa. The arrow detection node publishes 0 when no arrow is found and -1 and 1 for the left and right arrow respectively.

The image below shows the +/-30 [deg] lines which this algorithm focuses on:

Integration

Using rqt_graph, the following graph is given:

Results

Corridor challenge:	Simulation 1:

Simulation 2:	Maze challenge:

Practical issues

TF tree

Viewing a tf tree can be done by:

$ rosrun tf view_frames

GMapping and Move_base in simulations

1. Start Gazebo:

$ gazebo 

2. Using another terminal, spawn the maze:

$ rosrun gazebo_map_spawner spawn_maze 

3. Launch PICO:

$ roslaunch pico_gazebo pico.launch 

4. Start RViz to visualize how PICO perceives the world through its sensors.

$ rosrun pico_visualization rviz 

5. Launch the gmapping plugin for PICO

$ roslaunch pico_2ndnav gmap.launch 

6. Launch the move_base package for PICO to navigate through the maze with the preferred settings.

$ roslaunch pico_2ndnav move_base.launch 

Explore_stage in simulations

1. Get in the right directory:

$ roscd explore_stage 

2. Launch the explore_stage launch file:

$ roslaunch explore_slam.launch 

Goal planning - OpenCV

In some cases, your script will not find the required include:

#include <move_base_msgs/MoveBaseActionFeedback.h> // Contains the location of pico 

The solution (ubuntu command):

sudo cp -a /opt/ros/groovy/stacks/navigation/move_base_msgs/msg_gen/cpp/include/. /opt/ros/groovy/include/

The OpenCV C++ files can be compiled using one of the given commands below. For the g++ command, the packages need to be specified manually.

$ g++ demo_opencv.cpp `pkg-config opencv --cflags --libs` -o demo_opencv 

Or you can make have ROS find the libaries for you:

$ rosmake demo_opencv 

Executing the program:

$ ./demo_opencv <argcount> <arg(s)> 

Goal planning - OpenCV Troubleshooting

There have been problems with the OpenCV and Highgui libraries (static libraries). This problem can be solved by replacing the following code:

// #include "cv.h" // (commented out)
#include "opencv2/opencv.hpp"
// #include <highgui.h> // (commented out)
#include <opencv/highgui.h>

The program might not always respond to input, this is can be due to a subscription to the wrong topic. Make sure you are streaming the images and check the ros-topic with:

  rostopic list  

Then the output gives "/pico/camera", "/pico/asusxtion/rgb/image_color", or a different name. Replace the referece in the following line of code:

  ros::Subscriber cam_img_sub = nh.subscribe("/pico/asusxtion/rgb/image_color", 1, &imageCallback); 

Runnig OpenCV on an image stream from a bag file:

In three separate terminals:

roscore
rosbag play <location of bag file>
./demo_opencv

Arrow recognition

The image is cropped using:

cv::Rect myROI(120,220,400,200);  // Rect_(x,y,width,height)
cv::Mat croppedRef(img_rgb, myROI); // The window is 480 rows and 640 cols

Pico experiment setup

Terminal 1:

ssh pico
pico123
pstart

Terminal 2:

pico-core
rviz rviz

Terminal 3:

ssh pico
pico123

roscd gmapping
**check if group 11 has gmapping if so delete gmapping map**
cd ..
rm -rf gmapping

cd ros/groovy/emc/groups/emc07
svn up

roscd pico_2ndnav
roslaunch pico_2ndnav gmap.launch

Terminal 4:

ssh pico
pico123
roscd pico_2ndnav
roslaunch pico_2ndnav move_base.launch

Terminal 5:

ssh pico
pico123
roscd goal_planner_roel2
rosmake goal_planner_roel2
./bin/goal_xtc

Terminal 6:

ssh pico
pico123
roscd demo_opencv_wesley
rosmake demo_opencv_wesley
./bin/demo_xtc

Time Table

Overview of time spent per group member

	Lectures	Group meetings	Mastering ROS and C++	Preparing midterm assignment	Preparing final assignment	Wiki progress report	Other activities
Week 1
Pim	2
Roel	2
Maurice	2
Wesley	2
Glenn	2
Week 2
Pim	2	2	4				3
Roel	0	2	10				6
Maurice	2	2	6				3
Wesley	2	2	4				2
Glenn	2	2	5				2
Week 3
Pim	2	1	12	2
Roel	2	4	10	2
Maurice	2	3	10	4
Wesley	2	3	10	1
Glenn	2	2	13	4
Week 4
Pim	2	5	1	7		3
Roel	2	5
Maurice	2	5		15
Wesley	2	5	9	1
Glenn	2	5		6			1
Week 5
Pim	2	2.5			7	0.5
Roel	2	2.5			6		1
Maurice	2	2.5			8	0.5
Wesley	2	5	7		3
Glenn	2	2.5			7
Week 6
Pim		4			6
Roel		4			6
Maurice		4			8
Wesley		4	8		14
Glenn		4			8	1
Week 7
Pim	2	2.5			10
Roel	2	4			8		3
Maurice	2	4			9	2
Wesley	2	4	2		12
Glenn	2	2			8	5
Week 8+9
Pim	4	1.5			23
Roel		2.5			20		4
Maurice		0.5			20	2	4
Wesley		0.5			29
Glenn					10	5