Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14204] pkg/trunk
|
From: <ei...@us...> - 2009-04-22 01:20:17
|
Revision: 14204
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14204&view=rev
Author: eitanme
Date: 2009-04-22 01:20:02 +0000 (Wed, 22 Apr 2009)
Log Message:
-----------
Moved a lot of code out of the action and into the ROS wrappers for the navstack
Modified Paths:
--------------
pkg/trunk/highlevel/nav/include/nav/move_base_action.h
pkg/trunk/highlevel/nav/src/move_base_action.cpp
pkg/trunk/highlevel/test_nav/launch_navstack.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/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
Modified: pkg/trunk/highlevel/nav/include/nav/move_base_action.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-22 01:20:02 UTC (rev 14204)
@@ -87,38 +87,19 @@
bool sleepLeftover(ros::Time start, ros::Duration cycle_time, ros::Duration& actual);
/**
- * @brief Publishes the footprint of the robot for visualization purposes
- */
- void publishFootprint();
-
- /**
- * @brief Publish a path for visualization purposes
- */
- void publishPath(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a);
-
- /**
* @brief Make a new global plan
* @param goal The goal to plan to
*/
void makePlan(const robot_msgs::PoseStamped& goal);
/**
- * @brief Trim off parts of the global plan that are far enough behind the robot
+ * @brief Get the current pose of the robot in the specified frame
+ * @param frame The frame to get the pose in
+ * @param pose The pose returned
*/
- void prunePlan();
+ void getRobotPose(std::string frame, tf::Stamped<tf::Pose>& pose);
/**
- * @brief Get the current pose of the robot in the global frame and set the global_pose_ variable
- */
- void updateGlobalPose();
-
- /**
- * @brief Clear the footprint of the robot in a given cost map
- * @param cost_map The costmap to apply the clearing opertaion on
- */
- void clearRobotFootprint(costmap_2d::Costmap2D& cost_map);
-
- /**
* @brief Resets the costmaps to the static map outside a given window
*/
void resetCostMaps();
@@ -133,15 +114,13 @@
navfn::NavfnROS* planner_;
std::vector<robot_msgs::PoseStamped> global_plan_;
std::vector<robot_msgs::Point> footprint_;
- std::string global_frame_, robot_base_frame_;
- bool valid_plan_;
+ std::string robot_base_frame_;
+ bool valid_plan_, new_plan_;
boost::recursive_mutex lock_;
robot_msgs::PoseStamped goal_;
tf::Stamped<tf::Pose> global_pose_;
- double inscribed_radius_, circumscribed_radius_, inflation_radius_;
double controller_frequency_;
- double transform_tolerance_; // timeout before transform errors
};
};
Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-22 01:20:02 UTC (rev 14204)
@@ -45,38 +45,20 @@
MoveBaseAction::MoveBaseAction(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),
- planner_(NULL), valid_plan_(false) {
+ planner_(NULL), valid_plan_(false), new_plan_(false) {
//get some parameters that will be global to the move base node
- ros_node_.param("~global_frame", global_frame_, std::string("map"));
- ros_node_.param("~robot_base_frame", robot_base_frame_, std::string("base_link"));
+ ros_node_.param("~navfn/robot_base_frame", robot_base_frame_, std::string("base_link"));
ros_node_.param("~controller_frequency", controller_frequency_, 20.0);
- ros_node_.param("~transform_tolerance", transform_tolerance_, 0.1);
- //for display purposes
- ros_node_.advertise<robot_msgs::Polyline2D>("gui_path", 1);
- ros_node_.advertise<robot_msgs::Polyline2D>("local_path", 1);
- ros_node_.advertise<robot_msgs::Polyline2D>("robot_footprint", 1);
-
//for comanding the base
ros_node_.advertise<robot_msgs::PoseDot>("cmd_vel", 1);
- //pass on some parameters to the components of the move base node if they are not explicitly overridden
- //(perhaps the controller and the planner could operate in different frames)
- if(!ros_node_.hasParam("~base_local_planner/global_frame")) ros_node_.setParam("~base_local_planner/global_frame", global_frame_);
- if(!ros_node_.hasParam("~base_local_planner/robot_base_frame")) ros_node_.setParam("~base_local_planner/robot_base_frame", robot_base_frame_);
- if(!ros_node_.hasParam("~navfn/global_frame")) ros_node_.setParam("~navfn/global_frame", global_frame_);
- if(!ros_node_.hasParam("~navfn/robot_base_frame")) ros_node_.setParam("~navfn/robot_base_frame", robot_base_frame_);
+ double inscribed_radius, circumscribed_radius;
+ //we'll assume the radius of the robot to be consistent with what's specified for the costmaps
+ ros_node_.param("~navfn/costmap/inscribed_radius", inscribed_radius, 0.325);
+ ros_node_.param("~navfn/costmap/circumscribed_radius", circumscribed_radius, 0.46);
- ros_node_.param("~inscribed_radius", inscribed_radius_, 0.325);
- ros_node_.param("~circumscribed_radius", circumscribed_radius_, 0.46);
- ros_node_.param("~inflation_radius", inflation_radius_, 0.55);
-
- //pass on inlfation parameters to the planner's costmap if they're not set explicitly
- if(!ros_node_.hasParam("~navfn/costmap/inscribed_radius")) ros_node_.setParam("~navfn/costmap/inscribed_radius", inscribed_radius_);
- if(!ros_node_.hasParam("~navfn/costmap/circumscribed_radius")) ros_node_.setParam("~navfn/costmap/circumscribed_radius", circumscribed_radius_);
- if(!ros_node_.hasParam("~navfn/costmap/inflation_radius")) ros_node_.setParam("~navfn/costmap/inflation_radius", inflation_radius_);
-
//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_);
@@ -85,32 +67,27 @@
planner_ = new NavfnROS(ros_node_, tf_, planner_cost_map_);
ROS_INFO("MAP SIZE: %d, %d", planner_cost_map_.cellSizeX(), planner_cost_map_.cellSizeY());
- //pass on inlfation parameters to the controller's costmap if they're not set explicitly
- if(!ros_node_.hasParam("~base_local_planner/costmap/inscribed_radius")) ros_node_.setParam("~base_local_planner/costmap/inscribed_radius", inscribed_radius_);
- if(!ros_node_.hasParam("~base_local_planner/costmap/circumscribed_radius")) ros_node_.setParam("~base_local_planner/costmap/circumscribed_radius", circumscribed_radius_);
- if(!ros_node_.hasParam("~base_local_planner/costmap/inflation_radius")) ros_node_.setParam("~base_local_planner/costmap/inflation_radius", inflation_radius_);
-
//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_);
robot_msgs::Point pt;
//create a square footprint
- pt.x = inscribed_radius_ + .01;
- pt.y = -1 * (inscribed_radius_ + .01);
+ pt.x = inscribed_radius + .01;
+ pt.y = -1 * (inscribed_radius + .01);
footprint_.push_back(pt);
- pt.x = -1 * (inscribed_radius_ + .01);
- pt.y = -1 * (inscribed_radius_ + .01);
+ pt.x = -1 * (inscribed_radius + .01);
+ pt.y = -1 * (inscribed_radius + .01);
footprint_.push_back(pt);
- pt.x = -1 * (inscribed_radius_ + .01);
- pt.y = inscribed_radius_ + .01;
+ pt.x = -1 * (inscribed_radius + .01);
+ pt.y = inscribed_radius + .01;
footprint_.push_back(pt);
- pt.x = inscribed_radius_ + .01;
- pt.y = inscribed_radius_ + .01;
+ pt.x = inscribed_radius + .01;
+ pt.y = inscribed_radius + .01;
footprint_.push_back(pt);
//give the robot a nose
- pt.x = circumscribed_radius_;
+ pt.x = circumscribed_radius;
pt.y = 0;
footprint_.push_back(pt);
@@ -134,35 +111,15 @@
delete controller_cost_map_ros_;
}
- void MoveBaseAction::clearRobotFootprint(Costmap2D& cost_map){
- 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 = tc_->drawFootprint(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw);
-
- //set the associated costs in the cost map to be free
- if(!cost_map.setConvexPolygonCost(oriented_footprint, costmap_2d::FREE_SPACE))
- return;
-
- 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);
-
- }
-
void MoveBaseAction::makePlan(const robot_msgs::PoseStamped& goal){
//since this gets called on handle activate
if(planner_cost_map_ros_ == NULL)
return;
- //make a plan for controller
+ //update the copy of the costmap the planner uses
planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
- //make sure we clear the robot's footprint from the cost map
- clearRobotFootprint(planner_cost_map_);
-
std::vector<robot_msgs::PoseStamped> global_plan;
bool valid_plan = planner_->makePlan(goal, global_plan);
@@ -173,20 +130,19 @@
lock_.lock();
//copy over the new global plan
valid_plan_ = valid_plan;
+ new_plan_ = true;
global_plan_ = global_plan;
lock_.unlock();
-
- publishPath(global_plan, "gui_path", 0.0, 1.0, 0.0, 0.0);
}
- void MoveBaseAction::updateGlobalPose(){
+ void MoveBaseAction::getRobotPose(std::string frame, tf::Stamped<tf::Pose>& pose){
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
- ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
robot_pose.stamp_ = ros::Time();
+
try{
- tf_.transformPose(global_frame_, robot_pose, global_pose_);
+ tf_.transformPose(frame, robot_pose, pose);
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
@@ -198,83 +154,57 @@
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
- return;
}
}
- void MoveBaseAction::prunePlan(){
- lock_.lock();
- std::vector<robot_msgs::PoseStamped>::iterator it = global_plan_.begin();
- while(it != global_plan_.end()){
- const robot_msgs::PoseStamped& w = *it;
- // Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
- double x_diff = global_pose_.getOrigin().x() - w.pose.position.x;
- double y_diff = global_pose_.getOrigin().y() - w.pose.position.y;
- double distance = sqrt(x_diff * x_diff + y_diff * y_diff);
- if(distance < 1){
- ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), w.pose.position.x, w.pose.position.y);
- break;
- }
- it = global_plan_.erase(it);
- }
- lock_.unlock();
- }
-
robot_actions::ResultStatus MoveBaseAction::execute(const robot_msgs::PoseStamped& goal, robot_msgs::PoseStamped& feedback){
//update the goal
goal_ = goal;
- //update the global pose
- updateGlobalPose();
-
//first... make a plan to the goal
makePlan(goal_);
ros::Duration cycle_time = ros::Duration(1.0 / controller_frequency_);
while(!isPreemptRequested()){
+ struct timeval start, end;
+ double start_t, end_t, t_diff;
+ gettimeofday(&start, NULL);
+
//get the start time of the loop
ros::Time start_time = ros::Time::now();
- //update the global pose
- updateGlobalPose();
-
//update feedback to correspond to our current position
- tf::PoseStampedTFToMsg(global_pose_, feedback);
+ tf::Stamped<tf::Pose> global_pose;
+ getRobotPose(goal_.header.frame_id, global_pose);
+ tf::PoseStampedTFToMsg(global_pose, feedback);
//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 that we clear the robot footprint in the cost map
- clearRobotFootprint(controller_cost_map_);
-
- //prune the plan before we pass it to the controller
- prunePlan();
-
- struct timeval start, end;
- double start_t, end_t, t_diff;
- gettimeofday(&start, NULL);
-
//check that the observation buffers for the costmap are current
if(!controller_cost_map_ros_->isCurrent()){
ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
continue;
}
+
bool valid_control = false;
robot_msgs::PoseDot cmd_vel;
- std::vector<robot_msgs::PoseStamped> local_plan;
//pass plan to controller
lock_.lock();
if(valid_plan_){
+ //if we have a new plan... we'll update the plan for the controller
+ if(new_plan_){
+ tc_->updatePlan(global_plan_);
+ new_plan_ = false;
+ }
//get observations for the non-costmap controllers
std::vector<Observation> observations;
controller_cost_map_ros_->getMarkingObservations(observations);
- valid_control = tc_->computeVelocityCommands(global_plan_, cmd_vel, local_plan, observations);
+ valid_control = tc_->computeVelocityCommands(cmd_vel, observations);
}
else{
//we don't have a valid plan... so we want to stop
@@ -298,11 +228,6 @@
makePlan(goal_);
}
- //for visualization purposes
- publishPath(global_plan_, "gui_path", 0.0, 1.0, 0.0, 0.0);
- publishPath(local_plan, "local_path", 0.0, 0.0, 1.0, 0.0);
- publishFootprint();
-
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
@@ -339,47 +264,6 @@
controller_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
}
- void MoveBaseAction::publishFootprint(){
- double useless_pitch, useless_roll, yaw;
- global_pose_.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
- std::vector<robot_msgs::Point> footprint = tc_->drawFootprint(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw);
- robot_msgs::Polyline2D footprint_msg;
- footprint_msg.header.frame_id = global_frame_;
- footprint_msg.set_points_size(footprint.size());
- footprint_msg.color.r = 1.0;
- footprint_msg.color.g = 0;
- footprint_msg.color.b = 0;
- footprint_msg.color.a = 0;
- for(unsigned int i = 0; i < footprint.size(); ++i){
- footprint_msg.points[i].x = footprint[i].x;
- footprint_msg.points[i].y = footprint[i].y;
- }
- ros_node_.publish("robot_footprint", footprint_msg);
- }
-
- void MoveBaseAction::publishPath(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a){
- //given an empty path we won't do anything
- if(path.empty())
- return;
-
- // Extract the plan in world co-ordinates, we assume the path is all in the same frame
- robot_msgs::Polyline2D gui_path_msg;
- gui_path_msg.header.frame_id = path[0].header.frame_id;
- gui_path_msg.header.stamp = path[0].header.stamp;
- gui_path_msg.set_points_size(path.size());
- for(unsigned int i=0; i < path.size(); i++){
- gui_path_msg.points[i].x = path[i].pose.position.x;
- gui_path_msg.points[i].y = path[i].pose.position.y;
- }
-
- gui_path_msg.color.r = r;
- gui_path_msg.color.g = g;
- gui_path_msg.color.b = b;
- gui_path_msg.color.a = a;
-
- ros_node_.publish(topic, gui_path_msg);
- }
-
};
int main(int argc, char** argv){
Modified: pkg/trunk/highlevel/test_nav/launch_navstack.xml
===================================================================
--- pkg/trunk/highlevel/test_nav/launch_navstack.xml 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/highlevel/test_nav/launch_navstack.xml 2009-04-22 01:20:02 UTC (rev 14204)
@@ -3,6 +3,10 @@
<group name="wg">
<node pkg="nav" type="move_base" respawn="false" name="move_base_node" output="screen">
<remap from="/move_base_node/activate" to="/goal" />
+ <remap from="~base_local_planner/global_plan" to="/gui_path" />
+ <remap from="~base_local_planner/local_plan" to="/local_path" />
+ <remap from="~base_local_planner/robot_footprint" to="/robot_footprint" />
+
<param name="global_frame" value="map" />
<param name="robot_base_frame" value="base_link" />
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-22 01:20:02 UTC (rev 14204)
@@ -42,6 +42,7 @@
#include <costmap_2d/costmap_2d.h>
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/Point.h>
+#include <robot_msgs/Polyline2D.h>
#include <tf/transform_listener.h>
#include <vector>
#include <robot_msgs/Point.h>
@@ -55,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, const costmap_2d::Costmap2D& cost_map);
+ NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& cost_map);
/**
* @brief Given a goal pose in the world, compute a plan
@@ -79,15 +80,22 @@
*/
double getPointPotential(const robot_msgs::Point& world_point);
+ /**
+ * @brief Publish a path for visualization purposes
+ */
+ void publishPlan(const std::vector<robot_msgs::PoseStamped>& path, double r, double g, double b, double a);
+
~NavfnROS(){}
private:
+ void clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my);
ros::Node& ros_node_;
tf::TransformListener& tf_;
- const costmap_2d::Costmap2D& cost_map_;
+ costmap_2d::Costmap2D& cost_map_;
NavFn planner_;
std::string global_frame_, robot_base_frame_;
double transform_tolerance_; // timeout before transform errors
+ double inscribed_radius_, circumscribed_radius_, inflation_radius_;
};
};
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-22 01:20:02 UTC (rev 14204)
@@ -37,12 +37,20 @@
#include <navfn/navfn_ros.h>
namespace navfn {
- NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, const costmap_2d::Costmap2D& cost_map) : ros_node_(ros_node), tf_(tf),
+ 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()) {
+ //advertise our plan visualization
+ ros_node_.advertise<robot_msgs::Polyline2D>("~navfn/plan", 1);
+
//read parameters for the planner
ros_node_.param("~/navfn/global_frame", global_frame_, std::string("map"));
ros_node_.param("~/navfn/robot_base_frame", robot_base_frame_, std::string("base_link"));
ros_node_.param("~/navfn/transform_tolerance", transform_tolerance_, 0.1);
+
+ //we'll get the parameters for the robot radius from the costmap we're associated with
+ ros_node_.param("~navfn/costmap/inscribed_radius", inscribed_radius_, 0.325);
+ ros_node_.param("~navfn/costmap/circumscribed_radius", circumscribed_radius_, 0.46);
+ ros_node_.param("~navfn/costmap/inflation_radius", circumscribed_radius_, 0.55);
}
double NavfnROS::getPointPotential(const robot_msgs::Point& world_point){
@@ -75,6 +83,20 @@
return planner_.calcNavFnDijkstra();
}
+ void NavfnROS::clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my){
+ double useless_pitch, useless_roll, yaw;
+ 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);
+
+ 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);
+
+ }
+
bool NavfnROS::makePlan(const robot_msgs::PoseStamped& goal, std::vector<robot_msgs::PoseStamped>& plan){
//get the pose of the robot in the global frame
tf::Stamped<tf::Pose> robot_pose, global_pose, orig_goal, goal_pose;
@@ -109,12 +131,15 @@
double wx = global_pose.getOrigin().x();
double wy = global_pose.getOrigin().y();
- planner_.setCostMap(cost_map_.getCharMap());
-
unsigned int mx, my;
if(!cost_map_.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());
+
int map_start[2];
map_start[0] = mx;
map_start[1] = my;
@@ -157,6 +182,31 @@
}
}
+ //publish the plan for visualization purposes
+ publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
return success;
}
+
+ void NavfnROS::publishPlan(const std::vector<robot_msgs::PoseStamped>& path, double r, double g, double b, double a){
+ //given an empty path we won't do anything
+ if(path.empty())
+ return;
+
+ // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+ robot_msgs::Polyline2D gui_path_msg;
+ gui_path_msg.header.frame_id = path[0].header.frame_id;
+ gui_path_msg.header.stamp = path[0].header.stamp;
+ gui_path_msg.set_points_size(path.size());
+ for(unsigned int i=0; i < path.size(); i++){
+ gui_path_msg.points[i].x = path[i].pose.position.x;
+ gui_path_msg.points[i].y = path[i].pose.position.y;
+ }
+
+ gui_path_msg.color.r = r;
+ gui_path_msg.color.g = g;
+ gui_path_msg.color.b = b;
+ gui_path_msg.color.a = a;
+
+ ros_node_.publish("~navfn/plan", gui_path_msg);
+ }
};
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-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-22 01:20:02 UTC (rev 14204)
@@ -53,6 +53,7 @@
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/Point.h>
+#include <robot_msgs/Polyline2D.h>
#include <laser_scan/LaserScan.h>
#include <tf/message_notifier.h>
@@ -81,7 +82,7 @@
* @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,
- const costmap_2d::Costmap2D& cost_map, std::vector<robot_msgs::Point> footprint_spec,
+ costmap_2d::Costmap2D& cost_map, std::vector<robot_msgs::Point> footprint_spec,
const costmap_2d::Costmap2D* planner_map = NULL);
/**
@@ -100,18 +101,20 @@
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
- * @param orig_global_plan The plan to pass to the controller
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
- * @param local_plan Will be set from the points of the selected trajectory for display purposes
* @param observations A vector of updates from the robot's sensors in world space, is sometimes unused depending on the model
* @return True if a valid trajectory was found, false otherwise
*/
- bool computeVelocityCommands(const std::vector<robot_msgs::PoseStamped>& orig_global_plan,
- robot_msgs::PoseDot& cmd_vel,
- std::vector<robot_msgs::PoseStamped>& local_plan,
+ bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel,
const std::vector<costmap_2d::Observation>& observations = std::vector<costmap_2d::Observation>(0));
/**
+ * @brief Update the plan that the controller is following
+ * @param orig_global_plan The plan to pass to the controller
+ */
+ void updatePlan(const std::vector<robot_msgs::PoseStamped>& orig_global_plan);
+
+ /**
* @brief Returns the local goal the robot is pursuing
* @param x Will be set to the x position of the goal in world coordinates
* @param y Will be set to the y position of the goal in world coordinates
@@ -163,20 +166,45 @@
* @param y1 The first y point
* @param x2 The second x point
* @param y2 The second y point
- * @return
*/
double distance(double x1, double y1, double x2, double y2);
+ /**
+ * @brief Trim off parts of the global plan that are far enough behind the robot
+ * @param global_pose The pose of the robot in the global frame
+ * @param plan The plan to be pruned
+ */
+ void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<robot_msgs::PoseStamped>& plan);
+
+ /**
+ * @brief Publishes the footprint of the robot for visualization purposes
+ * @param global_pose The pose of the robot in the global frame
+ */
+ 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 cost_map The costmap to apply the clearing opertaion on
+ */
+ void clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, costmap_2d::Costmap2D& cost_map);
+
+ /**
+ * @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);
+
void baseScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message);
void tiltScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message);
void odomCallback();
-
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
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
+ 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
laser_scan::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
boost::recursive_mutex obs_lock_; ///< @brief Lock for accessing data in callbacks safely
@@ -188,7 +216,9 @@
std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
double rot_stopped_velocity_, trans_stopped_velocity_;
double xy_goal_tolerance_, yaw_goal_tolerance_, min_in_place_vel_th_;
+ double inscribed_radius_, circumscribed_radius_, inflation_radius_;
bool goal_reached_;
+ std::vector<robot_msgs::PoseStamped> global_plan_;
};
};
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-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-22 01:20:02 UTC (rev 14204)
@@ -46,8 +46,8 @@
namespace base_local_planner {
TrajectoryPlannerROS::TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- const Costmap2D& cost_map, std::vector<Point> footprint_spec, const Costmap2D* planner_map)
- : world_model_(NULL), tc_(NULL), base_scan_notifier_(NULL), tf_(tf), laser_scans_(2),
+ 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),
point_grid_(NULL), voxel_grid_(NULL), rot_stopped_velocity_(1e-2), trans_stopped_velocity_(1e-2), goal_reached_(true){
double acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity;
int vx_samples, vtheta_samples;
@@ -57,6 +57,11 @@
double max_vel_x, min_vel_x, max_vel_th, min_vel_th;
string world_model_type;
+ //adverstise the fact that we'll publish the robot footprint
+ ros_node.advertise<robot_msgs::Polyline2D>("~base_local_planner/robot_footprint", 1);
+ ros_node.advertise<robot_msgs::Polyline2D>("~base_local_planner/global_plan", 1);
+ ros_node.advertise<robot_msgs::Polyline2D>("~base_local_planner/local_plan", 1);
+
ros_node.param("~base_local_planner/global_frame", global_frame_, string("map"));
ros_node.param("~base_local_planner/robot_base_frame", robot_base_frame_, string("base_link"));
@@ -77,9 +82,10 @@
//"tilt_scan", global_frame_, 50);
"tilt_scan", global_frame_, 50);
- double inscribed_radius, circumscribed_radius;
- ros_node.param("~base_local_planner/inscribed_radius", inscribed_radius, 0.325);
- ros_node.param("~base_local_planner/circumscribed_radius", circumscribed_radius, 0.46);
+ //we'll get the parameters for the robot radius from the costmap we're associated with
+ 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);
+ ros_node.param("~base_local_planner/costmap/inflation_radius", circumscribed_radius_, 0.55);
ros_node.param("~base_local_planner/acc_lim_x", acc_lim_x, 2.5);
ros_node.param("~base_local_planner/acc_lim_y", acc_lim_y, 2.5);
@@ -148,7 +154,7 @@
ROS_INFO("Costmap\n");
}
- tc_ = new TrajectoryPlanner(*world_model_, cost_map, footprint_spec, inscribed_radius, circumscribed_radius,
+ tc_ = new TrajectoryPlanner(*world_model_, cost_map, 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_,
@@ -299,27 +305,42 @@
return goal_reached_;
}
- bool TrajectoryPlannerROS::computeVelocityCommands(const std::vector<robot_msgs::PoseStamped>& orig_global_plan,
- robot_msgs::PoseDot& cmd_vel,
- std::vector<robot_msgs::PoseStamped>& local_plan,
- const std::vector<costmap_2d::Observation>& observations){
+ void TrajectoryPlannerROS::updatePlan(const std::vector<robot_msgs::PoseStamped>& orig_global_plan){
+ //reset the global plan
+ global_plan_.clear();
+ //transform the plan into the frame of the controller
+ try{
+ for(unsigned int i = 0; i < orig_global_plan.size(); ++i){
+ robot_msgs::PoseStamped new_pose;
+ tf_.transformPose(global_frame_, orig_global_plan[i], new_pose);
+ global_plan_.push_back(new_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;
+ }
+ }
+
+ bool TrajectoryPlannerROS::computeVelocityCommands(robot_msgs::PoseDot& cmd_vel, const std::vector<costmap_2d::Observation>& observations){
+ std::vector<robot_msgs::PoseStamped> local_plan;
tf::Stamped<tf::Pose> robot_pose, global_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
robot_pose.stamp_ = ros::Time();
- std::vector<robot_msgs::PoseStamped> global_plan;
-
//get the global pose of the robot
try{
tf_.transformPose(global_frame_, robot_pose, global_pose);
- //transform the plan for the robot into the global frame for the controller
- for(unsigned int i = 0; i < orig_global_plan.size(); ++i){
- robot_msgs::PoseStamped new_pose;
- tf_.transformPose(global_frame_, orig_global_plan[i], new_pose);
- global_plan.push_back(new_pose);
- }
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
@@ -334,15 +355,18 @@
return false;
}
+ //now we'll prune the plan based on the position of the robot
+ prunePlan(global_pose, global_plan_);
+
+ //we also want to clear the robot footprint from the costmap we're using
+ clearRobotFootprint(global_pose, cost_map_);
+
// Set current velocities from odometry
robot_msgs::PoseDot global_vel;
global_vel.vel.vx = base_odom_.vel.x;
global_vel.vel.vy = base_odom_.vel.y;
global_vel.ang_vel.vz = base_odom_.vel.th;
-
- local_plan.clear();
-
tf::Stamped<tf::Pose> drive_cmds;
drive_cmds.frame_id_ = robot_base_frame_;
@@ -359,11 +383,11 @@
*/
//if the global plan passed in is empty... we won't do anything
- if(global_plan.empty())
+ if(global_plan_.empty())
return false;
tf::Stamped<tf::Pose> goal_point;
- tf::PoseStampedMsgToTF(global_plan.back(), goal_point);
+ tf::PoseStampedMsgToTF(global_plan_.back(), goal_point);
//we assume the global goal is the last point in the global plan
double goal_x = goal_point.getOrigin().getX();
double goal_y = goal_point.getOrigin().getY();
@@ -394,11 +418,17 @@
rotateToGoal(global_pose, goal_th, cmd_vel);
}
+ //publish the robot footprint and an empty plan because we've reached our goal position
+ global_plan_.clear();
+ publishFootprint(global_pose);
+ publishPlan(global_plan_, "global_plan", 0.0, 1.0, 0.0, 0.0);
+ publishPlan(local_plan, "local_plan", 0.0, 0.0, 1.0, 0.0);
+
//we don't actually want to run the controller when we're just rotating to goal
return true;
}
- tc_->updatePlan(global_plan);
+ tc_->updatePlan(global_plan_);
obs_lock_.lock();
//compute what trajectory to drive along
@@ -421,8 +451,13 @@
cmd_vel.ang_vel.vz = yaw;
//if we cannot move... tell someone
- if(path.cost_ < 0)
+ if(path.cost_ < 0){
+ local_plan.clear();
+ publishFootprint(global_pose);
+ publishPlan(global_plan_, "global_plan", 0.0, 1.0, 0.0, 0.0);
+ publishPlan(local_plan, "local_plan", 0.0, 0.0, 1.0, 0.0);
return false;
+ }
// Fill out the local plan
for(unsigned int i = 0; i < path.getPointsSize(); ++i){
@@ -451,10 +486,89 @@
}
*/
+ //publish information to the visualizer
+ publishFootprint(global_pose);
+ publishPlan(global_plan_, "global_plan", 0.0, 1.0, 0.0, 0.0);
+ publishPlan(local_plan, "local_plan", 0.0, 0.0, 1.0, 0.0);
return true;
}
void TrajectoryPlannerROS::getLocalGoal(double& x, double& y){
return tc_->getLocalGoal(x, y);
}
+
+ void TrajectoryPlannerROS::prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<robot_msgs::PoseStamped>& plan){
+ std::vector<robot_msgs::PoseStamped>::iterator it = plan.begin();
+ while(it != plan.end()){
+ const robot_msgs::PoseStamped& w = *it;
+ // Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
+ double x_diff = global_pose.getOrigin().x() - w.pose.position.x;
+ double y_diff = global_pose.getOrigin().y() - w.pose.position.y;
+ double distance = sqrt(x_diff * x_diff + y_diff * y_diff);
+ if(distance < 1){
+ ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.getOrigin().x(), global_pose.getOrigin().y(), w.pose.position.x, w.pose.position.y);
+ break;
+ }
+ it = plan.erase(it);
+ }
+ }
+
+ void TrajectoryPlannerROS::clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, Costmap2D& cost_map){
+ 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(!cost_map.setConvexPolygonCost(oriented_footprint, costmap_2d::FREE_SPACE))
+ return;
+
+ 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);
+
+ }
+
+ 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);
+ std::vector<robot_msgs::Point> footprint = drawFootprint(global_pose.getOrigin().x(), global_pose.getOrigin().y(), yaw);
+ robot_msgs::Polyline2D footprint_msg;
+ footprint_msg.header.frame_id = global_frame_;
+ footprint_msg.set_points_size(footprint.size());
+ footprint_msg.color.r = 1.0;
+ footprint_msg.color.g = 0;
+ footprint_msg.color.b = 0;
+ footprint_msg.color.a = 0;
+ for(unsigned int i = 0; i < footprint.size(); ++i){
+ footprint_msg.points[i].x = footprint[i].x;
+ footprint_msg.points[i].y = footprint[i].y;
+ }
+ ros_node_.publish("~base_local_planner/robot_footprint", footprint_msg);
+ }
+
+ void TrajectoryPlannerROS::publishPlan(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a){
+ //given an empty path we won't do anything
+ if(path.empty())
+ return;
+
+ // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+ robot_msgs::Polyline2D gui_path_msg;
+ gui_path_msg.header.frame_id = path[0].header.frame_id;
+ gui_path_msg.header.stamp = path[0].header.stamp;
+ gui_path_msg.set_points_size(path.size());
+ for(unsigned int i=0; i < path.size(); i++){
+ gui_path_msg.points[i].x = path[i].pose.position.x;
+ gui_path_msg.points[i].y = path[i].pose.position.y;
+ }
+
+ gui_path_msg.color.r = r;
+ gui_path_msg.color.g = g;
+ gui_path_msg.color.b = b;
+ gui_path_msg.color.a = a;
+
+ ros_node_.publish("~base_local_planner/" + topic, gui_path_msg);
+ }
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
