Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16866] pkg/trunk
|
From: <ei...@us...> - 2009-06-09 17:38:17
|
Revision: 16866
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16866&view=rev
Author: eitanme
Date: 2009-06-09 17:38:10 +0000 (Tue, 09 Jun 2009)
Log Message:
-----------
NavfnROS now takes a reference to a Costmap2DROS object instead of a Costmap2D object to move complexity out of the MoveBase action
Modified Paths:
--------------
pkg/trunk/highlevel/move_base/include/move_base/move_base.h
pkg/trunk/highlevel/move_base/src/move_base.cpp
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/src/navfn_ros.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 17:35:35 UTC (rev 16865)
+++ pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-09 17:38:10 UTC (rev 16866)
@@ -123,7 +123,6 @@
bool run_planner_;
base_local_planner::TrajectoryPlannerROS* tc_;
costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
- 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 17:35:35 UTC (rev 16865)
+++ pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-09 17:38:10 UTC (rev 16866)
@@ -124,11 +124,10 @@
//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("navfn"), footprint_);
- planner_costmap_ros_->getCostmapCopy(planner_costmap_);
//initialize the NavFn planner
- planner_ = new NavfnROS(ros_node_, tf_, planner_costmap_);
- ROS_INFO("MAP SIZE: %d, %d", planner_costmap_.cellSizeX(), planner_costmap_.cellSizeY());
+ planner_ = new NavfnROS(ros_node_, tf_, *planner_costmap_ros_);
+ ROS_INFO("MAP SIZE: %d, %d", planner_costmap_ros_->cellSizeX(), planner_costmap_ros_->cellSizeY());
//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_);
@@ -215,12 +214,7 @@
//update the copy of the costmap the planner uses
clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
- planner_costmap_ros_->clearRobotFootprint();
- 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_costmap_); //now done in sensors
-
//if we have a tolerance on the goal point that is greater
//than the resolution of the map... compute the full potential function
double resolution = planner_costmap_ros_->resolution();
@@ -292,13 +286,6 @@
if(planner_costmap_ros_ == NULL)
return;
- //update the copy of the costmap the planner uses
- planner_costmap_ros_->clearRobotFootprint();
- 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_costmap_); //now done in sensors
-
std::vector<robot_msgs::PoseStamped> global_plan;
bool valid_plan = planner_->makePlan(goal, global_plan);
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-06-09 17:35:35 UTC (rev 16865)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-06-09 17:38:10 UTC (rev 16866)
@@ -39,7 +39,7 @@
#include <ros/node.h>
#include <navfn/navfn.h>
-#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/costmap_2d_ros.h>
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/Point.h>
#include <visualization_msgs/Polyline.h>
@@ -54,9 +54,9 @@
* @brief Constructor for the NavFnROS object
* @param ros_node The a reference to the ros node running
* @param tf A reference to a TransformListener
- * @param cos_map A reference to the costmap to use
+ * @param costmap_ros A reference to the ROS wrapper of the costmap to use
*/
- NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& costmap);
+ NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2DROS& costmap_ros);
/**
* @brief Given a goal pose in the world, compute a plan
@@ -98,7 +98,8 @@
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& costmap_;
+ costmap_2d::Costmap2DROS& costmap_ros_;
+ 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_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-06-09 17:35:35 UTC (rev 16865)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-06-09 17:38:10 UTC (rev 16866)
@@ -37,8 +37,11 @@
#include <navfn/navfn_ros.h>
namespace navfn {
- 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()) {
+ NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2DROS& costmap_ros) : ros_node_(ros_node), tf_(tf),
+ costmap_ros_(costmap_ros), planner_(costmap_ros.cellSizeX(), costmap_ros.cellSizeY()) {
+ //get an initial copy of the costmap
+ costmap_ros_.getCostmapCopy(costmap_);
+
//advertise our plan visualization
ros_node_.advertise<visualization_msgs::Polyline>("~navfn/plan", 1);
@@ -72,6 +75,10 @@
}
bool NavfnROS::computePotential(const robot_msgs::Point& world_point){
+ //make sure that we have the latest copy of the costmap and that we clear the footprint of obstacles
+ costmap_ros_.clearRobotFootprint();
+ costmap_ros_.getCostmapCopy(costmap_);
+
planner_.setCostmap(costmap_.getCharMap());
unsigned int mx, my;
@@ -110,6 +117,10 @@
}
bool NavfnROS::makePlan(const robot_msgs::PoseStamped& goal, std::vector<robot_msgs::PoseStamped>& plan){
+ //make sure that we have the latest copy of the costmap and that we clear the footprint of obstacles
+ costmap_ros_.clearRobotFootprint();
+ costmap_ros_.getCostmapCopy(costmap_);
+
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if(goal.header.frame_id != global_frame_){
ROS_ERROR("The goal passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame_.c_str(), goal.header.frame_id.c_str());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
