Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14291] pkg/trunk
|
From: <ge...@us...> - 2009-04-23 00:05:30
|
Revision: 14291
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14291&view=rev
Author: gerkey
Date: 2009-04-23 00:05:27 +0000 (Thu, 23 Apr 2009)
Log Message:
-----------
Pushed params into local namespace for fake_localization and wavefront;
updated launch scripts accordingly. Removed amcl_player from 2dnav_stage.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
pkg/trunk/demos/2dnav_stage/manifest.xml
pkg/trunk/nav/amcl/src/amcl_node.cpp
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/wavefront/manifest.xml
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
Added Paths:
-----------
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_5cm.launch
Removed Paths:
-------------
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -3,11 +3,11 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
- <node pkg="wavefront" type="wavefront" respawn="false" >
+ <node pkg="wavefront" type="wavefront" name="wavefront" respawn="false" >
<remap from="scan" to="base_scan" />
+ <param name="robot_radius" value="0.325"/>
</node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
- <param name="max_publish_frequency" value="20.0"/>
<node pkg="fake_localization" type="fake_localization" respawn="false" />
</group>
</launch>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -5,9 +5,9 @@
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" />
<node pkg="wavefront" type="wavefront" respawn="false" >
<remap from="scan" to="base_scan" />
+ <param name="robot_radius" value="0.325"/>
</node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
- <param name="max_publish_frequency" value="20.0"/>
<node pkg="fake_localization" type="fake_localization" respawn="false" />
</group>
</launch>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -15,7 +15,6 @@
</node>
<node pkg="nav_view" type="nav_view" respawn="false" ns="robot_1"/>
<node pkg="nav_view" type="nav_view" respawn="false" ns="robot_0"/>
- <param name="max_publish_frequency" value="20.0"/>
<param name="robot_0/fake_localization_0/tf_prefix" value="robot_0" /><!--Broken see ros ticket:941 replaced by above-->
<node pkg="fake_localization" type="fake_localization" respawn="false" name="fake_localization_0" ns="robot_0" output="screen">
<param name="tf_prefix" value="robot_0" />
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -3,21 +3,40 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" output="screen"/>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" output="screen"/>
- <param name="pf_laser_max_beams" value="20"/>
- <param name="pf_min_samples" value="100"/>
- <param name="pf_max_samples" value="1000"/>
- <param name="pf_odom_drift_xx" value="0.5"/>
- <param name="pf_odom_drift_yy" value="0.5"/>
- <param name="pf_odom_drift_aa" value="0.5"/>
- <param name="pf_odom_drift_xa" value="0.5"/>
- <param name="pf_min_d" value="0.25"/> <!-- 25cm -->
- <param name="pf_min_a" value="0.349"/> <!-- 20 degrees -->
- <node pkg="amcl_player" type="amcl" respawn="false" output="screen">
+ <node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
+ <param name="laser_max_beams" value="30"/>
+ <param name="min_particles" value="100"/>
+ <param name="max_particles" value="5000"/>
+ <param name="kld_err" value="0.05"/>
+ <param name="kld_z" value="0.99"/>
+ <param name="odom_alpha1" value="0.2"/>
+ <param name="odom_alpha2" value="0.2"/>
+ <param name="odom_alpha3" value="0.2"/>
+ <param name="odom_alpha4" value="0.2"/>
+ <param name="laser_z_hit" value="0.95"/>
+ <param name="laser_z_short" value="0.1"/>
+ <param name="laser_z_max" value="0.05"/>
+ <param name="laser_z_rand" value="0.05"/>
+ <param name="laser_sigma_hit" value="0.3"/>
+ <param name="laser_lambda_short" value="0.1"/>
+ <param name="laser_lambda_short" value="0.1"/>
+ <param name="laser_model_type" value="likelihood_field"/>
+ <param name="laser_likelihood_max_dist" value="2.0"/>
+ <param name="update_min_d" value="0.2"/>
+ <param name="update_min_a" value="0.5"/>
+ <param name="odom_frame_id" value="odom"/>
+ <param name="resample_interval" value="2"/>
+ <param name="transform_tolerance" value="0.1"/>
+ <param name="recovery_alpha_slow" value="0.0"/>
+ <param name="recovery_alpha_fast" value="0.0"/>
+ <param name="robot_x_start" value="0.0"/>
+ <param name="robot_y_start" value="0.0"/>
+ <param name="robot_th_start" value="0.0"/>
</node>
- <param name="robot_radius" value="0.325"/>
- <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
+ <node pkg="wavefront" type="wavefront" name="wavefront" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
+ <param name="robot_radius" value="0.325"/>
</node>
<node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
<!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/--><!-- You must have nav_view_sdl installed it is not a tracked dependency-->
Deleted: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -1,26 +0,0 @@
-<launch>
-
- <group name="wg">
- <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-2.5cm.world" respawn="false" output="screen"/>
- <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.025.pgm 0.025" respawn="false" output="screen"/>
- <param name="pf_laser_max_beams" value="6"/>
- <param name="pf_min_samples" value="100"/>
- <param name="pf_max_samples" value="1000"/>
- <param name="pf_odom_drift_xx" value="0.1"/>
- <param name="pf_odom_drift_yy" value="0.1"/>
- <param name="pf_odom_drift_aa" value="0.1"/>
- <param name="pf_odom_drift_xa" value="0.1"/>
- <param name="pf_min_d" value="0.25"/> <!-- 25cm -->
- <param name="pf_min_a" value="0.349"/> <!-- 20 degrees -->
- <node pkg="amcl_player" type="amcl_player" respawn="false" output="screen">
- <remap from="scan" to="base_scan" />
- </node>
- <param name="robot_radius" value="0.325"/>
- <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
- <remap from="scan" to="base_scan" />
- </node>
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
- <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> --><!-- You must have nav_view_sdl installed it is not a tracked dependency-->
- </group>
-</launch>
-
Copied: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_5cm.launch (from rev 14280, pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch)
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_5cm.launch (rev 0)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_5cm.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -0,0 +1,17 @@
+<launch>
+
+ <group name="wg">
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" output="screen"/>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" output="screen"/>
+ <node pkg="amcl" type="amcl" respawn="false" output="screen">
+ <remap from="scan" to="base_scan" />
+ </node>
+ <node pkg="wavefront" type="wavefront" name="wavefront" respawn="false" output="screen">
+ <remap from="scan" to="base_scan" />
+ <param name="robot_radius" value="0.325"/>
+ </node>
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
+ <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> --><!-- You must have nav_view_sdl installed it is not a tracked dependency-->
+ </group>
+</launch>
+
Property changes on: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_5cm.launch
___________________________________________________________________
Added: svn:mergeinfo
+
Deleted: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -1,45 +0,0 @@
-<launch>
-
- <group name="wg">
- <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" output="screen"/>
- <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" output="screen"/>
- <node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
- <remap from="scan" to="base_scan" />
- <param name="laser_max_beams" value="30"/>
- <param name="min_particles" value="100"/>
- <param name="max_particles" value="5000"/>
- <param name="kld_err" value="0.05"/>
- <param name="kld_z" value="0.99"/>
- <param name="odom_alpha1" value="0.2"/>
- <param name="odom_alpha2" value="0.2"/>
- <param name="odom_alpha3" value="0.2"/>
- <param name="odom_alpha4" value="0.2"/>
- <param name="laser_z_hit" value="0.95"/>
- <param name="laser_z_short" value="0.1"/>
- <param name="laser_z_max" value="0.05"/>
- <param name="laser_z_rand" value="0.05"/>
- <param name="laser_sigma_hit" value="0.3"/>
- <param name="laser_lambda_short" value="0.1"/>
- <param name="laser_lambda_short" value="0.1"/>
- <param name="laser_model_type" value="likelihood_field"/>
- <param name="laser_likelihood_max_dist" value="2.0"/>
- <param name="update_min_d" value="0.2"/>
- <param name="update_min_a" value="0.5"/>
- <param name="odom_frame_id" value="odom"/>
- <param name="resample_interval" value="10"/>
- <param name="transform_tolerance" value="0.1"/>
- <param name="recovery_alpha_slow" value="0.001"/>
- <param name="recovery_alpha_fast" value="0.1"/>
- <param name="robot_x_start" value="0.0"/>
- <param name="robot_y_start" value="0.0"/>
- <param name="robot_th_start" value="0.0"/>
- </node>
- <param name="robot_radius" value="0.325"/>
- <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
- <remap from="scan" to="base_scan" />
- </node>
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
- <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/--><!-- You must have nav_view_sdl installed it is not a tracked dependency-->
- </group>
-</launch>
-
Modified: pkg/trunk/demos/2dnav_stage/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/manifest.xml 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/manifest.xml 2009-04-23 00:05:27 UTC (rev 14291)
@@ -6,8 +6,6 @@
<url>http://pr.willowgarage.com/wiki/FIXME</url>
<depend package="nav_view"/>
<depend package="amcl"/>
- <!-- TODO: remove amcl_player -->
- <depend package="amcl_player"/>
<depend package="fake_localization"/>
<depend package="stage"/>
<depend package="map_server"/>
Modified: pkg/trunk/nav/amcl/src/amcl_node.cpp
===================================================================
--- pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-23 00:05:27 UTC (rev 14291)
@@ -84,10 +84,10 @@
- @b "~kld_z" (double) : Upper standard normal quantile for (1 - p), where p is the probability that the error on the estimated distrubition will be less than kld_err, default: 0.99
- @b "~update_min_d" (double) : Translational movement required before performing a filter update, default: 0.2 meters
- @b "~update_min_a" (double) : Rotational movement required before performing a filter update, default: M_PI/6.0 radians
- - @b "~resample_interval" (int) : Number of filter updates required before resampling, default: 10
+ - @b "~resample_interval" (int) : Number of filter updates required before resampling, default: 2
- @b "~transform_tolerance" (double) : Time with which to post-date the transform that is published, to indicate that this transform is valid into the future, default: 0.1 seconds
- - @b "~recovery_alpha_slow" (double) : Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses, default: 0.001
- - @b "~recovery_alpha_fast" (double) : Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses, default: 0.1
+ - @b "~recovery_alpha_slow" (double) : Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses, default: 0.0, which means disabled (a good value might be 0.001)
+ - @b "~recovery_alpha_fast" (double) : Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses, default: 0.0, which means disabled (a good value might be 0.1)
- @b "~initial_pose_x" (double) : Initial pose estimate (x), used to initialize filter with Gaussian distribution, default: 0.0 meters
- @b "~initial_pose_y" (double) : Initial pose estimate (y), used to initialize filter with Gaussian distribution, default: 0.0 meters
- @b "~initial_pose_a" (double) : Initial pose estimate (yaw), used to initialize filter with Gaussian distribution, default: 0.0 radians
@@ -302,7 +302,7 @@
ros::Node::instance()->param("~update_min_d", d_thresh_, 0.2);
ros::Node::instance()->param("~update_min_a", a_thresh_, M_PI/6.0);
ros::Node::instance()->param("~odom_frame_id", odom_frame_id_, std::string("odom"));
- ros::Node::instance()->param("~resample_interval", resample_interval_, 10);
+ ros::Node::instance()->param("~resample_interval", resample_interval_, 2);
double tmp_tol;
ros::Node::instance()->param("~transform_tolerance", tmp_tol, 0.1);
ros::Node::instance()->param("~recovery_alpha_slow", alpha_slow, 0.001);
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-04-23 00:05:27 UTC (rev 14291)
@@ -40,13 +40,15 @@
@htmlinclude manifest.html
-@b odom_localization is simply forwards the odometry information
+@b odom_localization Takes in ground truth pose information for a robot
+base (e.g., from a simulator or motion capture system) and republishes
+it as if a localization system were in use.
<hr>
@section usage Usage
@verbatim
-$ odom_localization
+$ fake_localization
@endverbatim
<hr>
@@ -54,28 +56,26 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "base_pose_ground_truth"/robot_msgs::PoseWithRatesStamped : robot's odometric pose. Only the position information is used (velocity is ignored).
-- @b "initialpose"/Pose2DFloat32 : robot's odometric pose. Only the position information is used (velocity is ignored).
+- @b "base_pose_ground_truth" robot_msgs/PoseWithRatesStamped : robot's odometric pose. Only the position information is used (velocity is ignored).
Publishes to (name / type):
-- @b "localizedpose"/RobotBase2DOdom : robot's localized map pose. Only the position information is set (no velocity).
-- @b "particlecloud"/ParticleCloud : fake set of particles being maintained by the filter (one paricle only).
+- @b "amcl_pose" robot_msgs/PoseWithCovariance : robot's estimated pose in the map, with covariance
+- @b "particlecloud" robot_msgs/ParticleCloud : fake set of particles being maintained by the filter (one paricle only).
<hr>
@section parameters ROS parameters
-- None
+- "~odom_frame_id" (string) : The odometry frame to be used, default: "odom"
**/
#include <ros/node.h>
#include <ros/time.h>
-#include <deprecated_msgs/RobotBase2DOdom.h>
#include <robot_msgs/PoseWithRatesStamped.h>
#include <robot_msgs/ParticleCloud.h>
-#include <deprecated_msgs/Pose2DFloat32.h>
+#include <robot_msgs/PoseWithCovariance.h>
#include <angles/angles.h>
@@ -91,7 +91,7 @@
public:
FakeOdomNode(void) : ros::Node("fake_localization")
{
- advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose",1);
+ advertise<robot_msgs::PoseWithCovariance>("amcl_pose",1);
advertise<robot_msgs::ParticleCloud>("particlecloud",1);
m_tfServer = new tf::TransformBroadcaster(*this);
m_tfListener = new tf::TransformListener(*this);
@@ -99,8 +99,7 @@
m_base_pos_received = false;
- param("pf_odom_frame_id", odom_frame_id_, std::string("odom"));
- m_iniPos.x = m_iniPos.y = m_iniPos.th = 0.0;
+ param("~odom_frame_id", odom_frame_id_, std::string("odom"));
m_particleCloud.set_particles_size(1);
notifier = new tf::MessageNotifier<robot_msgs::PoseWithRatesStamped>(m_tfListener, this,
boost::bind(&FakeOdomNode::update, this, _1),
@@ -139,8 +138,7 @@
robot_msgs::PoseWithRatesStamped m_basePosMsg;
robot_msgs::ParticleCloud m_particleCloud;
- deprecated_msgs::RobotBase2DOdom m_currentPos;
- deprecated_msgs::Pose2DFloat32 m_iniPos;
+ robot_msgs::PoseWithCovariance m_currentPos;
//parameter for what odom to use
std::string odom_frame_id_;
@@ -163,13 +161,13 @@
message->pos.position.y,
0.0*message->pos.position.z )); // zero height for base_footprint
- double x = txi.getOrigin().x() + m_iniPos.x;
- double y = txi.getOrigin().y() + m_iniPos.y;
+ double x = txi.getOrigin().x();
+ double y = txi.getOrigin().y();
double z = txi.getOrigin().z();
double yaw, pitch, roll;
txi.getBasis().getEulerZYX(yaw, pitch, roll);
- yaw = angles::normalize_angle(yaw + m_iniPos.th);
+ yaw = angles::normalize_angle(yaw);
tf::Transform txo(tf::Quaternion(yaw, pitch, roll), tf::Point(x, y, z));
@@ -204,16 +202,16 @@
// Publish localized pose
m_currentPos.header = message->header;
m_currentPos.header.frame_id = "map"; ///\todo fixme hack
- m_currentPos.pos.x = x;
- m_currentPos.pos.y = y;
- m_currentPos.pos.th = yaw;
- publish("localizedpose", m_currentPos);
+ m_currentPos.pose.position.x = x;
+ m_currentPos.pose.position.y = y;
+ // Leave z as zero
+ tf::QuaternionTFToMsg(tf::Quaternion(yaw, 0.0, 0.0),
+ m_currentPos.pose.orientation);
+ // Leave covariance as zero
+ publish("amcl_pose", m_currentPos);
// The particle cloud is the current position. Quite convenient.
- robot_msgs::Pose pos;
- tf::PoseTFToMsg(tf::Pose(tf::Quaternion(m_currentPos.pos.th, 0, 0), tf::Vector3(m_currentPos.pos.x, m_currentPos.pos.y, 0)),
- pos);
- m_particleCloud.particles[0] = pos;
+ m_particleCloud.particles[0] = m_currentPos.pose;
publish("particlecloud", m_particleCloud);
}
};
Modified: pkg/trunk/nav/wavefront/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront/manifest.xml 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/nav/wavefront/manifest.xml 2009-04-23 00:05:27 UTC (rev 14291)
@@ -1,5 +1,5 @@
<package>
- <description>A ROS node that uses the Player libwavefront_standalone library, which implements wavefront path-planning for a planar robot.</description>
+ <description>A ROS node that uses the Player wavefront library, which implements wavefront path-planning for a planar robot.</description>
<author>Brian P. Gerkey</author>
<license>LGPL</license>
<review status="unreviewed" notes=""/>
Modified: pkg/trunk/nav/wavefront/src/wavefront_node.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-23 00:05:27 UTC (rev 14291)
@@ -1,31 +1,21 @@
/*
- * wavefront_player
+ * wavefront
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
*
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the <ORGANIZATION> nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
*
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
@@ -34,7 +24,7 @@
@htmlinclude manifest.html
-@b wavefront_player is a path-planning system for a robot moving in 2D. It
+@b wavefront is a path-planning system for a robot moving in 2D. It
implements the wavefront algorithm (described in various places; Latombe's
book is a good reference), which uses dynamic programming over an occupancy
grid map to find the lowest-cost path from the robot's pose to the goal.
@@ -43,8 +33,7 @@
documentation, consult <a
href="http://playerstage.sourceforge.net/doc/Player-cvs/player/group__driver__wavefront.html">Player
wavefront documentation</a>. Note that this node does not actually wrap the @b
-wavefront driver, but rather calls into the underlying library, @b
-libwavefront_standalone.
+wavefront driver, but rather the underlying planner code.
The planning algorithm assumes that the robot is circular and holonomic.
Under these assumptions, it efficiently dilates obstacles and then plans
@@ -64,7 +53,7 @@
@section usage Usage
@verbatim
-$ wavefront_player
+$ wavefront
@endverbatim
<hr>
@@ -72,30 +61,27 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "localizedpose"/RobotBase2DOdom : robot's map pose. Only the position information is used (velocity is ignored).
-- @b "goal"/robot_msgs/PoseStamped : goal for the robot.
-- @b "scan"/laser_scan/LaserScan : laser scans. Used to temporarily modify the map for dynamic obstacles.
+- @b "tf_message" tf/tfMessage: robot's pose in the "map" frame
+- @b "goal" robot_msgs/PoseStamped : goal for the robot.
+- @b "scan" laser_scan/LaserScan : laser scans. Used to temporarily modify the map for dynamic obstacles.
Publishes to (name / type):
-- @b "cmd_vel"/PoseDot : velocity commands to robot
-- @b "state"/Planner2DState : current planner state (e.g., goal reached, no
-path)
-- @b "gui_path"/Polyline2D : current global path (for visualization)
-- @b "gui_laser"/Polyline2D : re-projected laser scans (for visualization)
+- @b "cmd_vel" robot_msgs/PoseDot : velocity commands to robot
+- @b "state" robot_actions/MoveBaseState : current planner state (e.g., goal reached, no path)
+- @b "gui_path" robot_msgs/Polyline2D : current global path (for visualization)
+- @b "gui_laser" robot_msgs/Polyline2D : re-projected laser scans (for visualization)
-@todo Start using libTF for transform management:
- - subscribe to odometry and use transform to recover map pose.
-
<hr>
@section parameters ROS parameters
-- None
+ - @b "~dist_eps" (double) : Goal distance tolerance (how close the robot must be to the goal before stopping), default: 1.0 meters
+ - @b "~robot_radius" (double) : The robot's largest radius (the planner treats it as a circle), default: 0.175 meters
+ - @b "~dist_penalty" (double) : Penalty factor for coming close to obstacles, default: 2.0
-@todo There are an enormous number of parameters, values for which are all
-currently hardcoded. These should be exposed as ROS parameters. In
-particular, robot radius and safety distance must be changed for each
-robot.
+@todo There are many more parameters of the underlying planner, values
+for which are currently hardcoded. These should be exposed as ROS
+parameters.
**/
#include <stdio.h>
@@ -243,7 +229,7 @@
#define _xy_tolerance 1e-3
#define _th_tolerance 1e-3
-#define USAGE "USAGE: wavefront_player"
+#define USAGE "USAGE: wavefront"
int
main(int argc, char** argv)
@@ -264,7 +250,7 @@
}
WavefrontNode::WavefrontNode() :
- ros::Node("wavefront_player"),
+ ros::Node("wavefront"),
planner_state(NO_GOAL),
enable(true),
rotate_dir(0),
@@ -294,9 +280,9 @@
///\todo does this need to be initialized? global_pose.setIdentity();
// set a few parameters. leave defaults just as in the ctor initializer list
- param("dist_eps", dist_eps, 1.0);
- param("robot_radius", robot_radius, 0.175);
- param("dist_penalty", dist_penalty, 2.0);
+ param("~dist_eps", dist_eps, 1.0);
+ param("~robot_radius", robot_radius, 0.175);
+ param("~dist_penalty", dist_penalty, 2.0);
// get map via RPC
robot_srvs::StaticMap::Request req;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
