i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged attention branch to trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@657 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2012-12-11 18:05:05 +00:00
parent 7eb9253e32
commit d8d9118a00
81 changed files with 126 additions and 4352 deletions
Download Patch File Download Diff File Expand all files Collapse all files
fmodex/CMakeLists.txt
-30
View File
@@ -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})
fmodex/FindFmodex.cmake
-53
View File
@@ -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)
fmodex/Makefile
-20
View File
@@ -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
fmodex/mainpage.dox
-14
View File
@@ -1,14 +0,0 @@
/**
\mainpage
\htmlinclude manifest.html
\b fmodex
<!--
Provide an overview of your package.
-->
-->
*/
fmodex/manifest.xml
-14
View File
@@ -1,14 +0,0 @@
<package>
<description brief="fmodex">
fmodex
</description>
<author>Mathieu Labbé</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/fmodex</url>
</package>
rtabmap/CMakeLists.txt
-23
View File
@@ -41,34 +41,11 @@ find_package(OpenCV REQUIRED)
rosbuild_add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp) rosbuild_add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
target_link_libraries(rtabmap ${OpenCV_LIBS}) 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) FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui)
IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND) IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
INCLUDE(${QT_USE_FILE}) INCLUDE(${QT_USE_FILE})
rosbuild_add_executable(rtabmap_gui src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp) 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") 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() ELSE()
MESSAGE(STATUS "[WARNING] Qt4 not found, the GUI node will not be compiled...") MESSAGE(STATUS "[WARNING] Qt4 not found, the GUI node will not be compiled...")
ENDIF() ENDIF()
rtabmap/launch/all_tr_learning.launch
-22
View File
@@ -1,22 +0,0 @@
<launch>
<!-- BRING UP TELEROBOT -->
<!-- BUS CAN MANAGER -->
<group ns="tr">
<param name="driver_type" value="TRCANDriver"/>
<node name="can_manager" type="can_manager" pkg="can_manager"
args="tr">
<remap from="/tr/tr/cmd_vel" to="/cmd_vel" />
</node>
</group>
<!-- TELEOP -->
<include file="$(find rtabmap)/launch/teleop.launch"/>
<!-- RTAB-MAP -->
<include file="$(find rtabmap)/launch/rtabmap_sm.launch"/>
<!-- CAMERA -->
<include file="$(find rtabmap)/launch/cameraOpenCV.launch"/>
</launch>
rtabmap/launch/az2_learning.launch
-62
View File
@@ -1,62 +0,0 @@
<launch>
<!-- BRING UP AZIMUT2 -->
<include file="$(find az2_bringup)/az2_base_controller.launch"/>
<!-- TELEOP JOYSTICK -->
<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
<remap from="cmd_vel" to="user/cmd_vel" />
</node>
<node name="joystick" pkg="joy" type="joy_node">
<param name="autorepeat_rate" value="20" type="double" />
</node>
<!-- RTAB-MAP SENSORY-MOTOR VERSION -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- cmd_vel_a has priority on cmd_vel_b -->
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
<param name="commands_hz" value="10.0" type="double"/>
<param name="cmd_vel_a_buffered" value="false" type="bool"/>
<param name="cmd_vel_b_buffered" value="true" type="bool"/>
<param name="stats_logged" value="true" type="bool"/>
<remap from="cmd_vel_a" to="user/cmd_vel" />
<remap from="cmd_vel_b" to="rtabmap/cmd_vel" />
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
</node>
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="twist_to_poses" pkg="rtabmap" type="twist_to_poses">
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
</node>
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<remap from="sm_state" to="sm_state" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="rtabmap_in" pkg="rtabmap" type="rtabmap_in">
<param name="nb_commands" value="2" type="int"/>
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
<remap from="sm_state" to="sm_state" />
<remap from="sensor_data" to="sensor_data" />
</node>
<node name="image_to_sms" pkg="rtabmap" type="image_to_sms">
<param name="image_hz" value="5.0" type="double"/>
<param name="resize_image_width" value="80" type="int"/>
<param name="resize_image_height" value="60" type="int"/>
<param name="keypoints_extracted" value="0" type="int"/>
<remap from="image" to="camera/image" />
<remap from="sm_state" to="sensor_data" />
</node>
</launch>
rtabmap/launch/az2_teleop.launch
-14
View File
@@ -1,14 +0,0 @@
<launch>
<!-- BRING UP AZIMUT2 -->
<include file="$(find az2_bringup)/az2_base_controller.launch"/>
<!-- TELEOP JOYSTICK -->
<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
</node>
<node name="joystick" pkg="joy" type="joy_node">
<param name="autorepeat_rate" value="20" type="double" />
</node>
</launch>
rtabmap/launch/az3_learning.launch
-62
View File
@@ -1,62 +0,0 @@
<launch>
<!-- BRING UP AZIMUT3 -->
<include file="$(find az3_bringup)/az3_base_controller.launch"/>
<!-- TELEOP JOYSTICK -->
<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
<remap from="cmd_vel" to="user/cmd_vel" />
</node>
<node name="joystick" pkg="joy" type="joy_node">
<param name="autorepeat_rate" value="20" type="double" />
</node>
<!-- RTAB-MAP SENSORY-MOTOR VERSION -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- cmd_vel_a has priority on cmd_vel_b -->
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
<param name="commands_hz" value="10.0" type="double"/>
<param name="cmd_vel_a_buffered" value="false" type="bool"/>
<remap from="cmd_vel_a" to="user/cmd_vel" />
<remap from="cmd_vel_b" to="rtabmap/cmd_vel" />
<remap from="cmd_vel" to="az3/base_controller/cmd_vel" />
</node>
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
<param name="commands_hz" value="10.0" type="double"/>
<param name="idle_null_commands_sent" value="false" type="bool"/>
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="twist_to_poses" pkg="rtabmap" type="twist_to_poses">
<remap from="cmd_vel" to="az3/base_controller/cmd_vel" />
</node>
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<remap from="sm_state" to="sm_state" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="rtabmap_in" pkg="rtabmap" type="rtabmap_in">
<param name="nb_commands" value="2" type="int"/>
<remap from="cmd_vel" to="az3/base_controller/cmd_vel" />
<remap from="sm_state" to="sm_state" />
<remap from="sensor_data" to="sensor_data" />
</node>
<node name="image_to_sms" pkg="rtabmap" type="image_to_sms">
<param name="image_hz" value="5.0" type="double"/>
<param name="resize_image_width" value="80" type="int"/>
<param name="resize_image_height" value="60" type="int"/>
<param name="keypoints_extracted" value="0" type="int"/>
<remap from="image" to="camera/image" />
<remap from="sm_state" to="sensor_data" />
</node>
</launch>
rtabmap/launch/az3_teleop.launch
-14
View File
@@ -1,14 +0,0 @@
<launch>
<!-- BRING UP AZIMUT2 -->
<include file="$(find az3_bringup)/az3_base_controller.launch"/>
<!-- TELEOP JOYSTICK -->
<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
<remap from="cmd_vel" to="az3/base_controller/cmd_vel" />
</node>
<node name="joystick" pkg="joy" type="joy_node">
<param name="autorepeat_rate" value="20" type="double" />
</node>
</launch>
rtabmap/launch/cameraOmni.launch
-14
View File
@@ -1,14 +0,0 @@
<launch>
<!-- CAMERA -->
<node pkg="omni_camera_capture" type="omni_publisher"
name="omni_camera" respawn="false">
<param name="device" value="/dev/video0"/>
<param name="refresh_rate" value="1"/>
<param name="frame_id" value="omni_camera_link" />
<param name="image_encoding" value="RGB" /> <!-- GREY or RGB-->
<param name="image_size" value="FULL" /> <!-- PARTIAL or FULL -->
<param name="scale_divisor" value="1" /> <!-- 1, 2, 3, 4 or 6 in PARTIAL size-->
<remap from="/image" to="/image_raw" />
</node>
</launch>
rtabmap/launch/cameraUVC.launch
-10
View File
@@ -1,10 +0,0 @@
<launch>
<node pkg="uvc_camera" type="camera_node" name="uvc_camera" output="screen">
<param name="width" type="int" value="640" />
<param name="height" type="int" value="480" />
<param name="fps" type="int" value="5" />
<param name="frame" type="string" value="wide_stereo" />
<param name="device" type="string" value="/dev/video0" />
<!-- <param name="camera_info_url" type="string" value="file://$(find uvc_camera)/example.yaml" /> -->
</node>
</launch>
rtabmap/launch/loop_closure_detection.launch
+5 -4
View File
@@ -9,11 +9,12 @@
<node name="rtabmap_gui" pkg="rtabmap" type="rtabmap_gui" output="screen"/> <node name="rtabmap_gui" pkg="rtabmap" type="rtabmap_gui" output="screen"/>
<node name="camera" pkg="rtabmap_image" type="camera" output="screen"> <node name="camera" pkg="uimage" type="camera" output="screen">
<remap from="/camera/image" to="/image"/> <remap from="/camera/image" to="/image"/>
<param name="device_id" value="0" type="int"/> <param name="device_id" value="0" type="int"/>
<param name="frame_rate" value="1.0" type="double"/> <param name="video_or_images_path" value="/media/SD-32GB/NewCollege" type="string"/>
<param name="width" value="640" type="int"/> <param name="frame_rate" value="2.0" type="double"/>
<param name="height" value="480" type="int"/> <param name="width" value="0" type="int"/>
<param name="height" value="0" type="int"/>
</node> </node>
</launch> </launch>
rtabmap/launch/loop_closure_detection_with_audio.launch
-36
View File
@@ -1,36 +0,0 @@
<launch>
<!-- RTAB-MAP LOOP CLOSURE DETECTION VERSION : with sensorimotor memory type-->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<remap from="/image" to="/image_not_used"/> <!-- Just to make sure that rtabmap does not subscribe
to camera image, here we use sensorimotor input -->
</node>
<node name="rtabmap_gui" pkg="rtabmap" type="rtabmap_gui" output="screen"/>
<node name="camera" pkg="rtabmap" type="camera" output="screen">
<remap from="/camera/image" to="/image"/>
<param name="device_id" value="0" type="int"/>
<param name="image_hz" value="5.0" type="double"/>
<param name="image_width" value="80" type="int"/>
<param name="image_height" value="60" type="int"/>
</node>
<node name="my_audio_recorder" pkg="rtabmap" type="audio_recorder" output="screen">
<param name="device_id" value="0" type="int"/>
<param name="file_name" value="" type="string"/> <!-- use the micro -->
<param name="frame_length" value="4800" type="int"/> <!-- Must match with image_hz of the camera
(5 Hz at 24000 fs = 4800 samples / frame) -->
<param name="fs" value="24000" type="int"/>
<param name="sample_size" value="2" type="int"/> <!-- 16bits/sample -->
<param name="channels" value="1" type="int"/> <!-- mono -->
</node>
<!-- This node will synchronize images and audio frames, and
transform them to a Sensorimotor topic used by RTAB-Map. -->
<node name="my_input_node" pkg="rtabmap" type="input_image_audio_node" />
</launch>
rtabmap/launch/sensors_capture.launch
-41
View File
@@ -1,41 +0,0 @@
<launch>
<!-- teleop -->
<node pkg="pr2_teleop" type="teleop_pr2_keyboard" name="spawn_teleop_keyboard" output="screen">
<remap from="cmd_vel" to="user/cmd_vel" />
<param name="walk_vel" value="0.5" />
<param name="run_vel" value="1.0" />
<param name="yaw_rate" value="1.0" />
<param name="yaw_run_rate" value="1.5" />
</node>
<!-- camera -->
<node name="camera" pkg="rtabmap" type="camera" output="screen">
<remap from="camera/image" to="image"/>
<param name="device_id" value="0" type="int"/>
<param name="image_hz" value="10.0" type="double"/>
<param name="image_width" value="80" type="int"/>
<param name="image_height" value="60" type="int"/>
</node>
<!-- mic -->
<node name="my_audio_recorder" pkg="rtabmap" type="audio_recorder" output="screen">
<param name="device_id" value="0" type="int"/>
<param name="file_name" value="" type="string"/> <!-- use the micro -->
<param name="frame_length" value="2400" type="int"/> <!-- Must match with image_hz of the camera
(10 Hz at 24000 fs = 2400 samples / frame) -->
<param name="fs" value="24000" type="int"/>
<param name="sample_size" value="2" type="int"/> <!-- 16bits/sample -->
<param name="channels" value="1" type="int"/> <!-- mono -->
</node>
<!-- Lasers -->
<!-- ls -l /dev/ttyACM* -->
<!-- sudo chmod a+rw /dev/ttyACM* -->
<node pkg="hokuyo_node" type="hokuyo_node" name="laser_top_publisher" ns="laser_top">
<param name="frame_id" type="string" value="laser_top"/>
<param name="port" type="string" value="/dev/ttyACM0"/>
</node>
</launch>
rtabmap/launch/teleop.launch
-9
View File
@@ -1,9 +0,0 @@
<launch>
<!-- TELEOP JOYSTICK -->
<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
<remap from="cmd_vel" to="tr/cmd_vel" />
</node>
<node name="joystick" pkg="joy" type="joy_node">
<param name="autorepeat_rate" value="20" type="double" />
</node>
</launch>
rtabmap/launch/teleop_keyboard.launch
-10
View File
@@ -1,10 +0,0 @@
<launch>
<node pkg="pr2_teleop" type="teleop_pr2_keyboard" name="spawn_teleop_keyboard" output="screen">
<remap from="cmd_vel" to="tr/cmd_vel" />
<param name="walk_vel" value="0.5" />
<param name="run_vel" value="1.0" />
<param name="yaw_rate" value="1.0" />
<param name="yaw_run_rate" value="1.5" />
</node>
</launch>
rtabmap/launch/test_cameraOpenCV.launch
-16
View File
@@ -1,16 +0,0 @@
<launch>
<!-- Nodes -->
<node name="camera_receiver" pkg="rtabmap" type="camera_receiver" output="screen">
<remap from="image" to="camera/image"/>
<remap from="image/compressed" to="camera/image/compressed"/>
<param name="image_transport" value="raw" type="str"/>
</node>
<!-- Nodes -->
<node name="camera" pkg="rtabmap" type="camera" output="screen">
<param name="device_id" value="0" type="int"/>
<param name="image_hz" value="30.0" type="double"/>
<param name="image_width" value="640" type="int"/>
<param name="image_height" value="480" type="int"/>
</node>
</launch>
rtabmap/launch/test_cameraUVC.launch
-13
View File
@@ -1,13 +0,0 @@
<launch>
<!-- Nodes -->
<node name="camera_receiver" pkg="rtabmap" type="camera_receiver" output="screen"/>
<node pkg="uvc_camera" type="camera_node" name="uvc_camera" output="screen">
<param name="width" type="int" value="640" />
<param name="height" type="int" value="480" />
<param name="fps" type="int" value="30" />
<param name="frame" type="string" value="wide_stereo" />
<param name="device" type="string" value="/dev/video0" />
<!-- <param name="camera_info_url" type="string" value="file://$(find uvc_camera)/example.yaml" /> -->
</node>
</launch>
rtabmap/launch/test_learning.launch
-57
View File
@@ -1,57 +0,0 @@
<launch>
<!-- launch teleop with teleop_keyboard.launch -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<remap from="/image" to="/image_not_used"/> <!-- Just to make sure that rtabmap does not subscribe
to camera image, here we use sensorimotor input -->
<remap from="sensorimotor" to="sensorimotor" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<!-- Arbitration -->
<!-- cmd_vel_a has priority on cmd_vel_b -->
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
<param name="commands_hz" value="10.0" type="double"/>
<param name="cmd_vel_a_buffered" value="false" type="bool"/>
<param name="cmd_vel_b_buffered" value="true" type="bool"/>
<param name="stats_logged" value="false" type="bool"/>
<remap from="cmd_vel_a" to="user/cmd_vel" />
<remap from="cmd_vel_b" to="rtabmap/cmd_vel" />
<remap from="cmd_vel" to="cmd_vel" />
</node>
<!-- INPUT NODE -->
<node name="my_input_node" pkg="rtabmap" type="input_image_audio_twist_node">
<remap from="cmd_vel" to="cmd_vel"/>
</node>
<!-- SENSORS -->
<!-- camera -->
<node name="camera" pkg="rtabmap" type="camera" output="screen">
<remap from="camera/image" to="image"/>
<param name="device_id" value="0" type="int"/>
<param name="image_hz" value="10.0" type="double"/>
<param name="image_width" value="80" type="int"/>
<param name="image_height" value="60" type="int"/>
</node>
<!-- mic -->
<node name="my_audio_recorder" pkg="rtabmap" type="audio_recorder" output="screen">
<param name="device_id" value="0" type="int"/>
<param name="file_name" value="" type="string"/> <!-- use the micro -->
<param name="frame_length" value="2400" type="int"/> <!-- Must match with image_hz of the camera
(10 Hz at 24000 fs = 2400 samples / frame) -->
<param name="fs" value="24000" type="int"/>
<param name="sample_size" value="2" type="int"/> <!-- 16bits/sample -->
<param name="channels" value="1" type="int"/> <!-- mono -->
</node>
</launch>
rtabmap/launch/test_learning_image_twist.launch
-38
View File
@@ -1,38 +0,0 @@
<launch>
<!-- when rosbag ... "rosbag play -.-clock my.bag"-->
<param name="use_sim_time" type="bool" value="True"/>
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<remap from="/image" to="/image_not_used"/> <!-- Just to make sure that rtabmap does not subscribe
to camera image, here we use sensorimotor input -->
<remap from="sensorimotor" to="sensorimotor" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<!-- Arbitration -->
<!-- cmd_vel_a has priority on cmd_vel_b -->
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
<param name="commands_hz" value="10.0" type="double"/>
<param name="cmd_vel_a_buffered" value="false" type="bool"/>
<param name="cmd_vel_b_buffered" value="true" type="bool"/>
<param name="stats_logged" value="false" type="bool"/>
<remap from="cmd_vel_a" to="user/cmd_vel" />
<remap from="cmd_vel_b" to="rtabmap/cmd_vel" />
<remap from="cmd_vel" to="cmd_vel" />
</node>
<!-- INPUT NODE -->
<node name="my_input_node" pkg="rtabmap" type="input_image_twist_node">
<remap from="cmd_vel" to="cmd_vel"/>
<remap from="image" to="image_local_polar_reconstructed"/>
</node>
</launch>
rtabmap/launch/test_step_by_step_actions_only.launch
-48
View File
@@ -1,48 +0,0 @@
<launch>
<!-- RTAB-MAP SENSORY-MOTOR VERSION -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- Nodes -->
<!-- cmd_vel_a has priority on cmd_vel_b -->
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
<param name="commands_hz" value="1.0" type="double"/>
<param name="cmd_vel_a_buffered" value="true" type="bool"/>
<remap from="cmd_vel_a" to="user/cmd_vel" />
<!-- <remap from="cmd_vel_b" to="rtabmap/cmd_vel" /> -->
<remap from="cmd_vel" to="cmd_vel" />
</node>
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
<param name="commands_hz" value="1.0" type="double"/>
<param name="idle_null_commands_sent" value="false" type="bool"/>
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<remap from="sm_state" to="sm_state" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="rtabmap_in" pkg="rtabmap" type="rtabmap_in">
<param name="nb_commands" value="1" type="int"/>
<remap from="cmd_vel" to="cmd_vel" />
<remap from="sm_state" to="sm_state" />
<remap from="sensor_data" to="sensor_data" />
</node>
<node name="twist_to_sms" pkg="rtabmap" type="twist_to_sms">
<remap from="cmd_vel" to="cmd_vel" />
<remap from="sm_state" to="sensor_data" />
</node>
<node name="twist_to_turtle_vel" pkg="rtabmap" type="twist_to_turtle_vel">
</node>
<node name="turtlesim" pkg="turtlesim" type="turtlesim_node">
</node>
</launch>
rtabmap/launch/test_step_by_step_az2.launch
-63
View File
@@ -1,63 +0,0 @@
<launch>
<!-- BRING UP AZIMUT2 -->
<include file="$(find az2_bringup)/az2_base_controller.launch"/>
<!-- RTAB-MAP SENSORY-MOTOR VERSION -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- Nodes -->
<!-- cmd_vel_a has priority on cmd_vel_b -->
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
<param name="commands_hz" value="0.5" type="double"/>
<param name="cmd_vel_a_buffered" value="true" type="bool"/>
<remap from="cmd_vel_a" to="user/cmd_vel" />
<!-- <remap from="cmd_vel_b" to="rtabmap/cmd_vel" /> -->
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
</node>
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
<param name="commands_hz" value="1.0" type="double"/>
<param name="idle_null_commands_sent" value="false" type="bool"/>
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<remap from="sm_state" to="sm_state" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="rtabmap_in" pkg="rtabmap" type="rtabmap_in">
<param name="nb_commands" value="1" type="int"/>
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
<remap from="sm_state" to="sm_state" />
<remap from="sensor_data" to="sensor_data" />
</node>
<node name="image_to_sms" pkg="rtabmap" type="image_to_sms">
<param name="image_hz" value="1" type="double"/>
<param name="resize_image_width" value="160" type="int"/>
<param name="resize_image_height" value="120" type="int"/>
<param name="keypoints_extracted" value="0" type="int"/>
<remap from="image" to="camera/image" />
<remap from="sm_state" to="sensor_data" />
</node>
<!--
<node name="twist_to_sms" pkg="rtabmap" type="twist_to_sms">
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
<remap from="sm_state" to="sensor_data" />
</node>
-->
<node name="twist_to_turtle_vel" pkg="rtabmap" type="twist_to_turtle_vel_node">
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
</node>
</launch>
rtabmap/launch/test_visual_attention.launch
-34
View File
@@ -1,34 +0,0 @@
<launch>
<!-- Nodes -->
<node name="camera" pkg="rtabmap_image" type="camera" output="screen">
<remap from="camera/image" to="image"/>
<param name="device_id" value="0" type="int"/>
<param name="image_hz" value="0" type="double"/>
<param name="image_width" value="640" type="int"/>
<param name="image_height" value="480" type="int"/>
</node>
<node name="visual_attention" pkg="rtabmap" type="visual_attention" output="screen"/>
<!--
<node name="image" pkg="rtabmap_image" type="image_view_qt" output="screen">
<remap from="image" to="image"/>
</node>
-->
<node name="image_motion" pkg="rtabmap_image" type="image_view_qt" output="screen">
<remap from="image" to="image_motion"/>
</node>
<node name="image_motion_local" pkg="rtabmap_image" type="image_view_qt" output="screen">
<remap from="image" to="image_motion_local"/>
</node>
<node name="image_local" pkg="rtabmap_image" type="image_view_qt" output="screen">
<remap from="image" to="image_local"/>
</node>
<node name="image_motion_local_polar" pkg="rtabmap_image" type="image_view_qt" output="screen">
<remap from="image" to="image_motion_local_polar"/>
</node>
<node name="image_motion_local_polar_reconstructed" pkg="rtabmap_image" type="image_view_qt" output="screen">
<remap from="image" to="image_motion_local_polar_reconstructed"/>
</node>
</launch>
rtabmap/manifest.xml
+1 -3
View File
@@ -17,9 +17,7 @@
<depend package="turtlesim"/> <depend package="turtlesim"/>
<depend package="tf"/> <depend package="tf"/>
<depend package="rtabmap_lib"/> <depend package="rtabmap_lib"/>
<depend package="rtabmap_audio"/> <depend package="cv_bridge"/>
<depend package="rtabmap_image"/>
</package> </package>
rtabmap/msg/ActuatorMsg.msg
-6
View File
@@ -1,6 +0,0 @@
########################################
# Actuator
########################################
uint32 type # Actuator type, refer to rtabmap::Actuator::Type enum)
rtabmap/CvMatMsg matrix
rtabmap/msg/CvMatMsg.msg
-10
View File
@@ -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)
rtabmap/msg/Info.msg
+10
View File
@@ -0,0 +1,10 @@
########################################
# If a loop is found with the current image ("refId"),
# "loopClosureId" is not null.
########################################
Header header
int32 refId
int32 loopClosureId
rtabmap/msg/RtabmapInfoEx.msg → rtabmap/msg/InfoEx.msg
+5 -6
View File
@@ -1,13 +1,12 @@
######################################## ########################################
# Statistics stuff: # Extended info msg with statistics
# These fields are empty if RTAb-Map's
# parameter publishStats=false
######################################## ########################################
Header header Header header
int32 refChild int32 refId
int32 loopClosureId
# std::map<int, float> posterior; # std::map<int, float> posterior;
int32[] posteriorKeys int32[] posteriorKeys
@@ -25,8 +24,8 @@ int32[] weightsValues
string[] statsKeys string[] statsKeys
float32[] statsValues float32[] statsValues
rtabmap/SensorMsg[] refRawData sensor_msgs/Image refImage
rtabmap/SensorMsg[] loopRawData sensor_msgs/Image loopImage
# #
# For features2d : std::multimap<int, cv::Keypoint> words # For features2d : std::multimap<int, cv::Keypoint> words
rtabmap/msg/RtabmapInfo.msg
-16
View File
@@ -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
rtabmap/msg/SensorMsg.msg
-8
View File
@@ -1,8 +0,0 @@
########################################
# Sensor
########################################
uint32 type # Sensor type (e.g. for sensor: image, audio),
# refer to rtabmap::Sensor::Type).
rtabmap/CvMatMsg matrix
rtabmap/msg/Sensorimotor.msg
-4
View File
@@ -1,4 +0,0 @@
Header header
rtabmap/SensorMsg[] sensors
rtabmap/ActuatorMsg[] actuators
rtabmap/src/AbtrVelocityNode.cpp
-197
View File
@@ -1,197 +0,0 @@
/*
* CameraNode.cpp
*
* Author: labm2414
*/
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
#include <utilite/UMutex.h>
#include <queue>
#include <utilite/ULogger.h>
std::queue<float> commandsA;
std::queue<float> commandsB;
std::vector<float> 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<float>(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<geometry_msgs::TwistStamped>("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<float>();
}
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<float>();
}
}
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<float>();
}
}
++index;
}
if(statsLogged)
{
ULogger::flush();
}
return 0;
}
rtabmap/src/CmdVelToTurtleVelNode.cpp
-33
View File
@@ -1,33 +0,0 @@
/*
* CameraNode.cpp
*
* Created on: 1 févr. 2010
* Author: labm2414
*/
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Velocity.h>
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<turtlesim::Velocity>("turtle1/command_velocity", 1);
ros::Subscriber image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
ros::spin();
return 0;
}
rtabmap/src/CoreWrapper.cpp
+68 -86
View File
@@ -6,13 +6,14 @@
*/ */
#include "CoreWrapper.h" #include "CoreWrapper.h"
#include "MsgConversion.h"
#include <ros/ros.h> #include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/RtabmapEvent.h> #include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Rtabmap.h> #include <rtabmap/core/Rtabmap.h>
#include <rtabmap/core/Camera.h> #include <rtabmap/core/Camera.h>
#include <rtabmap/core/Parameters.h> #include <rtabmap/core/Parameters.h>
#include <rtabmap/core/SensorimotorEvent.h>
#include <utilite/UEventsManager.h> #include <utilite/UEventsManager.h>
#include <utilite/ULogger.h> #include <utilite/ULogger.h>
#include <utilite/UFile.h> #include <utilite/UFile.h>
@@ -20,9 +21,8 @@
#include <opencv2/highgui/highgui.hpp> #include <opencv2/highgui/highgui.hpp>
//msgs //msgs
#include "rtabmap/RtabmapInfo.h" #include "rtabmap/Info.h"
#include "rtabmap/RtabmapInfoEx.h" #include "rtabmap/InfoEx.h"
#include "rtabmap/CvMatMsg.h"
using namespace rtabmap; using namespace rtabmap;
@@ -30,8 +30,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
rtabmap_(0) rtabmap_(0)
{ {
ros::NodeHandle nh("~"); ros::NodeHandle nh("~");
infoPub_ = nh.advertise<rtabmap::RtabmapInfo>("info", 1); infoPub_ = nh.advertise<rtabmap::Info>("info", 1);
infoPubEx_ = nh.advertise<rtabmap::RtabmapInfo>("infoEx", 1); infoPubEx_ = nh.advertise<rtabmap::InfoEx>("infoEx", 1);
parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1); parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1);
rtabmap_ = new Rtabmap(); rtabmap_ = new Rtabmap();
@@ -52,7 +52,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
nh = ros::NodeHandle(); nh = ros::NodeHandle();
parametersUpdatedTopic_ = nh.subscribe("rtabmap_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this); parametersUpdatedTopic_ = nh.subscribe("rtabmap_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
sensorimotorTopic_ = nh.subscribe("sensorimotor", 1, &CoreWrapper::sensorimotorReceivedCallback, this);
image_transport::ImageTransport it(nh); image_transport::ImageTransport it(nh);
imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this); 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()); 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<Sensor> sensors;
std::list<Actuator> actuators;
for(unsigned int i=0; i<msg->sensors.size(); ++i)
{
sensors.push_back(Sensor(fromCvMatMsgToCvMat(msg->sensors[i].matrix), (Sensor::Type)msg->sensors[i].type));
}
for(unsigned int i=0; i<msg->actuators.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) void CoreWrapper::imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
{ {
if(msg->data.size()) if(msg->data.size())
@@ -197,101 +171,109 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
{ {
if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers()) if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers())
{ {
ROS_INFO("Sending RtabmapInfo msg...");
RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent; RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
const Statistics & stat = rtabmapEvent->getStats(); 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<Actuator>::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()) 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); infoPub_.publish(msg);
} }
if(infoPubEx_.getNumSubscribers()) 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 // Detailed info
if(stat.extended()) if(stat.extended())
{ {
if(stat.refRawData().size()) if(!stat.refImage().empty())
{ {
msg->infoEx.refRawData.resize(stat.refRawData().size()); cv_bridge::CvImage img;
i=0; if(stat.refImage().channels() == 1)
for(std::list<Sensor>::const_iterator iter = stat.refRawData().begin(); iter!=stat.refRawData().end(); ++iter)
{ {
msg->infoEx.refRawData[i].type = iter->type(); img.encoding = sensor_msgs::image_encodings::MONO8;
fromCvMatToCvMatMsg(msg->infoEx.refRawData[i++].matrix, iter->data());
} }
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()); cv_bridge::CvImage img;
i=0; if(stat.loopImage().channels() == 1)
for(std::list<Sensor>::const_iterator iter = stat.loopClosureRawData().begin(); iter!=stat.loopClosureRawData().end(); ++iter)
{ {
msg->infoEx.loopRawData[i].type = iter->type(); img.encoding = sensor_msgs::image_encodings::MONO8;
fromCvMatToCvMatMsg(msg->infoEx.loopRawData[i++].matrix, iter->data());
} }
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 //Posterior, likelihood, childCount
msg->infoEx.posteriorKeys = uKeys(stat.posterior()); msg->posteriorKeys = uKeys(stat.posterior());
msg->infoEx.posteriorValues = uValues(stat.posterior()); msg->posteriorValues = uValues(stat.posterior());
msg->infoEx.likelihoodKeys = uKeys(stat.likelihood()); msg->likelihoodKeys = uKeys(stat.likelihood());
msg->infoEx.likelihoodValues = uValues(stat.likelihood()); msg->likelihoodValues = uValues(stat.likelihood());
msg->infoEx.weightsKeys = uKeys(stat.weights()); msg->weightsKeys = uKeys(stat.weights());
msg->infoEx.weightsValues = uValues(stat.weights()); msg->weightsValues = uValues(stat.weights());
//Features stuff... //Features stuff...
msg->infoEx.refWordsKeys = uListToVector(uKeys(stat.refWords())); msg->refWordsKeys = uKeys(stat.refWords());
msg->infoEx.refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size()); msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
int index = 0; int index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin(); for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
i!=stat.refWords().end(); i!=stat.refWords().end();
++i) ++i)
{ {
msg->infoEx.refWordsValues.at(index).angle = i->second.angle; msg->refWordsValues.at(index).angle = i->second.angle;
msg->infoEx.refWordsValues.at(index).response = i->second.response; msg->refWordsValues.at(index).response = i->second.response;
msg->infoEx.refWordsValues.at(index).ptx = i->second.pt.x; msg->refWordsValues.at(index).ptx = i->second.pt.x;
msg->infoEx.refWordsValues.at(index).pty = i->second.pt.y; msg->refWordsValues.at(index).pty = i->second.pt.y;
msg->infoEx.refWordsValues.at(index).size = i->second.size; msg->refWordsValues.at(index).size = i->second.size;
msg->infoEx.refWordsValues.at(index).octave = i->second.octave; msg->refWordsValues.at(index).octave = i->second.octave;
msg->infoEx.refWordsValues.at(index).class_id = i->second.class_id; msg->refWordsValues.at(index).class_id = i->second.class_id;
++index; ++index;
} }
msg->infoEx.loopWordsKeys = uListToVector(uKeys(stat.loopWords())); msg->loopWordsKeys = uKeys(stat.loopWords());
msg->infoEx.loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size()); msg->loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
index = 0; index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin(); for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
i!=stat.loopWords().end(); i!=stat.loopWords().end();
++i) ++i)
{ {
msg->infoEx.loopWordsValues.at(index).angle = i->second.angle; msg->loopWordsValues.at(index).angle = i->second.angle;
msg->infoEx.loopWordsValues.at(index).response = i->second.response; msg->loopWordsValues.at(index).response = i->second.response;
msg->infoEx.loopWordsValues.at(index).ptx = i->second.pt.x; msg->loopWordsValues.at(index).ptx = i->second.pt.x;
msg->infoEx.loopWordsValues.at(index).pty = i->second.pt.y; msg->loopWordsValues.at(index).pty = i->second.pt.y;
msg->infoEx.loopWordsValues.at(index).size = i->second.size; msg->loopWordsValues.at(index).size = i->second.size;
msg->infoEx.loopWordsValues.at(index).octave = i->second.octave; msg->loopWordsValues.at(index).octave = i->second.octave;
msg->infoEx.loopWordsValues.at(index).class_id = i->second.class_id; msg->loopWordsValues.at(index).class_id = i->second.class_id;
++index; ++index;
} }
// Statistics data // Statistics data
msg->infoEx.statsKeys = uKeys(stat.data()); msg->statsKeys = uKeys(stat.data());
msg->infoEx.statsValues = uValues(stat.data()); msg->statsValues = uValues(stat.data());
} }
infoPubEx_.publish(msg); infoPubEx_.publish(msg);
} }
rtabmap/src/CoreWrapper.h
-3
View File
@@ -18,7 +18,6 @@
#include <sensor_msgs/Image.h> #include <sensor_msgs/Image.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <image_transport/image_transport.h> #include <image_transport/image_transport.h>
#include "rtabmap/Sensorimotor.h"
namespace rtabmap namespace rtabmap
{ {
@@ -34,7 +33,6 @@ public:
void start(); void start();
private: private:
void sensorimotorReceivedCallback(const rtabmap::SensorimotorConstPtr & msg);
void imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg); void imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg);
void twistCallback(const geometry_msgs::TwistConstPtr & msg); void twistCallback(const geometry_msgs::TwistConstPtr & msg);
void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg); void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg);
@@ -51,7 +49,6 @@ private:
private: private:
rtabmap::Rtabmap * rtabmap_; rtabmap::Rtabmap * rtabmap_;
ros::Subscriber sensorimotorTopic_;
image_transport::Subscriber imageTopic_; image_transport::Subscriber imageTopic_;
ros::Subscriber audioFrameFreqSqrdMagnTopic_; ros::Subscriber audioFrameFreqSqrdMagnTopic_;
ros::Subscriber twistTopic_; ros::Subscriber twistTopic_;
rtabmap/src/GuiWrapper.cpp
+32 -75
View File
@@ -6,7 +6,6 @@
*/ */
#include "GuiWrapper.h" #include "GuiWrapper.h"
#include "MsgConversion.h"
#include <QtGui/QApplication> #include <QtGui/QApplication>
#include <cv_bridge/cv_bridge.h> #include <cv_bridge/cv_bridge.h>
@@ -21,8 +20,6 @@
#include <rtabmap/core/RtabmapEvent.h> #include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Parameters.h> #include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Camera.h> #include <rtabmap/core/Camera.h>
#include <rtabmap/core/Sensor.h>
#include <rtabmap/core/SensorimotorEvent.h>
#include "PreferencesDialogROS.h" #include "PreferencesDialogROS.h"
@@ -31,8 +28,7 @@ using namespace rtabmap;
GuiWrapper::GuiWrapper(int & argc, char** argv) GuiWrapper::GuiWrapper(int & argc, char** argv)
{ {
ros::NodeHandle nh; ros::NodeHandle nh;
infoTopic_ = nh.subscribe("rtabmap/infoEx", 1, &GuiWrapper::infoReceivedCallback, this); infoExTopic_ = nh.subscribe("rtabmap/infoEx", 1, &GuiWrapper::infoExReceivedCallback, this);
velocity_sub_ = nh.subscribe("cmd_vel", 1, &GuiWrapper::velocityReceivedCallback, this);
app_ = new QApplication(argc, argv); app_ = new QApplication(argc, argv);
mainWindow_ = new MainWindow(new PreferencesDialogROS()); mainWindow_ = new MainWindow(new PreferencesDialogROS());
mainWindow_->show(); mainWindow_->show();
@@ -62,9 +58,9 @@ int GuiWrapper::exec()
return app_->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 // Map from ROS struct to rtabmap struct
rtabmap::Statistics * stat = new rtabmap::Statistics(); rtabmap::Statistics * stat = new rtabmap::Statistics();
@@ -72,112 +68,73 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
stat->setExtended(true); // Extended stat->setExtended(true); // Extended
stat->setRefImageId(msg->refId); stat->setRefImageId(msg->refId);
std::list<Sensor> sensors;
for(unsigned int i=0; i<msg->infoEx.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); stat->setLoopClosureId(msg->loopClosureId);
sensors.clear();
for(unsigned int i=0; i<msg->infoEx.loopRawData.size(); ++i) if(msg->refImage.data.size())
{ {
Sensor s(fromCvMatMsgToCvMat(msg->infoEx.loopRawData[i].matrix), (Sensor::Type)msg->infoEx.loopRawData[i].type); stat->setRefImage(cv_bridge::toCvShare(msg->refImage, msg)->image.clone());
if(s.data().total()) }
{ if(msg->loopImage.data.size())
sensors.push_back(s); {
} stat->setLoopImage(cv_bridge::toCvShare(msg->loopImage, msg)->image.clone());
} }
stat->setLoopClosureRawData(sensors);
//Posterior, likelihood, childCount //Posterior, likelihood, childCount
std::map<int, float> mapIntFloat; std::map<int, float> mapIntFloat;
for(unsigned int i=0; i<msg->infoEx.posteriorKeys.size() && i<msg->infoEx.posteriorValues.size(); ++i) for(unsigned int i=0; i<msg->posteriorKeys.size() && i<msg->posteriorValues.size(); ++i)
{ {
mapIntFloat.insert(std::pair<int, float>(msg->infoEx.posteriorKeys.at(i), msg->infoEx.posteriorValues.at(i))); mapIntFloat.insert(std::pair<int, float>(msg->posteriorKeys.at(i), msg->posteriorValues.at(i)));
} }
stat->setPosterior(mapIntFloat); stat->setPosterior(mapIntFloat);
mapIntFloat.clear(); mapIntFloat.clear();
for(unsigned int i=0; i<msg->infoEx.likelihoodKeys.size() && i<msg->infoEx.likelihoodValues.size(); ++i) for(unsigned int i=0; i<msg->likelihoodKeys.size() && i<msg->likelihoodValues.size(); ++i)
{ {
mapIntFloat.insert(std::pair<int, float>(msg->infoEx.likelihoodKeys.at(i), msg->infoEx.likelihoodValues.at(i))); mapIntFloat.insert(std::pair<int, float>(msg->likelihoodKeys.at(i), msg->likelihoodValues.at(i)));
} }
stat->setLikelihood(mapIntFloat); stat->setLikelihood(mapIntFloat);
std::map<int, int> mapIntInt; std::map<int, int> mapIntInt;
for(unsigned int i=0; i<msg->infoEx.weightsKeys.size() && i<msg->infoEx.weightsValues.size(); ++i) for(unsigned int i=0; i<msg->weightsKeys.size() && i<msg->weightsValues.size(); ++i)
{ {
mapIntInt.insert(std::pair<int, int>(msg->infoEx.weightsKeys.at(i), msg->infoEx.weightsValues.at(i))); mapIntInt.insert(std::pair<int, int>(msg->weightsKeys.at(i), msg->weightsValues.at(i)));
} }
stat->setWeights(mapIntInt); stat->setWeights(mapIntInt);
//SURF stuff... //SURF stuff...
std::multimap<int, cv::KeyPoint> mapIntKeypoint; std::multimap<int, cv::KeyPoint> mapIntKeypoint;
for(unsigned int i=0; i<msg->infoEx.refWordsKeys.size() && i<msg->infoEx.refWordsValues.size(); ++i) for(unsigned int i=0; i<msg->refWordsKeys.size() && i<msg->refWordsValues.size(); ++i)
{ {
cv::KeyPoint pt; cv::KeyPoint pt;
pt.angle = msg->infoEx.refWordsValues.at(i).angle; pt.angle = msg->refWordsValues.at(i).angle;
pt.response = msg->infoEx.refWordsValues.at(i).response; pt.response = msg->refWordsValues.at(i).response;
pt.pt.x = msg->infoEx.refWordsValues.at(i).ptx; pt.pt.x = msg->refWordsValues.at(i).ptx;
pt.pt.y = msg->infoEx.refWordsValues.at(i).pty; pt.pt.y = msg->refWordsValues.at(i).pty;
pt.size = msg->infoEx.refWordsValues.at(i).size; pt.size = msg->refWordsValues.at(i).size;
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->infoEx.refWordsKeys.at(i), pt)); mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->refWordsKeys.at(i), pt));
} }
stat->setRefWords(mapIntKeypoint); stat->setRefWords(mapIntKeypoint);
mapIntKeypoint.clear(); mapIntKeypoint.clear();
for(unsigned int i=0; i<msg->infoEx.loopWordsKeys.size() && i<msg->infoEx.loopWordsValues.size(); ++i) for(unsigned int i=0; i<msg->loopWordsKeys.size() && i<msg->loopWordsValues.size(); ++i)
{ {
cv::KeyPoint pt; cv::KeyPoint pt;
pt.angle = msg->infoEx.loopWordsValues.at(i).angle; pt.angle = msg->loopWordsValues.at(i).angle;
pt.response = msg->infoEx.loopWordsValues.at(i).response; pt.response = msg->loopWordsValues.at(i).response;
pt.pt.x = msg->infoEx.loopWordsValues.at(i).ptx; pt.pt.x = msg->loopWordsValues.at(i).ptx;
pt.pt.y = msg->infoEx.loopWordsValues.at(i).pty; pt.pt.y = msg->loopWordsValues.at(i).pty;
pt.size = msg->infoEx.loopWordsValues.at(i).size; pt.size = msg->loopWordsValues.at(i).size;
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->infoEx.loopWordsKeys.at(i), pt)); mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->loopWordsKeys.at(i), pt));
} }
stat->setLoopWords(mapIntKeypoint); stat->setLoopWords(mapIntKeypoint);
//Actions
std::list<Actuator> actuators;
for(unsigned int i=0; i<msg->actuators.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 // Statistics data
for(unsigned int i=0; i<msg->infoEx.statsKeys.size() && i<msg->infoEx.statsValues.size(); i++) for(unsigned int i=0; i<msg->statsKeys.size() && i<msg->statsValues.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..."); ROS_INFO("Publishing statistics...");
UEventsManager::post(new rtabmap::RtabmapEvent(&stat)); 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<float>(0) = (float)msg->twist.linear.x;
data.at<float>(1) = (float)msg->twist.linear.y;
data.at<float>(2) = (float)msg->twist.linear.z;
data.at<float>(3) = (float)msg->twist.angular.x;
data.at<float>(4) = (float)msg->twist.angular.y;
data.at<float>(5) = (float)msg->twist.angular.z;
std::list<Actuator> actuators;
actuators.push_back(Actuator(data, rtabmap::Actuator::kTypeTwist));
this->post(new rtabmap::SensorimotorEvent(std::list<Sensor>(), actuators));
}
void GuiWrapper::handleEvent(UEvent * anEvent) void GuiWrapper::handleEvent(UEvent * anEvent)
{ {
if(anEvent->getClassName().compare("ParamEvent") == 0) if(anEvent->getClassName().compare("ParamEvent") == 0)
rtabmap/src/GuiWrapper.h
+3 -6
View File
@@ -9,8 +9,7 @@
#define GUIWRAPPER_H_ #define GUIWRAPPER_H_
#include <ros/ros.h> #include <ros/ros.h>
#include "rtabmap/RtabmapInfo.h" #include "rtabmap/InfoEx.h"
#include "rtabmap/RtabmapInfoEx.h"
#include "utilite/UEventsHandler.h" #include "utilite/UEventsHandler.h"
#include <geometry_msgs/TwistStamped.h> #include <geometry_msgs/TwistStamped.h>
@@ -33,12 +32,10 @@ protected:
virtual void handleEvent(UEvent * anEvent); virtual void handleEvent(UEvent * anEvent);
private: private:
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg); void infoExReceivedCallback(const rtabmap::InfoExConstPtr & infoMsg);
void velocityReceivedCallback(const geometry_msgs::TwistStampedConstPtr & msg);
private: private:
ros::Subscriber infoTopic_; ros::Subscriber infoExTopic_;
ros::Subscriber velocity_sub_;
QApplication * app_; QApplication * app_;
rtabmap::MainWindow * mainWindow_; rtabmap::MainWindow * mainWindow_;
rtabmap/src/ImageAudioInputNode.cpp
-90
View File
@@ -1,90 +0,0 @@
/*
* InputNode.cpp
*
* Created on: 2012-05-27
* Author: mathieu
*/
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h"
#include "rtabmap/Sensorimotor.h"
#include <opencv2/core/core.hpp>
#include <cv_bridge/cv_bridge.h>
#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<rtabmap::Sensorimotor>("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<sensor_msgs::Image> image_sub;
message_filters::Subscriber<rtabmap_audio::AudioFrameFreqSqrdMagn > audio_sub;
//synchronization stuff
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, rtabmap_audio::AudioFrameFreqSqrdMagn> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> 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;
}
rtabmap/src/ImageAudioTwistInputNode.cpp
-107
View File
@@ -1,107 +0,0 @@
/*
* InputNode.cpp
*
* Created on: 2012-05-27
* Author: mathieu
*/
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/TwistStamped.h>
#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h"
#include "rtabmap/Sensorimotor.h"
#include <opencv2/core/core.hpp>
#include <cv_bridge/cv_bridge.h>
#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<rtabmap::Sensorimotor>("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<float>(0) = (float)twist->twist.linear.x;
data.at<float>(1) = (float)twist->twist.linear.y;
data.at<float>(2) = (float)twist->twist.linear.z;
data.at<float>(3) = (float)twist->twist.angular.x;
data.at<float>(4) = (float)twist->twist.angular.y;
data.at<float>(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<sensor_msgs::Image> image_sub;
message_filters::Subscriber<rtabmap_audio::AudioFrameFreqSqrdMagn> audio_sub;
message_filters::Subscriber<geometry_msgs::TwistStamped> twist_sub;
//synchronization stuff
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, rtabmap_audio::AudioFrameFreqSqrdMagn, geometry_msgs::TwistStamped> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> 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;
}
rtabmap/src/ImageTwistInputNode.cpp
-94
View File
@@ -1,94 +0,0 @@
/*
* InputNode.cpp
*
* Created on: 2012-05-27
* Author: mathieu
*/
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/TwistStamped.h>
#include "rtabmap/Sensorimotor.h"
#include <opencv2/core/core.hpp>
#include <cv_bridge/cv_bridge.h>
#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<rtabmap::Sensorimotor>("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<float>(0) = (float)twist->twist.linear.x;
data.at<float>(1) = (float)twist->twist.linear.y;
data.at<float>(2) = (float)twist->twist.linear.z;
data.at<float>(3) = (float)twist->twist.angular.x;
data.at<float>(4) = (float)twist->twist.angular.y;
data.at<float>(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<sensor_msgs::Image> image_sub;
message_filters::Subscriber<geometry_msgs::TwistStamped> twist_sub;
//synchronization stuff
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::TwistStamped> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> 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;
}
rtabmap/src/MsgConversion.h
-68
View File
@@ -1,68 +0,0 @@
/*
* MsgConversion.h
*
* Created on: 2012-05-27
* Author: mathieu
*/
#ifndef MSGCONVERSION_H_
#define MSGCONVERSION_H_
#include <rtabmap/core/Sensor.h>
#include <rtabmap/core/Actuator.h>
#include "rtabmap/CvMatMsg.h"
#include "rtabmap/SensorMsg.h"
#include "rtabmap/ActuatorMsg.h"
#include <opencv2/highgui/highgui.hpp>
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_ */
rtabmap/src/OutputNode.cpp
-78
View File
@@ -1,78 +0,0 @@
/*
* CameraNode.cpp
*
* Author: labm2414
*/
#include <ros/ros.h>
#include <rtabmap/core/Actuator.h>
#include "rtabmap/RtabmapInfo.h"
#include "rtabmap/RtabmapInfoEx.h"
#include <geometry_msgs/Twist.h>
#include <utilite/UMutex.h>
#include <utilite/ULogger.h>
ros::Publisher rosPublisher;
bool statsLogged = true;
const char * statsFileName = "OuputStats.txt";
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
{
for(unsigned int i=0; i<msg->actuators.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<geometry_msgs::Twist>("rtabmap/cmd_vel", 1);
ros::spin();
if(statsLogged)
{
ULogger::flush();
}
return 0;
}
rtabmap/src/Twist2Pose.cpp
-65
View File
@@ -1,65 +0,0 @@
/*
* TwistToPoses.cpp
*
* Created on: 2011-11-30
* Author: matlab
*/
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/tf.h>
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<geometry_msgs::PoseStamped>("pose_linear", 1);
rosPublisherAngular = nh.advertise<geometry_msgs::PoseStamped>("pose_angular", 1);
ros::Subscriber twist_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
ros::spin();
return 0;
}
rtabmap/src/Twist2TwistStamped.cpp
-35
View File
@@ -1,35 +0,0 @@
/*
* TwistToPoses.cpp
*
* Created on: 2011-11-30
* Author: matlab
*/
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
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<geometry_msgs::TwistStamped>("cmd_vel_stamped", 1);
ros::Subscriber twist_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
ros::spin();
return 0;
}
rtabmap/src/VisualAttentionNode.cpp
-507
View File
@@ -1,507 +0,0 @@
/*
* VisualAttentionNode.cpp
*/
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <signal.h>
#include <utilite/UPlot.h>
#include <QtGui/QApplication>
#include <QtGui/QSpinBox>
#include <QtGui/QCheckBox>
#include <QtGui/QVBoxLayout>
#include <opencv2/imgproc/imgproc_c.h>
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<motion.rows; ++j)
{
for(int i=0; i<motion.cols; ++i)
{
float b = (float)imageData[j*widthStep+i*3+0];
float g = (float)imageData[j*widthStep+i*3+1];
float r = (float)imageData[j*widthStep+i*3+2];
float previous_b = (float)previous_imageData[j*widthStep+i*3+0];
float previous_g = (float)previous_imageData[j*widthStep+i*3+1];
float previous_r = (float)previous_imageData[j*widthStep+i*3+2];
if(!(fabs(b-previous_b)/256.0f>=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<motionPolarROI.rows; ++j)
{
for(int i=0; i<motionPolarROI.cols; ++i)
{
float b = (float)motionPolarData[j*widthStep+i*3+0];
float g = (float)motionPolarData[j*widthStep+i*3+1];
float r = (float)motionPolarData[j*widthStep+i*3+2];
float previous_b = (float)previousPolarData[j*widthStep+i*3+0];
float previous_g = (float)previousPolarData[j*widthStep+i*3+1];
float previous_r = (float)previousPolarData[j*widthStep+i*3+2];
if(!(fabs(b-previous_b)/256.0f>=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; j<reconstructedROI.rows; ++j)
{
for(int i=0; i<reconstructedROI.cols; ++i)
{
if(reconstructedData[j*widthStep+i*3+0] ||
reconstructedData[j*widthStep+i*3+1] ||
reconstructedData[j*widthStep+i*3+2])
{
float dist = std::sqrt(float((i-centerROILocal.x)*(i-centerROILocal.x) + (j-centerROILocal.y)*(j-centerROILocal.y)));
if((nearestMovingPixel.x == 0 && nearestMovingPixel.y == 0) ||
dist < distNearest)
{
nearestMovingPixel.y = j;
nearestMovingPixel.x = i;
distNearest = dist;
}
}
}
}
}
// PLOT
if(g_curveR && g_curveG && g_curveB)
{
cv::Mat ref = ptr->image;
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<motion.rows; ++j)
{
for(int i=0; i<motion.cols; ++i)
{
if(data[j*widthStep+i*3+0] ||
data[j*widthStep+i*3+1] ||
data[j*widthStep+i*3+2])
{
float dist = std::sqrt(float((i-centerROIGlobal.x)*(i-centerROIGlobal.x) + (j-centerROIGlobal.y)*(j-centerROIGlobal.y)));
if((nearestMovingPixel.x == 0 && nearestMovingPixel.y == 0) ||
dist < distNearest)
{
nearestMovingPixel.y = j;
nearestMovingPixel.x = i;
distNearest = dist;
}
}
}
}
}
if(!attentionDisabled && nearestMovingPixel.x && nearestMovingPixel.y)
{
if(localAttention)
{
ROS_INFO("nearestMovingPixel center(local:global)=(%d,%d:%d,%d) nearest(local:global)=(%d,%d:%d,%d)",
centerROILocal.x,
centerROILocal.y,
centerROILocal.x+roi.x,
centerROILocal.y+roi.y,
nearestMovingPixel.x,
nearestMovingPixel.y,
nearestMovingPixel.x+roi.x,
nearestMovingPixel.y+roi.y);
nearestMovingPixel.x += roi.x;
nearestMovingPixel.y += roi.y;
if( nearestMovingPixel.x > roi.x &&
nearestMovingPixel.x<roi.x+roi.width &&
nearestMovingPixel.x>roi.width/2 &&
nearestMovingPixel.x<ptr->image.cols - roi.width/2)
{
roi.x = nearestMovingPixel.x-roi.width/2;
}
if( nearestMovingPixel.y>roi.y &&
nearestMovingPixel.y<roi.y+roi.height &&
nearestMovingPixel.y>roi.height/2 &&
nearestMovingPixel.y<ptr->image.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.x<ptr->image.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.y<ptr->image.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 = size<ptr->image.cols?(ptr->image.cols-size)/2:0;
int y = size<ptr->image.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;
}
rtabmap_audio/AudioPlayerNode.cpp
-389
View File
@@ -1,389 +0,0 @@
/*
* AudioPlayerNode.cpp
*/
#include <fmod.hpp>
#include <fmod_errors.h>
#include <ros/ros.h>
#include <utilite/ULogger.h>
#include <utilite/UMath.h>
#include <utilite/UThreadNode.h>
#include <utilite/UPlot.h>
#include <utilite/USpectrogram.h>
#include "rtabmap_audio/AudioFrame.h"
#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h"
#include <QApplication>
#include <signal.h>
#define DEFAULT_GUI_USED true
#define PLOT_SAMPLING_RATIO 64
// Ring buffer
unsigned int g_readPtr = 0;
unsigned int g_writePtr = 0;
std::vector<char> 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<int> 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<v.size(); ++i)
{
uMinMax(p + i*downSamplingFactor, downSamplingFactor, min, max);
v[i] = abs(min) > abs(max) ? min : max;
}
}
else if(sampleSize == 2)
{
short * p = (short*)data;
short min,max;
for(int i=0; i<v.size(); ++i)
{
uMinMax(p + i*downSamplingFactor, downSamplingFactor, min, max);
v[i] = abs(min) > abs(max) ? min : max;
}
}
else if(sampleSize == 4)
{
int * p = (int*)data;
int min,max;
for(int i=0; i<v.size(); ++i)
{
uMinMax(p + i*downSamplingFactor, downSamplingFactor, min, max);
v[i] = abs(min) > abs(max) ? min : max;
}
}
QMetaObject::invokeMethod(curve, "addValues", Q_ARG(QVector<int>, 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; count<datalen; ++count)
{
*(buffer++) = g_ringBuffer[g_readPtr++];
g_readPtr %= g_ringBuffer.size();
}
if(g_readPtr < start)
{
++g_readingLooped;
}
if(g_plot && g_plot->isVisible() && 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; i<datalen; ++i)
{
*(buffer++) = 0;
}
}
return FMOD_OK;
}
FMOD_RESULT F_CALLBACK pcmsetposcallback(FMOD_SOUND *sound, int subsound, unsigned int position, FMOD_TIMEUNIT postype)
{
/*
This is useful if the user calls Sound::setPosition and you want to seek your data accordingly.
*/
return FMOD_OK;
}
/**
* decoderBufferSize = in bytes
*/
bool initAudioPlayer(unsigned int decoderBufferSize, int fs, int channels, int bytesPerSample)
{
ROS_INFO("Init player with fs=%d, channels=%d, bytesPerSample=%d", fs, channels, bytesPerSample);
UASSERT(bytesPerSample == 1 || bytesPerSample == 2 || bytesPerSample == 4);
FMOD_RESULT result;
FMOD_CREATESOUNDEXINFO createsoundexinfo;
unsigned int version;
FMOD_MODE mode = FMOD_2D | FMOD_OPENUSER | FMOD_LOOP_NORMAL | FMOD_HARDWARE | FMOD_CREATESTREAM;
/*
Create a System object and initialize.
*/
result = FMOD::System_Create(&g_system);
ERRCHECK(result);
result = g_system->getVersion(&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<char>(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<float> 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<float>, 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<int> >("QVector<int>");
qRegisterMetaType<std::vector<float> >("std::vector<float>");
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;
}
rtabmap_audio/AudioRecorderNode.cpp
-272
View File
@@ -1,272 +0,0 @@
/*
* AudioRecorderNode.cpp
*/
#include <ros/ros.h>
#include <utilite/ULogger.h>
#include <utilite/UFile.h>
#include <utilite/UEventsHandler.h>
#include <utilite/UEventsManager.h>
#include <utilite/UEvent.h>
#include <utilite/UThreadNode.h>
#include <std_msgs/Empty.h>
#include <rtabmap/core/Micro.h>
#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<rtabmap_audio::AudioFrame>("audioFrame", 1);
audioFrameFreqPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreq>("audioFrameFreq", 1);
audioFrameFreqSqrdMagnPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreqSqrdMagn>("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<rtabmap_audio::AudioFrame>("audioFrame", 1);
audioFrameFreqPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreq>("audioFrameFreq", 1);
audioFrameFreqSqrdMagnPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreqSqrdMagn>("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; i<msg->data.size(); i+=data.elemSize()*data.rows)
{
for(int j=0; j<data.rows; ++j)
{
memcpy(msg->data.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<sqrdMagn.rows; ++i)
{
cv::Mat rowFreq = freq.row(i);
cv::Mat rowSqrdMagn = sqrdMagn.row(i);
float re;
float im;
for(int j=0; j<rowSqrdMagn.cols; ++j)
{
re = rowFreq.at<float>(0, j*2);
im = rowFreq.at<float>(0, j*2+1);
rowSqrdMagn.at<float>(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;
}
rtabmap_audio/CMakeLists.txt
-45
View File
@@ -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})
rtabmap_audio/FindFmodex.cmake
-53
View File
@@ -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)
rtabmap_audio/Makefile
-1
View File
@@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk
rtabmap_audio/mainpage.dox
-14
View File
@@ -1,14 +0,0 @@
/**
\mainpage
\htmlinclude manifest.html
\b audio
<!--
Provide an overview of your package.
-->
-->
*/
rtabmap_audio/manifest.xml
-21
View File
@@ -1,21 +0,0 @@
<package>
<description brief="rtabmap_audio">
audio
</description>
<author>Mathieu Labbé</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/rtabmap_audio</url>
<depend package="std_msgs"/>
<depend package="roscpp"/>
<depend package="rtabmap_lib"/>
<depend package="fmodex"/>
<rosdep name="libqt4-dev"/>
</package>
rtabmap_audio/msg/AudioFrame.msg
-11
View File
@@ -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
rtabmap_audio/msg/AudioFrameFreq.msg
-10
View File
@@ -1,10 +0,0 @@
########################################
# Audio frame in frequency domain (with real and imaginary parts)
########################################
Header header
uint32 frameLength
uint32 nChannels
uint32 fs
float32[] data
rtabmap_audio/msg/AudioFrameFreqSqrdMagn.msg
-10
View File
@@ -1,10 +0,0 @@
########################################
# Audio frame in frequency domain (squared magnitude)
########################################
Header header
uint32 frameLength
uint32 nChannels
uint32 fs
float32[] data
rtabmap_image/.cproject
-57
View File
@@ -1,57 +0,0 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.283151101">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.283151101" moduleId="org.eclipse.cdt.core.settings" name="Default">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactName="${ProjName}" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.base.283151101" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.base.283151101.248109540" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.base.167646636" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.788965009" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
<builder arguments="VERBOSE=TRUE" command="make" id="cdt.managedbuild.target.gnu.builder.base.1653582432" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="cdt.managedbuild.target.gnu.builder.base"/>
<tool id="cdt.managedbuild.tool.gnu.archiver.base.1628870487" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.813130495" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base">
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.408339469" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1588707877" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base">
<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1741107391" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.c.linker.base.491007307" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.705663340" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base">
<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.1225880006" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
<additionalInput kind="additionalinput" paths="$(LIBS)"/>
</inputType>
</tool>
<tool id="cdt.managedbuild.tool.gnu.assembler.base.1515840903" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base">
<inputType id="cdt.managedbuild.tool.gnu.assembler.input.1604501456" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
</tool>
</toolChain>
</folderInfo>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="rtabmap-image.null.1862955112" name="rtabmap-image"/>
</storageModule>
<storageModule moduleId="refreshScope" versionNumber="1">
<resource resourceType="PROJECT" workspacePath="/rtabmap-image"/>
</storageModule>
</cproject>
rtabmap_image/.project
-79
View File
@@ -1,79 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>rtabmap-image</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
<triggers>clean,full,incremental,</triggers>
<arguments>
<dictionary>
<key>?name?</key>
<value></value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.append_environment</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.autoBuildTarget</key>
<value>all</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value>VERBOSE=TRUE</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>
<value>make</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
<value>clean</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.contents</key>
<value>org.eclipse.cdt.make.core.activeConfigSettings</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableAutoBuild</key>
<value>false</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableCleanBuild</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableFullBuild</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.fullBuildTarget</key>
<value>all</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.stopOnError</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
<value>false</value>
</dictionary>
</arguments>
</buildCommand>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
<triggers>full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>org.eclipse.cdt.core.cnature</nature>
<nature>org.eclipse.cdt.core.ccnature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
</natures>
</projectDescription>
rtabmap_image/CMakeLists.txt
-55
View File
@@ -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})
rtabmap_image/Makefile
-1
View File
@@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk
rtabmap_image/cfg/Camera.cfg
-16
View File
@@ -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"))
rtabmap_image/cfg/MotionFilter.cfg
-11
View File
@@ -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"))
rtabmap_image/cfg/rgb2ind.cfg
-22
View File
@@ -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"))
rtabmap_image/cfg/xy2polar.cfg
-12
View File
@@ -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"))
rtabmap_image/launch/camera.launch
-9
View File
@@ -1,9 +0,0 @@
<launch>
<!-- Nodes -->
<node name="camera" pkg="rtabmap" type="camera" output="screen">
<param name="device_id" value="0" type="int"/>
<param name="image_hz" value="0" type="double"/>
<param name="image_width" value="640" type="int"/>
<param name="image_height" value="480" type="int"/>
</node>
</launch>
rtabmap_image/launch/test_rtabmap_image.launch
-31
View File
@@ -1,31 +0,0 @@
<launch>
<!-- Nodes -->
<node name="camera" pkg="rtabmap_image" type="camera" output="screen">
<remap from="camera/image" to="image"/>
<param name="device_id" value="0" type="int"/>
<param name="frame_rate" value="10" type="double"/>
<param name="width" value="640" type="int"/>
<param name="height" value="480" type="int"/>
</node>
<node name="rgb2ind" pkg="rtabmap_image" type="rgb2ind" output="screen"/>
<node name="xy2polar" pkg="rtabmap_image" type="xy2polar" output="screen"/>
<node name="motion_filter" pkg="rtabmap_image" type="motion_filter" output="screen"/>
<!-- Create some image_view_qt to see images -->
<node name="view_indexed" pkg="rtabmap_image" type="image_view_qt">
<remap from="image" to="image_indexed"/>
</node>
<node name="view_polar" pkg="rtabmap_image" type="image_view_qt">
<remap from="image" to="image_polar"/>
</node>
<node name="view_polar_reconstructed" pkg="rtabmap_image" type="image_view_qt">
<remap from="image" to="image_polar_reconstructed"/>
</node>
<node name="view_motion" pkg="rtabmap_image" type="image_view_qt">
<remap from="image" to="image_motion_filtered"/>
</node>
<!-- pop up a dynamic reconfigure -->
<node name="dynamic_reconfigure" pkg="dynamic_reconfigure" type="reconfigure_gui"/>
</launch>
rtabmap_image/mainpage.dox
-14
View File
@@ -1,14 +0,0 @@
/**
\mainpage
\htmlinclude manifest.html
\b rtabmap_image
<!--
Provide an overview of your package.
-->
-->
*/
rtabmap_image/manifest.xml
-22
View File
@@ -1,22 +0,0 @@
<package>
<description brief="rtabmap_image">
rtabmap_image
</description>
<author>Mathieu Labbé</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/rtabmap_image</url>
<depend package="roscpp"/>
<depend package="rospy"/>
<depend package="std_msgs"/>
<depend package="rtabmap_lib"/>
<depend package="image_transport"/>
<depend package="cv_bridge"/>
<depend package="sensor_msgs"/>
<depend package="dynamic_reconfigure"/>
</package>
rtabmap_image/src/CameraNode.cpp
-263
View File
@@ -1,263 +0,0 @@
/*
* CameraNode.cpp
*
* Created on: 1 févr. 2010
* Author: labm2414
*/
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <std_msgs/Empty.h>
#include <image_transport/image_transport.h>
#include <std_srvs/Empty.h>
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/Parameters.h>
#include <utilite/ULogger.h>
#include <utilite/UEventsHandler.h>
#include <utilite/UEventsManager.h>
#include <utilite/UDirectory.h>
#include <utilite/UFile.h>
#include <dynamic_reconfigure/server.h>
#include <rtabmap_image/cameraConfig.h>
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<rtabmap::CameraVideo *>(camera_);
rtabmap::CameraImages * imagesCam = dynamic_cast<rtabmap::CameraImages *>(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<rtabmap_image::cameraConfig> server;
dynamic_reconfigure::Server<rtabmap_image::cameraConfig>::CallbackType f;
f = boost::bind(&callback, _1, _2);
server.setCallback(f);
ros::spin();
//cleanup
if(camera)
{
delete camera;
}
return 0;
}
rtabmap_image/src/Cartesian2PolarNode.cpp
-96
View File
@@ -1,96 +0,0 @@
/*
* RGB2IndexedNode.cpp
*/
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc_c.h>
#include <dynamic_reconfigure/server.h>
#include <rtabmap_image/xy2polarConfig.h>
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 = radiusX<radiusY?radiusX:radiusX;
float M = dp_rings/std::log(radius);
cv::Mat polar(dp_rays, dp_rings, CV_8UC3);
IplImage iplPolar = polar;
IplImage iplImage = ptr->image;
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<rtabmap_image::xy2polarConfig> server;
dynamic_reconfigure::Server<rtabmap_image::xy2polarConfig>::CallbackType f;
f = boost::bind(&callback, _1, _2);
server.setCallback(f);
ros::spin();
return 0;
}
rtabmap_image/src/ImageViewQt.hpp
-231
View File
@@ -1,231 +0,0 @@
/*
* ImageViewQt.hpp
*
* Created on: 2012-06-20
* Author: mathieu
*/
#ifndef IMAGEVIEWQT_HPP_
#define IMAGEVIEWQT_HPP_
#include <QtCore/QTimer>
#include <QtGui/QMouseEvent>
#include <QtGui/QApplication>
#include <QtGui/QWidget>
#include <QtGui/QPainter>
#include <QtGui/QToolTip>
#include <QtGui/QMenu>
#include <utilite/UPlot.h>
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<QPair<int,int>, 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<QPair<int,int>, 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<int,int>(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()<pixmap_.width() &&
pos.y()>=0 && pos.y()<pixmap_.height())
{
QToolTip::showText(event->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()<pixmap_.width() &&
pos.y()>=0 && pos.y()<pixmap_.height())
{
QMenu menu;
QAction * a_plot = menu.addAction(tr("Plot pixel (%1,%2) variation").arg(pos.x()).arg(pos.y()));
QAction * a_frameRate = menu.addAction(tr("Show frame rate"));
a_frameRate->setCheckable(true);
a_frameRate->setChecked(showFrameRate_);
QAction * action = menu.exec(event->globalPos());
if(action == a_plot)
{
if(!pixelMap_.contains(QPair<int,int>(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<int,int>(pos.x(), pos.y()), plot);
}
}
else if(action == a_frameRate)
{
showFrameRate_ = action->isChecked();
}
}
}
}
private:
QPixmap pixmap_;
QMap<QPair<int,int>, RGBPlot*> pixelMap_;
QTime time_;
bool showFrameRate_;
int lastTime_;
};
#endif /* IMAGEVIEWQT_HPP_ */
rtabmap_image/src/ImageViewQtNode.cpp
-162
View File
@@ -1,162 +0,0 @@
/*
* CameraNodeReceiver.cpp
*
* Created on: 2 févr. 2010
* Author: labm2414
*/
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/highgui/highgui.hpp>
#include <utilite/UDirectory.h>
#include <utilite/UConversion.h>
#include <signal.h>
#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;
}
rtabmap_image/src/MotionFilterNode.cpp
-89
View File
@@ -1,89 +0,0 @@
/*
* RGB2IndexedNode.cpp
*/
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <signal.h>
#include <dynamic_reconfigure/server.h>
#include <rtabmap_image/motionFilterConfig.h>
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<motion.rows; ++j)
{
for(int i=0; i<motion.cols; ++i)
{
float b = (float)imageData[j*widthStep+i*3+0];
float g = (float)imageData[j*widthStep+i*3+1];
float r = (float)imageData[j*widthStep+i*3+2];
float previous_b = (float)previous_imageData[j*widthStep+i*3+0];
float previous_g = (float)previous_imageData[j*widthStep+i*3+1];
float previous_r = (float)previous_imageData[j*widthStep+i*3+2];
if(!(fabs(b-previous_b)/256.0f>=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<rtabmap_image::motionFilterConfig> server;
dynamic_reconfigure::Server<rtabmap_image::motionFilterConfig>::CallbackType f;
f = boost::bind(&callback, _1, _2);
server.setCallback(f);
ros::spin();
return 0;
}
rtabmap_image/src/RGB2IndexedNode.cpp
-82
View File
@@ -1,82 +0,0 @@
/*
* RGB2IndexedNode.cpp
*/
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <signal.h>
#include <rtabmap/core/ColorTable.h>
#include <dynamic_reconfigure/server.h>
#include <rtabmap_image/rgb2indConfig.h>
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; i<ind.rows; ++i)
{
for(int j=0; j<ind.cols; ++j)
{
unsigned char & b = imageData[i*widthStep+j*3+0];
unsigned char & g = imageData[i*widthStep+j*3+1];
unsigned char & r = imageData[i*widthStep+j*3+2];
int index = (int)colorTable.getIndex(r, g, b);
colorTable.getRgb(index, r, g , b);
}
}
cv_bridge::CvImage img;
img.header.stamp = ros::Time::now();
img.header.frame_id = ptr->header.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<rtabmap_image::rgb2indConfig> server;
dynamic_reconfigure::Server<rtabmap_image::rgb2indConfig>::CallbackType f;
f = boost::bind(&callback, _1, _2);
server.setCallback(f);
ros::spin();
return 0;
}
rtabmap_lib/Makefile
+1 -1
View File
@@ -4,7 +4,7 @@ INSTALL_DIR = rtabmap
all: installed all: installed
SVN_DIR = build/rtabmap-svn 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_REVISION = -rHEAD
#SVN_PATCH = opencvVer.patch #SVN_PATCH = opencvVer.patch
include $(shell rospack find mk)/svn_checkout.mk include $(shell rospack find mk)/svn_checkout.mk
rtabmap_lib/manifest.xml
+1 -1
View File
@@ -23,7 +23,7 @@
<rosdep name="libqt4-dev"/> <rosdep name="libqt4-dev"/>
<rosdep name="sqlite3"/> <rosdep name="sqlite3"/>
<rosdep name="libsqlite3-dev"/> <rosdep name="libsqlite3-dev"/>
<rosdep name="libfftw3-dev"/> <rosdep name="libfftw3"/>
</package> </package>
utilite/Makefile
-30
View File
@@ -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
utilite/manifest.xml
-24
View File
@@ -1,24 +0,0 @@
<package>
<description brief="utilite">
UtiLite library
</description>
<author>Mathieu Labbe</author>
<license>GPL</license>
<review status="unreviewed" notes=""/>
<url>http://utilite.googlecode.com</url>
<export>
<cpp cflags="-I${prefix}/utilite/include" lflags="-L${prefix}/utilite/lib -Wl,-rpath, -lutilite -lutilite_qt -lutilite_audio"/>
</export>
<versioncontrol type="svn" url="http://utilite.googlecode.com/svn/trunk/"/>
<depend package="roscpp"/>
<depend package="fmodex"/>
<rosdep name="libqt4-dev"/>
<rosdep name="libmp3lame-dev"/>
<rosdep name="libfftw3-dev"/>
</package>
utilite/patched
View File
utilite/rospack_nosubdirs
View File