Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16847] pkg/trunk
|
From: <ei...@us...> - 2009-06-09 01:15:03
|
Revision: 16847
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16847&view=rev
Author: eitanme
Date: 2009-06-09 01:14:54 +0000 (Tue, 09 Jun 2009)
Log Message:
-----------
Beginning to clean up the navstack.
-TrajectoryPlannerROS no longer subscribes to base/tilt independently of its costmap
-Support for specifying the robot footprint on the parameter server
-Cleaned up the WorldModel interface, this removes support for the freespace controller until it can be updated accordingly (needs a rolling window version)
-Cleaned up the TrajectoryPlanner code a bit, still needs some more work, but getting there
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/highlevel/move_base_stage/move_base/base_local_planner_params.yaml
pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch
pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/point_grid.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/voxel_grid_model.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/world_model.h
pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/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:14:38 UTC (rev 16846)
+++ pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -94,8 +94,6 @@
*/
void makePlan(const robot_msgs::PoseStamped& goal);
- bool escape(double escape_dist, unsigned int max_attempts, const robot_msgs::PoseStamped& robot_pose);
-
/**
* @brief Publish a goal to the visualizer
* @param goal The goal to visualize
Modified: pkg/trunk/highlevel/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-09 01:14:54 UTC (rev 16847)
@@ -44,6 +44,11 @@
using namespace robot_actions;
namespace move_base {
+
+ double sign(double x){
+ return x < 0.0 ? -1.0 : 1.0;
+ }
+
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_costmap_ros_(NULL), controller_costmap_ros_(NULL),
@@ -69,25 +74,54 @@
robot_msgs::Point pt;
double padding;
ros_node_.param("~footprint_padding", padding, 0.01);
- //create a square footprint
- pt.x = inscribed_radius_ + padding;
- pt.y = -1 * (inscribed_radius_ + padding);
- footprint_.push_back(pt);
- pt.x = -1 * (inscribed_radius_ + padding);
- pt.y = -1 * (inscribed_radius_ + padding);
- footprint_.push_back(pt);
- pt.x = -1 * (inscribed_radius_ + padding);
- pt.y = inscribed_radius_ + padding;
- footprint_.push_back(pt);
- pt.x = inscribed_radius_ + padding;
- pt.y = inscribed_radius_ + padding;
- footprint_.push_back(pt);
- //give the robot a nose
- pt.x = circumscribed_radius_;
- pt.y = 0;
- footprint_.push_back(pt);
+ //grab the footprint from the parameter server if possible
+ XmlRpc::XmlRpcValue footprint_list;
+ if(ros_node_.getParam("~footprint", footprint_list)){
+ //make sure we have a list of lists
+ ROS_ASSERT_MSG(footprint_list.getType() == XmlRpcValue::TypeArray && footprint_list.size() > 2,
+ "The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
+ for(int i = 0; i < footprint_list.size(); ++i){
+ //make sure we have a list of lists of size 2
+ XmlRpc::XmlRpcValue point = footprint_list[i];
+ ROS_ASSERT_MSG(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2,
+ "The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
+ //make sure that the value we're looking at is either a double or an int
+ ROS_ASSERT(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+ pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]);
+ pt.x += sign(pt.x) * padding;
+
+ //make sure that the value we're looking at is either a double or an int
+ ROS_ASSERT(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+ pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]);
+ pt.y += sign(pt.y) * padding;
+
+ footprint_.push_back(pt);
+
+ }
+ }
+ else{
+ //if no explicit footprint is set on the param server... create a square footprint
+ pt.x = inscribed_radius_ + padding;
+ pt.y = -1 * (inscribed_radius_ + padding);
+ footprint_.push_back(pt);
+ pt.x = -1 * (inscribed_radius_ + padding);
+ pt.y = -1 * (inscribed_radius_ + padding);
+ footprint_.push_back(pt);
+ pt.x = -1 * (inscribed_radius_ + padding);
+ pt.y = inscribed_radius_ + padding;
+ footprint_.push_back(pt);
+ pt.x = inscribed_radius_ + padding;
+ pt.y = inscribed_radius_ + padding;
+ footprint_.push_back(pt);
+
+ //give the robot a nose
+ pt.x = circumscribed_radius_;
+ pt.y = 0;
+ footprint_.push_back(pt);
+ }
+
//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_);
@@ -101,7 +135,7 @@
controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_, &planner_costmap_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_);
//advertise a service for getting a plan
ros_node_.advertiseService("~make_plan", &MoveBase::planService, this);
@@ -297,35 +331,6 @@
lock_.unlock();
}
- bool MoveBase::escape(double escape_dist, unsigned int max_attempts, const robot_msgs::PoseStamped& robot_pose){
- ROS_DEBUG("Attempting to generate an escape goal %.2f meters away", escape_dist);
- unsigned int attempts = 0;
- ros::Rate r(controller_frequency_);
- robot_msgs::PoseStamped goal_pose = robot_pose;
- srand(time(0));
- valid_plan_ = false;
- while(!valid_plan_ && attempts < max_attempts){
- double angle = 2 * M_PI * rand() / RAND_MAX;
- double x_diff = escape_dist * cos(angle);
- double y_diff = escape_dist * sin(angle);
- goal_pose = robot_pose;
- goal_pose.pose.position.x += x_diff;
- goal_pose.pose.position.y += y_diff;
- makePlan(goal_pose);
- attempts++;
- r.sleep();
- }
-
- if(valid_plan_){
- ROS_DEBUG("Attempting to escape to point (%.2f, %.2f)", goal_pose.pose.position.x, goal_pose.pose.position.y);
- return true;
- }
-
- ROS_INFO("Could not find a valid escape point");
- return false;
- }
-
-
void MoveBase::getRobotPose(std::string frame, tf::Stamped<tf::Pose>& pose){
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
@@ -408,10 +413,8 @@
}
}
- //get observations for the non-costmap controllers
- std::vector<Observation> observations;
- controller_costmap_ros_->getMarkingObservations(observations);
- valid_control = tc_->computeVelocityCommands(cmd_vel, observations);
+ //compute velocity commands to send to the base
+ valid_control = tc_->computeVelocityCommands(cmd_vel);
ROS_DEBUG("Velocity commands produced by controller: vx: %.2f, vy: %.2f, vth: %.2f", cmd_vel.vel.vx, cmd_vel.vel.vy, cmd_vel.ang_vel.vz);
if(valid_control)
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:14:38 UTC (rev 16846)
+++ pkg/trunk/highlevel/move_base/src/move_base_local.cpp 2009-06-09 01:14:54 UTC (rev 16847)
@@ -92,7 +92,7 @@
//footprint_.push_back(pt);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_, &controller_costmap_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_);
}
MoveBaseLocal::~MoveBaseLocal(){
@@ -197,11 +197,10 @@
}
bool valid_control = false;
- //get observations for the non-costmap controllers
- std::vector<Observation> observations;
- controller_costmap_ros_->getMarkingObservations(observations);
- valid_control = tc_->computeVelocityCommands(cmd_vel, observations, false);
+ //compute veloctiy commands to send to the base... don't prune the path
+ valid_control = tc_->computeVelocityCommands(cmd_vel, false);
+
//give the base the velocity command
ros_node_.publish("cmd_vel", cmd_vel);
Modified: pkg/trunk/highlevel/move_base_stage/move_base/base_local_planner_params.yaml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base/base_local_planner_params.yaml 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/highlevel/move_base_stage/move_base/base_local_planner_params.yaml 2009-06-09 01:14:54 UTC (rev 16847)
@@ -1,3 +1,4 @@
+footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.325, -0.325]]
base_local_planner:
#Independent settings for the local costmap
costmap:
Modified: pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch 2009-06-09 01:14:54 UTC (rev 16847)
@@ -4,7 +4,7 @@
<param name="/use_sim_time" value="true"/>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
- <param name="footprint_padding" value="0.02" />
+ <param name="footprint_padding" value="0.01" />
<rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="navfn" />
<rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
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-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -72,15 +72,6 @@
virtual double footprintCost(const robot_msgs::Point& position, const std::vector<robot_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
- /**
- * @brief The costmap already keeps track of world observations, so for this world model this method does nothing
- * @param footprint The footprint of the robot in its current location
- * @param observations The observations from various sensors
- * @param laser_scans The scans used to clear freespace
- */
- virtual void updateWorld(const std::vector<robot_msgs::Point>& footprint,
- const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans) {}
-
private:
/**
* @brief Rasterizes a line in the costmap grid and checks for collisions
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/point_grid.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/point_grid.h 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/point_grid.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -99,7 +99,7 @@
* @param observations The observations from various sensors
* @param laser_scans The laser scans used to clear freespace (the point grid only uses the first scan which is assumed to be the base laser)
*/
- virtual void updateWorld(const std::vector<robot_msgs::Point>& footprint,
+ void updateWorld(const std::vector<robot_msgs::Point>& footprint,
const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
/**
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-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -131,14 +131,10 @@
* @param global_pose The current pose of the robot in world space
* @param global_vel The current velocity of the robot in world space
* @param drive_velocities Will be set to velocities to send to the robot base
- * @param observations A vector of updates from the robot's sensors in world space, is sometimes unused depending on the model
- * @param laser_scans The latest scans taken... used to clear freespace in front of the robot depending on the model
* @return The selected path or trajectory
*/
Trajectory findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
- tf::Stamped<tf::Pose>& drive_velocities,
- std::vector<costmap_2d::Observation> observations = std::vector<costmap_2d::Observation>(0),
- std::vector<PlanarLaserScan> laser_scans = std::vector<PlanarLaserScan>(0));
+ tf::Stamped<tf::Pose>& drive_velocities);
/**
* @brief Update the plan that the controller is following
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:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -37,9 +37,10 @@
#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
-#include <ros/node.h>
+#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_publisher.h>
+#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/point_grid.h>
#include <base_local_planner/costmap_model.h>
@@ -79,11 +80,9 @@
* @param tf A reference to a transform listener
* @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& costmap, std::vector<robot_msgs::Point> footprint_spec,
- const costmap_2d::Costmap2D* planner_map = NULL);
+ costmap_2d::Costmap2D& costmap, std::vector<robot_msgs::Point> footprint_spec);
/**
* @brief Destructor for the wrapper
@@ -106,8 +105,7 @@
* @param prune_plan Set to true if you would like the plan to be pruned as the robot drives, false to leave the plan as is
* @return True if a valid trajectory was found, false otherwise
*/
- bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel,
- const std::vector<costmap_2d::Observation>& observations = std::vector<costmap_2d::Observation>(0), bool prune_plan = true);
+ bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel, bool prune_plan = true);
/**
* @brief Update the plan that the controller is following
@@ -211,23 +209,14 @@
*/
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& 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
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
- std::vector<PlanarLaserScan> laser_scans_; ///< @breif Storage for the last scan the lasers took... used for clearing free-space in front of the robot
- PointGrid* point_grid_; ///< @brief If using a freespace grid... we want to access it
- VoxelGridModel* voxel_grid_; ///< @brief If using a voxel grid... we want to access it
double max_sensor_range_; ///< @brief Keep track of the effective maximum range of our sensors
deprecated_msgs::RobotBase2DOdom odom_msg_, base_odom_; ///< @brief Used to get the velocity of the robot
std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/voxel_grid_model.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/voxel_grid_model.h 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/voxel_grid_model.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -93,7 +93,7 @@
* @param observations The observations from various sensors
* @param laser_scan The scans used to clear freespace
*/
- virtual void updateWorld(const std::vector<robot_msgs::Point>& footprint,
+ void updateWorld(const std::vector<robot_msgs::Point>& footprint,
const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
void getPoints(robot_msgs::PointCloud& cloud);
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/world_model.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/world_model.h 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/world_model.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -51,15 +51,6 @@
class WorldModel{
public:
/**
- * @brief Subclass will implement this method to insert observations from sensors into its world model
- * @param footprint The footprint of the robot in its current location
- * @param observations The observations from various sensors
- * @param laser_scans The planar laser scans used to clear freespace
- */
- virtual void updateWorld(const std::vector<robot_msgs::Point>& footprint,
- const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans) = 0;
-
- /**
* @brief Subclass will implement this method to check a footprint at a given position and orientation for legality in the world
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-06-09 01:14:54 UTC (rev 16847)
@@ -471,7 +471,8 @@
double vtheta, double vx_samp, double vy_samp, double vtheta_samp){
Trajectory t;
double impossible_cost = map_.map_.size();
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_lim_x_, acc_lim_y_, acc_lim_theta_, impossible_cost, t);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_lim_x_, acc_lim_y_, acc_lim_theta_, impossible_cost, t);
//if the trajectory is a legal one... the check passes
if(t.cost_ >= 0)
@@ -482,7 +483,8 @@
}
//create the trajectories we wish to score
- Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta,
+ Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,
+ double vx, double vy, double vtheta,
double acc_x, double acc_y, double acc_theta){
//compute feasible velocity limits in robot space
double max_vel_x, max_vel_theta;
@@ -531,7 +533,8 @@
for(int i = 0; i < vx_samples_; ++i){
vtheta_samp = 0;
//first sample the straight trajectory
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
@@ -543,7 +546,8 @@
vtheta_samp = min_vel_theta;
//next sample all theta trajectories
for(int j = 0; j < vtheta_samples_ - 1; ++j){
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
@@ -562,7 +566,8 @@
vx_samp = 0.1;
vy_samp = 0.1;
vtheta_samp = 0.0;
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
@@ -574,7 +579,8 @@
vx_samp = 0.1;
vy_samp = -0.1;
vtheta_samp = 0.0;
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
@@ -595,12 +601,17 @@
for(int i = 0; i < vtheta_samples_; ++i){
//enforce a minimum rotational velocity because the base can't handle small in-place rotations
- double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_) : min(vtheta_samp, -1.0 * min_in_place_vel_th_);
+ double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_)
+ : min(vtheta_samp, -1.0 * min_in_place_vel_th_);
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
- //if the new trajectory is better... let's take it... note if we can legally rotate in place we prefer to do that rather than move with y velocity
- if(comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0) && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
+ //if the new trajectory is better... let's take it...
+ //note if we can legally rotate in place we prefer to do that rather than move with y velocity
+ if(comp_traj->cost_ >= 0
+ && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0)
+ && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
double x_r, y_r, th_r;
comp_traj->getEndpoint(x_r, y_r, th_r);
x_r += heading_lookahead_ * cos(th_r);
@@ -698,7 +709,8 @@
vtheta_samp = 0;
vy_samp = y_vels_[i];
//sample completely horizontal trajectories
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0)){
@@ -790,7 +802,8 @@
vtheta_samp = 0.0;
vx_samp = -0.1;
vy_samp = 0.0;
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
/*
@@ -843,8 +856,7 @@
//given the current state of the robot, find a good trajectory
Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
- tf::Stamped<tf::Pose>& drive_velocities, vector<costmap_2d::Observation> observations,
- vector<PlanarLaserScan> laser_scans){
+ tf::Stamped<tf::Pose>& drive_velocities){
double uselessPitch, uselessRoll, yaw, velYaw;
global_pose.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
@@ -858,20 +870,6 @@
double vy = global_vel.getOrigin().getY();
double vtheta = velYaw;
- //build the oriented footprint at the robot's current location
- double cos_th = cos(theta);
- double sin_th = sin(theta);
- vector<Point> oriented_footprint;
- for(unsigned int i = 0; i < footprint_spec_.size(); ++i){
- Point new_pt;
- new_pt.x = x + (footprint_spec_[i].x * cos_th - footprint_spec_[i].y * sin_th);
- new_pt.y = y + (footprint_spec_[i].x * sin_th + footprint_spec_[i].y * cos_th);
- oriented_footprint.push_back(new_pt);
- }
-
- //update the point grid with new observations
- world_model_.updateWorld(oriented_footprint, observations, laser_scans);
-
//reset the map for new operations
map_.resetPathDist();
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:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-06-09 01:14:54 UTC (rev 16847)
@@ -48,9 +48,9 @@
namespace base_local_planner {
TrajectoryPlannerROS::TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- 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){
+ Costmap2D& costmap, std::vector<Point> footprint_spec)
+ : world_model_(NULL), tc_(NULL), costmap_(costmap), 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;
double pdist_scale, gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta;
@@ -89,15 +89,6 @@
// Subscribe to odometry messages to get global pose
ros_node.subscribe(odom_topic, odom_msg_, &TrajectoryPlannerROS::odomCallback, this, 1);
- base_scan_notifier_ = new tf::MessageNotifier<LaserScan>(&tf_, &ros_node,
- boost::bind(&TrajectoryPlannerROS::baseScanCallback, this, _1),
- "base_scan", global_frame_, 50);
-
- tilt_scan_notifier_ = new tf::MessageNotifier<LaserScan>(&tf_, &ros_node,
- boost::bind(&TrajectoryPlannerROS::tiltScanCallback, this, _1),
- //"tilt_scan", global_frame_, 50);
- "tilt_scan", global_frame_, 50);
-
//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);
@@ -123,7 +114,7 @@
ros_node.param("~base_local_planner/max_vel_th", max_vel_th, 1.0);
ros_node.param("~base_local_planner/min_vel_th", min_vel_th, -1.0);
ros_node.param("~base_local_planner/min_in_place_vel_th", min_in_place_vel_th_, 0.4);
- ros_node.param("~base_local_planner/world_model", world_model_type, string("freespace"));
+ ros_node.param("~base_local_planner/world_model", world_model_type, string("costmap"));
ros_node.param("~base_local_planner/dwa", dwa, false);
ros_node.param("~base_local_planner/heading_scoring", heading_scoring, false);
ros_node.param("~base_local_planner/heading_scoring_timestep", heading_scoring_timestep, 0.1);
@@ -136,41 +127,8 @@
ros_node.param("~base_local_planner/point_grid/max_obstacle_height", max_obstacle_height, 2.0);
ros_node.param("~base_local_planner/point_grid/grid_resolution", grid_resolution, 0.2);
- if(world_model_type == "freespace"){
- ROS_ASSERT_MSG(planner_map != NULL, "Until a rolling window version of the freespace controller is implemented... the costmap the planner uses must be passed in for sizing info.");
- Point origin;
- origin.x = planner_map->originX();
- origin.y = planner_map->originY();
- unsigned int cmap_width, cmap_height;
- cmap_width = planner_map->cellSizeX();
- cmap_height = planner_map->cellSizeY();
- point_grid_ = new PointGrid(cmap_width * planner_map->resolution(), cmap_height * planner_map->resolution(), grid_resolution,
- origin, max_obstacle_height, max_sensor_range_, min_pt_separation);
- world_model_ = point_grid_;
- ROS_DEBUG("Freespace Origin: (%.4f, %.4f), Width: %.4f, Height: %.4f\n", origin.x, origin.y, cmap_width * planner_map->resolution(), cmap_height * planner_map->resolution());
- /*For Debugging
- ros_node.advertise<PointCloud>("point_grid", 1);
- */
- }
- else if(world_model_type == "voxel"){
- ROS_ASSERT_MSG(planner_map != NULL, "Until a rolling window version of the voxel controller is implemented... the costmap the planner uses must be passed in for sizing info.");
- double origin_x, origin_y;
- origin_x = planner_map->originX();
- origin_y = planner_map->originY();
- unsigned int cmap_width, cmap_height;
- cmap_width = planner_map->cellSizeX();
- cmap_height = planner_map->cellSizeY();
- voxel_grid_ = new VoxelGridModel(cmap_width, cmap_height, 10, planner_map->resolution(), max_obstacle_height / 10,
- origin_x, origin_y, 0.0, max_obstacle_height, max_sensor_range_);
- world_model_ = voxel_grid_;
- /*For Debugging
- ros_node.advertise<PointCloud>("point_grid", 1);
- */
- }
- else{
- world_model_ = new CostmapModel(costmap);
- ROS_INFO("Costmap\n");
- }
+ ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");
+ world_model_ = new CostmapModel(costmap);
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,
@@ -179,80 +137,10 @@
dwa, heading_scoring, heading_scoring_timestep, simple_attractor);
}
- void TrajectoryPlannerROS::baseScanCallback(const tf::MessageNotifier<LaserScan>::MessagePtr& message){
- //project the laser into a point cloud
- PointCloud base_cloud;
- base_cloud.header = message->header;
- //we want all values... even those out of range
- projector_.projectLaser(*message, base_cloud, -1.0, true);
- tf::Stamped<btVector3> global_origin;
-
- obs_lock_.lock();
- laser_scans_[0].angle_min = message->angle_min;
- laser_scans_[0].angle_max = message->angle_max;
- laser_scans_[0].angle_increment = message->angle_increment;
-
- //we know the transform is available from the laser frame to the global frame
- try{
- //transform the origin for the sensor
- tf::Stamped<btVector3> local_origin(btVector3(0, 0, 0), base_cloud.header.stamp, base_cloud.header.frame_id);
- tf_.transformPoint(global_frame_, local_origin, global_origin);
- laser_scans_[0].origin.x = global_origin.getX();
- laser_scans_[0].origin.y = global_origin.getY();
- laser_scans_[0].origin.z = global_origin.getZ();
-
- //transform the point cloud
- tf_.transformPointCloud(global_frame_, base_cloud, laser_scans_[0].cloud);
- }
- catch(tf::TransformException& ex){
- ROS_ERROR("TF Exception that should never happen %s", ex.what());
- return;
- }
- obs_lock_.unlock();
- }
-
- void TrajectoryPlannerROS::tiltScanCallback(const tf::MessageNotifier<LaserScan>::MessagePtr& message){
- //project the laser into a point cloud
- PointCloud tilt_cloud;
- tilt_cloud.header = message->header;
- //we want all values... even those out of range
- projector_.projectLaser(*message, tilt_cloud, -1.0, true);
- tf::Stamped<btVector3> global_origin;
-
- obs_lock_.lock();
- laser_scans_[1].angle_min = message->angle_min;
- laser_scans_[1].angle_max = message->angle_max;
- laser_scans_[1].angle_increment = message->angle_increment;
-
- //we know the transform is available from the laser frame to the global frame
- try{
- //transform the origin for the sensor
- tf::Stamped<btVector3> local_origin(btVector3(0, 0, 0), tilt_cloud.header.stamp, tilt_cloud.header.frame_id);
- tf_.transformPoint(global_frame_, local_origin, global_origin);
- laser_scans_[1].origin.x = global_origin.getX();
- laser_scans_[1].origin.y = global_origin.getY();
- laser_scans_[1].origin.z = global_origin.getZ();
-
- //transform the point cloud
- tf_.transformPointCloud(global_frame_, tilt_cloud, laser_scans_[1].cloud);
- }
- catch(tf::TransformException& ex){
- ROS_ERROR("TF Exception that should never happen %s", ex.what());
- return;
- }
- obs_lock_.unlock();
- }
-
TrajectoryPlannerROS::~TrajectoryPlannerROS(){
if(costmap_publisher_ != NULL)
delete costmap_publisher_;
- if(base_scan_notifier_ != NULL)
- delete base_scan_notifier_;
-
- if(tilt_scan_notifier_ != NULL)
- delete tilt_scan_notifier_;
-
if(tc_ != NULL)
delete tc_;
@@ -403,8 +291,7 @@
return true;
}
- bool TrajectoryPlannerROS::computeVelocityCommands(robot_msgs::PoseDot& cmd_vel,
- const std::vector<costmap_2d::Observation>& observations, bool prune_plan){
+ bool TrajectoryPlannerROS::computeVelocityCommands(robot_msgs::PoseDot& cmd_vel, bool prune_plan){
//assume at the beginning of our control cycle that we could have a new goal
goal_reached_ = false;
@@ -509,10 +396,8 @@
else {
tc_->updatePlan(transformed_plan);
- obs_lock_.lock();
//compute what trajectory to drive along
- Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds, observations, laser_scans_);
- obs_lock_.unlock();
+ Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
if(!rotateToGoal(global_pose, robot_vel, goal_th, cmd_vel))
return false;
}
@@ -528,10 +413,8 @@
tc_->updatePlan(transformed_plan);
- obs_lock_.lock();
//compute what trajectory to drive along
- Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds, observations, laser_scans_);
- obs_lock_.unlock();
+ Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
/* For timing uncomment
gettimeofday(&end, NULL);
@@ -568,22 +451,6 @@
local_plan.push_back(pose);
}
- /* For Debugging
- if(point_grid_ != NULL){
- PointCloud grid_cloud;
- grid_cloud.header.frame_id = global_frame_;
- point_grid_->getPoints(grid_cloud);
- ros::Node::instance()->publish("point_grid", grid_cloud);
- }
-
- if(voxel_grid_ != NULL){
- PointCloud grid_cloud;
- grid_cloud.header.frame_id = global_frame_;
- voxel_grid_->getPoints(grid_cloud);
- ros::Node::instance()->publish("point_grid", grid_cloud);
- }
- */
-
//publish information to the visualizer
publishFootprint(global_pose);
publishPlan(transformed_plan, "global_plan", 0.0, 1.0, 0.0, 0.0);
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:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-06-09 01:14:54 UTC (rev 16847)
@@ -103,7 +103,7 @@
controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_, &planner_costmap_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_);
//initially clear any unknown space around the robot
planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
@@ -270,9 +270,7 @@
}
//get observations for the non-costmap controllers
- std::vector<Observation> observations;
- controller_costmap_ros_->getMarkingObservations(observations);
- valid_control = tc_->computeVelocityCommands(cmd_vel, observations);
+ valid_control = tc_->computeVelocityCommands(cmd_vel);
if(valid_control)
last_valid_control_ = ros::Time::now();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
