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

switch rtabmap_lib svn co from the 0.3 branch to trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@110 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2011-06-20 16:49:22 +00:00
commit b4add8dbe8
49 changed files with 2494 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/src/InputNode.cpp
+86
View File
@@ -0,0 +1,86 @@
/*
* CameraNode.cpp
*
* Author: labm2414
*/
#include <ros/ros.h>
#include "rtabmap/SensoryMotorState.h"
#include <geometry_msgs/Twist.h>
#include <utilite/UMutex.h>
UMutex commandMutex;
std::list<std::vector<float> > commands;
ros::Publisher rosPublisher;
void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
{
ROS_INFO("Received camera data");
std::vector<float> tmp;
int sizeActions = -1;
commandMutex.lock();
{
for(std::list<std::vector<float> >::iterator iter = commands.begin(); iter!=commands.end();++iter)
{
tmp.insert(tmp.end(), iter->begin(), iter->end());
}
sizeActions = commands.size();
commands.clear();
}
commandMutex.unlock();
rtabmap::SensoryMotorStatePtr state(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);
}
void velocityReceivedCallback(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);
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 + 1 max
while(commands.size() > 11)
{
//remove the oldest
commands.pop_front();
}
}
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);
ros::spin();
}