Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16850] pkg/trunk
|
From: <ei...@us...> - 2009-06-09 02:22:15
|
Revision: 16850
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16850&view=rev
Author: eitanme
Date: 2009-06-09 01:36:55 +0000 (Tue, 09 Jun 2009)
Log Message:
-----------
Moving some logic out of the move_base action TrajectoryPlannerROS now holds a reference to a Costmap2DROS object instead of a Costmap2D obect
Modified Paths:
--------------
pkg/trunk/highlevel/move_base/include/move_base/move_base.h
pkg/trunk/highlevel/move_base/src/move_base.cpp
pkg/trunk/highlevel/move_base/src/move_base_local.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
Modified: pkg/trunk/highlevel/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-09 01:36:55 UTC (rev 16850)
@@ -123,7 +123,7 @@
bool run_planner_;
base_local_planner::TrajectoryPlannerROS* tc_;
costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
- costmap_2d::Costmap2D planner_costmap_, controller_costmap_;
+ costmap_2d::Costmap2D planner_costmap_;
navfn::NavfnROS* planner_;
std::vector<robot_msgs::PoseStamped> global_plan_;
Modified: pkg/trunk/highlevel/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-09 01:36:55 UTC (rev 16850)
@@ -132,10 +132,9 @@
//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"), footprint_);
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, *controller_costmap_ros_, footprint_);
//advertise a service for getting a plan
ros_node_.advertiseService("~make_plan", &MoveBase::planService, this);
@@ -384,7 +383,6 @@
//make sure to update the costmap we'll use for this cycle
controller_costmap_ros_->clearRobotFootprint();
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//check that the observation buffers for the costmap are current
if(!controller_costmap_ros_->isCurrent()){
Modified: pkg/trunk/highlevel/move_base/src/move_base_local.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base_local.cpp 2009-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/highlevel/move_base/src/move_base_local.cpp 2009-06-09 01:36:55 UTC (rev 16850)
@@ -65,7 +65,6 @@
//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_->clearRobotFootprint();
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//initially we'll stop all updates on the costmap
@@ -92,7 +91,7 @@
//footprint_.push_back(pt);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, *controller_costmap_ros_, footprint_);
}
MoveBaseLocal::~MoveBaseLocal(){
@@ -179,9 +178,6 @@
//push the feedback out
update(feedback);
- //make sure to update the costmap we'll use for this cycle
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
-
robot_msgs::PoseDot cmd_vel;
//check that the observation buffers for the costmap are current
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-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-06-09 01:36:55 UTC (rev 16850)
@@ -82,7 +82,7 @@
* @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
*/
TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- costmap_2d::Costmap2D& costmap, std::vector<robot_msgs::Point> footprint_spec);
+ costmap_2d::Costmap2DROS& costmap_ros, std::vector<robot_msgs::Point> footprint_spec);
/**
* @brief Destructor for the wrapper
@@ -128,12 +128,6 @@
*/
bool goalReached();
- /**
- * @brief Clear the footprint of the robot in a given cost map
- * @param costmap The costmap to apply the clearing opertaion on
- */
- void clearRobotFootprint(costmap_2d::Costmap2D& costmap);
-
private:
/**
* @brief Check whether the robot is stopped or not
@@ -198,13 +192,6 @@
void publishFootprint(const tf::Stamped<tf::Pose>& global_pose);
/**
- * @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 costmap The costmap to apply the clearing opertaion on
- */
- void clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, costmap_2d::Costmap2D& costmap);
-
- /**
* @brief Publish a plan for visualization purposes
*/
void publishPlan(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a);
@@ -213,7 +200,8 @@
WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
- costmap_2d::Costmap2D& costmap_; ///< @brief The costmap the controller will use
+ costmap_2d::Costmap2DROS& costmap_ros_; ///< @brief The ROS wrapper for the costmap the controller will use
+ costmap_2d::Costmap2D costmap_; ///< @brief The costmap the controller will use
tf::TransformListener& tf_; ///< @brief Used for transforming point clouds
ros::Node& ros_node_; ///< @brief The ros node we're running under
std::string global_frame_; ///< @brief The frame in which the controller will run
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-06-09 01:36:55 UTC (rev 16850)
@@ -48,8 +48,8 @@
namespace base_local_planner {
TrajectoryPlannerROS::TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- Costmap2D& costmap, std::vector<Point> footprint_spec)
- : world_model_(NULL), tc_(NULL), costmap_(costmap), tf_(tf), ros_node_(ros_node),
+ Costmap2DROS& costmap_ros, std::vector<Point> footprint_spec)
+ : world_model_(NULL), tc_(NULL), costmap_ros_(costmap_ros), tf_(tf), ros_node_(ros_node),
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;
@@ -59,6 +59,9 @@
double max_vel_x, min_vel_x, max_vel_th, min_vel_th;
string world_model_type;
+ //initialize the copy of the costmap the controller will use
+ costmap_ros_.getCostmapCopy(costmap_);
+
//adverstise the fact that we'll publish the robot footprint
ros_node.advertise<visualization_msgs::Polyline>("~base_local_planner/robot_footprint", 1);
ros_node.advertise<visualization_msgs::Polyline>("~base_local_planner/global_plan", 1);
@@ -128,9 +131,9 @@
ros_node.param("~base_local_planner/point_grid/grid_resolution", grid_resolution, 0.2);
ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");
- world_model_ = new CostmapModel(costmap);
+ world_model_ = new CostmapModel(costmap_);
- tc_ = new TrajectoryPlanner(*world_model_, costmap, 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, escape_reset_dist, escape_reset_theta, holonomic_robot,
max_vel_x, min_vel_x, max_vel_th, min_vel_th, min_in_place_vel_th_,
@@ -338,8 +341,11 @@
//we also want to clear the robot footprint from the costmap we're using
- clearRobotFootprint(global_pose, costmap_);
+ costmap_ros_.clearRobotFootprint();
+ //make sure to update the costmap we'll use for this cycle
+ costmap_ros_.getCostmapCopy(costmap_);
+
//after clearing the footprint... we want to push the changes to the costmap publisher
if(costmap_publisher_->active())
costmap_publisher_->updateCostmapData(costmap_);
@@ -481,60 +487,6 @@
}
}
- void TrajectoryPlannerROS::clearRobotFootprint(Costmap2D& costmap){
- tf::Stamped<tf::Pose> robot_pose, global_pose;
- robot_pose.setIdentity();
- robot_pose.frame_id_ = robot_base_frame_;
- robot_pose.stamp_ = ros::Time();
- ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
-
- //get the global pose of the robot
- try{
- tf_.transformPose(global_frame_, robot_pose, global_pose);
- }
- catch(tf::LookupException& ex) {
- ROS_ERROR("No Transform available Error: %s\n", ex.what());
- return;
- }
- catch(tf::ConnectivityException& ex) {
- ROS_ERROR("Connectivity Error: %s\n", ex.what());
- return;
- }
- catch(tf::ExtrapolationException& ex) {
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- return;
- }
- // check global_pose timeout
- if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) {
- ROS_ERROR("TrajcetoryPlannerROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
- current_time.toSec() ,global_pose.stamp_.toSec() ,transform_tolerance_);
- return;
- }
-
- clearRobotFootprint(global_pose, costmap);
- }
-
- 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);
-
- //set the associated costs in the cost map to be free
- 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
- 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
- costmap.reinflateWindow(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist + inflation_radius_, max_inflation_dist + inflation_radius_, false);
-
- }
-
void TrajectoryPlannerROS::publishFootprint(const tf::Stamped<tf::Pose>& global_pose){
double useless_pitch, useless_roll, yaw;
global_pose.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
Modified: pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-06-09 01:36:55 UTC (rev 16850)
@@ -100,10 +100,9 @@
//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"), footprint_);
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, *controller_costmap_ros_, footprint_);
//initially clear any unknown space around the robot
planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
@@ -239,7 +238,6 @@
//make sure to update the costmap we'll use for this cycle
controller_costmap_ros_->clearRobotFootprint();
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//check that the observation buffers for the costmap are current
if(!controller_costmap_ros_->isCurrent()){
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
