i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37: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/launch/all_tr_learning.launch
+1 -1
View File
@@ -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 -1
View File
@@ -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
+3 -1
View File
@@ -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
+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 &&