From d8d9118a00326c7d5be8a3b1a8951df3cb8c6a4f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 11 Dec 2012 18:05:05 +0000 Subject: [PATCH] merged attention branch to trunk git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@657 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- fmodex/CMakeLists.txt | 30 -- fmodex/FindFmodex.cmake | 53 -- fmodex/Makefile | 20 - fmodex/mainpage.dox | 14 - fmodex/manifest.xml | 14 - rtabmap/CMakeLists.txt | 23 - rtabmap/launch/all_tr_learning.launch | 22 - rtabmap/launch/az2_learning.launch | 62 --- rtabmap/launch/az2_teleop.launch | 14 - rtabmap/launch/az3_learning.launch | 62 --- rtabmap/launch/az3_teleop.launch | 14 - rtabmap/launch/cameraOmni.launch | 14 - rtabmap/launch/cameraUVC.launch | 10 - rtabmap/launch/loop_closure_detection.launch | 9 +- .../loop_closure_detection_with_audio.launch | 36 -- rtabmap/launch/sensors_capture.launch | 41 -- rtabmap/launch/teleop.launch | 9 - rtabmap/launch/teleop_keyboard.launch | 10 - rtabmap/launch/test_cameraOpenCV.launch | 16 - rtabmap/launch/test_cameraUVC.launch | 13 - rtabmap/launch/test_learning.launch | 57 -- .../launch/test_learning_image_twist.launch | 38 -- .../test_step_by_step_actions_only.launch | 48 -- rtabmap/launch/test_step_by_step_az2.launch | 63 --- rtabmap/launch/test_visual_attention.launch | 34 -- rtabmap/manifest.xml | 4 +- rtabmap/msg/ActuatorMsg.msg | 6 - rtabmap/msg/CvMatMsg.msg | 10 - rtabmap/msg/Info.msg | 10 + rtabmap/msg/{RtabmapInfoEx.msg => InfoEx.msg} | 11 +- rtabmap/msg/RtabmapInfo.msg | 16 - rtabmap/msg/SensorMsg.msg | 8 - rtabmap/msg/Sensorimotor.msg | 4 - rtabmap/src/AbtrVelocityNode.cpp | 197 ------- rtabmap/src/CmdVelToTurtleVelNode.cpp | 33 -- rtabmap/src/CoreWrapper.cpp | 154 +++--- rtabmap/src/CoreWrapper.h | 3 - rtabmap/src/GuiWrapper.cpp | 107 ++-- rtabmap/src/GuiWrapper.h | 9 +- rtabmap/src/ImageAudioInputNode.cpp | 90 ---- rtabmap/src/ImageAudioTwistInputNode.cpp | 107 ---- rtabmap/src/ImageTwistInputNode.cpp | 94 ---- rtabmap/src/MsgConversion.h | 68 --- rtabmap/src/OutputNode.cpp | 78 --- rtabmap/src/Twist2Pose.cpp | 65 --- rtabmap/src/Twist2TwistStamped.cpp | 35 -- rtabmap/src/VisualAttentionNode.cpp | 507 ------------------ rtabmap_audio/AudioPlayerNode.cpp | 389 -------------- rtabmap_audio/AudioRecorderNode.cpp | 272 ---------- rtabmap_audio/CMakeLists.txt | 45 -- rtabmap_audio/FindFmodex.cmake | 53 -- rtabmap_audio/Makefile | 1 - rtabmap_audio/mainpage.dox | 14 - rtabmap_audio/manifest.xml | 21 - rtabmap_audio/msg/AudioFrame.msg | 11 - rtabmap_audio/msg/AudioFrameFreq.msg | 10 - rtabmap_audio/msg/AudioFrameFreqSqrdMagn.msg | 10 - rtabmap_image/.cproject | 57 -- rtabmap_image/.project | 79 --- rtabmap_image/CMakeLists.txt | 55 -- rtabmap_image/Makefile | 1 - rtabmap_image/cfg/Camera.cfg | 16 - rtabmap_image/cfg/MotionFilter.cfg | 11 - rtabmap_image/cfg/rgb2ind.cfg | 22 - rtabmap_image/cfg/xy2polar.cfg | 12 - rtabmap_image/launch/camera.launch | 9 - .../launch/test_rtabmap_image.launch | 31 -- rtabmap_image/mainpage.dox | 14 - rtabmap_image/manifest.xml | 22 - rtabmap_image/src/CameraNode.cpp | 263 --------- rtabmap_image/src/Cartesian2PolarNode.cpp | 96 ---- rtabmap_image/src/ImageViewQt.hpp | 231 -------- rtabmap_image/src/ImageViewQtNode.cpp | 162 ------ rtabmap_image/src/MotionFilterNode.cpp | 89 --- rtabmap_image/src/RGB2IndexedNode.cpp | 82 --- rtabmap_lib/Makefile | 2 +- rtabmap_lib/manifest.xml | 2 +- utilite/Makefile | 30 -- utilite/manifest.xml | 24 - utilite/patched | 0 utilite/rospack_nosubdirs | 0 81 files changed, 126 insertions(+), 4352 deletions(-) delete mode 100644 fmodex/CMakeLists.txt delete mode 100644 fmodex/FindFmodex.cmake delete mode 100644 fmodex/Makefile delete mode 100644 fmodex/mainpage.dox delete mode 100644 fmodex/manifest.xml delete mode 100644 rtabmap/launch/all_tr_learning.launch delete mode 100644 rtabmap/launch/az2_learning.launch delete mode 100644 rtabmap/launch/az2_teleop.launch delete mode 100644 rtabmap/launch/az3_learning.launch delete mode 100644 rtabmap/launch/az3_teleop.launch delete mode 100644 rtabmap/launch/cameraOmni.launch delete mode 100644 rtabmap/launch/cameraUVC.launch delete mode 100644 rtabmap/launch/loop_closure_detection_with_audio.launch delete mode 100644 rtabmap/launch/sensors_capture.launch delete mode 100644 rtabmap/launch/teleop.launch delete mode 100644 rtabmap/launch/teleop_keyboard.launch delete mode 100644 rtabmap/launch/test_cameraOpenCV.launch delete mode 100644 rtabmap/launch/test_cameraUVC.launch delete mode 100644 rtabmap/launch/test_learning.launch delete mode 100644 rtabmap/launch/test_learning_image_twist.launch delete mode 100644 rtabmap/launch/test_step_by_step_actions_only.launch delete mode 100644 rtabmap/launch/test_step_by_step_az2.launch delete mode 100644 rtabmap/launch/test_visual_attention.launch delete mode 100644 rtabmap/msg/ActuatorMsg.msg delete mode 100644 rtabmap/msg/CvMatMsg.msg create mode 100644 rtabmap/msg/Info.msg rename rtabmap/msg/{RtabmapInfoEx.msg => InfoEx.msg} (77%) delete mode 100644 rtabmap/msg/RtabmapInfo.msg delete mode 100644 rtabmap/msg/SensorMsg.msg delete mode 100644 rtabmap/msg/Sensorimotor.msg delete mode 100644 rtabmap/src/AbtrVelocityNode.cpp delete mode 100644 rtabmap/src/CmdVelToTurtleVelNode.cpp delete mode 100644 rtabmap/src/ImageAudioInputNode.cpp delete mode 100644 rtabmap/src/ImageAudioTwistInputNode.cpp delete mode 100644 rtabmap/src/ImageTwistInputNode.cpp delete mode 100644 rtabmap/src/MsgConversion.h delete mode 100644 rtabmap/src/OutputNode.cpp delete mode 100644 rtabmap/src/Twist2Pose.cpp delete mode 100644 rtabmap/src/Twist2TwistStamped.cpp delete mode 100644 rtabmap/src/VisualAttentionNode.cpp delete mode 100644 rtabmap_audio/AudioPlayerNode.cpp delete mode 100644 rtabmap_audio/AudioRecorderNode.cpp delete mode 100644 rtabmap_audio/CMakeLists.txt delete mode 100644 rtabmap_audio/FindFmodex.cmake delete mode 100644 rtabmap_audio/Makefile delete mode 100644 rtabmap_audio/mainpage.dox delete mode 100644 rtabmap_audio/manifest.xml delete mode 100644 rtabmap_audio/msg/AudioFrame.msg delete mode 100644 rtabmap_audio/msg/AudioFrameFreq.msg delete mode 100644 rtabmap_audio/msg/AudioFrameFreqSqrdMagn.msg delete mode 100644 rtabmap_image/.cproject delete mode 100644 rtabmap_image/.project delete mode 100644 rtabmap_image/CMakeLists.txt delete mode 100644 rtabmap_image/Makefile delete mode 100755 rtabmap_image/cfg/Camera.cfg delete mode 100755 rtabmap_image/cfg/MotionFilter.cfg delete mode 100755 rtabmap_image/cfg/rgb2ind.cfg delete mode 100755 rtabmap_image/cfg/xy2polar.cfg delete mode 100644 rtabmap_image/launch/camera.launch delete mode 100644 rtabmap_image/launch/test_rtabmap_image.launch delete mode 100644 rtabmap_image/mainpage.dox delete mode 100644 rtabmap_image/manifest.xml delete mode 100644 rtabmap_image/src/CameraNode.cpp delete mode 100644 rtabmap_image/src/Cartesian2PolarNode.cpp delete mode 100644 rtabmap_image/src/ImageViewQt.hpp delete mode 100644 rtabmap_image/src/ImageViewQtNode.cpp delete mode 100644 rtabmap_image/src/MotionFilterNode.cpp delete mode 100644 rtabmap_image/src/RGB2IndexedNode.cpp delete mode 100644 utilite/Makefile delete mode 100644 utilite/manifest.xml delete mode 100644 utilite/patched delete mode 100644 utilite/rospack_nosubdirs diff --git a/fmodex/CMakeLists.txt b/fmodex/CMakeLists.txt deleted file mode 100644 index f8f1c9cc..00000000 --- a/fmodex/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# 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) - -#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}) diff --git a/fmodex/FindFmodex.cmake b/fmodex/FindFmodex.cmake deleted file mode 100644 index 1e3dc548..00000000 --- a/fmodex/FindFmodex.cmake +++ /dev/null @@ -1,53 +0,0 @@ -# - Find Fmodex -# This module finds an installed Fmod package. -# -# It sets the following variables: -# Fmodex_FOUND - Set to false, or undefined, if Fmod isn't found. -# Fmodex_INCLUDE_DIRS - The Fmod include directory. -# Fmodex_LIBRARIES - The Fmod library to link against. -# -# - -SET(Fmodex_ROOT) - -# Add ROS Fmodex directory if ROS is installed -FIND_PROGRAM(ROSPACK_EXEC NAME rospack PATHS) -IF(ROSPACK_EXEC) - EXECUTE_PROCESS(COMMAND ${ROSPACK_EXEC} find fmodex - OUTPUT_VARIABLE Fmodex_ROS_PATH - OUTPUT_STRIP_TRAILING_WHITESPACE - WORKING_DIRECTORY "./" - ) - IF(Fmodex_ROS_PATH) - MESSAGE(STATUS "Found Fmodex ROS pkg : ${Fmodex_ROS_PATH}") - SET(Fmodex_ROOT - ${Fmodex_ROS_PATH}/fmodex - ${Fmodex_ROOT} - ) - ENDIF(Fmodex_ROS_PATH) -ENDIF(ROSPACK_EXEC) - -FIND_PATH(Fmodex_INCLUDE_DIRS fmod.h PATHS ${Fmodex_ROOT}/include) - -IF(CMAKE_SIZEOF_VOID_P EQUAL 8) - FIND_LIBRARY(Fmodex_LIBRARIES NAMES fmodex64 PATHS ${Fmodex_ROOT}/lib) -ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8) -IF(NOT Fmodex_LIBRARIES) - FIND_LIBRARY(Fmodex_LIBRARIES NAMES fmodex PATHS ${Fmodex_ROOT}/lib) -ENDIF(NOT Fmodex_LIBRARIES) - -IF (Fmodex_INCLUDE_DIRS AND Fmodex_LIBRARIES) - SET(Fmodex_FOUND TRUE) -ENDIF (Fmodex_INCLUDE_DIRS AND Fmodex_LIBRARIES) - -IF (Fmodex_FOUND) - # show which Fmod was found only if not quiet - IF (NOT Fmodex_FIND_QUIETLY) - MESSAGE(STATUS "Found Fmod: ${Fmodex_LIBRARIES}") - ENDIF (NOT Fmodex_FIND_QUIETLY) -ELSE (Fmodex_FOUND) - # fatal error if Fmod is required but not found - IF (Fmodex_FIND_REQUIRED) - MESSAGE(FATAL_ERROR "Could not find Fmodex... (aka libfmodex)") - ENDIF (Fmodex_FIND_REQUIRED) -ENDIF (Fmodex_FOUND) diff --git a/fmodex/Makefile b/fmodex/Makefile deleted file mode 100644 index 66f8110b..00000000 --- a/fmodex/Makefile +++ /dev/null @@ -1,20 +0,0 @@ - -all: installed - -TARBALL = build/fmodex44008.tar.gz -TARBALL_URL = http://rtabmap.googlecode.com/files/fmodex44008.tar.gz -SOURCE_DIR = build/fmodex44008 -UNPACK_CMD = tar xzf -include $(shell rospack find mk)/download_unpack_build.mk - -installed: $(SOURCE_DIR)/unpacked - mkdir -p fmodex - cp -r $(SOURCE_DIR)/include fmodex/. - cp -r $(SOURCE_DIR)/lib fmodex/. - touch installed - -clean: - -rm -rf src $(SOURCE_DIR) installed - -wipe: clean - -rm -rf build diff --git a/fmodex/mainpage.dox b/fmodex/mainpage.dox deleted file mode 100644 index 06b27258..00000000 --- a/fmodex/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b fmodex - - - ---> - - -*/ diff --git a/fmodex/manifest.xml b/fmodex/manifest.xml deleted file mode 100644 index f20d20df..00000000 --- a/fmodex/manifest.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - fmodex - - - Mathieu Labbé - BSD - - http://ros.org/wiki/fmodex - - - - diff --git a/rtabmap/CMakeLists.txt b/rtabmap/CMakeLists.txt index 745cddb0..3891733e 100644 --- a/rtabmap/CMakeLists.txt +++ b/rtabmap/CMakeLists.txt @@ -41,34 +41,11 @@ find_package(OpenCV REQUIRED) rosbuild_add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp) target_link_libraries(rtabmap ${OpenCV_LIBS}) -rosbuild_add_executable(rtabmap_out src/OutputNode.cpp) -rosbuild_add_executable(abtr_velocity src/AbtrVelocityNode.cpp) -rosbuild_add_executable(twist_to_turtle_vel src/CmdVelToTurtleVelNode.cpp) -rosbuild_add_executable(twist_to_pose src/Twist2Pose.cpp) -rosbuild_add_executable(twist_to_twist_stamped src/Twist2TwistStamped.cpp) - -# Input nodes -rosbuild_add_boost_directories() -rosbuild_add_executable(input_image_audio_node src/ImageAudioInputNode.cpp) -target_link_libraries(input_image_audio_node ${OpenCV_LIBS}) -rosbuild_link_boost(input_image_audio_node signals) - -rosbuild_add_executable(input_image_audio_twist_node src/ImageAudioTwistInputNode.cpp) -target_link_libraries(input_image_audio_twist_node ${OpenCV_LIBS}) -rosbuild_link_boost(input_image_audio_twist_node signals) - -rosbuild_add_executable(input_image_twist_node src/ImageTwistInputNode.cpp) -target_link_libraries(input_image_twist_node ${OpenCV_LIBS}) -rosbuild_link_boost(input_image_twist_node signals) - FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui) IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND) INCLUDE(${QT_USE_FILE}) rosbuild_add_executable(rtabmap_gui src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp) target_link_libraries(rtabmap_gui ${QT_LIBRARIES} ${OpenCV_LIBS} "-lrtabmap_gui") - - rosbuild_add_executable(visual_attention src/VisualAttentionNode.cpp) - target_link_libraries(visual_attention ${QT_LIBRARIES} ${OpenCV_LIBS}) ELSE() MESSAGE(STATUS "[WARNING] Qt4 not found, the GUI node will not be compiled...") ENDIF() diff --git a/rtabmap/launch/all_tr_learning.launch b/rtabmap/launch/all_tr_learning.launch deleted file mode 100644 index 4c6528ed..00000000 --- a/rtabmap/launch/all_tr_learning.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/rtabmap/launch/az2_learning.launch b/rtabmap/launch/az2_learning.launch deleted file mode 100644 index e3a1bbb8..00000000 --- a/rtabmap/launch/az2_learning.launch +++ /dev/null @@ -1,62 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rtabmap/launch/az2_teleop.launch b/rtabmap/launch/az2_teleop.launch deleted file mode 100644 index 8fc3b389..00000000 --- a/rtabmap/launch/az2_teleop.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/rtabmap/launch/az3_learning.launch b/rtabmap/launch/az3_learning.launch deleted file mode 100644 index ab74088d..00000000 --- a/rtabmap/launch/az3_learning.launch +++ /dev/null @@ -1,62 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rtabmap/launch/az3_teleop.launch b/rtabmap/launch/az3_teleop.launch deleted file mode 100644 index 1fab8022..00000000 --- a/rtabmap/launch/az3_teleop.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/rtabmap/launch/cameraOmni.launch b/rtabmap/launch/cameraOmni.launch deleted file mode 100644 index cca66c47..00000000 --- a/rtabmap/launch/cameraOmni.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - diff --git a/rtabmap/launch/cameraUVC.launch b/rtabmap/launch/cameraUVC.launch deleted file mode 100644 index eafcf35d..00000000 --- a/rtabmap/launch/cameraUVC.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/rtabmap/launch/loop_closure_detection.launch b/rtabmap/launch/loop_closure_detection.launch index a7c9a0a4..07e0dd65 100644 --- a/rtabmap/launch/loop_closure_detection.launch +++ b/rtabmap/launch/loop_closure_detection.launch @@ -9,11 +9,12 @@ - + - - - + + + + diff --git a/rtabmap/launch/loop_closure_detection_with_audio.launch b/rtabmap/launch/loop_closure_detection_with_audio.launch deleted file mode 100644 index e7d17542..00000000 --- a/rtabmap/launch/loop_closure_detection_with_audio.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rtabmap/launch/sensors_capture.launch b/rtabmap/launch/sensors_capture.launch deleted file mode 100644 index 4dc42d59..00000000 --- a/rtabmap/launch/sensors_capture.launch +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/rtabmap/launch/teleop.launch b/rtabmap/launch/teleop.launch deleted file mode 100644 index 56c880dd..00000000 --- a/rtabmap/launch/teleop.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/rtabmap/launch/teleop_keyboard.launch b/rtabmap/launch/teleop_keyboard.launch deleted file mode 100644 index b37c4c55..00000000 --- a/rtabmap/launch/teleop_keyboard.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/rtabmap/launch/test_cameraOpenCV.launch b/rtabmap/launch/test_cameraOpenCV.launch deleted file mode 100644 index 091ad8a8..00000000 --- a/rtabmap/launch/test_cameraOpenCV.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/rtabmap/launch/test_cameraUVC.launch b/rtabmap/launch/test_cameraUVC.launch deleted file mode 100644 index 9c75057b..00000000 --- a/rtabmap/launch/test_cameraUVC.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/rtabmap/launch/test_learning.launch b/rtabmap/launch/test_learning.launch deleted file mode 100644 index d6b77548..00000000 --- a/rtabmap/launch/test_learning.launch +++ /dev/null @@ -1,57 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/rtabmap/launch/test_learning_image_twist.launch b/rtabmap/launch/test_learning_image_twist.launch deleted file mode 100644 index dcc522fd..00000000 --- a/rtabmap/launch/test_learning_image_twist.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rtabmap/launch/test_step_by_step_actions_only.launch b/rtabmap/launch/test_step_by_step_actions_only.launch deleted file mode 100644 index 04856bfe..00000000 --- a/rtabmap/launch/test_step_by_step_actions_only.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/rtabmap/launch/test_step_by_step_az2.launch b/rtabmap/launch/test_step_by_step_az2.launch deleted file mode 100644 index 0bf7afd6..00000000 --- a/rtabmap/launch/test_step_by_step_az2.launch +++ /dev/null @@ -1,63 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rtabmap/launch/test_visual_attention.launch b/rtabmap/launch/test_visual_attention.launch deleted file mode 100644 index 93371db7..00000000 --- a/rtabmap/launch/test_visual_attention.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rtabmap/manifest.xml b/rtabmap/manifest.xml index 4417bc9d..0a7a1777 100644 --- a/rtabmap/manifest.xml +++ b/rtabmap/manifest.xml @@ -17,9 +17,7 @@ - - - + diff --git a/rtabmap/msg/ActuatorMsg.msg b/rtabmap/msg/ActuatorMsg.msg deleted file mode 100644 index 21c05adb..00000000 --- a/rtabmap/msg/ActuatorMsg.msg +++ /dev/null @@ -1,6 +0,0 @@ -######################################## -# Actuator -######################################## - -uint32 type # Actuator type, refer to rtabmap::Actuator::Type enum) -rtabmap/CvMatMsg matrix \ No newline at end of file diff --git a/rtabmap/msg/CvMatMsg.msg b/rtabmap/msg/CvMatMsg.msg deleted file mode 100644 index 9117df1f..00000000 --- a/rtabmap/msg/CvMatMsg.msg +++ /dev/null @@ -1,10 +0,0 @@ - -######################################## -# OpenCV Matrix description -######################################## - -uint32 width # Matrix width -uint32 height # Matrix height -uint32 dataType # Matrix data type (refer to OpenCV type, e.g. CV_8UC3 for a RGB image 8bits) -uint8[] data # Matrix data -uint8 compressed # if data is compressed (in case of an image for example) \ No newline at end of file diff --git a/rtabmap/msg/Info.msg b/rtabmap/msg/Info.msg new file mode 100644 index 00000000..384db51f --- /dev/null +++ b/rtabmap/msg/Info.msg @@ -0,0 +1,10 @@ + +######################################## +# If a loop is found with the current image ("refId"), +# "loopClosureId" is not null. +######################################## + +Header header + +int32 refId +int32 loopClosureId \ No newline at end of file diff --git a/rtabmap/msg/RtabmapInfoEx.msg b/rtabmap/msg/InfoEx.msg similarity index 77% rename from rtabmap/msg/RtabmapInfoEx.msg rename to rtabmap/msg/InfoEx.msg index 59dbe63d..4c1d88c6 100644 --- a/rtabmap/msg/RtabmapInfoEx.msg +++ b/rtabmap/msg/InfoEx.msg @@ -1,13 +1,12 @@ ######################################## -# Statistics stuff: -# These fields are empty if RTAb-Map's -# parameter publishStats=false +# Extended info msg with statistics ######################################## Header header -int32 refChild +int32 refId +int32 loopClosureId # std::map posterior; int32[] posteriorKeys @@ -25,8 +24,8 @@ int32[] weightsValues string[] statsKeys float32[] statsValues -rtabmap/SensorMsg[] refRawData -rtabmap/SensorMsg[] loopRawData +sensor_msgs/Image refImage +sensor_msgs/Image loopImage # # For features2d : std::multimap words diff --git a/rtabmap/msg/RtabmapInfo.msg b/rtabmap/msg/RtabmapInfo.msg deleted file mode 100644 index 64430291..00000000 --- a/rtabmap/msg/RtabmapInfo.msg +++ /dev/null @@ -1,16 +0,0 @@ - -######################################## -# If a loop is found with the current image ("refId"), -# "loopClosureId" is not null. Field "actuators" contains -# (when actions are used) actions executed after the -# "loopClosureId". -######################################## - -Header header - -int32 refId -int32 loopClosureId - -rtabmap/ActuatorMsg[] actuators - -rtabmap/RtabmapInfoEx infoEx \ No newline at end of file diff --git a/rtabmap/msg/SensorMsg.msg b/rtabmap/msg/SensorMsg.msg deleted file mode 100644 index cef5b375..00000000 --- a/rtabmap/msg/SensorMsg.msg +++ /dev/null @@ -1,8 +0,0 @@ -######################################## -# Sensor -######################################## - -uint32 type # Sensor type (e.g. for sensor: image, audio), - # refer to rtabmap::Sensor::Type). -rtabmap/CvMatMsg matrix - diff --git a/rtabmap/msg/Sensorimotor.msg b/rtabmap/msg/Sensorimotor.msg deleted file mode 100644 index 4aae5671..00000000 --- a/rtabmap/msg/Sensorimotor.msg +++ /dev/null @@ -1,4 +0,0 @@ -Header header - -rtabmap/SensorMsg[] sensors -rtabmap/ActuatorMsg[] actuators \ No newline at end of file diff --git a/rtabmap/src/AbtrVelocityNode.cpp b/rtabmap/src/AbtrVelocityNode.cpp deleted file mode 100644 index 06a8ca56..00000000 --- a/rtabmap/src/AbtrVelocityNode.cpp +++ /dev/null @@ -1,197 +0,0 @@ -/* - * CameraNode.cpp - * - * Author: labm2414 - */ - -#include -#include -#include -#include -#include -#include - -std::queue commandsA; -std::queue commandsB; -std::vector lastCommandsB; -int commandSize = 0; -int commandIndex = 0; -double commandsHz = 10.0; -bool cmdABuffered = false; -bool cmdBBuffered = true; -ros::Publisher rosPublisher; -bool statsLogged = false; -const char * statsFileName = "AbtrStats.txt"; - -void velocityAReceivedCallback(const geometry_msgs::TwistConstPtr & msg) -{ - commandsA.push(msg->linear.x); - commandsA.push(msg->linear.y); - commandsA.push(msg->linear.z); - commandsA.push(msg->angular.x); - commandsA.push(msg->angular.y); - commandsA.push(msg->angular.z); -} - -void velocityBReceivedCallback(const geometry_msgs::TwistConstPtr & msg) -{ - commandsB.push(msg->linear.x); - commandsB.push(msg->linear.y); - commandsB.push(msg->linear.z); - commandsB.push(msg->angular.x); - commandsB.push(msg->angular.y); - commandsB.push(msg->angular.z); - - if(!lastCommandsB.size()) - { - lastCommandsB = std::vector(6); - } - lastCommandsB[0] = msg->linear.x; - lastCommandsB[1] = msg->linear.y; - lastCommandsB[2] = msg->linear.z; - lastCommandsB[3] = msg->angular.x; - lastCommandsB[4] = msg->angular.x; - lastCommandsB[5] = msg->angular.x; -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "abtr_velocity"); - ros::NodeHandle nh("~"); - - nh.param("commands_hz", commandsHz, commandsHz); - ROS_INFO("commands_hz=%f", commandsHz); - - nh.param("cmd_vel_a_buffered", cmdABuffered, cmdABuffered); - ROS_INFO("cmd_vel_a_buffered=%d", cmdABuffered); - nh.param("cmd_vel_b_buffered", cmdBBuffered, cmdBBuffered); - ROS_INFO("cmd_vel_b_buffered=%d", cmdBBuffered); - - nh.param("stats_logged", statsLogged, statsLogged); - ROS_INFO("stats_logged=%d", statsLogged); - - if(statsLogged) - { - ULogger::setPrintWhere(false); - ULogger::setBuffered(true); - ULogger::setPrintLevel(false); - ULogger::setPrintTime(false); - ULogger::setType(ULogger::kTypeFile, statsFileName, false); - ROS_INFO("stats log file = \"%s\"", statsFileName); - } - else - { - ULogger::setLevel(ULogger::kError); - } - - nh = ros::NodeHandle(); - ros::Subscriber velATopic; - ros::Subscriber velBTopic; - if(cmdABuffered) - { - velATopic = nh.subscribe("cmd_vel_a", 0, velocityAReceivedCallback); - } - else - { - velATopic = nh.subscribe("cmd_vel_a", 1, velocityAReceivedCallback); - } - if(cmdBBuffered) - { - velBTopic = nh.subscribe("cmd_vel_b", 0, velocityBReceivedCallback); - } - else - { - velBTopic = nh.subscribe("cmd_vel_b", 1, velocityBReceivedCallback); - } - - rosPublisher = nh.advertise("cmd_vel", 1); - - int index = 1; - ros::Rate loop_rate(commandsHz); // 10 Hz - while(ros::ok()) - { - loop_rate.sleep(); - ros::spinOnce(); - - geometry_msgs::TwistStampedPtr vel(new geometry_msgs::TwistStamped()); - vel->header.frame_id = "base_link"; - vel->header.stamp = ros::Time::now(); - - // priority for commandsA - if(commandsA.size()) - { - vel->twist.linear.x = commandsA.front(); - commandsA.pop(); - vel->twist.linear.y = commandsA.front(); - commandsA.pop(); - vel->twist.linear.z = commandsA.front(); - commandsA.pop(); - vel->twist.angular.x = commandsA.front(); - commandsA.pop(); - vel->twist.angular.y = commandsA.front(); - commandsA.pop(); - vel->twist.angular.z = commandsA.front(); - commandsA.pop(); - rosPublisher.publish(vel); - UINFO("%d A %f %f %f", index, vel->twist.linear.x, vel->twist.linear.y, vel->twist.angular.z); - if(!cmdABuffered) - { - commandsA = std::queue(); - } - if(commandsB.size()) - { - commandsB.pop(); - commandsB.pop(); - commandsB.pop(); - commandsB.pop(); - commandsB.pop(); - commandsB.pop(); - } - } - else if(commandsB.size()) - { - vel->twist.linear.x = commandsB.front(); - commandsB.pop(); - vel->twist.linear.y = commandsB.front(); - commandsB.pop(); - vel->twist.linear.z = commandsB.front(); - commandsB.pop(); - vel->twist.angular.x = commandsB.front(); - commandsB.pop(); - vel->twist.angular.y = commandsB.front(); - commandsB.pop(); - vel->twist.angular.z = commandsB.front(); - commandsB.pop(); - UINFO("%d B %f %f %f", index, vel->twist.linear.x, vel->twist.linear.y, vel->twist.angular.z); - rosPublisher.publish(vel); - if(!cmdBBuffered) - { - commandsB = std::queue(); - } - } - else - { - UINFO("%d NULL", index); - if(lastCommandsB.size()) - { - // Republish the last command one more time - // (if the sender cannot reach commandsHz for an iteration) - vel->twist.linear.x = lastCommandsB[0]; - vel->twist.linear.y = lastCommandsB[1]; - vel->twist.linear.z = lastCommandsB[2]; - vel->twist.angular.x = lastCommandsB[3]; - vel->twist.angular.y = lastCommandsB[4]; - vel->twist.angular.z = lastCommandsB[5]; - rosPublisher.publish(vel); - lastCommandsB = std::vector(); - } - } - ++index; - } - if(statsLogged) - { - ULogger::flush(); - } - - return 0; -} diff --git a/rtabmap/src/CmdVelToTurtleVelNode.cpp b/rtabmap/src/CmdVelToTurtleVelNode.cpp deleted file mode 100644 index 4df59f28..00000000 --- a/rtabmap/src/CmdVelToTurtleVelNode.cpp +++ /dev/null @@ -1,33 +0,0 @@ -/* - * CameraNode.cpp - * - * Created on: 1 févr. 2010 - * Author: labm2414 - */ - -#include -#include -#include - -ros::Publisher rosPublisher; - -void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg) -{ - turtlesim::Velocity vel; - vel.angular = msg->linear.y; - vel.linear = msg->linear.x; - rosPublisher.publish(vel); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "twist_to_turtle_vel"); - - ros::NodeHandle nh; - rosPublisher = nh.advertise("turtle1/command_velocity", 1); - ros::Subscriber image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback); - - ros::spin(); - - return 0; -} diff --git a/rtabmap/src/CoreWrapper.cpp b/rtabmap/src/CoreWrapper.cpp index b70277a7..87445bb6 100644 --- a/rtabmap/src/CoreWrapper.cpp +++ b/rtabmap/src/CoreWrapper.cpp @@ -6,13 +6,14 @@ */ #include "CoreWrapper.h" -#include "MsgConversion.h" #include +#include +#include +#include #include #include #include #include -#include #include #include #include @@ -20,9 +21,8 @@ #include //msgs -#include "rtabmap/RtabmapInfo.h" -#include "rtabmap/RtabmapInfoEx.h" -#include "rtabmap/CvMatMsg.h" +#include "rtabmap/Info.h" +#include "rtabmap/InfoEx.h" using namespace rtabmap; @@ -30,8 +30,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : rtabmap_(0) { ros::NodeHandle nh("~"); - infoPub_ = nh.advertise("info", 1); - infoPubEx_ = nh.advertise("infoEx", 1); + infoPub_ = nh.advertise("info", 1); + infoPubEx_ = nh.advertise("infoEx", 1); parametersLoadedPub_ = nh.advertise("parameters_loaded", 1); rtabmap_ = new Rtabmap(); @@ -52,7 +52,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : nh = ros::NodeHandle(); parametersUpdatedTopic_ = nh.subscribe("rtabmap_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this); - sensorimotorTopic_ = nh.subscribe("sensorimotor", 1, &CoreWrapper::sensorimotorReceivedCallback, this); image_transport::ImageTransport it(nh); imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this); @@ -116,31 +115,6 @@ void CoreWrapper::saveNodeParameters(const std::string & configFile) ROS_INFO("Database/long-term memory (%lu MB) is located at %s/LTM.db", UFile::length(databasePath)/1000000, databasePath.c_str()); } -void CoreWrapper::sensorimotorReceivedCallback(const rtabmap::SensorimotorConstPtr & msg) -{ - std::list sensors; - std::list actuators; - - for(unsigned int i=0; isensors.size(); ++i) - { - sensors.push_back(Sensor(fromCvMatMsgToCvMat(msg->sensors[i].matrix), (Sensor::Type)msg->sensors[i].type)); - } - for(unsigned int i=0; iactuators.size(); ++i) - { - actuators.push_back(Actuator(fromCvMatMsgToCvMat(msg->actuators[i].matrix), (Actuator::Type)msg->actuators[i].type)); - } - - if(!sensors.size() && !actuators.size()) - { - ROS_ERROR("Sensorimotor received is empty..."); - } - else - { - ROS_INFO("Received sensorimotor (%d sensors %d actuators).", sensors.size(), actuators.size()); - UEventsManager::post(new SensorimotorEvent(sensors, actuators)); - } -} - void CoreWrapper::imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg) { if(msg->data.size()) @@ -197,101 +171,109 @@ void CoreWrapper::handleEvent(UEvent * anEvent) { if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers()) { - ROS_INFO("Sending RtabmapInfo msg..."); RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent; const Statistics & stat = rtabmapEvent->getStats(); - rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo); - - // General info - msg->refId = stat.refImageId(); - msg->loopClosureId = stat.loopClosureId(); - - msg->actuators.resize(stat.getActuators().size()); - int i=0; - for(std::list::const_iterator iter = stat.getActuators().begin(); iter!=stat.getActuators().end(); ++iter) - { - msg->actuators[i].type = iter->type(); - fromCvMatToCvMatMsg(msg->actuators[i++].matrix, iter->data()); - } - if(infoPub_.getNumSubscribers()) { + ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId()); + rtabmap::InfoPtr msg(new rtabmap::Info); + msg->refId = stat.refImageId(); + msg->loopClosureId = stat.loopClosureId(); infoPub_.publish(msg); } if(infoPubEx_.getNumSubscribers()) { + ROS_INFO("Sending infoEx msg (last_id=%d)...", stat.refImageId()); + rtabmap::InfoExPtr msg(new rtabmap::InfoEx); + msg->refId = stat.refImageId(); + msg->loopClosureId = stat.loopClosureId(); + // Detailed info if(stat.extended()) { - if(stat.refRawData().size()) + if(!stat.refImage().empty()) { - msg->infoEx.refRawData.resize(stat.refRawData().size()); - i=0; - for(std::list::const_iterator iter = stat.refRawData().begin(); iter!=stat.refRawData().end(); ++iter) + cv_bridge::CvImage img; + if(stat.refImage().channels() == 1) { - msg->infoEx.refRawData[i].type = iter->type(); - fromCvMatToCvMatMsg(msg->infoEx.refRawData[i++].matrix, iter->data()); + img.encoding = sensor_msgs::image_encodings::MONO8; } + else + { + img.encoding = sensor_msgs::image_encodings::BGR8; + } + img.image = stat.refImage(); + sensor_msgs::ImagePtr rosMsg = img.toImageMsg(); + rosMsg->header.frame_id = "camera"; + rosMsg->header.stamp = ros::Time::now(); + msg->refImage = *rosMsg; } - if(stat.loopClosureRawData().size()) + if(!stat.loopImage().empty()) { - msg->infoEx.loopRawData.resize(stat.loopClosureRawData().size()); - i=0; - for(std::list::const_iterator iter = stat.loopClosureRawData().begin(); iter!=stat.loopClosureRawData().end(); ++iter) + cv_bridge::CvImage img; + if(stat.loopImage().channels() == 1) { - msg->infoEx.loopRawData[i].type = iter->type(); - fromCvMatToCvMatMsg(msg->infoEx.loopRawData[i++].matrix, iter->data()); + img.encoding = sensor_msgs::image_encodings::MONO8; } + else + { + img.encoding = sensor_msgs::image_encodings::BGR8; + } + img.image = stat.loopImage(); + sensor_msgs::ImagePtr rosMsg = img.toImageMsg(); + rosMsg->header.frame_id = "camera"; + rosMsg->header.stamp = ros::Time::now(); + msg->loopImage = *rosMsg; } //Posterior, likelihood, childCount - msg->infoEx.posteriorKeys = uKeys(stat.posterior()); - msg->infoEx.posteriorValues = uValues(stat.posterior()); - msg->infoEx.likelihoodKeys = uKeys(stat.likelihood()); - msg->infoEx.likelihoodValues = uValues(stat.likelihood()); - msg->infoEx.weightsKeys = uKeys(stat.weights()); - msg->infoEx.weightsValues = uValues(stat.weights()); + 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()); //Features stuff... - msg->infoEx.refWordsKeys = uListToVector(uKeys(stat.refWords())); - msg->infoEx.refWordsValues = std::vector(stat.refWords().size()); + msg->refWordsKeys = uKeys(stat.refWords()); + msg->refWordsValues = std::vector(stat.refWords().size()); int index = 0; for(std::multimap::const_iterator i=stat.refWords().begin(); i!=stat.refWords().end(); ++i) { - msg->infoEx.refWordsValues.at(index).angle = i->second.angle; - msg->infoEx.refWordsValues.at(index).response = i->second.response; - msg->infoEx.refWordsValues.at(index).ptx = i->second.pt.x; - msg->infoEx.refWordsValues.at(index).pty = i->second.pt.y; - msg->infoEx.refWordsValues.at(index).size = i->second.size; - msg->infoEx.refWordsValues.at(index).octave = i->second.octave; - msg->infoEx.refWordsValues.at(index).class_id = i->second.class_id; + 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->infoEx.loopWordsKeys = uListToVector(uKeys(stat.loopWords())); - msg->infoEx.loopWordsValues = std::vector(stat.loopWords().size()); + msg->loopWordsKeys = uKeys(stat.loopWords()); + msg->loopWordsValues = std::vector(stat.loopWords().size()); index = 0; for(std::multimap::const_iterator i=stat.loopWords().begin(); i!=stat.loopWords().end(); ++i) { - msg->infoEx.loopWordsValues.at(index).angle = i->second.angle; - msg->infoEx.loopWordsValues.at(index).response = i->second.response; - msg->infoEx.loopWordsValues.at(index).ptx = i->second.pt.x; - msg->infoEx.loopWordsValues.at(index).pty = i->second.pt.y; - msg->infoEx.loopWordsValues.at(index).size = i->second.size; - msg->infoEx.loopWordsValues.at(index).octave = i->second.octave; - msg->infoEx.loopWordsValues.at(index).class_id = i->second.class_id; + 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->infoEx.statsKeys = uKeys(stat.data()); - msg->infoEx.statsValues = uValues(stat.data()); + msg->statsKeys = uKeys(stat.data()); + msg->statsValues = uValues(stat.data()); } infoPubEx_.publish(msg); } diff --git a/rtabmap/src/CoreWrapper.h b/rtabmap/src/CoreWrapper.h index a00b406d..84d37e2e 100644 --- a/rtabmap/src/CoreWrapper.h +++ b/rtabmap/src/CoreWrapper.h @@ -18,7 +18,6 @@ #include #include #include -#include "rtabmap/Sensorimotor.h" namespace rtabmap { @@ -34,7 +33,6 @@ public: void start(); private: - void sensorimotorReceivedCallback(const rtabmap::SensorimotorConstPtr & msg); void imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg); void twistCallback(const geometry_msgs::TwistConstPtr & msg); void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg); @@ -51,7 +49,6 @@ private: private: rtabmap::Rtabmap * rtabmap_; - ros::Subscriber sensorimotorTopic_; image_transport::Subscriber imageTopic_; ros::Subscriber audioFrameFreqSqrdMagnTopic_; ros::Subscriber twistTopic_; diff --git a/rtabmap/src/GuiWrapper.cpp b/rtabmap/src/GuiWrapper.cpp index 824784c7..537ed4da 100644 --- a/rtabmap/src/GuiWrapper.cpp +++ b/rtabmap/src/GuiWrapper.cpp @@ -6,7 +6,6 @@ */ #include "GuiWrapper.h" -#include "MsgConversion.h" #include #include @@ -21,8 +20,6 @@ #include #include #include -#include -#include #include "PreferencesDialogROS.h" @@ -31,8 +28,7 @@ using namespace rtabmap; GuiWrapper::GuiWrapper(int & argc, char** argv) { ros::NodeHandle nh; - infoTopic_ = nh.subscribe("rtabmap/infoEx", 1, &GuiWrapper::infoReceivedCallback, this); - velocity_sub_ = nh.subscribe("cmd_vel", 1, &GuiWrapper::velocityReceivedCallback, this); + infoExTopic_ = nh.subscribe("rtabmap/infoEx", 1, &GuiWrapper::infoExReceivedCallback, this); app_ = new QApplication(argc, argv); mainWindow_ = new MainWindow(new PreferencesDialogROS()); mainWindow_->show(); @@ -62,9 +58,9 @@ int GuiWrapper::exec() return app_->exec(); } -void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg) +void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg) { - ROS_INFO("RTAB-Map info received!"); + ROS_INFO("RTAB-Map info ex received!"); // Map from ROS struct to rtabmap struct rtabmap::Statistics * stat = new rtabmap::Statistics(); @@ -72,112 +68,73 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg) stat->setExtended(true); // Extended stat->setRefImageId(msg->refId); - std::list sensors; - for(unsigned int i=0; iinfoEx.refRawData.size(); ++i) - { - Sensor s(fromCvMatMsgToCvMat(msg->infoEx.refRawData[i].matrix), (Sensor::Type)msg->infoEx.refRawData[i].type); - if(s.data().total()) - { - sensors.push_back(s); - } - } - stat->setRefRawData(sensors); - stat->setLoopClosureId(msg->loopClosureId); - sensors.clear(); - for(unsigned int i=0; iinfoEx.loopRawData.size(); ++i) + + if(msg->refImage.data.size()) { - Sensor s(fromCvMatMsgToCvMat(msg->infoEx.loopRawData[i].matrix), (Sensor::Type)msg->infoEx.loopRawData[i].type); - if(s.data().total()) - { - sensors.push_back(s); - } + stat->setRefImage(cv_bridge::toCvShare(msg->refImage, msg)->image.clone()); + } + if(msg->loopImage.data.size()) + { + stat->setLoopImage(cv_bridge::toCvShare(msg->loopImage, msg)->image.clone()); } - stat->setLoopClosureRawData(sensors); //Posterior, likelihood, childCount std::map mapIntFloat; - for(unsigned int i=0; iinfoEx.posteriorKeys.size() && iinfoEx.posteriorValues.size(); ++i) + for(unsigned int i=0; iposteriorKeys.size() && iposteriorValues.size(); ++i) { - mapIntFloat.insert(std::pair(msg->infoEx.posteriorKeys.at(i), msg->infoEx.posteriorValues.at(i))); + mapIntFloat.insert(std::pair(msg->posteriorKeys.at(i), msg->posteriorValues.at(i))); } stat->setPosterior(mapIntFloat); mapIntFloat.clear(); - for(unsigned int i=0; iinfoEx.likelihoodKeys.size() && iinfoEx.likelihoodValues.size(); ++i) + for(unsigned int i=0; ilikelihoodKeys.size() && ilikelihoodValues.size(); ++i) { - mapIntFloat.insert(std::pair(msg->infoEx.likelihoodKeys.at(i), msg->infoEx.likelihoodValues.at(i))); + mapIntFloat.insert(std::pair(msg->likelihoodKeys.at(i), msg->likelihoodValues.at(i))); } stat->setLikelihood(mapIntFloat); std::map mapIntInt; - for(unsigned int i=0; iinfoEx.weightsKeys.size() && iinfoEx.weightsValues.size(); ++i) + for(unsigned int i=0; iweightsKeys.size() && iweightsValues.size(); ++i) { - mapIntInt.insert(std::pair(msg->infoEx.weightsKeys.at(i), msg->infoEx.weightsValues.at(i))); + mapIntInt.insert(std::pair(msg->weightsKeys.at(i), msg->weightsValues.at(i))); } stat->setWeights(mapIntInt); //SURF stuff... std::multimap mapIntKeypoint; - for(unsigned int i=0; iinfoEx.refWordsKeys.size() && iinfoEx.refWordsValues.size(); ++i) + for(unsigned int i=0; irefWordsKeys.size() && irefWordsValues.size(); ++i) { cv::KeyPoint pt; - pt.angle = msg->infoEx.refWordsValues.at(i).angle; - pt.response = msg->infoEx.refWordsValues.at(i).response; - pt.pt.x = msg->infoEx.refWordsValues.at(i).ptx; - pt.pt.y = msg->infoEx.refWordsValues.at(i).pty; - pt.size = msg->infoEx.refWordsValues.at(i).size; - mapIntKeypoint.insert(std::pair(msg->infoEx.refWordsKeys.at(i), pt)); + pt.angle = msg->refWordsValues.at(i).angle; + pt.response = msg->refWordsValues.at(i).response; + 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(msg->refWordsKeys.at(i), pt)); } stat->setRefWords(mapIntKeypoint); mapIntKeypoint.clear(); - for(unsigned int i=0; iinfoEx.loopWordsKeys.size() && iinfoEx.loopWordsValues.size(); ++i) + for(unsigned int i=0; iloopWordsKeys.size() && iloopWordsValues.size(); ++i) { cv::KeyPoint pt; - pt.angle = msg->infoEx.loopWordsValues.at(i).angle; - pt.response = msg->infoEx.loopWordsValues.at(i).response; - pt.pt.x = msg->infoEx.loopWordsValues.at(i).ptx; - pt.pt.y = msg->infoEx.loopWordsValues.at(i).pty; - pt.size = msg->infoEx.loopWordsValues.at(i).size; - mapIntKeypoint.insert(std::pair(msg->infoEx.loopWordsKeys.at(i), pt)); + pt.angle = msg->loopWordsValues.at(i).angle; + pt.response = msg->loopWordsValues.at(i).response; + 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(msg->loopWordsKeys.at(i), pt)); } stat->setLoopWords(mapIntKeypoint); - //Actions - std::list actuators; - for(unsigned int i=0; iactuators.size(); ++i) - { - Actuator a(fromCvMatMsgToCvMat(msg->actuators[i].matrix), (Actuator::Type)msg->actuators[i].type); - if(a.data().total()) - { - actuators.push_back(a); - } - } - stat->setActuators(actuators); - // Statistics data - for(unsigned int i=0; iinfoEx.statsKeys.size() && iinfoEx.statsValues.size(); i++) + for(unsigned int i=0; istatsKeys.size() && istatsValues.size(); i++) { - stat->addStatistic(msg->infoEx.statsKeys.at(i), msg->infoEx.statsValues.at(i)); + stat->addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i)); } ROS_INFO("Publishing statistics..."); UEventsManager::post(new rtabmap::RtabmapEvent(&stat)); } -void GuiWrapper::velocityReceivedCallback(const geometry_msgs::TwistStampedConstPtr & msg) -{ - cv::Mat data = cv::Mat(1, 6, CV_32F); - data.at(0) = (float)msg->twist.linear.x; - data.at(1) = (float)msg->twist.linear.y; - data.at(2) = (float)msg->twist.linear.z; - data.at(3) = (float)msg->twist.angular.x; - data.at(4) = (float)msg->twist.angular.y; - data.at(5) = (float)msg->twist.angular.z; - - std::list actuators; - actuators.push_back(Actuator(data, rtabmap::Actuator::kTypeTwist)); - this->post(new rtabmap::SensorimotorEvent(std::list(), actuators)); -} - void GuiWrapper::handleEvent(UEvent * anEvent) { if(anEvent->getClassName().compare("ParamEvent") == 0) diff --git a/rtabmap/src/GuiWrapper.h b/rtabmap/src/GuiWrapper.h index 72e76d42..9d026b06 100644 --- a/rtabmap/src/GuiWrapper.h +++ b/rtabmap/src/GuiWrapper.h @@ -9,8 +9,7 @@ #define GUIWRAPPER_H_ #include -#include "rtabmap/RtabmapInfo.h" -#include "rtabmap/RtabmapInfoEx.h" +#include "rtabmap/InfoEx.h" #include "utilite/UEventsHandler.h" #include @@ -33,12 +32,10 @@ protected: virtual void handleEvent(UEvent * anEvent); private: - void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg); - void velocityReceivedCallback(const geometry_msgs::TwistStampedConstPtr & msg); + void infoExReceivedCallback(const rtabmap::InfoExConstPtr & infoMsg); private: - ros::Subscriber infoTopic_; - ros::Subscriber velocity_sub_; + ros::Subscriber infoExTopic_; QApplication * app_; rtabmap::MainWindow * mainWindow_; diff --git a/rtabmap/src/ImageAudioInputNode.cpp b/rtabmap/src/ImageAudioInputNode.cpp deleted file mode 100644 index c8e313ef..00000000 --- a/rtabmap/src/ImageAudioInputNode.cpp +++ /dev/null @@ -1,90 +0,0 @@ -/* - * InputNode.cpp - * - * Created on: 2012-05-27 - * Author: mathieu - */ - -#include -#include -#include -#include -#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h" -#include "rtabmap/Sensorimotor.h" -#include -#include -#include "MsgConversion.h" - -class ImageAudioInput -{ -public: - ImageAudioInput(ros::NodeHandle n) : - n_(n), - image_sub(n_, "image", 1), - audio_sub(n_, "audioFrameFreqSqrdMagn", 1), - sync(MySyncPolicy(10), image_sub, audio_sub) - { - sync.registerCallback(boost::bind(&ImageAudioInput::imageAudioCallback, this, _1, _2)); - sensorimotor_pub_ = n_.advertise("sensorimotor",1); - } - - void imageAudioCallback( - const sensor_msgs::ImageConstPtr& image, - const rtabmap_audio::AudioFrameFreqSqrdMagnConstPtr & audio) - { - if(!image->data.size()) - { - ROS_ERROR("Image is empty..."); - return; - } - if(!audio->data.size()) - { - ROS_ERROR("Audio is empty..."); - return; - } - - // Create a sensorimotor msg - rtabmap::SensorimotorPtr sm(new rtabmap::Sensorimotor()); - sm->header.stamp = ros::Time::now(); - sm->sensors.resize(2); - - //image - cv_bridge::CvImageConstPtr img = cv_bridge::toCvShare(image); - sm->sensors[0].type = rtabmap::Sensor::kTypeImage; - fromCvMatToCvMatMsg(sm->sensors[0].matrix, img->image, false); - - //audio - cv::Mat dataMat(audio->nChannels, audio->frameLength, CV_32F); - memcpy(dataMat.data, audio->data.data(), audio->data.size()*sizeof(float)); - sm->sensors[1].type = rtabmap::Sensor::kTypeAudioFreqSqrdMagn; - fromCvMatToCvMatMsg(sm->sensors[1].matrix, dataMat); - - sensorimotor_pub_.publish(sm); - } - -private: - ros::NodeHandle n_; - - //inputs - message_filters::Subscriber image_sub; - message_filters::Subscriber audio_sub; - - //synchronization stuff - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; - message_filters::Synchronizer sync; - - ros::Publisher sensorimotor_pub_; -}; - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "image_audio_input"); - ros::NodeHandle n; - ImageAudioInput iai(n); - - ros::spin(); - - return 0; -} - diff --git a/rtabmap/src/ImageAudioTwistInputNode.cpp b/rtabmap/src/ImageAudioTwistInputNode.cpp deleted file mode 100644 index 09de9104..00000000 --- a/rtabmap/src/ImageAudioTwistInputNode.cpp +++ /dev/null @@ -1,107 +0,0 @@ -/* - * InputNode.cpp - * - * Created on: 2012-05-27 - * Author: mathieu - */ - - -#include -#include -#include -#include -#include -#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h" -#include "rtabmap/Sensorimotor.h" -#include -#include -#include "MsgConversion.h" - -class ImageAudioTwistInput -{ -public: - ImageAudioTwistInput(ros::NodeHandle n) : - n_(n), - image_sub(n_, "image", 1), - audio_sub(n_, "audioFrameFreqSqrdMagn", 1), - twist_sub(n_, "cmd_vel", 1), - sync(MySyncPolicy(10), image_sub, audio_sub, twist_sub) - { - sync.registerCallback(boost::bind(&ImageAudioTwistInput::callback, this, _1, _2, _3)); - sensorimotor_pub_ = n_.advertise("sensorimotor",1); - } - - void callback( - const sensor_msgs::ImageConstPtr& image, - const rtabmap_audio::AudioFrameFreqSqrdMagnConstPtr & audio, - const geometry_msgs::TwistStampedConstPtr & twist) - { - if(!image->data.size()) - { - ROS_ERROR("Image is empty..."); - return; - } - if(!audio->data.size()) - { - ROS_ERROR("Audio is empty..."); - return; - } - - // Create a sensorimotor msg - cv::Mat data; - rtabmap::SensorimotorPtr sm(new rtabmap::Sensorimotor()); - sm->header.stamp = ros::Time::now(); - sm->sensors.resize(2); - - //image - cv_bridge::CvImageConstPtr img = cv_bridge::toCvShare(image); - sm->sensors[0].type = rtabmap::Sensor::kTypeImage; - fromCvMatToCvMatMsg(sm->sensors[0].matrix, img->image, false); - - //audio - data = cv::Mat(audio->nChannels, audio->frameLength, CV_32F); - memcpy(data.data, audio->data.data(), audio->data.size()*sizeof(float)); - sm->sensors[1].type = rtabmap::Sensor::kTypeAudioFreqSqrdMagn; - fromCvMatToCvMatMsg(sm->sensors[1].matrix, data); - - //twist - sm->actuators.resize(1); - data = cv::Mat(1, 6, CV_32F); - data.at(0) = (float)twist->twist.linear.x; - data.at(1) = (float)twist->twist.linear.y; - data.at(2) = (float)twist->twist.linear.z; - data.at(3) = (float)twist->twist.angular.x; - data.at(4) = (float)twist->twist.angular.y; - data.at(5) = (float)twist->twist.angular.z; - sm->actuators[0].type = rtabmap::Actuator::kTypeTwist; - fromCvMatToCvMatMsg(sm->actuators[0].matrix, data); - - sensorimotor_pub_.publish(sm); - } - -private: - ros::NodeHandle n_; - - //inputs - message_filters::Subscriber image_sub; - message_filters::Subscriber audio_sub; - message_filters::Subscriber twist_sub; - - //synchronization stuff - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; - message_filters::Synchronizer sync; - - ros::Publisher sensorimotor_pub_; -}; - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "image_audio_twist_input"); - ros::NodeHandle n; - ImageAudioTwistInput iati(n); - - ros::spin(); - - return 0; -} diff --git a/rtabmap/src/ImageTwistInputNode.cpp b/rtabmap/src/ImageTwistInputNode.cpp deleted file mode 100644 index 6cbdb1a2..00000000 --- a/rtabmap/src/ImageTwistInputNode.cpp +++ /dev/null @@ -1,94 +0,0 @@ -/* - * InputNode.cpp - * - * Created on: 2012-05-27 - * Author: mathieu - */ - - -#include -#include -#include -#include -#include -#include "rtabmap/Sensorimotor.h" -#include -#include -#include "MsgConversion.h" - -class ImageAudioTwistInput -{ -public: - ImageAudioTwistInput(ros::NodeHandle n) : - n_(n), - image_sub(n_, "image", 1), - twist_sub(n_, "cmd_vel", 1), - sync(MySyncPolicy(10), image_sub, twist_sub) - { - sync.registerCallback(boost::bind(&ImageAudioTwistInput::callback, this, _1, _2)); - sensorimotor_pub_ = n_.advertise("sensorimotor",1); - } - - void callback( - const sensor_msgs::ImageConstPtr& image, - const geometry_msgs::TwistStampedConstPtr & twist) - { - if(!image->data.size()) - { - ROS_ERROR("Image is empty..."); - return; - } - - // Create a sensorimotor msg - cv::Mat data; - rtabmap::SensorimotorPtr sm(new rtabmap::Sensorimotor()); - sm->header.stamp = ros::Time::now(); - sm->sensors.resize(2); - - //image - cv_bridge::CvImageConstPtr img = cv_bridge::toCvShare(image); - sm->sensors[0].type = rtabmap::Sensor::kTypeImage; - fromCvMatToCvMatMsg(sm->sensors[0].matrix, img->image, false); - - //twist - sm->actuators.resize(1); - data = cv::Mat(1, 6, CV_32F); - data.at(0) = (float)twist->twist.linear.x; - data.at(1) = (float)twist->twist.linear.y; - data.at(2) = (float)twist->twist.linear.z; - data.at(3) = (float)twist->twist.angular.x; - data.at(4) = (float)twist->twist.angular.y; - data.at(5) = (float)twist->twist.angular.z; - sm->actuators[0].type = rtabmap::Actuator::kTypeTwist; - fromCvMatToCvMatMsg(sm->actuators[0].matrix, data); - sm->sensors[1].type = rtabmap::Sensor::kTypeTwist; - sm->sensors[1].matrix = sm->actuators[0].matrix; - - sensorimotor_pub_.publish(sm); - } - -private: - ros::NodeHandle n_; - - //inputs - message_filters::Subscriber image_sub; - message_filters::Subscriber twist_sub; - - //synchronization stuff - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; - message_filters::Synchronizer sync; - - ros::Publisher sensorimotor_pub_; -}; - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "image_twist_input"); - ros::NodeHandle n; - ImageAudioTwistInput iati(n); - - ros::spin(); - - return 0; -} diff --git a/rtabmap/src/MsgConversion.h b/rtabmap/src/MsgConversion.h deleted file mode 100644 index bcee4939..00000000 --- a/rtabmap/src/MsgConversion.h +++ /dev/null @@ -1,68 +0,0 @@ -/* - * MsgConversion.h - * - * Created on: 2012-05-27 - * Author: mathieu - */ - -#ifndef MSGCONVERSION_H_ -#define MSGCONVERSION_H_ - -#include -#include -#include "rtabmap/CvMatMsg.h" -#include "rtabmap/SensorMsg.h" -#include "rtabmap/ActuatorMsg.h" -#include - -using namespace rtabmap; - -cv::Mat fromCvMatMsgToCvMat(const CvMatMsg & matrixMsg) -{ - cv::Mat data; - if(matrixMsg.compressed) - { - data = cv::imdecode(matrixMsg.data, -1); - if(data.cols != (int)matrixMsg.width || data.rows != (int)matrixMsg.height) - { - ROS_ERROR("Uncompressed size (%d/%d) is not %d/%d", data.cols, data.rows, matrixMsg.width, matrixMsg.height); - data = cv::Mat(); - } - } - else - { - data = cv::Mat(matrixMsg.height, matrixMsg.width, matrixMsg.dataType); - if(data.total() * data.elemSize() != matrixMsg.data.size()) - { - ROS_ERROR("Size attributes (total size=%d) is not equal to actual data size (%d)", data.total() * data.elemSize(), matrixMsg.data.size()); - data = cv::Mat(); - } - else - { - memcpy(data.data, matrixMsg.data.data(), matrixMsg.data.size()); - } - } - return data; -} - -void fromCvMatToCvMatMsg(CvMatMsg & matrixMsg, const cv::Mat & matrix, bool compressImage = false) -{ - matrixMsg.width = matrix.cols; - matrixMsg.height = matrix.rows; - matrixMsg.dataType = matrix.type(); - if(compressImage) - { - // compress images - matrixMsg.compressed = true; - cv::imencode(".png", matrix, matrixMsg.data); - } - else - { - matrixMsg.data.resize(matrix.total()*matrix.elemSize()); - memcpy(matrixMsg.data.data(), matrix.data, matrixMsg.data.size()); - matrixMsg.compressed = false; - } -} - - -#endif /* MSGCONVERSION_H_ */ diff --git a/rtabmap/src/OutputNode.cpp b/rtabmap/src/OutputNode.cpp deleted file mode 100644 index fb32c944..00000000 --- a/rtabmap/src/OutputNode.cpp +++ /dev/null @@ -1,78 +0,0 @@ -/* - * CameraNode.cpp - * - * Author: labm2414 - */ - -#include -#include -#include "rtabmap/RtabmapInfo.h" -#include "rtabmap/RtabmapInfoEx.h" -#include -#include -#include - -ros::Publisher rosPublisher; -bool statsLogged = true; -const char * statsFileName = "OuputStats.txt"; - -void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg) -{ - for(unsigned int i=0; iactuators.size(); ++i) - { - if(msg->actuators[i].type == rtabmap::Actuator::kTypeTwist) - { - if((msg->actuators[i].matrix.dataType & CV_32F) && msg->actuators[i].matrix.data.size()/sizeof(float) == 6) - { - geometry_msgs::TwistPtr vel(new geometry_msgs::Twist()); - float * twist = (float *)msg->actuators[i].matrix.data.data(); - vel->linear.x = twist[0]; - vel->linear.y = twist[1]; - vel->linear.z = twist[2]; - vel->angular.x = twist[3]; - vel->angular.y = twist[4]; - vel->angular.z = twist[5]; - UINFO("%f %f %f", vel->linear.x, vel->linear.y, vel->angular.z); - rosPublisher.publish(vel); - } - else - { - ROS_ERROR("Twist format is wrong..."); - } - } - } -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "rtabmap_out"); - ros::NodeHandle nh("~"); - - if(statsLogged) - { - ULogger::setPrintWhere(false); - ULogger::setBuffered(true); - ULogger::setPrintLevel(false); - ULogger::setPrintTime(false); - ULogger::setType(ULogger::kTypeFile, statsFileName, false); - ROS_INFO("stats log file = \"%s\"", statsFileName); - } - else - { - ULogger::setLevel(ULogger::kError); - } - - nh = ros::NodeHandle(); - ros::Subscriber infoTopic; - ros::Subscriber infoExTopic; - infoTopic = nh.subscribe("rtabmap/info", 1, infoReceivedCallback); - rosPublisher = nh.advertise("rtabmap/cmd_vel", 1); - - ros::spin(); - - if(statsLogged) - { - ULogger::flush(); - } - return 0; -} diff --git a/rtabmap/src/Twist2Pose.cpp b/rtabmap/src/Twist2Pose.cpp deleted file mode 100644 index ce85340d..00000000 --- a/rtabmap/src/Twist2Pose.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* - * TwistToPoses.cpp - * - * Created on: 2011-11-30 - * Author: matlab - */ - -#include -#include -#include -#include - -ros::Publisher rosPublisherLinear; -ros::Publisher rosPublisherAngular; - -void twistReceivedCallback(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); - - geometry_msgs::PoseStamped msgLinear; - geometry_msgs::PoseStamped msgAngular; - - if(fabs(msg->linear.x) < 0.0001 && fabs(msg->linear.y) < 0.0001) - { - msgLinear.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 3.14159/2, 0); - } - else - { - msgLinear.pose.orientation = tf::createQuaternionMsgFromYaw(std::atan2(msg->linear.y, msg->linear.x)); - } - if(fabs(msg->angular.z) < 0.0001) - { - msgAngular.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, -3.14159/2, 0); - } - else - { - msgAngular.pose.orientation = tf::createQuaternionMsgFromYaw(msg->angular.z); - } - msgLinear.header.frame_id = "/base_link"; - msgAngular.header.frame_id = "/base_link"; - msgLinear.header.stamp = ros::Time::now(); - msgAngular.header.stamp = ros::Time::now(); - rosPublisherLinear.publish(msgLinear); - rosPublisherAngular.publish(msgAngular); -} - -int main(int argc, char * argv[]) -{ - ros::init(argc, argv, "twist_to_poses"); - - ros::NodeHandle nh; - rosPublisherLinear = nh.advertise("pose_linear", 1); - rosPublisherAngular = nh.advertise("pose_angular", 1); - ros::Subscriber twist_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback); - - ros::spin(); - - return 0; -} diff --git a/rtabmap/src/Twist2TwistStamped.cpp b/rtabmap/src/Twist2TwistStamped.cpp deleted file mode 100644 index 60b3ab2f..00000000 --- a/rtabmap/src/Twist2TwistStamped.cpp +++ /dev/null @@ -1,35 +0,0 @@ -/* - * TwistToPoses.cpp - * - * Created on: 2011-11-30 - * Author: matlab - */ - -#include -#include -#include - -ros::Publisher pub; - -void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg) -{ - geometry_msgs::TwistStamped twistStamped; - - twistStamped.twist = *msg; - twistStamped.header.frame_id = "/base_link"; - twistStamped.header.stamp = ros::Time::now(); - pub.publish(twistStamped); -} - -int main(int argc, char * argv[]) -{ - ros::init(argc, argv, "twist_to_twist_stamped"); - - ros::NodeHandle nh; - pub = nh.advertise("cmd_vel_stamped", 1); - ros::Subscriber twist_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback); - - ros::spin(); - - return 0; -} diff --git a/rtabmap/src/VisualAttentionNode.cpp b/rtabmap/src/VisualAttentionNode.cpp deleted file mode 100644 index dc0bd158..00000000 --- a/rtabmap/src/VisualAttentionNode.cpp +++ /dev/null @@ -1,507 +0,0 @@ -/* - * VisualAttentionNode.cpp - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -image_transport::Publisher rosPublisherMotionGlobal; -image_transport::Publisher rosPublisherMotionLocal; -image_transport::Publisher rosPublisherLocal; -image_transport::Publisher rosPublisherMotionLocalPolar; -image_transport::Publisher rosPublisherMotionLocalPolarReconstructed; -image_transport::Publisher rosPublisherLocalPolar; -image_transport::Publisher rosPublisherLocalPolarReconstructed; -image_transport::Publisher rosPublisherWithRoi; -UPlotCurve * g_curveR = 0; -UPlotCurve * g_curveG = 0; -UPlotCurve * g_curveB = 0; - -UPlotCurve * g_curveRRatio = 0; -UPlotCurve * g_curveGRatio = 0; -UPlotCurve * g_curveBRatio = 0; - -QSpinBox * spinX = 0; -QSpinBox * spinY = 0; -QCheckBox * polarCheckBox = 0; - -#define ROI_RATIO 4 // default 8 -#define POLAR_RAYS 128 -#define POLAR_RINGS 64 -cv::Mat previousGlobalImage; -cv::Mat previousPolarROI; -cv::Rect roi; -float ratio = 0.2; -bool attentionDisabled = false; // may set ROI_RATIO=1 if true -bool localAttention = false; - -void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg) -{ - if(msg->data.size()) - { - cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg); - - if(ptr->image.depth() == CV_8U && ptr->image.channels() == 3) - { - cv::Mat motion = ptr->image.clone(); - if(previousGlobalImage.cols == ptr->image.cols && previousGlobalImage.rows == ptr->image.rows) - { - unsigned char * imageData = (unsigned char *)motion.data; - unsigned char * previous_imageData = (unsigned char *)previousGlobalImage.data; - int widthStep = motion.cols * motion.elemSize(); - cv::Point2i centerROILocal(roi.width/2, roi.height/2); - cv::Point2i centerROIGlobal(roi.x+roi.width/2, roi.y+roi.height/2); - cv::Point2i nearestMovingPixel; - - for(int j=0; j=ratio || fabs(g-previous_g)/256.0f >= ratio || fabs(r-previous_r)/256.0f >= ratio)) - { - imageData[j*widthStep+i*3+0] = 0; - imageData[j*widthStep+i*3+1] = 0; - imageData[j*widthStep+i*3+2] = 0; - } - } - } - - // Motion ROI - cv::Mat motionROI = cv::Mat(motion, roi); - if(rosPublisherMotionLocal.getNumSubscribers()) - { - cv_bridge::CvImage img; - img.header.frame_id = msg->header.frame_id; - img.header.stamp = msg->header.stamp; - img.encoding = sensor_msgs::image_encodings::BGR8; - img.image = motionROI; - rosPublisherMotionLocal.publish(img.toImageMsg()); - } - - /*{ - int radius = motionROI.cols/2; - float M = 64/std::log(radius); - cv::Mat tmpPolar(128, 64, CV_8UC3); - IplImage iplTmpPolar = tmpPolar; - IplImage iplPolar = motionROI; - cvLogPolar( &iplPolar, &iplTmpPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS ); - cvLogPolar( &iplTmpPolar, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP ); - - if(rosPublisherMotionLocalPolar.getNumSubscribers()) - { - cv_bridge::CvImage img; - img.encoding = sensor_msgs::image_encodings::BGR8; - img.image = motionROI; - rosPublisherMotionLocalPolar.publish(img.toImageMsg()); - } - }*/ - - - // Motion ROI polar - { - cv::Mat imageROI = cv::Mat(ptr->image, roi); - int radius = imageROI.cols/2; - float M = POLAR_RINGS/std::log(radius); - cv::Mat polarROI(POLAR_RAYS, POLAR_RINGS, CV_8UC3); - IplImage iplPolar = polarROI; - IplImage iplImageROI = imageROI; - cvLogPolar( &iplImageROI, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS ); - cv::Mat motionPolarROI = polarROI.clone(); - if(previousPolarROI.cols == motionPolarROI.cols && previousPolarROI.rows == motionPolarROI.rows) - { - unsigned char * motionPolarData = motionPolarROI.data; - unsigned char * previousPolarData = previousPolarROI.data; - widthStep = motionPolarROI.cols * motionPolarROI.elemSize(); - for(int j=0; j=ratio || fabs(g-previous_g)/256.0f >= ratio || fabs(r-previous_r)/256.0f >= ratio)) - { - motionPolarData[j*widthStep+i*3+0] = 0; - motionPolarData[j*widthStep+i*3+1] = 0; - motionPolarData[j*widthStep+i*3+2] = 0; - } - } - } - - if(rosPublisherLocalPolar.getNumSubscribers()) - { - cv_bridge::CvImage img; - img.header.frame_id = msg->header.frame_id; - img.header.stamp = msg->header.stamp; - img.encoding = sensor_msgs::image_encodings::BGR8; - img.image = polarROI; - rosPublisherLocalPolar.publish(img.toImageMsg()); - } - - if(rosPublisherLocalPolarReconstructed.getNumSubscribers()) - { - cv::Mat reconstructedROI(imageROI.rows, imageROI.cols, imageROI.type()); - IplImage iplPolarROI = polarROI; - IplImage iplReconstructedROI = reconstructedROI; - cvLogPolar( &iplPolarROI, &iplReconstructedROI, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP ); - - cv_bridge::CvImage img; - img.header.frame_id = msg->header.frame_id; - img.header.stamp = msg->header.stamp; - img.encoding = sensor_msgs::image_encodings::BGR8; - img.image = reconstructedROI; - rosPublisherLocalPolarReconstructed.publish(img.toImageMsg()); - } - - if(rosPublisherMotionLocalPolar.getNumSubscribers()) - { - cv_bridge::CvImage img; - img.header.frame_id = msg->header.frame_id; - img.header.stamp = msg->header.stamp; - img.encoding = sensor_msgs::image_encodings::BGR8; - img.image = motionPolarROI; - rosPublisherMotionLocalPolar.publish(img.toImageMsg()); - } - - //reconstruct image - cv::Mat reconstructedROI(imageROI.rows, imageROI.cols, imageROI.type()); - IplImage iplMotionPolarROI = motionPolarROI; - IplImage iplReconstructedROI = reconstructedROI; - cvLogPolar( &iplMotionPolarROI, &iplReconstructedROI, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP ); - - if(rosPublisherMotionLocalPolarReconstructed.getNumSubscribers()) - { - cv_bridge::CvImage img; - img.header.frame_id = msg->header.frame_id; - img.header.stamp = msg->header.stamp; - img.encoding = sensor_msgs::image_encodings::BGR8; - img.image = reconstructedROI; - rosPublisherMotionLocalPolarReconstructed.publish(img.toImageMsg()); - } - - if(localAttention) - { - float distNearest = 0.0f; - unsigned char * reconstructedData = reconstructedROI.data; - widthStep = reconstructedROI.cols * reconstructedROI.elemSize(); - for(int j=0; jimage; - if(polarCheckBox->isChecked()) - { - ref = reconstructedROI; - } - - if(spinX->value()<0 || spinX->value() > ref.cols) - { - spinX->setValue(ref.cols/2); - } - if(spinY->value()<0 || spinY->value() > ref.rows) - { - spinY->setValue(ref.rows/2); - } - - //take pixel (OpenCV is BGR) - float r = (float)*(ref.data + (spinX->value())*ref.elemSize() + (spinY->value())*ref.elemSize()*ref.cols + 2); - float g = (float)*(ref.data + (spinX->value())*ref.elemSize() + (spinY->value())*ref.elemSize()*ref.cols + 1); - float b = (float)*(ref.data + (spinX->value())*ref.elemSize() + (spinY->value())*ref.elemSize()*ref.cols + 0); - if(g_curveR->itemsSize()) - { - float lastR = g_curveR->getItemData(g_curveR->itemsSize()-1).y(); - float lastG = g_curveG->getItemData(g_curveG->itemsSize()-1).y(); - float lastB = g_curveB->getItemData(g_curveB->itemsSize()-1).y(); - QMetaObject::invokeMethod(g_curveRRatio, "addValue", Q_ARG(float, fabs(r-lastR)/255.0f) ); - QMetaObject::invokeMethod(g_curveGRatio, "addValue", Q_ARG(float, fabs(g-lastG)/255.0f) ); - QMetaObject::invokeMethod(g_curveBRatio, "addValue", Q_ARG(float, fabs(b-lastB)/255.0f) ); - } - - QMetaObject::invokeMethod(g_curveR, "addValue", Q_ARG(float, (float)r)); - QMetaObject::invokeMethod(g_curveG, "addValue", Q_ARG(float, (float)g)); - QMetaObject::invokeMethod(g_curveB, "addValue", Q_ARG(float, (float)b)); - } - } - - } - - if(!localAttention) - { - //global, may be outside of the ROI - float distNearest = 0.0f; - unsigned char * data = motion.data; - widthStep = motion.cols * motion.elemSize(); - for(int j=0; j roi.x && - nearestMovingPixel.xroi.width/2 && - nearestMovingPixel.ximage.cols - roi.width/2) - { - roi.x = nearestMovingPixel.x-roi.width/2; - } - if( nearestMovingPixel.y>roi.y && - nearestMovingPixel.yroi.height/2 && - nearestMovingPixel.yimage.rows - roi.height/2) - { - roi.y = nearestMovingPixel.y-roi.height/2; - } - } - else - { - ROS_INFO("nearestMovingPixel center(global)=(%d,%d) nearest(global)=(%d,%d)", - centerROIGlobal.x, - centerROIGlobal.y, - nearestMovingPixel.x, - nearestMovingPixel.y); - if( nearestMovingPixel.x>roi.width/2 && - nearestMovingPixel.ximage.cols - roi.width/2) - { - roi.x = nearestMovingPixel.x-roi.width/2; - } - else if(nearestMovingPixel.x>=ptr->image.cols - roi.width/2) - { - roi.x = ptr->image.cols - roi.width - 1; - } - else - { - roi.x =0; - } - if( nearestMovingPixel.y>roi.height/2 && - nearestMovingPixel.yimage.rows - roi.height/2) - { - roi.y = nearestMovingPixel.y-roi.height/2; - } - else if(nearestMovingPixel.y>=ptr->image.rows - roi.height/2) - { - roi.y = ptr->image.rows - roi.height - 1; - } - else - { - roi.y = 0; - } - } - } - cv::Mat newImageROI = cv::Mat(ptr->image, roi); - int radius = newImageROI.cols/2; - float M = POLAR_RINGS/std::log(radius); - previousPolarROI = cv::Mat(POLAR_RAYS, POLAR_RINGS, CV_8UC3); - IplImage iplPolar = previousPolarROI; - IplImage iplImageROI = newImageROI; - cvLogPolar( &iplImageROI, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS ); - } - else - { - int size = ptr->image.cols < ptr->image.rows?ptr->image.cols/ROI_RATIO: ptr->image.rows/ROI_RATIO; - int x = sizeimage.cols?(ptr->image.cols-size)/2:0; - int y = sizeimage.rows?(ptr->image.rows-size)/2:0; - roi = cv::Rect(x, y, size, size); - } - previousGlobalImage = ptr->image.clone(); - - ROS_INFO("polar ROI (%d,%d,%d,%d)", roi.x, roi.y, roi.width, roi.height); - cv::Mat localImage = cv::Mat(ptr->image, roi).clone(); - - /*int radius = polarImage.cols/2; - float M = 64/std::log(radius); - ROS_INFO("src size=(%d,%d) radius=%d, M=%f", polarImage.cols, polarImage.rows, radius, M); - cv::Mat tmpPolar(128, 64, CV_8UC3); - IplImage iplTmpPolar = tmpPolar; - IplImage iplPolar = polarImage; - cvLogPolar( &iplPolar, &iplTmpPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS ); - cvLogPolar( &iplTmpPolar, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP ); -*/ - - if(rosPublisherLocal.getNumSubscribers()) - { - cv_bridge::CvImage img; - img.header.frame_id = msg->header.frame_id; - img.header.stamp = msg->header.stamp; - img.encoding = sensor_msgs::image_encodings::BGR8; - img.image = localImage; - rosPublisherLocal.publish(img.toImageMsg()); - } - - - if(rosPublisherMotionGlobal.getNumSubscribers()) - { - cv_bridge::CvImage img; - img.header.frame_id = msg->header.frame_id; - img.header.stamp = msg->header.stamp; - cv::rectangle(motion, cv::Point2f(roi.x, roi.y), cv::Point2f(roi.x+roi.width, roi.y+roi.height), cv::Scalar(0, 255, 0), 1); - img.encoding = sensor_msgs::image_encodings::BGR8; - img.image = motion; - rosPublisherMotionGlobal.publish(img.toImageMsg()); - } - - if(rosPublisherWithRoi.getNumSubscribers()) - { - cv::Mat imageWithRoi = ptr->image.clone(); - cv::rectangle(imageWithRoi, cv::Point2f(roi.x, roi.y), cv::Point2f(roi.x+roi.width, roi.y+roi.height), cv::Scalar(0, 255, 0), 1); - cv_bridge::CvImage img; - img.header.frame_id = msg->header.frame_id; - img.header.stamp = msg->header.stamp; - img.encoding = sensor_msgs::image_encodings::BGR8; - img.image = imageWithRoi; - rosPublisherWithRoi.publish(img.toImageMsg()); - } - - } - } -} - -void my_handler(int s){ - QApplication::closeAllWindows(); - QApplication::exit(); -} - -int main(int argc, char * argv[]) -{ - ros::init(argc, argv, "visual_attention"); - - ros::NodeHandle n; - image_transport::ImageTransport it(n); - image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback); - rosPublisherMotionGlobal = it.advertise("image_motion", 1); - rosPublisherMotionLocal = it.advertise("image_motion_local", 1); - rosPublisherLocal = it.advertise("image_local", 1); - rosPublisherMotionLocalPolar = it.advertise("image_motion_local_polar", 1); - rosPublisherMotionLocalPolarReconstructed = it.advertise("image_motion_local_polar_reconstructed", 1); - rosPublisherLocalPolar = it.advertise("image_local_polar", 1); - rosPublisherLocalPolarReconstructed = it.advertise("image_local_polar_reconstructed", 1); - rosPublisherWithRoi = it.advertise("image_with_roi", 1); - - bool show_gui = true; - if(show_gui) - { - QApplication app(argc, argv); - - QWidget widget; - widget.setLayout(new QVBoxLayout()); - spinX = new QSpinBox(&widget); - spinY = new QSpinBox(&widget); - polarCheckBox = new QCheckBox("Polar", &widget); - polarCheckBox->setChecked(false); - QHBoxLayout * hLayout = new QHBoxLayout(); - hLayout->addWidget(spinX); - hLayout->addWidget(spinY); - hLayout->addWidget(polarCheckBox); - widget.layout()->addItem(hLayout); - UPlot * plot = new UPlot(&widget); - plot->setWindowTitle("Pixel value"); - plot->keepAllData(false); - plot->setMaxVisibleItems(50); - plot->setXLabel("Time (s)"); - g_curveR = plot->addCurve("R", Qt::red); - g_curveG = plot->addCurve("G", Qt::green); - g_curveB = plot->addCurve("B", Qt::blue); - plot->setMinimumSize(600, 400); - widget.layout()->addWidget(plot); - widget.show(); - - UPlot plotRatio; - plotRatio.setWindowTitle("Pixel ratio"); - plotRatio.keepAllData(false); - plotRatio.setMaxVisibleItems(50); - plotRatio.setXLabel("Time (s)"); - g_curveRRatio = plotRatio.addCurve("R", Qt::red); - g_curveGRatio = plotRatio.addCurve("G", Qt::green); - g_curveBRatio = plotRatio.addCurve("B", Qt::blue); - plotRatio.setMinimumSize(600, 400); - plotRatio.show(); - - // Catch ctrl-c to close the gui - // (Place this after QApplication's constructor) - struct sigaction sigIntHandler; - sigIntHandler.sa_handler = my_handler; - sigemptyset(&sigIntHandler.sa_mask); - sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); - - ros::AsyncSpinner spinner(4); // Use 4 threads - spinner.start(); - app.exec(); - spinner.stop(); - } - else - { - ros::spin(); - } - - return 0; -} diff --git a/rtabmap_audio/AudioPlayerNode.cpp b/rtabmap_audio/AudioPlayerNode.cpp deleted file mode 100644 index 1d764a5f..00000000 --- a/rtabmap_audio/AudioPlayerNode.cpp +++ /dev/null @@ -1,389 +0,0 @@ -/* - * AudioPlayerNode.cpp - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "rtabmap_audio/AudioFrame.h" -#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h" - -#include -#include - -#define DEFAULT_GUI_USED true -#define PLOT_SAMPLING_RATIO 64 - -// Ring buffer -unsigned int g_readPtr = 0; -unsigned int g_writePtr = 0; -std::vector g_ringBuffer; // interleaved audio -unsigned int g_readingLooped = 0; -unsigned int g_writingLooped = 0; -bool g_warn = true; - -// Fmod stuff -FMOD::System *g_system = 0; -FMOD::Sound *g_sound = 0; -FMOD::Channel *g_channel = 0; -bool initialized = false; - -// Display stuff -UPlot * g_plot = 0; -UPlotCurve * g_curveReceiving = 0; -UPlotCurve * g_curvePlaying = 0; -USpectrogram * g_spectrogram = 0; - -void ERRCHECK(FMOD_RESULT result) -{ - if (result != FMOD_OK) - { - ROS_ERROR("FMOD error! (%d) %s\n", result, FMOD_ErrorString(result)); - exit(-1); - } -} - -void updateCurve(UPlotCurve * curve, void * data, unsigned int dataSize, int channels, int sampleSize) -{ - if(!curve || !data || dataSize == 0) - { - return; - } - //ROS_INFO("channels=%d, bitsPerSample=%d", channels, sampleSize); - int downSamplingFactor = PLOT_SAMPLING_RATIO*channels; - QVector v(dataSize/downSamplingFactor/sampleSize); - //ROS_INFO("dataSize=%d downSamplingFactor = %d, v=%d", dataSize, downSamplingFactor, v.size()); - if(sampleSize == 1) - { - char * p = (char*)data; - char min,max; - for(int i=0; i abs(max) ? min : max; - } - } - else if(sampleSize == 2) - { - short * p = (short*)data; - short min,max; - for(int i=0; i abs(max) ? min : max; - } - } - else if(sampleSize == 4) - { - int * p = (int*)data; - int min,max; - for(int i=0; i abs(max) ? min : max; - } - } - QMetaObject::invokeMethod(curve, "addValues", Q_ARG(QVector, v)); -} - -FMOD_RESULT F_CALLBACK pcmreadcallback(FMOD_SOUND *sound, void *data, unsigned int datalen) -{ - //ROS_INFO("datalen=%d, g_readPtr=%d", datalen, g_readPtr); - unsigned int count; - char * buffer = (char *)data; - - bool okToCpy = false; - - if(g_readPtr < g_writePtr) - { - okToCpy = true; - } - else if(g_readingLooped < g_writingLooped) - { - okToCpy = true; - } - - if(okToCpy) - { - g_warn = true; - unsigned int start = g_readPtr; - for(count=0; countisVisible() && g_curvePlaying) - { - int channels, bitsPerSample; - FMOD_Sound_GetFormat(sound, 0, 0, &channels, &bitsPerSample); - updateCurve(g_curvePlaying, &g_ringBuffer[start], datalen, channels, bitsPerSample/8); - } - //ROS_INFO("datalen=%d, writePtr=%d, readPtr=%d", datalen, g_writePtr, g_readPtr); - } - else if(g_warn) - { - g_warn = false; - ROS_WARN("Empty buffer : stream down? (this warning is shown only one time)"); - - //fill data with zeros - for(unsigned int i=0; igetVersion(&version); - ERRCHECK(result); - - if (version < FMOD_VERSION) - { - printf("Error! You are using an old version of FMOD %08x. This program requires %08x\n", version, FMOD_VERSION); - return false; - } - - result = g_system->init(32, FMOD_INIT_NORMAL, 0); - ERRCHECK(result); - - memset(&createsoundexinfo, 0, sizeof(FMOD_CREATESOUNDEXINFO)); - createsoundexinfo.cbsize = sizeof(FMOD_CREATESOUNDEXINFO); /* required. */ - createsoundexinfo.length = decoderBufferSize; /* Length of PCM data in bytes of whole song (for Sound::getLength) */ - createsoundexinfo.numchannels = channels; /* Number of channels in the sound. */ - createsoundexinfo.defaultfrequency = fs; /* Default playback rate of sound. */ - createsoundexinfo.decodebuffersize = (decoderBufferSize / bytesPerSample) / channels; /* Chunk size of stream update in samples. This will be the amount of data passed to the user callback. */ - if(bytesPerSample == 1) - { - createsoundexinfo.format = FMOD_SOUND_FORMAT_PCM8; /* Data format of sound. */ - } - else if(bytesPerSample == 2) - { - createsoundexinfo.format = FMOD_SOUND_FORMAT_PCM16; /* Data format of sound. */ - } - else if(bytesPerSample == 4) - { - createsoundexinfo.format = FMOD_SOUND_FORMAT_PCM32; /* Data format of sound. */ - } - else - { - ROS_ERROR("Sample size format (%d) must be 1, 2 or 4", bytesPerSample); - return false; - } - createsoundexinfo.pcmreadcallback = pcmreadcallback; /* User callback for reading. */ - createsoundexinfo.pcmsetposcallback = pcmsetposcallback; /* User callback for seeking. */ - - result = g_system->createSound(0, mode, &createsoundexinfo, &g_sound); - ERRCHECK(result); - - /* - Play the sound. - */ - - result = g_system->playSound(FMOD_CHANNEL_FREE, g_sound, 0, &g_channel); - ERRCHECK(result); - - return true; -} - -void frameReceivedCallback(const rtabmap_audio::AudioFramePtr & msg) -{ - if(msg->data.size()) - { - if(!g_ringBuffer.size()) - { - g_ringBuffer = std::vector(msg->data.size() * 5 * msg->nChannels * msg->sampleSize, 0); - } - - //ROS_INFO("Received audio frame size=(%d), writePtr=%d", msg->data.size(), g_writePtr); - unsigned int start = g_writePtr; - if(g_writePtr + msg->data.size() <= g_ringBuffer.size()) - { - memcpy(g_ringBuffer.data()+g_writePtr, msg->data.data(), msg->data.size()); - g_writePtr += msg->data.size(); - g_writePtr %= g_ringBuffer.size(); - } - else - { - unsigned int size2 = (g_writePtr + msg->data.size()) - g_ringBuffer.size(); - unsigned int size1 = msg->data.size() - size2; - memcpy(g_ringBuffer.data()+g_writePtr, msg->data.data(), size1); - memcpy(g_ringBuffer.data(), msg->data.data() + size1, size2); - g_writePtr = size2; - } - if(g_writePtr < start) - { - ++g_writingLooped; - } - - if(!initialized) - { - //wait for second frame - if(g_writePtr > msg->data.size() * 2) - { - initialized = initAudioPlayer(msg->data.size(), msg->fs, msg->nChannels, msg->sampleSize); - if(!initialized) - { - ROS_ERROR("Cannot initialize the audio player..."); - exit(-1); - } - } - else if(g_plot) - { - if(g_plot->isVisible() && ((msg->data.size() / msg->nChannels) / msg->sampleSize) % PLOT_SAMPLING_RATIO != 0) - { - g_plot->setVisible(false); - ROS_WARN("Frame length (%d) must be a multiple of %d to show audio plot...", ((msg->data.size() / msg->nChannels) / msg->sampleSize), PLOT_SAMPLING_RATIO); - } - else if(g_curveReceiving && g_curvePlaying) - { - QMetaObject::invokeMethod(g_curveReceiving, "setXIncrement", Q_ARG(float, float(PLOT_SAMPLING_RATIO)/float(msg->fs))); - QMetaObject::invokeMethod(g_curvePlaying, "setXIncrement", Q_ARG(float, float(PLOT_SAMPLING_RATIO)/float(msg->fs))); - } - } - } - if(g_plot && g_plot->isVisible() && g_curveReceiving) - { - updateCurve(g_curveReceiving, msg->data.data(), msg->data.size(), msg->nChannels, msg->sampleSize); - } - } -} - -void frameFreqSqrdMagnReceivedCallback(const rtabmap_audio::AudioFrameFreqSqrdMagnPtr & msg) -{ - if(msg->data.size() && msg->nChannels && g_spectrogram && g_spectrogram->isVisible()) - { - QMetaObject::invokeMethod(g_spectrogram, "setSamplingRate", Q_ARG(int, msg->fs)); - std::vector data(msg->data.size()/msg->nChannels); - memcpy(data.data(), msg->data.data(), data.size()*sizeof(float)); // Just copy the first channel TODO support more channels... - QMetaObject::invokeMethod(g_spectrogram, "push", Q_ARG(std::vector, data)); - } -} - -void my_handler(int s){ - QApplication::closeAllWindows(); -} - -int main(int argc, char** argv) -{ - ULogger::setType(ULogger::kTypeConsole); - ULogger::setLevel(ULogger::kDebug); - ros::init(argc, argv, "audioPlayer"); - - ros::NodeHandle nh("~"); - - //Parameter - bool guiUsed = DEFAULT_GUI_USED; - nh.param("gui_used", guiUsed, guiUsed); - ROS_INFO("gui_used=%s", guiUsed?"true":"false"); - - nh = ros::NodeHandle(""); - ros::Subscriber audioSubs = nh.subscribe("audioFrame", 1, frameReceivedCallback); - ros::Subscriber audioFreqSqrdMagnSubs; - - if(guiUsed) - { - QApplication app(argc, argv); - - qRegisterMetaType >("QVector"); - qRegisterMetaType >("std::vector"); - - g_plot = new UPlot(); - g_plot->keepAllData(false); - g_plot->setMaxVisibleItems(1024); - g_plot->setXLabel("Time (s)"); - g_curveReceiving = g_plot->addCurve("Receiving"); // debugging purpose, to see what is received - g_curvePlaying = g_plot->addCurve("Playing"); - g_plot->setMinimumSize(600, 400); - - g_spectrogram = new USpectrogram(); - g_spectrogram->setMinimumSize(640, 480); - - g_spectrogram->show(); - g_plot->show(); - - audioFreqSqrdMagnSubs = nh.subscribe("audioFrameFreqSqrdMagn", 1, frameFreqSqrdMagnReceivedCallback); - - // Catch ctrl-c to close the gui - // (Place this after QApplication's constructor) - struct sigaction sigIntHandler; - sigIntHandler.sa_handler = my_handler; - sigemptyset(&sigIntHandler.sa_mask); - sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); - - ros::AsyncSpinner spinner(4); // Use 4 threads - spinner.start(); - app.exec(); - spinner.stop(); - } - else - { - ros::spin(); - } - - uSleep(100); // make sure all subscribers have terminated - /* - Shut down - */ - FMOD_RESULT result; - if(g_sound) - { - result = g_sound->release(); - ERRCHECK(result); - } - if(g_system) - { - result = g_system->close(); - ERRCHECK(result); - result = g_system->release(); - ERRCHECK(result); - } - - if(g_plot) - { - delete g_plot; - } - if(g_spectrogram) - { - delete g_spectrogram; - } - - return 0; -} diff --git a/rtabmap_audio/AudioRecorderNode.cpp b/rtabmap_audio/AudioRecorderNode.cpp deleted file mode 100644 index 8e7c1a93..00000000 --- a/rtabmap_audio/AudioRecorderNode.cpp +++ /dev/null @@ -1,272 +0,0 @@ -/* - * AudioRecorderNode.cpp - */ - -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include "rtabmap_audio/AudioFrame.h" -#include "rtabmap_audio/AudioFrameFreq.h" -#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h" - -#define DEFAULT_DEVICE_ID 0 -#define DEFAULT_FILE_NAME "" -#define DEFAULT_FRAME_LENGTH 4800 -#define DEFAULT_FS 48000 -#define DEFAULT_SAMPLE_SIZE 2 -#define DEFAULT_CHANNELS 1 - -class EndEvent : public UEvent -{ -public: - virtual std::string getClassName() const {return "EndEvent";} -}; - -class MicroWrapper : public UThreadNode -{ -public: - MicroWrapper(int deviceId, int frameLength, int fs, int sampleSize, int nChannels) - { - micro_ = new rtabmap::Micro(rtabmap::MicroEvent::kTypeFrame, deviceId, fs, frameLength, nChannels, sampleSize, 0); - ros::NodeHandle nh(""); - audioFramePublisher_ = nh.advertise("audioFrame", 1); - audioFrameFreqPublisher_ = nh.advertise("audioFrameFreq", 1); - audioFrameFreqSqrdMagnPublisher_ = nh.advertise("audioFrameFreqSqrdMagn", 1); - } - - MicroWrapper(const std::string & fileName, int frameLength) - { - micro_ = new rtabmap::Micro(rtabmap::MicroEvent::kTypeFrame, fileName, true, frameLength, 0); - ros::NodeHandle nh(""); - audioFramePublisher_ = nh.advertise("audioFrame", 1); - audioFrameFreqPublisher_ = nh.advertise("audioFrameFreq", 1); - audioFrameFreqSqrdMagnPublisher_ = nh.advertise("audioFrameFreqSqrdMagn", 1); - } - - virtual ~MicroWrapper() - { - this->join(true); - micro_->join(true); - delete micro_; - } - - bool init() - { - if(micro_) - { - return micro_->init(); - } - return false; - } - - unsigned int freq() const - { - if(micro_) - { - return micro_->fs(); - } - return 0; - } - - unsigned int sampleSize() const - { - return micro_->bytesPerSample(); - } - - unsigned int nChannels() const - { - return micro_->channels(); - } - -protected: - virtual void mainLoopBegin() - { - micro_->startRecorder(); - } - - virtual void mainLoop() - { - if(!micro_) - { - UERROR("micro_ is not initialized"); - this->kill(); - } - bool computeFFT = false; - if(audioFrameFreqPublisher_.getNumSubscribers() || audioFrameFreqSqrdMagnPublisher_.getNumSubscribers()) - { - computeFFT = true; - } - - cv::Mat data; - cv::Mat freq; - if(!computeFFT) - { - data = micro_->getFrame(); - } - else - { - data = micro_->getFrame(freq, false); - } - if(!data.empty()) - { - ros::Time now = ros::Time::now(); - if(audioFramePublisher_.getNumSubscribers()) - { - rtabmap_audio::AudioFramePtr msg(new rtabmap_audio::AudioFrame); - msg->header.frame_id = "micro"; - msg->header.stamp = now; - msg->data.resize(data.total()*data.elemSize()); - // Interleave the data - for(unsigned int i=0; idata.size(); i+=data.elemSize()*data.rows) - { - for(int j=0; jdata.data()+i+j*data.elemSize(), data.data + (i/(data.elemSize()*data.rows))*data.elemSize() + j*data.cols*data.elemSize(), data.elemSize()); - } - } - msg->frameLength = data.cols; - msg->fs = micro_->fs(); - msg->nChannels = data.rows; - msg->sampleSize = data.elemSize(); - audioFramePublisher_.publish(msg); - } - if(audioFrameFreqPublisher_.getNumSubscribers()) - { - rtabmap_audio::AudioFrameFreqPtr msg(new rtabmap_audio::AudioFrameFreq); - msg->header.frame_id = "micro"; - msg->header.stamp = now; - msg->data.resize(freq.total()*freq.elemSize()); - memcpy(msg->data.data(), freq.data, msg->data.size()); - msg->frameLength = freq.cols; - msg->fs = micro_->fs(); - msg->nChannels = freq.rows; - audioFrameFreqPublisher_.publish(msg); - } - if(audioFrameFreqSqrdMagnPublisher_.getNumSubscribers()) - { - rtabmap_audio::AudioFrameFreqSqrdMagnPtr msg(new rtabmap_audio::AudioFrameFreqSqrdMagn); - msg->header.frame_id = "micro"; - msg->header.stamp = now; - - //compute the squared magnitude - cv::Mat sqrdMagn(freq.rows, freq.cols/2, CV_32F); - //for each channels - for(int i=0; i(0, j*2); - im = rowFreq.at(0, j*2+1); - rowSqrdMagn.at(0, j) = re*re + im*im; - } - } - - msg->data.resize(sqrdMagn.cols * sqrdMagn.rows); - memcpy(msg->data.data(), sqrdMagn.data, sqrdMagn.total() * sqrdMagn.elemSize()); - msg->frameLength = sqrdMagn.cols; - msg->fs = micro_->fs(); - msg->nChannels = sqrdMagn.rows; - - audioFrameFreqSqrdMagnPublisher_.publish(msg); - } - } - else - { - UEventsManager::post(new EndEvent()); - this->kill(); - } - } - -private: - ros::Publisher audioFramePublisher_; - ros::Publisher audioFrameFreqPublisher_; - ros::Publisher audioFrameFreqSqrdMagnPublisher_; - rtabmap::Micro * micro_; -}; - -class Handler: public UEventsHandler -{ -public: - Handler() {UEventsManager::addHandler(this);} - virtual ~Handler() {UEventsManager::removeHandler(this);} -protected: - virtual void handleEvent(UEvent * event) - { - if(event->getClassName().compare("EndEvent") == 0) - { - ROS_INFO("End of stream reached... shutting down!"); - ros::shutdown(); - } - } -}; - -int main(int argc, char** argv) -{ - ULogger::setType(ULogger::kTypeConsole); - //ULogger::setLevel(ULogger::kDebug); - ros::init(argc, argv, "audioRecorder"); - ros::NodeHandle nh("~"); - - int deviceId = DEFAULT_DEVICE_ID; - std::string fileName = DEFAULT_FILE_NAME; - int frameLength = DEFAULT_FRAME_LENGTH; - int fs = DEFAULT_FS; - int sampleSize = DEFAULT_SAMPLE_SIZE; - int nChannels = DEFAULT_CHANNELS; - - nh.param("device_id", deviceId, deviceId); - nh.param("file_name", fileName, fileName); - nh.param("frame_length", frameLength, frameLength); - nh.param("fs", fs, fs); - nh.param("sample_size", sampleSize, sampleSize); - nh.param("channels", nChannels, nChannels); - - MicroWrapper * micro; - if(fileName.size() && UFile::exists(fileName)) - { - ROS_INFO("Recording from file %s", fileName.c_str()); - micro = new MicroWrapper(fileName, frameLength); - } - else - { - ROS_INFO("Recording from microphone %d", deviceId); - micro = new MicroWrapper(deviceId, frameLength, fs, sampleSize, nChannels); - } - - if(!micro->init()) - { - ROS_ERROR("Cannot initiate the audio recorder."); - } - else - { - ROS_INFO("frame_length=%d", frameLength); - ROS_INFO("fs=%d", micro->freq()); - ROS_INFO("sample_size=%d", micro->sampleSize()); - ROS_INFO("channels=%d", micro->nChannels()); - - Handler h; - // Start the mic - micro->start(); - ROS_INFO("Audio recorder started..."); - ros::spin(); - } - - micro->join(true); - delete micro; - - return 0; -} diff --git a/rtabmap_audio/CMakeLists.txt b/rtabmap_audio/CMakeLists.txt deleted file mode 100644 index 9808b7ec..00000000 --- a/rtabmap_audio/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -project(rtabmap-audio-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) - -SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}") - -#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(audio_recorder AudioRecorderNode.cpp) -target_link_libraries(audio_recorder) - -FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) -find_package(Fmodex REQUIRED) -INCLUDE(${QT_USE_FILE}) - -INCLUDE_DIRECTORIES(${Fmodex_INCLUDE_DIRS}) -rosbuild_add_executable(audio_player AudioPlayerNode.cpp) -target_link_libraries(audio_player ${Fmodex_LIBRARIES} ${QT_LIBRARIES}) diff --git a/rtabmap_audio/FindFmodex.cmake b/rtabmap_audio/FindFmodex.cmake deleted file mode 100644 index 1e3dc548..00000000 --- a/rtabmap_audio/FindFmodex.cmake +++ /dev/null @@ -1,53 +0,0 @@ -# - Find Fmodex -# This module finds an installed Fmod package. -# -# It sets the following variables: -# Fmodex_FOUND - Set to false, or undefined, if Fmod isn't found. -# Fmodex_INCLUDE_DIRS - The Fmod include directory. -# Fmodex_LIBRARIES - The Fmod library to link against. -# -# - -SET(Fmodex_ROOT) - -# Add ROS Fmodex directory if ROS is installed -FIND_PROGRAM(ROSPACK_EXEC NAME rospack PATHS) -IF(ROSPACK_EXEC) - EXECUTE_PROCESS(COMMAND ${ROSPACK_EXEC} find fmodex - OUTPUT_VARIABLE Fmodex_ROS_PATH - OUTPUT_STRIP_TRAILING_WHITESPACE - WORKING_DIRECTORY "./" - ) - IF(Fmodex_ROS_PATH) - MESSAGE(STATUS "Found Fmodex ROS pkg : ${Fmodex_ROS_PATH}") - SET(Fmodex_ROOT - ${Fmodex_ROS_PATH}/fmodex - ${Fmodex_ROOT} - ) - ENDIF(Fmodex_ROS_PATH) -ENDIF(ROSPACK_EXEC) - -FIND_PATH(Fmodex_INCLUDE_DIRS fmod.h PATHS ${Fmodex_ROOT}/include) - -IF(CMAKE_SIZEOF_VOID_P EQUAL 8) - FIND_LIBRARY(Fmodex_LIBRARIES NAMES fmodex64 PATHS ${Fmodex_ROOT}/lib) -ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8) -IF(NOT Fmodex_LIBRARIES) - FIND_LIBRARY(Fmodex_LIBRARIES NAMES fmodex PATHS ${Fmodex_ROOT}/lib) -ENDIF(NOT Fmodex_LIBRARIES) - -IF (Fmodex_INCLUDE_DIRS AND Fmodex_LIBRARIES) - SET(Fmodex_FOUND TRUE) -ENDIF (Fmodex_INCLUDE_DIRS AND Fmodex_LIBRARIES) - -IF (Fmodex_FOUND) - # show which Fmod was found only if not quiet - IF (NOT Fmodex_FIND_QUIETLY) - MESSAGE(STATUS "Found Fmod: ${Fmodex_LIBRARIES}") - ENDIF (NOT Fmodex_FIND_QUIETLY) -ELSE (Fmodex_FOUND) - # fatal error if Fmod is required but not found - IF (Fmodex_FIND_REQUIRED) - MESSAGE(FATAL_ERROR "Could not find Fmodex... (aka libfmodex)") - ENDIF (Fmodex_FIND_REQUIRED) -ENDIF (Fmodex_FOUND) diff --git a/rtabmap_audio/Makefile b/rtabmap_audio/Makefile deleted file mode 100644 index b75b928f..00000000 --- a/rtabmap_audio/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/rtabmap_audio/mainpage.dox b/rtabmap_audio/mainpage.dox deleted file mode 100644 index 324b3390..00000000 --- a/rtabmap_audio/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b audio - - - ---> - - -*/ diff --git a/rtabmap_audio/manifest.xml b/rtabmap_audio/manifest.xml deleted file mode 100644 index 6aa75874..00000000 --- a/rtabmap_audio/manifest.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - audio - - - Mathieu Labbé - BSD - - http://ros.org/wiki/rtabmap_audio - - - - - - - - - - - diff --git a/rtabmap_audio/msg/AudioFrame.msg b/rtabmap_audio/msg/AudioFrame.msg deleted file mode 100644 index 951a12d6..00000000 --- a/rtabmap_audio/msg/AudioFrame.msg +++ /dev/null @@ -1,11 +0,0 @@ -######################################## -# Audio frame in time domain (raw) -######################################## - -Header header - -uint32 sampleSize # bytes per sample -uint32 frameLength -uint32 nChannels -uint32 fs -uint8[] data \ No newline at end of file diff --git a/rtabmap_audio/msg/AudioFrameFreq.msg b/rtabmap_audio/msg/AudioFrameFreq.msg deleted file mode 100644 index ffe52d8d..00000000 --- a/rtabmap_audio/msg/AudioFrameFreq.msg +++ /dev/null @@ -1,10 +0,0 @@ -######################################## -# Audio frame in frequency domain (with real and imaginary parts) -######################################## - -Header header - -uint32 frameLength -uint32 nChannels -uint32 fs -float32[] data \ No newline at end of file diff --git a/rtabmap_audio/msg/AudioFrameFreqSqrdMagn.msg b/rtabmap_audio/msg/AudioFrameFreqSqrdMagn.msg deleted file mode 100644 index 85ab9404..00000000 --- a/rtabmap_audio/msg/AudioFrameFreqSqrdMagn.msg +++ /dev/null @@ -1,10 +0,0 @@ -######################################## -# Audio frame in frequency domain (squared magnitude) -######################################## - -Header header - -uint32 frameLength -uint32 nChannels -uint32 fs -float32[] data \ No newline at end of file diff --git a/rtabmap_image/.cproject b/rtabmap_image/.cproject deleted file mode 100644 index a30bc6ca..00000000 --- a/rtabmap_image/.cproject +++ /dev/null @@ -1,57 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rtabmap_image/.project b/rtabmap_image/.project deleted file mode 100644 index e818f675..00000000 --- a/rtabmap_image/.project +++ /dev/null @@ -1,79 +0,0 @@ - - - rtabmap-image - - - - - - org.eclipse.cdt.managedbuilder.core.genmakebuilder - clean,full,incremental, - - - ?name? - - - - org.eclipse.cdt.make.core.append_environment - true - - - org.eclipse.cdt.make.core.autoBuildTarget - all - - - org.eclipse.cdt.make.core.buildArguments - VERBOSE=TRUE - - - org.eclipse.cdt.make.core.buildCommand - make - - - org.eclipse.cdt.make.core.cleanBuildTarget - clean - - - org.eclipse.cdt.make.core.contents - org.eclipse.cdt.make.core.activeConfigSettings - - - org.eclipse.cdt.make.core.enableAutoBuild - false - - - org.eclipse.cdt.make.core.enableCleanBuild - true - - - org.eclipse.cdt.make.core.enableFullBuild - true - - - org.eclipse.cdt.make.core.fullBuildTarget - all - - - org.eclipse.cdt.make.core.stopOnError - true - - - org.eclipse.cdt.make.core.useDefaultBuildCmd - false - - - - - org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder - full,incremental, - - - - - - org.eclipse.cdt.core.cnature - org.eclipse.cdt.core.ccnature - org.eclipse.cdt.managedbuilder.core.managedBuildNature - org.eclipse.cdt.managedbuilder.core.ScannerConfigNature - - diff --git a/rtabmap_image/CMakeLists.txt b/rtabmap_image/CMakeLists.txt deleted file mode 100644 index fb399255..00000000 --- a/rtabmap_image/CMakeLists.txt +++ /dev/null @@ -1,55 +0,0 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# 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) - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -rosbuild_gensrv() - -#add dynamic reconfigure api -rosbuild_find_ros_package(dynamic_reconfigure) -include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) -gencfg() - -#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}) - -find_package(OpenCV REQUIRED) -rosbuild_add_executable(camera src/CameraNode.cpp) -target_link_libraries(camera ${OpenCV_LIBS}) - -rosbuild_add_executable(rgb2ind src/RGB2IndexedNode.cpp) -target_link_libraries(rgb2ind ${OpenCV_LIBS}) - -rosbuild_add_executable(xy2polar src/Cartesian2PolarNode.cpp) -target_link_libraries(xy2polar ${OpenCV_LIBS}) - -rosbuild_add_executable(motion_filter src/MotionFilterNode.cpp) -target_link_libraries(motion_filter ${OpenCV_LIBS}) - -FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) -INCLUDE(${QT_USE_FILE}) -#This will generate moc_* for Qt -QT4_WRAP_CPP(moc_srcs src/ImageViewQt.hpp) -rosbuild_add_executable(image_view_qt src/ImageViewQtNode.cpp ${moc_srcs}) -target_link_libraries(image_view_qt ${QT_LIBRARIES} ${OpenCV_LIBS}) diff --git a/rtabmap_image/Makefile b/rtabmap_image/Makefile deleted file mode 100644 index b75b928f..00000000 --- a/rtabmap_image/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/rtabmap_image/cfg/Camera.cfg b/rtabmap_image/cfg/Camera.cfg deleted file mode 100755 index 13609b35..00000000 --- a/rtabmap_image/cfg/Camera.cfg +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "rtabmap_image" -import roslib;roslib.load_manifest(PACKAGE) - -from dynamic_reconfigure.parameter_generator import * - -gen = ParameterGenerator() - -gen.add("device_id", int_t, 0, "Camera device ID", 0, 0, 7) -gen.add("frame_rate", double_t, 0, "Frame rate", 15.0, 0.0, 100.0) -gen.add("width", int_t, 0, "Width", 640, 0, 1920) -gen.add("height", int_t, 0, "Image height", 480, 0, 1080) -gen.add("video_or_images_path", str_t, 0, "Video or images directory path", "") -gen.add("auto_restart", bool_t, 0, "Auto restart the camera (with reading from images and video)", True) - -exit(gen.generate(PACKAGE, "dynamic_camera", "camera")) \ No newline at end of file diff --git a/rtabmap_image/cfg/MotionFilter.cfg b/rtabmap_image/cfg/MotionFilter.cfg deleted file mode 100755 index 1875e786..00000000 --- a/rtabmap_image/cfg/MotionFilter.cfg +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "rtabmap_image" -import roslib;roslib.load_manifest(PACKAGE) - -from dynamic_reconfigure.parameter_generator import * - -gen = ParameterGenerator() - -gen.add("ratio", double_t, 0, "Motion ratio thresholding", 0.2, 0.0, 1.0) - -exit(gen.generate(PACKAGE, "dynamic_motion_filter", "motionFilter")) \ No newline at end of file diff --git a/rtabmap_image/cfg/rgb2ind.cfg b/rtabmap_image/cfg/rgb2ind.cfg deleted file mode 100755 index b7a46bc3..00000000 --- a/rtabmap_image/cfg/rgb2ind.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "rtabmap_image" -import roslib;roslib.load_manifest(PACKAGE) - -from dynamic_reconfigure.parameter_generator import * - -gen = ParameterGenerator() - -size_enum = gen.enum([ gen.const("8", int_t, 0, "Color index table of size 8"), - gen.const("16", int_t, 1, "Color index table of size 16"), - gen.const("32", int_t, 2, "Color index table of size 32"), - gen.const("64", int_t, 3, "Color index table of size 64"), - gen.const("128", int_t, 4, "Color index table of size 128"), - gen.const("256", int_t, 5, "Color index table of size 256"), - gen.const("512", int_t, 6, "Color index table of size 512"), - gen.const("1024", int_t, 7, "Color index table of size 1024"), - gen.const("65536", int_t, 8, "Color index table of size 65536") ], - "An enum to set size") - -gen.add("color_table_size", int_t, 0, "Color index table size", 7, 0, 8, edit_method=size_enum) - -exit(gen.generate(PACKAGE, "dynamic_rgb2ind", "rgb2ind")) \ No newline at end of file diff --git a/rtabmap_image/cfg/xy2polar.cfg b/rtabmap_image/cfg/xy2polar.cfg deleted file mode 100755 index 948d7030..00000000 --- a/rtabmap_image/cfg/xy2polar.cfg +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "rtabmap_image" -import roslib;roslib.load_manifest(PACKAGE) - -from dynamic_reconfigure.parameter_generator import * - -gen = ParameterGenerator() - -gen.add("rays", int_t, 0, "Number of polar rays", 128, 1, 1024) -gen.add("rings", int_t, 0, "Number of polar rings", 64, 1, 1024) - -exit(gen.generate(PACKAGE, "dynamic_xy2polar", "xy2polar")) \ No newline at end of file diff --git a/rtabmap_image/launch/camera.launch b/rtabmap_image/launch/camera.launch deleted file mode 100644 index 0c503284..00000000 --- a/rtabmap_image/launch/camera.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/rtabmap_image/launch/test_rtabmap_image.launch b/rtabmap_image/launch/test_rtabmap_image.launch deleted file mode 100644 index 7f8ba569..00000000 --- a/rtabmap_image/launch/test_rtabmap_image.launch +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rtabmap_image/mainpage.dox b/rtabmap_image/mainpage.dox deleted file mode 100644 index 9b8c240b..00000000 --- a/rtabmap_image/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rtabmap_image - - - ---> - - -*/ diff --git a/rtabmap_image/manifest.xml b/rtabmap_image/manifest.xml deleted file mode 100644 index 4f4355b6..00000000 --- a/rtabmap_image/manifest.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - rtabmap_image - - - Mathieu Labbé - BSD - - http://ros.org/wiki/rtabmap_image - - - - - - - - - - - - diff --git a/rtabmap_image/src/CameraNode.cpp b/rtabmap_image/src/CameraNode.cpp deleted file mode 100644 index 55e6c7d0..00000000 --- a/rtabmap_image/src/CameraNode.cpp +++ /dev/null @@ -1,263 +0,0 @@ -/* - * CameraNode.cpp - * - * Created on: 1 févr. 2010 - * Author: labm2414 - */ - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -class CameraWrapper : public UEventsHandler -{ -public: - // Usb device like a Webcam - CameraWrapper(int usbDevice = 0, - float imageRate = 0, - unsigned int imageWidth = 0, - unsigned int imageHeight = 0) : - camera_(0) - { - ros::NodeHandle nh("~"); - image_transport::ImageTransport it(nh); - rosPublisher_ = it.advertise("image", 1); - startSrv_ = nh.advertiseService("start", &CameraWrapper::startSrv, this); - stopSrv_ = nh.advertiseService("stop", &CameraWrapper::stopSrv, this); - UEventsManager::addHandler(this); - } - - virtual ~CameraWrapper() - { - if(camera_) - { - camera_->join(true); - delete camera_; - } - } - - bool init() - { - if(camera_) - { - return camera_->init(); - } - return false; - } - - void start() - { - if(camera_) - { - return camera_->start(); - } - } - - bool startSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&) - { - ROS_INFO("Camera started..."); - if(camera_) - { - camera_->start(); - } - return true; - } - - bool stopSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&) - { - ROS_INFO("Camera stopped..."); - if(camera_) - { - camera_->kill(); - } - return true; - } - - void setParameters(int deviceId, double frameRate, int width, int height, const std::string & path, bool autoRestart) - { - if(camera_) - { - rtabmap::CameraVideo * videoCam = dynamic_cast(camera_); - rtabmap::CameraImages * imagesCam = dynamic_cast(camera_); - - if(imagesCam) - { - // images - if(!path.empty() && path.compare(imagesCam->getPath()) == 0) - { - imagesCam->setImageRate(frameRate); - imagesCam->setImageSize(width, height); - imagesCam->setAutoRestart(autoRestart); - } - else - { - delete camera_; - camera_ = 0; - } - } - else if(videoCam) - { - if(!path.empty() && path.compare(videoCam->getFilePath()) == 0) - { - // video - videoCam->setImageRate(frameRate); - videoCam->setImageSize(width, height); - videoCam->setAutoRestart(autoRestart); - } - else if(path.empty() && - videoCam->getFilePath().empty() && - videoCam->getUsbDevice() == deviceId) - { - // usb device - unsigned int w; - unsigned int h; - videoCam->getImageSize(w, h); - if((int)w == width && (int)h == height) - { - videoCam->setImageRate(frameRate); - videoCam->setAutoRestart(autoRestart); - } - else - { - delete camera_; - camera_ = 0; - } - } - else - { - delete camera_; - camera_ = 0; - } - } - else - { - ROS_ERROR("Wrong camera type ?!?"); - delete camera_; - camera_ = 0; - } - } - - if(!camera_) - { - if(!path.empty() && UDirectory::exists(path)) - { - //images - camera_ = new rtabmap::CameraImages(path, 1, false, frameRate, autoRestart, width, height); - } - else if(!path.empty() && UFile::exists(path)) - { - //video - camera_ = new rtabmap::CameraVideo(path, frameRate, autoRestart, width, height); - } - else - { - if(!path.empty() && !UDirectory::exists(path) && !UFile::exists(path)) - { - ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str()); - } - //usb device - camera_ = new rtabmap::CameraVideo(deviceId, frameRate, autoRestart, width, height); - } - init(); - start(); - } - } - -protected: - virtual void handleEvent(UEvent * event) - { - if(event->getClassName().compare("CameraEvent") == 0) - { - rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)event; - const cv::Mat & image = e->image(); - if(!image.empty() && image.depth() == CV_8U) - { - cv_bridge::CvImage img; - if(image.channels() == 1) - { - img.encoding = sensor_msgs::image_encodings::MONO8; - } - else - { - img.encoding = sensor_msgs::image_encodings::BGR8; - } - img.image = image; - sensor_msgs::ImagePtr rosMsg = img.toImageMsg(); - rosMsg->header.frame_id = "camera"; - rosMsg->header.stamp = ros::Time::now(); - rosPublisher_.publish(rosMsg); - } - } - else if(event->getClassName().compare("ULogEvent") == 0) - { - ULogEvent * e = (ULogEvent*)event; - if(e->getCode() == ULogger::kWarning) - { - ROS_WARN("%s", e->getMsg().c_str()); - } - else if(e->getCode() >= ULogger::kError) - { - ROS_ERROR("%s", e->getMsg().c_str()); - } - } - } - -private: - image_transport::Publisher rosPublisher_; - rtabmap::Camera * camera_; - ros::ServiceServer startSrv_; - ros::ServiceServer stopSrv_; -}; - -CameraWrapper * camera = 0; -void callback(rtabmap_image::cameraConfig &config, uint32_t level) -{ - if(camera) - { - camera->setParameters(config.device_id, config.frame_rate, config.width, config.height, config.video_or_images_path, config.auto_restart); - } -} - -int main(int argc, char** argv) -{ - ULogger::setType(ULogger::kTypeConsole); - //ULogger::setLevel(ULogger::kDebug); - ULogger::setEventLevel(ULogger::kWarning); - - ros::init(argc, argv, "camera"); - - ros::NodeHandle nh("~"); - - camera = new CameraWrapper(); // webcam device 0 - - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&callback, _1, _2); - server.setCallback(f); - - ros::spin(); - - //cleanup - if(camera) - { - delete camera; - } - - return 0; -} diff --git a/rtabmap_image/src/Cartesian2PolarNode.cpp b/rtabmap_image/src/Cartesian2PolarNode.cpp deleted file mode 100644 index 93844b76..00000000 --- a/rtabmap_image/src/Cartesian2PolarNode.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/* - * RGB2IndexedNode.cpp - */ - -#include -#include -#include -#include -#include -#include -#include - -int dp_rays = 128; -int dp_rings = 64; - -image_transport::Publisher rosPublisherPolar; -image_transport::Publisher rosPublisherPolarReconstructed; - -void callback(rtabmap_image::xy2polarConfig &config, uint32_t level) -{ - dp_rays = config.rays; - dp_rings = config.rings; -} - -void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg) -{ - if(!rosPublisherPolar.getNumSubscribers() && !rosPublisherPolarReconstructed.getNumSubscribers()) - { - return; - } - - if(msg->data.size()) - { - cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg); - - if(ptr->image.depth() == CV_8U && ptr->image.channels() == 3) - { - int radiusX = ptr->image.cols/2; - int radiusY = ptr->image.rows/2; - int radius = radiusXimage; - cvLogPolar( &iplImage, &iplPolar, cvPoint2D32f(radiusX, radiusY), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS ); - - if(rosPublisherPolar.getNumSubscribers()) - { - cv_bridge::CvImage img; - img.header.stamp = ros::Time::now(); - img.header.frame_id = msg->header.frame_id; - img.encoding = ptr->encoding; - img.image = polar; - rosPublisherPolar.publish(img.toImageMsg()); - } - - if(rosPublisherPolarReconstructed.getNumSubscribers()) - { - cv::Mat reconstructed(ptr->image.rows, ptr->image.cols, ptr->image.type()); - IplImage iplReconstructed = reconstructed; - cvLogPolar( &iplPolar, &iplReconstructed, cvPoint2D32f(radiusX, radiusY), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP ); - - cv_bridge::CvImage img; - img.header.stamp = ros::Time::now(); - img.header.frame_id = msg->header.frame_id; - img.encoding = ptr->encoding; - img.image = reconstructed; - rosPublisherPolarReconstructed.publish(img.toImageMsg()); - } - } - else - { - ROS_WARN("Image format should be 8bits - 3 channels"); - } - } -} - -int main(int argc, char * argv[]) -{ - ros::init(argc, argv, "xy2polar"); - - ros::NodeHandle n; - image_transport::ImageTransport it(n); - image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback); - rosPublisherPolar = it.advertise("image_polar", 1); - rosPublisherPolarReconstructed = it.advertise("image_polar_reconstructed", 1); - - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&callback, _1, _2); - server.setCallback(f); - - ros::spin(); - - return 0; -} diff --git a/rtabmap_image/src/ImageViewQt.hpp b/rtabmap_image/src/ImageViewQt.hpp deleted file mode 100644 index fc658ab7..00000000 --- a/rtabmap_image/src/ImageViewQt.hpp +++ /dev/null @@ -1,231 +0,0 @@ -/* - * ImageViewQt.hpp - * - * Created on: 2012-06-20 - * Author: mathieu - */ - -#ifndef IMAGEVIEWQT_HPP_ -#define IMAGEVIEWQT_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include - -class RGBPlot: public UPlot -{ -public: - RGBPlot(int x, int y, QWidget * parent = 0) : - x_(x), - y_(y) - { - r_ = this->addCurve("R", Qt::red); - g_ = this->addCurve("G", Qt::green); - b_ = this->addCurve("B", Qt::blue); - } - ~RGBPlot() {} - void setPixel(int r, int g, int b) - { - r_->addValue(r); - g_->addValue(g); - b_->addValue(b); - } - int x() const {return x_;} - int y() const {return y_;} -private: - int x_; - int y_; - UPlotCurve * r_; - UPlotCurve * g_; - UPlotCurve * b_; -}; - -class ImageViewQt : public QWidget -{ - Q_OBJECT; -public: - ImageViewQt(QWidget * parent = 0) : QWidget(parent), showFrameRate_(true), lastTime_(0) - { - this->setMouseTracking(true); - time_.start(); - } - ~ImageViewQt() {} - -public slots: - void setImage(const QImage & image) - { - lastTime_ = time_.restart(); - if(pixmap_.width() != image.width() || pixmap_.height() != image.height()) - { - for(QMap, RGBPlot*>::iterator iter = pixelMap_.begin(); iter!=pixelMap_.end();) - { - RGBPlot * plot = *iter; - iter = pixelMap_.erase(iter); - delete plot; - } - this->setMinimumSize(image.width(), image.height()); - this->setGeometry(this->geometry().x(), this->geometry().y(), image.width(), image.height()); - } - else - { - for(QMap, RGBPlot*>::iterator iter = pixelMap_.begin(); iter!=pixelMap_.end();++iter) - { - QRgb rgb = image.pixel((*iter)->x(), (*iter)->y()); - (*iter)->setPixel(qRed(rgb), qGreen(rgb), qBlue(rgb)); - } - } - pixmap_ = QPixmap::fromImage(image); - this->update(); - } - -private: - void computeScaleOffsets(float & scale, float & offsetX, float & offsetY) - { - scale = 1.0f; - offsetX = 0.0f; - offsetY = 0.0f; - - if(!pixmap_.isNull()) - { - float w = pixmap_.width(); - float h = pixmap_.height(); - float widthRatio = float(this->rect().width()) / w; - float heightRatio = float(this->rect().height()) / h; - - if(widthRatio < heightRatio) - { - scale = widthRatio; - } - else - { - scale = heightRatio; - } - - w *= scale; - h *= scale; - - if(w < this->rect().width()) - { - offsetX = (this->rect().width() - w)/2.0f; - } - if(h < this->rect().height()) - { - offsetY = (this->rect().height() - h)/2.0f; - } - } - } -private slots: - void removePlot(QObject * obj) - { - if(obj) - { - RGBPlot * plot = (RGBPlot*)obj; - pixelMap_.remove(QPair(plot->x(), plot->y())); - } - } - -protected: - virtual void paintEvent(QPaintEvent *event) - { - QPainter painter(this); - if(!pixmap_.isNull()) - { - painter.save(); - //Scale - float ratio, offsetX, offsetY; - this->computeScaleOffsets(ratio, offsetX, offsetY); - painter.translate(offsetX, offsetY); - painter.scale(ratio, ratio); - painter.drawPixmap(QPoint(0,0), pixmap_); - painter.restore(); - } - if(showFrameRate_ && lastTime_>0) - { - painter.setPen(QColor(Qt::green)); - painter.drawText(2, painter.font().pointSize()+2 , QString("%1 Hz").arg(1000/lastTime_)); - } - } - - virtual void mouseMoveEvent(QMouseEvent * event) - { - if(!pixmap_.isNull()) - { - QPoint pos = this->mapFromGlobal(event->globalPos()); - - float ratio, offsetX, offsetY; - computeScaleOffsets(ratio, offsetX, offsetY); - pos.rx()-=offsetX; - pos.ry()-=offsetY; - pos.rx()/=ratio; - pos.ry()/=ratio; - - if(pos.x()>=0 && pos.x()=0 && pos.y()globalPos(), - QString("[%1,%2]") - .arg(pos.x()) - .arg(pos.y())); - } - } - } - - virtual void contextMenuEvent(QContextMenuEvent * event) - { - if(!pixmap_.isNull()) - { - QPoint pos = this->mapFromGlobal(event->globalPos()); - - float ratio, offsetX, offsetY; - computeScaleOffsets(ratio, offsetX, offsetY); - pos.rx()-=offsetX; - pos.ry()-=offsetY; - pos.rx()/=ratio; - pos.ry()/=ratio; - - if(pos.x()>=0 && pos.x()=0 && pos.y()setCheckable(true); - a_frameRate->setChecked(showFrameRate_); - QAction * action = menu.exec(event->globalPos()); - if(action == a_plot) - { - if(!pixelMap_.contains(QPair(pos.x(), pos.y()))) - { - RGBPlot * plot = new RGBPlot(pos.x(), pos.y(), this); - plot->setWindowTitle(tr("Pixel (%1,%2)").arg(pos.x()).arg(pos.y())); - connect(plot, SIGNAL(destroyed(QObject *)), this, SLOT(removePlot(QObject *))); - plot->setMaxVisibleItems(100); - plot->show(); - pixelMap_.insert(QPair(pos.x(), pos.y()), plot); - } - } - else if(action == a_frameRate) - { - showFrameRate_ = action->isChecked(); - } - - } - } - } - -private: - QPixmap pixmap_; - QMap, RGBPlot*> pixelMap_; - QTime time_; - bool showFrameRate_; - int lastTime_; -}; - - -#endif /* IMAGEVIEWQT_HPP_ */ diff --git a/rtabmap_image/src/ImageViewQtNode.cpp b/rtabmap_image/src/ImageViewQtNode.cpp deleted file mode 100644 index edb4c58e..00000000 --- a/rtabmap_image/src/ImageViewQtNode.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/* - * CameraNodeReceiver.cpp - * - * Created on: 2 févr. 2010 - * Author: labm2414 - */ - -#include -#include -#include -#include - -#include - -#include -#include - -#include - -#include "ImageViewQt.hpp" - -ImageViewQt * view = 0; -bool imagesSaved = false; -int i = 0; - -// assume bgr -QImage cvtCvMat2QImage(const cv::Mat & image, bool isBgr = true) -{ - QImage qtemp; - if(!image.empty() && image.depth() == CV_8U && image.channels()==3) - { - const unsigned char * data = image.data; - if(image.channels() == 3) - { - qtemp = QImage(image.cols, image.rows, QImage::Format_RGB32); - for(int y = 0; y < image.rows; ++y, data += image.cols*image.elemSize()) - { - for(int x = 0; x < image.cols; ++x) - { - QRgb * p = ((QRgb*)qtemp.scanLine (y)) + x; - if(isBgr) - { - *p = qRgb(data[x * image.channels()+2], data[x * image.channels()+1], data[x * image.channels()]); - } - else - { - *p = qRgb(data[x * image.channels()], data[x * image.channels()+1], data[x * image.channels()+2]); - } - } - } - } - } - else if(!image.empty() && image.depth() != CV_8U) - { - printf("Wrong image format, must be 8_bits, 3 channels\n"); - } - return qtemp; -} - -void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg) -{ - if(msg->data.size()) - { - cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg); - - //ROS_INFO("Received an image size=(%d,%d)", ptr->image.cols, ptr->image.rows); - - if(imagesSaved) - { - std::string path = "./imagesSaved"; - if(!UDirectory::exists(path)) - { - if(!UDirectory::makeDir(path)) - { - ROS_ERROR("Cannot make dir %s", path.c_str()); - } - } - path.append("/"); - path.append(uNumber2Str(i++)); - path.append(".bmp"); - if(!cv::imwrite(path.c_str(), ptr->image)) - { - ROS_ERROR("Cannot save image to %s", path.c_str()); - } - else - { - ROS_INFO("Saved image %s", path.c_str()); - } - } - else if(view && view->isVisible()) - { - if(ptr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - { - // Process image in Qt thread... - QMetaObject::invokeMethod(view, "setImage", Q_ARG(const QImage &, QImage(ptr->image.data, ptr->image.cols, ptr->image.rows, ptr->image.cols, QImage::Format_Indexed8).copy())); - } - else if(ptr->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) - { - // Process image in Qt thread... - QMetaObject::invokeMethod(view, "setImage", Q_ARG(const QImage &, cvtCvMat2QImage(ptr->image))); - } - else if(ptr->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) - { - // Process image in Qt thread... - QMetaObject::invokeMethod(view, "setImage", Q_ARG(const QImage &, cvtCvMat2QImage(ptr->image, false))); - } - else - { - ROS_WARN("Encoding \"%s\" is not supported yet (try \"bgr8\" or \"rgb8\")", ptr->encoding.c_str()); - } - } - } -} - -void my_handler(int s){ - QApplication::closeAllWindows(); - QApplication::exit(); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "image_view_qt"); - ros::NodeHandle pn("~"); - - pn.param("images_saved", imagesSaved, imagesSaved); - ROS_INFO("images_saved=%d", imagesSaved?1:0); - - ros::NodeHandle n; - image_transport::ImageTransport it(n); - image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback); - - if(!imagesSaved) - { - QApplication app(argc, argv); - view = new ImageViewQt(); - view->setWindowTitle(ros::this_node::getName().c_str()); - view->show(); - - // Catch ctrl-c to close the gui - // (Place this after QApplication's constructor) - struct sigaction sigIntHandler; - sigIntHandler.sa_handler = my_handler; - sigemptyset(&sigIntHandler.sa_mask); - sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); - - ROS_INFO("Waiting for images..."); - ros::AsyncSpinner spinner(1); // Use 1 thread - spinner.start(); - app.exec(); - spinner.stop(); - - delete view; - } - else - { - ros::spin(); - } - - - return 0; -} diff --git a/rtabmap_image/src/MotionFilterNode.cpp b/rtabmap_image/src/MotionFilterNode.cpp deleted file mode 100644 index fe718e34..00000000 --- a/rtabmap_image/src/MotionFilterNode.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/* - * RGB2IndexedNode.cpp - */ - -#include -#include -#include -#include -#include - -#include -#include - -image_transport::Publisher rosPublisher; -cv::Mat previousImage; -double ratio = 0.2; - -void callback(rtabmap_image::motionFilterConfig &config, uint32_t level) -{ - ratio = config.ratio; -} - -void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg) -{ - if(rosPublisher.getNumSubscribers() && msg->data.size()) - { - cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg); - if(ptr->image.depth() == CV_8U && ptr->image.channels() == 3) - { - cv::Mat motion = ptr->image.clone(); - if(previousImage.cols == motion.cols && previousImage.rows == motion.rows) - { - unsigned char * imageData = (unsigned char *)motion.data; - unsigned char * previous_imageData = (unsigned char *)previousImage.data; - int widthStep = motion.cols * motion.elemSize(); - for(int j=0; j=ratio || fabs(g-previous_g)/256.0f >= ratio || fabs(r-previous_r)/256.0f >= ratio)) - { - imageData[j*widthStep+i*3+0] = 0; - imageData[j*widthStep+i*3+1] = 0; - imageData[j*widthStep+i*3+2] = 0; - } - } - } - } - previousImage = ptr->image.clone(); - - cv_bridge::CvImage img; - img.header.stamp = ros::Time::now(); - img.header.frame_id = ptr->header.frame_id; - img.encoding = ptr->encoding; - img.image = motion; - rosPublisher.publish(img.toImageMsg()); - } - else - { - ROS_WARN("Image format should be 8bits - 3 channels"); - } - } -} - -int main(int argc, char * argv[]) -{ - ros::init(argc, argv, "motion_filter"); - - ros::NodeHandle n; - image_transport::ImageTransport it(n); - image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback); - rosPublisher = it.advertise("image_motion_filtered", 1); - - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&callback, _1, _2); - server.setCallback(f); - - ros::spin(); - - return 0; -} diff --git a/rtabmap_image/src/RGB2IndexedNode.cpp b/rtabmap_image/src/RGB2IndexedNode.cpp deleted file mode 100644 index 4873b0df..00000000 --- a/rtabmap_image/src/RGB2IndexedNode.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/* - * RGB2IndexedNode.cpp - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -image_transport::Publisher rosPublisher; -rtabmap::ColorTable colorTable(rtabmap::ColorTable::kSize1024); - -void callback(rtabmap_image::rgb2indConfig &config, uint32_t level) -{ - if(config.color_table_size == 8) - { - colorTable = rtabmap::ColorTable(rtabmap::ColorTable::kSize65536); - } - else - { - colorTable = rtabmap::ColorTable(1<<(config.color_table_size+3)); - } -} - -void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg) -{ - if(rosPublisher.getNumSubscribers() && msg->data.size()) - { - cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg); - - if(ptr->image.depth() == CV_8U && ptr->image.channels() == 3) - { - cv::Mat ind = ptr->image.clone(); - unsigned char * imageData = (unsigned char *)ind.data; - int widthStep = ind.cols * ind.elemSize(); - for(int i=0; iheader.frame_id; - img.encoding = ptr->encoding; - img.image = ind; - rosPublisher.publish(img.toImageMsg()); - } - else - { - ROS_WARN("Image format should be 8bits - 3 channels"); - } - } -} - -int main(int argc, char * argv[]) -{ - ros::init(argc, argv, "rgb2ind"); - - ros::NodeHandle n; - image_transport::ImageTransport it(n); - image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback); - rosPublisher = it.advertise("image_indexed", 1); - - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&callback, _1, _2); - server.setCallback(f); - - ros::spin(); - - return 0; -} diff --git a/rtabmap_lib/Makefile b/rtabmap_lib/Makefile index 551bab08..be00c035 100644 --- a/rtabmap_lib/Makefile +++ b/rtabmap_lib/Makefile @@ -4,7 +4,7 @@ INSTALL_DIR = rtabmap all: installed SVN_DIR = build/rtabmap-svn -SVN_URL = https://rtabmap.googlecode.com/svn/trunk/rtabmap +SVN_URL = https://rtabmap.googlecode.com/svn/branches/attention/rtabmap SVN_REVISION = -rHEAD #SVN_PATCH = opencvVer.patch include $(shell rospack find mk)/svn_checkout.mk diff --git a/rtabmap_lib/manifest.xml b/rtabmap_lib/manifest.xml index 713e9547..2075af38 100644 --- a/rtabmap_lib/manifest.xml +++ b/rtabmap_lib/manifest.xml @@ -23,7 +23,7 @@ - + diff --git a/utilite/Makefile b/utilite/Makefile deleted file mode 100644 index 5a8ca5f0..00000000 --- a/utilite/Makefile +++ /dev/null @@ -1,30 +0,0 @@ -# avpd rev. HEAD -INSTALL_DIR = utilite - -all: installed - -SVN_DIR = build/utilite-svn -SVN_URL = https://utilite.googlecode.com/svn/trunk/ -SVN_REVISION = -rHEAD -#SVN_PATCH = opencvVer.patch -include $(shell rospack find mk)/svn_checkout.mk - -CMAKE = cmake -CMAKE_ARGS = -D CMAKE_BUILD_TYPE=RELEASE \ - -D BUILD_AUDIO=ON \ - -D BUILD_QT=ON \ - -D BUILD_EXAMPLES=OFF \ - -D BUILD_TESTS=OFF \ - -D CMAKE_INSTALL_PREFIX=`rospack find utilite`/$(INSTALL_DIR) - -installed: $(SVN_DIR) patched - mkdir -p $(SVN_DIR)/build - cd $(SVN_DIR)/build && $(CMAKE) $(CMAKE_ARGS) .. - cd $(SVN_DIR)/build && make $(ROS_PARALLEL_JOBS) && make install - touch Makefile - -clean: - -cd $(SVN_DIR)/build && make clean - rm -rf $(INSTALL_DIR) installed - -.PHONY : clean installfmodex diff --git a/utilite/manifest.xml b/utilite/manifest.xml deleted file mode 100644 index 7449c3a1..00000000 --- a/utilite/manifest.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - UtiLite library - - - Mathieu Labbe - GPL - - http://utilite.googlecode.com - - - - - - - - - - - - - - diff --git a/utilite/patched b/utilite/patched deleted file mode 100644 index e69de29b..00000000 diff --git a/utilite/rospack_nosubdirs b/utilite/rospack_nosubdirs deleted file mode 100644 index e69de29b..00000000