Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14588] pkg/trunk
|
From: <ei...@us...> - 2009-04-28 22:52:34
|
Revision: 14588
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14588&view=rev
Author: eitanme
Date: 2009-04-28 22:52:15 +0000 (Tue, 28 Apr 2009)
Log Message:
-----------
There was an inconsistency with naming in the costmap_2d package. Costmap was used in some places... and CostMap in others. To make this worse... member variables also were either costmap_ or cost_map_. To agree with the package name... a costmap is a Costmap is a costmap_.
Modified Paths:
--------------
pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/highlevel/nav/include/nav/move_base.h
pkg/trunk/highlevel/nav/include/nav/move_base_local.h
pkg/trunk/highlevel/nav/src/move_base.cpp
pkg/trunk/highlevel/nav/src/move_base_local.cpp
pkg/trunk/motion_planning/navfn/include/navfn/navfn.h
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/src/navfn.cpp
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/motion_planning/navfn/src/navtest.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.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/src/costmap_model.cpp
pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/nav/base_local_planner/test/utest.cpp
pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_publisher.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_publisher.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -157,7 +157,7 @@
/**
* @brief Resets the costmaps to the static map outside a given window
*/
- void resetCostMaps();
+ void resetCostmaps();
ros::Node& ros_node_;
tf::TransformListener& tf_;
Modified: pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -79,7 +79,7 @@
//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, "");
- planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
+ planner_cost_map_ros_->getCostmapCopy(planner_cost_map_);
//initialize the door opening planner
planner_ = new DoorReactivePlanner(ros_node_, tf_,&planner_cost_map_,control_frame_,global_frame_);
@@ -144,7 +144,7 @@
return;
//make a plan for controller
- planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
+ planner_cost_map_ros_->getCostmapCopy(planner_cost_map_);
//make sure we clear the robot's footprint from the cost map
clearRobotFootprint(planner_cost_map_);
@@ -248,7 +248,7 @@
updateGlobalPose();
//make sure to update the cost_map we'll use for this cycle
- //controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ //controller_cost_map_ros_->getCostmapCopy(controller_cost_map_);
//make sure that we clear the robot footprint in the cost map
//clearRobotFootprint(controller_cost_map_);
@@ -349,7 +349,7 @@
return true;
}
- void MoveBaseDoorAction::resetCostMaps(){
+ void MoveBaseDoorAction::resetCostmaps(){
planner_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
}
Modified: pkg/trunk/highlevel/nav/include/nav/move_base.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/nav/include/nav/move_base.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -94,14 +94,14 @@
/**
* @brief Resets the costmaps to the static map outside a given window
*/
- void resetCostMaps();
+ void resetCostmaps();
ros::Node& ros_node_;
tf::TransformListener& tf_;
bool run_planner_;
base_local_planner::TrajectoryPlannerROS* tc_;
- costmap_2d::Costmap2DROS* planner_cost_map_ros_, *controller_cost_map_ros_;
- costmap_2d::Costmap2D planner_cost_map_, controller_cost_map_;
+ costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
+ costmap_2d::Costmap2D planner_costmap_, controller_costmap_;
navfn::NavfnROS* planner_;
std::vector<robot_msgs::PoseStamped> global_plan_;
Modified: pkg/trunk/highlevel/nav/include/nav/move_base_local.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_local.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_local.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -95,13 +95,13 @@
/**
* @brief Resets the costmaps to the static map outside a given window
*/
- void resetCostMaps();
+ void resetCostmaps();
ros::Node& ros_node_;
tf::TransformListener& tf_;
base_local_planner::TrajectoryPlannerROS* tc_;
- costmap_2d::Costmap2DROS* controller_cost_map_ros_;
- costmap_2d::Costmap2D controller_cost_map_;
+ costmap_2d::Costmap2DROS* controller_costmap_ros_;
+ costmap_2d::Costmap2D controller_costmap_;
std::vector<robot_msgs::Point> footprint_;
std::string robot_base_frame_;
Modified: pkg/trunk/highlevel/nav/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/nav/src/move_base.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -44,7 +44,7 @@
namespace nav {
MoveBase::MoveBase(ros::Node& ros_node, tf::TransformListener& tf) :
Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
- run_planner_(true), tc_(NULL), planner_cost_map_ros_(NULL), controller_cost_map_ros_(NULL),
+ run_planner_(true), tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
planner_(NULL), valid_plan_(false), new_plan_(false) {
//get some parameters that will be global to the move base node
@@ -60,16 +60,16 @@
ros_node_.param("~navfn/costmap/circumscribed_radius", circumscribed_radius, 0.46);
//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
- planner_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("navfn"));
- planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
+ planner_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("navfn"));
+ planner_costmap_ros_->getCostmapCopy(planner_costmap_);
//initialize the NavFn planner
- planner_ = new NavfnROS(ros_node_, tf_, planner_cost_map_);
- ROS_INFO("MAP SIZE: %d, %d", planner_cost_map_.cellSizeX(), planner_cost_map_.cellSizeY());
+ planner_ = new NavfnROS(ros_node_, tf_, planner_costmap_);
+ ROS_INFO("MAP SIZE: %d, %d", planner_costmap_.cellSizeX(), planner_costmap_.cellSizeY());
- //create the ros wrapper for the controller's cost_map... and initializer a pointer we'll use with the underlying map
- controller_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"));
- controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
+ controller_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"));
+ controller_costmap_ros_->getCostmapCopy(controller_costmap_);
robot_msgs::Point pt;
//create a square footprint
@@ -92,7 +92,7 @@
footprint_.push_back(pt);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_cost_map_, footprint_, &planner_cost_map_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_, &planner_costmap_);
//TODO:spawn planning thread here?
}
@@ -104,24 +104,24 @@
if(tc_ != NULL)
delete tc_;
- if(planner_cost_map_ros_ != NULL)
- delete planner_cost_map_ros_;
+ if(planner_costmap_ros_ != NULL)
+ delete planner_costmap_ros_;
- if(controller_cost_map_ros_ != NULL)
- delete controller_cost_map_ros_;
+ if(controller_costmap_ros_ != NULL)
+ delete controller_costmap_ros_;
}
void MoveBase::makePlan(const robot_msgs::PoseStamped& goal){
//since this gets called on handle activate
- if(planner_cost_map_ros_ == NULL)
+ if(planner_costmap_ros_ == NULL)
return;
//update the copy of the costmap the planner uses
- planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
+ planner_costmap_ros_->getCostmapCopy(planner_costmap_);
//since we have a controller that knows the full footprint of the robot... we may as well clear it
- tc_->clearRobotFootprint(planner_cost_map_);
+ tc_->clearRobotFootprint(planner_costmap_);
std::vector<robot_msgs::PoseStamped> global_plan;
bool valid_plan = planner_->makePlan(goal, global_plan);
@@ -181,11 +181,11 @@
//push the feedback out
update(feedback);
- //make sure to update the cost_map we'll use for this cycle
- controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ //make sure to update the costmap we'll use for this cycle
+ controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//check that the observation buffers for the costmap are current
- if(!controller_cost_map_ros_->isCurrent()){
+ if(!controller_costmap_ros_->isCurrent()){
ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
continue;
}
@@ -203,7 +203,7 @@
}
//get observations for the non-costmap controllers
std::vector<Observation> observations;
- controller_cost_map_ros_->getMarkingObservations(observations);
+ controller_costmap_ros_->getMarkingObservations(observations);
valid_control = tc_->computeVelocityCommands(cmd_vel, observations);
}
else{
@@ -229,7 +229,7 @@
//if planning fails here... try to revert to the static map outside a given area
if(!valid_plan_){
- resetCostMaps();
+ resetCostmaps();
}
}
@@ -247,9 +247,9 @@
return robot_actions::PREEMPTED;
}
- void MoveBase::resetCostMaps(){
- planner_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
- controller_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
+ void MoveBase::resetCostmaps(){
+ planner_costmap_ros_->resetMapOutsideWindow(5.0, 5.0);
+ controller_costmap_ros_->resetMapOutsideWindow(5.0, 5.0);
}
};
Modified: pkg/trunk/highlevel/nav/src/move_base_local.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_local.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/nav/src/move_base_local.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -44,7 +44,7 @@
namespace nav {
MoveBaseLocal::MoveBaseLocal(ros::Node& ros_node, tf::TransformListener& tf) :
Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
- tc_(NULL), controller_cost_map_ros_(NULL) {
+ tc_(NULL), controller_costmap_ros_(NULL) {
//get some parameters that will be global to the move base node
ros_node_.param("~base_local_planner/robot_base_frame", robot_base_frame_, std::string("base_link"));
@@ -58,9 +58,9 @@
ros_node_.param("~base_local_planner/costmap/inscribed_radius", inscribed_radius, 0.325);
ros_node_.param("~base_local_planner/costmap/circumscribed_radius", circumscribed_radius, 0.46);
- //create the ros wrapper for the controller's cost_map... and initializer a pointer we'll use with the underlying map
- controller_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"));
- controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
+ controller_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"));
+ controller_costmap_ros_->getCostmapCopy(controller_costmap_);
robot_msgs::Point pt;
//create a square footprint
@@ -83,15 +83,15 @@
footprint_.push_back(pt);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_cost_map_, footprint_, &controller_cost_map_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_, &controller_costmap_);
}
MoveBaseLocal::~MoveBaseLocal(){
if(tc_ != NULL)
delete tc_;
- if(controller_cost_map_ros_ != NULL)
- delete controller_cost_map_ros_;
+ if(controller_costmap_ros_ != NULL)
+ delete controller_costmap_ros_;
}
void MoveBaseLocal::getRobotPose(std::string frame, tf::Stamped<tf::Pose>& pose){
@@ -134,11 +134,11 @@
//push the feedback out
update(feedback);
- //make sure to update the cost_map we'll use for this cycle
- controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ //make sure to update the costmap we'll use for this cycle
+ controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//check that the observation buffers for the costmap are current
- if(!controller_cost_map_ros_->isCurrent()){
+ if(!controller_costmap_ros_->isCurrent()){
ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
continue;
}
@@ -155,7 +155,7 @@
tc_->updatePlan(global_plan);
//get observations for the non-costmap controllers
std::vector<Observation> observations;
- controller_cost_map_ros_->getMarkingObservations(observations);
+ controller_costmap_ros_->getMarkingObservations(observations);
valid_control = tc_->computeVelocityCommands(cmd_vel, observations);
//give the base the velocity command
@@ -202,8 +202,8 @@
return true;
}
- void MoveBaseLocal::resetCostMaps(){
- controller_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
+ void MoveBaseLocal::resetCostmaps(){
+ controller_costmap_ros_->resetMapOutsideWindow(5.0, 5.0);
}
};
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -50,7 +50,7 @@
#define COST_OBS 254 // 255 and 254 for forbidden regions
#define COST_OBS_ROS 253 // ROS values of 253 are obstacles
#define COST_NEUTRAL 50 // Set this to "open space" value
-#define COST_FACTOR 3 // Used for translating costs in NavFn::setCostMap()
+#define COST_FACTOR 3 // Used for translating costs in NavFn::setCostmap()
// Define the cost type in the case that it is not set. However, this allows
// clients to modify it without changing the file. Arguably, it is better to require it to
@@ -104,7 +104,7 @@
void setNavArr(int nx, int ny); /**< sets or resets the size of the map */
int nx, ny, ns; /**< size of grid, in pixels */
- void setCostMap(const COSTTYPE *cmap, bool isROS=true); /**< sets up the cost map */
+ void setCostmap(const COSTTYPE *cmap, bool isROS=true); /**< sets up the cost map */
bool calcNavFnAstar(); /**< calculates a plan, returns true if found */
bool calcNavFnDijkstra(); /**< calculates the full navigation function */
float *getPathX(); /**< x-coordinates of path */
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -56,7 +56,7 @@
* @param tf A reference to a TransformListener
* @param cos_map A reference to the costmap to use
*/
- NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& cost_map);
+ NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& costmap);
/**
* @brief Given a goal pose in the world, compute a plan
@@ -91,7 +91,7 @@
void clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my);
ros::Node& ros_node_;
tf::TransformListener& tf_;
- costmap_2d::Costmap2D& cost_map_;
+ costmap_2d::Costmap2D& costmap_;
NavFn planner_;
std::string global_frame_, robot_base_frame_;
double transform_tolerance_; // timeout before transform errors
Modified: pkg/trunk/motion_planning/navfn/src/navfn.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/motion_planning/navfn/src/navfn.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -223,7 +223,7 @@
//
void
-NavFn::setCostMap(const COSTTYPE *cmap, bool isROS)
+NavFn::setCostmap(const COSTTYPE *cmap, bool isROS)
{
COSTTYPE *cm = costarr;
if (isROS) // ROS-type cost array
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -37,8 +37,8 @@
#include <navfn/navfn_ros.h>
namespace navfn {
- NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& cost_map) : ros_node_(ros_node), tf_(tf),
- cost_map_(cost_map), planner_(cost_map.cellSizeX(), cost_map.cellSizeY()) {
+ NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& costmap) : ros_node_(ros_node), tf_(tf),
+ costmap_(costmap), planner_(costmap.cellSizeX(), costmap.cellSizeY()) {
//advertise our plan visualization
ros_node_.advertise<robot_msgs::Polyline>("~navfn/plan", 1);
@@ -60,7 +60,7 @@
double NavfnROS::getPointPotential(const robot_msgs::Point& world_point){
unsigned int mx, my;
- if(!cost_map_.worldToMap(world_point.x, world_point.y, mx, my))
+ if(!costmap_.worldToMap(world_point.x, world_point.y, mx, my))
return DBL_MAX;
unsigned int index = my * planner_.nx + mx;
@@ -68,10 +68,10 @@
}
bool NavfnROS::computePotential(const robot_msgs::Point& world_point){
- planner_.setCostMap(cost_map_.getCharMap());
+ planner_.setCostmap(costmap_.getCharMap());
unsigned int mx, my;
- if(!cost_map_.worldToMap(world_point.x, world_point.y, mx, my))
+ if(!costmap_.worldToMap(world_point.x, world_point.y, mx, my))
return false;
int map_start[2];
@@ -93,12 +93,12 @@
global_pose.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
//set the associated costs in the cost map to be free
- cost_map_.setCost(mx, my, costmap_2d::FREE_SPACE);
+ costmap_.setCost(mx, my, costmap_2d::FREE_SPACE);
double max_inflation_dist = inflation_radius_ + inscribed_radius_;
//make sure to re-inflate obstacles in the affected region
- cost_map_.reinflateWindow(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist, max_inflation_dist);
+ costmap_.reinflateWindow(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist, max_inflation_dist);
}
@@ -143,13 +143,13 @@
double wy = global_pose.getOrigin().y();
unsigned int mx, my;
- if(!cost_map_.worldToMap(wx, wy, mx, my))
+ if(!costmap_.worldToMap(wx, wy, mx, my))
return false;
//clear the starting cell within the costmap because we know it can't be an obstacle
clearRobotCell(global_pose, mx, my);
- planner_.setCostMap(cost_map_.getCharMap());
+ planner_.setCostmap(costmap_.getCharMap());
int map_start[2];
map_start[0] = mx;
@@ -158,7 +158,7 @@
wx = goal_pose.getOrigin().x();
wy = goal_pose.getOrigin().y();
- if(!cost_map_.worldToMap(wx, wy, mx, my))
+ if(!costmap_.worldToMap(wx, wy, mx, my))
return false;
int map_goal[2];
@@ -182,7 +182,7 @@
//convert the plan to world coordinates
double world_x, world_y;
- cost_map_.mapToWorld(cell_x, cell_y, world_x, world_y);
+ costmap_.mapToWorld(cell_x, cell_y, world_x, world_y);
robot_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
Modified: pkg/trunk/motion_planning/navfn/src/navtest.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navtest.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/motion_planning/navfn/src/navtest.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -71,8 +71,8 @@
COSTTYPE *cmap = NULL;
// cmap = readPGM("maps/willow-full-0.05.pgm",&sx,&sy);
cmap = readPGM("maps/navfn_troubles.pgm",&sx,&sy,true);
- // cmap = readPGM("initial_cost_map_1165_945.pgm",&sx,&sy,true);
- // cmap = readPGM("initial_cost_map_2332_1825.pgm",&sx,&sy,true);
+ // cmap = readPGM("initial_costmap_1165_945.pgm",&sx,&sy,true);
+ // cmap = readPGM("initial_costmap_2332_1825.pgm",&sx,&sy,true);
if (cmap)
{
nav = new NavFn(sx,sy);
@@ -149,7 +149,7 @@
// set up cost map from file, if it exists
if (cmap)
{
- nav->setCostMap(cmap);
+ nav->setCostmap(cmap);
nav->setupNavFn(true);
}
else
@@ -171,7 +171,7 @@
// set up cost map from file, if it exists
if (cmap)
{
- nav->setCostMap(cmap);
+ nav->setCostmap(cmap);
nav->setupNavFn(true);
}
else
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -51,10 +51,10 @@
public:
/**
* @brief Constructor for the CostmapModel
- * @param cost_map The costmap that should be used
+ * @param costmap The costmap that should be used
* @return
*/
- CostmapModel(const costmap_2d::Costmap2D& cost_map);
+ CostmapModel(const costmap_2d::Costmap2D& costmap);
/**
* @brief Destructor for the world model
@@ -100,7 +100,7 @@
*/
double pointCost(int x, int y);
- const costmap_2d::Costmap2D& cost_map_; ///< @brief Allows access of costmap obstacle information
+ const costmap_2d::Costmap2D& costmap_; ///< @brief Allows access of costmap obstacle information
};
};
Modified: 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.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -73,7 +73,7 @@
/**
* @brief Constructs a trajectory controller
* @param world_model The WorldModel the trajectory controller uses to check for collisions
- * @param cost_map A reference to the CostMap the controller should use
+ * @param costmap A reference to the Costmap the controller should use
* @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
@@ -102,7 +102,7 @@
* @param y_vels A vector of the y velocities the controller will explore
*/
TrajectoryPlanner(WorldModel& world_model,
- const costmap_2d::Costmap2D& cost_map,
+ const costmap_2d::Costmap2D& costmap,
std::vector<robot_msgs::Point> footprint_spec,
double inscribed_radius, double circumscribed_radius,
double acc_lim_x = 1.0, double acc_lim_y = 1.0, double acc_lim_theta = 1.0,
@@ -249,7 +249,7 @@
void setPathCells();
MapGrid map_; ///< @brief The local map grid where we propagate goal and path distance
- const costmap_2d::Costmap2D& cost_map_; ///< @brief Provides access to cost map information
+ const costmap_2d::Costmap2D& costmap_; ///< @brief Provides access to cost map information
WorldModel& world_model_; ///< @brief The world model that the controller uses for collision detection
std::vector<robot_msgs::Point> footprint_spec_; ///< @brief The footprint specification of the robot
@@ -304,7 +304,7 @@
check_cell->path_mark = true;
//if the cell is an obstacle set the max path distance
- unsigned char cost = cost_map_.getCost(check_cell->cx, check_cell->cy);
+ unsigned char cost = costmap_.getCost(check_cell->cx, check_cell->cy);
if(!map_(check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)){
check_cell->path_dist = map_.map_.size();
return;
@@ -328,7 +328,7 @@
check_cell->goal_mark = true;
//if the cell is an obstacle set the max goal distance
- unsigned char cost = cost_map_.getCost(check_cell->cx, check_cell->cy);
+ unsigned char cost = costmap_.getCost(check_cell->cx, check_cell->cy);
if(!map_(check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)){
check_cell->goal_dist = map_.map_.size();
return;
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -77,12 +77,12 @@
* @brief Constructs the ros wrapper
* @param ros_node The node that is running the controller, used to get parameters from the parameter server
* @param tf A reference to a transform listener
- * @param cost_map The cost map to use for assigning costs to trajectories
+ * @param costmap The cost map to use for assigning costs to trajectories
* @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
* @param planner_map Used to size the map for the freespace controller... this will go away once a rolling window version of the point grid is in place
*/
TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- costmap_2d::Costmap2D& cost_map, std::vector<robot_msgs::Point> footprint_spec,
+ costmap_2d::Costmap2D& costmap, std::vector<robot_msgs::Point> footprint_spec,
const costmap_2d::Costmap2D* planner_map = NULL);
/**
@@ -130,9 +130,9 @@
/**
* @brief Clear the footprint of the robot in a given cost map
- * @param cost_map The costmap to apply the clearing opertaion on
+ * @param costmap The costmap to apply the clearing opertaion on
*/
- void clearRobotFootprint(costmap_2d::Costmap2D& cost_map);
+ void clearRobotFootprint(costmap_2d::Costmap2D& costmap);
private:
/**
@@ -191,9 +191,9 @@
/**
* @brief Clear the footprint of the robot in a given cost map
* @param global_pose The pose of the robot in the global frame
- * @param cost_map The costmap to apply the clearing opertaion on
+ * @param costmap The costmap to apply the clearing opertaion on
*/
- void clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, costmap_2d::Costmap2D& cost_map);
+ void clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, costmap_2d::Costmap2D& costmap);
/**
* @brief Publish a plan for visualization purposes
@@ -206,7 +206,7 @@
WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
- costmap_2d::Costmap2D& cost_map_; ///< @brief The costmap the controller will use
+ costmap_2d::Costmap2D& costmap_; ///< @brief The costmap the controller will use
tf::MessageNotifier<laser_scan::LaserScan>* base_scan_notifier_; ///< @brief Used to guarantee that a transform is available for base scans
tf::MessageNotifier<laser_scan::LaserScan>* tilt_scan_notifier_; ///< @brief Used to guarantee that a transform is available for tilt scans
tf::TransformListener& tf_; ///< @brief Used for transforming point clouds
Modified: pkg/trunk/nav/base_local_planner/src/costmap_model.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/costmap_model.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/base_local_planner/src/costmap_model.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -41,7 +41,7 @@
using namespace costmap_2d;
namespace base_local_planner {
- CostmapModel::CostmapModel(const Costmap2D& ma) : cost_map_(ma) {}
+ CostmapModel::CostmapModel(const Costmap2D& ma) : costmap_(ma) {}
double CostmapModel::footprintCost(const Point& position, const vector<Point>& footprint,
double inscribed_radius, double circumscribed_radius){
@@ -52,19 +52,19 @@
unsigned int cell_x, cell_y;
//get the cell coord of the center point of the robot
- if(!cost_map_.worldToMap(position.x, position.y, cell_x, cell_y))
+ if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
return -1.0;
//for circular robots the circumscribed radius will equal the inscribed radius and we can do a point check
if(circumscribed_radius == inscribed_radius){
- unsigned char cost = cost_map_.getCost(cell_x, cell_y);
+ unsigned char cost = costmap_.getCost(cell_x, cell_y);
if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
return -1.0;
return 1.0;
}
//for non-circular robots... we can still save time by checking if the circumscribed circle is clear of obstacles
- if(!cost_map_.circumscribedCell(cell_x, cell_y))
+ if(!costmap_.circumscribedCell(cell_x, cell_y))
return 1.0;
//now we really have to lay down the footprint in the costmap grid
@@ -74,11 +74,11 @@
//we need to rasterize each line in the footprint
for(unsigned int i = 0; i < footprint.size() - 1; ++i){
//get the cell coord of the first point
- if(!cost_map_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
+ if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
return -1.0;
//get the cell coord of the second point
- if(!cost_map_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
+ if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
return -1.0;
line_cost = lineCost(x0, x1, y0, y1);
@@ -90,11 +90,11 @@
//we also need to connect the first point in the footprint to the last point
//get the cell coord of the last point
- if(!cost_map_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
+ if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
return -1.0;
//get the cell coord of the first point
- if(!cost_map_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
+ if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
return -1.0;
line_cost = lineCost(x0, x1, y0, y1);
@@ -189,11 +189,11 @@
double CostmapModel::pointCost(int x, int y){
//if the cell is in an obstacle the path is invalid
- if(cost_map_.getCost(x, y) == LETHAL_OBSTACLE){
+ if(costmap_.getCost(x, y) == LETHAL_OBSTACLE){
return -1;
}
- return cost_map_.getCost(x, y);
+ return costmap_.getCost(x, y);
}
};
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -43,7 +43,7 @@
namespace base_local_planner{
TrajectoryPlanner::TrajectoryPlanner(WorldModel& world_model,
- const Costmap2D& cost_map,
+ const Costmap2D& costmap,
std::vector<Point> footprint_spec,
double inscribed_radius, double circumscribed_radius,
double acc_lim_x, double acc_lim_y, double acc_lim_theta,
@@ -56,7 +56,7 @@
double max_vel_th, double min_vel_th, double min_in_place_vel_th,
bool dwa, bool heading_scoring, double heading_scoring_timestep, bool simple_attractor,
vector<double> y_vels)
- : map_(cost_map.cellSizeX(), cost_map.cellSizeY()), cost_map_(cost_map),
+ : map_(costmap.cellSizeX(), costmap.cellSizeY()), costmap_(costmap),
world_model_(world_model), footprint_spec_(footprint_spec),
inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius),
goal_x_(0), goal_y_(0),
@@ -103,7 +103,7 @@
double g_x = global_plan_[i].pose.position.x;
double g_y = global_plan_[i].pose.position.y;
unsigned int map_x, map_y;
- if(cost_map_.worldToMap(g_x, g_y, map_x, map_y)){
+ if(costmap_.worldToMap(g_x, g_y, map_x, map_y)){
MapCell& current = map_(map_x, map_y);
current.path_dist = 0.0;
current.path_mark = true;
@@ -120,7 +120,7 @@
if(local_goal_x >= 0 && local_goal_y >= 0){
MapCell& current = map_(local_goal_x, local_goal_y);
- cost_map_.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
+ costmap_.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
current.goal_dist = 0.0;
current.goal_mark = true;
goal_dist_queue.push(¤t);
@@ -259,7 +259,7 @@
unsigned int cell_x, cell_y;
//we don't want a path that goes off the know map
- if(!cost_map_.worldToMap(x_i, y_i, cell_x, cell_y)){
+ if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){
traj.cost_ = -1.0;
return;
}
@@ -273,7 +273,7 @@
return;
}
- occ_cost += cost_map_.getCost(cell_x, cell_y);
+ occ_cost += costmap_.getCost(cell_x, cell_y);
double cell_pdist = map_(cell_x, cell_y).path_dist;
double cell_gdist = map_(cell_x, cell_y).goal_dist;
@@ -338,10 +338,10 @@
unsigned int goal_cell_x, goal_cell_y;
//find a clear line of sight from the robot's cell to a point on the path
for(int i = global_plan_.size() - 1; i >=0; --i){
- if(cost_map_.worldToMap(global_plan_[i].pose.position.x, global_plan_[i].pose.position.y, goal_cell_x, goal_cell_y)){
+ if(costmap_.worldToMap(global_plan_[i].pose.position.x, global_plan_[i].pose.position.y, goal_cell_x, goal_cell_y)){
if(lineCost(cell_x, goal_cell_x, cell_y, goal_cell_y) >= 0){
double gx, gy;
- cost_map_.mapToWorld(goal_cell_x, goal_cell_y, gx, gy);
+ costmap_.mapToWorld(goal_cell_x, goal_cell_y, gx, gy);
double v1_x = gx - x;
double v1_y = gy - y;
double v2_x = cos(heading);
@@ -443,7 +443,7 @@
}
double TrajectoryPlanner::pointCost(int x, int y){
- unsigned char cost = cost_map_.getCost(x, y);
+ unsigned char cost = costmap_.getCost(x, y);
//if the cell is in an obstacle the path is invalid
if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE){
return -1;
@@ -583,7 +583,7 @@
unsigned int cell_x, cell_y;
//make sure that we'll be looking at a legal cell
- if(cost_map_.worldToMap(x_r, y_r, cell_x, cell_y)){
+ if(costmap_.worldToMap(x_r, y_r, cell_x, cell_y)){
double ahead_gdist = map_(cell_x, cell_y).goal_dist;
if(ahead_gdist < heading_dist){
//if we haven't already tried rotating left since we've moved forward
@@ -679,7 +679,7 @@
unsigned int cell_x, cell_y;
//make sure that we'll be looking at a legal cell
- if(cost_map_.worldToMap(x_r, y_r, cell_x, cell_y)){
+ if(costmap_.worldToMap(x_r, y_r, cell_x, cell_y)){
double ahead_gdist = map_(cell_x, cell_y).goal_dist;
if(ahead_gdist < heading_dist){
//if we haven't already tried strafing left since we've moved forward
@@ -976,7 +976,7 @@
Point pt;
for(unsigned int i = 0; i < footprint_cells.size(); ++i){
double pt_x, pt_y;
- cost_map_.mapToWorld(footprint_cells[i].x, footprint_cells[i].y, pt_x, pt_y);
+ costmap_.mapToWorld(footprint_cells[i].x, footprint_cells[i].y, pt_x, pt_y);
pt.x = pt_x;
pt.y = pt_y;
footprint_pts.push_back(pt);
@@ -1003,12 +1003,12 @@
//find the cell coordinates of the first segment point
new_x = x_i + (footprint_spec_[i].x * cos_th - footprint_spec_[i].y * sin_th);
new_y = y_i + (footprint_spec_[i].x * sin_th + footprint_spec_[i].y * cos_th);
- cost_map_.worldToMap(new_x, new_y, x0, y0);
+ costmap_.worldToMap(new_x, new_y, x0, y0);
//find the cell coordinates of the second segment point
new_x = x_i + (footprint_spec_[i + 1].x * cos_th - footprint_spec_[i + 1].y * sin_th);
new_y = y_i + (footprint_spec_[i + 1].x * sin_th + footprint_spec_[i + 1].y * cos_th);
- cost_map_.worldToMap(new_x, new_y, x1, y1);
+ costmap_.worldToMap(new_x, new_y, x1, y1);
getLineCells(x0, x1, y0, y1, footprint_cells);
}
@@ -1016,11 +1016,11 @@
//we need to close the loop, so we also have to raytrace from the last pt to first pt
new_x = x_i + (footprint_spec_[last_index].x * cos_th - footprint_spec_[last_index].y * sin_th);
new_y = y_i + (footprint_spec_[last_index].x * sin_th + footprint_spec_[last_index].y * cos_th);
- cost_map_.worldToMap(new_x, new_y, x0, y0);
+ costmap_.worldToMap(new_x, new_y, x0, y0);
new_x = x_i + (footprint_spec_[0].x * cos_th - footprint_spec_[0].y * sin_th);
new_y = y_i + (footprint_spec_[0].x * sin_th + footprint_spec_[0].y * cos_th);
- cost_map_.worldToMap(new_x, new_y, x1, y1);
+ costmap_.worldToMap(new_x, new_y, x1, y1);
getLineCells(x0, x1, y0, y1, footprint_cells);
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -48,8 +48,8 @@
namespace base_local_planner {
TrajectoryPlannerROS::TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- Costmap2D& cost_map, std::vector<Point> footprint_spec, const Costmap2D* planner_map)
- : world_model_(NULL), tc_(NULL), cost_map_(cost_map), base_scan_notifier_(NULL), tf_(tf), ros_node_(ros_node), laser_scans_(2),
+ Costmap2D& costmap, std::vector<Point> footprint_spec, const Costmap2D* planner_map)
+ : world_model_(NULL), tc_(NULL), costmap_(costmap), base_scan_notifier_(NULL), tf_(tf), ros_node_(ros_node), laser_scans_(2),
point_grid_(NULL), voxel_grid_(NULL), rot_stopped_velocity_(1e-2), trans_stopped_velocity_(1e-2), goal_reached_(true), costmap_publisher_(NULL){
double acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity;
int vx_samples, vtheta_samples;
@@ -69,7 +69,7 @@
double map_publish_frequency;
ros_node.param("~base_local_planner/costmap_visualization_rate", map_publish_frequency, 2.0);
- costmap_publisher_ = new costmap_2d::Costmap2DPublisher(ros_node, cost_map, map_publish_frequency, global_frame_, "base_local_planner");
+ costmap_publisher_ = new costmap_2d::Costmap2DPublisher(ros_node, costmap, map_publish_frequency, global_frame_, "base_local_planner");
//we need to make sure that the transform between the robot base frame and the global frame is available
while(!tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(5.0))){
@@ -161,11 +161,11 @@
*/
}
else{
- world_model_ = new CostmapModel(cost_map);
+ world_model_ = new CostmapModel(costmap);
ROS_INFO("Costmap\n");
}
- tc_ = new TrajectoryPlanner(*world_model_, cost_map, footprint_spec, inscribed_radius_, circumscribed_radius_,
+ tc_ = new TrajectoryPlanner(*world_model_, costmap, footprint_spec, inscribed_radius_, circumscribed_radius_,
acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity, vx_samples, vtheta_samples, pdist_scale,
gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, holonomic_robot,
max_vel_x, min_vel_x, max_vel_th, min_vel_th, min_in_place_vel_th_,
@@ -373,7 +373,7 @@
prunePlan(global_pose, global_plan_);
//we also want to clear the robot footprint from the costmap we're using
- clearRobotFootprint(global_pose, cost_map_);
+ clearRobotFootprint(global_pose, costmap_);
// Set current velocities from odometry
robot_msgs::PoseDot global_vel;
@@ -526,7 +526,7 @@
}
}
- void TrajectoryPlannerROS::clearRobotFootprint(Costmap2D& cost_map){
+ void TrajectoryPlannerROS::clearRobotFootprint(Costmap2D& costmap){
tf::Stamped<tf::Pose> robot_pose, global_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
@@ -549,29 +549,29 @@
return;
}
- clearRobotFootprint(global_pose, cost_map);
+ clearRobotFootprint(global_pose, costmap);
}
- void TrajectoryPlannerROS::clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, Costmap2D& cost_map){
+ void TrajectoryPlannerROS::clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, Costmap2D& costmap){
double useless_pitch, useless_roll, yaw;
global_pose.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
//get the oriented footprint of the robot
std::vector<robot_msgs::Point> oriented_footprint = drawFootprint(global_pose.getOrigin().x(), global_pose.getOrigin().y(), yaw);
- cost_map.lock();
+ costmap.lock();
//set the associated costs in the cost map to be free
- if(!cost_map.setConvexPolygonCost(oriented_footprint, costmap_2d::FREE_SPACE))
+ if(!costmap.setConvexPolygonCost(oriented_footprint, costmap_2d::FREE_SPACE))
return;
double max_inflation_dist = inflation_radius_ + circumscribed_radius_;
//clear all non-lethal obstacles out to the maximum inflation distance of an obstacle in the robot footprint
- cost_map.clearNonLethal(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist, max_inflation_dist);
+ costmap.clearNonLethal(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist, max_inflation_dist);
//make sure to re-inflate obstacles in the affected region... plus those obstalces that could
- cost_map.reinflateWindow(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist + inflation_radius_, max_inflation_dist + inflation_radius_, false);
- cost_map.unlock();
+ costmap.reinflateWindow(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist + inflation_radius_, max_inflation_dist + inflation_radius_, false);
+ costmap.unlock();
}
Modified: pkg/trunk/nav/base_local_planner/test/utest.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/test/utest.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/base_local_planner/test/utest.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -67,11 +67,11 @@
for (unsigned int y = 0; y < size_y_; y++){
unsigned int ind = x + (y * size_x_);
if(map_(x, y).occ_state == 1)
- cost_map_[ind] = costmap_2d::LETHAL_OBSTACLE;
+ costmap_[ind] = costmap_2d::LETHAL_OBSTACLE;
else if(map_(x, y).occ_dist < outer_radius_)
- cost_map_[ind] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE/2;
+ costmap_[ind] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE/2;
else
- cost_map_[ind] = 0;
+ costmap_[ind] = 0;
}
}
}
Modified: pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h
===================================================================
--- pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -123,7 +123,7 @@
/**
* @brief Resets the costmaps to the static map outside a given window
*/
- void resetCostMaps();
+ void resetCostmaps();
ros::Node& ros_node_;
tf::TransformListener& tf_;
Modified: pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -65,7 +65,7 @@
//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("navfn"));
- planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
+ planner_cost_map_ros_->getCostmapCopy(planner_cost_map_);
//initialize the NavFn planner
planner_ = new NavfnROS(ros_node_, tf_, planner_cost_map_);
@@ -73,7 +73,7 @@
//create the ros wrapper for the controller's cost_map... and initializer a pointer we'll use with the underlying map
controller_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"));
- controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ controller_cost_map_ros_->getCostmapCopy(controller_cost_map_);
robot_msgs::Point pt;
//create a square footprint
@@ -122,7 +122,7 @@
return;
//update the copy of the costmap the planner uses
- planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
+ planner_cost_map_ros_->getCostmapCopy(planner_cost_map_);
// set cost of forbidden region
@@ -209,7 +209,7 @@
update(feedback);
//make sure to update the cost_map we'll use for this cycle
- controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ controller_cost_map_ros_->getCostmapCopy(controller_cost_map_);
//check that the observation buffers for the costmap are current
if(!controller_cost_map_ros_->isCurrent()){
@@ -286,7 +286,7 @@
return true;
}
- void MoveBaseConstrained::resetCostMaps(){
+ void MoveBaseConstrained::resetCostmaps(){
planner_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
controller_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
}
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -460,7 +460,7 @@
* @param cost The cost
*/
inline void updateCellCost(unsigned int index, unsigned char cost){
- unsigned char* cell_cost = &cost_map_[index];
+ unsigned char* cell_cost = &costmap_[index];
if(*cell_cost == NO_INFORMATION)
*cell_cost = cost;
else
@@ -515,7 +515,7 @@
double origin_x_;
double origin_y_;
unsigned char* static_map_;
- unsigned char* cost_map_;
+ unsigned char* costmap_;
unsigned char* markers_;
double sq_obstacle_range_;
double max_obstacle_height_;
@@ -532,38 +532,38 @@
//functors for raytracing actions
class ClearCell {
public:
- ClearCell(unsigned char* cost_map) : cost_map_(cost_map) {}
+ ClearCell(unsigned char* costmap) : costmap_(costmap) {}
inline void operator()(unsigned int offset){
- cost_map_[offset] = 0;
+ costmap_[offset] = 0;
}
private:
- unsigned char* cost_map_;
+ unsigned char* costmap_;
};
class MarkCell {
public:
- MarkCell(unsigned char* cost_map) : cost_map_(cost_map) {}
+ MarkCell(unsigned char* costmap) : costmap_(costmap) {}
inline void operator()(unsigned int offset){
- cost_map_[offset] = LETHAL_OBSTACLE;
+ costmap_[offset] = LETHAL_OBSTACLE;
}
private:
- unsigned char* cost_map_;
+ unsigned char* costmap_;
};
class PolygonOutlineCells {
public:
- PolygonOutlineCells(const Costmap2D& cost_map, const unsigned char* char_map, std::vector<MapLocation>& cells)
- : cost_map_(cost_map), char_map_(char_map), cells_(cells){}
+ PolygonOutlineCells(const Costmap2D& costmap, const unsigned char* char_map, std::vector<MapLocation>& cells)
+ : costmap_(costmap), char_map_(char_map), cells_(cells){}
//just push the relevant cells back onto the list
inline void operator()(unsigned int offset){
MapLocation loc;
- cost_map_.indexToCells(offset, loc.x, loc.y);
+ costmap_.indexToCells(offset, loc.x, loc.y);
cells_.push_back(loc);
}
private:
- const Costmap2D& cost_map_;
+ const Costmap2D& costmap_;
const unsigned char* char_map_;
std::vector<MapLocation>& cells_;
};
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_publisher.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_publisher.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_publisher.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -46,16 +46,16 @@
namespace costmap_2d {
class Costmap2DPublisher {
public:
- Costmap2DPublisher(ros::Node& ros_node, Costmap2D& cost_map, double publish_frequency, std::string global_frame, std::string topic_prefix = std::string(""));
+ Costmap2DPublisher(ros::Node& ros_node, Costmap2D& costmap, double publish_frequency, std::string global_frame, std::string topic_prefix = std::string(""));
~Costmap2DPublisher();
- void publishCostMap();
+ void publishCostmap();
private:
void mapPublishLoop(double frequency);
ros::Node& ros_node_;
- Costmap2D& cost_map_;
+ Costmap2D& costmap_;
std::string global_frame_,topic_prefix_;
boost::thread* visualizer_thread_; ///< @brief A thread for publising to the visualizer
};
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -138,9 +138,9 @@
/**
* @brief Returns a copy of the underlying costmap
- * @param cost_map A reference to the map to populate
+ * @param costmap A reference to the map to populate
*/
- void getCostMapCopy(Costmap2D& cost_map);
+ void getCostmapCopy(Costmap2D& costmap);
/**
* @brief Returns a copy of the underlying unsigned character array <B>(NOTE: THE USER IS RESPONSIBLE FOR DELETION)</B>
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -46,12 +46,12 @@
double max_obstacle_height, double raytrace_range, double weight,
const std::vector<unsigned char>& static_data, unsigned char lethal_threshold) : size_x_(cells_size_x),
size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y), static_map_(NULL),
- cost_map_(NULL), markers_(NULL), sq_obstacle_range_(obstacle_range * obstacle_range),
+ costmap_(NULL), markers_(NULL), sq_obstacle_range_(obstacle_range * obstacle_range),
max_obstacle_height_(max_obstacle_height), raytrace_range_(raytrace_range), cached_costs_(NULL), cached_distances_(NULL),
inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), inflation_radius_(inflation_radius),
weight_(weight), inflation_queue_(){
- //creat the cost_map, static_map, and markers
- cost_map_ = new unsigned char[size_x_ * size_y_];
+ //creat the costmap, static_map, and markers
+ costmap_ = new unsigned char[size_x_ * size_y_];
static_map_ = new unsigned char[size_x_ * size_y_];
markers_ = new unsigned char[size_x_ * size_y_];
memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
@@ -87,8 +87,8 @@
for(unsigned int j = 0; j < size_y_; ++j){
unsigned int index = getIndex(i, j);
//if the static value is above the threshold... it is a lethal obstacle... otherwise just take the cost
- cost_map_[index] = static_data[index] >= lethal_threshold ? LETHAL_OBSTACLE : static_data[index];
- if(cost_map_[index] == LETHAL_OBSTACLE){
+ costmap_[index] = static_data[index] >= lethal_threshold ? LETHAL_OBSTACLE : static_data[index];
+ if(costmap_[index] == LETHAL_OBSTACLE){
unsigned int mx, my;
indexToCells(index, mx, my);
enqueue(index, mx, my, mx, my, inflation_queue_);
@@ -99,12 +99,12 @@
inflateObstacles(inflation_queue_);
//we also want to keep a copy of the current costmap as the static map
- memcpy(static_map_, cost_map_, size_x_ * size_y_ * sizeof(unsigned char));
+ memcpy(static_map_, costmap_, size_x_ * size_y_ * sizeof(unsigned char));
}
else{
//everything is unknown initially if we don't have a static map
memset(static_map_, NO_INFORMATION, size_x_ * size_y_ * sizeof(unsigned char));
- memset(cost_map_, NO_INFORMATION, size_x_ * size_y_ * sizeof(unsigned char));
+ memset(costmap_, NO_INFORMATION, size_x_ * size_y_ * sizeof(unsigned char));
}
}
@@ -114,7 +114,7 @@
return *this;
//clean up old data
- if(cost_map_ != NULL) delete[] cost_map_;
+ if(costmap_ != NULL) delete[] costmap_;
if(static_map_ != NULL) delete[] static_map_;
if(markers_ != NULL) delete[] markers_;
@@ -139,7 +139,7 @@
origin_y_ = map.origin_y_;
//initialize our various maps
- cost_map_ = new unsigned char[size_x_ * size_y_];
+ costmap_ = new unsigned char[size_x_ * size_y_];
static_map_ = new unsigned char[size_x_ * size_y_];
markers_ = new unsigned char[size_x_ * size_y_];
@@ -150,7 +150,7 @@
memcpy(static_map_, map.static_map_, size_x_ * size_y_ * sizeof(unsigned char));
//copy the cost map
- memcpy(cost_map_, map.cost_map_, size_x_ * size_y_ * sizeof(unsigned char));
+ memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));
sq_obstacle_range_ = map.sq_obstacle_range_;
max_obstacle_height_ = map.max_obstacle_height_;
@@ -184,16 +184,16 @@
return *this;
}
- Costmap2D::Costmap2D(const Costmap2D& map) : static_map_(NULL), cost_map_(NULL), markers_(NULL), cached_costs_(NULL), cached_distances_(NULL) {
+ Costmap2D::Costmap2D(const Costmap2D& map) : static_map_(NULL), costmap_(NULL), markers_(NULL), cached_costs_(NULL), cached_distances_(NULL) {
*this = map;
}
//just initialize everything to NULL by default
Costmap2D::Costmap2D() : size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), static_map_(NULL),
- cost_map_(NULL), markers_(NULL), cached_costs_(NULL), cached_distances_(NULL) {}
+ costmap_(NULL), markers_(NULL), cached_costs_(NULL), cached_distances_(NULL) {}
Costmap2D::~Costmap2D(){
- if(cost_map_ != NULL) delete[] cost_map_;
+ if(costmap_ != NULL) delete[] costmap_;
if(static_map_ != NULL) delete[] static_map_;
if(markers_ != NULL) delete[] markers_;
@@ -219,22 +219,22 @@
unsigned char* Costmap2D::getCharMapCopy() const {
unsigned char* map_copy = new unsigned char[size_x_ * size_y_];
- memcpy(map_copy, cost_map_, size_x_ * size_y_ * sizeof(unsigned char));
+ memcpy(map_copy, costmap_, size_x_ * size_y_ * sizeof(unsigned char));
return map_copy;
}
const unsigned char* Costmap2D::getCharMap() const {
- return cost_map_;
+ return costmap_;
}
unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const {
ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot get the cost of a cell that is outside the bounds of the costmap");
- return cost_map_[getIndex(mx, my)];
+ return costmap_[getIndex(mx, my)];
}
void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) {
ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot set the cost of a cell that is outside the bounds of the costmap");
- cost_map_[getIndex(mx, my)] = cost;
+ costmap_[getIndex(mx, my)] = cost;
}
void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const {
@@ -289,30 +289,30 @@
unsigned char local_map[cell_size_x * cell_size_y];
//copy the local window in the costmap to the local map
- unsigned char* cost_map_cell = &cost_map_[getIndex(start_x, start_y)];
+ unsigned char* costmap_cell = &costmap_[getIndex(start_x, start_y)];
unsigned char* local_map_cell = local_map;
for(unsigned int y = 0; y < cell_size_y; ++y){
for(unsigned int x = 0; x < cell_size_x; ++x){
- *local_map_cell = *cost_map_cell;
+ *local_map_cell = *costmap_cell;
local_map_cell++;
- cost_map_cell++;
+ costmap_cell++;
}
- cost_map_cell += size_x_ - cell_size_x;
+ costmap_cell += size_x_ - cell_size_x;
}
...
[truncated message content] |
