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

Added command's rate parameter in input_node and output_node

Browse Source 
Set default to 2 Hz for telerobot example

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@308 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2011-08-23 18:46:51 +00:00
parent e2b147285f
commit db8606effd
5 changed files with 23 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/src/InputNode.cpp
+10 -4
View File
@@ -18,6 +18,7 @@ rtabmap::SensoryMotorStatePtr state;
bool stateUpdated = false;
bool actionsUpdated = false;
int nbCommands = 10;
void publish()
{
@@ -78,14 +79,14 @@ void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
commands.push_back(v);
// 10 Hz max
while(commands.size() > 10)
while(commands.size() > nbCommands)
{
ROS_WARN("Too many commands (%zu) > 10, removing the oldest...", commands.size());
ROS_WARN("Too many commands (%zu) > %d, removing the oldest...", commands.size(), nbCommands);
//remove the oldest
commands.pop_front();
}
if(commands.size() == 10)
if(commands.size() == nbCommands)
{
actionsUpdated = true;
publish();
@@ -95,7 +96,12 @@ void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
int main(int argc, char** argv)
{
ros::init(argc, argv, "input_node");
ros::NodeHandle n;
ros::NodeHandle n("~");
n.param("nb_commands", nbCommands, nbCommands);
ROS_INFO("nb_commands=%d", nbCommands);
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);