Merge pull request IntelRealSense#5 from otcshare/RAR-142-Navigation-…

…Documentation

Rar 142 navigation documentation

camera/licenses/ReadMe.txt

@@ -1,8 +1,12 @@
-Copyright (c)  2016, Intel Corporation.
-
-This RealSense-ROS ("Software") is furnished under license and may only be used or copied in accordance with the terms of that license. 
-No license, express or implied, by estoppel or otherwise, to any intellectual property rights is granted by this document. 
-The Software is subject to change without notice, and should not be construed as a commitment by Intel Corporation to market, license, sell or support any product or technology. 
-Unless otherwise provided for in the license under which this Software is provided, the Software is provided AS IS, with no warranties of any kind, express or implied. 
-Except as expressly permitted by the Software license, neither Intel Corporation nor its suppliers assumes any responsibility or liability for any errors or inaccuracies that may appear herein. 
-Except as expressly permitted by the Software license, no part of the Software may be reproduced, stored in a retrieval system, transmitted in any form, or distributed by any means without the express written consent of Intel Corporation.
+Copyright (c) 2016, Intel Corporation
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+2. 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.
+
+3. 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.

camera/ros-realsense-nav/turtlebot/README.md

@@ -0,0 +1,122 @@
+<link rel="stylesheet" href="style-doc.css" />
+
+# Navigation with Intel&reg; RealSense&trade; **R200** camera
+This document presents the transition from a **Kinect** to the **R200** for the ROS Navigation stack.
+It assumes the **R200** drivers were already installed, and that the navigation using the Kinect is working.
+
+#### Document outline
+Click on the following links to navigate this document
+
+- [Initial setup](#a-initial-setup)
+- [Mapping](#b-mapping)
+- [Navigation](#c-navigation)
+- [Simulating a mapping](#d-simulating-a-mapping)
+
+## A - Initial setup
+
+First, make sure you have the latest versions of the ROS navigation stack. Specifically, check that the following packages are up-to-date:
+
+- **turtlebot_description**
+- **turtlebot_bringup**
+
+Before being able to use the navigation stack with the **R200**, you need to set a few things up.
+
+The `install_realsense_navigation.sh` script (located in the `install_resources` folder) will do everything needed. It will place the required files in their respective locations.
+
+From now on, if you want to use the **R200** as your turtlebot 3d sensor, you need to change environment variables:
+
+```bash
+export TURTLEBOT_3D_SENSOR=r200
+export TURTLEBOT_STACKS=minimal
+```
+
+To have quick shortcuts, consider pasting this code snippet in your `~/.bash_aliases` file:
+
+```bash
+alias setr200='export TURTLEBOT_3D_SENSOR=r200 && export TURTLEBOT_STACKS=minimal'
+alias setkinect='export TURTLEBOT_3D_SENSOR=kinect && export TURTLEBOT_STACKS=hexagons'
+```
+
+## B - Mapping
+
+The only difference from the kinect version of the navigation stack is that you need to start the camera driver before the navigation: `roslaunch realsense_camera realsense_r200_nodelet_standalone_preset.launch`.
+
+So, the normal flow would be :
+
+```bash
+roslaunch turtlebot_bringup minimal.launch
+roslaunch realsense_camera realsense_r200_nodelet_standalone_preset.launch
+roslaunch realsense_navigation gmapping.launch
+roslaunch turtlebot_rviz_launchers view_navigation.launch
+```
+
+You may also want to teleoperate the robot, for instance with the keyboard.
+
+![](doc/img/mapping_screen.png)
+
+Once you're done mapping, you can save the map with the following command:
+
+```bash
+rosrun map_server map_saver -f <map-file>
+```
+
+> Don't put an extension to your map name, the .*yaml* and .*pgm* files will be created accordingly in you `~/.ros` folder. If you want, you can also provide an absolute path.
+
+
+## C - Navigation
+
+Once you have a map, you can start the navigation with the following commands
+
+```bash
+roslaunch turtlebot_bringup minimal.launch
+roslaunch realsense_camera realsense_r200_nodelet_standalone_preset.launch
+roslaunch realsense_navigation navigation_demo.launch map:=<map-file>
+```
+
+## D - Simulating a mapping
+Now that you know how to map a room using the **R200**, you might need to repeat the process several times, to adjust all the parameters of the algorithm, or to test with depth enhancement. To avoid using the robot, we can simulate the scanning process (= robot moves + camera) thanks to **rosbag**.
+
+### 1. Recording a scan
+The first thing to do is to record the scanning process. We will store in a *bag file* the required contents so we can later launch gmapping **on the same input data**.
+
+So we first need to start the turtlebot (+ the teleop of your choice) and the camera:
+
+```bash
+roslaunch turtlebot_bringup minimal.launch
+roslaunch realsense_camera realsense_r200_nodelet_standalone_preset.launch
+```
+
+Then, we will store the relevent topics in a bag file:
+
+* _/camera/depth/image_raw_
+* _/camera/depth/camera_info_
+* _/scan_
+* _/tf_
+
+![](doc/img/bag_screen.png)
+*You can visualize the bagfile with __rqt_bag__*
+
+> Note that we don't need to actually build the map, so calling gmapping is not necessary. But you can do it, to have an idea if you're not moving the robot too fast, for instance.
+
+```bash
+rosbag record -O <output.bag> /camera/depth/image_raw /camera/depth/camera_info /mobile_base/commands/velocity /tf
+```
+
+You can now begin to move/teleop your robot around the room. Once you're done, kill first the rosbag record process, then the other nodes.
+
+### 2. Playing it back
+From now on, you'll be able to simulate this mapping again. To do so, you should simply type this line in a terminal (without any other running node):
+
+```bash
+roslaunch realsense_navigation simulate_mapping.launch bag_file:=<output.bag>
+```
+
+> The path to the bagfile is relative to the user `~` folder. To change this, use the *home* argument as follows: 
+>
+> ```bash
+> roslaunch realsense simulate_mapping.launch home:=/tmp bag_file:=<output.bag>
+> ```
+
+When the RVIZ window is up, you can press `SPACE` in your terminal to start the playback.
+
+Consider taking a look at the launchfile to see the different arguments.

camera/ros-realsense-nav/turtlebot/doc/style-doc.css

@@ -0,0 +1,46 @@
+/* Custom CSS for a better looking doc */
+body {
+    width: 60em;
+    outline-color: #eee;
+}
+
+/* For smaller screens, adapt so it's still legible */
+@media screen and (max-width: 70em) {
+	body {
+		width: 40em;
+	}	
+}
+@media screen and (max-width: 45em) {
+	body {
+		width: 90%;
+	}
+
+	.markdown-body ul, .markdown-body ol {
+	  padding-left: 1em;
+	}
+}
+
+.markdown-body h1 {
+    text-align: center;
+    border-width: 4px;
+}
+
+.markdown-body h1 small {
+    font-weight: normal;
+
+}
+
+.markdown-body h1 small:after {
+    content: ">";
+    padding-left: 1em;
+    padding-right: 1em;
+}
+
+.markdown-body hr {
+	background-color: #eee;
+}
+
+.markdown-body .codehilite pre, .markdown-body pre {
+    background-color: #f0f0ed;
+}
+

camera/ros-realsense-nav/turtlebot/install_realsense_navigation.sh

@@ -0,0 +1,10 @@
+#!/bin/bash
+
+# Installation script for realsense_navigation
+# Requires root privileges
+
+sudo cp ./robot_description/kobuki_minimal_r200.urdf.xacro /opt/ros/$ROS_DISTRO/share/turtlebot_description/robots/
+sudo cp ./robot_description/r200.launch.xml /opt/ros/$ROS_DISTRO/share/turtlebot_bringup/launch/includes/3dsensor/
+
+sudo mkdir -p /opt/ros/$ROS_DISTRO/share/realsense_navigation
+sudo cp -r ./* /opt/ros/$ROS_DISTRO/share/realsense_navigation

camera/ros-realsense-nav/turtlebot/launch/amcl.launch

@@ -0,0 +1,78 @@
+<launch>
+  <arg name="3d_sensor" default="$(env TURTLEBOT_3D_SENSOR)" />
+  <arg name="custom_param_file" default="$(find realsense_navigation)/robot_description/custom_costmap_params.yaml"/>
+
+  <include file="$(find turtlebot_bringup)/launch/3dsensor.launch">
+    <arg name="rgb_processing" value="false" />
+    <arg name="depth_registration" value="false" />
+    <arg name="depth_processing" value="false" />
+    <arg name="3d_sensor"     value="$(arg 3d_sensor)"/>
+
+    <!-- We must specify an absolute topic name because if not it will be prefixed by "$(arg camera)".
+         Probably is a bug in the nodelet manager: https://github.com/ros/nodelet_core/issues/7 --> 
+    <arg name="scan_topic" value="/scan" />
+  </include>
+
+  <!-- Map server -->
+  <arg name="map_file" default="$(find turtlebot_navigation)/maps/willow-2010-02-18-0.10.yaml"/>
+  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
+
+  <!-- Start position on the map -->
+  <arg name="initial_pose_x" default="0.0"/>
+  <arg name="initial_pose_y" default="0.0"/>
+  <arg name="initial_pose_a" default="0.0"/>
+
+  <arg name="use_map_topic"   default="false"/>
+  <arg name="scan_topic"      default="scan"/> 
+  <arg name="odom_frame_id"   default="odom"/>
+  <arg name="base_frame_id"   default="base_footprint"/>
+  <arg name="global_frame_id" default="map"/>
+
+  <node pkg="amcl" type="amcl" name="amcl">
+    <param name="use_map_topic"             value="$(arg use_map_topic)"/>
+    <!-- Publish scans from best pose at a max of 10 Hz -->
+    <param name="odom_model_type"           value="diff"/>
+    <param name="odom_alpha5"               value="0.1"/>
+    <param name="gui_publish_rate"          value="10.0"/>
+    <param name="laser_max_beams"             value="60"/>
+    <param name="laser_max_range"           value="12.0"/>
+    <param name="min_particles"             value="500"/>
+    <param name="max_particles"             value="2000"/>
+    <param name="kld_err"                   value="0.05"/>
+    <param name="kld_z"                     value="0.99"/>
+    <param name="odom_alpha1"               value="0.2"/>
+    <param name="odom_alpha2"               value="0.2"/>
+    <!-- translation std dev, m -->
+    <param name="odom_alpha3"               value="0.2"/>
+    <param name="odom_alpha4"               value="0.2"/>
+    <param name="laser_z_hit"               value="0.5"/>
+    <param name="laser_z_short"             value="0.05"/>
+    <param name="laser_z_max"               value="0.05"/>
+    <param name="laser_z_rand"              value="0.5"/>
+    <param name="laser_sigma_hit"           value="0.2"/>
+    <param name="laser_lambda_short"        value="0.1"/>
+    <param name="laser_model_type"          value="likelihood_field"/>
+    <!-- <param name="laser_model_type" value="beam"/> -->
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d"              value="0.25"/>
+    <param name="update_min_a"              value="0.2"/>
+    <param name="odom_frame_id"             value="$(arg odom_frame_id)"/> 
+    <param name="base_frame_id"             value="$(arg base_frame_id)"/> 
+    <param name="global_frame_id"           value="$(arg global_frame_id)"/>
+    <param name="resample_interval"         value="1"/>
+    <!-- Increase tolerance because the computer can get quite busy -->
+    <param name="transform_tolerance"       value="1.0"/>
+    <param name="recovery_alpha_slow"       value="0.0"/>
+    <param name="recovery_alpha_fast"       value="0.0"/>
+    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
+    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
+    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
+    <remap from="scan"                      to="$(arg scan_topic)"/>
+  </node>
+
+  <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml">
+    <arg name="custom_param_file" value="$(arg custom_param_file)"/>
+  </include>
+
+</launch>
+

camera/ros-realsense-nav/turtlebot/launch/gmapping.launch

@@ -0,0 +1,59 @@
+<launch>
+	<arg name="3d_sensor"	default="$(env TURTLEBOT_3D_SENSOR)" />
+	<arg name="scan_topic"  default="scan" />
+	<arg name="base_frame"  default="base_footprint"/>
+	<arg name="odom_frame"  default="odom"/>
+
+	<include file="$(find turtlebot_bringup)/launch/3dsensor.launch">
+		<arg name="rgb_processing"		value="false" />
+		<arg name="depth_registration"	value="false" />
+		<arg name="depth_processing"	value="false" />
+		<arg name="3d_sensor"			value="$(arg 3d_sensor)"/>
+
+		<!-- We must specify an absolute topic name because if not it will be prefixed by "$(arg camera)".
+		Probably is a bug in the nodelet manager: https://github.com/ros/nodelet_core/issues/7 --> 
+		<arg name="scan_topic" value="/scan" />
+	</include>
+
+	<!-- Copied from the gmapping.launch.xml file (turtlebot_navigation), so we can change here the parameters of the algorithm -->
+	<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
+		<param name="base_frame" value="$(arg base_frame)"/>
+		<param name="odom_frame" value="$(arg odom_frame)"/>
+		<param name="map_update_interval" value="5.0"/>
+		<param name="maxUrange" value="6.0"/>
+		<param name="maxRange" value="8.0"/>
+		<param name="sigma" value="0.05"/>
+		<param name="kernelSize" value="1"/>
+		<param name="lstep" value="0.05"/>
+		<param name="astep" value="0.05"/>
+		<param name="iterations" value="5"/>
+		<param name="lsigma" value="0.075"/>
+		<param name="ogain" value="3.0"/>
+		<param name="lskip" value="0"/>
+		<param name="minimumScore" value="250"/> <!-- previous: 50 -->
+		<param name="srr" value="0.01"/>
+		<param name="srt" value="0.02"/>
+		<param name="str" value="0.01"/>
+		<param name="stt" value="0.02"/>
+		<param name="linearUpdate" value="0.5"/>
+		<param name="angularUpdate" value="0.218"/> <!-- prev 0.436 -->
+		<param name="temporalUpdate" value="5.0"/>
+		<param name="resampleThreshold" value="0.5"/>
+		<param name="particles" value="80"/> <!-- prev 80 -->
+
+		<param name="xmin" value="-1.0"/>
+		<param name="ymin" value="-1.0"/>
+		<param name="xmax" value="1.0"/>
+		<param name="ymax" value="1.0"/>
+
+		<param name="delta" value="0.01"/> <!-- previous: 0.05 -->
+		<param name="llsamplerange" value="0.01"/>
+		<param name="llsamplestep" value="0.01"/>
+		<param name="lasamplerange" value="0.005"/>
+		<param name="lasamplestep" value="0.005"/>
+		<remap from="scan" to="$(arg scan_topic)"/>
+	</node>
+
+	<include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
+
+</launch>

camera/ros-realsense-nav/turtlebot/launch/navigation_demo.launch

@@ -0,0 +1,40 @@
+<!--
+Launch file to launch the navigation demo
+Doesn't launch the camera!
+
+Author: Raphael Groscot
+
+Arguments:
+  [REQ] ~map: the file relative to home
+
+  [OPT] ~home: Parent folder. Default = ~ (user home)
+
+  [OPT] ~camera: The name of the camera to use (r200, kinect, ...). Default = r200
+
+  [OPT] ~depth_enhance: Use the depth enhance nodelet. Default = false
+
+  [OPT] ~display_depth: display, in RVIZ, the depth image. Default = true
+-->
+<launch>
+  <!-- ARGS -->
+  <arg name="map"/>
+  <arg name="home" default="$(env HOME)"/>
+<!--   <arg name="camera" default="r200"/>
+  <arg name="depth_enhance" default="false"/> -->
+  <arg name="display_depth" default="true"/>
+
+  <!-- RVIZ with custom layout -->
+  <node if="$(arg display_depth)" pkg="rviz" type="rviz" name="rviz" args="-d $(find realsense_navigation)/rviz/navigation_r200.rviz"/>
+  <node unless="$(arg display_depth)" pkg="rviz" type="rviz" name="rviz" args="-d $(find realsense_navigation)/rviz/navigation_r200_without_depth.rviz"/>
+
+  <!-- AMCL -->
+  <include file="$(find realsense_navigation)/launch/amcl.launch">
+    <arg name="map_file" value="$(arg home)/$(arg map)"/>
+  </include>
+
+  <!-- Depth enhance nodelet -->
+<!--   <include if="$(arg depth_enhance)" file="$(find realsense_depth_enhance)/launch/realsense_depth_enhance.launch">
+    <arg name="manager" value="camera/camera_nodelet_manager"/>
+  </include> -->
+
+</launch>

camera/ros-realsense-nav/turtlebot/launch/realsense_robot_description.launch

@@ -0,0 +1,14 @@
+<launch>
+	<arg name="base" default="$(env TURTLEBOT_BASE)"/>
+	<arg name="3d_sensor" default="r200"/>
+	<arg name="stacks" default="minimal"/>
+
+	<include file="$(find turtlebot_bringup)/launch/includes/description.launch.xml">
+		<arg name="base" value="$(arg base)"/>
+		<arg name="3d_sensor" value="$(arg 3d_sensor)"/>
+		<arg name="stacks" value="$(arg stacks)"/>
+		<!-- Uncomment this to set a custom urdf/xacro description file
+		<arg name="urdf_file" value="$(find realsense)/.../..."/>
+		-->
+	</include>
+</launch>