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

Updated Camera usage:

Browse Source 
-now using uvc_camera (OpenCV capture is no longer supported in ROS : https://code.ros.org/trac/ros-pkg/ticket/4980)
-Added image_rate parameter in imageToSensorimotorState

Fixed synchronization in InputNode, between image (1Hz) and actions (10Hz)
Updated launch files

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@306 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2011-08-23 15:36:43 +00:00
parent b3c49ce59d
commit 259c8ec7a5
19 changed files with 344 additions and 262 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/CMakeLists.txt
+1 -1
View File
@@ -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})
rtabmap/launch/all_tr_learning.launch
+1 -9
View File
@@ -17,14 +17,6 @@
<include file="$(find rtabmap)/launch/rtabmap_sm.launch"/>
<!-- CAMERA -->
<node pkg="omni_camera_capture" type="omni_publisher"
name="omni_camera" respawn="false">
<param name="device" value="/dev/video0"/>
<param name="refresh_rate" value="2"/>
<param name="frame_id" value="omni_camera_link" />
<param name="image_encoding" value="RGB" /> <!-- GREY or RGB-->
<param name="image_size" value="FULL" /> <!-- PARTIAL or FULL -->
<param name="scale_divisor" value="1" /> <!-- 1, 2, 3, 4 or 6 in PARTIAL size-->
</node>
<include file="$(find rtabmap)/launch/camera.launch"/>
</launch>
rtabmap/launch/all_tr_learning_omni.launch
+31
View File
@@ -0,0 +1,31 @@
<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 -->
<node pkg="omni_camera_capture" type="omni_publisher"
name="omni_camera" respawn="false">
<param name="device" value="/dev/video0"/>
<param name="refresh_rate" value="2"/>
<param name="frame_id" value="omni_camera_link" />
<param name="image_encoding" value="RGB" /> <!-- GREY or RGB-->
<param name="image_size" value="FULL" /> <!-- PARTIAL or FULL -->
<param name="scale_divisor" value="1" /> <!-- 1, 2, 3, 4 or 6 in PARTIAL size-->
<remap from="/image" to="/image_raw" />
</node>
</launch>
rtabmap/launch/camera.launch
+8 -8
View File
@@ -1,10 +1,10 @@
<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 pkg="uvc_camera" type="camera_node" name="uvc_camera" output="screen">
<param name="width" type="int" value="640" />
<param name="height" type="int" value="480" />
<param name="fps" type="int" value="5" />
<param name="frame" type="string" value="wide_stereo" />
<param name="device" type="string" value="/dev/video0" />
<!-- <param name="camera_info_url" type="string" value="file://$(find uvc_camera)/example.yaml" /> -->
</node>
</launch>
rtabmap/launch/cameraOpenCV.launch
+10
View File
@@ -0,0 +1,10 @@
<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"/>
</launch>
rtabmap/launch/rtabmap_lc.launch
+8 -8
View File
@@ -1,14 +1,14 @@
<launch>
<!-- RTAB-MAP LOOP CLOSURE DETECTION VERSION -->
<!-- Camera parameters -->
<param name="device_id" value="0" type="int"/>
<param name="image_rate" value="1" type="int"/>
<param name="image_width" value="640" type="int"/>
<param name="image_height" value="480" type="int"/>
<!-- 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"/>
<node name="camera" pkg="rtabmap" type="camera_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
+8 -9
View File
@@ -2,10 +2,6 @@
<!-- RTAB-MAP SENSORY-MOTOR VERSION -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- image_to_sm parameters -->
<param name="resize_image_width" value="640" type="int"/>
<param name="resize_image_height" value="480" type="int"/>
<!-- Nodes -->
<!-- cmd_vel_a has priority on cmd_vel_b -->
@@ -28,13 +24,16 @@
</node>
<node name="rtabmap_input" pkg="rtabmap" type="input_node">
<remap from="cmd_vel" to="cmd_vel" />
<remap from="sm_state" to="sm_state" />
<remap from="camera_data" to="camera_data" />
<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">
<remap from="image" to="image" />
<remap from="sm_state" to="camera_data" />
<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/testCamera.launch
+9 -7
View File
@@ -1,11 +1,13 @@
<launch>
<!-- Camera parameters -->
<param name="cam/device_id" value="0" type="int"/>
<param name="cam/image_rate" value="10" type="int"/>
<param name="cam/image_width" value="640" type="int"/>
<param name="cam/image_height" value="480" type="int"/>
<!-- Nodes -->
<node name="cameraReceiver" pkg="rtabmap" type="camera_node_receiver" output="screen"/>
<node name="camera" pkg="rtabmap" type="camera_node"/>
<node pkg="uvc_camera" type="camera_node" name="uvc_camera" output="screen">
<param name="width" type="int" value="640" />
<param name="height" type="int" value="480" />
<param name="fps" type="int" value="30" />
<param name="frame" type="string" value="wide_stereo" />
<param name="device" type="string" value="/dev/video0" />
<!-- <param name="camera_info_url" type="string" value="file://$(find uvc_camera)/example.yaml" /> -->
</node>
</launch>
rtabmap/manifest.xml
+1
View File
@@ -14,6 +14,7 @@
<depend package="sensor_msgs"/>
<depend package="std_srvs"/>
<depend package="opencv2"/>
<depend package="uvc_camera"/>
<depend package="rtabmap_lib"/>
<depend package="turtlesim"/>
rtabmap/src/AbtrVelocityNode.cpp
+2
View File
@@ -91,4 +91,6 @@ int main(int argc, char** argv)
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
rtabmap/src/CameraNode.cpp
+3 -1
View File
@@ -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;
}
rtabmap/src/CameraNodeReceiver.cpp
+3 -1
View File
@@ -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;
}
rtabmap/src/CameraNodeReceiverSM.cpp
+3 -1
View File
@@ -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;
}
rtabmap/src/CameraWrapper.cpp
+2 -2
View File
@@ -21,7 +21,7 @@
CameraWrapper::CameraWrapper() :
camera_(0)
{
rosPublisher_ = nh_.advertise<sensor_msgs::Image>("image", 1);
rosPublisher_ = nh_.advertise<sensor_msgs::Image>("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());
}
rtabmap/src/CoreNode.cpp
+2
View File
@@ -32,4 +32,6 @@ int main(int argc, char** argv)
ROS_INFO("RTAB-Map started...");
ros::spin();
return 0;
}
rtabmap/src/ImageToSensorimotorStateNode.cpp
+176
View File
@@ -0,0 +1,176 @@
/*
* CameraNode.cpp
*
* Created on: 1 févr. 2010
* Author: labm2414
*/
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/SMState.h>
#include <utilite/ULogger.h>
#include <utilite/UTimer.h>
#include <cv_bridge/CvBridge.h>
#include "rtabmap/SensoryMotorState.h"
#include <std_msgs/Empty.h>
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<float> sensors;
int sensorStep = 0;
smState->getSensorsMerged(sensors, sensorStep);
msg->sensors = sensors;
msg->sensorStep = sensorStep;
std::vector<float> 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<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)
{
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<rtabmap::SensoryMotorState>("/sm_state", 1);
ros::Subscriber image_sub = nh.subscribe("/image_raw", 1, imgReceivedCallback);
updateParameters();
timer.start();
ros::spin();
return 0;
}
rtabmap/src/ImageToSensoryMotorStateNode.cpp
-149
View File
@@ -1,149 +0,0 @@
/*
* CameraNode.cpp
*
* Created on: 1 févr. 2010
* Author: labm2414
*/
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/SMState.h>
#include <utilite/ULogger.h>
#include <utilite/UTimer.h>
#include <cv_bridge/CvBridge.h>
#include "rtabmap/SensoryMotorState.h"
#include <std_msgs/Empty.h>
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<float> sensors;
int sensorStep = 0;
smState->getSensorsMerged(sensors, sensorStep);
msg->sensors = sensors;
msg->sensorStep = sensorStep;
std::vector<float> 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<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)
{
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<rtabmap::SensoryMotorState>("sm_state", 1);
updateParameters();
ros::spin();
}
rtabmap/src/InputNode.cpp
+55 -33
View File
@@ -8,39 +8,53 @@
#include "rtabmap/SensoryMotorState.h"
#include <geometry_msgs/Twist.h>
#include <utilite/UMutex.h>
#include <utilite/UTimer.h>
UMutex commandMutex;
std::list<std::vector<float> > 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<float> tmp;
int sizeActions = -1;
commandMutex.lock();
if(stateUpdated && actionsUpdated)
{
std::vector<float> actions;
int sizeActions = -1;
for(std::list<std::vector<float> >::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<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);
// 10 Hz max
while(commands.size() > 10)
{
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;
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<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);
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);
timer.start();
ros::spin();
return 0;
}
rtabmap/src/OutputNode.cpp
+21 -33
View File
@@ -10,7 +10,6 @@
#include <geometry_msgs/Twist.h>
#include <utilite/UMutex.h>
UMutex commandMutex;
std::vector<float> 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;
}