Added command's rate parameter in input_node and output_node

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

rtabmap/launch/all_tr_learning.launch

@@ -17,6 +17,6 @@
 	<include file="$(find rtabmap)/launch/rtabmap_sm.launch"/>
 	
 	<!-- CAMERA -->
-	<include file="$(find rtabmap)/launch/camera.launch"/>
+	<include file="$(find rtabmap)/launch/cameraOpenCV.launch"/>
 
 </launch>

rtabmap/launch/cameraOpenCV.launch

@@ -1,7 +1,7 @@
 <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_rate" value="2" type="int"/>
 	<param name="cam/image_width" value="640" type="int"/>
 	<param name="cam/image_height" value="480" type="int"/>
 	

rtabmap/launch/rtabmap_sm.launch

@@ -12,6 +12,7 @@
 	</node>
 	
 	<node name="rtabmap_output" pkg="rtabmap" type="output_node">
+	    <param name="commands_hz" value="5" type="int"/>
 		<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
 		<remap from="rtabmap_info" to="rtabmap_info" />
 		<remap from="rtabmap_info_x" to="rtabmap_info_x" />
@@ -24,13 +25,14 @@
 	</node>
 
 	<node name="rtabmap_input" pkg="rtabmap" type="input_node">
+		<param name="nb_commands" value="5" type="int"/>
 		<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">
-		<param name="image_rate" value="1" type="int"/>
+		<param name="image_rate" value="2" 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" />

rtabmap/src/InputNode.cpp

@@ -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

@@ -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);
+	infoTopic = n.subscribe("/rtabmap_info", 1, infoReceivedCallback);
-	infoExTopic = n.subscribe("rtabmap_info_x", 1, infoExReceivedCallback);
+	infoExTopic = n.subscribe("/rtabmap_info_x", 1, infoExReceivedCallback);
-	rosPublisher = n.advertise<geometry_msgs::Twist>("rtabmap/cmd_vel", 1);
+	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 &&