diff --git a/rtabmap/CMakeLists.txt b/rtabmap/CMakeLists.txt index 280b25d8..d745337a 100644 --- a/rtabmap/CMakeLists.txt +++ b/rtabmap/CMakeLists.txt @@ -44,7 +44,7 @@ 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/ImageToSensoryMotorStateNode.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}) diff --git a/rtabmap/launch/all_tr_learning.launch b/rtabmap/launch/all_tr_learning.launch index 9562743a..8309ec04 100644 --- a/rtabmap/launch/all_tr_learning.launch +++ b/rtabmap/launch/all_tr_learning.launch @@ -17,14 +17,6 @@ - - - - - - - - + \ No newline at end of file diff --git a/rtabmap/launch/all_tr_learning_omni.launch b/rtabmap/launch/all_tr_learning_omni.launch new file mode 100644 index 00000000..607a25fe --- /dev/null +++ b/rtabmap/launch/all_tr_learning_omni.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rtabmap/launch/camera.launch b/rtabmap/launch/camera.launch index 3cc2ec6a..eafcf35d 100644 --- a/rtabmap/launch/camera.launch +++ b/rtabmap/launch/camera.launch @@ -1,10 +1,10 @@ - - - - - - - - + + + + + + + + diff --git a/rtabmap/launch/cameraOpenCV.launch b/rtabmap/launch/cameraOpenCV.launch new file mode 100644 index 00000000..c155fb78 --- /dev/null +++ b/rtabmap/launch/cameraOpenCV.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/rtabmap/launch/rtabmap_lc.launch b/rtabmap/launch/rtabmap_lc.launch index 41d368ea..09181bfe 100644 --- a/rtabmap/launch/rtabmap_lc.launch +++ b/rtabmap/launch/rtabmap_lc.launch @@ -1,14 +1,14 @@ - - - - - - - - + + + + + + + + \ No newline at end of file diff --git a/rtabmap/launch/rtabmap_sm.launch b/rtabmap/launch/rtabmap_sm.launch index e0483d54..913871b8 100644 --- a/rtabmap/launch/rtabmap_sm.launch +++ b/rtabmap/launch/rtabmap_sm.launch @@ -2,10 +2,6 @@ - - - - @@ -28,13 +24,16 @@ - - - + + + - - + + + + + \ No newline at end of file diff --git a/rtabmap/launch/testCamera.launch b/rtabmap/launch/testCamera.launch index b323a730..3fd8b1bc 100644 --- a/rtabmap/launch/testCamera.launch +++ b/rtabmap/launch/testCamera.launch @@ -1,11 +1,13 @@ - - - - - - - + + + + + + + + + diff --git a/rtabmap/manifest.xml b/rtabmap/manifest.xml index e412548e..f1d17ad1 100644 --- a/rtabmap/manifest.xml +++ b/rtabmap/manifest.xml @@ -14,6 +14,7 @@ + diff --git a/rtabmap/src/AbtrVelocityNode.cpp b/rtabmap/src/AbtrVelocityNode.cpp index 471d4ea0..a63bf99e 100644 --- a/rtabmap/src/AbtrVelocityNode.cpp +++ b/rtabmap/src/AbtrVelocityNode.cpp @@ -91,4 +91,6 @@ int main(int argc, char** argv) ros::spinOnce(); loop_rate.sleep(); } + + return 0; } diff --git a/rtabmap/src/CameraNode.cpp b/rtabmap/src/CameraNode.cpp index bb809577..4d143ae1 100644 --- a/rtabmap/src/CameraNode.cpp +++ b/rtabmap/src/CameraNode.cpp @@ -46,7 +46,7 @@ int main(int argc, char** argv) if(!camera || (camera && !camera->init())) { - ROS_ERROR("Cannot initiate the camera"); + ROS_ERROR("Cannot initiate the camera. Verify if OpenCV is built with ffmpeg support."); } else { @@ -61,4 +61,6 @@ int main(int argc, char** argv) { delete camera; } + + return 0; } diff --git a/rtabmap/src/CameraNodeReceiver.cpp b/rtabmap/src/CameraNodeReceiver.cpp index 7ac53b0e..ba903405 100644 --- a/rtabmap/src/CameraNodeReceiver.cpp +++ b/rtabmap/src/CameraNodeReceiver.cpp @@ -42,11 +42,13 @@ int main(int argc, char** argv) ros::init(argc, argv, "camera_node_receiver"); ros::NodeHandle n; - ros::Subscriber image_sub = n.subscribe("image", 1, imgReceivedCallback); + ros::Subscriber image_sub = n.subscribe("/image_raw", 1, imgReceivedCallback); ROS_INFO("Waiting for images..."); ros::spin(); cvDestroyWindow("ImageReceived"); + + return 0; } diff --git a/rtabmap/src/CameraNodeReceiverSM.cpp b/rtabmap/src/CameraNodeReceiverSM.cpp index 61bf4da9..93c61c6b 100644 --- a/rtabmap/src/CameraNodeReceiverSM.cpp +++ b/rtabmap/src/CameraNodeReceiverSM.cpp @@ -35,11 +35,13 @@ int main(int argc, char** argv) ros::init(argc, argv, "camera_node_receiver_sm"); ros::NodeHandle n; - ros::Subscriber image_sub = n.subscribe("sm_state", 1, smReceivedCallback); + ros::Subscriber image_sub = n.subscribe("/sm_state", 1, smReceivedCallback); ROS_INFO("Waiting for Sensorimotor states (containing images)..."); ros::spin(); cvDestroyWindow("ImageReceived"); + + return 0; } diff --git a/rtabmap/src/CameraWrapper.cpp b/rtabmap/src/CameraWrapper.cpp index da56f564..5f916a56 100644 --- a/rtabmap/src/CameraWrapper.cpp +++ b/rtabmap/src/CameraWrapper.cpp @@ -21,7 +21,7 @@ CameraWrapper::CameraWrapper() : camera_(0) { - rosPublisher_ = nh_.advertise("image", 1); + rosPublisher_ = nh_.advertise("image_raw", 1); changeCameraImgRateSrv_ = nh_.advertiseService("changeCameraImgRate", &CameraWrapper::changeCameraImgRateCallback, this); UEventsManager::addHandler(this); } @@ -118,7 +118,7 @@ void CameraWrapper::handleEvent(UEvent* anEvent) sensor_msgs::ImagePtr msg = sensor_msgs::CvBridge::cvToImgMsg(smState->getImage()); rosPublisher_.publish(msg); } - catch (sensor_msgs::CvBridgeException ex) + catch (sensor_msgs::CvBridgeException & ex) { ROS_ERROR("%s", ex.what()); } diff --git a/rtabmap/src/CoreNode.cpp b/rtabmap/src/CoreNode.cpp index 98576e46..0121bde8 100644 --- a/rtabmap/src/CoreNode.cpp +++ b/rtabmap/src/CoreNode.cpp @@ -32,4 +32,6 @@ int main(int argc, char** argv) ROS_INFO("RTAB-Map started..."); ros::spin(); + + return 0; } diff --git a/rtabmap/src/ImageToSensorimotorStateNode.cpp b/rtabmap/src/ImageToSensorimotorStateNode.cpp new file mode 100644 index 00000000..a645c9ba --- /dev/null +++ b/rtabmap/src/ImageToSensorimotorStateNode.cpp @@ -0,0 +1,176 @@ +/* + * CameraNode.cpp + * + * Created on: 1 févr. 2010 + * Author: labm2414 + */ + +#include +#include +#include +#include +#include +#include +#include +#include "rtabmap/SensoryMotorState.h" +#include + +rtabmap::CamKeypointTreatment kpThreatment; +ros::Publisher rosPublisher; + +int imgWidth = 0; +int imgHeight = 0; +int imgRate = 0; + +UTimer timer; + +void updateParameters() +{ + ros::NodeHandle nh; + rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); + for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + std::string value; + if(nh.getParam(iter->first, value)) + { + iter->second = value; + } + } + ROS_INFO("Updating parameters"); + kpThreatment.parseParameters(parameters); +} + +void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg) +{ + updateParameters(); +} + +void parametersLoadedCallback(const std_msgs::EmptyConstPtr & msg) +{ + updateParameters(); +} + +void imgReceivedCallback(const sensor_msgs::ImageConstPtr & imgMsg) +{ + double period = 0.0; + if(imgRate > 0) + { + period = 1.0/double(imgRate); + period -= 0.01 * period; // 1% error + } + double elapsed = timer.getElapsedTime(); + if(imgRate == 0 || elapsed > period) + { + timer.start(); + + IplImage * image = 0; + sensor_msgs::CvBridge bridge; + if(imgMsg->data.size()) + { + image = bridge.imgMsgToCv(imgMsg); + bool resized = false; + if(image && + imgWidth && + imgHeight && + imgWidth != image->width && + imgHeight != image->height) + { + // declare a destination IplImage object with correct size, depth and channels + IplImage * resampledImg = cvCreateImage( cvSize((int)(imgWidth) , + (int)(imgHeight) ), + image->depth, image->nChannels ); + + //use cvResize to resize source to a destination image (linear interpolation) + cvResize(image, resampledImg); + image = resampledImg; + resized = true; + } + + if(image) + { + UTimer processTimer; + rtabmap::SMState * smState = kpThreatment.process(image); + double processTime = processTimer.ticks(); + if(smState) + { + rtabmap::SensoryMotorStatePtr msg(new rtabmap::SensoryMotorState); + + std::vector sensors; + int sensorStep = 0; + smState->getSensorsMerged(sensors, sensorStep); + msg->sensors = sensors; + msg->sensorStep = sensorStep; + + std::vector actuators; + int actuatorStep = 0; + smState->getActuatorsMerged(actuators, actuatorStep); + msg->actuators = actuators; + msg->actuatorStep = actuatorStep; + + if(!resized) + { + msg->image = *imgMsg; + } + else + { + sensor_msgs::CvBridge::fromIpltoRosImage(image, msg->image); + } + const std::list & keypoints = smState->getKeypoints(); + msg->keypoints = std::vector(keypoints.size()); + int i=0; + for(std::list::const_iterator iter = keypoints.begin(); iter!=keypoints.end(); ++iter) + { + msg->keypoints.at(i).angle = iter->angle; + msg->keypoints.at(i).octave = iter->octave; + msg->keypoints.at(i).ptx = iter->pt.x; + msg->keypoints.at(i).pty = iter->pt.y; + msg->keypoints.at(i).response = iter->response; + msg->keypoints.at(i).size = iter->size; + msg->keypoints.at(i).class_id = iter->class_id; + ++i; + } + + ROS_INFO("Publishing smState (processing time=%fs, period=%fs, %f Hz)...", processTime, elapsed, elapsed>0?1/elapsed:0); + rosPublisher.publish(msg); + } + if(resized) + { + cvReleaseImage(&image); + } + } + } + } + else + { + //ROS_INFO("Ignored frame..."); + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "image_to_sms_node"); + //ULogger::setType(ULogger::kTypeConsole); + //ULogger::setLevel(ULogger::kDebug); + + ros::NodeHandle nh("~"); + + nh.param("image_rate", imgRate, imgRate); + nh.param("resize_image_width", imgWidth, imgWidth); + nh.param("resize_image_height", imgHeight, imgHeight); + + ROS_INFO("imgRate=%d\nresize_image_width=%d\nresize_image_height=%d", imgRate, imgWidth, imgHeight); + + 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::Subscriber image_sub = nh.subscribe("/image_raw", 1, imgReceivedCallback); + + updateParameters(); + + timer.start(); + + ros::spin(); + + return 0; +} diff --git a/rtabmap/src/ImageToSensoryMotorStateNode.cpp b/rtabmap/src/ImageToSensoryMotorStateNode.cpp deleted file mode 100644 index 1eb35a42..00000000 --- a/rtabmap/src/ImageToSensoryMotorStateNode.cpp +++ /dev/null @@ -1,149 +0,0 @@ -/* - * CameraNode.cpp - * - * Created on: 1 févr. 2010 - * Author: labm2414 - */ - -#include -#include -#include -#include -#include -#include -#include -#include "rtabmap/SensoryMotorState.h" -#include - - -rtabmap::CamKeypointTreatment kpThreatment; -ros::Publisher rosPublisher; - -int imgWidth = 0; -int imgHeight = 0; - -void updateParameters() -{ - ros::NodeHandle nh; - rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); - for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) - { - std::string value; - if(nh.getParam(iter->first, value)) - { - iter->second = value; - } - } - ROS_INFO("Updating parameters"); - kpThreatment.parseParameters(parameters); -} - -void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg) -{ - updateParameters(); -} - -void parametersLoadedCallback(const std_msgs::EmptyConstPtr & msg) -{ - updateParameters(); -} - -void imgReceivedCallback(const sensor_msgs::ImageConstPtr & imgMsg) -{ - IplImage * image = 0; - sensor_msgs::CvBridge bridge; - if(imgMsg->data.size()) - { - image = bridge.imgMsgToCv(imgMsg); - bool resized = false; - if(image && - imgWidth && - imgHeight && - imgWidth != image->width && - imgHeight != image->height) - { - // declare a destination IplImage object with correct size, depth and channels - IplImage * resampledImg = cvCreateImage( cvSize((int)(imgWidth) , - (int)(imgHeight) ), - image->depth, image->nChannels ); - - //use cvResize to resize source to a destination image (linear interpolation) - cvResize(image, resampledImg); - image = resampledImg; - resized = true; - } - - if(image) - { - UTimer timer; - rtabmap::SMState * smState = kpThreatment.process(image); - ROS_INFO("Time processing image %fs", timer.ticks()); - if(smState) - { - rtabmap::SensoryMotorStatePtr msg(new rtabmap::SensoryMotorState); - - std::vector sensors; - int sensorStep = 0; - smState->getSensorsMerged(sensors, sensorStep); - msg->sensors = sensors; - msg->sensorStep = sensorStep; - - std::vector actuators; - int actuatorStep = 0; - smState->getActuatorsMerged(actuators, actuatorStep); - msg->actuators = actuators; - msg->actuatorStep = actuatorStep; - - if(!resized) - { - msg->image = *imgMsg; - } - else - { - sensor_msgs::CvBridge::fromIpltoRosImage(image, msg->image); - } - const std::list & keypoints = smState->getKeypoints(); - msg->keypoints = std::vector(keypoints.size()); - int i=0; - for(std::list::const_iterator iter = keypoints.begin(); iter!=keypoints.end(); ++iter) - { - msg->keypoints.at(i).angle = iter->angle; - msg->keypoints.at(i).octave = iter->octave; - msg->keypoints.at(i).ptx = iter->pt.x; - msg->keypoints.at(i).pty = iter->pt.y; - msg->keypoints.at(i).response = iter->response; - msg->keypoints.at(i).size = iter->size; - msg->keypoints.at(i).class_id = iter->class_id; - ++i; - } - - rosPublisher.publish(msg); - } - if(resized) - { - cvReleaseImage(&image); - } - } - } -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "image_to_sms_node"); - ULogger::setType(ULogger::kTypeConsole); - ULogger::setLevel(ULogger::kDebug); - - ros::NodeHandle nh; - - nh.param("resize_image_width", imgWidth, imgWidth); - nh.param("resize_image_height", imgHeight, imgHeight); - - ros::Subscriber parametersUpdatedTopic = nh.subscribe("parameters_updated", 1, parametersUpdatedCallback); - ros::Subscriber parametersLoadedTopic = nh.subscribe("parameters_loaded", 1, parametersLoadedCallback); - ros::Subscriber image_sub = nh.subscribe("image", 1, imgReceivedCallback); - rosPublisher = nh.advertise("sm_state", 1); - - updateParameters(); - - ros::spin(); -} diff --git a/rtabmap/src/InputNode.cpp b/rtabmap/src/InputNode.cpp index 802a75e2..fdd954f3 100644 --- a/rtabmap/src/InputNode.cpp +++ b/rtabmap/src/InputNode.cpp @@ -8,39 +8,53 @@ #include "rtabmap/SensoryMotorState.h" #include #include +#include UMutex commandMutex; std::list > commands; ros::Publisher rosPublisher; +UTimer timer; +rtabmap::SensoryMotorStatePtr state; -void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg) +bool stateUpdated = false; +bool actionsUpdated = false; + +void publish() { - ROS_INFO("Received camera data"); - std::vector tmp; - - int sizeActions = -1; - commandMutex.lock(); + if(stateUpdated && actionsUpdated) { + std::vector actions; + int sizeActions = -1; for(std::list >::iterator iter = commands.begin(); iter!=commands.end();++iter) { - tmp.insert(tmp.end(), iter->begin(), iter->end()); + actions.insert(actions.end(), iter->begin(), iter->end()); } sizeActions = commands.size(); commands.clear(); - } - commandMutex.unlock(); + state->actuators = actions; - rtabmap::SensoryMotorStatePtr state(new rtabmap::SensoryMotorState); + rosPublisher.publish(state); + float elapsed = timer.ticks(); + ROS_INFO("Sensorimotor state sent (sizeActions=%d, %f Hz)", sizeActions, elapsed>0?1/elapsed:0); + stateUpdated = false; + actionsUpdated = false; + } +} + +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; - state->actuators = tmp; state->actuatorStep = 6; state->image = msg->image; state->keypoints = msg->keypoints; - rosPublisher.publish(state); - ROS_INFO("Sensorimotor state sent (sizeActions=%d)", sizeActions); + stateUpdated = true; + publish(); } void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg) @@ -52,35 +66,43 @@ void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg) msg->angular.x, msg->angular.y, msg->angular.z); - commandMutex.lock(); + + 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); + + // 10 Hz max + while(commands.size() > 10) { - 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; + ROS_WARN("Too many commands (%zu) > 10, removing the oldest...", commands.size()); + //remove the oldest + commands.pop_front(); + } - commands.push_back(v); - - // 10 Hz + 1 max - while(commands.size() > 11) - { - //remove the oldest - commands.pop_front(); - } + if(commands.size() == 10) + { + actionsUpdated = true; + publish(); } - commandMutex.unlock(); } int main(int argc, char** argv) { ros::init(argc, argv, "input_node"); ros::NodeHandle n; - 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); + 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); + + timer.start(); ros::spin(); + + return 0; } diff --git a/rtabmap/src/OutputNode.cpp b/rtabmap/src/OutputNode.cpp index 0b23bea1..d71b73b7 100644 --- a/rtabmap/src/OutputNode.cpp +++ b/rtabmap/src/OutputNode.cpp @@ -10,7 +10,6 @@ #include #include -UMutex commandMutex; std::vector commands; int commandSize = 0; int commandIndex = 0; @@ -18,27 +17,19 @@ ros::Publisher rosPublisher; void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg) { - commandMutex.lock(); - { - commands = msg->actuators; - commandSize = msg->actuatorStep; - commandIndex = 0; - ROS_INFO("Rtabmap's actions received, commandSize=%d", commandSize); - } - commandMutex.unlock(); + 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"); - commandMutex.lock(); - { - commands = msg->info.actuators; - commandSize = msg->info.actuatorStep; - ROS_INFO("Rtabmap's actions received, commandSize=%d, id=%d", commandSize, msg->info.refId); - commandIndex = 0; - } - commandMutex.unlock(); + 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) @@ -54,26 +45,23 @@ int main(int argc, char** argv) ros::Rate loop_rate(10); // 10 Hz while(ros::ok()) { - commandMutex.lock(); + if(commandIndex>=0 && + commandSize==6 && + commandIndex + (commandSize-1) < (int)commands.size()) { - if(commandIndex>=0 && - commandSize==6 && - commandIndex + (commandSize-1) < (int)commands.size()) - { - geometry_msgs::TwistPtr vel(new geometry_msgs::Twist()); - vel->linear.x = commands[commandIndex++]; - vel->linear.y = commands[commandIndex++]; - vel->linear.z = commands[commandIndex++]; - vel->angular.x = commands[commandIndex++]; - vel->angular.y = commands[commandIndex++]; - vel->angular.z = commands[commandIndex++]; - ROS_INFO("Publishing vel"); - rosPublisher.publish(vel); - } + geometry_msgs::TwistPtr vel(new geometry_msgs::Twist()); + vel->linear.x = commands[commandIndex++]; + vel->linear.y = commands[commandIndex++]; + vel->linear.z = commands[commandIndex++]; + vel->angular.x = commands[commandIndex++]; + vel->angular.y = commands[commandIndex++]; + vel->angular.z = commands[commandIndex++]; + ROS_INFO("Publishing vel"); + rosPublisher.publish(vel); } - commandMutex.unlock(); ros::spinOnce(); loop_rate.sleep(); } + return 0; }