## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0=[[rc_visard/Tutorials/FirstSteps]]
## descriptive title for the tutorial
## title = Create octomap using rc_visard's onboard SLAM (Docker)
## multi-line description to be displayed in search 
## description = This tutorial shows how to create an [[octomap|OctoMap]] using the pointclouds, SLAM trajectory of the rc_visard and [[docker]]
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=[[rc_visard/Tutorials/Navigation with TurtleBot (Docker)]]
## next.1.link=
## what level user is this tutorial for 
## level= BeginnerCategory
## keywords = rc_visard, octomap, slam
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== Goal ==

This tutorial shows how to create an [[octomap|OctoMap]] using the pointclouds and the SLAM trajectory of the rc_visard - as demonstrated in the first half of the following "fruitful"  video:

<<Youtube(wdK23-gapqw)>>

(If the video doesn't show up, click [[https://youtu.be/wdK23-gapqw|here]])

== Overview ==

To create a 3D voxel representation of the environment we explore the environment with the rc_visard while recording the pointclouds. The optional onboard SLAM module on the rc_visard is running during recording, so we can get the optimized trajectory afterwards.
We then use this trajectory together with the recorded pointclouds to replay the bag file for generating an octomap.

The reason for not building the octomap "online" is that we can achieve a better quality/consisteny in the octomap if we build it afterwards when we already have the optimized trajectory (after loop closures).
One could also build the octomap incrementally/online while exploring the environment, but then points/voxels only observed once will be at the initially observed position and won't benefit from the loop closures of the onboard SLAM.

To collect the data, one possibility is to mount the rc_visard on a TurtleBot and drive it around using Teleop.

While this is fun to do, it is not necessary to have a turtlebot for creating the map and you could also just carry around the rc_visard by hand or mount it on any other platform.

== Setup ==

The Dockerfile is inside the [[https://github.com/roboception/rc_visard_turtlebot_tutorial/tree/melodic|tutorial repository]].

Download the Tutorial repository and change into its directory:
{{{
 $ git clone https://github.com/roboception/rc_visard_turtlebot_tutorial.git && cd rc_visard_turtlebot_tutorial
}}}

To build the Dockerimage execute inside the tutorial directory:
{{{
 $ sudo docker build -t turtlebot_image .
}}}

If your user is member of the ''docker'' group, the prefixed '''sudo''' is not necessary.
'''This tutorial assumes that your user is a member of this group.''' 

'''To execute ros-commands inside the container use the scripts found in the docker directory inside the tutorial directory.''' 

=== Setup with ssh ===

This is not necessary but convenient to monitor and start your process on the Turtlebot.
We use [[https://wiki.archlinux.org/index.php/GNU_Screen|screen]] as a terminal multiplexer so that we don't need to establish a new SSH-connection for each new bash-screen.
Each new screen is also in the working directory from which you started the screen session. Make sure to have added the source of ROS to your bashrc before starting your screen session:
{{{
 $ echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
}}}

=== Setup of the Turtlebot ===

You can skip this part if you are not using the Turtlebot.

Install the kobuki-ftdi package:
{{{
 $ sudo apt install ros-$ROS_DISTRO-kobuki-ftdi
}}}
Source ROS:
{{{
 $ source /opt/ros/$ROS_DISTRO/setup.bash
}}}

After sourcing your ROS set up the Turtlebot with a udev rule. If this is not set, the Turtlebot won't be connected at /dev/kobuki. Instead it will have a random name.  '''(Remember to have a'''(remember to have a [[roscore]] running when you are trying to use rosrun)'' [[roscore]] running when you are trying to use rosrun)''':
{{{
 $ roscore &
}}}
{{{
 $ rosrun kobuki_ftdi create_udev_rules 
}}}
{{{
 $ fg
}}}

Now stop the roscore with CTRL-C.

You need to reattach the USB-connection to update /dev.

To start the Turtlebot:
{{{
 $ ./docker/turtlebot_launch.sh
}}}
This will print out an error message around every 20 seconds but this message can be ignored.

For teleoperation with the keyboard use this:
{{{
 $ ./docker/keyop_teleop.sh
}}}

If you are also using teleoperation with a ps3-controller, launch it with:
{{{
 $ ./docker/ps3_teleop.sh
}}}
To move around keep the '''L1''' Button depressed.

== Execute ==

=== Start rc_visard_driver with trajectory publishing ===

To start the rc_visard_driver you need its serial number.
You can find the serial number using [[https://doc.rc-visard.com/latest/en/installation.html#web-gui|rcdiscover]]:
{{{
 $ rcdiscover
}}}
{{{
 $ ./docker/start_rc_visard <serial_number_of_your_device>
}}}

Make sure you configure the rc_visard appropriately for the application. You can use [[dynamic_reconfigure]] for this, see also [[rc_visard_driver]] for parameters.
For instance, settings for the Depth Image to consider are:

 * `depth_maxdepth`: Maximum depth. Don't set it too low, as the free information is valuable
 * `depth_maxdeptherr`: Maximum depth error. Don't set it too low, for the same reason, but also not too high, to avoid inaccuracies. For indoors 0.2-0.5 is good.
 * `depth_minconf`: Minimum confidence. Might be worth to increase, if you have problems with artifacts.

It's also a good idea to start RViz so you can see what the rc_visard is seeing.
If you are using a SSH-connection use your hostmachine. To connect your hostmachine to the ROS-network on the Turtlebot use this:
{{{
 $ export ROS_MASTER_URI=http://<ip_address_of_your_turtlebot>:11311
}}}
For more information about ROS-variables see [[http://wiki.ros.org/ROS/EnvironmentVariables|this]].

Start rviz with the configuration-file provided by this tutorial. If you have it installed on the machine, where you are using rviz:
{{{
 $ rviz -d $(rospack find rc_visard_turtlebot_tutorial)/cfg/map_generation.rviz
}}}
If you have not installed the tutorial there:
{{{
 $ rviz -d <path/to/rc_visard_turtlebot_tutorial/cfg/map_generation.rviz>
}}}

=== Record bagfile with pointclouds ===

In this part we will create and store bagfiles for our map in a map directory:
{{{
 $ mkdir map
}}}
{{{
 $ ./docker/run_script.sh "rosbag record --lz4 -O /map/clouds.bag /stereo/points2 __name:=tutorial_bag"
}}}
Now move the rc_visard around and explore the environment. Make sure that you are not mapping moving objects like colleagues since they would be shown as obstacles in the octomap.
When you are done, stop the rosbag record with
{{{
 $ rosnode kill /tutorial_bag
}}}

=== Get trajectory from rc_visard and store onboard SLAM map ===

Use the ''save_trajectory_as_tf.py'' script from the [[https://github.com/roboception/rc_visard_turtlebot_tutorial|tutorial repository]] to get the optimized trajectory from rc_visard and add the SLAM corrected tf transformations to the recorded pointclouds:
{{{
 $ ./docker/run_script.sh "rosrun rc_visard_turtlebot_tutorial save_trajectory_as_tf.py /map/clouds.bag map/clouds_with_tf.bag"
}}}

Stop the [[https://doc.rc-visard.com/v1.6/pdf/rc_visard_manual_en.pdf#section.7.1|SLAM module]] on rc_visard and store the internal SLAM map.
{{{
 $ rosservice call /rc_visard_driver/dynamics_stop
}}}
{{{
 $ rosservice call /rc_visard_driver/slam_save_map
}}}

'''Stop the rc_visard_driver and the turtlebot now.'''
To do this change into the windows, in which you started the turtlebot, the teleoperation, and the rc_visard_driver and terminate all of these with CTRL-C.


=== Generate octomap ===

Start the [[octomap_server]] with the launch file provided in the [[https://github.com/roboception/rc_visard_turtlebot_tutorial|tutorial repository]]
{{{
 $ ./docker/run_script.sh "roslaunch rc_visard_turtlebot_tutorial octomap_mapping.launch"
}}}

Playback the bagfile:
{{{
 $ ./docker/run_script.sh "rosbag play -r 1 /map/clouds_with_tf.bag"
}}}
{{{#!wiki caution
Since you are playing back tf data, make sure no other transformations are sent out on /tf, e.g., from the rc_visard or the turtlebot drivers}}}

You can adapt the playback rate depending on your CPU (if the octomap_server_node uses far less than 100% of one cpu core, you may increase the rate for faster processing, if it is close to 100% reduce it to make sure all point clouds are processed).
Finally save the octomap when the octomap is done:
{{{
 $ ./docker/run_script.sh "rosrun octomap_server octomap_saver -f /map/mapfile.ot"
}}}

Before starting the next tutorial go to the window, where you started the octomap_mapping.launch, and shut it down with CTRL-C.

== What Next? ==

 * [[rc_visard/Tutorials/Navigation with TurtleBot (Docker)|Autonomous Navigation using OctoMap and rc_visard on TurtleBot]]

## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE