switch rtabmap_lib svn co from the 0.3 branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@110 f169173b-cf89-36c8-b27e-44dbe73f0c83

rtabmap/.cproject

@@ -0,0 +1,212 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?fileVersion 4.0.0?>
+
+<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
+<storageModule moduleId="org.eclipse.cdt.core.settings">
+<cconfiguration id="0.250647335">
+<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.250647335" moduleId="org.eclipse.cdt.core.settings" name="Default">
+<externalSettings/>
+<extensions>
+<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+<extension id="org.eclipse.cdt.core.MakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+</extensions>
+</storageModule>
+<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+<configuration artifactName="rtabmap-pkg" buildProperties="" description="" id="0.250647335" name="Default" parent="org.eclipse.cdt.build.core.prefbase.cfg">
+<folderInfo id="0.250647335." name="/" resourcePath="">
+<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.1185920931" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
+<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.1185920931.1819283928" name=""/>
+<builder arguments="-C ${ProjDirPath}/build VERBOSE=true" command="make" id="org.eclipse.cdt.build.core.settings.default.builder.425794023" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
+<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1445630059" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
+<tool id="org.eclipse.cdt.build.core.settings.holder.1764610277" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
+<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1671812416" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+</tool>
+<tool id="org.eclipse.cdt.build.core.settings.holder.1195149217" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
+<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.747334269" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+</tool>
+<tool id="org.eclipse.cdt.build.core.settings.holder.1906811302" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
+<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.537988677" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+</tool>
+</toolChain>
+</folderInfo>
+</configuration>
+</storageModule>
+<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"/>
+<storageModule moduleId="scannerConfiguration">
+<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="makefileGenerator">
+<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/${specs_file}&quot;'" command="sh" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-c 'g++ -E -P -v -dD &quot;${plugin_state_location}/specs.cpp&quot;'" command="sh" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/specs.c&quot;'" command="sh" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<scannerConfigBuildInfo instanceId="0.250647335">
+<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
+<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="makefileGenerator">
+<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/${specs_file}&quot;'" command="sh" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-c 'g++ -E -P -v -dD &quot;${plugin_state_location}/specs.cpp&quot;'" command="sh" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
+<buildOutputProvider>
+<openAction enabled="true" filePath=""/>
+<parser enabled="true"/>
+</buildOutputProvider>
+<scannerInfoProvider id="specsFile">
+<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/specs.c&quot;'" command="sh" useDefault="true"/>
+<parser enabled="true"/>
+</scannerInfoProvider>
+</profile>
+</scannerConfigBuildInfo>
+</storageModule>
+<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
+<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
+</cconfiguration>
+</storageModule>
+<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+<project id="rtabmap-pkg.null.595292537" name="rtabmap-pkg"/>
+</storageModule>
+</cproject>

rtabmap/.project

@@ -0,0 +1,78 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>rtabmap-pkg</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
+			<triggers>clean,full,incremental,</triggers>
+			<arguments>
+				<dictionary>
+					<key>?name?</key>
+					<value></value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.append_environment</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.autoBuildTarget</key>
+					<value>all</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.buildArguments</key>
+					<value>-C ${ProjDirPath}/build VERBOSE=true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.buildCommand</key>
+					<value>make</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
+					<value>clean</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.contents</key>
+					<value>org.eclipse.cdt.make.core.activeConfigSettings</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableAutoBuild</key>
+					<value>false</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableCleanBuild</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableFullBuild</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.fullBuildTarget</key>
+					<value>all</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.stopOnError</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
+					<value>false</value>
+				</dictionary>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.core.ccnature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
+	</natures>
+</projectDescription>

rtabmap/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+project(rtabmap-pkg)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+# For CMake 2.6
+SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
+SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
+SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+rosbuild_genmsg()
+#uncomment if you have defined services
+rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+
+
+rosbuild_add_executable(camera_node src/CameraNode.cpp src/CameraWrapper.cpp)
+rosbuild_add_executable(camera_node_receiver src/CameraNodeReceiver.cpp)
+rosbuild_add_executable(camera_node_receiver_sm src/CameraNodeReceiverSM.cpp)
+rosbuild_add_executable(core_node src/CoreNode.cpp src/CoreWrapper.cpp)
+rosbuild_add_executable(input_node src/InputNode.cpp)
+rosbuild_add_executable(output_node src/OutputNode.cpp)
+rosbuild_add_executable(abtr_velocity_node src/AbtrVelocityNode.cpp)
+rosbuild_add_executable(image_to_sms_node src/ImageToSensoryMotorStateNode.cpp)
+FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui)
+IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
+    INCLUDE(${QT_USE_FILE})
+    rosbuild_add_executable(gui_node src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp)
+    target_link_libraries(gui_node ${QT_LIBRARIES} rtabmap_gui )
+ELSE()
+    MESSAGE(STATUS "[WARNING] Qt4 not found, the GUI node will not be compiled...")
+ENDIF()

rtabmap/Makefile

@@ -0,0 +1,2 @@
+include $(shell rospack find mk)/cmake.mk
+

rtabmap/Readme.txt

@@ -0,0 +1,8 @@
+
+Notes:
+
+Check memory leaks with ROS:
+  In the ROS launch file, add in the node tag : <node {...} launch-prefix="valgrind --tool=memcheck --leak-check=yes" {...}/>
+
+Eclipse issue with ROS lib:
+  Launch Eclipse with a script like the one in this folder "eclipse-launch.sh". This will setup ROS paths.

rtabmap/eclipse-launch.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+export GDK_NATIVE_WINDOWS=1
+
+## Source ROS setup.sh (adjust to your version : boxturtle, cturtle, diamondback, e...)
+source /opt/ros/diamondback/setup.sh
+
+## Setup ROS_PACKAGE_PATH
+export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/workspace/ros-pkg
+
+## Start eclipse
+~/eclipse/eclipse

rtabmap/launch/all_learning.launch

@@ -0,0 +1,12 @@
+<launch>
+
+	<!-- TELEOP -->
+	<include file="$(find rtabmap)/launch/teleop.launch"/>
+	
+	<!-- RTAB-MAP -->
+	<include file="$(find rtabmap)/launch/rtabmap_sm.launch"/>
+	
+	<!-- CAMERA -->
+	<include file="$(find rtabmap)/launch/camera.launch"/>
+
+</launch>

rtabmap/launch/all_tr_learning.launch

@@ -0,0 +1,30 @@
+<launch>
+
+	<!-- BRING UP TELEROBOT -->
+	<!-- BUS CAN MANAGER -->
+	<group ns="tr">
+        <param name="driver_type" value="TRCANDriver"/>
+        <node name="can_manager" type="can_manager" pkg="can_manager"
+                args="tr">
+        <remap from="/tr/tr/cmd_vel" to="/cmd_vel" />
+        </node>
+    </group>
+
+	<!-- TELEOP -->
+	<include file="$(find rtabmap)/launch/teleop.launch"/>
+	
+	<!-- RTAB-MAP -->
+	<include file="$(find rtabmap)/launch/rtabmap_sm.launch"/>
+	
+	<!-- CAMERA -->
+	<node pkg="omni_camera_capture" type="omni_publisher" 
+		name="omni_camera" respawn="false">
+		<param name="device" value="/dev/video0"/>
+		<param name="refresh_rate" value="2"/>
+		<param name="frame_id" value="omni_camera_link" />
+		<param name="image_encoding" value="RGB" /> <!-- GREY or RGB-->
+		<param name="image_size" value="FULL" /> <!-- PARTIAL or FULL -->
+		<param name="scale_divisor" value="1" /> <!-- 1, 2, 3, 4 or 6  in PARTIAL size-->
+	</node>
+
+</launch>

rtabmap/launch/camera.launch

@@ -0,0 +1,10 @@
+<launch>
+	<!-- Camera parameters -->
+	<param name="cam/device_id" value="0" type="int"/>
+	<param name="cam/image_rate" value="1" type="int"/>
+	<param name="cam/image_width" value="640" type="int"/>
+	<param name="cam/image_height" value="480" type="int"/>
+	
+	<!-- Nodes -->
+	<node name="camera" pkg="rtabmap" type="camera_node"/>
+</launch>

rtabmap/launch/rtabmap_lc.launch

@@ -0,0 +1,14 @@
+<launch>
+    <!-- RTAB-MAP LOOP CLOSURE DETECTION VERSION -->
+	<!-- Camera parameters -->
+	<param name="device_id" value="0" type="int"/>
+	<param name="image_rate" value="1" type="int"/>
+	<param name="image_width" value="640" type="int"/>
+	<param name="image_height" value="480" type="int"/>
+
+	<!-- Nodes -->
+	<node name="rtabmap_core" pkg="rtabmap" type="core_node"/>
+	<node name="rtabmap_gui" pkg="rtabmap" type="gui_node"/>
+	<node name="image_to_sms" pkg="rtabmap" type="image_to_sms_node"/>
+	<node name="camera" pkg="rtabmap" type="camera_node"/>
+</launch>

rtabmap/launch/rtabmap_sm.launch

@@ -0,0 +1,40 @@
+<launch>
+	<!-- RTAB-MAP SENSORY-MOTOR VERSION -->
+	<!-- WARNING : Database is automatically deleted on each startup -->
+	<!--           See "delete_db_on_start" option below... -->
+
+	<!-- image_to_sm parameters -->
+	<param name="resize_image_width" value="640" type="int"/>
+	<param name="resize_image_height" value="480" type="int"/>
+	
+	<!-- Nodes -->	
+	<!-- cmd_vel_a has priority on cmd_vel_b -->
+	<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity_node">
+		<remap from="cmd_vel_a" to="user/cmd_vel" />
+		<remap from="cmd_vel_b" to="rtabmap/cmd_vel" />
+		<remap from="cmd_vel" to="cmd_vel" />
+	</node>
+	
+	<node name="rtabmap_output" pkg="rtabmap" type="output_node">
+		<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
+		<remap from="rtabmap_info" to="rtabmap_info" />
+		<remap from="rtabmap_info_x" to="rtabmap_info_x" />
+	</node>
+		
+	<node name="rtabmap_core" pkg="rtabmap" type="core_node" output="screen" args="--delete_db_on_start">
+		<remap from="sm_state" to="sm_state" />
+		<remap from="rtabmap_info" to="rtabmap_info" />
+		<remap from="rtabmap_info_x" to="rtabmap_info_x" />
+	</node>
+
+	<node name="rtabmap_input" pkg="rtabmap" type="input_node">
+		<remap from="cmd_vel" to="cmd_vel" />
+		<remap from="sm_state" to="sm_state" />
+		<remap from="camera_data" to="camera_data" />
+	</node>
+	
+	<node name="image_to_sms" pkg="rtabmap" type="image_to_sms_node">
+		<remap from="image" to="image" />
+		<remap from="sm_state" to="camera_data" />
+	</node>
+</launch>

rtabmap/launch/teleop.launch

@@ -0,0 +1,9 @@
+<launch>	
+	<!-- TELEOP JOYSTICK -->
+	<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
+		<remap from="cmd_vel" to="user/cmd_vel" />
+	</node>
+	<node name="joystick" pkg="joy" type="joy_node">
+		<param name="autorepeat_rate" value="20" type="double" />
+	</node>
+</launch>

rtabmap/launch/testCamera.launch

@@ -0,0 +1,11 @@
+<launch>
+	<!-- Camera parameters -->
+	<param name="cam/device_id" value="0" type="int"/>
+	<param name="cam/image_rate" value="10" type="int"/>
+	<param name="cam/image_width" value="640" type="int"/>
+	<param name="cam/image_height" value="480" type="int"/>
+	
+	<!-- Nodes -->
+	<node name="cameraReceiver" pkg="rtabmap" type="camera_node_receiver" output="screen"/>
+	<node name="camera" pkg="rtabmap" type="camera_node"/>
+</launch>

rtabmap/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b avpd is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

rtabmap/manifest.xml

@@ -0,0 +1,22 @@
+<package>
+  <description brief="rtabmap">
+
+     RTAB-Map - Real-Time Appearance-Based Mapping
+
+  </description>
+  <author>Mathieu Labbe</author>
+  <license>GPL</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://rtabmap-ros-pkg.googlecode.com</url> 
+  <depend package="std_msgs"/>
+  <depend package="roscpp"/>
+  <depend package="cv_bridge"/>
+  <depend package="sensor_msgs"/>
+  <depend package="std_srvs"/>
+  <depend package="opencv2"/>
+  <depend package="rtabmap_lib"/>
+  <depend package="turtlesim"/>
+
+</package>
+
+

rtabmap/msg/KeyPoint.msg

@@ -0,0 +1,17 @@
+#class cv::KeyPoint
+#{
+#    Point2f pt;
+#    float size;
+#    float angle;
+#    float response;
+#    int octave;
+#    int class_id;
+#}
+
+float32 ptx
+float32 pty
+float32 size
+float32 angle
+float32 response
+int32 octave
+int32 class_id

rtabmap/msg/RtabmapInfo.msg

@@ -0,0 +1,13 @@
+#
+#
+#
+#
+#
+
+Header header
+
+int32 refId
+int32 loopClosureId
+
+uint32 actuatorStep
+float32[] actuators

rtabmap/msg/RtabmapInfoEx.msg

@@ -0,0 +1,39 @@
+#
+#
+#
+#
+#
+
+Header header
+
+rtabmap/RtabmapInfo info
+
+sensor_msgs/CompressedImage refImage
+int32 refChild
+
+sensor_msgs/CompressedImage loopClosureImage
+
+#std::map<int, float> posterior;
+int32[] posteriorKeys
+float32[] posteriorValues
+
+#std::map<int, float> likelihood;
+int32[] likelihoodKeys
+float32[] likelihoodValues
+
+#std::map<int, int> weights;
+int32[] weightsKeys
+int32[] weightsValues
+
+#std::map<std::string, float> stats
+string[] statsKeys
+float32[] statsValues
+
+##Keypoints##
+#std::multimap<int, cv::KeyPoint> refWords
+int32[] refWordsKeys
+rtabmap/KeyPoint[] refWordsValues
+
+#std::multimap<int, cv::KeyPoint> loopWords
+int32[] loopWordsKeys
+rtabmap/KeyPoint[] loopWordsValues

rtabmap/msg/SensoryMotorState.msg

@@ -0,0 +1,14 @@
+
+# Wrap the rtabmap::SMState class
+
+Header header
+
+uint32 sensorStep
+float32[] sensors
+
+uint32 actuatorStep
+float32[] actuators
+
+# Optional fields
+sensor_msgs/Image image
+rtabmap/KeyPoint[] keypoints

rtabmap/src/AbtrVelocityNode.cpp

@@ -0,0 +1,94 @@
+/*
+ * CameraNode.cpp
+ *
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <utilite/UMutex.h>
+
+UMutex commandMutex;
+std::vector<float> commandsA;
+std::vector<float> commandsB;
+int commandSize = 0;
+int commandIndex = 0;
+ros::Publisher rosPublisher;
+
+void velocityAReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
+{
+	//ROS_INFO("Received command velocity A (%f,%f)", msg->linear, msg->angular);
+	commandMutex.lock();
+	{
+		commandsA.push_back(msg->linear.x);
+		commandsA.push_back(msg->linear.y);
+		commandsA.push_back(msg->linear.z);
+		commandsA.push_back(msg->angular.x);
+		commandsA.push_back(msg->angular.y);
+		commandsA.push_back(msg->angular.z);
+	}
+	commandMutex.unlock();
+}
+
+void velocityBReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
+{
+	//ROS_INFO("Received command velocity B (%f,%f)", msg->linear, msg->angular);
+	commandMutex.lock();
+	{
+		commandsB.push_back(msg->linear.x);
+		commandsB.push_back(msg->linear.y);
+		commandsB.push_back(msg->linear.z);
+		commandsB.push_back(msg->angular.x);
+		commandsB.push_back(msg->angular.y);
+		commandsB.push_back(msg->angular.z);
+	}
+	commandMutex.unlock();
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "input_node");
+	ros::NodeHandle n;
+	ros::Subscriber velATopic;
+	ros::Subscriber velBTopic;
+	velATopic = n.subscribe("cmd_vel_a", 1, velocityAReceivedCallback);
+	velBTopic = n.subscribe("cmd_vel_b", 1, velocityBReceivedCallback);
+	rosPublisher = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+
+	ros::Rate loop_rate(10); // 10 Hz
+	while(ros::ok())
+	{
+		commandMutex.lock();
+		{
+			// priority for commandsA
+			if(commandsA.size())
+			{
+				geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
+				vel->linear.x = commandsA[0];
+				vel->linear.y = commandsA[1];
+				vel->linear.z = commandsA[2];
+				vel->angular.x = commandsA[3];
+				vel->angular.y = commandsA[4];
+				vel->angular.z = commandsA[5];
+				rosPublisher.publish(vel);
+			}
+			else if(commandsB.size())
+			{
+				geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
+				vel->linear.x = commandsB[0];
+				vel->linear.y = commandsB[1];
+				vel->linear.z = commandsB[2];
+				vel->angular.x = commandsB[3];
+				vel->angular.y = commandsB[4];
+				vel->angular.z = commandsB[5];
+				rosPublisher.publish(vel);
+			}
+			commandsA.clear();
+			commandsB.clear();
+		}
+		commandMutex.unlock();
+
+		ros::spinOnce();
+		loop_rate.sleep();
+	}
+}

rtabmap/src/CameraNode.cpp

@@ -0,0 +1,64 @@
+/*
+ * CameraNode.cpp
+ *
+ *  Created on: 1 févr. 2010
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include "CameraWrapper.h"
+#include <rtabmap/core/Camera.h>
+#include <utilite/ULogger.h>
+
+// See the launch file to change the camera values
+#define DEFAULT_DEVICE_ID 0
+#define DEFAULT_IMG_RATE 1 //Hz
+#define DEFAULT_AUTO_RESTART 0
+#define DEFAULT_IMG_WIDTH 0
+#define DEFAULT_IMG_HEIGHT 0
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "camera_node");
+	CameraWrapper * camera = 0;
+	ros::NodeHandle nh;
+
+	int deviceId = DEFAULT_DEVICE_ID;
+	int imgRate = DEFAULT_IMG_RATE;
+	int autoRestart = DEFAULT_AUTO_RESTART;
+	int imgWidth = DEFAULT_IMG_WIDTH;
+	int imgHeight = DEFAULT_IMG_HEIGHT;
+
+	nh.param("cam/device_id", deviceId, deviceId);
+	nh.param("cam/image_rate", imgRate, imgRate);
+	nh.param("cam/auto_restart", autoRestart, autoRestart);
+	nh.param("cam/image_width", imgWidth, imgWidth);
+	nh.param("cam/image_height", imgHeight, imgHeight);
+
+	ROS_INFO("cam/device_id=%d", deviceId);
+	ROS_INFO("cam/image_rate=%d", imgRate);
+	ROS_INFO("cam/auto_restart=%d", autoRestart);
+	ROS_INFO("cam/image_width=%d", imgWidth);
+	ROS_INFO("cam/image_height=%d", imgHeight);
+
+	camera = new CameraVideoWrapper(deviceId, imgRate, autoRestart, imgWidth, imgHeight); // webcam device 0
+
+	if(!camera || (camera && !camera->init()))
+	{
+		ROS_ERROR("Cannot initiate the camera");
+	}
+	else
+	{
+		// Start the camera
+		camera->start();
+		ROS_INFO("Camera started...");
+		ros::spin();
+	}
+
+	//cleanup
+	if(camera)
+	{
+		delete camera;
+	}
+}

rtabmap/src/CameraNodeReceiver.cpp

@@ -0,0 +1,52 @@
+/*
+ * CameraNodeReceiver.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+//#include <sensor_msgs/CompressedImage.h>
+#include <cv_bridge/CvBridge.h>
+#include <highgui.h>
+
+void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
+{
+	// Decompress
+	//const CvMat compressed = cvMat(1, image->data.size(), CV_8UC1, const_cast<unsigned char*>(&image->data[0]));
+	//IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
+
+	//ROS_INFO("Received an image size=(%d,%d)", decompressed->width, decompressed->height);
+	//cvShowImage( "ImageReceived", decompressed );
+	//cvReleaseImage(&decompressed);
+
+	IplImage * image = 0;
+	sensor_msgs::CvBridge bridge;
+	if(msg->data.size())
+	{
+		image = bridge.imgMsgToCv(msg);
+	}
+
+	if(image)
+	{
+		ROS_INFO("Received an image size=(%d,%d)", image->width, image->height);
+		cvShowImage( "ImageReceived", image);
+	}
+}
+
+int main(int argc, char** argv)
+{
+	cvStartWindowThread();
+	cvNamedWindow("ImageReceived", CV_WINDOW_AUTOSIZE);
+	cvMoveWindow("ImageReceived", 100, 100); // offset from the UL corner of the screen
+
+	ros::init(argc, argv, "camera_node_receiver");
+	ros::NodeHandle n;
+	ros::Subscriber image_sub = n.subscribe("image", 1, imgReceivedCallback);
+
+	ROS_INFO("Waiting for images...");
+
+	ros::spin();
+
+	cvDestroyWindow("ImageReceived");
+}

rtabmap/src/CameraNodeReceiverSM.cpp

@@ -0,0 +1,45 @@
+/*
+ * CameraNodeReceiver.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include "rtabmap/SensoryMotorState.h"
+#include <cv_bridge/CvBridge.h>
+#include <highgui.h>
+
+void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
+{
+	IplImage * image = 0;
+	sensor_msgs::CvBridge bridge;
+	if( msg->image.data.size())
+	{
+		bridge.fromImage(msg->image);
+		image = bridge.toIpl();
+	}
+
+	if(image)
+	{
+		ROS_INFO("Received an image size=(%d,%d)", image->width, image->height);
+		cvShowImage( "ImageReceived", image);
+	}
+}
+
+int main(int argc, char** argv)
+{
+	cvStartWindowThread();
+	cvNamedWindow("ImageReceived", CV_WINDOW_AUTOSIZE);
+	cvMoveWindow("ImageReceived", 100, 100); // offset from the UL corner of the screen
+
+	ros::init(argc, argv, "camera_node_receiver_sm");
+	ros::NodeHandle n;
+	ros::Subscriber image_sub = n.subscribe("sm_state", 1, smReceivedCallback);
+
+	ROS_INFO("Waiting for Sensorimotor states (containing images)...");
+
+	ros::spin();
+
+	cvDestroyWindow("ImageReceived");
+}

rtabmap/src/CameraWrapper.cpp

@@ -0,0 +1,171 @@
+/*
+ * CameraWrapper.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "CameraWrapper.h"
+#include <cv_bridge/CvBridge.h>
+//#include <sensor_msgs/CompressedImage.h>
+#include <sensor_msgs/Image.h>
+#include <rtabmap/core/CameraEvent.h>
+#include <rtabmap/core/SMState.h>
+#include <utilite/UEventsManager.h>
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/Parameters.h>
+
+//msgs
+#include "rtabmap/SensoryMotorState.h"
+
+CameraWrapper::CameraWrapper() :
+	camera_(0)
+{
+	rosPublisher_ = nh_.advertise<sensor_msgs::Image>("image", 1);
+	changeCameraImgRateSrv_ = nh_.advertiseService("changeCameraImgRate", &CameraWrapper::changeCameraImgRateCallback, this);
+	UEventsManager::addHandler(this);
+}
+
+CameraWrapper::~CameraWrapper()
+{
+}
+
+void CameraWrapper::setCamera(rtabmap::Camera * camera)
+{
+	if(camera)
+	{
+		if(camera_)
+		{
+			delete camera_;
+			camera_ = 0;
+		}
+
+		camera_ = camera;
+	}
+}
+
+// ownership is transferred
+void CameraWrapper::setPostThreatement(rtabmap::CamPostTreatment * strategy)
+{
+	if(camera_ && strategy)
+	{
+		camera_->setPostThreatement(strategy);
+	}
+}
+
+bool CameraWrapper::init()
+{
+	if(camera_)
+	{
+		return camera_->init();
+	}
+	return false;
+}
+
+void CameraWrapper::start()
+{
+	if(camera_)
+	{
+		return camera_->start();
+	}
+}
+
+bool CameraWrapper::changeCameraImgRateCallback(rtabmap::ChangeCameraImgRate::Request & request, rtabmap::ChangeCameraImgRate::Response & response)
+{
+	nh_.setParam("image_rate", request.imgRate);
+	nh_.setParam("auto_restart", request.autoRestart);
+	UEventsManager::post(new rtabmap::CameraEvent(rtabmap::CameraEvent::kCmdChangeParam, request.imgRate, request.autoRestart));
+	return true;
+}
+
+void CameraWrapper::handleEvent(UEvent* anEvent)
+{
+	if(anEvent->getClassName().compare("SMStateEvent") == 0)
+	{
+		rtabmap::SMStateEvent * e = (rtabmap::SMStateEvent*)anEvent;
+		const rtabmap::SMState * smState = e->getSMState();
+		if(smState && smState->getImage())
+		{
+			try
+			{
+				//int params[3] = {0};
+
+				//JPEG compression
+				//std::string format = "jpeg";
+				//params[0] = CV_IMWRITE_JPEG_QUALITY;
+				//params[1] = 80; // default: 80% quality
+
+				//PNG compression
+				//std::string format = "png";
+				//params[0] = CV_IMWRITE_PNG_COMPRESSION;
+				//params[1] = 9; // default: maximum compression
+
+				//std::string extension = '.' + format;
+
+				// Compress image
+				//const IplImage* image = e->getImage();
+				//CvMat* buf = cvEncodeImage(extension.c_str(), image, params);
+
+				// Set up message and publish
+				//sensor_msgs::CompressedImage compressed;
+				//compressed.format = format;
+				//compressed.data.resize(buf->width);
+				//memcpy(&compressed.data[0], buf->data.ptr, buf->width);
+				//cvReleaseMat(&buf);
+
+				//ROS_INFO("Publishing an image");
+				//rosPublisher_.publish(compressed);
+				sensor_msgs::ImagePtr msg = sensor_msgs::CvBridge::cvToImgMsg(smState->getImage());
+				rosPublisher_.publish(msg);
+			}
+			catch (sensor_msgs::CvBridgeException ex)
+			{
+				ROS_ERROR("%s", ex.what());
+			}
+		}
+	}
+}
+
+// Camera Image wrapper
+CameraImagesWrapper::CameraImagesWrapper(
+		const std::string & path,
+		int startAt,
+		bool refreshDir,
+		float imageRate,
+		bool autoRestart,
+		unsigned int imageWidth,
+		unsigned int imageHeight)
+{
+	this->setCamera(new rtabmap::CameraImages(path, startAt, refreshDir, imageRate, autoRestart, imageWidth, imageHeight));
+}
+
+// Camera Video wrapper
+CameraVideoWrapper::CameraVideoWrapper(int usbDevice,
+										float imageRate,
+										bool autoRestart,
+										unsigned int imageWidth,
+										unsigned int imageHeight)
+{
+	this->setCamera(new rtabmap::CameraVideo(usbDevice, imageRate, autoRestart, imageWidth, imageHeight));
+}
+
+
+CameraVideoWrapper::CameraVideoWrapper(const std::string & fileName,
+										float imageRate,
+										bool autoRestart,
+										unsigned int imageWidth,
+										unsigned int imageHeight)
+{
+	this->setCamera(new rtabmap::CameraVideo(fileName, imageRate, autoRestart, imageWidth, imageHeight));
+}
+
+// Camera database wrapper
+CameraDatabaseWrapper::CameraDatabaseWrapper(const std::string & path,
+		bool ignoreChildren,
+		float imageRate,
+		bool autoRestart,
+		unsigned int imageWidth,
+		unsigned int imageHeight)
+{
+	this->setCamera(new rtabmap::CameraDatabase(path, ignoreChildren, imageRate, autoRestart, imageWidth, imageHeight));
+}

rtabmap/src/CameraWrapper.h

@@ -0,0 +1,106 @@
+/*
+ * CameraWrapper.h
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#ifndef CAMERAWRAPPER_H_
+#define CAMERAWRAPPER_H_
+
+#include "utilite/UEventsHandler.h"
+#include <ros/ros.h>
+#include "rtabmap/ChangeCameraImgRate.h"
+#include <std_msgs/Empty.h>
+
+namespace Util
+{
+	class Event;
+}
+
+namespace rtabmap
+{
+	class Camera;
+	class CamPostTreatment;
+}
+
+class CameraWrapper : public UEventsHandler
+{
+public:
+	virtual ~CameraWrapper();
+
+	bool init();
+	void start();
+
+	void setPostThreatement(rtabmap::CamPostTreatment * strategy); // ownership is transferred
+	void updateParameters();
+
+protected:
+	CameraWrapper();
+	virtual void handleEvent(UEvent * anEvent);
+	void setCamera(rtabmap::Camera * camera);
+
+private:
+	bool changeCameraImgRateCallback(rtabmap::ChangeCameraImgRate::Request&, rtabmap::ChangeCameraImgRate::Response&);
+
+private:
+	ros::NodeHandle nh_;
+	ros::Publisher rosPublisher_;
+	rtabmap::Camera * camera_;
+	ros::ServiceServer changeCameraImgRateSrv_;
+};
+
+
+// Specialized wrappers
+
+//Image
+class CameraImagesWrapper : public CameraWrapper
+{
+public:
+	CameraImagesWrapper(const std::string & path,
+						int startAt = 1,
+						bool refreshDir = false,
+						float imageRate = 0,
+						bool autoRestart = false,
+						unsigned int imageWidth = 0,
+						unsigned int imageHeight = 0);
+	virtual ~CameraImagesWrapper() {}
+};
+
+// Video
+class CameraVideoWrapper : public CameraWrapper
+{
+public:
+	// Usb device like a Webcam
+	CameraVideoWrapper(int usbDevice = 0,
+						float imageRate = 0,
+						bool autoRestart = false,
+						unsigned int imageWidth = 0,
+						unsigned int imageHeight = 0);
+
+	// for a video file (AVI)
+	CameraVideoWrapper(const std::string & fileName,
+						float imageRate = 0,
+						bool autoRestart = false,
+						unsigned int imageWidth = 0,
+						unsigned int imageHeight = 0);
+
+	virtual ~CameraVideoWrapper() {}
+};
+
+// Database
+class CameraDatabaseWrapper : public CameraWrapper
+{
+public:
+	// Usb device like a Webcam
+	CameraDatabaseWrapper(const std::string & path,
+			bool ignoreChildren,
+			float imageRate = 0,
+			bool autoRestart = false,
+			unsigned int imageWidth = 0,
+			unsigned int imageHeight = 0);
+
+	virtual ~CameraDatabaseWrapper() {}
+};
+
+#endif /* CAMERAWRAPPER_H_ */

rtabmap/src/CoreNode.cpp

@@ -0,0 +1,35 @@
+/*
+ * CoreNode.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "CoreWrapper.h"
+#include "utilite/ULogger.h"
+
+int main(int argc, char** argv)
+{
+	ROS_INFO("Starting node...");
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ros::init(argc, argv, "core_node");
+	const char* opt_delete_db_on_start    = "--delete_db_on_start";
+
+	bool deleteDbOnStart = false;
+	for(int i=1;i<argc;i++)
+	{
+		if(!strncmp(argv[i], opt_delete_db_on_start, strlen(opt_delete_db_on_start)))
+		{
+			deleteDbOnStart = true;
+		}
+	}
+
+	CoreWrapper rtabmap(deleteDbOnStart);
+	rtabmap.start();
+
+	ROS_INFO("RTAB-Map started...");
+	ros::spin();
+}

rtabmap/src/CoreWrapper.cpp

@@ -0,0 +1,330 @@
+/*
+ * CoreWrapper.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "CoreWrapper.h"
+#include <rtabmap/core/CameraEvent.h>
+#include <ros/ros.h>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/SMState.h>
+#include "utilite/UtiLite.h"
+#include <highgui.h>
+
+//msgs
+#include "rtabmap/RtabmapInfo.h"
+#include "rtabmap/RtabmapInfoEx.h"
+
+using namespace rtabmap;
+
+CoreWrapper::CoreWrapper(bool deleteDbOnStart) : rtabmap_(0)
+{
+	infoPub_ = nh_.advertise<rtabmap::RtabmapInfo>("rtabmap_info", 1);
+	infoExPub_ = nh_.advertise<rtabmap::RtabmapInfoEx>("rtabmap_info_x", 1);
+	parametersLoadedPub_ = nh_.advertise<std_msgs::Empty>("parameters_loaded", 1);
+
+	rtabmap_ = new Rtabmap();
+	loadNodeParameters(rtabmap_->getIniFilePath());
+
+	if(deleteDbOnStart)
+	{
+		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDeleteMemory));
+	}
+
+	rtabmap_->init();
+
+	imageTopic_ = nh_.subscribe("sm_state", 1, &CoreWrapper::smReceivedCallback, this);
+	parametersUpdatedTopic_ = nh_.subscribe("parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
+
+	resetMemorySrv_ = nh_.advertiseService("resetMemory", &CoreWrapper::resetMemoryCallback, this);
+	dumpMemorySrv_ = nh_.advertiseService("dumpMemory", &CoreWrapper::dumpMemoryCallback, this);
+	deleteMemorySrv_ = nh_.advertiseService("deleteMemory", &CoreWrapper::deleteMemoryCallback, this);
+	dumpPredictionSrv_ = nh_.advertiseService("dumpPrediction", &CoreWrapper::dumpPredictionCallback, this);
+	UEventsManager::addHandler(this);
+}
+
+CoreWrapper::~CoreWrapper()
+{
+	this->saveNodeParameters(rtabmap_->getIniFilePath());
+	delete rtabmap_;
+}
+
+void CoreWrapper::start()
+{
+	rtabmap_->start();
+}
+
+void CoreWrapper::loadNodeParameters(const std::string & configFile)
+{
+	ROS_INFO("Loading parameters from %s", configFile.c_str());
+	if(!UFile::exists(configFile.c_str()))
+	{
+		ROS_WARN("Config file doesn't exist!");
+	}
+
+	ParametersMap parameters = Parameters::getDefaultParameters();
+	Rtabmap::readParameters(configFile.c_str(), parameters);
+
+	for(ParametersMap::const_iterator i=parameters.begin(); i!=parameters.end(); ++i)
+	{
+		nh_.setParam(i->first, i->second);
+	}
+	parametersLoadedPub_.publish(std_msgs::Empty());
+}
+
+void CoreWrapper::saveNodeParameters(const std::string & configFile)
+{
+	ROS_INFO("Saving parameters to %s", configFile.c_str());
+
+	if(!UFile::exists(configFile.c_str()))
+	{
+		ROS_WARN("Config file doesn't exist, a new one will be created.");
+	}
+
+	ParametersMap parameters = Parameters::getDefaultParameters();
+	for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string value;
+		if(nh_.getParam(iter->first,value))
+		{
+			iter->second = value;
+		}
+	}
+
+	Rtabmap::writeParameters(configFile.c_str(), parameters);
+
+	std::string databasePath = parameters.at(Parameters::kRtabmapWorkingDirectory())+"/LTM.db";
+	ROS_INFO("Database/long-term memory (%lu MB) is located at %s/LTM.db", UFile::length(databasePath)/1000000, databasePath.c_str());
+}
+
+
+void CoreWrapper::smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
+{
+	IplImage * image = 0;
+	std::list<cv::KeyPoint> keypoints;
+
+	if(msg->image.data.size())
+	{
+		sensor_msgs::CvBridge bridge;
+		bridge.fromImage(msg->image);
+		image = cvCloneImage(bridge.toIpl());
+	}
+
+	for(unsigned int i=0; i<msg->keypoints.size() && i<msg->keypoints.size(); i++)
+	{
+		cv::KeyPoint pt;
+		pt.angle = msg->keypoints.at(i).angle;
+		pt.response = msg->keypoints.at(i).response;
+		pt.pt.x = msg->keypoints.at(i).ptx;
+		pt.pt.y = msg->keypoints.at(i).pty;
+		pt.size = msg->keypoints.at(i).size;
+		keypoints.push_back(pt);
+	}
+	rtabmap::SMState * smState = new rtabmap::SMState(msg->sensors, msg->sensorStep, msg->actuators, msg->actuatorStep);
+	smState->setImage(image);
+	smState->setKeypoints(keypoints);
+	UEventsManager::post(new SMStateEvent(smState));
+}
+
+bool CoreWrapper::resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResetMemory));
+	return true;
+}
+
+bool CoreWrapper::dumpMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory));
+	return true;
+}
+
+bool CoreWrapper::deleteMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDeleteMemory));
+	return true;
+}
+
+bool CoreWrapper::dumpPredictionCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpPrediction));
+	return true;
+}
+
+void CoreWrapper::parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg)
+{
+	rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+	for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string value;
+		if(nh_.getParam(iter->first, value))
+		{
+			iter->second = value;
+		}
+	}
+	ROS_INFO("Updating parameters");
+	UEventsManager::post(new ParamEvent(parameters));
+}
+
+void CoreWrapper::handleEvent(UEvent * anEvent)
+{
+	if(anEvent->getClassName().compare("RtabmapEvent") == 0)
+	{
+		RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
+		const Statistics & stat = rtabmapEvent->getStats();
+
+		//prepare ros message
+
+		if(stat.extended())
+		{
+			rtabmap::RtabmapInfoExPtr msg(new rtabmap::RtabmapInfoEx);
+			msg->info.refId = stat.refImageId();
+			if(stat.refImage())
+			{
+				int params[3] = {0};
+
+				//JPEG compression
+				std::string format = "jpeg";
+				params[0] = CV_IMWRITE_JPEG_QUALITY;
+				params[1] = 80; // default: 80% quality
+
+				//PNG compression
+				//std::string format = "png";
+				//params[0] = CV_IMWRITE_PNG_COMPRESSION;
+				//params[1] = 9; // default: maximum compression
+
+				std::string extension = '.' + format;
+
+				// Compress image
+				const IplImage* image = stat.refImage();
+				CvMat* buf = cvEncodeImage(extension.c_str(), image, params);
+
+				// Set up message and publish
+				sensor_msgs::CompressedImage compressed;
+				compressed.format = format;
+				compressed.data.resize(buf->width);
+				memcpy(&compressed.data[0], buf->data.ptr, buf->width);
+				cvReleaseMat(&buf);
+
+				msg->refImage = compressed;
+			}
+			msg->info.loopClosureId = stat.loopClosureId();
+			if(stat.loopClosureImage())
+			{
+				int params[3] = {0};
+
+				//JPEG compression
+				std::string format = "jpeg";
+				params[0] = CV_IMWRITE_JPEG_QUALITY;
+				params[1] = 80; // default: 80% quality
+
+				//PNG compression
+				//std::string format = "png";
+				//params[0] = CV_IMWRITE_PNG_COMPRESSION;
+				//params[1] = 9; // default: maximum compression
+
+				std::string extension = '.' + format;
+
+				// Compress image
+				const IplImage* image = stat.loopClosureImage();
+				CvMat* buf = cvEncodeImage(extension.c_str(), image, params);
+
+				// Set up message and publish
+				sensor_msgs::CompressedImage compressed;
+				compressed.format = format;
+				compressed.data.resize(buf->width);
+				memcpy(&compressed.data[0], buf->data.ptr, buf->width);
+				cvReleaseMat(&buf);
+
+				msg->loopClosureImage = compressed;
+			}
+
+			const std::list<std::vector<float> > & actuators = stat.getActions();
+			if(actuators.size())
+			{
+				msg->info.actuatorStep = actuators.front().size();
+			}
+			for(std::list<std::vector<float> >::const_iterator iter=actuators.begin();iter!=actuators.end();++iter)
+			{
+				if((iter->size() == 0 && msg->info.actuatorStep > 0) || msg->info.actuatorStep % iter->size() != 0)
+				{
+					ROS_ERROR("Actuators must have all the same length.");
+				}
+				msg->info.actuators.insert(msg->info.actuators.end(), iter->begin(), iter->end());
+			}
+
+			//Posterior, likelihood, childCount
+			msg->posteriorKeys = uKeys(stat.posterior());
+			msg->posteriorValues = uValues(stat.posterior());
+			msg->likelihoodKeys = uKeys(stat.likelihood());
+			msg->likelihoodValues = uValues(stat.likelihood());
+			msg->weightsKeys = uKeys(stat.weights());
+			msg->weightsValues = uValues(stat.weights());
+
+			//SURF stuff...
+			msg->refWordsKeys = uListToVector(uKeys(stat.refWords()));
+			msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
+			int index = 0;
+			for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
+				i!=stat.refWords().end();
+				++i)
+			{
+				msg->refWordsValues.at(index).angle = i->second.angle;
+				msg->refWordsValues.at(index).response = i->second.response;
+				msg->refWordsValues.at(index).ptx = i->second.pt.x;
+				msg->refWordsValues.at(index).pty = i->second.pt.y;
+				msg->refWordsValues.at(index).size = i->second.size;
+				msg->refWordsValues.at(index).octave = i->second.octave;
+				msg->refWordsValues.at(index).class_id = i->second.class_id;
+				++index;
+			}
+			msg->loopWordsKeys = uListToVector(uKeys(stat.loopWords()));
+			msg->loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
+			index = 0;
+			for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
+				i!=stat.loopWords().end();
+				++i)
+			{
+				msg->loopWordsValues.at(index).angle = i->second.angle;
+				msg->loopWordsValues.at(index).response = i->second.response;
+				msg->loopWordsValues.at(index).ptx = i->second.pt.x;
+				msg->loopWordsValues.at(index).pty = i->second.pt.y;
+				msg->loopWordsValues.at(index).size = i->second.size;
+				msg->loopWordsValues.at(index).octave = i->second.octave;
+				msg->loopWordsValues.at(index).class_id = i->second.class_id;
+				++index;
+			}
+
+			// Statistics data
+			msg->statsKeys = uKeys(stat.data());
+			msg->statsValues = uValues(stat.data());
+
+			ROS_INFO("Publishing statistics...");
+			infoExPub_.publish(msg);
+		}
+		else
+		{
+			rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo);
+			ROS_INFO("Loop closure detected! newId=%d with oldId=%d", stat.refImageId(), stat.loopClosureId());
+			msg->refId = stat.refImageId();
+			msg->loopClosureId = stat.loopClosureId();
+			const std::list<std::vector<float> > & actuators = stat.getActions();
+			if(actuators.size())
+			{
+				msg->actuatorStep = actuators.front().size();
+			}
+			for(std::list<std::vector<float> >::const_iterator iter=actuators.begin();iter!=actuators.end();++iter)
+			{
+				if((iter->size() == 0 && msg->actuatorStep > 0) || msg->actuatorStep % iter->size() != 0)
+				{
+					ROS_ERROR("Actuators must have all the same length.");
+				}
+				msg->actuators.insert(msg->actuators.end(), iter->begin(), iter->end());
+			}
+			infoPub_.publish(msg);
+		}
+	}
+}

rtabmap/src/CoreWrapper.h

@@ -0,0 +1,65 @@
+/*
+ * CoreWrapper.h
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#ifndef COREWRAPPER_H_
+#define COREWRAPPER_H_
+
+
+#include <ros/ros.h>
+#include <std_srvs/Empty.h>
+#include <std_msgs/Empty.h>
+#include <cv_bridge/CvBridge.h>
+#include "utilite/UEventsHandler.h"
+#include <rtabmap/core/RtabmapEvent.h>
+#include <sensor_msgs/Image.h>
+#include "rtabmap/SensoryMotorState.h"
+
+namespace rtabmap
+{
+	class Rtabmap;
+}
+
+class CoreWrapper : public UEventsHandler
+{
+public:
+	CoreWrapper(bool deleteDbOnStart = false);
+	virtual ~CoreWrapper();
+
+	void start();
+
+private:
+	void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg);
+	void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg);
+
+	bool resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool dumpMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool dumpPredictionCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool deleteMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+
+	void loadNodeParameters(const std::string & configFile);
+	void saveNodeParameters(const std::string & configFile);
+
+	virtual void handleEvent(UEvent * anEvent);
+
+private:
+	ros::NodeHandle nh_;
+	rtabmap::Rtabmap * rtabmap_;
+	ros::Subscriber imageTopic_;
+	ros::Subscriber compressedImageTopic_;
+	ros::Subscriber parametersUpdatedTopic_;
+	ros::Publisher infoPub_;
+	ros::Publisher infoExPub_;
+	ros::Publisher parametersLoadedPub_;
+	std::string configFile_;
+
+	ros::ServiceServer resetMemorySrv_;
+	ros::ServiceServer dumpMemorySrv_;
+	ros::ServiceServer deleteMemorySrv_;
+	ros::ServiceServer dumpPredictionSrv_;
+};
+
+#endif /* COREWRAPPER_H_ */

