i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-08 10:47:46 +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);
rtabmap/src/OutputNode.cpp
+8 -5
View File
@@ -14,6 +14,7 @@ std::vector<float> commands;
int commandSize = 0;
int commandIndex = 0;
ros::Publisher rosPublisher;
int commandsHz = 10; //10 Hz
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
{
@@ -35,14 +36,16 @@ void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
int main(int argc, char** argv)
{
ros::init(argc, argv, "output_node");
ros::NodeHandle n;
ros::NodeHandle n("~");
n.param("commands_hz", commandsHz, commandsHz);
ROS_INFO("commands_hz=%d", commandsHz);
ros::Subscriber infoTopic;
ros::Subscriber infoExTopic;
infoTopic = n.subscribe("rtabmap_info", 1, infoReceivedCallback);
infoExTopic = n.subscribe("rtabmap_info_x", 1, infoExReceivedCallback);
rosPublisher = n.advertise<geometry_msgs::Twist>("rtabmap/cmd_vel", 1);
infoTopic = n.subscribe("/rtabmap_info", 1, infoReceivedCallback);
infoExTopic = n.subscribe("/rtabmap_info_x", 1, infoExReceivedCallback);
rosPublisher = n.advertise<geometry_msgs::Twist>("/rtabmap/cmd_vel", 1);
ros::Rate loop_rate(10); // 10 Hz
ros::Rate loop_rate(commandsHz); // Hz
while(ros::ok())
{
if(commandIndex>=0 &&