mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
merged attention branch to trunk
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@657 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
@@ -41,34 +41,11 @@ find_package(OpenCV REQUIRED)
|
||||
rosbuild_add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
|
||||
target_link_libraries(rtabmap ${OpenCV_LIBS})
|
||||
|
||||
rosbuild_add_executable(rtabmap_out src/OutputNode.cpp)
|
||||
rosbuild_add_executable(abtr_velocity src/AbtrVelocityNode.cpp)
|
||||
rosbuild_add_executable(twist_to_turtle_vel src/CmdVelToTurtleVelNode.cpp)
|
||||
rosbuild_add_executable(twist_to_pose src/Twist2Pose.cpp)
|
||||
rosbuild_add_executable(twist_to_twist_stamped src/Twist2TwistStamped.cpp)
|
||||
|
||||
# Input nodes
|
||||
rosbuild_add_boost_directories()
|
||||
rosbuild_add_executable(input_image_audio_node src/ImageAudioInputNode.cpp)
|
||||
target_link_libraries(input_image_audio_node ${OpenCV_LIBS})
|
||||
rosbuild_link_boost(input_image_audio_node signals)
|
||||
|
||||
rosbuild_add_executable(input_image_audio_twist_node src/ImageAudioTwistInputNode.cpp)
|
||||
target_link_libraries(input_image_audio_twist_node ${OpenCV_LIBS})
|
||||
rosbuild_link_boost(input_image_audio_twist_node signals)
|
||||
|
||||
rosbuild_add_executable(input_image_twist_node src/ImageTwistInputNode.cpp)
|
||||
target_link_libraries(input_image_twist_node ${OpenCV_LIBS})
|
||||
rosbuild_link_boost(input_image_twist_node signals)
|
||||
|
||||
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui)
|
||||
IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
|
||||
INCLUDE(${QT_USE_FILE})
|
||||
rosbuild_add_executable(rtabmap_gui src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp)
|
||||
target_link_libraries(rtabmap_gui ${QT_LIBRARIES} ${OpenCV_LIBS} "-lrtabmap_gui")
|
||||
|
||||
rosbuild_add_executable(visual_attention src/VisualAttentionNode.cpp)
|
||||
target_link_libraries(visual_attention ${QT_LIBRARIES} ${OpenCV_LIBS})
|
||||
ELSE()
|
||||
MESSAGE(STATUS "[WARNING] Qt4 not found, the GUI node will not be compiled...")
|
||||
ENDIF()
|
||||
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -9,11 +9,12 @@
|
||||
|
||||
<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"/>
|
||||
<param name="device_id" value="0" type="int"/>
|
||||
<param name="frame_rate" value="1.0" type="double"/>
|
||||
<param name="width" value="640" type="int"/>
|
||||
<param name="height" value="480" type="int"/>
|
||||
<param name="video_or_images_path" value="/media/SD-32GB/NewCollege" type="string"/>
|
||||
<param name="frame_rate" value="2.0" type="double"/>
|
||||
<param name="width" value="0" type="int"/>
|
||||
<param name="height" value="0" type="int"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -17,9 +17,7 @@
|
||||
<depend package="turtlesim"/>
|
||||
<depend package="tf"/>
|
||||
<depend package="rtabmap_lib"/>
|
||||
<depend package="rtabmap_audio"/>
|
||||
<depend package="rtabmap_image"/>
|
||||
|
||||
<depend package="cv_bridge"/>
|
||||
</package>
|
||||
|
||||
|
||||
|
||||
@@ -1,6 +0,0 @@
|
||||
########################################
|
||||
# Actuator
|
||||
########################################
|
||||
|
||||
uint32 type # Actuator type, refer to rtabmap::Actuator::Type enum)
|
||||
rtabmap/CvMatMsg matrix
|
||||
@@ -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)
|
||||
@@ -0,0 +1,10 @@
|
||||
|
||||
########################################
|
||||
# If a loop is found with the current image ("refId"),
|
||||
# "loopClosureId" is not null.
|
||||
########################################
|
||||
|
||||
Header header
|
||||
|
||||
int32 refId
|
||||
int32 loopClosureId
|
||||
@@ -1,13 +1,12 @@
|
||||
|
||||
########################################
|
||||
# Statistics stuff:
|
||||
# These fields are empty if RTAb-Map's
|
||||
# parameter publishStats=false
|
||||
# Extended info msg with statistics
|
||||
########################################
|
||||
|
||||
Header header
|
||||
|
||||
int32 refChild
|
||||
int32 refId
|
||||
int32 loopClosureId
|
||||
|
||||
# std::map<int, float> posterior;
|
||||
int32[] posteriorKeys
|
||||
@@ -25,8 +24,8 @@ int32[] weightsValues
|
||||
string[] statsKeys
|
||||
float32[] statsValues
|
||||
|
||||
rtabmap/SensorMsg[] refRawData
|
||||
rtabmap/SensorMsg[] loopRawData
|
||||
sensor_msgs/Image refImage
|
||||
sensor_msgs/Image loopImage
|
||||
|
||||
#
|
||||
# For features2d : std::multimap<int, cv::Keypoint> words
|
||||
@@ -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
|
||||
@@ -1,8 +0,0 @@
|
||||
########################################
|
||||
# Sensor
|
||||
########################################
|
||||
|
||||
uint32 type # Sensor type (e.g. for sensor: image, audio),
|
||||
# refer to rtabmap::Sensor::Type).
|
||||
rtabmap/CvMatMsg matrix
|
||||
|
||||
@@ -1,4 +0,0 @@
|
||||
Header header
|
||||
|
||||
rtabmap/SensorMsg[] sensors
|
||||
rtabmap/ActuatorMsg[] actuators
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
+68
-86
@@ -6,13 +6,14 @@
|
||||
*/
|
||||
|
||||
#include "CoreWrapper.h"
|
||||
#include "MsgConversion.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/Rtabmap.h>
|
||||
#include <rtabmap/core/Camera.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/core/SensorimotorEvent.h>
|
||||
#include <utilite/UEventsManager.h>
|
||||
#include <utilite/ULogger.h>
|
||||
#include <utilite/UFile.h>
|
||||
@@ -20,9 +21,8 @@
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
//msgs
|
||||
#include "rtabmap/RtabmapInfo.h"
|
||||
#include "rtabmap/RtabmapInfoEx.h"
|
||||
#include "rtabmap/CvMatMsg.h"
|
||||
#include "rtabmap/Info.h"
|
||||
#include "rtabmap/InfoEx.h"
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
@@ -30,8 +30,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
||||
rtabmap_(0)
|
||||
{
|
||||
ros::NodeHandle nh("~");
|
||||
infoPub_ = nh.advertise<rtabmap::RtabmapInfo>("info", 1);
|
||||
infoPubEx_ = nh.advertise<rtabmap::RtabmapInfo>("infoEx", 1);
|
||||
infoPub_ = nh.advertise<rtabmap::Info>("info", 1);
|
||||
infoPubEx_ = nh.advertise<rtabmap::InfoEx>("infoEx", 1);
|
||||
parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1);
|
||||
|
||||
rtabmap_ = new Rtabmap();
|
||||
@@ -52,7 +52,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
||||
|
||||
nh = ros::NodeHandle();
|
||||
parametersUpdatedTopic_ = nh.subscribe("rtabmap_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
|
||||
sensorimotorTopic_ = nh.subscribe("sensorimotor", 1, &CoreWrapper::sensorimotorReceivedCallback, this);
|
||||
|
||||
image_transport::ImageTransport it(nh);
|
||||
imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
|
||||
@@ -116,31 +115,6 @@ void CoreWrapper::saveNodeParameters(const std::string & configFile)
|
||||
ROS_INFO("Database/long-term memory (%lu MB) is located at %s/LTM.db", UFile::length(databasePath)/1000000, databasePath.c_str());
|
||||
}
|
||||
|
||||
void CoreWrapper::sensorimotorReceivedCallback(const rtabmap::SensorimotorConstPtr & msg)
|
||||
{
|
||||
std::list<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)
|
||||
{
|
||||
if(msg->data.size())
|
||||
@@ -197,101 +171,109 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
|
||||
{
|
||||
if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers())
|
||||
{
|
||||
ROS_INFO("Sending RtabmapInfo msg...");
|
||||
RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
|
||||
const Statistics & stat = rtabmapEvent->getStats();
|
||||
|
||||
rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo);
|
||||
|
||||
// General info
|
||||
msg->refId = stat.refImageId();
|
||||
msg->loopClosureId = stat.loopClosureId();
|
||||
|
||||
msg->actuators.resize(stat.getActuators().size());
|
||||
int i=0;
|
||||
for(std::list<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())
|
||||
{
|
||||
ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId());
|
||||
rtabmap::InfoPtr msg(new rtabmap::Info);
|
||||
msg->refId = stat.refImageId();
|
||||
msg->loopClosureId = stat.loopClosureId();
|
||||
infoPub_.publish(msg);
|
||||
}
|
||||
|
||||
if(infoPubEx_.getNumSubscribers())
|
||||
{
|
||||
ROS_INFO("Sending infoEx msg (last_id=%d)...", stat.refImageId());
|
||||
rtabmap::InfoExPtr msg(new rtabmap::InfoEx);
|
||||
msg->refId = stat.refImageId();
|
||||
msg->loopClosureId = stat.loopClosureId();
|
||||
|
||||
// Detailed info
|
||||
if(stat.extended())
|
||||
{
|
||||
if(stat.refRawData().size())
|
||||
if(!stat.refImage().empty())
|
||||
{
|
||||
msg->infoEx.refRawData.resize(stat.refRawData().size());
|
||||
i=0;
|
||||
for(std::list<Sensor>::const_iterator iter = stat.refRawData().begin(); iter!=stat.refRawData().end(); ++iter)
|
||||
cv_bridge::CvImage img;
|
||||
if(stat.refImage().channels() == 1)
|
||||
{
|
||||
msg->infoEx.refRawData[i].type = iter->type();
|
||||
fromCvMatToCvMatMsg(msg->infoEx.refRawData[i++].matrix, iter->data());
|
||||
img.encoding = sensor_msgs::image_encodings::MONO8;
|
||||
}
|
||||
else
|
||||
{
|
||||
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||
}
|
||||
img.image = stat.refImage();
|
||||
sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
|
||||
rosMsg->header.frame_id = "camera";
|
||||
rosMsg->header.stamp = ros::Time::now();
|
||||
msg->refImage = *rosMsg;
|
||||
}
|
||||
if(stat.loopClosureRawData().size())
|
||||
if(!stat.loopImage().empty())
|
||||
{
|
||||
msg->infoEx.loopRawData.resize(stat.loopClosureRawData().size());
|
||||
i=0;
|
||||
for(std::list<Sensor>::const_iterator iter = stat.loopClosureRawData().begin(); iter!=stat.loopClosureRawData().end(); ++iter)
|
||||
cv_bridge::CvImage img;
|
||||
if(stat.loopImage().channels() == 1)
|
||||
{
|
||||
msg->infoEx.loopRawData[i].type = iter->type();
|
||||
fromCvMatToCvMatMsg(msg->infoEx.loopRawData[i++].matrix, iter->data());
|
||||
img.encoding = sensor_msgs::image_encodings::MONO8;
|
||||
}
|
||||
else
|
||||
{
|
||||
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||
}
|
||||
img.image = stat.loopImage();
|
||||
sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
|
||||
rosMsg->header.frame_id = "camera";
|
||||
rosMsg->header.stamp = ros::Time::now();
|
||||
msg->loopImage = *rosMsg;
|
||||
}
|
||||
|
||||
//Posterior, likelihood, childCount
|
||||
msg->infoEx.posteriorKeys = uKeys(stat.posterior());
|
||||
msg->infoEx.posteriorValues = uValues(stat.posterior());
|
||||
msg->infoEx.likelihoodKeys = uKeys(stat.likelihood());
|
||||
msg->infoEx.likelihoodValues = uValues(stat.likelihood());
|
||||
msg->infoEx.weightsKeys = uKeys(stat.weights());
|
||||
msg->infoEx.weightsValues = uValues(stat.weights());
|
||||
msg->posteriorKeys = uKeys(stat.posterior());
|
||||
msg->posteriorValues = uValues(stat.posterior());
|
||||
msg->likelihoodKeys = uKeys(stat.likelihood());
|
||||
msg->likelihoodValues = uValues(stat.likelihood());
|
||||
msg->weightsKeys = uKeys(stat.weights());
|
||||
msg->weightsValues = uValues(stat.weights());
|
||||
|
||||
//Features stuff...
|
||||
msg->infoEx.refWordsKeys = uListToVector(uKeys(stat.refWords()));
|
||||
msg->infoEx.refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
|
||||
msg->refWordsKeys = uKeys(stat.refWords());
|
||||
msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
|
||||
int index = 0;
|
||||
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
|
||||
i!=stat.refWords().end();
|
||||
++i)
|
||||
{
|
||||
msg->infoEx.refWordsValues.at(index).angle = i->second.angle;
|
||||
msg->infoEx.refWordsValues.at(index).response = i->second.response;
|
||||
msg->infoEx.refWordsValues.at(index).ptx = i->second.pt.x;
|
||||
msg->infoEx.refWordsValues.at(index).pty = i->second.pt.y;
|
||||
msg->infoEx.refWordsValues.at(index).size = i->second.size;
|
||||
msg->infoEx.refWordsValues.at(index).octave = i->second.octave;
|
||||
msg->infoEx.refWordsValues.at(index).class_id = i->second.class_id;
|
||||
msg->refWordsValues.at(index).angle = i->second.angle;
|
||||
msg->refWordsValues.at(index).response = i->second.response;
|
||||
msg->refWordsValues.at(index).ptx = i->second.pt.x;
|
||||
msg->refWordsValues.at(index).pty = i->second.pt.y;
|
||||
msg->refWordsValues.at(index).size = i->second.size;
|
||||
msg->refWordsValues.at(index).octave = i->second.octave;
|
||||
msg->refWordsValues.at(index).class_id = i->second.class_id;
|
||||
++index;
|
||||
}
|
||||
|
||||
msg->infoEx.loopWordsKeys = uListToVector(uKeys(stat.loopWords()));
|
||||
msg->infoEx.loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
|
||||
msg->loopWordsKeys = uKeys(stat.loopWords());
|
||||
msg->loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
|
||||
index = 0;
|
||||
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
|
||||
i!=stat.loopWords().end();
|
||||
++i)
|
||||
{
|
||||
msg->infoEx.loopWordsValues.at(index).angle = i->second.angle;
|
||||
msg->infoEx.loopWordsValues.at(index).response = i->second.response;
|
||||
msg->infoEx.loopWordsValues.at(index).ptx = i->second.pt.x;
|
||||
msg->infoEx.loopWordsValues.at(index).pty = i->second.pt.y;
|
||||
msg->infoEx.loopWordsValues.at(index).size = i->second.size;
|
||||
msg->infoEx.loopWordsValues.at(index).octave = i->second.octave;
|
||||
msg->infoEx.loopWordsValues.at(index).class_id = i->second.class_id;
|
||||
msg->loopWordsValues.at(index).angle = i->second.angle;
|
||||
msg->loopWordsValues.at(index).response = i->second.response;
|
||||
msg->loopWordsValues.at(index).ptx = i->second.pt.x;
|
||||
msg->loopWordsValues.at(index).pty = i->second.pt.y;
|
||||
msg->loopWordsValues.at(index).size = i->second.size;
|
||||
msg->loopWordsValues.at(index).octave = i->second.octave;
|
||||
msg->loopWordsValues.at(index).class_id = i->second.class_id;
|
||||
++index;
|
||||
}
|
||||
|
||||
// Statistics data
|
||||
msg->infoEx.statsKeys = uKeys(stat.data());
|
||||
msg->infoEx.statsValues = uValues(stat.data());
|
||||
msg->statsKeys = uKeys(stat.data());
|
||||
msg->statsValues = uValues(stat.data());
|
||||
}
|
||||
infoPubEx_.publish(msg);
|
||||
}
|
||||
|
||||
@@ -18,7 +18,6 @@
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include "rtabmap/Sensorimotor.h"
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
@@ -34,7 +33,6 @@ public:
|
||||
void start();
|
||||
|
||||
private:
|
||||
void sensorimotorReceivedCallback(const rtabmap::SensorimotorConstPtr & msg);
|
||||
void imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg);
|
||||
void twistCallback(const geometry_msgs::TwistConstPtr & msg);
|
||||
void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg);
|
||||
@@ -51,7 +49,6 @@ private:
|
||||
|
||||
private:
|
||||
rtabmap::Rtabmap * rtabmap_;
|
||||
ros::Subscriber sensorimotorTopic_;
|
||||
image_transport::Subscriber imageTopic_;
|
||||
ros::Subscriber audioFrameFreqSqrdMagnTopic_;
|
||||
ros::Subscriber twistTopic_;
|
||||
|
||||
+32
-75
@@ -6,7 +6,6 @@
|
||||
*/
|
||||
|
||||
#include "GuiWrapper.h"
|
||||
#include "MsgConversion.h"
|
||||
#include <QtGui/QApplication>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
@@ -21,8 +20,6 @@
|
||||
#include <rtabmap/core/RtabmapEvent.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/core/Camera.h>
|
||||
#include <rtabmap/core/Sensor.h>
|
||||
#include <rtabmap/core/SensorimotorEvent.h>
|
||||
|
||||
#include "PreferencesDialogROS.h"
|
||||
|
||||
@@ -31,8 +28,7 @@ using namespace rtabmap;
|
||||
GuiWrapper::GuiWrapper(int & argc, char** argv)
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
infoTopic_ = nh.subscribe("rtabmap/infoEx", 1, &GuiWrapper::infoReceivedCallback, this);
|
||||
velocity_sub_ = nh.subscribe("cmd_vel", 1, &GuiWrapper::velocityReceivedCallback, this);
|
||||
infoExTopic_ = nh.subscribe("rtabmap/infoEx", 1, &GuiWrapper::infoExReceivedCallback, this);
|
||||
app_ = new QApplication(argc, argv);
|
||||
mainWindow_ = new MainWindow(new PreferencesDialogROS());
|
||||
mainWindow_->show();
|
||||
@@ -62,9 +58,9 @@ int GuiWrapper::exec()
|
||||
return app_->exec();
|
||||
}
|
||||
|
||||
void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
||||
void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
|
||||
{
|
||||
ROS_INFO("RTAB-Map info received!");
|
||||
ROS_INFO("RTAB-Map info ex received!");
|
||||
|
||||
// Map from ROS struct to rtabmap struct
|
||||
rtabmap::Statistics * stat = new rtabmap::Statistics();
|
||||
@@ -72,112 +68,73 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
||||
stat->setExtended(true); // Extended
|
||||
|
||||
stat->setRefImageId(msg->refId);
|
||||
std::list<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);
|
||||
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);
|
||||
if(s.data().total())
|
||||
{
|
||||
sensors.push_back(s);
|
||||
}
|
||||
stat->setRefImage(cv_bridge::toCvShare(msg->refImage, msg)->image.clone());
|
||||
}
|
||||
if(msg->loopImage.data.size())
|
||||
{
|
||||
stat->setLoopImage(cv_bridge::toCvShare(msg->loopImage, msg)->image.clone());
|
||||
}
|
||||
stat->setLoopClosureRawData(sensors);
|
||||
|
||||
//Posterior, likelihood, childCount
|
||||
std::map<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);
|
||||
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);
|
||||
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);
|
||||
|
||||
//SURF stuff...
|
||||
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;
|
||||
pt.angle = msg->infoEx.refWordsValues.at(i).angle;
|
||||
pt.response = msg->infoEx.refWordsValues.at(i).response;
|
||||
pt.pt.x = msg->infoEx.refWordsValues.at(i).ptx;
|
||||
pt.pt.y = msg->infoEx.refWordsValues.at(i).pty;
|
||||
pt.size = msg->infoEx.refWordsValues.at(i).size;
|
||||
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->infoEx.refWordsKeys.at(i), pt));
|
||||
pt.angle = msg->refWordsValues.at(i).angle;
|
||||
pt.response = msg->refWordsValues.at(i).response;
|
||||
pt.pt.x = msg->refWordsValues.at(i).ptx;
|
||||
pt.pt.y = msg->refWordsValues.at(i).pty;
|
||||
pt.size = msg->refWordsValues.at(i).size;
|
||||
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->refWordsKeys.at(i), pt));
|
||||
}
|
||||
stat->setRefWords(mapIntKeypoint);
|
||||
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;
|
||||
pt.angle = msg->infoEx.loopWordsValues.at(i).angle;
|
||||
pt.response = msg->infoEx.loopWordsValues.at(i).response;
|
||||
pt.pt.x = msg->infoEx.loopWordsValues.at(i).ptx;
|
||||
pt.pt.y = msg->infoEx.loopWordsValues.at(i).pty;
|
||||
pt.size = msg->infoEx.loopWordsValues.at(i).size;
|
||||
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->infoEx.loopWordsKeys.at(i), pt));
|
||||
pt.angle = msg->loopWordsValues.at(i).angle;
|
||||
pt.response = msg->loopWordsValues.at(i).response;
|
||||
pt.pt.x = msg->loopWordsValues.at(i).ptx;
|
||||
pt.pt.y = msg->loopWordsValues.at(i).pty;
|
||||
pt.size = msg->loopWordsValues.at(i).size;
|
||||
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->loopWordsKeys.at(i), pt));
|
||||
}
|
||||
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
|
||||
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...");
|
||||
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)
|
||||
{
|
||||
if(anEvent->getClassName().compare("ParamEvent") == 0)
|
||||
|
||||
@@ -9,8 +9,7 @@
|
||||
#define GUIWRAPPER_H_
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "rtabmap/RtabmapInfo.h"
|
||||
#include "rtabmap/RtabmapInfoEx.h"
|
||||
#include "rtabmap/InfoEx.h"
|
||||
#include "utilite/UEventsHandler.h"
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
|
||||
@@ -33,12 +32,10 @@ protected:
|
||||
virtual void handleEvent(UEvent * anEvent);
|
||||
|
||||
private:
|
||||
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg);
|
||||
void velocityReceivedCallback(const geometry_msgs::TwistStampedConstPtr & msg);
|
||||
void infoExReceivedCallback(const rtabmap::InfoExConstPtr & infoMsg);
|
||||
|
||||
private:
|
||||
ros::Subscriber infoTopic_;
|
||||
ros::Subscriber velocity_sub_;
|
||||
ros::Subscriber infoExTopic_;
|
||||
QApplication * app_;
|
||||
rtabmap::MainWindow * mainWindow_;
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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_ */
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user