rtabmap/src/GuiNode.cpp

@@ -0,0 +1,32 @@
+/*
+ * CoreNode.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "GuiWrapper.h"
+#include "utilite/ULogger.h"
+
+#include <QApplication>
+#include <rtabmap/gui/MainWindow.h>
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "gui_node");
+
+	GuiWrapper gui(argc, argv);
+
+	// Here start the ROS events loop
+	ros::AsyncSpinner spinner(4); // Use 4 threads
+	spinner.start();
+
+	ROS_INFO("Node started.");
+	// Now wait for application to finish
+	int r = gui.exec();// MUST be called by the Main Thread
+
+	spinner.stop();
+
+	ROS_INFO("All done! Closing...");
+	return r;
+}

rtabmap/src/GuiWrapper.cpp

@@ -0,0 +1,231 @@
+/*
+ * GuiWrapper.cpp
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "GuiWrapper.h"
+#include <rtabmap/gui/MainWindow.h>
+#include "PreferencesDialogROS.h"
+#include <QtGui/QApplication>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <cv_bridge/CvBridge.h>
+#include "utilite/UEventsManager.h"
+#include "std_srvs/Empty.h"
+#include "std_msgs/Empty.h"
+#include <rtabmap/core/Parameters.h>
+#include <highgui.h>
+#include <rtabmap/core/CameraEvent.h>
+#include "rtabmap/ChangeCameraImgRate.h"
+
+using namespace rtabmap;
+
+GuiWrapper::GuiWrapper(int & argc, char** argv)
+{
+	infoTopic_ = nh_.subscribe("rtabmap_info", 1, &GuiWrapper::infoReceivedCallback, this);
+	infoExTopic_ = nh_.subscribe("rtabmap_info_x", 1, &GuiWrapper::infoExReceivedCallback, this);
+	app_ = new QApplication(argc, argv);
+	mainWindow_ = new MainWindow(new PreferencesDialogROS());
+	mainWindow_->show();
+	mainWindow_->changeState(MainWindow::kMonitoring);
+	app_->connect( app_, SIGNAL( lastWindowClosed() ), app_, SLOT( quit() ) );
+
+	resetMemoryClient_ = nh_.serviceClient<std_srvs::Empty>("resetMemory");
+	dumpMemoryClient_ = nh_.serviceClient<std_srvs::Empty>("dumpMemory");
+	dumpPredictionClient_ = nh_.serviceClient<std_srvs::Empty>("dumpPrediction");
+	deleteMemoryClient_ = nh_.serviceClient<std_srvs::Empty>("deleteMemory");
+	changeCameraImgRateClient_ = nh_.serviceClient<rtabmap::ChangeCameraImgRate>("changeCameraImgRate");
+
+	parametersUpdatedPub_ = nh_.advertise<std_msgs::Empty>("parameters_updated", 1);
+
+	UEventsManager::addHandler(this);
+	UEventsManager::addHandler(mainWindow_);
+}
+
+GuiWrapper::~GuiWrapper()
+{
+	delete mainWindow_;
+	delete app_;
+}
+
+int GuiWrapper::exec()
+{
+	return app_->exec();
+}
+
+void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
+{
+	ROS_INFO("Loop closure detected! newId=%d with oldId=%d", msg->refId, msg->loopClosureId);
+	rtabmap::Statistics * stat = new rtabmap::Statistics();
+	stat->setLoopClosureId(msg->loopClosureId);
+	UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
+}
+
+void GuiWrapper::infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
+{
+	ROS_INFO("Statistics received!");
+	sensor_msgs::CvBridge bridge;
+
+	// Map from ROS struct to rtabmap struct
+	rtabmap::Statistics * stat = new rtabmap::Statistics();
+
+	stat->setExtended(true); // Extended
+
+	stat->setRefImageId(msg->info.refId);
+	if(msg->refImage.data.size() > 0)
+	{
+		// Decompress
+		const CvMat compressed = cvMat(1, msg->refImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->refImage.data[0]));
+		IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
+		stat->setRefImage(&decompressed);
+	}
+	stat->setLoopClosureId(msg->info.loopClosureId);
+	if(msg->loopClosureImage.data.size() > 0)
+	{
+		// Decompress
+		const CvMat compressed = cvMat(1, msg->loopClosureImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->loopClosureImage.data[0]));
+		IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
+		stat->setLoopClosureImage(&decompressed);
+	}
+
+	//Posterior, likelihood, childCount
+	std::map<int, float> mapIntFloat;
+	for(unsigned int i=0; i<msg->posteriorKeys.size() && i<msg->posteriorValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(msg->posteriorKeys.at(i), msg->posteriorValues.at(i)));
+	}
+	stat->setPosterior(mapIntFloat);
+	mapIntFloat.clear();
+	for(unsigned int i=0; i<msg->likelihoodKeys.size() && i<msg->likelihoodValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(msg->likelihoodKeys.at(i), msg->likelihoodValues.at(i)));
+	}
+	stat->setLikelihood(mapIntFloat);
+	std::map<int, int> mapIntInt;
+	for(unsigned int i=0; i<msg->weightsKeys.size() && i<msg->weightsValues.size(); ++i)
+	{
+		mapIntInt.insert(std::pair<int, int>(msg->weightsKeys.at(i), msg->weightsValues.at(i)));
+	}
+	stat->setWeights(mapIntInt);
+
+	//SURF stuff...
+	std::multimap<int, cv::KeyPoint> mapIntKeypoint;
+	for(unsigned int i=0; i<msg->refWordsKeys.size() && i<msg->refWordsValues.size(); i++)
+	{
+		cv::KeyPoint pt;
+		pt.angle = msg->refWordsValues.at(i).angle;
+		pt.response = msg->refWordsValues.at(i).response;
+		//pt.laplacian = msg->refWordsValues.at(i).laplacian;
+		pt.pt.x = msg->refWordsValues.at(i).ptx;
+		pt.pt.y = msg->refWordsValues.at(i).pty;
+		pt.size = msg->refWordsValues.at(i).size;
+		mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->refWordsKeys.at(i), pt));
+	}
+	stat->setRefWords(mapIntKeypoint);
+	mapIntKeypoint.clear();
+	for(unsigned int i=0; i<msg->loopWordsKeys.size() && i<msg->loopWordsValues.size(); i++)
+	{
+		cv::KeyPoint pt;
+		pt.angle = msg->loopWordsValues.at(i).angle;
+		pt.response = msg->loopWordsValues.at(i).response;
+		//pt.laplacian = msg->loopWordsValues.at(i).laplacian;
+		pt.pt.x = msg->loopWordsValues.at(i).ptx;
+		pt.pt.y = msg->loopWordsValues.at(i).pty;
+		pt.size = msg->loopWordsValues.at(i).size;
+		mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->loopWordsKeys.at(i), pt));
+	}
+	stat->setLoopWords(mapIntKeypoint);
+
+	// Statistics data
+	for(unsigned int i=0; i<msg->statsKeys.size() && i<msg->statsValues.size(); i++)
+	{
+		stat->addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i));
+	}
+
+	ROS_INFO("Publishing statistics...");
+	UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
+}
+
+void GuiWrapper::handleEvent(UEvent * anEvent)
+{
+	if(anEvent->getClassName().compare("CameraEvent") == 0)
+	{
+		rtabmap::CameraEvent * camEvent = (rtabmap::CameraEvent *)anEvent;
+		if(camEvent->getCommand() == rtabmap::CameraEvent::kCmdChangeParam)
+		{
+			rtabmap::ChangeCameraImgRate srv;
+			srv.request.imgRate = camEvent->getImageRate();
+			srv.request.autoRestart = camEvent->getAutoRestart();
+			if(!changeCameraImgRateClient_.call(srv))
+			{
+				ROS_WARN("Can't call \"changeCameraImgRate\" service. Ignore this warning if the rtabmap/camera_node is not used...");
+			}
+		}
+		else
+		{
+			ROS_WARN("Only ChangeImgRate command for the camera is supported yet...");
+		}
+	}
+	else if(anEvent->getClassName().compare("ParamEvent") == 0)
+	{
+		const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
+		rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
+		bool modified = false;
+		for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
+		{
+			//save only parameters with valid names
+			if(defaultParameters.find((*i).first) != defaultParameters.end())
+			{
+				nh_.setParam((*i).first, (*i).second);
+				modified = true;
+			}
+			else if((*i).first.find('/') != (*i).first.npos)
+			{
+				ROS_WARN("Parameter %s is not used by the rtabmap node.", (*i).first.c_str());
+			}
+		}
+		if(modified)
+		{
+			ROS_INFO("Parameters updated");
+			parametersUpdatedPub_.publish(std_msgs::Empty());
+		}
+	}
+	else if(anEvent->getClassName().compare("RtabmapEventCmd") == 0)
+	{
+		std_srvs::Empty srv;
+		rtabmap::RtabmapEventCmd::Cmd cmd = ((rtabmap::RtabmapEventCmd *)anEvent)->getCmd();
+		if(cmd == rtabmap::RtabmapEventCmd::kCmdDumpMemory)
+		{
+			if(!dumpMemoryClient_.call(srv))
+			{
+				ROS_ERROR("Can't call \"dumpMemory\" service");
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdDumpPrediction)
+		{
+			if(!dumpPredictionClient_.call(srv))
+			{
+				ROS_ERROR("Can't call \"dumpPrediction\" service");
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdResetMemory)
+		{
+			if(!resetMemoryClient_.call(srv))
+			{
+				ROS_ERROR("Can't call \"resetMemory\" service");
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdDeleteMemory)
+		{
+			if(!deleteMemoryClient_.call(srv))
+			{
+				ROS_ERROR("Can't call \"deleteMemory\" service");
+			}
+		}
+		else
+		{
+			ROS_WARN("Unknown command...");
+		}
+	}
+}

rtabmap/src/GuiWrapper.h

@@ -0,0 +1,54 @@
+/*
+ * GuiWrapper.h
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#ifndef GUIWRAPPER_H_
+#define GUIWRAPPER_H_
+
+#include <ros/ros.h>
+#include "rtabmap/RtabmapInfo.h"
+#include "rtabmap/RtabmapInfoEx.h"
+#include "utilite/UEventsHandler.h"
+
+namespace rtabmap
+{
+	class MainWindow;
+}
+
+class QApplication;
+
+class GuiWrapper : public UEventsHandler
+{
+public:
+	GuiWrapper(int & argc, char** argv);
+	virtual ~GuiWrapper();
+
+	int exec();
+
+protected:
+	virtual void handleEvent(UEvent * anEvent);
+
+private:
+	void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg);
+	void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & infoExMsg);
+
+private:
+	ros::NodeHandle nh_;
+	ros::Subscriber infoTopic_;
+	ros::Subscriber infoExTopic_;
+	QApplication * app_;
+	rtabmap::MainWindow * mainWindow_;
+
+	ros::ServiceClient resetMemoryClient_;
+	ros::ServiceClient dumpMemoryClient_;
+	ros::ServiceClient changeCameraImgRateClient_;
+	ros::ServiceClient deleteMemoryClient_;
+	ros::ServiceClient dumpPredictionClient_;
+
+	ros::Publisher parametersUpdatedPub_;
+};
+
+#endif /* GUIWRAPPER_H_ */

