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

MERGE branch STM 325:449 into trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@450 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2012-03-03 01:46:30 +00:00
parent 06eb9a2424
commit c26a61f0c5
42 changed files with 1023 additions and 425 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/CMakeLists.txt
+20 -10
View File
@@ -36,20 +36,30 @@ rosbuild_gensrv()
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
find_package(OpenCV REQUIRED)
rosbuild_add_executable(camera src/CameraNode.cpp src/CameraWrapper.cpp)
target_link_libraries(camera ${OpenCV_LIBS})
rosbuild_add_executable(camera_receiver src/CameraNodeReceiver.cpp)
target_link_libraries(camera_receiver ${OpenCV_LIBS})
rosbuild_add_executable(camera_receiver_sms src/CameraNodeReceiverSM.cpp)
target_link_libraries(camera_receiver_sms ${OpenCV_LIBS})
rosbuild_add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
target_link_libraries(rtabmap ${OpenCV_LIBS})
rosbuild_add_executable(rtabmap_in src/InputNode.cpp)
target_link_libraries(rtabmap_in ${OpenCV_LIBS})
rosbuild_add_executable(rtabmap_out src/OutputNode.cpp)
rosbuild_add_executable(abtr_velocity src/AbtrVelocityNode.cpp)
rosbuild_add_executable(image_to_sms src/ImageToSensorimotorStateNode.cpp)
target_link_libraries(image_to_sms ${OpenCV_LIBS})
rosbuild_add_executable(twist_to_sms src/TwistToSensorimotorStateNode.cpp)
rosbuild_add_executable(twist_to_turtle_vel src/CmdVelToTurtleVelNode.cpp)
rosbuild_add_executable(twist_to_poses src/TwistToPoses.cpp)
rosbuild_add_executable(camera_node src/CameraNode.cpp src/CameraWrapper.cpp)
rosbuild_add_executable(camera_node_receiver src/CameraNodeReceiver.cpp)
rosbuild_add_executable(camera_node_receiver_sm src/CameraNodeReceiverSM.cpp)
rosbuild_add_executable(core_node src/CoreNode.cpp src/CoreWrapper.cpp)
rosbuild_add_executable(input_node src/InputNode.cpp)
rosbuild_add_executable(output_node src/OutputNode.cpp)
rosbuild_add_executable(abtr_velocity_node src/AbtrVelocityNode.cpp)
rosbuild_add_executable(image_to_sms_node src/ImageToSensorimotorStateNode.cpp)
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui)
IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
INCLUDE(${QT_USE_FILE})
rosbuild_add_executable(gui_node src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp)
target_link_libraries(gui_node ${QT_LIBRARIES} rtabmap_gui )
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")
ELSE()
MESSAGE(STATUS "[WARNING] Qt4 not found, the GUI node will not be compiled...")
ENDIF()
rtabmap/launch/all_learning.launch
-12
View File
@@ -1,12 +0,0 @@
<launch>
<!-- TELEOP -->
<include file="$(find rtabmap)/launch/teleop.launch"/>
<!-- RTAB-MAP -->
<include file="$(find rtabmap)/launch/rtabmap_sm.launch"/>
<!-- CAMERA -->
<include file="$(find rtabmap)/launch/camera.launch"/>
</launch>
rtabmap/launch/all_tr_learning_omni.launch
-22
View File
@@ -1,22 +0,0 @@
<launch>
<!-- BRING UP TELEROBOT -->
<!-- BUS CAN MANAGER -->
<group ns="tr">
<param name="driver_type" value="TRCANDriver"/>
<node name="can_manager" type="can_manager" pkg="can_manager"
args="tr">
<remap from="/tr/tr/cmd_vel" to="/cmd_vel" />
</node>
</group>
<!-- TELEOP -->
<include file="$(find rtabmap)/launch/teleop.launch"/>
<!-- RTAB-MAP -->
<include file="$(find rtabmap)/launch/rtabmap_sm.launch"/>
<!-- CAMERA -->
<include file="$(find rtabmap)/launch/cameraOmni.launch"/>
</launch>
rtabmap/launch/az2_learning.launch
+62
View File
@@ -0,0 +1,62 @@
<launch>
<!-- BRING UP AZIMUT2 -->
<include file="$(find az2_bringup)/az2_base_controller.launch"/>
<!-- TELEOP JOYSTICK -->
<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
<remap from="cmd_vel" to="user/cmd_vel" />
</node>
<node name="joystick" pkg="joy" type="joy_node">
<param name="autorepeat_rate" value="20" type="double" />
</node>
<!-- RTAB-MAP SENSORY-MOTOR VERSION -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- cmd_vel_a has priority on cmd_vel_b -->
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
<param name="commands_hz" value="10.0" type="double"/>
<param name="cmd_vel_a_buffered" value="false" type="bool"/>
<param name="cmd_vel_b_buffered" value="true" type="bool"/>
<param name="stats_logged" value="true" type="bool"/>
<remap from="cmd_vel_a" to="user/cmd_vel" />
<remap from="cmd_vel_b" to="rtabmap/cmd_vel" />
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
</node>
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="twist_to_poses" pkg="rtabmap" type="twist_to_poses">
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
</node>
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<remap from="sm_state" to="sm_state" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="rtabmap_in" pkg="rtabmap" type="rtabmap_in">
<param name="nb_commands" value="2" type="int"/>
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
<remap from="sm_state" to="sm_state" />
<remap from="sensor_data" to="sensor_data" />
</node>
<node name="image_to_sms" pkg="rtabmap" type="image_to_sms">
<param name="image_hz" value="5.0" type="double"/>
<param name="resize_image_width" value="80" type="int"/>
<param name="resize_image_height" value="60" type="int"/>
<param name="keypoints_extracted" value="0" type="int"/>
<remap from="image" to="camera/image" />
<remap from="sm_state" to="sensor_data" />
</node>
</launch>
rtabmap/launch/az2_teleop.launch
+14
View File
@@ -0,0 +1,14 @@
<launch>
<!-- BRING UP AZIMUT2 -->
<include file="$(find az2_bringup)/az2_base_controller.launch"/>
<!-- TELEOP JOYSTICK -->
<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
</node>
<node name="joystick" pkg="joy" type="joy_node">
<param name="autorepeat_rate" value="20" type="double" />
</node>
</launch>
rtabmap/launch/az3_learning.launch
+62
View File
@@ -0,0 +1,62 @@
<launch>
<!-- BRING UP AZIMUT3 -->
<include file="$(find az3_bringup)/az3_base_controller.launch"/>
<!-- TELEOP JOYSTICK -->
<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
<remap from="cmd_vel" to="user/cmd_vel" />
</node>
<node name="joystick" pkg="joy" type="joy_node">
<param name="autorepeat_rate" value="20" type="double" />
</node>
<!-- RTAB-MAP SENSORY-MOTOR VERSION -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- cmd_vel_a has priority on cmd_vel_b -->
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
<param name="commands_hz" value="10.0" type="double"/>
<param name="cmd_vel_a_buffered" value="false" type="bool"/>
<remap from="cmd_vel_a" to="user/cmd_vel" />
<remap from="cmd_vel_b" to="rtabmap/cmd_vel" />
<remap from="cmd_vel" to="az3/base_controller/cmd_vel" />
</node>
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
<param name="commands_hz" value="10.0" type="double"/>
<param name="idle_null_commands_sent" value="false" type="bool"/>
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="twist_to_poses" pkg="rtabmap" type="twist_to_poses">
<remap from="cmd_vel" to="az3/base_controller/cmd_vel" />
</node>
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<remap from="sm_state" to="sm_state" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="rtabmap_in" pkg="rtabmap" type="rtabmap_in">
<param name="nb_commands" value="2" type="int"/>
<remap from="cmd_vel" to="az3/base_controller/cmd_vel" />
<remap from="sm_state" to="sm_state" />
<remap from="sensor_data" to="sensor_data" />
</node>
<node name="image_to_sms" pkg="rtabmap" type="image_to_sms">
<param name="image_hz" value="5.0" type="double"/>
<param name="resize_image_width" value="80" type="int"/>
<param name="resize_image_height" value="60" type="int"/>
<param name="keypoints_extracted" value="0" type="int"/>
<remap from="image" to="camera/image" />
<remap from="sm_state" to="sensor_data" />
</node>
</launch>
rtabmap/launch/az3_teleop.launch
+14
View File
@@ -0,0 +1,14 @@
<launch>
<!-- BRING UP AZIMUT2 -->
<include file="$(find az3_bringup)/az3_base_controller.launch"/>
<!-- TELEOP JOYSTICK -->
<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
<remap from="cmd_vel" to="az3/base_controller/cmd_vel" />
</node>
<node name="joystick" pkg="joy" type="joy_node">
<param name="autorepeat_rate" value="20" type="double" />
</node>
</launch>
rtabmap/launch/cameraOpenCV.launch
+6 -7
View File
@@ -1,10 +1,9 @@
<launch>
<!-- Camera parameters -->
<param name="cam/device_id" value="0" type="int"/>
<param name="cam/image_rate" value="1" type="int"/>
<param name="cam/image_width" value="640" type="int"/>
<param name="cam/image_height" value="480" type="int"/>
<!-- Nodes -->
<node name="camera" pkg="rtabmap" type="camera_node"/>
<node name="camera" pkg="rtabmap" type="camera" output="screen">
<param name="device_id" value="0" type="int"/>
<param name="image_hz" value="5.0" type="double"/>
<param name="image_width" value="640" type="int"/>
<param name="image_height" value="480" type="int"/>
</node>
</launch>
rtabmap/launch/camera.launch → rtabmap/launch/cameraUVC.launch
View File
rtabmap/launch/loop_closure_detection.launch
+19
View File
@@ -0,0 +1,19 @@
<launch>
<!-- RTAB-MAP LOOP CLOSURE DETECTION VERSION -->
<!-- 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"/>
<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="1.0" type="double"/>
<param name="image_width" value="640" type="int"/>
<param name="image_height" value="480" type="int"/>
</node>
</launch>
rtabmap/launch/rtabmap_lc.launch
-14
View File
@@ -1,14 +0,0 @@
<launch>
<!-- RTAB-MAP LOOP CLOSURE DETECTION VERSION -->
<!-- Nodes -->
<node name="rtabmap_core" pkg="rtabmap" type="core_node"/>
<node name="rtabmap_gui" pkg="rtabmap" type="gui_node"/>
<node name="image_to_sms" pkg="rtabmap" type="image_to_sms_node">
<param name="image_rate" value="1" type="int"/>
<param name="resize_image_width" value="640" type="int"/>
<param name="resize_image_height" value="480" type="int"/>
</node>
<!-- CAMERA -->
<include file="$(find rtabmap)/launch/camera.launch"/>
</launch>
rtabmap/launch/rtabmap_sm.launch
-41
View File
@@ -1,41 +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_node">
<remap from="cmd_vel_a" to="user/cmd_vel" />
<remap from="cmd_vel_b" to="rtabmap/cmd_vel" />
<remap from="cmd_vel" to="cmd_vel" />
</node>
<node name="rtabmap_output" pkg="rtabmap" type="output_node">
<param name="commands_hz" value="10" type="int"/>
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
<remap from="rtabmap_info" to="rtabmap_info" />
<remap from="rtabmap_info_x" to="rtabmap_info_x" />
</node>
<node name="rtabmap_core" pkg="rtabmap" type="core_node" output="screen" args="--delete_db_on_start">
<remap from="sm_state" to="sm_state" />
<remap from="rtabmap_info" to="rtabmap_info" />
<remap from="rtabmap_info_x" to="rtabmap_info_x" />
</node>
<node name="rtabmap_input" pkg="rtabmap" type="input_node">
<param name="nb_commands" value="10" type="int"/>
<remap from="/cmd_vel" to="/cmd_vel" />
<remap from="/sm_state" to="/sm_state" />
<remap from="/camera_data" to="/camera_data" />
</node>
<node name="image_to_sms" pkg="rtabmap" type="image_to_sms_node">
<param name="image_rate" value="1" type="int"/>
<param name="resize_image_width" value="640" type="int"/>
<param name="resize_image_height" value="480" type="int"/>
<remap from="/image_raw" to="/image_raw" />
<remap from="/sm_state" to="/camera_data" />
</node>
</launch>
rtabmap/launch/teleop_keyboard.launch
+10
View File
@@ -0,0 +1,10 @@
<launch>
<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>
</launch>
rtabmap/launch/test_cameraOpenCV.launch
+16
View File
@@ -0,0 +1,16 @@
<launch>
<!-- Nodes -->
<node name="camera_receiver" pkg="rtabmap" type="camera_receiver" output="screen">
<remap from="image" to="camera/image"/>
<remap from="image/compressed" to="camera/image/compressed"/>
<param name="image_transport" value="raw" type="str"/>
</node>
<!-- Nodes -->
<node name="camera" pkg="rtabmap" type="camera" output="screen">
<param name="device_id" value="0" type="int"/>
<param name="image_hz" value="30.0" type="double"/>
<param name="image_width" value="640" type="int"/>
<param name="image_height" value="480" type="int"/>
</node>
</launch>
rtabmap/launch/testCamera.launch → rtabmap/launch/test_cameraUVC.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch>
<!-- Nodes -->
<node name="cameraReceiver" pkg="rtabmap" type="camera_node_receiver" output="screen"/>
<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" />
rtabmap/launch/test_step_by_step_actions_only.launch
+48
View File
@@ -0,0 +1,48 @@
<launch>
<!-- RTAB-MAP SENSORY-MOTOR VERSION -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- Nodes -->
<!-- cmd_vel_a has priority on cmd_vel_b -->
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
<param name="commands_hz" value="1.0" type="double"/>
<param name="cmd_vel_a_buffered" value="true" type="bool"/>
<remap from="cmd_vel_a" to="user/cmd_vel" />
<!-- <remap from="cmd_vel_b" to="rtabmap/cmd_vel" /> -->
<remap from="cmd_vel" to="cmd_vel" />
</node>
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
<param name="commands_hz" value="1.0" type="double"/>
<param name="idle_null_commands_sent" value="false" type="bool"/>
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<remap from="sm_state" to="sm_state" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="rtabmap_in" pkg="rtabmap" type="rtabmap_in">
<param name="nb_commands" value="1" type="int"/>
<remap from="cmd_vel" to="cmd_vel" />
<remap from="sm_state" to="sm_state" />
<remap from="sensor_data" to="sensor_data" />
</node>
<node name="twist_to_sms" pkg="rtabmap" type="twist_to_sms">
<remap from="cmd_vel" to="cmd_vel" />
<remap from="sm_state" to="sensor_data" />
</node>
<node name="twist_to_turtle_vel" pkg="rtabmap" type="twist_to_turtle_vel">
</node>
<node name="turtlesim" pkg="turtlesim" type="turtlesim_node">
</node>
</launch>
rtabmap/launch/test_step_by_step_az2.launch
+63
View File
@@ -0,0 +1,63 @@
<launch>
<!-- BRING UP AZIMUT2 -->
<include file="$(find az2_bringup)/az2_base_controller.launch"/>
<!-- RTAB-MAP SENSORY-MOTOR VERSION -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- Nodes -->
<!-- cmd_vel_a has priority on cmd_vel_b -->
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
<param name="commands_hz" value="0.5" type="double"/>
<param name="cmd_vel_a_buffered" value="true" type="bool"/>
<remap from="cmd_vel_a" to="user/cmd_vel" />
<!-- <remap from="cmd_vel_b" to="rtabmap/cmd_vel" /> -->
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
</node>
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
<param name="commands_hz" value="1.0" type="double"/>
<param name="idle_null_commands_sent" value="false" type="bool"/>
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<remap from="sm_state" to="sm_state" />
<remap from="rtabmap/info" to="rtabmap/info" />
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
</node>
<node name="rtabmap_in" pkg="rtabmap" type="rtabmap_in">
<param name="nb_commands" value="1" type="int"/>
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
<remap from="sm_state" to="sm_state" />
<remap from="sensor_data" to="sensor_data" />
</node>
<node name="image_to_sms" pkg="rtabmap" type="image_to_sms">
<param name="image_hz" value="1" type="double"/>
<param name="resize_image_width" value="160" type="int"/>
<param name="resize_image_height" value="120" type="int"/>
<param name="keypoints_extracted" value="0" type="int"/>
<remap from="image" to="camera/image" />
<remap from="sm_state" to="sensor_data" />
</node>
<!--
<node name="twist_to_sms" pkg="rtabmap" type="twist_to_sms">
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
<remap from="sm_state" to="sensor_data" />
</node>
-->
<node name="twist_to_turtle_vel" pkg="rtabmap" type="twist_to_turtle_vel_node">
<remap from="cmd_vel" to="az2/base_controller/cmd_vel" />
</node>
</launch>
rtabmap/manifest.xml
+5 -3
View File
@@ -10,13 +10,15 @@
<url>http://rtabmap-ros-pkg.googlecode.com</url>
<depend package="std_msgs"/>
<depend package="roscpp"/>
<depend package="image_transport"/>
<depend package="cv_bridge"/>
<depend package="sensor_msgs"/>
<depend package="std_srvs"/>
<depend package="opencv2"/>
<depend package="uvc_camera"/>
<depend package="rtabmap_lib"/>
<depend package="turtlesim"/>
<depend package="tf"/>
<depend package="rtabmap_lib"/>
<rosdep name="opencv2.3"/>
</package>
rtabmap/msg/RtabmapInfoEx.msg
+5 -1
View File
@@ -36,4 +36,8 @@ rtabmap/KeyPoint[] refWordsValues
#std::multimap<int, cv::KeyPoint> loopWords
int32[] loopWordsKeys
rtabmap/KeyPoint[] loopWordsValues
rtabmap/KeyPoint[] loopWordsValues
##SM masks##
uint8[] refMotionMask
uint8[] loopMotionMask
rtabmap/src/AbtrVelocityNode.cpp
+158 -60
View File
@@ -7,89 +7,187 @@
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <utilite/UMutex.h>
#include <queue>
#include <utilite/ULogger.h>
UMutex commandMutex;
std::vector<float> commandsA;
std::vector<float> commandsB;
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)
{
//ROS_INFO("Received command velocity A (%f,%f)", msg->linear, msg->angular);
commandMutex.lock();
{
commandsA.push_back(msg->linear.x);
commandsA.push_back(msg->linear.y);
commandsA.push_back(msg->linear.z);
commandsA.push_back(msg->angular.x);
commandsA.push_back(msg->angular.y);
commandsA.push_back(msg->angular.z);
}
commandMutex.unlock();
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)
{
//ROS_INFO("Received command velocity B (%f,%f)", msg->linear, msg->angular);
commandMutex.lock();
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())
{
commandsB.push_back(msg->linear.x);
commandsB.push_back(msg->linear.y);
commandsB.push_back(msg->linear.z);
commandsB.push_back(msg->angular.x);
commandsB.push_back(msg->angular.y);
commandsB.push_back(msg->angular.z);
lastCommandsB = std::vector<float>(6);
}
commandMutex.unlock();
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, "input_node");
ros::NodeHandle n;
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;
velATopic = n.subscribe("cmd_vel_a", 1, velocityAReceivedCallback);
velBTopic = n.subscribe("cmd_vel_b", 1, velocityBReceivedCallback);
rosPublisher = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
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);
}
ros::Rate loop_rate(10); // 10 Hz
rosPublisher = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
int index = 1;
ros::Rate loop_rate(commandsHz); // 10 Hz
while(ros::ok())
{
commandMutex.lock();
{
// priority for commandsA
if(commandsA.size())
{
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
vel->linear.x = commandsA[0];
vel->linear.y = commandsA[1];
vel->linear.z = commandsA[2];
vel->angular.x = commandsA[3];
vel->angular.y = commandsA[4];
vel->angular.z = commandsA[5];
rosPublisher.publish(vel);
}
else if(commandsB.size())
{
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
vel->linear.x = commandsB[0];
vel->linear.y = commandsB[1];
vel->linear.z = commandsB[2];
vel->angular.x = commandsB[3];
vel->angular.y = commandsB[4];
vel->angular.z = commandsB[5];
rosPublisher.publish(vel);
}
commandsA.clear();
commandsB.clear();
}
commandMutex.unlock();
ros::spinOnce();
loop_rate.sleep();
ros::spinOnce();
// priority for commandsA
if(commandsA.size())
{
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
vel->linear.x = commandsA.front();
commandsA.pop();
vel->linear.y = commandsA.front();
commandsA.pop();
vel->linear.z = commandsA.front();
commandsA.pop();
vel->angular.x = commandsA.front();
commandsA.pop();
vel->angular.y = commandsA.front();
commandsA.pop();
vel->angular.z = commandsA.front();
commandsA.pop();
rosPublisher.publish(vel);
UINFO("%d A %f %f %f", index, vel->linear.x, vel->linear.y, vel->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())
{
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
vel->linear.x = commandsB.front();
commandsB.pop();
vel->linear.y = commandsB.front();
commandsB.pop();
vel->linear.z = commandsB.front();
commandsB.pop();
vel->angular.x = commandsB.front();
commandsB.pop();
vel->angular.y = commandsB.front();
commandsB.pop();
vel->angular.z = commandsB.front();
commandsB.pop();
UINFO("%d B %f %f %f", index, vel->linear.x, vel->linear.y, vel->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)
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
vel->linear.x = lastCommandsB[0];
vel->linear.y = lastCommandsB[1];
vel->linear.z = lastCommandsB[2];
vel->angular.x = lastCommandsB[3];
vel->angular.y = lastCommandsB[4];
vel->angular.z = lastCommandsB[5];
rosPublisher.publish(vel);
lastCommandsB = std::vector<float>();
}
}
++index;
}
if(statsLogged)
{
ULogger::flush();
}
return 0;
rtabmap/src/CameraNode.cpp
+21 -15
View File
@@ -13,36 +13,42 @@
// See the launch file to change the camera values
#define DEFAULT_DEVICE_ID 0
#define DEFAULT_IMG_RATE 1 //Hz
#define DEFAULT_IMG_RATE 1.0 //Hz
#define DEFAULT_AUTO_RESTART 0
#define DEFAULT_IMG_WIDTH 0
#define DEFAULT_IMG_HEIGHT 0
int main(int argc, char** argv)
{
ros::init(argc, argv, "camera_node");
ros::init(argc, argv, "camera");
CameraWrapper * camera = 0;
ros::NodeHandle nh;
ros::NodeHandle nh("~");
int deviceId = DEFAULT_DEVICE_ID;
int imgRate = DEFAULT_IMG_RATE;
double imgRate = DEFAULT_IMG_RATE;
int autoRestart = DEFAULT_AUTO_RESTART;
int imgWidth = DEFAULT_IMG_WIDTH;
int imgHeight = DEFAULT_IMG_HEIGHT;
nh.param("cam/device_id", deviceId, deviceId);
nh.param("cam/image_rate", imgRate, imgRate);
nh.param("cam/auto_restart", autoRestart, autoRestart);
nh.param("cam/image_width", imgWidth, imgWidth);
nh.param("cam/image_height", imgHeight, imgHeight);
nh.param("device_id", deviceId, deviceId);
nh.param("image_hz", imgRate, imgRate);
nh.param("auto_restart", autoRestart, autoRestart);
nh.param("image_width", imgWidth, imgWidth);
nh.param("image_height", imgHeight, imgHeight);
ROS_INFO("cam/device_id=%d", deviceId);
ROS_INFO("cam/image_rate=%d", imgRate);
ROS_INFO("cam/auto_restart=%d", autoRestart);
ROS_INFO("cam/image_width=%d", imgWidth);
ROS_INFO("cam/image_height=%d", imgHeight);
ROS_INFO("device_id=%d", deviceId);
ROS_INFO("image_hz=%f", imgRate);
ROS_INFO("auto_restart=%d", autoRestart);
ROS_INFO("image_width=%d", imgWidth);
ROS_INFO("image_height=%d", imgHeight);
camera = new CameraVideoWrapper(deviceId, imgRate, autoRestart, imgWidth, imgHeight); // webcam device 0
nh.setParam("device_id", deviceId);
nh.setParam("image_hz", imgRate);
nh.setParam("auto_restart", autoRestart);
nh.setParam("image_width", imgWidth);
nh.setParam("image_height", imgHeight);
camera = new CameraVideoWrapper(deviceId, float(imgRate), autoRestart, imgWidth, imgHeight); // webcam device 0
if(!camera || (camera && !camera->init()))
{
rtabmap/src/CameraNodeReceiver.cpp
+55 -24
View File
@@ -6,49 +6,80 @@
*/
#include <ros/ros.h>
//#include <sensor_msgs/CompressedImage.h>
#include <cv_bridge/CvBridge.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <highgui.h>
#include <utilite/UDirectory.h>
#include <utilite/UConversion.h>
bool imagesSaved = false;
int i = 0;
void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
{
// Decompress
//const CvMat compressed = cvMat(1, image->data.size(), CV_8UC1, const_cast<unsigned char*>(&image->data[0]));
//IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
//ROS_INFO("Received an image size=(%d,%d)", decompressed->width, decompressed->height);
//cvShowImage( "ImageReceived", decompressed );
//cvReleaseImage(&decompressed);
IplImage * image = 0;
sensor_msgs::CvBridge bridge;
if(msg->data.size())
{
image = bridge.imgMsgToCv(msg);
}
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
IplImage image = ptr->image;
if(image)
{
ROS_INFO("Received an image size=(%d,%d)", image->width, image->height);
cvShowImage( "ImageReceived", image);
ROS_INFO("Received an image size=(%d,%d)", image.width, image.height);
if(imagesSaved)
{
std::string path = "./imagesSaved";
if(!UDirectory::exists(path))
{
if(!UDirectory::makeDir(path))
{
ROS_ERROR("Cannot make dir %s", path.c_str());
}
}
path.append("/");
path.append(uNumber2Str(i++));
path.append(".bmp");
if(!cvSaveImage(path.c_str(), &image))
{
ROS_ERROR("Cannot save image to %s", path.c_str());
}
else
{
ROS_INFO("Saved image %s", path.c_str());
}
}
else
{
cvShowImage( "ImageReceived", &image);
}
}
}
int main(int argc, char** argv)
{
cvStartWindowThread();
cvNamedWindow("ImageReceived", CV_WINDOW_AUTOSIZE);
cvMoveWindow("ImageReceived", 100, 100); // offset from the UL corner of the screen
ros::init(argc, argv, "camera_receiver");
ros::NodeHandle pn("~");
pn.param("images_saved", imagesSaved, imagesSaved);
ROS_INFO("images_saved=%d", imagesSaved?1:0);
if(!imagesSaved)
{
cvStartWindowThread();
cvNamedWindow("ImageReceived", CV_WINDOW_AUTOSIZE);
cvMoveWindow("ImageReceived", 100, 100); // offset from the UL corner of the screen
}
ros::init(argc, argv, "camera_node_receiver");
ros::NodeHandle n;
ros::Subscriber image_sub = n.subscribe("/image_raw", 1, imgReceivedCallback);
image_transport::ImageTransport it(n);
image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
ROS_INFO("Waiting for images...");
ros::spin();
cvDestroyWindow("ImageReceived");
if(!imagesSaved)
{
cvDestroyWindow("ImageReceived");
}
return 0;
}
rtabmap/src/CameraNodeReceiverSM.cpp
+7 -11
View File
@@ -7,23 +7,19 @@
#include <ros/ros.h>
#include "rtabmap/SensoryMotorState.h"
#include <cv_bridge/CvBridge.h>
#include <cv_bridge/cv_bridge.h>
#include <highgui.h>
void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
{
IplImage * image = 0;
sensor_msgs::CvBridge bridge;
if( msg->image.data.size())
{
bridge.fromImage(msg->image);
image = bridge.toIpl();
}
boost::shared_ptr<sensor_msgs::Image> tracked_object;
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg->image, tracked_object);
IplImage image = ptr->image;
if(image)
{
ROS_INFO("Received an image size=(%d,%d)", image->width, image->height);
cvShowImage( "ImageReceived", image);
ROS_INFO("Received an image size=(%d,%d)", image.width, image.height);
cvShowImage( "ImageReceived", &image);
}
}
@@ -33,7 +29,7 @@ int main(int argc, char** argv)
cvNamedWindow("ImageReceived", CV_WINDOW_AUTOSIZE);
cvMoveWindow("ImageReceived", 100, 100); // offset from the UL corner of the screen
ros::init(argc, argv, "camera_node_receiver_sm");
ros::init(argc, argv, "camera_receiver_sms");
ros::NodeHandle n;
ros::Subscriber image_sub = n.subscribe("/sm_state", 1, smReceivedCallback);
rtabmap/src/CameraWrapper.cpp
+17 -45
View File
@@ -6,8 +6,8 @@
*/
#include "CameraWrapper.h"
#include <cv_bridge/CvBridge.h>
//#include <sensor_msgs/CompressedImage.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/Image.h>
#include <rtabmap/core/CameraEvent.h>
#include <rtabmap/core/SMState.h>
@@ -21,8 +21,10 @@
CameraWrapper::CameraWrapper() :
camera_(0)
{
rosPublisher_ = nh_.advertise<sensor_msgs::Image>("image_raw", 1);
changeCameraImgRateSrv_ = nh_.advertiseService("changeCameraImgRate", &CameraWrapper::changeCameraImgRateCallback, this);
ros::NodeHandle nh("~");
image_transport::ImageTransport it(nh);
rosPublisher_ = it.advertise("image", 1);
changeCameraImgRateSrv_ = nh.advertiseService("changeImgRate", &CameraWrapper::changeCameraImgRateCallback, this);
UEventsManager::addHandler(this);
}
@@ -72,9 +74,11 @@ void CameraWrapper::start()
bool CameraWrapper::changeCameraImgRateCallback(rtabmap::ChangeCameraImgRate::Request & request, rtabmap::ChangeCameraImgRate::Response & response)
{
nh_.setParam("image_rate", request.imgRate);
nh_.setParam("auto_restart", request.autoRestart);
UEventsManager::post(new rtabmap::CameraEvent(rtabmap::CameraEvent::kCmdChangeParam, request.imgRate, request.autoRestart));
ros::NodeHandle nh("~");
nh.setParam("image_hz", request.imgRate);
nh.setParam("auto_restart", request.autoRestart);
camera_->setImageRate(request.imgRate);
camera_->setAutoRestart(request.autoRestart);
return true;
}
@@ -86,42 +90,10 @@ void CameraWrapper::handleEvent(UEvent* anEvent)
const rtabmap::SMState * smState = e->getSMState();
if(smState && smState->getImage())
{
try
{
//int params[3] = {0};
//JPEG compression
//std::string format = "jpeg";
//params[0] = CV_IMWRITE_JPEG_QUALITY;
//params[1] = 80; // default: 80% quality
//PNG compression
//std::string format = "png";
//params[0] = CV_IMWRITE_PNG_COMPRESSION;
//params[1] = 9; // default: maximum compression
//std::string extension = '.' + format;
// Compress image
//const IplImage* image = e->getImage();
//CvMat* buf = cvEncodeImage(extension.c_str(), image, params);
// Set up message and publish
//sensor_msgs::CompressedImage compressed;
//compressed.format = format;
//compressed.data.resize(buf->width);
//memcpy(&compressed.data[0], buf->data.ptr, buf->width);
//cvReleaseMat(&buf);
//ROS_INFO("Publishing an image");
//rosPublisher_.publish(compressed);
sensor_msgs::ImagePtr msg = sensor_msgs::CvBridge::cvToImgMsg(smState->getImage());
rosPublisher_.publish(msg);
}
catch (sensor_msgs::CvBridgeException & ex)
{
ROS_ERROR("%s", ex.what());
}
cv_bridge::CvImage img;
img.encoding = sensor_msgs::image_encodings::BGR8;
img.image = smState->getImage();
rosPublisher_.publish(img.toImageMsg());
}
}
}
@@ -161,11 +133,11 @@ CameraVideoWrapper::CameraVideoWrapper(const std::string & fileName,
// Camera database wrapper
CameraDatabaseWrapper::CameraDatabaseWrapper(const std::string & path,
bool ignoreChildren,
bool actionsLoaded,
float imageRate,
bool autoRestart,
unsigned int imageWidth,
unsigned int imageHeight)
{
this->setCamera(new rtabmap::CameraDatabase(path, ignoreChildren, imageRate, autoRestart, imageWidth, imageHeight));
this->setCamera(new rtabmap::CameraDatabase(path, actionsLoaded, imageRate, autoRestart, imageWidth, imageHeight));
}
rtabmap/src/CameraWrapper.h
+3 -3
View File
@@ -12,6 +12,7 @@
#include <ros/ros.h>
#include "rtabmap/ChangeCameraImgRate.h"
#include <std_msgs/Empty.h>
#include <image_transport/image_transport.h>
namespace Util
{
@@ -44,8 +45,7 @@ private:
bool changeCameraImgRateCallback(rtabmap::ChangeCameraImgRate::Request&, rtabmap::ChangeCameraImgRate::Response&);
private:
ros::NodeHandle nh_;
ros::Publisher rosPublisher_;
image_transport::Publisher rosPublisher_;
rtabmap::Camera * camera_;
ros::ServiceServer changeCameraImgRateSrv_;
};
@@ -94,7 +94,7 @@ class CameraDatabaseWrapper : public CameraWrapper
public:
// Usb device like a Webcam
CameraDatabaseWrapper(const std::string & path,
bool ignoreChildren,
bool actionsLoaded,
float imageRate = 0,
bool autoRestart = false,
unsigned int imageWidth = 0,
rtabmap/src/CmdVelToTurtleVelNode.cpp
+33
View File
@@ -0,0 +1,33 @@
/*
* CameraNode.cpp
*
* Created on: 1 févr. 2010
* Author: labm2414
*/
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Velocity.h>
ros::Publisher rosPublisher;
void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
{
turtlesim::Velocity vel;
vel.angular = msg->linear.y;
vel.linear = msg->linear.x;
rosPublisher.publish(vel);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "twist_to_turtle_vel");
ros::NodeHandle nh;
rosPublisher = nh.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);
ros::Subscriber image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
ros::spin();
return 0;
}
rtabmap/src/CoreNode.cpp
+7 -4
View File
@@ -15,16 +15,19 @@ int main(int argc, char** argv)
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
ros::init(argc, argv, "core_node");
const char* opt_delete_db_on_start = "--delete_db_on_start";
ros::init(argc, argv, "rtabmap");
bool deleteDbOnStart = false;
for(int i=1;i<argc;i++)
for(int i=1;i<argc;++i)
{
if(!strncmp(argv[i], opt_delete_db_on_start, strlen(opt_delete_db_on_start)))
if(strcmp(argv[i], "--delete_db_on_start") == 0)
{
deleteDbOnStart = true;
}
else if(!strcmp(argv[i], "--udebug"))
{
ULogger::setLevel(ULogger::kDebug);
}
}
CoreWrapper rtabmap(deleteDbOnStart);
rtabmap/src/CoreWrapper.cpp
+48 -25
View File
@@ -21,11 +21,13 @@
using namespace rtabmap;
CoreWrapper::CoreWrapper(bool deleteDbOnStart) : rtabmap_(0)
CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
rtabmap_(0)
{
infoPub_ = nh_.advertise<rtabmap::RtabmapInfo>("rtabmap_info", 1);
infoExPub_ = nh_.advertise<rtabmap::RtabmapInfoEx>("rtabmap_info_x", 1);
parametersLoadedPub_ = nh_.advertise<std_msgs::Empty>("parameters_loaded", 1);
ros::NodeHandle nh("~");
infoPub_ = nh.advertise<rtabmap::RtabmapInfo>("info", 1);
infoExPub_ = nh.advertise<rtabmap::RtabmapInfoEx>("info_x", 1);
parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1);
rtabmap_ = new Rtabmap();
loadNodeParameters(rtabmap_->getIniFilePath());
@@ -37,13 +39,16 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : rtabmap_(0)
rtabmap_->init();
imageTopic_ = nh_.subscribe("sm_state", 1, &CoreWrapper::smReceivedCallback, this);
parametersUpdatedTopic_ = nh_.subscribe("parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
resetMemorySrv_ = nh.advertiseService("resetMemory", &CoreWrapper::resetMemoryCallback, this);
dumpMemorySrv_ = nh.advertiseService("dumpMemory", &CoreWrapper::dumpMemoryCallback, this);
deleteMemorySrv_ = nh.advertiseService("deleteMemory", &CoreWrapper::deleteMemoryCallback, this);
dumpPredictionSrv_ = nh.advertiseService("dumpPrediction", &CoreWrapper::dumpPredictionCallback, this);
nh = ros::NodeHandle();
smStateTopic_ = nh.subscribe("sm_state", 1, &CoreWrapper::smReceivedCallback, this);
imageTopic_ = nh.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
parametersUpdatedTopic_ = nh.subscribe("rtabmap_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
resetMemorySrv_ = nh_.advertiseService("resetMemory", &CoreWrapper::resetMemoryCallback, this);
dumpMemorySrv_ = nh_.advertiseService("dumpMemory", &CoreWrapper::dumpMemoryCallback, this);
deleteMemorySrv_ = nh_.advertiseService("deleteMemory", &CoreWrapper::deleteMemoryCallback, this);
dumpPredictionSrv_ = nh_.advertiseService("dumpPrediction", &CoreWrapper::dumpPredictionCallback, this);
UEventsManager::addHandler(this);
}
@@ -69,9 +74,10 @@ void CoreWrapper::loadNodeParameters(const std::string & configFile)
ParametersMap parameters = Parameters::getDefaultParameters();
Rtabmap::readParameters(configFile.c_str(), parameters);
ros::NodeHandle nh("~");
for(ParametersMap::const_iterator i=parameters.begin(); i!=parameters.end(); ++i)
{
nh_.setParam(i->first, i->second);
nh.setParam(i->first, i->second);
}
parametersLoadedPub_.publish(std_msgs::Empty());
}
@@ -86,10 +92,11 @@ void CoreWrapper::saveNodeParameters(const std::string & configFile)
}
ParametersMap parameters = Parameters::getDefaultParameters();
ros::NodeHandle nh("~");
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string value;
if(nh_.getParam(iter->first,value))
if(nh.getParam(iter->first,value))
{
iter->second = value;
}
@@ -105,24 +112,23 @@ void CoreWrapper::saveNodeParameters(const std::string & configFile)
void CoreWrapper::smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
{
IplImage * image = 0;
std::list<cv::KeyPoint> keypoints;
std::vector<cv::KeyPoint> keypoints(msg->keypoints.size());
if(msg->image.data.size())
{
sensor_msgs::CvBridge bridge;
bridge.fromImage(msg->image);
image = cvCloneImage(bridge.toIpl());
boost::shared_ptr<sensor_msgs::Image> tracked_object;
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg->image, tracked_object);
IplImage img = ptr->image;
image = cvCloneImage(&img);
}
for(unsigned int i=0; i<msg->keypoints.size() && i<msg->keypoints.size(); i++)
{
cv::KeyPoint pt;
pt.angle = msg->keypoints.at(i).angle;
pt.response = msg->keypoints.at(i).response;
pt.pt.x = msg->keypoints.at(i).ptx;
pt.pt.y = msg->keypoints.at(i).pty;
pt.size = msg->keypoints.at(i).size;
keypoints.push_back(pt);
keypoints[i].angle = msg->keypoints.at(i).angle;
keypoints[i].response = msg->keypoints.at(i).response;
keypoints[i].pt.x = msg->keypoints.at(i).ptx;
keypoints[i].pt.y = msg->keypoints.at(i).pty;
keypoints[i].size = msg->keypoints.at(i).size;
}
rtabmap::SMState * smState = new rtabmap::SMState(msg->sensors, msg->sensorStep, msg->actuators, msg->actuatorStep);
smState->setImage(image);
@@ -130,6 +136,19 @@ void CoreWrapper::smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr &
UEventsManager::post(new SMStateEvent(smState));
}
void CoreWrapper::imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
{
if(msg->data.size())
{
boost::shared_ptr<sensor_msgs::Image> tracked_object;
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
IplImage imgTmp = ptr->image;
IplImage * image = &imgTmp;
rtabmap::SMState * smState = new rtabmap::SMState(cvCloneImage(image));
UEventsManager::post(new SMStateEvent(smState));
}
}
bool CoreWrapper::resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResetMemory));
@@ -157,10 +176,11 @@ bool CoreWrapper::dumpPredictionCallback(std_srvs::Empty::Request&, std_srvs::Em
void CoreWrapper::parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg)
{
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
ros::NodeHandle nh("~");
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string value;
if(nh_.getParam(iter->first, value))
if(nh.getParam(iter->first, value))
{
iter->second = value;
}
@@ -298,11 +318,14 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
++index;
}
// SM masks
msg->refMotionMask = stat.refMotionMask();
msg->loopMotionMask = stat.loopMotionMask();
// Statistics data
msg->statsKeys = uKeys(stat.data());
msg->statsValues = uValues(stat.data());
ROS_INFO("Publishing statistics...");
infoExPub_.publish(msg);
}
else
rtabmap/src/CoreWrapper.h
+3 -3
View File
@@ -12,7 +12,7 @@
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <std_msgs/Empty.h>
#include <cv_bridge/CvBridge.h>
#include <cv_bridge/cv_bridge.h>
#include "utilite/UEventsHandler.h"
#include <rtabmap/core/RtabmapEvent.h>
#include <sensor_msgs/Image.h>
@@ -33,6 +33,7 @@ public:
private:
void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg);
void imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg);
void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg);
bool resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -46,10 +47,9 @@ private:
virtual void handleEvent(UEvent * anEvent);
private:
ros::NodeHandle nh_;
rtabmap::Rtabmap * rtabmap_;
ros::Subscriber smStateTopic_;
ros::Subscriber imageTopic_;
ros::Subscriber compressedImageTopic_;
ros::Subscriber parametersUpdatedTopic_;
ros::Publisher infoPub_;
ros::Publisher infoExPub_;
rtabmap/src/GuiNode.cpp
+14 -1
View File
@@ -10,13 +10,26 @@
#include <QApplication>
#include <rtabmap/gui/MainWindow.h>
#include <signal.h>
void my_handler(int s){
QApplication::closeAllWindows();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "gui_node");
ros::init(argc, argv, "rtabmap_gui");
GuiWrapper gui(argc, argv);
// 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);
// Here start the ROS events loop
ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
rtabmap/src/GuiWrapper.cpp
+68 -31
View File
@@ -10,7 +10,7 @@
#include "PreferencesDialogROS.h"
#include <QtGui/QApplication>
#include <rtabmap/core/RtabmapEvent.h>
#include <cv_bridge/CvBridge.h>
#include <cv_bridge/cv_bridge.h>
#include "utilite/UEventsManager.h"
#include "std_srvs/Empty.h"
#include "std_msgs/Empty.h"
@@ -18,26 +18,33 @@
#include <highgui.h>
#include <rtabmap/core/CameraEvent.h>
#include "rtabmap/ChangeCameraImgRate.h"
#include "rtabmap/core/SMState.h"
using namespace rtabmap;
GuiWrapper::GuiWrapper(int & argc, char** argv)
GuiWrapper::GuiWrapper(int & argc, char** argv) :
nbCommands_(2)
{
infoTopic_ = nh_.subscribe("rtabmap_info", 1, &GuiWrapper::infoReceivedCallback, this);
infoExTopic_ = nh_.subscribe("rtabmap_info_x", 1, &GuiWrapper::infoExReceivedCallback, this);
ros::NodeHandle nh;
infoTopic_ = nh.subscribe("rtabmap/info", 1, &GuiWrapper::infoReceivedCallback, this);
infoExTopic_ = nh.subscribe("rtabmap/info_x", 1, &GuiWrapper::infoExReceivedCallback, this);
velocity_sub_ = nh.subscribe("cmd_vel", 1, &GuiWrapper::velocityReceivedCallback, this);
app_ = new QApplication(argc, argv);
mainWindow_ = new MainWindow(new PreferencesDialogROS());
mainWindow_->show();
mainWindow_->changeState(MainWindow::kMonitoring);
app_->connect( app_, SIGNAL( lastWindowClosed() ), app_, SLOT( quit() ) );
resetMemoryClient_ = nh_.serviceClient<std_srvs::Empty>("resetMemory");
dumpMemoryClient_ = nh_.serviceClient<std_srvs::Empty>("dumpMemory");
dumpPredictionClient_ = nh_.serviceClient<std_srvs::Empty>("dumpPrediction");
deleteMemoryClient_ = nh_.serviceClient<std_srvs::Empty>("deleteMemory");
changeCameraImgRateClient_ = nh_.serviceClient<rtabmap::ChangeCameraImgRate>("changeCameraImgRate");
resetMemoryClient_ = nh.serviceClient<std_srvs::Empty>("rtabmap/resetMemory");
dumpMemoryClient_ = nh.serviceClient<std_srvs::Empty>("rtabmap/dumpMemory");
dumpPredictionClient_ = nh.serviceClient<std_srvs::Empty>("rtabmap/dumpPrediction");
deleteMemoryClient_ = nh.serviceClient<std_srvs::Empty>("rtabmap/deleteMemory");
changeCameraImgRateClient_ = nh.serviceClient<rtabmap::ChangeCameraImgRate>("camera/changeImgRate");
parametersUpdatedPub_ = nh_.advertise<std_msgs::Empty>("parameters_updated", 1);
nh = ros::NodeHandle("~");
parametersUpdatedPub_ = nh.advertise<std_msgs::Empty>("parameters_updated", 1);
nh.param("nb_commands", nbCommands_, nbCommands_);
ROS_INFO("nb_commands=%d", nbCommands_);
UEventsManager::addHandler(this);
UEventsManager::addHandler(mainWindow_);
@@ -58,14 +65,25 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
{
ROS_INFO("Loop closure detected! newId=%d with oldId=%d", msg->refId, msg->loopClosureId);
rtabmap::Statistics * stat = new rtabmap::Statistics();
stat->setRefImageId(msg->refId);
stat->setLoopClosureId(msg->loopClosureId);
std::list<std::vector<float> > actions;
for(unsigned int i=0; i<msg->actuators.size(); i+=msg->actuatorStep)
{
std::vector<float> a(msg->actuatorStep);
for(unsigned int j=0; j<a.size(); ++j)
{
a[j] = msg->actuators[i+j];
}
actions.push_back(a);
}
stat->setActions(actions);
UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
}
void GuiWrapper::infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
{
ROS_INFO("Statistics received!");
sensor_msgs::CvBridge bridge;
// Map from ROS struct to rtabmap struct
rtabmap::Statistics * stat = new rtabmap::Statistics();
@@ -137,6 +155,23 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & m
}
stat->setLoopWords(mapIntKeypoint);
//SM stuff
stat->setRefMotionMask(msg->refMotionMask);
stat->setLoopMotionMask(msg->loopMotionMask);
//Actions
std::list<std::vector<float> > actions;
for(unsigned int i=0; i<msg->info.actuators.size(); i+=msg->info.actuatorStep)
{
std::vector<float> a(msg->info.actuatorStep);
for(unsigned int j=0; j<a.size(); ++j)
{
a[j] = msg->info.actuators[i+j];
}
actions.push_back(a);
}
stat->setActions(actions);
// Statistics data
for(unsigned int i=0; i<msg->statsKeys.size() && i<msg->statsValues.size(); i++)
{
@@ -147,37 +182,39 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & m
UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
}
void GuiWrapper::velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
{
std::vector<float> v(6);
v[0] = msg->linear.x;
v[1] = msg->linear.y;
v[2] = msg->linear.z;
v[3] = msg->angular.x;
v[4] = msg->angular.y;
v[5] = msg->angular.z;
commands_.push_back(v);
if(commands_.size() == (unsigned int)nbCommands_)
{
this->post(new SMStateEvent(new SMState(cv::Mat(), commands_)));
commands_.clear();
}
}
void GuiWrapper::handleEvent(UEvent * anEvent)
{
if(anEvent->getClassName().compare("CameraEvent") == 0)
{
rtabmap::CameraEvent * camEvent = (rtabmap::CameraEvent *)anEvent;
if(camEvent->getCommand() == rtabmap::CameraEvent::kCmdChangeParam)
{
rtabmap::ChangeCameraImgRate srv;
srv.request.imgRate = camEvent->getImageRate();
srv.request.autoRestart = camEvent->getAutoRestart();
if(!changeCameraImgRateClient_.call(srv))
{
ROS_WARN("Can't call \"changeCameraImgRate\" service. Ignore this warning if the rtabmap/camera_node is not used...");
}
}
else
{
ROS_WARN("Only ChangeImgRate command for the camera is supported yet...");
}
}
else if(anEvent->getClassName().compare("ParamEvent") == 0)
if(anEvent->getClassName().compare("ParamEvent") == 0)
{
const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
bool modified = false;
ros::NodeHandle nh("rtabmap");
for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
{
//save only parameters with valid names
if(defaultParameters.find((*i).first) != defaultParameters.end())
{
nh_.setParam((*i).first, (*i).second);
nh.setParam((*i).first, (*i).second);
modified = true;
}
else if((*i).first.find('/') != (*i).first.npos)
rtabmap/src/GuiWrapper.h
+6 -1
View File
@@ -12,6 +12,7 @@
#include "rtabmap/RtabmapInfo.h"
#include "rtabmap/RtabmapInfoEx.h"
#include "utilite/UEventsHandler.h"
#include <geometry_msgs/Twist.h>
namespace rtabmap
{
@@ -34,11 +35,12 @@ protected:
private:
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg);
void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & infoExMsg);
void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg);
private:
ros::NodeHandle nh_;
ros::Subscriber infoTopic_;
ros::Subscriber infoExTopic_;
ros::Subscriber velocity_sub_;
QApplication * app_;
rtabmap::MainWindow * mainWindow_;
@@ -49,6 +51,9 @@ private:
ros::ServiceClient dumpPredictionClient_;
ros::Publisher parametersUpdatedPub_;
int nbCommands_;
std::list<std::vector<float> > commands_;
};
#endif /* GUIWRAPPER_H_ */
rtabmap/src/ImageToSensorimotorStateNode.cpp
+37 -26
View File
@@ -11,16 +11,20 @@
#include <rtabmap/core/SMState.h>
#include <utilite/ULogger.h>
#include <utilite/UTimer.h>
#include <cv_bridge/CvBridge.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include "rtabmap/SensoryMotorState.h"
#include <std_msgs/Empty.h>
#include <opencv2/imgproc/imgproc_c.h>
rtabmap::CamKeypointTreatment kpThreatment;
ros::Publisher rosPublisher;
int imgWidth = 0;
int imgHeight = 0;
int imgRate = 0;
double imgRate = 0.0;
int keypointsExtracted = 0;
UTimer timer;
@@ -55,22 +59,23 @@ void imgReceivedCallback(const sensor_msgs::ImageConstPtr & imgMsg)
double period = 0.0;
if(imgRate > 0)
{
period = 1.0/double(imgRate);
period = 1.0/imgRate;
period -= 0.01 * period; // 1% error
}
double elapsed = timer.getElapsedTime();
if(imgRate == 0 || elapsed > period)
if(imgRate == 0.0 || elapsed > period)
{
timer.start();
IplImage * image = 0;
sensor_msgs::CvBridge bridge;
if(imgMsg->data.size())
{
image = bridge.imgMsgToCv(imgMsg);
boost::shared_ptr<sensor_msgs::Image> tracked_object;
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(imgMsg);
IplImage imgTmp = ptr->image;
IplImage * image = &imgTmp;
bool resized = false;
if(image &&
imgWidth &&
if(imgWidth &&
imgHeight &&
imgWidth != image->width &&
imgHeight != image->height)
@@ -88,10 +93,11 @@ void imgReceivedCallback(const sensor_msgs::ImageConstPtr & imgMsg)
if(image)
{
UTimer processTimer;
rtabmap::SMState * smState = new rtabmap::SMState(image);
kpThreatment.process(smState);
double processTime = processTimer.ticks();
if(keypointsExtracted)
{
kpThreatment.process(smState);
}
if(smState)
{
rtabmap::SensoryMotorStatePtr msg(new rtabmap::SensoryMotorState);
@@ -114,12 +120,15 @@ void imgReceivedCallback(const sensor_msgs::ImageConstPtr & imgMsg)
}
else
{
sensor_msgs::CvBridge::fromIpltoRosImage(image, msg->image);
cv_bridge::CvImage img;
img.encoding = sensor_msgs::image_encodings::BGR8;
img.image = image;
msg->image = *img.toImageMsg();
}
const std::list<cv::KeyPoint> & keypoints = smState->getKeypoints();
const std::vector<cv::KeyPoint> & keypoints = smState->getKeypoints();
msg->keypoints = std::vector<rtabmap::KeyPoint>(keypoints.size());
int i=0;
for(std::list<cv::KeyPoint>::const_iterator iter = keypoints.begin(); iter!=keypoints.end(); ++iter)
for(std::vector<cv::KeyPoint>::const_iterator iter = keypoints.begin(); iter!=keypoints.end(); ++iter)
{
msg->keypoints.at(i).angle = iter->angle;
msg->keypoints.at(i).octave = iter->octave;
@@ -131,7 +140,6 @@ void imgReceivedCallback(const sensor_msgs::ImageConstPtr & imgMsg)
++i;
}
ROS_INFO("Publishing smState (processing time=%fs, period=%fs, %f Hz)...", processTime, elapsed, elapsed>0?1/elapsed:0);
rosPublisher.publish(msg);
}
if(resized)
@@ -149,23 +157,26 @@ void imgReceivedCallback(const sensor_msgs::ImageConstPtr & imgMsg)
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_to_sms_node");
ros::init(argc, argv, "image_to_sms");
//ULogger::setType(ULogger::kTypeConsole);
//ULogger::setLevel(ULogger::kDebug);
ros::NodeHandle nh("~");
ros::NodeHandle np("~");
nh.param("image_rate", imgRate, imgRate);
nh.param("resize_image_width", imgWidth, imgWidth);
nh.param("resize_image_height", imgHeight, imgHeight);
np.param("image_hz", imgRate, imgRate);
np.param("resize_image_width", imgWidth, imgWidth);
np.param("resize_image_height", imgHeight, imgHeight);
np.param("keypoints_extracted", keypointsExtracted, keypointsExtracted);
ROS_INFO("imgRate=%d\nresize_image_width=%d\nresize_image_height=%d", imgRate, imgWidth, imgHeight);
ROS_INFO("image_hz=%f\nresize_image_width=%d\nresize_image_height=%d\nkeypointsExtracted=%d", imgRate, imgWidth, imgHeight, keypointsExtracted);
ros::Subscriber parametersUpdatedTopic = nh.subscribe("/parameters_updated", 1, parametersUpdatedCallback);
ros::Subscriber parametersLoadedTopic = nh.subscribe("/parameters_loaded", 1, parametersLoadedCallback);
rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("/sm_state", 1);
ros::NodeHandle nh;
ros::Subscriber parametersUpdatedTopic = nh.subscribe("rtabmap_gui/parameters_updated", 1, parametersUpdatedCallback);
ros::Subscriber parametersLoadedTopic = nh.subscribe("rtabmap/parameters_loaded", 1, parametersLoadedCallback);
rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
ros::Subscriber image_sub = nh.subscribe("/image_raw", 1, imgReceivedCallback);
image_transport::ImageTransport it(nh);
image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
updateParameters();
rtabmap/src/InputNode.cpp
+32 -8
View File
@@ -9,6 +9,7 @@
#include <geometry_msgs/Twist.h>
#include <utilite/UMutex.h>
#include <utilite/UTimer.h>
#include <utilite/ULogger.h>
UMutex commandMutex;
std::list<std::vector<float> > commands;
@@ -19,6 +20,9 @@ rtabmap::SensoryMotorStatePtr state;
bool stateUpdated = false;
bool actionsUpdated = false;
int nbCommands = 10;
bool statsLogged = true;
const char * statsFileName = "InputStats.txt";
int index2 = 1;
void publish()
{
@@ -28,6 +32,7 @@ void publish()
int sizeActions = -1;
for(std::list<std::vector<float> >::iterator iter = commands.begin(); iter!=commands.end();++iter)
{
UINFO("%d %f %f %f", index2, iter->at(0), iter->at(1), iter->at(5));
actions.insert(actions.end(), iter->begin(), iter->end());
}
sizeActions = commands.size();
@@ -40,13 +45,12 @@ void publish()
ROS_INFO("Sensorimotor state sent (sizeActions=%d, %f Hz)", sizeActions, elapsed>0?1/elapsed:0);
stateUpdated = false;
actionsUpdated = false;
++index2;
}
}
void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
{
ROS_INFO("Received camera data");
state = rtabmap::SensoryMotorStatePtr(new rtabmap::SensoryMotorState);
state->sensors = msg->sensors;
state->sensorStep = msg->sensorStep;
@@ -81,6 +85,7 @@ void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
// 10 Hz max
while(commands.size() > (unsigned int)nbCommands)
{
UINFO("Ignored %f %f %f", commands.front().at(0), commands.front().at(1), commands.front().at(5));
ROS_WARN("Too many commands (%zu) > %d, removing the oldest...", commands.size(), nbCommands);
//remove the oldest
commands.pop_front();
@@ -95,20 +100,39 @@ void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
int main(int argc, char** argv)
{
ros::init(argc, argv, "input_node");
ros::NodeHandle n("~");
ros::init(argc, argv, "rtabmap_in");
ros::NodeHandle nh("~");
n.param("nb_commands", nbCommands, nbCommands);
nh.param("nb_commands", nbCommands, nbCommands);
ROS_INFO("nb_commands=%d", nbCommands);
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);
}
rosPublisher = n.advertise<rtabmap::SensoryMotorState>("/sm_state", 1);
ros::Subscriber image_sub = n.subscribe("/camera_data", 1, smReceivedCallback);
ros::Subscriber velocity_sub = n.subscribe("/cmd_vel", 1, velocityReceivedCallback);
nh = ros::NodeHandle();
rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
ros::Subscriber image_sub = nh.subscribe("sensor_data", 1, smReceivedCallback);
ros::Subscriber velocity_sub = nh.subscribe("cmd_vel", 1, velocityReceivedCallback);
timer.start();
ros::spin();
if(statsLogged)
{
ULogger::flush();
}
return 0;
}
rtabmap/src/OutputNode.cpp
+53 -47
View File
@@ -9,48 +9,19 @@
#include "rtabmap/RtabmapInfoEx.h"
#include <geometry_msgs/Twist.h>
#include <utilite/UMutex.h>
#include <utilite/ULogger.h>
std::vector<float> commands;
int commandSize = 0;
int commandIndex = 0;
ros::Publisher rosPublisher;
int commandsHz = 10; //10 Hz
bool statsLogged = true;
const char * statsFileName = "OuputStats.txt";
int index2 = 1;
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
void publishCommands(const std::vector<float> & commands, int commandSize)
{
commands = msg->actuators;
commandSize = msg->actuatorStep;
commandIndex = 0;
ROS_INFO("Rtabmap's actions received, commandSize=%d", commandSize);
}
void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
{
ROS_INFO("Rtabmap's actions received");
commands = msg->info.actuators;
commandSize = msg->info.actuatorStep;
ROS_INFO("Rtabmap's actions received, commandSize=%d, id=%d", commandSize, msg->info.refId);
commandIndex = 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "output_node");
ros::NodeHandle n("~");
n.param("commands_hz", commandsHz, commandsHz);
ROS_INFO("commands_hz=%d", commandsHz);
ros::Subscriber infoTopic;
ros::Subscriber infoExTopic;
infoTopic = n.subscribe("/rtabmap_info", 1, infoReceivedCallback);
infoExTopic = n.subscribe("/rtabmap_info_x", 1, infoExReceivedCallback);
rosPublisher = n.advertise<geometry_msgs::Twist>("/rtabmap/cmd_vel", 1);
ros::Rate loop_rate(commandsHz); // Hz
while(ros::ok())
if(commandSize && commandSize%6 == 0)
{
if(commandIndex>=0 &&
commandSize==6 &&
commandIndex + (commandSize-1) < (int)commands.size())
unsigned int commandIndex = 0;
while(commandIndex < commands.size())
{
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
vel->linear.x = commands[commandIndex++];
@@ -59,19 +30,54 @@ int main(int argc, char** argv)
vel->angular.x = commands[commandIndex++];
vel->angular.y = commands[commandIndex++];
vel->angular.z = commands[commandIndex++];
ROS_INFO("Publishing vel");
rosPublisher.publish(vel);
}
else
{
//publish null
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
ROS_INFO("Publishing null vel");
UINFO("%d %f %f %f", index2, vel->linear.x, vel->linear.y, vel->angular.z);
rosPublisher.publish(vel);
}
++index2;
}
}
ros::spinOnce();
loop_rate.sleep();
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
{
publishCommands(msg->actuators, msg->actuatorStep);
}
void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
{
publishCommands(msg->info.actuators, msg->info.actuatorStep);
}
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);
infoExTopic = nh.subscribe("rtabmap/info_x", 1, infoExReceivedCallback);
rosPublisher = nh.advertise<geometry_msgs::Twist>("rtabmap/cmd_vel", 1);
ros::spin();
if(statsLogged)
{
ULogger::flush();
}
return 0;
}
rtabmap/src/PreferencesDialogROS.cpp
+4 -2
View File
@@ -30,7 +30,8 @@ PreferencesDialogROS::~PreferencesDialogROS()
void PreferencesDialogROS::readCameraSettings(const QString & filePath)
{
double imgRate = 0;
nh_.getParam("cam/image_rate", imgRate);
ros::NodeHandle nh;
nh.getParam("camera/image_hz", imgRate);
this->setImgRate(imgRate);
}
@@ -43,13 +44,14 @@ void PreferencesDialogROS::readCoreSettings(const QString & filePath)
{
if(filePath.isEmpty())
{
ros::NodeHandle nh("rtabmap");
ROS_INFO("%s", this->getParamMessage().toStdString().c_str());
bool validParameters = true;
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
{
std::string value;
if(nh_.getParam((*i).first,value))
if(nh.getParam((*i).first,value))
{
PreferencesDialog::setParameter((*i).first, value);
}
rtabmap/src/PreferencesDialogROS.h
-3
View File
@@ -25,9 +25,6 @@ protected:
virtual void readCameraSettings(const QString & filePath);
virtual void readCoreSettings(const QString & filePath);
virtual void writeSettings(const QString & filePath);
private:
ros::NodeHandle nh_;
};
#endif /* PREFERENCESDIALOGROS_H_ */
rtabmap/src/TwistToPoses.cpp
+65
View File
@@ -0,0 +1,65 @@
/*
* 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";
rosPublisherLinear.publish(msgLinear);
rosPublisherAngular.publish(msgAngular);
}
#include <ros/ros.h>
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 image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
ros::spin();
return 0;
}
rtabmap/src/TwistToSensorimotorStateNode.cpp
+41
View File
@@ -0,0 +1,41 @@
/*
* CameraNode.cpp
*
* Created on: 1 févr. 2010
* Author: labm2414
*/
#include <ros/ros.h>
#include "rtabmap/SensoryMotorState.h"
#include <geometry_msgs/Twist.h>
ros::Publisher rosPublisher;
void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
{
std::vector<float> v(6);
v[0] = msg->linear.x*100;
v[1] = msg->linear.y*100;
v[2] = msg->linear.z*100;
v[3] = msg->angular.x*100;
v[4] = msg->angular.y*100;
v[5] = msg->angular.z*100;
rtabmap::SensoryMotorStatePtr smMsg(new rtabmap::SensoryMotorState);
smMsg->sensors = v;
smMsg->sensorStep = v.size();
rosPublisher.publish(smMsg);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "twist_to_sms");
ros::NodeHandle nh;
rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
ros::Subscriber image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
ros::spin();
return 0;
}