Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13931] pkg/trunk
|
From: <ei...@us...> - 2009-04-16 02:48:40
|
Revision: 13931
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13931&view=rev
Author: eitanme
Date: 2009-04-16 02:48:36 +0000 (Thu, 16 Apr 2009)
Log Message:
-----------
r21187@cib: eitan | 2009-04-10 14:30:46 -0700
Making a branch for re-working the navigation stack
r21188@cib: eitan | 2009-04-10 14:32:03 -0700
Working to re-structure the trajectory controller for the new navstack
r21191@cib: eitan | 2009-04-10 14:49:06 -0700
Removed dependency on deprecated messages
r21192@cib: eitan | 2009-04-10 15:22:26 -0700
Removing old tester that is no longer needed
r21193@cib: eitan | 2009-04-10 15:53:40 -0700
Created a ros wrapper for the navfn planner
r21194@cib: eitan | 2009-04-10 17:26:29 -0700
Got it moving a bit
r21237@cib: eitan | 2009-04-13 13:50:57 -0700
Added support for specifying which observations should be used to clear space out.
r21238@cib: eitan | 2009-04-13 13:54:48 -0700
Updated documentation
r21255@cib: eitan | 2009-04-13 17:04:09 -0700
Modified to work with better cost map copies
r21256@cib: eitan | 2009-04-13 17:05:19 -0700
Better copying of costmap... sets a reference now rather than newing a pointer
r21257@cib: eitan | 2009-04-13 17:07:18 -0700
changed to work with better cost map copies
r21258@cib: eitan | 2009-04-13 17:45:58 -0700
Overloaded an assignment operator for the cost map
r21284@cib: eitan | 2009-04-14 10:36:29 -0700
Added z value to pose message, added a missing delete to action runner
r21285@cib: eitan | 2009-04-14 10:54:54 -0700
Fixed another memory leak
r21286@cib: eitan | 2009-04-14 11:37:03 -0700
Now valgrind clean
r21287@cib: eitan | 2009-04-14 11:37:48 -0700
Now valgrind clean
r21288@cib: eitan | 2009-04-14 11:38:30 -0700
build for release by default
r21333@cib: eitan | 2009-04-14 14:36:17 -0700
Works with point grid now
r21334@cib: eitan | 2009-04-14 14:36:49 -0700
Allow outside entities to get observations from the observation buffers
r21335@cib: eitan | 2009-04-14 14:38:45 -0700
Modified to work with new costmap for freespace controller
r21350@cib: eitan | 2009-04-14 16:03:59 -0700
Put in a check to see if the robot is stopped
r21352@cib: eitan | 2009-04-15 11:20:06 -0700
Fixed improper delete
r21391@cib: eitan | 2009-04-15 14:19:49 -0700
The old trajectory rollout package
r21392@cib: eitan | 2009-04-15 16:30:31 -0700
Changing to old_costmap_2d
r21393@cib: eitan | 2009-04-15 16:31:30 -0700
A compiling version of the old_costmap_2d
r21394@cib: eitan | 2009-04-15 16:36:21 -0700
Removed cpp message dir
r21395@cib: eitan | 2009-04-15 16:39:36 -0700
Removed msg/cpp from commit
r21396@cib: eitan | 2009-04-15 16:42:09 -0700
Updated to work with new action model
r21397@cib: eitan | 2009-04-15 16:50:43 -0700
Renaming new costmap to costmap_2d
r21398@cib: eitan | 2009-04-15 16:51:56 -0700
Compiling version
r21399@cib: eitan | 2009-04-15 16:53:06 -0700
Massive restructuring of naviagtion package names and directories
r21400@cib: eitan | 2009-04-15 17:12:55 -0700
Trajectory rollout is now base_local_planner
r21401@cib: eitan | 2009-04-15 17:17:55 -0700
Renamed TrajectoryController to TrajectoryPlanner
r21402@cib: eitan | 2009-04-15 17:28:11 -0700
Modified to work with new naming
r21403@cib: eitan | 2009-04-15 17:35:42 -0700
Now compiling with tests passing
r21404@cib: eitan | 2009-04-15 17:46:39 -0700
Updated to work with new costmap
r21405@cib: eitan | 2009-04-15 17:48:12 -0700
Updated to work with renaming of costmap and traj_rollout
r21406@cib: eitan | 2009-04-15 17:49:41 -0700
Remapping from goal to activate
r21407@cib: eitan | 2009-04-15 18:09:46 -0700
Update to work with new naming of old_costmap_2d
r21408@cib: eitan | 2009-04-15 18:14:52 -0700
costmap_2d has become old_costmap_2d and is now deprecated. This package should switch to the new costmap_2d.
r21462@cib: eitan | 2009-04-15 18:22:51 -0700
costmap_2d is now deprecated and named old_costmap_2d, this package should be moved to the new costmap_2d
r21463@cib: eitan | 2009-04-15 18:24:30 -0700
Removed a dependency on costmap_2d that didn't exist. Added one on angles that did
r21464@cib: eitan | 2009-04-15 18:25:44 -0700
costmap_2d is now deprecated and named old_costmap_2d, this package should be moved to the new costmap_2d
r21465@cib: eitan | 2009-04-15 18:38:03 -0700
New namespace for old_costmap_2d
r21466@cib: eitan | 2009-04-15 18:55:32 -0700
Namespace change for old_costmap_2d
r21467@cib: eitan | 2009-04-15 18:57:30 -0700
Namespace change for old_costmap_2d
r21468@cib: eitan | 2009-04-15 19:00:51 -0700
Namespace change for old_costmap_2d
r21469@cib: eitan | 2009-04-15 19:02:29 -0700
Namespace change for old_costmap_2d
r21470@cib: eitan | 2009-04-15 19:06:45 -0700
Now works with deprecated version of costmap_2d... old_costmap_2d
r21477@cib: eitan | 2009-04-15 19:13:44 -0700
Changed launch map a bit
r21478@cib: eitan | 2009-04-15 19:16:13 -0700
Changed INFO to DBUG in a few places
r21479@cib: eitan | 2009-04-15 19:24:58 -0700
Added a test_nav package
r21482@cib: eitan | 2009-04-15 19:45:08 -0700
Tyring to get merge to work
Merging new navigation stack into trunk
Modified Paths:
--------------
pkg/trunk/deprecated/highlevel_controllers/include/highlevel_controllers/move_base.hh
pkg/trunk/deprecated/highlevel_controllers/manifest.xml
pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_follow.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn_constrained.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/deprecated/old_costmap_2d/CMakeLists.txt
pkg/trunk/deprecated/old_costmap_2d/manifest.xml
pkg/trunk/deprecated/old_costmap_2d/src/basic_observation_buffer.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_standalone.cpp
pkg/trunk/deprecated/old_costmap_2d/src/observation_buffer.cpp
pkg/trunk/deprecated/old_costmap_2d/src/obstacle_map_accessor.cpp
pkg/trunk/deprecated/old_costmap_2d/src/test/benchmark.cpp
pkg/trunk/deprecated/old_costmap_2d/src/test/costmap2d_pqueue_benchmark.cpp
pkg/trunk/deprecated/old_costmap_2d/src/test/module-tests.cpp
pkg/trunk/highlevel/nav/CMakeLists.txt
pkg/trunk/highlevel/nav/manifest.xml
pkg/trunk/highlevel/nav/src/nav.cpp
pkg/trunk/highlevel/robot_actions/include/robot_actions/message_adapter.h
pkg/trunk/highlevel/robot_actions/msg/Pose2D.msg
pkg/trunk/motion_planning/mpbench/CMakeLists.txt
pkg/trunk/motion_planning/mpbench/manifest.xml
pkg/trunk/motion_planning/mpbench/src/gfx.cpp
pkg/trunk/motion_planning/mpbench/src/mpbenchmark.cpp
pkg/trunk/motion_planning/mpbench/src/setup.cpp
pkg/trunk/motion_planning/mpglue/include/mpglue/costmap.h
pkg/trunk/motion_planning/mpglue/manifest.xml
pkg/trunk/motion_planning/mpglue/src/costmap.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
pkg/trunk/motion_planning/navfn/CMakeLists.txt
pkg/trunk/motion_planning/navfn/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/include/sbpl_planner_node/sbpl_planner_node.h
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/src/sbpl_planner_node.cpp
pkg/trunk/nav/base_local_planner/CMakeLists.txt
pkg/trunk/nav/base_local_planner/manifest.xml
pkg/trunk/nav/base_local_planner/scripts/indefinite_nav.py
pkg/trunk/nav/base_local_planner/src/costmap_model.cpp
pkg/trunk/nav/base_local_planner/src/map_cell.cpp
pkg/trunk/nav/base_local_planner/src/map_grid.cpp
pkg/trunk/nav/base_local_planner/src/point_grid.cpp
pkg/trunk/nav/base_local_planner/src/trajectory.cpp
pkg/trunk/nav/base_local_planner/src/voxel_grid_model.cpp
pkg/trunk/nav/base_local_planner/test/utest.cpp
pkg/trunk/world_models/new_costmap_2d/CMakeLists.txt
pkg/trunk/world_models/new_costmap_2d/costmap_demo.xml
pkg/trunk/world_models/new_costmap_2d/launch_costmap_2d_ros.xml
pkg/trunk/world_models/new_costmap_2d/launch_map.xml
pkg/trunk/world_models/new_costmap_2d/mainpage.dox
pkg/trunk/world_models/new_costmap_2d/manifest.xml
pkg/trunk/world_models/new_costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/new_costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/new_costmap_2d/src/observation_buffer.cpp
Added Paths:
-----------
pkg/trunk/deprecated/highlevel_controllers/
pkg/trunk/deprecated/old_costmap_2d/
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/basic_observation_buffer.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap2d_pqueue_benchmark.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap_2d.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap_node.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/observation.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/observation_buffer.h
pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/obstacle_map_accessor.h
pkg/trunk/deprecated/trajectory_rollout/
pkg/trunk/deprecated/trajectory_rollout/CMakeLists.txt
pkg/trunk/deprecated/trajectory_rollout/Makefile
pkg/trunk/deprecated/trajectory_rollout/include/
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/costmap_model.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/governor_node.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/map_cell.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/map_grid.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/planar_laser_scan.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/point_grid.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/trajectory.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/trajectory_controller.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/trajectory_inc.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/voxel_grid_model.h
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/world_model.h
pkg/trunk/deprecated/trajectory_rollout/lib/
pkg/trunk/deprecated/trajectory_rollout/mainpage.dox
pkg/trunk/deprecated/trajectory_rollout/manifest.xml
pkg/trunk/deprecated/trajectory_rollout/msg/
pkg/trunk/deprecated/trajectory_rollout/msg/GlobalPlan.msg
pkg/trunk/deprecated/trajectory_rollout/msg/Position2DInt.msg
pkg/trunk/deprecated/trajectory_rollout/msg/ScoreMap2D.msg
pkg/trunk/deprecated/trajectory_rollout/msg/ScoreMapCell2D.msg
pkg/trunk/deprecated/trajectory_rollout/msg/WavefrontPlan.msg
pkg/trunk/deprecated/trajectory_rollout/scripts/
pkg/trunk/deprecated/trajectory_rollout/scripts/indefinite_nav.py
pkg/trunk/deprecated/trajectory_rollout/src/
pkg/trunk/deprecated/trajectory_rollout/src/costmap_model.cpp
pkg/trunk/deprecated/trajectory_rollout/src/governor_node.cpp
pkg/trunk/deprecated/trajectory_rollout/src/map_cell.cpp
pkg/trunk/deprecated/trajectory_rollout/src/map_grid.cpp
pkg/trunk/deprecated/trajectory_rollout/src/point_grid.cpp
pkg/trunk/deprecated/trajectory_rollout/src/trajectory.cpp
pkg/trunk/deprecated/trajectory_rollout/src/trajectory_controller.cpp
pkg/trunk/deprecated/trajectory_rollout/src/trajectory_controller_ros.cpp
pkg/trunk/deprecated/trajectory_rollout/src/voxel_grid_model.cpp
pkg/trunk/deprecated/trajectory_rollout/test/
pkg/trunk/deprecated/trajectory_rollout/test/utest.cpp
pkg/trunk/highlevel/nav/src/move_base_action.cpp
pkg/trunk/highlevel/test_nav/
pkg/trunk/highlevel/test_nav/config/
pkg/trunk/highlevel/test_nav/config/base_local_planner_params.xml
pkg/trunk/highlevel/test_nav/config/navfn_params.xml
pkg/trunk/highlevel/test_nav/launch_navstack.xml
pkg/trunk/highlevel/test_nav/manifest.xml
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/nav/base_local_planner/
pkg/trunk/nav/base_local_planner/include/base_local_planner/
pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/map_cell.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/map_grid.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/planar_laser_scan.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/point_grid.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_inc.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/voxel_grid_model.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/world_model.h
pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/world_models/new_costmap_2d/
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/cell_data.h
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/cost_values.h
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/observation.h
pkg/trunk/world_models/new_costmap_2d/include/costmap_2d/observation_buffer.h
Removed Paths:
-------------
pkg/trunk/highlevel/highlevel_controllers/
pkg/trunk/nav/trajectory_rollout/
pkg/trunk/world_models/costmap_2d/
pkg/trunk/world_models/new_costmap/
Property Changed:
----------------
pkg/trunk/
pkg/trunk/world_models/new_costmap_2d/src/observation_buffer.cpp
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/deprecated/highlevel_controllers/include/highlevel_controllers/move_base.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-04-16 02:48:36 UTC (rev 13931)
@@ -39,8 +39,8 @@
#include <trajectory_rollout/trajectory_controller_ros.h>
// Costmap used for the map representation
-#include <costmap_2d/costmap_2d.h>
-#include <costmap_2d/basic_observation_buffer.h>
+#include <old_costmap_2d/costmap_2d.h>
+#include <old_costmap_2d/basic_observation_buffer.h>
//Ransac ground filter used to see small obstacles
#include <pr2_msgs/PlaneStamped.h>
@@ -103,7 +103,7 @@
* @brief Accessor for the cost map. Use mainly for initialization
* of specialized map strunture for planning
*/
- const costmap_2d::CostMapAccessor& getCostMap() const {return *global_map_accessor_;}
+ const old_costmap_2d::CostMapAccessor& getCostMap() const {return *global_map_accessor_;}
/**
* @brief A handler to be over-ridden in the derived class to handle a diff stream from the
@@ -242,18 +242,18 @@
// are looked up. If we wanted to generalize this node further, we could use a factory pattern to dynamically
// load specific derived classes. For that we would need a hand-shaking pattern to register subscribers for them
// with this node
- costmap_2d::BasicObservationBuffer* baseScanBuffer_;
- costmap_2d::BasicObservationBuffer* baseCloudBuffer_;
- costmap_2d::BasicObservationBuffer* tiltScanBuffer_;
- costmap_2d::BasicObservationBuffer* lowObstacleBuffer_;
- costmap_2d::BasicObservationBuffer* stereoCloudBuffer_;
+ old_costmap_2d::BasicObservationBuffer* baseScanBuffer_;
+ old_costmap_2d::BasicObservationBuffer* baseCloudBuffer_;
+ old_costmap_2d::BasicObservationBuffer* tiltScanBuffer_;
+ old_costmap_2d::BasicObservationBuffer* lowObstacleBuffer_;
+ old_costmap_2d::BasicObservationBuffer* stereoCloudBuffer_;
/** Should encapsulate as a controller wrapper that is not resident in the trajectory rollout package */
trajectory_rollout::TrajectoryControllerROS* controller_;
- costmap_2d::CostMap2D* costMap_; /**< The cost map mainatined incrementally from laser scans */
- costmap_2d::CostMapAccessor* global_map_accessor_; /**< Read-only access to global cost map */
- costmap_2d::CostMapAccessor* local_map_accessor_; /**< Read-only access to a window on the cost map */
+ old_costmap_2d::CostMap2D* costMap_; /**< The cost map mainatined incrementally from laser scans */
+ old_costmap_2d::CostMapAccessor* global_map_accessor_; /**< Read-only access to global cost map */
+ old_costmap_2d::CostMapAccessor* local_map_accessor_; /**< Read-only access to a window on the cost map */
robot_srvs::StaticMap::Response costmap_response_;
Modified: pkg/trunk/deprecated/highlevel_controllers/manifest.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/manifest.xml 2009-04-16 02:48:36 UTC (rev 13931)
@@ -23,7 +23,7 @@
<depend package="pr2_msgs"/>
<depend package="trajectory_rollout"/>
<depend package="laser_scan" />
- <depend package="costmap_2d"/>
+ <depend package="old_costmap_2d"/>
<depend package="tf"/>
<depend package="map_server"/>
<depend package="sbpl"/>
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -122,7 +122,7 @@
#include <boost/thread.hpp>
#include <robot_filter/RobotFilter.h>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
namespace ros {
namespace highlevel_controllers {
@@ -232,25 +232,25 @@
}
// Then allocate observation buffers
- baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
+ baseScanBuffer_ = new old_costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
ros::Duration().fromSec(base_laser_keepalive),
- costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
+ old_costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- baseCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
+ baseCloudBuffer_ = new old_costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
ros::Duration().fromSec(base_laser_keepalive),
- costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
+ old_costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), global_frame_, tf_,
+ tiltScanBuffer_ = new old_costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), global_frame_, tf_,
ros::Duration().fromSec(tilt_laser_keepalive),
- costmap_2d::BasicObservationBuffer::computeRefreshInterval(tilt_laser_update_rate),
+ old_costmap_2d::BasicObservationBuffer::computeRefreshInterval(tilt_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), global_frame_, tf_,
+ lowObstacleBuffer_ = new old_costmap_2d::BasicObservationBuffer(std::string("odom_combined"), global_frame_, tf_,
ros::Duration().fromSec(low_obstacle_keepalive),
- costmap_2d::BasicObservationBuffer::computeRefreshInterval(low_obstacle_update_rate),
+ old_costmap_2d::BasicObservationBuffer::computeRefreshInterval(low_obstacle_update_rate),
inscribedRadius, -10.0, maxZ_, filter_);
- stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), global_frame_, tf_,
+ stereoCloudBuffer_ = new old_costmap_2d::BasicObservationBuffer(std::string("stereo_link"), global_frame_, tf_,
ros::Duration().fromSec(stereo_keepalive),
- costmap_2d::BasicObservationBuffer::computeRefreshInterval(stereo_update_rate),
+ old_costmap_2d::BasicObservationBuffer::computeRefreshInterval(stereo_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
@@ -809,7 +809,7 @@
ROS_DEBUG("getting observations.\n");
lock();
// Aggregate buffered observations across all sources. Must be thread safe
- std::vector<costmap_2d::Observation> observations;
+ std::vector<old_costmap_2d::Observation> observations;
baseCloudBuffer_->get_observations(observations);
//if we are not getting filtered base scans... we'll add the base scans as obstacle points
@@ -821,7 +821,7 @@
stereoCloudBuffer_->get_observations(observations);
std::vector<robot_msgs::PointCloud> points_storage(observations.size()); //needed to deep copy observations
- std::vector<costmap_2d::Observation> stored_observations(observations.size());
+ std::vector<old_costmap_2d::Observation> stored_observations(observations.size());
//we need to perform a deep copy on the observations to be thread safe
for(unsigned int i = 0; i < observations.size(); ++i){
@@ -1028,7 +1028,7 @@
lock();
// Aggregate buffered observations across all sources. Must be thread safe
- std::vector<costmap_2d::Observation> observations, raytrace_obs;
+ std::vector<old_costmap_2d::Observation> observations, raytrace_obs;
baseCloudBuffer_->get_observations(observations);
//get the observations from the base scan to use for raytracing
@@ -1044,7 +1044,7 @@
stereoCloudBuffer_->get_observations(observations);
std::vector<robot_msgs::PointCloud> points_storage(observations.size()); //needed to deep copy observations
- std::vector<costmap_2d::Observation> stored_observations(observations.size());
+ std::vector<old_costmap_2d::Observation> stored_observations(observations.size());
//we need to perform a deep copy on the observations to be thread safe
for(unsigned int i = 0; i < observations.size(); ++i){
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_door.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -71,7 +71,7 @@
#include <angles/angles.h>
#include <robot_msgs/Polyline2D.h>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
using namespace deprecated_msgs;
namespace ros
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_follow.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_follow.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_follow.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -37,7 +37,7 @@
#include <highlevel_controllers/move_base.hh>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
namespace ros {
namespace highlevel_controllers {
@@ -130,7 +130,7 @@
lock();
// Aggregate buffered observations across all sources. Must be thread safe
- std::vector<costmap_2d::Observation> observations;
+ std::vector<old_costmap_2d::Observation> observations;
baseScanBuffer_->get_observations(observations);
tiltScanBuffer_->get_observations(observations);
lowObstacleBuffer_->get_observations(observations);
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_navfn.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -66,7 +66,7 @@
#include <highlevel_controllers/move_base.hh>
#include <navfn/navfn.h>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
namespace ros {
namespace highlevel_controllers {
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn_constrained.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_navfn_constrained.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_navfn_constrained.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -68,7 +68,7 @@
#include <robot_actions/Pose2D.h>
#include <navfn/navfn.h>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
using robot_msgs::Polyline2D;
using deprecated_msgs::Point2DFloat32;
using std::vector;
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_sbpl.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_sbpl.cpp 2009-04-16 02:48:36 UTC (rev 13931)
@@ -72,7 +72,7 @@
#include <sbpl/headers.h>
#include <err.h>
-using namespace costmap_2d;
+using namespace old_costmap_2d;
namespace {
Modified: pkg/trunk/deprecated/old_costmap_2d/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/costmap_2d/CMakeLists.txt 2009-04-16 01:55:13 UTC (rev 13928)
+++ pkg/trunk/deprecated/old_costmap_2d/CMakeLists.txt 2009-04-16 02:48:36 UTC (rev 13931)
@@ -1,27 +1,27 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
set(ROS_BUILD_TYPE Release)
-rospack(costmap_2d)
+rospack(old_costmap_2d)
genmsg()
rospack_add_boost_directories()
-rospack_add_library(costmap_2d src/costmap_2d.cpp
+rospack_add_library(old_costmap_2d src/costmap_2d.cpp
src/obstacle_map_accessor.cpp
src/observation_buffer.cpp
src/basic_observation_buffer.cpp
src/costmap_node.cpp)
-rospack_link_boost(costmap_2d thread)
+rospack_link_boost(old_costmap_2d thread)
# Test target for module tests to be included in gtest regression test harness
rospack_add_gtest(utest src/test/module-tests.cpp)
-target_link_libraries(utest costmap_2d)
+target_link_libraries(utest old_costmap_2d)
# Target for benchmarking the costmap
rospack_add_executable(benchmark src/test/benchmark.cpp )
-target_link_libraries(benchmark costmap_2d)
+target_link_libraries(benchmark old_costmap_2d)
# Target for running a standalone version of costmap
rospack_add_executable(costmap_standalone src/costmap_standalone.cpp )
-target_link_libraries(costmap_standalone costmap_2d)
+target_link_libraries(costmap_standalone old_costmap_2d)
Added: pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/basic_observation_buffer.h
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/basic_observation_buffer.h (rev 0)
+++ pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/basic_observation_buffer.h 2009-04-16 02:48:36 UTC (rev 13931)
@@ -0,0 +1,75 @@
+/*
+ * 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:
+ *
+ * * 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 Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * 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.
+ *
+ * Authors: Conor McGann
+ */
+
+#ifndef COSTMAP_2D_BASIC_OBSERVATION_BUFFER_H
+#define COSTMAP_2D_BASIC_OBSERVATION_BUFFER_H
+
+#include <old_costmap_2d/observation_buffer.h>
+#include <boost/thread/mutex.hpp>
+
+namespace robot_filter {
+ class RobotFilter;
+}
+
+namespace old_costmap_2d {
+
+ /**
+ * @brief Extend base class to handle buffering until a transform is available, and to support locking for mult-threaded
+ * access
+ */
+ class BasicObservationBuffer : public ObservationBuffer {
+ public:
+ BasicObservationBuffer(const std::string& frame_id, const std::string& global_frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,double robotRadius, double minZ, double maxZ, robot_filter::RobotFilter* filter = NULL);
+
+ virtual void buffer_cloud(const robot_msgs::PointCloud& local_cloud);
+
+ virtual void get_observations(std::vector<Observation>& observations);
+
+ private:
+
+ /**
+ * @brief Test if point in the square footprint of the robot
+ */
+ bool inFootprint(double x, double y) const;
+
+ /**
+ * @brief Provide a filtered set of points based on the extraction of the robot footprint and the
+ * filter based on the min and max z values
+ */
+ robot_msgs::PointCloud * extractFootprintAndGround(const robot_msgs::PointCloud& baseFrameCloud) const;
+ tf::TransformListener& tf_;
+ std::deque<robot_msgs::PointCloud> point_clouds_; /**< Buffer point clouds until a transform is available */
+ boost::mutex buffer_mutex_;
+ const double robotRadius_, minZ_, maxZ_; /**< Constraints for filtering points */
+ robot_filter::RobotFilter* filter_;
+ };
+}
+#endif
Added: pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap2d_pqueue_benchmark.h
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap2d_pqueue_benchmark.h (rev 0)
+++ pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap2d_pqueue_benchmark.h 2009-04-16 02:48:36 UTC (rev 13931)
@@ -0,0 +1,338 @@
+/*
+ * 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:
+ *
+ * * 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 Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * 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.
+ *
+ * Authors: E. Gil Jones, Conor McGann
+ */
+
+#ifndef COSTMAP_2D_COSTMAP_2D_H
+#define COSTMAP_2D_COSTMAP_2D_H
+
+/**
+
+ @mainpage
+
+ @htmlinclude manifest.html
+
+ This library provides functionality for maintaining a 2D cost map. The primary inputs are:
+ - a priori 2-D occupancy grid map
+ - 3D obstacle data
+ - control parameters
+
+ This cost map is initialized with the a priori map, and thereafter updated
+ with obstacle data. A sliding window model of persistence is used, in which
+ obstacle data lives in the map for a fixed (but configurable) amount of time
+ before being discarded. The static map lives forever. Dynamic obstacle data is filtered
+ to account for the robot body, and based on a z-value threshold to permit projection of 3D
+ data to the 2D plane. Control parameters dictate the sliding window length, z threshold and
+ inflation radius.
+
+ For examples of usage see the test harness.
+*/
+
+// Base class
+#include "obstacle_map_accessor.h"
+
+//c++
+#include <list>
+#include <vector>
+#include <map>
+#include <set>
+#include <string>
+#include <map>
+#include <queue>
+
+// For point clouds <Could Make it a template>
+#include "robot_msgs/PointCloud.h"
+
+namespace old_costmap_2d {
+
+ typedef unsigned char TICK;
+
+ class CostMap2D;
+
+ class QueueElement {
+ public:
+ QueueElement(double d, unsigned int s, unsigned int i): distance(d), source(s), ind(i) {}
+
+ QueueElement(const QueueElement& org): distance(org.distance), source(org.source), ind(org.ind){}
+
+ double distance;
+ unsigned int source;
+ unsigned int ind;
+ };
+
+ class QueueElementComparator {
+ public:
+ /**
+ * Weak ordering property respected
+ */
+ inline bool operator()(const QueueElement* lhs, const QueueElement* rhs){
+ return lhs->distance > rhs->distance;
+ }
+ };
+
+ typedef std::queue< QueueElement*, std::deque<QueueElement*> > QUEUE;
+ typedef std::priority_queue< QueueElement*, std::vector<QueueElement*>, QueueElementComparator > PQUEUE;
+
+ class CostMap2D: public ObstacleMapAccessor
+ {
+ public:
+
+ /**
+ * @brief Constructor.
+ *
+ * @param width width of map [cells]
+ * @param height height of map [cells]
+ * @param data row-major obstacle data, each value indicates a cost
+ * @param resolution resolution of map [m/cell]
+ * @param window_length how long to hold onto obstacle data [sec]
+ * @param threshold The cost threshold where a cell is considered an obstacle
+ * @param maxZ gives the cut-off for points in 3D space
+ * @param freeSpaceProjectionHeight gives the upper bound for evaluating points in z for projecting free space
+ * @param inflationRadius the radius used to bound inflation - limit of cost propagation
+ * @param circumscribedRadius the radius used to indicate objects in the circumscribed circle around the robot
+ * @param inscribedRadius the radius used to indicate objects in the inscribed circle around the robot
+ */
+ CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
+ double resolution, double window_length,
+ unsigned char threshold,
+ double maxZ = 0, double freeSpaceProjectionHeight = 0,
+ double inflationRadius = 0, double circumscribedRadius = 0, double inscribedRadius = 0);
+
+ /**
+ * @brief Destructor.
+ */
+ virtual ~CostMap2D();
+
+ /**
+ * @brief Updates the cost map accounting for the new value of time and a new set of obstacles.
+ * @param current time stamp
+ * @param cloud holds projected scan data
+ * @param updates holds the updated cell ids and values
+ *
+ * @see removeStaleObstacles
+ */
+ void updateDynamicObstacles(double ts,
+ const robot_msgs::PointCloud& cloud,
+ std::vector<unsigned int>& updates);
+
+ /**
+ * @brief Updates the cost map accounting for the new value of time and a new set of obstacles.
+ * @param current time stamp
+ * @param wx The current x position
+ * @param wy The current y position
+ * @param cloud holds projected scan data
+ * @param updates holds the updated cell ids and values
+ *
+ * @see removeStaleObstacles
+ */
+ void updateDynamicObstacles(double ts,
+ double wx, double wy,
+ const robot_msgs::PointCloud& cloud,
+ std::vector<unsigned int>& updates);
+ /**
+ * @brief A convenience method which will skip calculating the diffs
+ * @param current time stamp
+ * @param cloud holds projected scan data
+ *
+ * @see updateDynamicObstacles
+ */
+ void updateDynamicObstacles(double ts, const robot_msgs::PointCloud& cloud);
+
+ /**
+ * @brief Updates the cost map, removing stale obstacles based on the new time stamp.
+ * @param current time stamp
+ * @param deletedObstacleCells holds vector for returning newly unoccupied ids
+ */
+ void removeStaleObstacles(double ts, std::vector<unsigned int>& updates);
+
+ /**
+ * @brief Get pointer into the obstacle map (which contains both static
+ * and dynamic obstacles)
+ */
+ const unsigned char* getMap() const;
+
+ /**
+ * @brief Obtain the collection of all occupied cells: the union of static and dynamic obstacles
+ */
+ void getOccupiedCellDataIndexList(std::vector<unsigned int>& results) const;
+
+ /**
+ * @brief Accessor for contents of full map cell by cell index
+ */
+ unsigned char operator [](unsigned int ind) const;
+
+ /**
+ * @brief Accessor by map coordinates
+ */
+ unsigned char getCost(unsigned int mx, unsigned int my) const;
+
+ /**
+ * @brief Utility for debugging
+ */
+ std::string toString() const;
+
+ private:
+ /**
+ * @brief Internal method that does not clear the updates first
+ */
+ void removeStaleObstaclesInternal(double ts, std::vector<unsigned int>& updates);
+
+ /**
+ * @brief Compute the number of ticks that have elapsed between the given timestamp ts and the last time stamp.
+ * Will update the last time stamp value
+ *
+ * @param ts The current time stamp. Must be >= lastTimeStamp_
+ */
+ TICK getElapsedTime(double ts);
+
+ /**
+ * @brief Helper method to compute the inflated cells around an obstacle based on resolution and inflation radius
+ */
+ void computeInflation(unsigned int ind, std::vector< std::pair<unsigned int, unsigned char> >& inflation) const;
+
+ /**
+ * @brief Utility to encapsulate dynamic cell updates
+ * @return true if the cell value is updated, otherwise false
+ */
+ void updateCellCost(unsigned int cell, unsigned char cellState, std::vector<unsigned int>& updates);
+
+ /**
+ * @brief Utility to encapsulate marking cells free.
+ */
+ void markFreeSpace(unsigned int cell, std::vector<unsigned int>& updates);
+
+ /**
+ * @brief Utility to propagate costs
+ * @param A priority queue to seed propagation
+ * @param A collection to retrieve all updated cells
+ */
+ void propagateCosts(std::vector<unsigned int>& updates);
+
+ /**
+ * @brief Utility to push free space inferred from a laser point hit via ray-tracing
+ * @param ind the cell index of the obstacle detected
+ * @param updates the buffer for updated cells as a result
+ */
+ void updateFreeSpace(unsigned int ind, std::vector<unsigned int>& updates);
+
+ /**
+ * @brief Simple test for a cell having been marked during current propaagtion
+ */
+ bool marked(unsigned int ind) const;
+
+ /**
+ * @brief Encapsulate calls to peth the watchdog, which impacts the set of dynamic obstacles
+ */
+ void petWatchDog(unsigned int cell, unsigned char value);
+
+ /**
+ * Added to provide a single update location. Only need to add the update if we have not visitied the cell
+ * with an update already. This case is checked because of the watchdog value. This prevents multiply updating cells
+ * for free space and cost propagation.
+ */
+ inline void addUpdate(unsigned int cell, std::vector<unsigned int>& updates){
+ updates.push_back(cell);
+ }
+
+ /**
+ * Utilities for cost propagation
+ */
+ char enqueueNeighbors(unsigned int source, unsigned int ind);
+
+ char enqueue(unsigned int source, unsigned int mx, unsigned int my);
+
+ double computeDistance(unsigned int a, unsigned int b) const;
+
+ unsigned char computeCost(double distance) const;
+
+ static const TICK MARKED_FOR_COST = 255; /**< The value used to denote a cell has been marked in the current iteration of cost propagation */
+ static const TICK WATCHDOG_LIMIT = 254; /**< The value for a reset watchdog time for observing dynamic obstacles */
+ const double tickLength_; /**< The duration in seconds of a tick, used to manage the watchdog timeout on obstacles. Computed from window length */
+ const double maxZ_; /**< Points above this will be excluded from consideration */
+ const double freeSpaceProjectionHeight_; /**< Filters points for free space projection */
+ const unsigned int inflationRadius_; /**< The radius in meters to propagate cost and obstacle information */
+ const unsigned int circumscribedRadius_;
+ const unsigned int inscribedRadius_;
+ unsigned char* staticData_; /**< data loaded from the static map */
+ unsigned char* fullData_; /**< the full map data that has both static and obstacle data */
+ TICK* obsWatchDog_; /**< Records time remaining in ticks before expiration of the observation */
+ double* storedDist_; /**< Stored distances of cells that have already been seen during propagation */
+ double lastTimeStamp_; /** < The last recorded time value for when obstacles were added */
+ unsigned int mx_; /** < The x position of the robot in the grid */
+ unsigned int my_; /** < The y position of the robot in the grid */
+
+
+ std::list<unsigned int> dynamicObstacles_; /**< Dynamic Obstacle Collection */
+ std::vector<unsigned int> staticObstacles_; /**< Vector of statically occupied cells */
+
+ QUEUE queue_, otherqueue_; /**< Used for cost propagation */
+ PQUEUE pqueue_; /**< Used for cost propagation */
+
+ double** cachedDistances; /**< Cached distances indexed by dx, dy */
+ const bool usePriorityQueue;
+ const double queueThreshold;
+
+ };
+
+ /**
+ * Wrapper class that provides a local window onto a global cost map
+ */
+ class CostMapAccessor: public ObstacleMapAccessor {
+ public:
+ /**
+ * @brief Constructor
+ * @param costMap The underlying global cost map
+ * @param maxSize The maximum width and height of the window in meters
+ * @param the current x position in global coords
+ * @param the current y position in global coords
+ */
+ CostMapAccessor(const CostMap2D& costMap, double maxSize, double pose_x, double pose_y);
+
+ /** Implementation for base class interface **/
+ virtual unsigned char operator[](unsigned int ind) const;
+ virtual unsigned char getCost(unsigned int mx, unsigned int my) const;
+
+ /**
+ * @brief Set the pose for the robot. Will adjust other parameters accordingly.
+ */
+ void updateForRobotPosition(double wx, double wy);
+
+ private:
+
+ static double computeWX(const CostMap2D& costMap, double maxSize, double wx, double wy);
+ static double computeWY(const CostMap2D& costMap, double maxSize, double wx, double wy);
+ static unsigned int computeSize(double maxSize, double resolution);
+
+ const CostMap2D& costMap_;
+ const double maxSize_;
+ unsigned int mx_0_;
+ unsigned int my_0_;
+ };
+}
+#endif
Added: pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap_2d.h (rev 0)
+++ pkg/trunk/deprecated/old_costmap_2d/include/old_costmap_2d/costmap_2d.h 2009-04-16 02:48:36 UTC (rev 13931)
@@ -0,0 +1,333 @@
+/*
+ * 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:
+ *
+ * * 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 Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * 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.
+ *
+ * Authors: E. Gil Jones, Conor McGann
+ */
+
+#ifndef COSTMAP_2D_COSTMAP_2D_H
+#define COSTMAP_2D_COSTMAP_2D_H
+
+/**
+
+ @mainpage
+
+ @htmlinclude manifest.html
+
+ This library provides functionality for maintaining a 2D cost map. The primary inputs are:
+ - a priori 2-D occupancy grid map
+ - 3D obstacle data
+ - control parameters
+
+ This cost map is initialized with the a priori map, and thereafter updated
+ with obstacle data. A sliding window model of persistence is used, in which
+ obstacle data lives in the map for a fixed (but configurable) amount of time
+ before being discarded. The static map lives forever. Dynamic obstacle data is filtered
+ to account for the robot body, and based on a z-value threshold to permit projection of 3D
+ data to the 2D plane. Control parameters dictate the sliding window length, z threshold and
+ inflation radius.
+
+ For examples of usage see the test harness.
+ */
+
+// Base class
+#include "obstacle_map_accessor.h"
+
+// Need observations
+#include "observation.h"
+
+//c++
+#include <vector>
+#include <string>
+#include <queue>
+
+namespace std_msgs {
+ class Point2DFloat32;
+};
+
+namespace old_costmap_2d {
+
+ typedef unsigned char TICK;
+
+ class CostMap2D;
+
+ class QueueElement {
+ public:
+ QueueElement(double d, unsigned int s, unsigned int i): distance(d), source(s), ind(i) {}
+
+ QueueElement(const QueueElement& org): distance(org.distance), source(org.source), ind(org.ind){}
+
+ double distance;
+ unsigned int source;
+ unsigned int ind;
+ };
+
+ class QueueElementComparator {
+ public:
+ /**
+ * Weak ordering property respected
+ */
+ inline bool operator()(const QueueElement* lhs, const QueueElement* rhs){
+ return lhs->distance > rhs->distance;
+ }
+ };
+
+ typedef std::priority_queue< QueueElement*, std::vector<QueueElement*>, QueueElementComparator > QUEUE;
+
+
+ class CostMap2D: public ObstacleMapAccessor
+ {
+ public:
+
+ static std::vector<robot_msgs::PointCloud*> toVector(robot_msgs::PointCloud& cloud){
+ std::vector<robot_msgs::PointCloud*> v;
+ v.push_back(&cloud);
+ return v;
+ }
+
+ /**
+ * @brief Constructor.
+ *
+ * @param width width of map [cells]
+ * @param height height of map [cells]
+ * @param data row-major obstacle data, each value indicates a cost
+ * @param resolution resolution of map [m/cell]
+ * @param threshold The cost threshold where a cell is considered an obstacle
+ * @param maxZ gives the cut-off for points in 3D space
+ * @param zLB lower bound for evaluating points in z for projecting free space
+ * @param zUB upper bound for evaluating points in z for projecting free space
+ * @param inflationRadius the radius used to bound inflation - limit of cost propagation
+ * @param circumscribedRadius the radius used to indicate objects in the circumscribed circle around the robot
+ * @param inscribedRadius the radius used to indicate objects in the inscribed circle around the robot
+ * @param weight the scaling factor in the cost function. Should be <=1. Lower values reduce the effective cost
+ * @param raytraceWindow the size of the window in which raytracing is done.
+ */
+ CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
+ double resolution, unsigned char threshold,
+ double maxZ = 0.5, double zLB = 0.15, double zUB = 0.20,
+ double inflationRadius = 0, double circumscribedRadius = 0, double inscribedRadius = 0, double weight = 1, double obstacleRange = 10.0, double raytraceRange = 10.0,
+ double raytraceWindow = 2.5);
+
+ /**
+ * @brief Destructor.
+ */
+ virtual ~CostMap2D();
+
+ // Hack for backward compatability
+ void updateDynamicObstacles(robot_msgs::PointCloud& cloud,
+ std::vector<unsigned int>& updates){
+ updateDynamicObstacles(0, 0, toVector(cloud), updates);
+ }
+
+ /**
+ * @brief Updates the cost map accounting for the new value of time and a new set of obstacles.
+ * @param wx The current x position
+ * @param wy The current y position
+ * @param cloud holds projected scan data
+ * @param updates holds the updated cell ids and values
+ */
+ void updateDynamicObstacles(double wx, double wy,
+ const std::vector<robot_msgs::PointCloud*>& clouds,
+ std::vector<unsigned int>& updates);
+ /**
+ * @brief Updates dynamic obstacles
+ * @param wx The current x position
+ * @param wy The current y position
+ * @param clouds holds projected scan data
+ */
+ void updateDynamicObstacles(double wx, double wy,
+ const std::vector<robot_msgs::PointCloud*>& clouds);
+
+ /**
+ * @brief Updates dynamic obstacles based on a buffer of observations
+ * @param wx The current x position
+ * @param wy The current y position
+ * @param observations The collection of observations from all data sources
+ */
+ void updateDynamicObstacles(double wx, double wy, const std::vector<Observation>& observations, const Observation* raytrace_obs = NULL);
+
+ /**
+ * @brief Obtain the collection of all occupied cells: the union of static and dynamic obstacles
+ */
+ void getOccupiedCellDataIndexList(std::vector<unsigned int>& results) const;
+
+ /**
+ * @brief The weight for scaling in the cost function
+ */
+ double getWeight() const {return weight_;}
+
+ /**
+ * @brief Will reset the cost data
+ * @param wx the x position in world coordinates
+ * @param wy the y position in world coordinates
+ */
+ void revertToStaticMap(double wx = 0.0, double wy = 0.0);
+
+
+ private:
+
+ /**
+ * @brief Helper method to compute the inflated cells around an obstacle based on resolution and inflation radius
+ */
+ void computeInflation(unsigned int ind, std::vector< std::pair<unsigned int, unsigned char> >& inflation) const;
+
+ /**
+ * @brief Utility to propagate costs
+ * @param A priority queue to seed propagation
+ * @param A collection to retrieve all updated cells
+ */
+ void propagateCosts();
+
+ /**
+ * @brief Utility to push free space inferred from a laser point hit via ray-tracing
+ * @param The origin from which to trace out
+ * @param The target in map co-ordinates to trace to
+ */
+ void updateFreeSpace(const robot_msgs::Point& origin, double wx, double wy);
+
+ /**
+ * @brief Simple test for a cell having been marked during current propaagtion
+ */
+ bool marked(unsigned int ind) const {return xy_markers_[ind];}
+
+ /**
+ * @mark a cell
+ */
+ void mark(unsigned int ind){xy_markers_[ind] = true;}
+
+ /**
+ * Utilities for cost propagation
+ */
+ void enqueueNeighbors(unsigned int source, unsigned int ind);
+
+ void enqueue(unsigned int source, unsigned int mx, unsigned int my);
+
+
+ /**
+ * Euclidean distance
+ */
+ inline double computeDistance(unsigned int a, unsigned int b) const{
+ unsigned int mx_a, my_a, mx_b, my_b;
+ IND_MC(a, mx_a, my_a);
+ IND_MC(b, mx_b, my_b);
+ unsigned int dx = abs((int)(mx_a) - (int) mx_b);
+ unsigned int dy = abs((int)(my_a) - (int) my_b);
+
+ ROS_ASSERT((dx <= inflationRadius_) && (dy <= inflationRadius_));
+ double distance = cachedDistances[dx][dy];
+
+ return distance;
+ }
+
+ inline unsigned char computeCost(double distance) const{
+ unsigned char cost = 0;
+ if(distance == 0)
+ cost = LETHAL_OBSTACLE;
+ else if(distance <= inscribedRadius_)
+ cost = INSCRIBED_INFLATED_OBSTACLE;
+ else {
+ // The cost for a non-lethal obstacle should be in the range of [0, inscribed_inflated_obstacle - 1].
+ double factor = weight_ / (1 + pow(distance - inscribedRadius_, 2));
+ cost = (unsigned char) ( (INSCRIBED_INFLATED_OBSTACLE -1) * factor);
+ }
+ return cost;
+ }
+
+ void updateCellCost(unsigned int ind, unsigned char cost);
+
+ bool in_projection_range(double z){return z >= zLB_ && z <= zUB_;}
+
+ const double maxZ_; /**< Points above this will be excluded from consideration */
+ const double zLB_; /**< Filters points for free space projection */
+ const double zUB_; /**< Filters points for free space projection */
+ const unsigned int inflationRadius_; /**< The radius in cells to propagate cost and obstacle information */
+ const unsigned int circumscribedRadius_; /**< The radius for the circumscribed radius, in cells */
+ const unsigned int inscribedRadius_; /**< The radius for the inscribed radius, in cells */
+ const double weight_; /**< The weighting to apply to a normalized cost value */
+ const double raytraceWindow_; /**< The window in which statics are raytraced */
+
+ //used squared distance because square root computations are expensive
+ double sq_obstacle_range_; /** The range out to which we will consider laser hitpoints **/
+ double sq_raytrace_range_; /** The range out to which we will raytrace **/
+
+ unsigned char* staticData_; /**< initial static map */
+ bool* xy_markers_; /**< Records time remaining in ticks before expiration of the observation */
+ QUEUE queue_; /**< Used for cost propagation */
+
+ double** cachedDistances; /**< Cached distances indexed by dx, dy */
+ const unsigned int kernelWidth_; /**< The width of the kernel matrix, which will be square */
+ unsigned char* kernelData_; /**< kernel data structure for cost map updates around the robot */
+ const unsigned int raytraceCells_; /**< Raytracing cells */
+ double robotX_, robotY_; /**< The position of the robot */
+ };
+
+ /**
+ * Wrapper class that provides a local window onto a global cost map
+ */
+ class CostMapAccessor: public ObstacleMapAccessor {
+ public:
+ /**
+ * @brief Constructor
+ * @param costMap The underlying global cost map
+ * @param maxSize The maximum width and height of the window in meters
+ * @param the current x position in global coords
+ * @param the current y position in global coords
+ */
+ CostMapAccessor(const CostMap2D& costMap, double maxSize, double pose_x, double pose_y);
+
+ /**
+ * @broef Constructor
+ * Use when we want to access the whole cost map rather than a window
+ */
+ CostMapAccessor(const CostMap2D& costMap);
+
+ /**
+ * @brief Set the pose for the robot. Will adjust other parameters accordingly.
+ */
+ void updateForRobotPosition(double wx, double wy);
+
+ /**
+ * @brief Refresh the copied cost data structure from the source cost map
+ */
+ void refresh();
+
+ /**
+ * @brief Return the dimensions of the underlying costmap
+ */
+ void getCostmapDimensions(unsigned int& width, unsigned int& height) const;
+
+ private:
+
+ stati...
[truncated message content] |