rtabmap/src/ImageToSensoryMotorStateNode.cpp

@@ -0,0 +1,149 @@
+/*
+ * CameraNode.cpp
+ *
+ *  Created on: 1 févr. 2010
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/SMState.h>
+#include <utilite/ULogger.h>
+#include <utilite/UTimer.h>
+#include <cv_bridge/CvBridge.h>
+#include "rtabmap/SensoryMotorState.h"
+#include <std_msgs/Empty.h>
+
+
+rtabmap::CamKeypointTreatment kpThreatment;
+ros::Publisher rosPublisher;
+
+int imgWidth = 0;
+int imgHeight = 0;
+
+void updateParameters()
+{
+	ros::NodeHandle nh;
+	rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+	for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string value;
+		if(nh.getParam(iter->first, value))
+		{
+			iter->second = value;
+		}
+	}
+	ROS_INFO("Updating parameters");
+	kpThreatment.parseParameters(parameters);
+}
+
+void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg)
+{
+	updateParameters();
+}
+
+void parametersLoadedCallback(const std_msgs::EmptyConstPtr & msg)
+{
+	updateParameters();
+}
+
+void imgReceivedCallback(const sensor_msgs::ImageConstPtr & imgMsg)
+{
+	IplImage * image = 0;
+	sensor_msgs::CvBridge bridge;
+	if(imgMsg->data.size())
+	{
+		image = bridge.imgMsgToCv(imgMsg);
+		bool resized = false;
+		if(image &&
+		   imgWidth &&
+		   imgHeight &&
+		   imgWidth != image->width &&
+		   imgHeight != image->height)
+		{
+			// declare a destination IplImage object with correct size, depth and channels
+			IplImage * resampledImg = cvCreateImage( cvSize((int)(imgWidth) ,
+												   (int)(imgHeight) ),
+												   image->depth, image->nChannels );
+
+			//use cvResize to resize source to a destination image (linear interpolation)
+			cvResize(image, resampledImg);
+			image = resampledImg;
+			resized = true;
+		}
+
+		if(image)
+		{
+			UTimer timer;
+			rtabmap::SMState * smState = kpThreatment.process(image);
+			ROS_INFO("Time processing image %fs", timer.ticks());
+			if(smState)
+			{
+				rtabmap::SensoryMotorStatePtr msg(new rtabmap::SensoryMotorState);
+
+				std::vector<float> sensors;
+				int sensorStep = 0;
+				smState->getSensorsMerged(sensors, sensorStep);
+				msg->sensors = sensors;
+				msg->sensorStep = sensorStep;
+
+				std::vector<float> actuators;
+				int actuatorStep = 0;
+				smState->getActuatorsMerged(actuators, actuatorStep);
+				msg->actuators = actuators;
+				msg->actuatorStep = actuatorStep;
+
+				if(!resized)
+				{
+					msg->image = *imgMsg;
+				}
+				else
+				{
+					sensor_msgs::CvBridge::fromIpltoRosImage(image, msg->image);
+				}
+				const std::list<cv::KeyPoint> & keypoints = smState->getKeypoints();
+				msg->keypoints = std::vector<rtabmap::KeyPoint>(keypoints.size());
+				int i=0;
+				for(std::list<cv::KeyPoint>::const_iterator iter = keypoints.begin(); iter!=keypoints.end(); ++iter)
+				{
+					msg->keypoints.at(i).angle = iter->angle;
+					msg->keypoints.at(i).octave = iter->octave;
+					msg->keypoints.at(i).ptx = iter->pt.x;
+					msg->keypoints.at(i).pty = iter->pt.y;
+					msg->keypoints.at(i).response = iter->response;
+					msg->keypoints.at(i).size = iter->size;
+					msg->keypoints.at(i).class_id = iter->class_id;
+					++i;
+				}
+
+				rosPublisher.publish(msg);
+			}
+			if(resized)
+			{
+				cvReleaseImage(&image);
+			}
+		}
+	}
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "image_to_sms_node");
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kDebug);
+
+	ros::NodeHandle nh;
+
+	nh.param("resize_image_width", imgWidth, imgWidth);
+	nh.param("resize_image_height", imgHeight, imgHeight);
+
+	ros::Subscriber parametersUpdatedTopic = nh.subscribe("parameters_updated", 1, parametersUpdatedCallback);
+	ros::Subscriber parametersLoadedTopic = nh.subscribe("parameters_loaded", 1, parametersLoadedCallback);
+	ros::Subscriber image_sub = nh.subscribe("image", 1, imgReceivedCallback);
+	rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
+
+	updateParameters();
+
+	ros::spin();
+}

rtabmap/src/InputNode.cpp

@@ -0,0 +1,86 @@
+/*
+ * CameraNode.cpp
+ *
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include "rtabmap/SensoryMotorState.h"
+#include <geometry_msgs/Twist.h>
+#include <utilite/UMutex.h>
+
+UMutex commandMutex;
+std::list<std::vector<float> > commands;
+ros::Publisher rosPublisher;
+
+void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
+{
+	ROS_INFO("Received camera data");
+	std::vector<float> tmp;
+
+	int sizeActions = -1;
+	commandMutex.lock();
+	{
+		for(std::list<std::vector<float> >::iterator iter = commands.begin(); iter!=commands.end();++iter)
+		{
+			tmp.insert(tmp.end(), iter->begin(), iter->end());
+		}
+		sizeActions = commands.size();
+		commands.clear();
+
+	}
+	commandMutex.unlock();
+
+	rtabmap::SensoryMotorStatePtr state(new rtabmap::SensoryMotorState);
+	state->sensors = msg->sensors;
+	state->sensorStep = msg->sensorStep;
+	state->actuators = tmp;
+	state->actuatorStep = 6;
+	state->image = msg->image;
+	state->keypoints = msg->keypoints;
+
+	rosPublisher.publish(state);
+	ROS_INFO("Sensorimotor state sent (sizeActions=%d)", sizeActions);
+}
+
+void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
+{
+	ROS_INFO("Received command velocity linear=(%f,%f,%f) angular=(%f,%f,%f)",
+			msg->linear.x,
+			msg->linear.y,
+			msg->linear.z,
+			msg->angular.x,
+			msg->angular.y,
+			msg->angular.z);
+	commandMutex.lock();
+	{
+		std::vector<float> v(6);
+		v[0] = msg->linear.x;
+		v[1] = msg->linear.y;
+		v[2] = msg->linear.z;
+		v[3] = msg->angular.x;
+		v[4] = msg->angular.y;
+		v[5] = msg->angular.z;
+
+		commands.push_back(v);
+
+		// 10 Hz + 1 max
+		while(commands.size() > 11)
+		{
+			//remove the oldest
+			commands.pop_front();
+		}
+	}
+	commandMutex.unlock();
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "input_node");
+	ros::NodeHandle n;
+	rosPublisher = n.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
+	ros::Subscriber image_sub = n.subscribe("camera_data", 1, smReceivedCallback);
+	ros::Subscriber velocity_sub = n.subscribe("cmd_vel", 1, velocityReceivedCallback);
+
+	ros::spin();
+}

rtabmap/src/OutputNode.cpp

@@ -0,0 +1,79 @@
+/*
+ * CameraNode.cpp
+ *
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include "rtabmap/RtabmapInfo.h"
+#include "rtabmap/RtabmapInfoEx.h"
+#include <geometry_msgs/Twist.h>
+#include <utilite/UMutex.h>
+
+UMutex commandMutex;
+std::vector<float> commands;
+int commandSize = 0;
+int commandIndex = 0;
+ros::Publisher rosPublisher;
+
+void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
+{
+	commandMutex.lock();
+	{
+		commands = msg->actuators;
+		commandSize = msg->actuatorStep;
+		commandIndex = 0;
+		ROS_INFO("Rtabmap's actions received, commandSize=%d", commandSize);
+	}
+	commandMutex.unlock();
+}
+
+void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
+{
+	ROS_INFO("Rtabmap's actions received");
+	commandMutex.lock();
+	{
+		commands = msg->info.actuators;
+		commandSize = msg->info.actuatorStep;
+		ROS_INFO("Rtabmap's actions received, commandSize=%d, id=%d", commandSize, msg->info.refId);
+		commandIndex = 0;
+	}
+	commandMutex.unlock();
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "output_node");
+	ros::NodeHandle n;
+	ros::Subscriber infoTopic;
+	ros::Subscriber infoExTopic;
+	infoTopic = n.subscribe("rtabmap_info", 1, infoReceivedCallback);
+	infoExTopic = n.subscribe("rtabmap_info_x", 1, infoExReceivedCallback);
+	rosPublisher = n.advertise<geometry_msgs::Twist>("rtabmap/cmd_vel", 1);
+
+	ros::Rate loop_rate(10); // 10 Hz
+	while(ros::ok())
+	{
+		commandMutex.lock();
+		{
+			if(commandIndex>=0 &&
+				commandSize==6 &&
+				commandIndex + (commandSize-1) < (int)commands.size())
+			{
+				geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
+				vel->linear.x = commands[commandIndex++];
+				vel->linear.y = commands[commandIndex++];
+				vel->linear.z = commands[commandIndex++];
+				vel->angular.x = commands[commandIndex++];
+				vel->angular.y = commands[commandIndex++];
+				vel->angular.z = commands[commandIndex++];
+				ROS_INFO("Publishing vel");
+				rosPublisher.publish(vel);
+			}
+		}
+		commandMutex.unlock();
+
+		ros::spinOnce();
+		loop_rate.sleep();
+	}
+}

rtabmap/src/PreferencesDialogROS.cpp

@@ -0,0 +1,104 @@
+/*
+ * PreferencesDialogROS.cpp
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "PreferencesDialogROS.h"
+#include <rtabmap/core/Parameters.h>
+#include <QtCore/QDir>
+#include <QtCore/QSettings>
+#include <QtGui/QHBoxLayout>
+#include <QtCore/QTimer>
+#include <QtGui/QLabel>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <QtGui/QMessageBox>
+#include <ros/exceptions.h>
+
+using namespace rtabmap;
+
+PreferencesDialogROS::PreferencesDialogROS()
+{
+}
+
+PreferencesDialogROS::~PreferencesDialogROS()
+{
+
+}
+
+void PreferencesDialogROS::readCameraSettings(const QString & filePath)
+{
+	double imgRate = 0;
+	nh_.getParam("cam/image_rate", imgRate);
+	this->setImgRate(imgRate);
+}
+
+QString PreferencesDialogROS::getParamMessage()
+{
+	return tr("Reading parameters from the ROS server...");
+}
+
+void PreferencesDialogROS::readCoreSettings(const QString & filePath)
+{
+	if(filePath.isEmpty())
+	{
+		ROS_INFO("%s", this->getParamMessage().toStdString().c_str());
+		bool validParameters = true;
+		rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+		for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
+		{
+			std::string value;
+			if(nh_.getParam((*i).first,value))
+			{
+				PreferencesDialog::setParameter((*i).first, value);
+			}
+			else
+			{
+				validParameters = false;
+				break;
+			}
+		}
+
+		if(validParameters)
+		{
+			ROS_INFO("Parameters successfully read.");
+		}
+		else
+		{
+			validParameters = false;
+			QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap/core_node may be not started or some parameters won't work...");
+			ROS_ERROR("%s", warning.toStdString().c_str());
+			QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning);
+		}
+	}
+	else
+	{
+		PreferencesDialog::readCoreSettings(filePath);
+	}
+
+}
+
+void PreferencesDialogROS::writeSettings(const QString & filePath)
+{
+	writeGuiSettings(filePath);
+
+	// This will tell the MainWindow that the
+	//parameters are updated. The MainWindow will send an Event that
+	// will be handled by the GuiWrapper where we will write
+	// parameters in ROS and the rtabmap_node will be notified.
+	if(_parameters.size())
+	{
+		emit settingsChanged(_parameters);
+
+	}
+
+	if(_obsoletePanels)
+	{
+		emit settingsChanged(_obsoletePanels);
+	}
+
+	_parameters = rtabmap::ParametersMap();
+	_obsoletePanels = kPanelDummy;
+}
+

rtabmap/src/PreferencesDialogROS.h

@@ -0,0 +1,33 @@
+/*
+ * PreferencesDialogROS.h
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#ifndef PREFERENCESDIALOGROS_H_
+#define PREFERENCESDIALOGROS_H_
+
+#include <ros/ros.h>
+#include <rtabmap/gui/PreferencesDialog.h>
+
+using namespace rtabmap;
+
+class PreferencesDialogROS : public PreferencesDialog
+{
+public:
+	PreferencesDialogROS();
+	virtual ~PreferencesDialogROS();
+
+protected:
+	virtual QString getParamMessage();
+
+	virtual void readCameraSettings(const QString & filePath);
+	virtual void readCoreSettings(const QString & filePath);
+	virtual void writeSettings(const QString & filePath);
+
+private:
+	ros::NodeHandle nh_;
+};
+
+#endif /* PREFERENCESDIALOGROS_H_ */

rtabmap/srv/ChangeCameraImgRate.srv

@@ -0,0 +1,3 @@
+float32 imgRate
+bool autoRestart
+---