i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 01:07:49 +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/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;
}