Merge pull request ETHZ-RobotX#13 from VIS4ROB-lab/feature/ros_nav_stack

SMB 2.0 Integration

Author: lucaBartolomei

LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2018, ETHZ ASL
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -1,130 +1,73 @@
 # Super Mega Bot Path Planner
 
-Package for path planning of Super Mega Bots for the first ETH Robotics 
-Summer School, 2019. 
+Package for path planning of Super Mega Bots for the ETH Robotics Summer School. 
 The package has been tested under ROS Melodic and Ubuntu 18.04.
 
-__Author: Luca Bartolomei__  
-__Affiliation: Vision For Robotics Lab, ETH Zurich__  
-__Contact: Luca Bartolomei, lbartolomei@ethz.ch__
-# Getting Started
-If the packages from the repository 
-[`ethz-asl/eth_robotics_summer_school_2019`](https://github.com/ethz-asl/eth_robotics_summer_school_2019) have already been downloaded and 
-built, it is possible to skip the "Installation" section and go directly to the
-["Structure of the code"](#structure-of-the-code).  
-If this is not the case, follow the installation instructions below.
-## Installation
-This package is intended to be used with Ubuntu 18.04 and [ROS melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) or above.
-After installing ROS, install some extra dependencies:
-```asm
-$ sudo apt-get install ros-melodic-cmake-modules ros-melodic-ompl ros-melodic-costmap-2d ros-melodic-grid-map ros-melodic-grid-map-visualization ros-melodic-velodyne-gazebo-plugins python-wstool python-catkin-tools libyaml-cpp-dev protobuf-compiler autoconf
-```
-Then if not already done so, set up a new catkin workspace:
-```asm
-$ source /opt/ros/melodic/setup.bash
-$ mkdir -p ~/catkin_ws/src
-$ cd ~/catkin_ws
-$ catkin init
-$ catkin config --extend /opt/ros/melodic
-$ catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
-$ catkin config --merge-devel
-```
-In this installation guide, it is assumed that the following packages for 
-simulation have already been downloaded and built in the same catkin workspace:
-* `any_node`
-* `message_logger`
-* `smb_sim`
-* `smb_path_following_controller`
-* `elevation_mapping`
-* `elevation_mapping_demo`
-* `traversability_estimation`
+__Author__: Luca Bartolomei  
+__Affiliation__: Vision For Robotics Lab, ETH Zurich  
+__Contact__: Luca Bartolomei, lbartolomei@ethz.ch  
 
-To download all the other necessary dependencies for the path planner, run 
-the following script in the source folder of your catkin workspace, by 
-specifying the destination `src_workspace_path` of the clone procedure:
-```asm
-$ chmod +x smb_path_planner/bin/clone_repos.sh
-$ ./smb_path_planner/bin/clone_repos.sh -f src_workspace_path
+# Installation instructions  
+Install the following packages first:
 ```
-To show the help of the installation script, make it executable and run:
-```asm
-$ ./smb_path_planner/bin/clone_repos.sh -h
+sudo apt-get install ros-melodic-cmake-modules ros-melodic-velodyne-gazebo-plugins python-wstool python-catkin-tools ros-melodic-ompl ros-melodic-move-base ros-melodic-navfn ros-melodic-dwa-local-planner ros-melodic-costmap-2d ros-melodic-teb-local-planner ros-melodic-robot-self-filter ros-melodic-pointcloud-to-laserscan
 ```
-Then, compile:
-```asm
-$ cd ~/catkin_ws
-$ catkin build smb_path_planner
-```
-## Structure of the code
-The `smb_path_planner` package is composed by 4 parts:
-* `smb_planner_common`: this package implements all the utility functions for 
-visualization, parameter reading and traversability estimation that are 
-needed by both the global and the local planners.
-* `smb_planner_rviz`: this package is the implementation for the planning 
-panel in RViz.
-* `smb_global_planner`: global planner package. It is possible to select 
-different planning algorithms (RRT*, Informed RRT*, PRM) by modifying the 
-parameter file `smb_planner_parameters.yaml`.
-* `smb_local_planner`: local planner package. It implements the CHOMP solver 
-with nonholonomic constraints.  
+Then follow the instructions [here](https://github.com/ETHZ-RobotX/SMB_dev). **Note:** the project may not be publicly available yet. If not refer to the original ETHZ Summer School [repository](https://github.com/ethz-asl/eth_supermegabot).
 
-The parameters for the planners and the elevation and traversability mapping 
-are stored in `smb_path_planner/smb_planner_common/cfg/`.
+## Additional documentation
+Additional documentation on `move_base` can be found [here](https://wiki.ros.org/move_base), while for the local planner, refer to the TEB Local Planner instructions [here](https://wiki.ros.org/teb_local_planner). Finally, the documentation about `costmap_2d` is available [here](https://wiki.ros.org/costmap_2d).
+
+## Structure of the code
+The `smb_path_planner` package is composed by 3 packages:
+* `smb_ompl_planner`: global planner for `move_base` based on the [OMPL library](http://ompl.kavrakilab.org/) for motion planning;
+* `smb_navigation`: package containing utilities, configurations and launch files for the planning with `move_base`;
+* `smb_navigation_rviz`: package containing the RViz plugin to put a goal for the planner easily;
+* `traversability_layer`: custom `costmap_2d` implementation to incorporate traversability maps.
 
 ## Planning Panel in RViz
-Make sure all the packages have built successfully. As a sanity check, 
-re-source your workspace (`$ source ~/catkin_ws/devel/setup.bash`) and start 
-up RViz (`$ rviz`).
-In RViz, select `Panels -> Add New Panel` and select `Planning Panel` under 
-`smb_planner_rviz`.
+Make sure all the packages have built successfully. As a sanity check, re-source your workspace (`$ source ~/catkin_ws/devel/setup.bash`) and start up RViz (`$ rviz`). In RViz, select `Panels -> Add New Panel` and select `Planning Panel` under `smb_navigation_rviz`.
 
-Next, under `Displays`, add an `InteractiveMarkers` display with the topic 
-`/planning_markers/update`. You should be able to see the interactive markers
- and the planning panel.
+Next, under `Displays`, add an `InteractiveMarkers` display with the topic `/planning_markers/update`. You should be able to see the interactive markers and the planning panel.
 
 ## How to run the planner in simulation
 First, start the simulation in Gazebo, RViz and RQT Plugin to select the 
 controller, by running:
-```asm
+```
 $ roslaunch smb_sim sim_path_planner.launch
 ```
-In the controller panel, select `SmbPathFollowingController` from 
-the list. If this controller does not show up, press the refresh button and 
-try again. To start the controller, press the play button.  
-Then, start the `elevation_mapping` node:
-```asm
-$ roslaunch smb_local_planner smb_elevation_mapping_simulation.launch
-``` 
-Finally, start the local and global planners:
-```asm
-$ roslaunch smb_local_planner smb_planner_simulation.launch
+In the controller panel, select `MpcTrackLocalPlan` from the list. If this controller does not show up, press the refresh button and try again. To start the controller, press the play button. Finally, start the local and global planners. If you want to use RRTs as global planner, run:
+```
+$ roslaunch smb_navigation navigate2d_ompl.launch
+```  
+Otherwise, to use a standard global planner from `move_base`, run:
 ```
-To send a global goal position, select it in the planning panel and press the
-button `Global Planner Service`. Once a global path has been computed and 
-showed in RViz, start the local planner by pressing the button `Start Local 
-Planner`.  
+$ roslaunch smb_navigation navigate2d.launch
+```  
+To send a global goal position there a set of different possibilities:
+* Set a goal with the planning panel and press the button `Start Planning`;
+* Use RViz direcly by using the button `2D Nav Goal` and setting the goal pose;
+* Publish directly on the topic `/move_base_simple/goal`.  
 
-## How to run the planner on the real robot
-Start the `LPC` on the robot and the `OPC` on the operator side. To run the 
-`LPC`, run the following commands in the PC of the robot in multiple terminals:
-```asm
-$ roscore # terminal 1
-$ roslaunch smb_lpc lpc.launch # terminal 2
+### How to run the planner in another frame
+If the `world` frame is not available, it is possible to use the odometry frame for planning. for example, if the frame in use is called `odom`, run:
 ```
-Start the LiDAR and the mapping in another terminal in the robot PC:
-```asm
-$ roslaunch ethzasl_icp_mapper supermegabot_robosense_dynamic_mapper.launch # terminal 3
-``` 
- Then to start the planners, run on the PC of the robot in separate terminals:
-```asm
-$ roslaunch smb_local_planner smb_elevation_mapping_real.launch # terminal 4
-$ roslaunch smb_local_planner smb_planner_real.launch # terminal 5
+$ roslaunch smb_navigation navigate2d_ompl.launch global_frame:=odom
+```  
+
+### Running with traversability estimation
+Start the simulation as in the previous case, and then run:
 ```
-On the operator side, start the `OPC`. First connect the `rosmaster` of the 
-PC to the `rosmaster` of the robot and then run in a terminal:
-```asm
-$ roslaunch smb_opc opc.launch
+$ roslaunch smb_navigation navigate2d_ompl.launch run_traversability:=true
 ```
-To send a global goal position, follow the same procedure as in simulation:
-1. Select the `SmbPathFollowingController` in the control panel and start it;
-2. Send the global goal using the `Planning Panel` in RViz.
+Notice that in this case, there are 3 different cost layers (static, laser scans and inflation layers), but **only** the static layer is active. 
+
+It is also possible to use a custom layer (`traversability_layer`). To use it, follow the instructions in the configuration file `smb_navigation/config/move_base_costmaps/local_costmap_params_traversability.yaml`.  
+In this case, notice that it is not possible to run the obstacle layer (based on laser scans) and the traversability layer at the same time in the current configuration, as the laser scan clears the traversability map. You are more than welcome to find a proper way to fuse these two maps!
+
+## How to run the planner on the real robot
+Connect to the robot and start the state estimation and control pipeline. Once it is started, run the planner as before:
+```
+$ roslaunch smb_navigation navigate2d_ompl.launch
+```  
+If necessary, set the right global frame used for planning.
+