3D Autonomous Navigation

If using this code, please cite the associated publication:

@INPROCEEDINGS{6719360,
author={M. Gianni and G. Gonnelli and A. Sinha and M. Menna and F. Pirri},
booktitle={2013 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR)},
title={An Augmented Reality approach for trajectory planning and control of tracked vehicles in rescue environments},
year={2013},
volume={},
number={},
pages={1-6},
keywords={augmented reality;mobile robots;path planning;pose estimation;SLAM (robots);tracked vehicles;tracking;trajectory control;augmented reality;trajectory planning;tracked vehicles;rescue environments;human operator;AR-based interface;3D path planning;obstacle negotiation;3D movements;marker pen;trajectory tracking controller;localization system;position feedback;dead reckoning system;ICP-based SLAM;pose estimation;3D map;planning framework;autonomous robot navigation;Trajectory;Vehicles;Three-dimensional displays;Robot kinematics;Tracking;Planning;Augmented Reality;Trajectory tracking;Trajectory control;Unmanned Ground Vehicles;Urban Search;Rescue Robotics},
doi={10.1109/SSRR.2013.6719360},
ISSN={2374-3247},
month={Oct},}

It seems that PCL needs to be recompiled with the -std=c++11 flag enabled in order to not cause segfaults on the initialization of boost.

To correct this, you need to compile PCL from their git repo. One way to make PCL compile with the c++11 option is to add this to the CMakeLists.txt: SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")

When you have the PCL compiled, you need to make install, and change CMakeLists.txt on tradr_path_planner to use these libraries, this can be done by changing the line: find_package(PCL 1.7 REQUIRED COMPONENTS common io) to find_package(PCL REQUIRED COMPONENTS common io)"

1. Download version 3.2.2 (tested) from http://coppeliarobotics.com/files/V-REP_PRO_V3_2_2_64_Linux.tar.gz
   You don't need to compile anything. Just extract the files in your V-REP installation folder and you are ready to execute the main launcher (vrep.sh) from there.

2. Set the environment variable VREP_ROOT_DIR:
   add in your .bashrc the following line
   export VREP_ROOT_DIR=<here you put the absolute path of your V-REP installation folder (which contains the launcher vrep.sh)>

• Open a terminal
• Then execute

$ cd ~/trav_nav_indigo_ws

• Then compile

$ catkin_make -j8

Once you have compiled the tradr-simulation stack, you have to copy the lib ~/trav_nav_indigo_ws/devel/lib/libv_repExtRos.so in the installation folder VREP_ROOT_DIR (NOTE: this lib enables V-REP to get and parse track velocity command messages)

• Open a terminal, source the workspace and execute

$ roslaunch vrep_ugv_simulation vrep_ugv_simulation.launch

• Press the play button on V-REP

Once you completed one of the above procedures, you should have your simulator running and a small window with title "UGV TeleOp" should appear. Keep the focus on that window (click on it) and you will be able to move the UGV with the arrow keys. By using the keys 'A','S','D','W' you are also able to change the configuration of its flippers in order to climb stairs.

1. Lauch the UGV simulation on V-REP: open a new terminal, source the workspace and run

$ roslaunch vrep_ugv_simulation vrep_ugv_simulation.launch

2. Press the play button on V-REP

3. Run the path planner with its RVIZ interface

   • If you want to run the single-waypoint path planner, open a new terminal and run

   
   $ roslaunch tradr_path_planner sim_main_tradr_path_planner_ugv1.launch
   

   On RVIZ, you can set a goal for the planner by using the dedicated interactive marker. The interactive marker (a sphere) will appear over the robot when you launch the tradr_path_planner node.

   Move the marker at your desired position (hold left click on it and move it w.r.t. image plane; if you also hold SHIFT button you will change the depth of the marker) Then right-click on it and select from the menu the action "Select Goal". If you want to abort the goal once is selected, select the action "Abort Goal" from the same menu. Text messages will appear over the marker explaining you what is happening. The marker color will change accordingly:

   • Grey, path planner is waiting for a goal selection
   • Yellow, the path planner is planning
   • Green, a path has been found
   • Red, the path planner could not find a path

   N.B.: a path to the designated goal can be actually computed if the shown traversability map actually "connect" the goal and the robot positions. You may be required to wait few seconds till the traversability node actually complete the traversability map construction from the mapper inputs.

   Once a path has been found, the path planner will publish it towards the trajectory controller which will make the robot automatically follow the path.

   • otherwise, if you want to run the multi-waypoint path planner, open a new terminal and run

   
   $ roslaunch tradr_path_planner sim_main_queue_tradr_path_planner_ugv1.launch
   

   On RVIZ:

   • Press the key 'M' in order to add a new waypoint directly on the traversability cloud. You can move each created waypoint by holding right click on it and moving. The waypoints should automatically stick to the traversability cloud.
   • Once you have selected your desired number of waypoints you can right click on one of them and select from the menu the action "Append Task". If you want the robot to continuously revisit the waypoints you set (cyclic path), then select the action "Append Cyclic Task". The marker colors will change accordingly:
     • Orange, the marker has not been added
     • Yellow, the path planner is planning
     • Green, a path has been found
     • Red, the path planner could not find a path Once the waypoints get green, you can right click on one of them and select from the menu the action "Stop the controller" in order to stop the trajectory control and the robot.

To tune the parameters of these functionalities open a new terminal, source the workspace and execute

$ rosrun rqt_reconfigure rqt_reconfigure

A concise description of these parameters can be found in ~/trav_nav_indigo_ws/src/tradr-loc-map-nav/tradr_path_planner/README.md file

If you want to use the version of Octomap modified to merge point cloud coming from different channels (sensors) in a single octree then you have to modify the launch file where your octomap node is called as follows

• Change the name of the package from

pkg="octomap_server"

to

pkg="ms_octomap_server"

• Remap the point cloud_in1, cloud_in2 and cloud_in3 topics with the topics where the point clouds coming from differents sensors are published. For example

<remap from = "cloud_in1" to = "/point_cloud_from_laser1"/>
<remap from = "cloud_in2" to = "/point_cloud_from_laser2"/>
<remap from = "cloud_in3" to = "/point_cloud_from_rgbd_camera"/>