diff --git a/base_local_planner/include/base_local_planner/local_planner_util.h b/base_local_planner/include/base_local_planner/local_planner_util.h index fd21edbfc2..ad422cb082 100644 --- a/base_local_planner/include/base_local_planner/local_planner_util.h +++ b/base_local_planner/include/base_local_planner/local_planner_util.h @@ -61,7 +61,7 @@ class LocalPlannerUtil { std::string name_; std::string global_frame_; - costmap_2d::Costmap2D* costmap_; + costmap_2d::LayeredCostmap* layered_costmap_; tf2_ros::Buffer* tf_; @@ -86,8 +86,7 @@ class LocalPlannerUtil { ~LocalPlannerUtil() { } - void initialize(tf2_ros::Buffer* tf, - costmap_2d::Costmap2D* costmap, + void initialize(tf2_ros::Buffer* tf, costmap_2d::LayeredCostmap* layered_costmap, std::string global_frame); bool getGoal(geometry_msgs::PoseStamped& goal_pose); @@ -98,7 +97,9 @@ class LocalPlannerUtil { bool getLocalPlan(const geometry_msgs::PoseStamped& global_pose, std::vector& transformed_plan); - costmap_2d::Costmap2D* getCostmap(); + costmap_2d::Costmap2D* getCostmap(double t = 0); + + costmap_2d::LayeredCostmap* getLayeredCostmap(); LocalPlannerLimits getCurrentLimits(); diff --git a/base_local_planner/include/base_local_planner/obstacle_cost_function.h b/base_local_planner/include/base_local_planner/obstacle_cost_function.h index c36fe31f5a..5b5ffa613c 100644 --- a/base_local_planner/include/base_local_planner/obstacle_cost_function.h +++ b/base_local_planner/include/base_local_planner/obstacle_cost_function.h @@ -42,6 +42,7 @@ #include #include +#include namespace base_local_planner { @@ -53,8 +54,7 @@ namespace base_local_planner { class ObstacleCostFunction : public TrajectoryCostFunction { public: - ObstacleCostFunction(costmap_2d::Costmap2D* costmap); - ~ObstacleCostFunction(); + ObstacleCostFunction(costmap_2d::LayeredCostmap* layered_costmap); ExePathOutcome prepare(const geometry_msgs::PoseStamped& current_pose); double scoreTrajectory(Trajectory &traj); @@ -72,13 +72,12 @@ class ObstacleCostFunction : public TrajectoryCostFunction { const double& y, const double& th, const std::vector& scaled_footprint, - costmap_2d::Costmap2D* costmap, - base_local_planner::WorldModel* world_model); + costmap_2d::LayeredCostmap* layered_costmap, + double t = 0.0); private: - costmap_2d::Costmap2D* costmap_; + costmap_2d::LayeredCostmap* layered_costmap_; std::vector footprint_spec_; - base_local_planner::WorldModel* world_model_; double max_trans_vel_; bool sum_scores_; //footprint scaling with velocity; diff --git a/base_local_planner/src/local_planner_util.cpp b/base_local_planner/src/local_planner_util.cpp index fa4b1e2ba0..b4e1a53ea8 100644 --- a/base_local_planner/src/local_planner_util.cpp +++ b/base_local_planner/src/local_planner_util.cpp @@ -43,11 +43,11 @@ namespace base_local_planner { void LocalPlannerUtil::initialize( tf2_ros::Buffer* tf, - costmap_2d::Costmap2D* costmap, + costmap_2d::LayeredCostmap* layered_costmap, std::string global_frame) { if(!initialized_) { tf_ = tf; - costmap_ = costmap; + layered_costmap_ = layered_costmap; global_frame_ = global_frame; initialized_ = true; } @@ -70,8 +70,12 @@ void LocalPlannerUtil::reconfigureCB(LocalPlannerLimits &config, bool restore_de limits_ = LocalPlannerLimits(config); } -costmap_2d::Costmap2D* LocalPlannerUtil::getCostmap() { - return costmap_; +costmap_2d::Costmap2D* LocalPlannerUtil::getCostmap(double t) { + return layered_costmap_->getCostmap(t); +} + +costmap_2d::LayeredCostmap* LocalPlannerUtil::getLayeredCostmap() { + return layered_costmap_; } LocalPlannerLimits LocalPlannerUtil::getCurrentLimits() { @@ -108,7 +112,7 @@ bool LocalPlannerUtil::getLocalPlan(const geometry_msgs::PoseStamped& global_pos *tf_, global_plan_, global_pose, - *costmap_, + *layered_costmap_->getCostmap(), global_frame_, transformed_plan)) { ROS_WARN("Could not transform the global plan to the frame of the controller"); diff --git a/base_local_planner/src/obstacle_cost_function.cpp b/base_local_planner/src/obstacle_cost_function.cpp index 8688f041f7..215bb8513d 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -44,11 +44,8 @@ namespace base_local_planner { -ObstacleCostFunction::ObstacleCostFunction(costmap_2d::Costmap2D* costmap) - : costmap_(costmap), sum_scores_(false), sideward_inflation_scale_(1.0) { - if (costmap != NULL) { - world_model_ = new base_local_planner::CostmapModel(*costmap_); - } +ObstacleCostFunction::ObstacleCostFunction(costmap_2d::LayeredCostmap* layered_costmap) + : layered_costmap_(layered_costmap), sum_scores_(false), sideward_inflation_scale_(1.0) { ros::NodeHandle pnh("~"); sideward_inflation_scale_sub_ = pnh.subscribe("sideward_inflation_scale", 1, [&](const std_msgs::Float32ConstPtr& msg){ @@ -56,12 +53,6 @@ ObstacleCostFunction::ObstacleCostFunction(costmap_2d::Costmap2D* costmap) }); } -ObstacleCostFunction::~ObstacleCostFunction() { - if (world_model_ != NULL) { - delete world_model_; - } -} - void ObstacleCostFunction::setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation, double scaling_speed, bool occdist_use_footprint) { // TODO: move this to prepare if possible @@ -119,7 +110,7 @@ double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) { traj.getPoint(i, px, py, pth); double f_cost = footprintCost(px, py, pth, scaled_footprint, - costmap_, world_model_); + layered_costmap_, i * traj.time_delta_); if(f_cost < 0){ return f_cost; @@ -148,13 +139,20 @@ double ObstacleCostFunction::footprintCost ( const double& y, const double& th, const std::vector& scaled_footprint, - costmap_2d::Costmap2D* costmap, - base_local_planner::WorldModel* world_model) { + costmap_2d::LayeredCostmap* layered_costmap, + double t) { //check if the footprint is legal // TODO: Cache inscribed radius - double footprint_cost = world_model->footprintCost(x, y, th, scaled_footprint); + costmap_2d::Costmap2D* costmap = layered_costmap->getCostmap(t); + if (costmap == NULL) { + return -10.0; // What to return ??????????? + } + + base_local_planner::CostmapModel world_model(*costmap); + double footprint_cost = world_model.footprintCost(x, y, th, scaled_footprint); + if (footprint_cost < 0) { return -6.0; } diff --git a/costmap_2d/include/costmap_2d/costmap_2d_ros.h b/costmap_2d/include/costmap_2d/costmap_2d_ros.h index d4cbc36d61..580722b731 100644 --- a/costmap_2d/include/costmap_2d/costmap_2d_ros.h +++ b/costmap_2d/include/costmap_2d/costmap_2d_ros.h @@ -267,6 +267,8 @@ class Costmap2DROS pluginlib::ClassLoader plugin_loader_; geometry_msgs::PoseStamped old_pose_; Costmap2DPublisher* publisher_; + Costmap2DPublisher* timed_publisher_; + double timestep_, prediction_time_; dynamic_reconfigure::Server *dsrv_; boost::recursive_mutex configuration_mutex_; diff --git a/costmap_2d/include/costmap_2d/layer.h b/costmap_2d/include/costmap_2d/layer.h index 79f57ceb11..1b8f37a07e 100644 --- a/costmap_2d/include/costmap_2d/layer.h +++ b/costmap_2d/include/costmap_2d/layer.h @@ -64,12 +64,53 @@ class Layer virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) {} + /** + * @brief Modified updateBounds() to use time argument t for timed layers. + * Calls the above function without t by default. + */ + virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y, double t) + { + updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); + } + /** * @brief Actually update the underlying costmap, only within the bounds * calculated during UpdateBounds(). */ virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {} + /** + * @brief Modified updateCosts() to use time argument t for timed layers. + * Calls the above function without t by default. + */ + virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j, double t) + { + updateCosts(master_grid, min_i, min_j, max_i, max_j); + } + + /** + * @brief Returns true for layers that have a timed logic. + */ + bool isTimed() const + { + return timed_; + } + + /** + * @brief Returns true for layers that are time dependent but don't have a timed logic yet. + * These layers should not be part of the static costmap but should be painted in the + * first timed costmap (where t = 0). + * This includes layers like the obstacle or stvl layer, which paint observations that + * might include dynamic obstacles and are thus timed but should not be painted in + * every timestep. + * To-Do: Change logic and remove this function (ideally make all layers either timed or not timed) + */ + bool paintOnlyCurrentTime() const + { + return paint_only_current_time_; + } + /** @brief Stop publishers. */ virtual void deactivate() {} @@ -139,6 +180,7 @@ class Layer LayeredCostmap* layered_costmap_; bool current_; bool enabled_; + bool timed_, paint_only_current_time_; std::string name_; tf2_ros::Buffer *tf_; diff --git a/costmap_2d/include/costmap_2d/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index 72a8bdd2df..47a0387de3 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -56,9 +56,9 @@ class LayeredCostmap { public: /** - * @brief Constructor for a costmap + * @brief Constructor for a timed costmap */ - LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown); + LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown, double prediction_time = 0.0, double timestep = 0.0); /** * @brief Destructor @@ -79,19 +79,27 @@ class LayeredCostmap void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked = false); - void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy) + void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy, double t = 0) { - minx = minx_; - miny = miny_; - maxx = maxx_; - maxy = maxy_; + unsigned int i = getTimeIndex(t); + minx = timed_bounds_[i].minx; + miny = timed_bounds_[i].miny; + maxx = timed_bounds_[i].maxx; + maxy = timed_bounds_[i].maxy; } bool isCurrent(); - Costmap2D* getCostmap() + costmap_2d::Costmap2D* getCostmap(double t = 0.0); + + unsigned int getTimeIndex(double t) { - return &costmap_; + unsigned int i; + if (timestep_ <= 0 || prediction_time_ <= 0) + i = 0; + else + i = std::min((int)timed_bounds_.size()-1, (int)round(t/timestep_)); + return i; } bool isRolling() @@ -101,7 +109,7 @@ class LayeredCostmap bool isTrackingUnknown() { - return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION; + return getCostmap()->getDefaultValue() == costmap_2d::NO_INFORMATION; } std::vector >* getPlugins() @@ -119,12 +127,13 @@ class LayeredCostmap return size_locked_; } - void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn) + void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn, double t = 0.0) { - *x0 = bx0_; - *xn = bxn_; - *y0 = by0_; - *yn = byn_; + unsigned int i = getTimeIndex(t); + *x0 = timed_bounds_[i].bx0; + *xn = timed_bounds_[i].bxn; + *y0 = timed_bounds_[i].by0; + *yn = timed_bounds_[i].byn; } bool isInitialized() @@ -154,15 +163,63 @@ class LayeredCostmap * This is updated by setFootprint(). */ double getInscribedRadius() { return inscribed_radius_; } +protected: + /** + * ! Function copied from Costmap2D ! + * To-Do: Modify function for this use-case + * @brief Copy a region of a source map into a destination map + * @param source_map The source map + * @param sm_lower_left_x The lower left x point of the source map to start the copy + * @param sm_lower_left_y The lower left y point of the source map to start the copy + * @param sm_size_x The x size of the source map + * @param dest_map The destination map + * @param dm_lower_left_x The lower left x point of the destination map to start the copy + * @param dm_lower_left_y The lower left y point of the destination map to start the copy + * @param dm_size_x The x size of the destination map + * @param region_size_x The x size of the region to copy + * @param region_size_y The y size of the region to copy + */ + template + void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, + unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x, + unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, + unsigned int region_size_y) + { + // we'll first need to compute the starting points for each map + data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x); + data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x); + + // now, we'll copy the source map into the destination map + for (unsigned int i = 0; i < region_size_y; ++i) + { + memcpy(dm_index, sm_index, region_size_x * sizeof(data_type)); + sm_index += sm_size_x; + dm_index += dm_size_x; + } + } + private: - Costmap2D costmap_; + + // Struct to store the bounds for each timed costmaps + struct Costmap2DBounds + { + double minx, miny, maxx, maxy; + unsigned int bx0, bxn, by0, byn; + }; + + std::vector timed_costmaps_; + std::vector timed_bounds_; + + Costmap2D static_costmap_; std::string global_frame_; bool rolling_window_; /// < @brief Whether or not the costmap should roll with the robot bool current_; - double minx_, miny_, maxx_, maxy_; - unsigned int bx0_, bxn_, by0_, byn_; + + double static_minx_, static_miny_, static_maxx_, static_maxy_; // Bounds for static costmap + int static_bx0_, static_bxn_, static_by0_, static_byn_; + double timestep_, prediction_time_; std::vector > plugins_; diff --git a/costmap_2d/plugins/obstacle_layer.cpp b/costmap_2d/plugins/obstacle_layer.cpp index 76e883e673..147d825b67 100644 --- a/costmap_2d/plugins/obstacle_layer.cpp +++ b/costmap_2d/plugins/obstacle_layer.cpp @@ -69,6 +69,14 @@ void ObstacleLayer::onInitialize() ObstacleLayer::matchSize(); current_ = true; + // The obstacle layer is considered time dependent as it paints also dynamic obstacles that are + // observed by sensord. However, it does not yet have a timed logic, so we don't want to paint these + // Observations in all timed snapshots + // To-Do: Add timed logic (--> Dynamic Obstacle Layer) + // How to implement this depends on how perception component provides information... + timed_ = true; + paint_only_current_time_ = true; + global_frame_ = layered_costmap_->getGlobalFrameID(); double transform_tolerance; nh.param("transform_tolerance", transform_tolerance, 0.2); diff --git a/costmap_2d/src/costmap_2d_ros.cpp b/costmap_2d/src/costmap_2d_ros.cpp index 59196412e9..78d4c17415 100644 --- a/costmap_2d/src/costmap_2d_ros.cpp +++ b/costmap_2d/src/costmap_2d_ros.cpp @@ -75,6 +75,7 @@ Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf) : last_publish_(0), plugin_loader_("costmap_2d", "costmap_2d::Layer"), publisher_(NULL), + timed_publisher_(NULL), dsrv_(NULL), footprint_padding_(0.0) { @@ -88,6 +89,15 @@ Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf) : private_nh.param("global_frame", global_frame_, std::string("map")); private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link")); +// Get params for timed_costmap + private_nh.param("prediction_time", prediction_time_, 0.0); + private_nh.param("timestep", timestep_, 0.0); + if(prediction_time_ > 0 && timestep_ == 0) + { + timestep_ = 0.1; // Default value? + ROS_WARN("%s/prediction_time is set to %.2fs, but %s/timestep is set to 0s... Using default value %.2fs for timestep instead", name.c_str(), prediction_time_, name.c_str(), timestep_); + } + ros::Time last_error = ros::Time::now(); std::string tf_error; // we need to make sure that the transform between the robot base frame and the global frame is available @@ -112,7 +122,7 @@ Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf) : private_nh.param("track_unknown_space", track_unknown_space, false); private_nh.param("always_send_full_costmap", always_send_full_costmap, false); - layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space); + layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space, prediction_time_, timestep_); if (!private_nh.hasParam("plugins")) { @@ -162,6 +172,9 @@ Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf) : publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap); + // Publish future timed costmap (t = 5s) for debugging timed costmap (just for visualizing in rviz...) + timed_publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(5), global_frame_, "timed_costmap", + always_send_full_costmap); // create a thread to handle updating the map stop_updates_ = false; initialized_ = true; @@ -195,6 +208,9 @@ Costmap2DROS::~Costmap2DROS() if (publisher_ != NULL) delete publisher_; + if (timed_publisher_ != NULL) + delete timed_publisher_; + delete layered_costmap_; delete dsrv_; } @@ -463,12 +479,14 @@ void Costmap2DROS::mapUpdateLoop(double frequency) unsigned int x0, y0, xn, yn; layered_costmap_->getBounds(&x0, &xn, &y0, &yn); publisher_->updateBounds(x0, xn, y0, yn); + timed_publisher_->updateBounds(x0, xn, y0, yn); ros::Time now = ros::Time::now(); ROS_WARN_COND(now < last_publish_, "ROS Time jumped backwards by %.3f s. Publishing costmaps anyway.", (last_publish_ - now).toSec()); if (now < last_publish_ || last_publish_ + publish_cycle < now) { publisher_->publishCostmap(); + timed_publisher_->publishCostmap(); last_publish_ = now; } } diff --git a/costmap_2d/src/layer.cpp b/costmap_2d/src/layer.cpp index 003c18ed41..acf752f40f 100644 --- a/costmap_2d/src/layer.cpp +++ b/costmap_2d/src/layer.cpp @@ -36,6 +36,8 @@ Layer::Layer() : layered_costmap_(NULL) , current_(false) , enabled_(false) + , timed_(false) + , paint_only_current_time_(false) , name_() , tf_(NULL) {} diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index bf1823fce4..6931a943e4 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -47,28 +47,50 @@ using std::vector; namespace costmap_2d { -LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) : - costmap_(), +LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown, double prediction_time, double timestep) : + static_costmap_(), + timestep_(timestep), + prediction_time_(prediction_time), global_frame_(global_frame), rolling_window_(rolling_window), current_(false), - minx_(0.0), - miny_(0.0), - maxx_(0.0), - maxy_(0.0), - bx0_(0), - bxn_(0), - by0_(0), - byn_(0), + static_minx_(0.0), + static_miny_(0.0), + static_maxx_(0.0), + static_maxy_(0.0), + static_bx0_(0), + static_bxn_(0), + static_by0_(0), + static_byn_(0), initialized_(false), size_locked_(false), circumscribed_radius_(1.0), inscribed_radius_(0.1) { + if (timestep_ <= 0 || prediction_time_ <= 0) + timed_costmaps_.resize(1); + else + timed_costmaps_.resize(ceil(prediction_time_/timestep_)); + + timed_bounds_.resize(timed_costmaps_.size()); + for (auto& bounds : timed_bounds_) + { + bounds.minx = bounds.maxx = bounds.miny = bounds.maxy = 0.0; + bounds.bx0 = bounds.bxn = bounds.by0 = bounds.byn = 0; + } + if (track_unknown) - costmap_.setDefaultValue(NO_INFORMATION); + { + static_costmap_.setDefaultValue(NO_INFORMATION); + for(auto& costmap : timed_costmaps_) + costmap.setDefaultValue(NO_INFORMATION); + } else - costmap_.setDefaultValue(FREE_SPACE); + { + static_costmap_.setDefaultValue(FREE_SPACE); + for(auto& costmap : timed_costmaps_) + costmap.setDefaultValue(FREE_SPACE); + } } LayeredCostmap::~LayeredCostmap() @@ -82,11 +104,16 @@ LayeredCostmap::~LayeredCostmap() void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked) { - boost::unique_lock lock(*(costmap_.getMutex())); + boost::unique_lock lock(*(static_costmap_.getMutex())); size_locked_ = size_locked; - costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); + static_costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); + for(Costmap2D& costmap : timed_costmaps_) + { + boost::unique_lock lock(*(costmap.getMutex())); + costmap.resizeMap(size_x, size_y, resolution, origin_x, origin_y); + } for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); - ++plugin) + ++plugin) { (*plugin)->matchSize(); } @@ -94,71 +121,176 @@ void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { - // Lock for the remainder of this function, some plugins (e.g. VoxelLayer) - // implement thread unsafe updateBounds() functions. - boost::unique_lock lock(*(costmap_.getMutex())); + boost::unique_lock lock(*(static_costmap_.getMutex())); // Uneccessary to lock? // if we're using a rolling buffer costmap... we need to update the origin using the robot's position if (rolling_window_) { - double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2; - double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2; - costmap_.updateOrigin(new_origin_x, new_origin_y); + double new_origin_x = robot_x - timed_costmaps_.front().getSizeInMetersX() / 2; + double new_origin_y = robot_y - timed_costmaps_.front().getSizeInMetersY() / 2; + static_costmap_.updateOrigin(new_origin_x, new_origin_y); + for(costmap_2d::Costmap2D& costmap : timed_costmaps_) + { + boost::unique_lock lock(*(costmap.getMutex())); + costmap.updateOrigin(new_origin_x, new_origin_y); + } } if (plugins_.size() == 0) return; - minx_ = miny_ = 1e30; - maxx_ = maxy_ = -1e30; + vector >::iterator current_plugin; + + static_minx_ = static_miny_ = 1e30; + static_maxx_ = static_maxy_ = -1e30; + // In this first loop we create a 'static_costmap' that we later copy into the timed costmaps, so + // we don't have to recompute costs that don't change over time for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); - ++plugin) + ++plugin) { - if(!(*plugin)->isEnabled()) + current_plugin = plugin; // Save the last plugin we looked at to know where to start the timed loop + + if(!(*plugin)->isEnabled()) + { continue; - double prev_minx = minx_; - double prev_miny = miny_; - double prev_maxx = maxx_; - double prev_maxy = maxy_; - (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); - if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) + } + // We can't just skip timed plugins, since later plugins costs depend on previous ones, so a layers + // cost could change over time even if it is itself not timed (e.g. inflation layer) -> break. + if((*plugin)->isTimed()) + { + break; + } + // To-Do:Check wether there currenty are timed changes, otherwise make isTimed() return false + // For this to have an effect on the current configuration we need to make obstacle and dynamic obstacle layer into one... + + double prev_minx = static_minx_; + double prev_miny = static_miny_; + double prev_maxx = static_maxx_; + double prev_maxy = static_maxy_; + (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &static_minx_, &static_miny_, &static_maxx_, &static_maxy_); + if (static_minx_ > prev_minx || static_miny_ > prev_miny || static_maxx_ < prev_maxx || static_maxy_ < prev_maxy) { ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s", prev_minx, prev_miny, prev_maxx , prev_maxy, - minx_, miny_, maxx_ , maxy_, + static_minx_, static_miny_, static_maxx_ , static_maxy_, (*plugin)->getName().c_str()); } + // ROS_DEBUG("%s is updating area x: [%f, %f] y: [%f, %f] ", (*plugin)->getName(), static_minx_, static_maxx_, static_miny_, static_maxy_); } - int x0, xn, y0, yn; - costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0); - costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn); + int static_x0, static_xn, static_y0, static_yn; + static_costmap_.worldToMapEnforceBounds(static_minx_, static_miny_, static_x0, static_y0); + static_costmap_.worldToMapEnforceBounds(static_maxx_, static_maxy_, static_xn, static_yn); - x0 = std::max(0, x0); - xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1); - y0 = std::max(0, y0); - yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1); + static_x0 = std::max(0, static_x0); + static_xn = std::min(int(static_costmap_.getSizeInCellsX()), static_xn + 1); + static_y0 = std::max(0, static_y0); + static_yn = std::min(int(static_costmap_.getSizeInCellsY()), static_yn + 1); - ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); - - if (xn < x0 || yn < y0) - return; + // ROS_DEBUG("Updating area x: [%f, %f] y: [%f, %f] ", static_minx_, static_maxx_, static_miny_, static_maxy_); - costmap_.resetMap(x0, y0, xn, yn); - for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); - ++plugin) + if (static_xn > static_x0 && static_yn > static_y0) { - if((*plugin)->isEnabled()) - (*plugin)->updateCosts(costmap_, x0, y0, xn, yn); + static_costmap_.resetMap(static_x0, static_y0, static_xn, static_yn); // Reset all Maps here? + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + if(!(*plugin)->isEnabled()) + continue; + else if((*plugin)->isTimed()) + break; + else + (*plugin)->updateCosts(static_costmap_, static_x0, static_y0, static_xn, static_yn); + } } - bx0_ = x0; - bxn_ = xn; - by0_ = y0; - byn_ = yn; + // In this second loop we create the actual 'timed_costmaps'. + // We loop through the timesteps, and compute bounds and costs for each timed costmap + // based on index i (i = t / timestep) + for (size_t i = 0; i < timed_costmaps_.size(); ++i) + { + Costmap2D& costmap = timed_costmaps_[i]; + Costmap2DBounds& bounds = timed_bounds_[i]; + + // We set the timed bounds to the static bounds to include changes from the static layers + bounds.minx = static_minx_; + bounds.miny = static_miny_; + bounds.maxx = static_maxx_; + bounds.maxy = static_maxy_; + double prev_minx = bounds.minx; + double prev_miny = bounds.miny; + double prev_maxx = bounds.maxx; + double prev_maxy = bounds.maxy; + + boost::unique_lock lock(*(costmap.getMutex())); + + for (vector >::iterator plugin = current_plugin; plugin != plugins_.end(); + ++plugin) + { + // Currently we have some layers that are time dependent but don't have a timed logic yet. This means + // that we don't want to paint these layers in the static_costmap, otherwise we paint it in all of our timed + // snapshots. Instead, we paint it only in the first costmap where t = 0, and skip these plugins for t > 0. + // E.g.: Obstacle Layer -> Since the obstacle layer paints all observation, it would include dynamic obstacles + // at their current position. We don't want to paint these dynamic obstacle at their current position in future + // costmaps, otherwise we would have to manually remove them again. + // To-Do: Combine Obstacle and Dynamic Obstacle Layer into one Layer with a timed logic + // A proper implementation of this depends on the logic of the perception component..... + if ((i > 0 && (*plugin)->paintOnlyCurrentTime())) + continue; + + // To-Do: Calculate Bounds only for first and last costmap? + (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, + &bounds.minx, &bounds.miny, + &bounds.maxx, &bounds.maxy, i*timestep_); + if (bounds.minx > prev_minx || bounds.miny > prev_miny || + bounds.maxx < prev_maxx || bounds.maxy < prev_maxy) + { + ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " + "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s", + prev_minx, prev_miny, prev_maxx , prev_maxy, + bounds.minx, bounds.miny, + bounds.maxx , bounds.maxy, + (*plugin)->getName().c_str()); + } + + // ROS_DEBUG_STREAM("%s is updating area x: [%f, %f] y: [%f, %f], i: %d i: %f ", (*plugin)->getName(), bounds.minx, bounds.maxx, bounds.miny, bounds.maxy, i, i*timestep_); + } + int x0, xn, y0, yn; + costmap.worldToMapEnforceBounds(bounds.minx, bounds.miny, x0, y0); + costmap.worldToMapEnforceBounds(bounds.maxx, bounds.maxy, xn, yn); + + x0 = std::max(0, x0); + xn = std::min(int(costmap.getSizeInCellsX()), xn + 1); + y0 = std::max(0, y0); + yn = std::min(int(costmap.getSizeInCellsY()), yn + 1); + // ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d], i: %d i: %f ", x0, xn, y0, yn, i, i*timestep_); + + if (xn < x0 || yn < y0) + continue; + + // Instead of resetting the costmap within the bounds, we copy the region of the static costmap we precomputed. + // The region includes all changes from both the static and timed layers. + copyMapRegion(static_costmap_.getCharMap(), x0, y0, static_costmap_.getSizeInCellsX(), costmap.getCharMap(), x0, y0, costmap.getSizeInCellsX(), xn - x0, yn - y0); + + for (vector >::iterator plugin = current_plugin; plugin != plugins_.end(); + ++plugin) + { + if ((i > 0 && (*plugin)->paintOnlyCurrentTime())) + continue; + + // Now we update the costs of the timed layers... + (*plugin)->updateCosts(costmap, x0, y0, xn, yn, i*timestep_); // i is not used here atm... + } + + bounds.bx0 = x0; + bounds.bxn = xn; + bounds.by0 = y0; + bounds.byn = yn; + } + initialized_ = true; } @@ -174,6 +306,14 @@ bool LayeredCostmap::isCurrent() return current_; } +costmap_2d::Costmap2D* LayeredCostmap::getCostmap(double t) +{ + if (timed_costmaps_.empty()) + return nullptr; + unsigned int i = getTimeIndex(t); + return &timed_costmaps_[i]; +} + void LayeredCostmap::setFootprint(const std::vector& footprint_spec) { footprint_ = footprint_spec; diff --git a/dwa_local_planner/include/dwa_local_planner/dwa_planner_ros.h b/dwa_local_planner/include/dwa_local_planner/dwa_planner_ros.h index 02397ca4e4..e1bbf1ec17 100644 --- a/dwa_local_planner/include/dwa_local_planner/dwa_planner_ros.h +++ b/dwa_local_planner/include/dwa_local_planner/dwa_planner_ros.h @@ -50,6 +50,8 @@ #include #include + +#include #include #include @@ -181,21 +183,28 @@ namespace dwa_local_planner { void publishScaledFootprint(const geometry_msgs::PoseStamped& pose, const base_local_planner::Trajectory &traj) const; + /** + * @brief Publish robot footprint of even points on the chosen trajectory + */ + void publishProjectedFootprints(const std::vector& footprint, + const base_local_planner::Trajectory& traj) const; + bool finishedBestEffort(); void resetBestEffort(); tf2_ros::Buffer* tf_; ///< @brief Used for transforming point clouds - // for visualisation, publishers of global and local plan - ros::Publisher g_plan_pub_, l_plan_pub_, scaled_fp_pub_; + // for visualisation, publishers of global plan, local plan, and also footprints + ros::Publisher g_plan_pub_, l_plan_pub_, scaled_fp_pub_, projected_fp_pub_; base_local_planner::LocalPlannerUtil planner_util_; boost::shared_ptr dp_; ///< @brief The trajectory controller costmap_2d::Costmap2DROS* costmap_ros_; - + base_local_planner::WorldModel* world_model_; + dynamic_reconfigure::Server *dsrv_; dwa_local_planner::DWAPlannerConfig default_config_; base_local_planner::LocalPlannerLimits _latest_limits; ///< @brief latest limits set by dynamic reconfigure @@ -208,6 +217,8 @@ namespace dwa_local_planner { bool initialized_; + bool publish_projected_fp_ = false; + double controller_frequency_; ///< Calling frequency to this plugin base_local_planner::OdometryHelperRos odom_helper_; diff --git a/dwa_local_planner/src/dwa_planner.cpp b/dwa_local_planner/src/dwa_planner.cpp index ef55c097e5..2cb5a77a0b 100644 --- a/dwa_local_planner/src/dwa_planner.cpp +++ b/dwa_local_planner/src/dwa_planner.cpp @@ -122,7 +122,7 @@ namespace dwa_local_planner { DWAPlanner::DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util) : planner_util_(planner_util), - obstacle_costs_(planner_util->getCostmap()), + obstacle_costs_(planner_util->getLayeredCostmap()), path_costs_(planner_util->getCostmap()), goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true), goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true), diff --git a/dwa_local_planner/src/dwa_planner_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index 79841dcbae..a29db9421e 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -53,6 +53,7 @@ #include #include +#include // register this planner as a MBF's CostmapController plugin PLUGINLIB_EXPORT_CLASS(dwa_local_planner::DWAPlannerROS, mbf_costmap_core::CostmapController) @@ -102,7 +103,7 @@ namespace dwa_local_planner { } DWAPlannerROS::DWAPlannerROS() : initialized_(false), - odom_helper_("odom"), setup_(false), prev_vel_dir_(0), oscillating_(false), latched_inner_goal_(false) { + odom_helper_("odom"), setup_(false), prev_vel_dir_(0), oscillating_(false), latched_inner_goal_(false), world_model_(NULL){ } @@ -122,14 +123,17 @@ namespace dwa_local_planner { g_plan_pub_ = private_nh.advertise("global_plan", 1); l_plan_pub_ = private_nh.advertise("local_plan", 1); scaled_fp_pub_ = private_nh.advertise("scaled_footprint", 1); + projected_fp_pub_ = private_nh.advertise("projected_footprints", 1); tf_ = tf; costmap_ros_ = costmap_ros; costmap_ros_->getRobotPose(current_pose_); + world_model_ = new base_local_planner::CostmapModel(*costmap_ros_->getCostmap()); + // make sure to update the costmap we'll use for this cycle - costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap(); + costmap_2d::LayeredCostmap* layered_costmap = costmap_ros_->getLayeredCostmap(); - planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID()); + planner_util_.initialize(tf, layered_costmap, costmap_ros_->getGlobalFrameID()); //create the actual planner that we'll use.. it'll configure itself from the parameter server dp_ = boost::shared_ptr(new DWAPlanner(name, &planner_util_)); @@ -141,6 +145,8 @@ namespace dwa_local_planner { initialized_ = true; + private_nh.getParam("publish_projected_fp", publish_projected_fp_); + // Warn about deprecated parameters -- remove this block in N-turtle nav_core::warnRenamedParameter(private_nh, "max_vel_trans", "max_trans_vel"); nav_core::warnRenamedParameter(private_nh, "min_vel_trans", "min_trans_vel"); @@ -261,6 +267,9 @@ namespace dwa_local_planner { DWAPlannerROS::~DWAPlannerROS(){ //make sure to clean things up delete dsrv_; + if (world_model_ != NULL) { + delete world_model_; + } } void DWAPlannerROS::resetBestEffort() { @@ -296,6 +305,47 @@ namespace dwa_local_planner { return latched_inner_goal_ || bypassed_goal || oscillating_; } + void DWAPlannerROS::publishProjectedFootprints(const std::vector& footprint, + const base_local_planner::Trajectory& traj) const + { + if (footprint.empty()) + { + return; + } + visualization_msgs::MarkerArray footprint_marker; + double x, y, th, footprint_cost; + // loop through the trajectory + for (unsigned int i = 0; i < traj.getPointsSize(); i++) + { + if (i % 2 != 0) + { + continue; + } + traj.getPoint(i, x, y, th); + std::vector oriented_footprint; + costmap_2d::transformFootprint(x, y, th, footprint, oriented_footprint); + footprint_cost = world_model_->footprintCost(current_pose_.pose.position, oriented_footprint, 0.0, 0.0); + visualization_msgs::Marker vertex_marker; + vertex_marker.header.frame_id = "map"; + vertex_marker.header.stamp = ros::Time::now(); + vertex_marker.type = visualization_msgs::Marker::LINE_STRIP; + vertex_marker.action = visualization_msgs::Marker::ADD; + vertex_marker.pose.orientation.w = 1; + vertex_marker.color.r = (footprint_cost < 0) ? 1.0 : footprint_cost / 255.; + vertex_marker.color.g = (footprint_cost >= 0) ? 1.0 : 1 - footprint_cost / 255.; + vertex_marker.color.b = 0.0; + vertex_marker.color.a = 1.0; + vertex_marker.scale.x = 0.01; + vertex_marker.id = i; + vertex_marker.lifetime = ros::Duration(1.); + oriented_footprint.push_back(oriented_footprint[0]); + vertex_marker.points = oriented_footprint; + footprint_marker.markers.push_back(vertex_marker); + } + + projected_fp_pub_.publish(footprint_marker); + } + uint32_t DWAPlannerROS::dwaComputeVelocityCommands(geometry_msgs::PoseStamped& global_pose, geometry_msgs::TwistStamped& cmd_vel, std::string& message) { // dynamic window sampling approach to get useful velocity commands @@ -388,6 +438,10 @@ namespace dwa_local_planner { //publish information to the visualizer publishScaledFootprint(global_pose, path); publishLocalPlan(local_plan); + if (publish_projected_fp_) + { + publishProjectedFootprints(costmap_ros_->getRobotFootprint(), path); + } return mbf_msgs::ExePathResult::SUCCESS; }