diff --git a/rosdep.yaml b/rosdep.yaml index a0913909..f7fc8a81 100644 --- a/rosdep.yaml +++ b/rosdep.yaml @@ -1,6 +1,6 @@ sqlite3: ubuntu: libsqlite3-dev sqlite3 debian: libsqlite3-dev sqlite3 - macports: sqlite3 - arch: sqlite3 - gentoo: sqlite3 +fftw3: + ubuntu: libfftw3-dev + debian: libfftw3-dev diff --git a/rtabmap/CMakeLists.txt b/rtabmap/CMakeLists.txt index d745337a..c3512052 100644 --- a/rtabmap/CMakeLists.txt +++ b/rtabmap/CMakeLists.txt @@ -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() diff --git a/rtabmap/launch/all_learning.launch b/rtabmap/launch/all_learning.launch deleted file mode 100644 index 7867396e..00000000 --- a/rtabmap/launch/all_learning.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - \ No newline at end of file diff --git a/rtabmap/launch/all_tr_learning_omni.launch b/rtabmap/launch/all_tr_learning_omni.launch deleted file mode 100644 index 174c0dd6..00000000 --- a/rtabmap/launch/all_tr_learning_omni.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/rtabmap/launch/az2_learning.launch b/rtabmap/launch/az2_learning.launch new file mode 100644 index 00000000..e3a1bbb8 --- /dev/null +++ b/rtabmap/launch/az2_learning.launch @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rtabmap/launch/az2_teleop.launch b/rtabmap/launch/az2_teleop.launch new file mode 100644 index 00000000..8fc3b389 --- /dev/null +++ b/rtabmap/launch/az2_teleop.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/rtabmap/launch/az3_learning.launch b/rtabmap/launch/az3_learning.launch new file mode 100644 index 00000000..ab74088d --- /dev/null +++ b/rtabmap/launch/az3_learning.launch @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rtabmap/launch/az3_teleop.launch b/rtabmap/launch/az3_teleop.launch new file mode 100644 index 00000000..1fab8022 --- /dev/null +++ b/rtabmap/launch/az3_teleop.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/rtabmap/launch/cameraOpenCV.launch b/rtabmap/launch/cameraOpenCV.launch index 3cc2ec6a..b17d8d1b 100644 --- a/rtabmap/launch/cameraOpenCV.launch +++ b/rtabmap/launch/cameraOpenCV.launch @@ -1,10 +1,9 @@ - - - - - - - + + + + + + diff --git a/rtabmap/launch/camera.launch b/rtabmap/launch/cameraUVC.launch similarity index 100% rename from rtabmap/launch/camera.launch rename to rtabmap/launch/cameraUVC.launch diff --git a/rtabmap/launch/loop_closure_detection.launch b/rtabmap/launch/loop_closure_detection.launch new file mode 100644 index 00000000..55ca2822 --- /dev/null +++ b/rtabmap/launch/loop_closure_detection.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/rtabmap/launch/rtabmap_lc.launch b/rtabmap/launch/rtabmap_lc.launch deleted file mode 100644 index 09181bfe..00000000 --- a/rtabmap/launch/rtabmap_lc.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - \ No newline at end of file diff --git a/rtabmap/launch/rtabmap_sm.launch b/rtabmap/launch/rtabmap_sm.launch deleted file mode 100644 index 51f3dab1..00000000 --- a/rtabmap/launch/rtabmap_sm.launch +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rtabmap/launch/teleop_keyboard.launch b/rtabmap/launch/teleop_keyboard.launch new file mode 100644 index 00000000..9d0d37b8 --- /dev/null +++ b/rtabmap/launch/teleop_keyboard.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/rtabmap/launch/test_cameraOpenCV.launch b/rtabmap/launch/test_cameraOpenCV.launch new file mode 100644 index 00000000..091ad8a8 --- /dev/null +++ b/rtabmap/launch/test_cameraOpenCV.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/rtabmap/launch/testCamera.launch b/rtabmap/launch/test_cameraUVC.launch similarity index 84% rename from rtabmap/launch/testCamera.launch rename to rtabmap/launch/test_cameraUVC.launch index 3fd8b1bc..9c75057b 100644 --- a/rtabmap/launch/testCamera.launch +++ b/rtabmap/launch/test_cameraUVC.launch @@ -1,6 +1,6 @@ - + diff --git a/rtabmap/launch/test_step_by_step_actions_only.launch b/rtabmap/launch/test_step_by_step_actions_only.launch new file mode 100644 index 00000000..04856bfe --- /dev/null +++ b/rtabmap/launch/test_step_by_step_actions_only.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rtabmap/launch/test_step_by_step_az2.launch b/rtabmap/launch/test_step_by_step_az2.launch new file mode 100644 index 00000000..0bf7afd6 --- /dev/null +++ b/rtabmap/launch/test_step_by_step_az2.launch @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rtabmap/manifest.xml b/rtabmap/manifest.xml index f1d17ad1..274ed926 100644 --- a/rtabmap/manifest.xml +++ b/rtabmap/manifest.xml @@ -10,13 +10,15 @@ http://rtabmap-ros-pkg.googlecode.com + - - - + + + + diff --git a/rtabmap/msg/RtabmapInfoEx.msg b/rtabmap/msg/RtabmapInfoEx.msg index b9a08802..2777e665 100644 --- a/rtabmap/msg/RtabmapInfoEx.msg +++ b/rtabmap/msg/RtabmapInfoEx.msg @@ -36,4 +36,8 @@ rtabmap/KeyPoint[] refWordsValues #std::multimap loopWords int32[] loopWordsKeys -rtabmap/KeyPoint[] loopWordsValues \ No newline at end of file +rtabmap/KeyPoint[] loopWordsValues + +##SM masks## +uint8[] refMotionMask +uint8[] loopMotionMask \ No newline at end of file diff --git a/rtabmap/src/AbtrVelocityNode.cpp b/rtabmap/src/AbtrVelocityNode.cpp index a63bf99e..955f3de5 100644 --- a/rtabmap/src/AbtrVelocityNode.cpp +++ b/rtabmap/src/AbtrVelocityNode.cpp @@ -7,89 +7,187 @@ #include #include #include +#include +#include -UMutex commandMutex; -std::vector commandsA; -std::vector commandsB; +std::queue commandsA; +std::queue commandsB; +std::vector lastCommandsB; int commandSize = 0; int commandIndex = 0; +double commandsHz = 10.0; +bool cmdABuffered = false; +bool cmdBBuffered = true; ros::Publisher rosPublisher; +bool statsLogged = false; +const char * statsFileName = "AbtrStats.txt"; void velocityAReceivedCallback(const geometry_msgs::TwistConstPtr & msg) { - //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(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("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("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(); + } + 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(); + } + } + 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(); + } + } + ++index; + } + if(statsLogged) + { + ULogger::flush(); } return 0; diff --git a/rtabmap/src/CameraNode.cpp b/rtabmap/src/CameraNode.cpp index 4d143ae1..021855bc 100644 --- a/rtabmap/src/CameraNode.cpp +++ b/rtabmap/src/CameraNode.cpp @@ -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())) { diff --git a/rtabmap/src/CameraNodeReceiver.cpp b/rtabmap/src/CameraNodeReceiver.cpp index ba903405..f3ee609a 100644 --- a/rtabmap/src/CameraNodeReceiver.cpp +++ b/rtabmap/src/CameraNodeReceiver.cpp @@ -6,49 +6,80 @@ */ #include -//#include -#include +#include +#include #include +#include +#include + +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(&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; } diff --git a/rtabmap/src/CameraNodeReceiverSM.cpp b/rtabmap/src/CameraNodeReceiverSM.cpp index 93c61c6b..58b62df9 100644 --- a/rtabmap/src/CameraNodeReceiverSM.cpp +++ b/rtabmap/src/CameraNodeReceiverSM.cpp @@ -7,23 +7,19 @@ #include #include "rtabmap/SensoryMotorState.h" -#include +#include #include 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 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); diff --git a/rtabmap/src/CameraWrapper.cpp b/rtabmap/src/CameraWrapper.cpp index 5f916a56..a3e36dbc 100644 --- a/rtabmap/src/CameraWrapper.cpp +++ b/rtabmap/src/CameraWrapper.cpp @@ -6,8 +6,8 @@ */ #include "CameraWrapper.h" -#include -//#include +#include +#include #include #include #include @@ -21,8 +21,10 @@ CameraWrapper::CameraWrapper() : camera_(0) { - rosPublisher_ = nh_.advertise("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)); } diff --git a/rtabmap/src/CameraWrapper.h b/rtabmap/src/CameraWrapper.h index 2961292e..19ff1e8e 100644 --- a/rtabmap/src/CameraWrapper.h +++ b/rtabmap/src/CameraWrapper.h @@ -12,6 +12,7 @@ #include #include "rtabmap/ChangeCameraImgRate.h" #include +#include 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, diff --git a/rtabmap/src/CmdVelToTurtleVelNode.cpp b/rtabmap/src/CmdVelToTurtleVelNode.cpp new file mode 100644 index 00000000..4df59f28 --- /dev/null +++ b/rtabmap/src/CmdVelToTurtleVelNode.cpp @@ -0,0 +1,33 @@ +/* + * CameraNode.cpp + * + * Created on: 1 févr. 2010 + * Author: labm2414 + */ + +#include +#include +#include + +ros::Publisher rosPublisher; + +void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg) +{ + turtlesim::Velocity vel; + vel.angular = msg->linear.y; + vel.linear = msg->linear.x; + rosPublisher.publish(vel); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "twist_to_turtle_vel"); + + ros::NodeHandle nh; + rosPublisher = nh.advertise("turtle1/command_velocity", 1); + ros::Subscriber image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback); + + ros::spin(); + + return 0; +} diff --git a/rtabmap/src/CoreNode.cpp b/rtabmap/src/CoreNode.cpp index 0121bde8..c9e11bcd 100644 --- a/rtabmap/src/CoreNode.cpp +++ b/rtabmap/src/CoreNode.cpp @@ -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("rtabmap_info", 1); - infoExPub_ = nh_.advertise("rtabmap_info_x", 1); - parametersLoadedPub_ = nh_.advertise("parameters_loaded", 1); + ros::NodeHandle nh("~"); + infoPub_ = nh.advertise("info", 1); + infoExPub_ = nh.advertise("info_x", 1); + parametersLoadedPub_ = nh.advertise("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 keypoints; + std::vector keypoints(msg->keypoints.size()); if(msg->image.data.size()) { - sensor_msgs::CvBridge bridge; - bridge.fromImage(msg->image); - image = cvCloneImage(bridge.toIpl()); + boost::shared_ptr 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; ikeypoints.size() && ikeypoints.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 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 diff --git a/rtabmap/src/CoreWrapper.h b/rtabmap/src/CoreWrapper.h index bb628fdb..7c8fde46 100644 --- a/rtabmap/src/CoreWrapper.h +++ b/rtabmap/src/CoreWrapper.h @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include "utilite/UEventsHandler.h" #include #include @@ -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_; diff --git a/rtabmap/src/GuiNode.cpp b/rtabmap/src/GuiNode.cpp index 0cd7f236..427ee142 100644 --- a/rtabmap/src/GuiNode.cpp +++ b/rtabmap/src/GuiNode.cpp @@ -10,13 +10,26 @@ #include #include +#include + +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(); diff --git a/rtabmap/src/GuiWrapper.cpp b/rtabmap/src/GuiWrapper.cpp index 3d612e57..d7c1ce65 100644 --- a/rtabmap/src/GuiWrapper.cpp +++ b/rtabmap/src/GuiWrapper.cpp @@ -10,7 +10,7 @@ #include "PreferencesDialogROS.h" #include #include -#include +#include #include "utilite/UEventsManager.h" #include "std_srvs/Empty.h" #include "std_msgs/Empty.h" @@ -18,26 +18,33 @@ #include #include #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("resetMemory"); - dumpMemoryClient_ = nh_.serviceClient("dumpMemory"); - dumpPredictionClient_ = nh_.serviceClient("dumpPrediction"); - deleteMemoryClient_ = nh_.serviceClient("deleteMemory"); - changeCameraImgRateClient_ = nh_.serviceClient("changeCameraImgRate"); + resetMemoryClient_ = nh.serviceClient("rtabmap/resetMemory"); + dumpMemoryClient_ = nh.serviceClient("rtabmap/dumpMemory"); + dumpPredictionClient_ = nh.serviceClient("rtabmap/dumpPrediction"); + deleteMemoryClient_ = nh.serviceClient("rtabmap/deleteMemory"); + changeCameraImgRateClient_ = nh.serviceClient("camera/changeImgRate"); - parametersUpdatedPub_ = nh_.advertise("parameters_updated", 1); + nh = ros::NodeHandle("~"); + parametersUpdatedPub_ = nh.advertise("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 > actions; + for(unsigned int i=0; iactuators.size(); i+=msg->actuatorStep) + { + std::vector a(msg->actuatorStep); + for(unsigned int j=0; jactuators[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 > actions; + for(unsigned int i=0; iinfo.actuators.size(); i+=msg->info.actuatorStep) + { + std::vector a(msg->info.actuatorStep); + for(unsigned int j=0; jinfo.actuators[i+j]; + } + actions.push_back(a); + } + stat->setActions(actions); + // Statistics data for(unsigned int i=0; istatsKeys.size() && istatsValues.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 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) diff --git a/rtabmap/src/GuiWrapper.h b/rtabmap/src/GuiWrapper.h index 1664c4f2..2b9cb7f0 100644 --- a/rtabmap/src/GuiWrapper.h +++ b/rtabmap/src/GuiWrapper.h @@ -12,6 +12,7 @@ #include "rtabmap/RtabmapInfo.h" #include "rtabmap/RtabmapInfoEx.h" #include "utilite/UEventsHandler.h" +#include 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 > commands_; }; #endif /* GUIWRAPPER_H_ */ diff --git a/rtabmap/src/ImageToSensorimotorStateNode.cpp b/rtabmap/src/ImageToSensorimotorStateNode.cpp index 731f00f4..7b0a9b6f 100644 --- a/rtabmap/src/ImageToSensorimotorStateNode.cpp +++ b/rtabmap/src/ImageToSensorimotorStateNode.cpp @@ -11,16 +11,20 @@ #include #include #include -#include +#include +#include +#include #include "rtabmap/SensoryMotorState.h" #include +#include 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 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 & keypoints = smState->getKeypoints(); + const std::vector & keypoints = smState->getKeypoints(); msg->keypoints = std::vector(keypoints.size()); int i=0; - for(std::list::const_iterator iter = keypoints.begin(); iter!=keypoints.end(); ++iter) + for(std::vector::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("/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("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(); diff --git a/rtabmap/src/InputNode.cpp b/rtabmap/src/InputNode.cpp index 657bbe89..c8aa2575 100644 --- a/rtabmap/src/InputNode.cpp +++ b/rtabmap/src/InputNode.cpp @@ -9,6 +9,7 @@ #include #include #include +#include UMutex commandMutex; std::list > 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 >::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("/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("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; } diff --git a/rtabmap/src/OutputNode.cpp b/rtabmap/src/OutputNode.cpp index 0118d15e..5c2c937b 100644 --- a/rtabmap/src/OutputNode.cpp +++ b/rtabmap/src/OutputNode.cpp @@ -9,48 +9,19 @@ #include "rtabmap/RtabmapInfoEx.h" #include #include +#include -std::vector 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 & 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("/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("rtabmap/cmd_vel", 1); + + ros::spin(); + + if(statsLogged) + { + ULogger::flush(); } return 0; } diff --git a/rtabmap/src/PreferencesDialogROS.cpp b/rtabmap/src/PreferencesDialogROS.cpp index e2a614a1..18429c2f 100644 --- a/rtabmap/src/PreferencesDialogROS.cpp +++ b/rtabmap/src/PreferencesDialogROS.cpp @@ -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); } diff --git a/rtabmap/src/PreferencesDialogROS.h b/rtabmap/src/PreferencesDialogROS.h index d39c361e..548b7771 100644 --- a/rtabmap/src/PreferencesDialogROS.h +++ b/rtabmap/src/PreferencesDialogROS.h @@ -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_ */ diff --git a/rtabmap/src/TwistToPoses.cpp b/rtabmap/src/TwistToPoses.cpp new file mode 100644 index 00000000..210cceb0 --- /dev/null +++ b/rtabmap/src/TwistToPoses.cpp @@ -0,0 +1,65 @@ +/* + * TwistToPoses.cpp + * + * Created on: 2011-11-30 + * Author: matlab + */ + +#include +#include +#include +#include + +ros::Publisher rosPublisherLinear; +ros::Publisher rosPublisherAngular; + +void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg) +{ + ROS_INFO("Received command velocity linear=(%f,%f,%f) angular=(%f,%f,%f)", + msg->linear.x, + msg->linear.y, + msg->linear.z, + msg->angular.x, + msg->angular.y, + msg->angular.z); + + geometry_msgs::PoseStamped msgLinear; + geometry_msgs::PoseStamped msgAngular; + + if(fabs(msg->linear.x) < 0.0001 && fabs(msg->linear.y) < 0.0001) + { + msgLinear.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 3.14159/2, 0); + } + else + { + msgLinear.pose.orientation = tf::createQuaternionMsgFromYaw(std::atan2(msg->linear.y, msg->linear.x)); + } + if(fabs(msg->angular.z) < 0.0001) + { + msgAngular.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, -3.14159/2, 0); + } + else + { + msgAngular.pose.orientation = tf::createQuaternionMsgFromYaw(msg->angular.z); + } + msgLinear.header.frame_id = "/base_link"; + msgAngular.header.frame_id = "/base_link"; + rosPublisherLinear.publish(msgLinear); + rosPublisherAngular.publish(msgAngular); +} + +#include + +int main(int argc, char * argv[]) +{ + ros::init(argc, argv, "twist_to_poses"); + + ros::NodeHandle nh; + rosPublisherLinear = nh.advertise("pose_linear", 1); + rosPublisherAngular = nh.advertise("pose_angular", 1); + ros::Subscriber image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback); + + ros::spin(); + + return 0; +} diff --git a/rtabmap/src/TwistToSensorimotorStateNode.cpp b/rtabmap/src/TwistToSensorimotorStateNode.cpp new file mode 100644 index 00000000..e8c75811 --- /dev/null +++ b/rtabmap/src/TwistToSensorimotorStateNode.cpp @@ -0,0 +1,41 @@ +/* + * CameraNode.cpp + * + * Created on: 1 févr. 2010 + * Author: labm2414 + */ + +#include +#include "rtabmap/SensoryMotorState.h" +#include + +ros::Publisher rosPublisher; + +void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg) +{ + std::vector 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("sm_state", 1); + ros::Subscriber image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback); + + ros::spin(); + + return 0; +} diff --git a/rtabmap_lib/Makefile b/rtabmap_lib/Makefile index 7927c881..f064d487 100644 --- a/rtabmap_lib/Makefile +++ b/rtabmap_lib/Makefile @@ -4,7 +4,7 @@ INSTALL_DIR = rtabmap all: installed SVN_DIR = build/rtabmap-svn -SVN_URL = https://rtabmap.googlecode.com/svn/trunk/rtabmap +SVN_URL = https://rtabmap.googlecode.com/svn/branches/STM/rtabmap SVN_REVISION = -rHEAD #SVN_PATCH = opencvVer.patch include $(shell rospack find mk)/svn_checkout.mk diff --git a/rtabmap_lib/manifest.xml b/rtabmap_lib/manifest.xml index 7853a535..33bd8ebe 100644 --- a/rtabmap_lib/manifest.xml +++ b/rtabmap_lib/manifest.xml @@ -18,10 +18,11 @@ - + +