From 4f7d51a1317cafe9144dc98c46ec815e9742a08e Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Fri, 19 May 2023 14:58:50 +0900 Subject: [PATCH 01/25] WIP: Timed Costmap (in Layered Costmap) --- .../base_local_planner/local_planner_util.h | 9 +- .../obstacle_cost_function.h | 13 ++- base_local_planner/src/local_planner_util.cpp | 8 ++ .../src/obstacle_cost_function.cpp | 40 ++++++-- .../include/costmap_2d/costmap_2d_ros.h | 5 + costmap_2d/include/costmap_2d/layer.h | 8 ++ .../include/costmap_2d/layered_costmap.h | 9 ++ costmap_2d/src/layered_costmap.cpp | 99 +++++++++++-------- dwa_local_planner/src/dwa_planner.cpp | 10 +- dwa_local_planner/src/dwa_planner_ros.cpp | 5 +- 10 files changed, 151 insertions(+), 55 deletions(-) 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..afce3f488d 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 @@ -62,6 +62,10 @@ class LocalPlannerUtil { std::string global_frame_; costmap_2d::Costmap2D* costmap_; + std::vector* timed_costmaps_; + + costmap_2d::LayeredCostmap* layered_costmap_; + tf2_ros::Buffer* tf_; @@ -87,7 +91,7 @@ class LocalPlannerUtil { } void initialize(tf2_ros::Buffer* tf, - costmap_2d::Costmap2D* costmap, + costmap_2d::Costmap2D* costmap, std::vector* timed_costmaps, std::string global_frame); bool getGoal(geometry_msgs::PoseStamped& goal_pose); @@ -99,6 +103,9 @@ class LocalPlannerUtil { bool getLocalPlan(const geometry_msgs::PoseStamped& global_pose, std::vector& transformed_plan); costmap_2d::Costmap2D* getCostmap(); + + std::vector* getTimedCostmaps(); + 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..eab5f3d81a 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 @@ -53,7 +53,7 @@ namespace base_local_planner { class ObstacleCostFunction : public TrajectoryCostFunction { public: - ObstacleCostFunction(costmap_2d::Costmap2D* costmap); + ObstacleCostFunction(std::vector* timed_costmaps); ~ObstacleCostFunction(); ExePathOutcome prepare(const geometry_msgs::PoseStamped& current_pose); @@ -67,6 +67,7 @@ class ObstacleCostFunction : public TrajectoryCostFunction { // helper functions, made static for easy unit testing static double getScalingFactor(const Trajectory &traj, double scaling_speed, double max_trans_vel); + double footprintCost( const double& x, const double& y, @@ -75,7 +76,17 @@ class ObstacleCostFunction : public TrajectoryCostFunction { costmap_2d::Costmap2D* costmap, base_local_planner::WorldModel* world_model); + double footprintCost( + const double& x, + const double& y, + const double& th, + const std::vector& scaled_footprint, + std::vector* timed_costmaps, + base_local_planner::WorldModel* world_model, + double t); + private: + std::vector* timed_costmaps_; costmap_2d::Costmap2D* costmap_; std::vector footprint_spec_; base_local_planner::WorldModel* world_model_; diff --git a/base_local_planner/src/local_planner_util.cpp b/base_local_planner/src/local_planner_util.cpp index fa4b1e2ba0..b11289be50 100644 --- a/base_local_planner/src/local_planner_util.cpp +++ b/base_local_planner/src/local_planner_util.cpp @@ -44,10 +44,13 @@ namespace base_local_planner { void LocalPlannerUtil::initialize( tf2_ros::Buffer* tf, costmap_2d::Costmap2D* costmap, + std::vector* timed_costmaps, std::string global_frame) { + if(!initialized_) { tf_ = tf; costmap_ = costmap; + timed_costmaps_ = timed_costmaps; global_frame_ = global_frame; initialized_ = true; } @@ -74,6 +77,11 @@ costmap_2d::Costmap2D* LocalPlannerUtil::getCostmap() { return costmap_; } +std::vector* LocalPlannerUtil::getTimedCostmaps() { + std::vector a = *timed_costmaps_; + return timed_costmaps_; +} + LocalPlannerLimits LocalPlannerUtil::getCurrentLimits() { boost::mutex::scoped_lock l(limits_configuration_mutex_); return limits_; diff --git a/base_local_planner/src/obstacle_cost_function.cpp b/base_local_planner/src/obstacle_cost_function.cpp index 8688f041f7..d926687ece 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -44,11 +44,12 @@ 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(std::vector* timed_costmaps) + : timed_costmaps_(timed_costmaps), costmap_(), sum_scores_(false), sideward_inflation_scale_(1.0) { + // if (costmap_ != NULL) { + // world_model_ = new base_local_planner::CostmapModel(*costmap_); + // // Check what this is being used for!!!!!! + // } ros::NodeHandle pnh("~"); sideward_inflation_scale_sub_ = pnh.subscribe("sideward_inflation_scale", 1, [&](const std_msgs::Float32ConstPtr& msg){ @@ -103,6 +104,7 @@ ExePathOutcome ObstacleCostFunction::prepare(const geometry_msgs::PoseStamped& c double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) { double cost = 0; + double px, py, pth; if (footprint_spec_.size() == 0) { // Bug, should never happen @@ -119,7 +121,7 @@ double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) { traj.getPoint(i, px, py, pth); double f_cost = footprintCost(px, py, pth, scaled_footprint, - costmap_, world_model_); + timed_costmaps_, world_model_, i * traj.time_delta_); if(f_cost < 0){ return f_cost; @@ -153,7 +155,10 @@ double ObstacleCostFunction::footprintCost ( //check if the footprint is legal // TODO: Cache inscribed radius - double footprint_cost = world_model->footprintCost(x, y, th, scaled_footprint); + + base_local_planner::CostmapModel world_model_ = *costmap; // create new world model?? + + double footprint_cost = world_model_.footprintCost(x, y, th, scaled_footprint); if (footprint_cost < 0) { return -6.0; @@ -174,4 +179,25 @@ double ObstacleCostFunction::footprintCost ( return occ_cost; } +double ObstacleCostFunction::footprintCost ( + const double& x, + const double& y, + const double& th, + const std::vector& scaled_footprint, + std::vector* timed_costmaps, + base_local_planner::WorldModel* world_model, + double t) + { + double timestep = 0.3; // MAKE PARAMETER OR PASS FROM LAYERED COSTMAP (hard coded for testing) + int n = std::min((int)(t/timestep), (int)timed_costmaps->size()-1); + double cost = footprintCost(x, y, th, scaled_footprint, (&(*timed_costmaps)[n]), world_model); + + if (cost > 240 && t > 0.4) + { + ROS_INFO_STREAM("<- put breakpoint here for debugging"); + } + return cost; + } + + } /* namespace base_local_planner */ diff --git a/costmap_2d/include/costmap_2d/costmap_2d_ros.h b/costmap_2d/include/costmap_2d/costmap_2d_ros.h index d4cbc36d61..c90fbb7512 100644 --- a/costmap_2d/include/costmap_2d/costmap_2d_ros.h +++ b/costmap_2d/include/costmap_2d/costmap_2d_ros.h @@ -152,6 +152,11 @@ class Costmap2DROS return layered_costmap_->getCostmap(); } + std::vector* getTimedCostmaps() const + { + return layered_costmap_->getTimedCostmaps(); + } + /** * @brief Returns the global frame of the costmap * @return The global frame of the costmap diff --git a/costmap_2d/include/costmap_2d/layer.h b/costmap_2d/include/costmap_2d/layer.h index 79f57ceb11..dda2e1e5ad 100644 --- a/costmap_2d/include/costmap_2d/layer.h +++ b/costmap_2d/include/costmap_2d/layer.h @@ -64,12 +64,20 @@ 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) {} + 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) {} + 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 Stop publishers. */ virtual void deactivate() {} diff --git a/costmap_2d/include/costmap_2d/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index 72a8bdd2df..36c32aa171 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -94,6 +94,8 @@ class LayeredCostmap return &costmap_; } + std::vector* getTimedCostmaps(); + bool isRolling() { return rolling_window_; @@ -154,7 +156,13 @@ class LayeredCostmap * This is updated by setFootprint(). */ double getInscribedRadius() { return inscribed_radius_; } + + // unsigned char getCost(unsigned int mx, unsigned int my, double t) const; + + + private: + std::vector timed_costmaps_; Costmap2D costmap_; std::string global_frame_; @@ -162,6 +170,7 @@ class LayeredCostmap bool current_; double minx_, miny_, maxx_, maxy_; + double timestep_, prediction_time_; unsigned int bx0_, bxn_, by0_, byn_; std::vector > plugins_; diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index bf1823fce4..c55bb825db 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -49,6 +49,8 @@ namespace costmap_2d LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) : costmap_(), + timestep_(0.3), + prediction_time_(1.2), global_frame_(global_frame), rolling_window_(rolling_window), current_(false), @@ -64,6 +66,7 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo size_locked_(false), circumscribed_radius_(1.0), inscribed_radius_(0.1) + { if (track_unknown) costmap_.setDefaultValue(NO_INFORMATION); @@ -96,9 +99,9 @@ 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(*(costmap_.getMutex())); // Change to create new costmap_ object instead?? - // if we're using a rolling buffer costmap... we need to update the origin using the robot's position + // 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; @@ -112,54 +115,63 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) minx_ = miny_ = 1e30; maxx_ = maxy_ = -1e30; - for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); - ++plugin) + timed_costmaps_.clear(); + + // Create new costmap_ in this loop and add to costmap_ array? + for (double t = 0; t <= prediction_time_; t += timestep_) { - 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) + // Costmap2D costmap_ = costmap_; + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) { - 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_, - (*plugin)->getName().c_str()); + 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_, t); // Add time here ???? + if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || 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_, + (*plugin)->getName().c_str()); + } } - } - int x0, xn, y0, yn; - costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0); - costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn); + int x0, xn, y0, yn; + costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0); + costmap_.worldToMapEnforceBounds(maxx_, 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); + 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]", x0, xn, y0, yn); + ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); - if (xn < x0 || yn < y0) - return; + if (xn < x0 || yn < y0) + return; - costmap_.resetMap(x0, y0, xn, yn); - for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); - ++plugin) - { - if((*plugin)->isEnabled()) - (*plugin)->updateCosts(costmap_, x0, y0, xn, yn); - } + costmap_.resetMap(x0, y0, xn, yn); + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + if((*plugin)->isEnabled()) + (*plugin)->updateCosts(costmap_, x0, y0, xn, yn, t); // Add time here + } - bx0_ = x0; - bxn_ = xn; - by0_ = y0; - byn_ = yn; + bx0_ = x0; + bxn_ = xn; + by0_ = y0; + byn_ = yn; - initialized_ = true; + initialized_ = true; + timed_costmaps_.emplace_back(costmap_); + } + costmap_ = timed_costmaps_.front(); } bool LayeredCostmap::isCurrent() @@ -174,6 +186,13 @@ bool LayeredCostmap::isCurrent() return current_; } + +std::vector* LayeredCostmap::getTimedCostmaps() + { + return &timed_costmaps_; + } + + void LayeredCostmap::setFootprint(const std::vector& footprint_spec) { footprint_ = footprint_spec; diff --git a/dwa_local_planner/src/dwa_planner.cpp b/dwa_local_planner/src/dwa_planner.cpp index ef55c097e5..fb129ed50c 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->getTimedCostmaps()), 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), @@ -502,8 +502,8 @@ namespace dwa_local_planner { unsigned int num_points = 0; for(std::vector::iterator t=all_explored.begin(); t != all_explored.end(); ++t) { - if (t->cost_<0) - continue; + // if (t->cost_<0) + // continue; num_points += t->getPointsSize(); } @@ -511,8 +511,8 @@ namespace dwa_local_planner { sensor_msgs::PointCloud2Iterator iter_x(traj_cloud, "x"); for(std::vector::iterator t=all_explored.begin(); t != all_explored.end(); ++t) { - if(t->cost_<0) - continue; + // if(t->cost_<0) + // continue; // Fill out the plan for(unsigned int i = 0; i < t->getPointsSize(); ++i) { double p_x, p_y, p_th; diff --git a/dwa_local_planner/src/dwa_planner_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index 79841dcbae..34829bfb01 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -128,8 +128,10 @@ namespace dwa_local_planner { // make sure to update the costmap we'll use for this cycle costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap(); + std::vector* timed_costmaps = costmap_ros_->getTimedCostmaps(); - planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID()); + + planner_util_.initialize(tf, costmap, timed_costmaps, 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_)); @@ -394,6 +396,7 @@ namespace dwa_local_planner { uint32_t DWAPlannerROS::computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, geometry_msgs::TwistStamped& cmd_vel, std::string& message) { + // dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal if ( ! costmap_ros_->getRobotPose(current_pose_)) { message = "Could not get robot pose"; From b9a1c48f47a7b822f6d111b88f8cd34aec4d5ce0 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Fri, 19 May 2023 15:00:14 +0900 Subject: [PATCH 02/25] Revert changes in costmap_2d ros --- costmap_2d/include/costmap_2d/costmap_2d_ros.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/costmap_2d/include/costmap_2d/costmap_2d_ros.h b/costmap_2d/include/costmap_2d/costmap_2d_ros.h index c90fbb7512..d4cbc36d61 100644 --- a/costmap_2d/include/costmap_2d/costmap_2d_ros.h +++ b/costmap_2d/include/costmap_2d/costmap_2d_ros.h @@ -152,11 +152,6 @@ class Costmap2DROS return layered_costmap_->getCostmap(); } - std::vector* getTimedCostmaps() const - { - return layered_costmap_->getTimedCostmaps(); - } - /** * @brief Returns the global frame of the costmap * @return The global frame of the costmap From 384f810ebf6169e92fadba083a4969e646b35ec3 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Fri, 19 May 2023 15:43:25 +0900 Subject: [PATCH 03/25] Clean up and adress Tiagos comments --- .../base_local_planner/local_planner_util.h | 7 +--- .../obstacle_cost_function.h | 18 +++------ base_local_planner/src/local_planner_util.cpp | 9 ++--- .../src/obstacle_cost_function.cpp | 38 ++++++------------- .../include/costmap_2d/layered_costmap.h | 5 ++- costmap_2d/src/layered_costmap.cpp | 9 ++++- dwa_local_planner/src/dwa_planner.cpp | 2 +- dwa_local_planner/src/dwa_planner_ros.cpp | 4 +- 8 files changed, 37 insertions(+), 55 deletions(-) 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 afce3f488d..501499bd1b 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 @@ -62,8 +62,6 @@ class LocalPlannerUtil { std::string global_frame_; costmap_2d::Costmap2D* costmap_; - std::vector* timed_costmaps_; - costmap_2d::LayeredCostmap* layered_costmap_; tf2_ros::Buffer* tf_; @@ -91,7 +89,7 @@ class LocalPlannerUtil { } void initialize(tf2_ros::Buffer* tf, - costmap_2d::Costmap2D* costmap, std::vector* timed_costmaps, + costmap_2d::Costmap2D* costmap, costmap_2d::LayeredCostmap* layered_costmap, std::string global_frame); bool getGoal(geometry_msgs::PoseStamped& goal_pose); @@ -103,9 +101,8 @@ class LocalPlannerUtil { bool getLocalPlan(const geometry_msgs::PoseStamped& global_pose, std::vector& transformed_plan); costmap_2d::Costmap2D* getCostmap(); - - std::vector* getTimedCostmaps(); + 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 eab5f3d81a..1066d66677 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,8 @@ #include #include +#include + namespace base_local_planner { @@ -53,7 +55,7 @@ namespace base_local_planner { class ObstacleCostFunction : public TrajectoryCostFunction { public: - ObstacleCostFunction(std::vector* timed_costmaps); + ObstacleCostFunction(costmap_2d::LayeredCostmap* layered_costmap); ~ObstacleCostFunction(); ExePathOutcome prepare(const geometry_msgs::PoseStamped& current_pose); @@ -73,20 +75,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); - - double footprintCost( - const double& x, - const double& y, - const double& th, - const std::vector& scaled_footprint, - std::vector* timed_costmaps, + costmap_2d::LayeredCostmap* layered_costmap, base_local_planner::WorldModel* world_model, - double t); + double t = 0.0); private: - std::vector* timed_costmaps_; + costmap_2d::LayeredCostmap* layered_costmap_; costmap_2d::Costmap2D* costmap_; std::vector footprint_spec_; base_local_planner::WorldModel* world_model_; diff --git a/base_local_planner/src/local_planner_util.cpp b/base_local_planner/src/local_planner_util.cpp index b11289be50..fc1ff9f280 100644 --- a/base_local_planner/src/local_planner_util.cpp +++ b/base_local_planner/src/local_planner_util.cpp @@ -44,13 +44,13 @@ namespace base_local_planner { void LocalPlannerUtil::initialize( tf2_ros::Buffer* tf, costmap_2d::Costmap2D* costmap, - std::vector* timed_costmaps, + costmap_2d::LayeredCostmap* layered_costmap, std::string global_frame) { if(!initialized_) { tf_ = tf; costmap_ = costmap; - timed_costmaps_ = timed_costmaps; + layered_costmap_ = layered_costmap; global_frame_ = global_frame; initialized_ = true; } @@ -77,9 +77,8 @@ costmap_2d::Costmap2D* LocalPlannerUtil::getCostmap() { return costmap_; } -std::vector* LocalPlannerUtil::getTimedCostmaps() { - std::vector a = *timed_costmaps_; - return timed_costmaps_; +costmap_2d::LayeredCostmap* LocalPlannerUtil::getLayeredCostmap() { + return layered_costmap_; } LocalPlannerLimits LocalPlannerUtil::getCurrentLimits() { diff --git a/base_local_planner/src/obstacle_cost_function.cpp b/base_local_planner/src/obstacle_cost_function.cpp index d926687ece..d17bb64bf2 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -44,8 +44,8 @@ namespace base_local_planner { -ObstacleCostFunction::ObstacleCostFunction(std::vector* timed_costmaps) - : timed_costmaps_(timed_costmaps), costmap_(), sum_scores_(false), sideward_inflation_scale_(1.0) { +ObstacleCostFunction::ObstacleCostFunction(costmap_2d::LayeredCostmap* layered_costmap) + : layered_costmap_(layered_costmap), costmap_(layered_costmap->getCostmap()), sum_scores_(false), sideward_inflation_scale_(1.0) { // if (costmap_ != NULL) { // world_model_ = new base_local_planner::CostmapModel(*costmap_); // // Check what this is being used for!!!!!! @@ -121,7 +121,7 @@ double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) { traj.getPoint(i, px, py, pth); double f_cost = footprintCost(px, py, pth, scaled_footprint, - timed_costmaps_, world_model_, i * traj.time_delta_); + layered_costmap_, world_model_, i * traj.time_delta_); if(f_cost < 0){ return f_cost; @@ -150,13 +150,18 @@ 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, + base_local_planner::WorldModel* world_model, + double t) { //check if the footprint is legal // TODO: Cache inscribed radius + std::vector timed_costmaps = layered_costmap->getTimedCostmaps(); + costmap_2d::Costmap2D* costmap = layered_costmap->getCostmap(); + double timestep = layered_costmap->getTimestep(); + int n = round(std::min((int)(t/timestep), (int)timed_costmaps.size()-1)); - base_local_planner::CostmapModel world_model_ = *costmap; // create new world model?? + base_local_planner::CostmapModel world_model_ = timed_costmaps[n]; // create new world model?? double footprint_cost = world_model_.footprintCost(x, y, th, scaled_footprint); @@ -179,25 +184,4 @@ double ObstacleCostFunction::footprintCost ( return occ_cost; } -double ObstacleCostFunction::footprintCost ( - const double& x, - const double& y, - const double& th, - const std::vector& scaled_footprint, - std::vector* timed_costmaps, - base_local_planner::WorldModel* world_model, - double t) - { - double timestep = 0.3; // MAKE PARAMETER OR PASS FROM LAYERED COSTMAP (hard coded for testing) - int n = std::min((int)(t/timestep), (int)timed_costmaps->size()-1); - double cost = footprintCost(x, y, th, scaled_footprint, (&(*timed_costmaps)[n]), world_model); - - if (cost > 240 && t > 0.4) - { - ROS_INFO_STREAM("<- put breakpoint here for debugging"); - } - return cost; - } - - } /* namespace base_local_planner */ diff --git a/costmap_2d/include/costmap_2d/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index 36c32aa171..1343859128 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -94,7 +94,10 @@ class LayeredCostmap return &costmap_; } - std::vector* getTimedCostmaps(); + std::vector getTimedCostmaps(); + + double getTimestep(); + bool isRolling() { diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index c55bb825db..9236b56ce1 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -187,9 +187,14 @@ bool LayeredCostmap::isCurrent() } -std::vector* LayeredCostmap::getTimedCostmaps() +std::vector LayeredCostmap::getTimedCostmaps() { - return &timed_costmaps_; + return timed_costmaps_; + } + +double LayeredCostmap::getTimestep() + { + return timestep_; } diff --git a/dwa_local_planner/src/dwa_planner.cpp b/dwa_local_planner/src/dwa_planner.cpp index fb129ed50c..4a317f78ce 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->getTimedCostmaps()), + 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 34829bfb01..51124a0448 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -128,10 +128,10 @@ namespace dwa_local_planner { // make sure to update the costmap we'll use for this cycle costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap(); - std::vector* timed_costmaps = costmap_ros_->getTimedCostmaps(); + costmap_2d::LayeredCostmap* layered_costmap = costmap_ros_->getLayeredCostmap(); - planner_util_.initialize(tf, costmap, timed_costmaps, costmap_ros_->getGlobalFrameID()); + planner_util_.initialize(tf, costmap, 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_)); From b8cb0a15de702b7968982a6e40152794bcd27fe2 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Fri, 19 May 2023 18:47:52 +0900 Subject: [PATCH 04/25] PoC: Timed Costmap --- .../src/obstacle_cost_function.cpp | 11 +-- .../include/costmap_2d/layered_costmap.h | 11 +-- costmap_2d/src/layered_costmap.cpp | 94 ++++++++++--------- 3 files changed, 55 insertions(+), 61 deletions(-) diff --git a/base_local_planner/src/obstacle_cost_function.cpp b/base_local_planner/src/obstacle_cost_function.cpp index d17bb64bf2..67e0293f37 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -156,14 +156,13 @@ double ObstacleCostFunction::footprintCost ( //check if the footprint is legal // TODO: Cache inscribed radius - std::vector timed_costmaps = layered_costmap->getTimedCostmaps(); - costmap_2d::Costmap2D* costmap = layered_costmap->getCostmap(); - double timestep = layered_costmap->getTimestep(); - int n = round(std::min((int)(t/timestep), (int)timed_costmaps.size()-1)); + costmap_2d::Costmap2D* costmap = layered_costmap->getCostmap(t); - base_local_planner::CostmapModel world_model_ = timed_costmaps[n]; // create new world model?? + if (costmap_ != NULL) { + world_model_ = new base_local_planner::CostmapModel(*costmap_); + } - double footprint_cost = world_model_.footprintCost(x, y, th, scaled_footprint); + 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/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index 1343859128..bffe58e412 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -89,16 +89,10 @@ class LayeredCostmap bool isCurrent(); - Costmap2D* getCostmap() - { - return &costmap_; - } - - std::vector getTimedCostmaps(); + Costmap2D* getCostmap(double t = 0.0); double getTimestep(); - bool isRolling() { return rolling_window_; @@ -106,7 +100,7 @@ class LayeredCostmap bool isTrackingUnknown() { - return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION; + return getCostmap()->getDefaultValue() == costmap_2d::NO_INFORMATION; } std::vector >* getPlugins() @@ -166,7 +160,6 @@ class LayeredCostmap private: std::vector timed_costmaps_; - Costmap2D costmap_; std::string global_frame_; bool rolling_window_; /// < @brief Whether or not the costmap should roll with the robot diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index 9236b56ce1..d717c34979 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -48,9 +48,8 @@ namespace costmap_2d { LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) : - costmap_(), - timestep_(0.3), - prediction_time_(1.2), + timestep_(0.1), + prediction_time_(1.6), global_frame_(global_frame), rolling_window_(rolling_window), current_(false), @@ -68,10 +67,14 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo inscribed_radius_(0.1) { - if (track_unknown) - costmap_.setDefaultValue(NO_INFORMATION); - else - costmap_.setDefaultValue(FREE_SPACE); + timed_costmaps_.resize(ceil(prediction_time_/timestep_)); + for(auto& costmap : timed_costmaps_) + { + if (track_unknown) + costmap.setDefaultValue(NO_INFORMATION); + else + costmap.setDefaultValue(FREE_SPACE); + } } LayeredCostmap::~LayeredCostmap() @@ -85,42 +88,43 @@ 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())); - size_locked_ = size_locked; - costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); - for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); - ++plugin) + for(costmap_2d::Costmap2D& costmap : timed_costmaps_) { - (*plugin)->matchSize(); + boost::unique_lock lock(*(costmap.getMutex())); + size_locked_ = size_locked; + costmap.resizeMap(size_x, size_y, resolution, origin_x, origin_y); + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->matchSize(); + } } } 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())); // Change to create new costmap_ object instead?? - - // if we're using a rolling buffer costmap_... we need to update the origin using the robot's position - if (rolling_window_) + double t = -timestep_; + for(costmap_2d::Costmap2D& costmap : timed_costmaps_) { - 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); - } + t += timestep_; + // Lock for the remainder of this function, some plugins (e.g. VoxelLayer) + // implement thread unsafe updateBounds() functions. + boost::unique_lock lock(*(costmap.getMutex())); // Change to create new costmap_ object instead?? - if (plugins_.size() == 0) - return; + // 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); + } - minx_ = miny_ = 1e30; - maxx_ = maxy_ = -1e30; + if (plugins_.size() == 0) + return; - timed_costmaps_.clear(); + minx_ = miny_ = 1e30; + maxx_ = maxy_ = -1e30; - // Create new costmap_ in this loop and add to costmap_ array? - for (double t = 0; t <= prediction_time_; t += timestep_) - { - // Costmap2D costmap_ = costmap_; for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { @@ -130,7 +134,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) double prev_miny = miny_; double prev_maxx = maxx_; double prev_maxy = maxy_; - (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_, t); // Add time here ???? + (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_, t); // Add time here if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " @@ -142,25 +146,25 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) } int x0, xn, y0, yn; - costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0); - costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn); + costmap.worldToMapEnforceBounds(minx_, miny_, x0, y0); + costmap.worldToMapEnforceBounds(maxx_, maxy_, xn, yn); x0 = std::max(0, x0); - xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1); + xn = std::min(int(costmap.getSizeInCellsX()), xn + 1); y0 = std::max(0, y0); - yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1); + yn = std::min(int(costmap.getSizeInCellsY()), yn + 1); ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); if (xn < x0 || yn < y0) return; - costmap_.resetMap(x0, y0, xn, yn); + costmap.resetMap(x0, y0, xn, yn); for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { if((*plugin)->isEnabled()) - (*plugin)->updateCosts(costmap_, x0, y0, xn, yn, t); // Add time here + (*plugin)->updateCosts(costmap, x0, y0, xn, yn, t); // Add time here } bx0_ = x0; @@ -169,9 +173,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) byn_ = yn; initialized_ = true; - timed_costmaps_.emplace_back(costmap_); } - costmap_ = timed_costmaps_.front(); } bool LayeredCostmap::isCurrent() @@ -186,11 +188,11 @@ bool LayeredCostmap::isCurrent() return current_; } - -std::vector LayeredCostmap::getTimedCostmaps() - { - return timed_costmaps_; - } +costmap_2d::Costmap2D* LayeredCostmap::getCostmap(double t) +{ + int n = std::min((int)timed_costmaps_.size()-1, int(t/timestep_)); + return &timed_costmaps_[n]; +} double LayeredCostmap::getTimestep() { From 4920124d7e647be12dd92beba892409c764c7fae Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Wed, 24 May 2023 09:20:45 +0900 Subject: [PATCH 05/25] Fix: pass right costmap in obstacle cost function --- base_local_planner/src/obstacle_cost_function.cpp | 4 ++-- costmap_2d/src/layered_costmap.cpp | 2 +- dwa_local_planner/src/dwa_planner.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/base_local_planner/src/obstacle_cost_function.cpp b/base_local_planner/src/obstacle_cost_function.cpp index 67e0293f37..2f810d3818 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -158,8 +158,8 @@ double ObstacleCostFunction::footprintCost ( // TODO: Cache inscribed radius costmap_2d::Costmap2D* costmap = layered_costmap->getCostmap(t); - if (costmap_ != NULL) { - world_model_ = new base_local_planner::CostmapModel(*costmap_); + if (costmap != NULL) { + world_model_ = new base_local_planner::CostmapModel(*costmap); } double footprint_cost = world_model_->footprintCost(x, y, th, scaled_footprint); diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index d717c34979..bd11bb4fc8 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -49,7 +49,7 @@ namespace costmap_2d LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) : timestep_(0.1), - prediction_time_(1.6), + prediction_time_(3.0), global_frame_(global_frame), rolling_window_(rolling_window), current_(false), diff --git a/dwa_local_planner/src/dwa_planner.cpp b/dwa_local_planner/src/dwa_planner.cpp index 4a317f78ce..2cb5a77a0b 100644 --- a/dwa_local_planner/src/dwa_planner.cpp +++ b/dwa_local_planner/src/dwa_planner.cpp @@ -502,8 +502,8 @@ namespace dwa_local_planner { unsigned int num_points = 0; for(std::vector::iterator t=all_explored.begin(); t != all_explored.end(); ++t) { - // if (t->cost_<0) - // continue; + if (t->cost_<0) + continue; num_points += t->getPointsSize(); } @@ -511,8 +511,8 @@ namespace dwa_local_planner { sensor_msgs::PointCloud2Iterator iter_x(traj_cloud, "x"); for(std::vector::iterator t=all_explored.begin(); t != all_explored.end(); ++t) { - // if(t->cost_<0) - // continue; + if(t->cost_<0) + continue; // Fill out the plan for(unsigned int i = 0; i < t->getPointsSize(); ++i) { double p_x, p_y, p_th; From 4c233a70548885eccc740844b25efcf508b24d47 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Wed, 24 May 2023 17:50:57 +0900 Subject: [PATCH 06/25] Adress Tiagos comments --- .../base_local_planner/local_planner_util.h | 6 +-- .../obstacle_cost_function.h | 4 -- base_local_planner/src/local_planner_util.cpp | 8 ++-- .../src/obstacle_cost_function.cpp | 23 ++++------- .../include/costmap_2d/layered_costmap.h | 11 ++--- costmap_2d/src/layered_costmap.cpp | 41 +++++++++++++------ dwa_local_planner/src/dwa_planner_ros.cpp | 2 +- 7 files changed, 45 insertions(+), 50 deletions(-) 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 501499bd1b..b2b9783eba 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,6 @@ class LocalPlannerUtil { std::string name_; std::string global_frame_; - costmap_2d::Costmap2D* costmap_; costmap_2d::LayeredCostmap* layered_costmap_; tf2_ros::Buffer* tf_; @@ -88,8 +87,7 @@ class LocalPlannerUtil { ~LocalPlannerUtil() { } - void initialize(tf2_ros::Buffer* tf, - costmap_2d::Costmap2D* costmap, costmap_2d::LayeredCostmap* layered_costmap, + void initialize(tf2_ros::Buffer* tf, costmap_2d::LayeredCostmap* layered_costmap, std::string global_frame); bool getGoal(geometry_msgs::PoseStamped& goal_pose); @@ -100,7 +98,7 @@ 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(); 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 1066d66677..0519680b40 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 @@ -56,7 +56,6 @@ class ObstacleCostFunction : public TrajectoryCostFunction { public: ObstacleCostFunction(costmap_2d::LayeredCostmap* layered_costmap); - ~ObstacleCostFunction(); ExePathOutcome prepare(const geometry_msgs::PoseStamped& current_pose); double scoreTrajectory(Trajectory &traj); @@ -76,14 +75,11 @@ class ObstacleCostFunction : public TrajectoryCostFunction { const double& th, const std::vector& scaled_footprint, costmap_2d::LayeredCostmap* layered_costmap, - base_local_planner::WorldModel* world_model, double t = 0.0); private: costmap_2d::LayeredCostmap* layered_costmap_; - costmap_2d::Costmap2D* 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 fc1ff9f280..44ea9ec723 100644 --- a/base_local_planner/src/local_planner_util.cpp +++ b/base_local_planner/src/local_planner_util.cpp @@ -43,13 +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; @@ -73,8 +71,8 @@ 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() { @@ -115,7 +113,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 2f810d3818..975ed809b8 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -45,11 +45,7 @@ namespace base_local_planner { ObstacleCostFunction::ObstacleCostFunction(costmap_2d::LayeredCostmap* layered_costmap) - : layered_costmap_(layered_costmap), costmap_(layered_costmap->getCostmap()), sum_scores_(false), sideward_inflation_scale_(1.0) { - // if (costmap_ != NULL) { - // world_model_ = new base_local_planner::CostmapModel(*costmap_); - // // Check what this is being used for!!!!!! - // } + : 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){ @@ -57,12 +53,6 @@ ObstacleCostFunction::ObstacleCostFunction(costmap_2d::LayeredCostmap* layered_c }); } -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 @@ -121,7 +111,7 @@ double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) { traj.getPoint(i, px, py, pth); double f_cost = footprintCost(px, py, pth, scaled_footprint, - layered_costmap_, world_model_, i * traj.time_delta_); + layered_costmap_, i * traj.time_delta_); if(f_cost < 0){ return f_cost; @@ -151,19 +141,20 @@ double ObstacleCostFunction::footprintCost ( const double& th, const std::vector& scaled_footprint, costmap_2d::LayeredCostmap* layered_costmap, - base_local_planner::WorldModel* world_model, double t) { //check if the footprint is legal // TODO: Cache inscribed radius costmap_2d::Costmap2D* costmap = layered_costmap->getCostmap(t); - if (costmap != NULL) { - world_model_ = new base_local_planner::CostmapModel(*costmap); + if (costmap == NULL) { + return -1.0; // What to return ??????????? } - double footprint_cost = world_model_->footprintCost(x, y, th, scaled_footprint); + std::unique_ptr world_model(new base_local_planner::CostmapModel(*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/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index bffe58e412..ca04a40d8c 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -91,7 +91,7 @@ class LayeredCostmap Costmap2D* getCostmap(double t = 0.0); - double getTimestep(); + const double getTimestep(); bool isRolling() { @@ -153,11 +153,6 @@ class LayeredCostmap * This is updated by setFootprint(). */ double getInscribedRadius() { return inscribed_radius_; } - - // unsigned char getCost(unsigned int mx, unsigned int my, double t) const; - - - private: std::vector timed_costmaps_; std::string global_frame_; @@ -166,9 +161,11 @@ class LayeredCostmap bool current_; double minx_, miny_, maxx_, maxy_; - double timestep_, prediction_time_; unsigned int bx0_, bxn_, by0_, byn_; + // To-Do: Make parameters (ideally should be same as planner sim_time and sim_granularity?) + double timestep_, prediction_time_; + std::vector > plugins_; bool initialized_; diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index bd11bb4fc8..30b04af983 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -87,30 +87,41 @@ LayeredCostmap::~LayeredCostmap() void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked) -{ +{ + std::vector> locks; + for (costmap_2d::Costmap2D& costmap : timed_costmaps_) { + locks.emplace_back(*(costmap.getMutex())); + } + for(costmap_2d::Costmap2D& costmap : timed_costmaps_) { - boost::unique_lock lock(*(costmap.getMutex())); size_locked_ = size_locked; costmap.resizeMap(size_x, size_y, resolution, origin_x, origin_y); - for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + } + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) - { - (*plugin)->matchSize(); - } + { + (*plugin)->matchSize(); } } 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. + // Change to create new vector instead and copy after loop? + std::vector> locks; + for (costmap_2d::Costmap2D& costmap : timed_costmaps_) { + locks.emplace_back(*(costmap.getMutex())); + } + double t = -timestep_; + + + // To-Do: Only compute timed layers inside the loop, non timed layers won't change and thus need to be computed only once for(costmap_2d::Costmap2D& costmap : timed_costmaps_) { t += timestep_; - // Lock for the remainder of this function, some plugins (e.g. VoxelLayer) - // implement thread unsafe updateBounds() functions. - boost::unique_lock lock(*(costmap.getMutex())); // Change to create new costmap_ object instead?? - // if we're using a rolling buffer costmap_... we need to update the origin using the robot's position if (rolling_window_) { @@ -171,9 +182,10 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) bxn_ = xn; by0_ = y0; byn_ = yn; - - initialized_ = true; } + + initialized_ = true; + } bool LayeredCostmap::isCurrent() @@ -190,11 +202,14 @@ bool LayeredCostmap::isCurrent() costmap_2d::Costmap2D* LayeredCostmap::getCostmap(double t) { + if (timed_costmaps_.empty()) + return NULL; + int n = std::min((int)timed_costmaps_.size()-1, int(t/timestep_)); return &timed_costmaps_[n]; } -double LayeredCostmap::getTimestep() +const double LayeredCostmap::getTimestep() { return timestep_; } diff --git a/dwa_local_planner/src/dwa_planner_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index 51124a0448..cad533e276 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -131,7 +131,7 @@ namespace dwa_local_planner { costmap_2d::LayeredCostmap* layered_costmap = costmap_ros_->getLayeredCostmap(); - planner_util_.initialize(tf, costmap, layered_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_)); From 3ce683197f75839b373ce8810d4cc6e3037ce5aa Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Thu, 25 May 2023 10:02:04 +0900 Subject: [PATCH 07/25] Make function const --- costmap_2d/include/costmap_2d/layered_costmap.h | 4 ++-- costmap_2d/src/layered_costmap.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/costmap_2d/include/costmap_2d/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index ca04a40d8c..36e342a6f9 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -89,9 +89,9 @@ class LayeredCostmap bool isCurrent(); - Costmap2D* getCostmap(double t = 0.0); + costmap_2d::Costmap2D* getCostmap(double t = 0.0); - const double getTimestep(); + double getTimestep() const; bool isRolling() { diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index 30b04af983..7df1243c68 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -209,7 +209,7 @@ costmap_2d::Costmap2D* LayeredCostmap::getCostmap(double t) return &timed_costmaps_[n]; } -const double LayeredCostmap::getTimestep() +double LayeredCostmap::getTimestep() const { return timestep_; } From d65c70e146b468384fcd5af6f1535cfc5bdee7fa Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Thu, 25 May 2023 10:48:42 +0900 Subject: [PATCH 08/25] Adress Tiagos comments --- .../src/obstacle_cost_function.cpp | 6 ++-- costmap_2d/src/layered_costmap.cpp | 30 +++++++------------ dwa_local_planner/src/dwa_planner_ros.cpp | 1 - 3 files changed, 14 insertions(+), 23 deletions(-) diff --git a/base_local_planner/src/obstacle_cost_function.cpp b/base_local_planner/src/obstacle_cost_function.cpp index 975ed809b8..b18abcd7ed 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -148,10 +148,10 @@ double ObstacleCostFunction::footprintCost ( costmap_2d::Costmap2D* costmap = layered_costmap->getCostmap(t); if (costmap == NULL) { - return -1.0; // What to return ??????????? + return -10.0; // What to return ??????????? } - - std::unique_ptr world_model(new base_local_planner::CostmapModel(*costmap)); + + std::unique_ptr world_model = std::make_unique(*costmap); double footprint_cost = world_model->footprintCost(x, y, th, scaled_footprint); diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index 7df1243c68..126072d067 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -88,14 +88,10 @@ LayeredCostmap::~LayeredCostmap() void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked) { - std::vector> locks; - for (costmap_2d::Costmap2D& costmap : timed_costmaps_) { - locks.emplace_back(*(costmap.getMutex())); - } - + size_locked_ = size_locked; for(costmap_2d::Costmap2D& costmap : timed_costmaps_) { - size_locked_ = size_locked; + 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(); @@ -107,21 +103,16 @@ 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. - // Change to create new vector instead and copy after loop? - std::vector> locks; - for (costmap_2d::Costmap2D& costmap : timed_costmaps_) { - locks.emplace_back(*(costmap.getMutex())); - } - double t = -timestep_; - // To-Do: Only compute timed layers inside the loop, non timed layers won't change and thus need to be computed only once for(costmap_2d::Costmap2D& costmap : timed_costmaps_) { t += timestep_; + // Lock for the remainder of this function, some plugins (e.g. VoxelLayer) + // implement thread unsafe updateBounds() functions. + boost::unique_lock lock(*(costmap.getMutex())); + // if we're using a rolling buffer costmap_... we need to update the origin using the robot's position if (rolling_window_) { @@ -203,16 +194,17 @@ bool LayeredCostmap::isCurrent() costmap_2d::Costmap2D* LayeredCostmap::getCostmap(double t) { if (timed_costmaps_.empty()) - return NULL; + return nullptr; int n = std::min((int)timed_costmaps_.size()-1, int(t/timestep_)); return &timed_costmaps_[n]; } +// we're not using this rn... double LayeredCostmap::getTimestep() const - { - return timestep_; - } +{ + return timestep_; +} void LayeredCostmap::setFootprint(const std::vector& footprint_spec) diff --git a/dwa_local_planner/src/dwa_planner_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index cad533e276..1d15ac37aa 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -127,7 +127,6 @@ namespace dwa_local_planner { costmap_ros_->getRobotPose(current_pose_); // 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(); From a36ca75a4f6da3c1804abb656df4f5dea4885630 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Wed, 31 May 2023 16:06:35 +0900 Subject: [PATCH 09/25] WIP: Make work with costmap converter --- costmap_2d/src/layered_costmap.cpp | 90 +++++++++++++++++++++++------- 1 file changed, 71 insertions(+), 19 deletions(-) diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index 126072d067..1bcd220955 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -48,8 +48,8 @@ namespace costmap_2d { LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) : - timestep_(0.1), - prediction_time_(3.0), + timestep_(0.05), + prediction_time_(1.0), global_frame_(global_frame), rolling_window_(rolling_window), current_(false), @@ -103,26 +103,76 @@ void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { - double t = -timestep_; + boost::unique_lock lock(*(timed_costmaps_.front().getMutex())); - // To-Do: Only compute timed layers inside the loop, non timed layers won't change and thus need to be computed only once - for(costmap_2d::Costmap2D& costmap : timed_costmaps_) + // if we're using a rolling buffer costmap_... we need to update the origin using the robot's position + if (rolling_window_) { - t += timestep_; - // Lock for the remainder of this function, some plugins (e.g. VoxelLayer) - // implement thread unsafe updateBounds() functions. - boost::unique_lock lock(*(costmap.getMutex())); + double new_origin_x = robot_x - timed_costmaps_.front().getSizeInMetersX() / 2; + double new_origin_y = robot_y - timed_costmaps_.front().getSizeInMetersY() / 2; + timed_costmaps_.front().updateOrigin(new_origin_x, new_origin_y); + } + + if (plugins_.size() == 0) + return; - // if we're using a rolling buffer costmap_... we need to update the origin using the robot's position - if (rolling_window_) + minx_ = miny_ = 1e30; + maxx_ = maxy_ = -1e30; + + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + 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_); // Add time here + if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { - 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); + 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_, + (*plugin)->getName().c_str()); } + } - if (plugins_.size() == 0) - return; + int x0, xn, y0, yn; + timed_costmaps_.front().worldToMapEnforceBounds(minx_, miny_, x0, y0); + timed_costmaps_.front().worldToMapEnforceBounds(maxx_, maxy_, xn, yn); + + x0 = std::max(0, x0); + xn = std::min(int(timed_costmaps_.front().getSizeInCellsX()), xn + 1); + y0 = std::max(0, y0); + yn = std::min(int(timed_costmaps_.front().getSizeInCellsY()), yn + 1); + + ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); + + if (xn < x0 || yn < y0) + return; + + timed_costmaps_.front().resetMap(x0, y0, xn, yn); + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + if((*plugin)->isEnabled()) + (*plugin)->updateCosts(timed_costmaps_.front(), x0, y0, xn, yn); // Add time here + } + + bx0_ = x0; + bxn_ = xn; + by0_ = y0; + byn_ = yn; + + double t = 0; + + // To-Do: Only compute timed layers inside the loop, non timed layers won't change and thus need to be computed only once + for(costmap_2d::Costmap2D& costmap : timed_costmaps_) + { + t += timestep_; + boost::unique_lock lock(*(costmap.getMutex())); minx_ = miny_ = 1e30; maxx_ = maxy_ = -1e30; @@ -130,8 +180,10 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - if(!(*plugin)->isEnabled()) + if((*plugin)->getName() != "local_costmap/dynamic_obstacle") // ask for all timed plugins instead continue; + + ROS_ERROR_STREAM((*plugin)->getName()); double prev_minx = minx_; double prev_miny = miny_; double prev_maxx = maxx_; @@ -156,7 +208,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) y0 = std::max(0, y0); yn = std::min(int(costmap.getSizeInCellsY()), yn + 1); - ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); + ROS_ERROR("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); if (xn < x0 || yn < y0) return; @@ -165,7 +217,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - if((*plugin)->isEnabled()) + if((*plugin)->getName() == "local_costmap/dynamic_obstacle") (*plugin)->updateCosts(costmap, x0, y0, xn, yn, t); // Add time here } From 0eda572322e092c541283a2d1591d1f2cb743e94 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Thu, 1 Jun 2023 09:47:14 +0900 Subject: [PATCH 10/25] Make work with costmap converter & add second publisher for visualizing timed costmap --- costmap_2d/include/costmap_2d/costmap_2d_ros.h | 2 ++ costmap_2d/src/costmap_2d_ros.cpp | 9 +++++++++ costmap_2d/src/layered_costmap.cpp | 7 +++---- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/costmap_2d/include/costmap_2d/costmap_2d_ros.h b/costmap_2d/include/costmap_2d/costmap_2d_ros.h index d4cbc36d61..8503b69a35 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_; + dynamic_reconfigure::Server *dsrv_; boost::recursive_mutex configuration_mutex_; diff --git a/costmap_2d/src/costmap_2d_ros.cpp b/costmap_2d/src/costmap_2d_ros.cpp index 59196412e9..da880c037f 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) { @@ -162,6 +163,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 for debugging timed costmap (just for visualizing in rviz...) + timed_publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(3), global_frame_, "timed_costmap", + always_send_full_costmap); // create a thread to handle updating the map stop_updates_ = false; initialized_ = true; @@ -194,6 +198,9 @@ Costmap2DROS::~Costmap2DROS() } if (publisher_ != NULL) delete publisher_; + + if (timed_publisher_ != NULL) + delete timed_publisher_; delete layered_costmap_; delete dsrv_; @@ -463,12 +470,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/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index 1bcd220955..9b24f897ab 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -48,8 +48,8 @@ namespace costmap_2d { LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) : - timestep_(0.05), - prediction_time_(1.0), + timestep_(0.1), + prediction_time_(1.2), global_frame_(global_frame), rolling_window_(rolling_window), current_(false), @@ -183,7 +183,6 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) if((*plugin)->getName() != "local_costmap/dynamic_obstacle") // ask for all timed plugins instead continue; - ROS_ERROR_STREAM((*plugin)->getName()); double prev_minx = minx_; double prev_miny = miny_; double prev_maxx = maxx_; @@ -208,7 +207,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) y0 = std::max(0, y0); yn = std::min(int(costmap.getSizeInCellsY()), yn + 1); - ROS_ERROR("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); + ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); if (xn < x0 || yn < y0) return; From edfa8aecf8e599cdf00e4ef69577ded6c6e49b4b Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Thu, 1 Jun 2023 11:44:00 +0900 Subject: [PATCH 11/25] PoC: Timed Costmap with Costmap Converter --- costmap_2d/src/layered_costmap.cpp | 32 +++++++++++++++++++----------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index 9b24f897ab..ae39055b92 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -110,7 +110,8 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { double new_origin_x = robot_x - timed_costmaps_.front().getSizeInMetersX() / 2; double new_origin_y = robot_y - timed_costmaps_.front().getSizeInMetersY() / 2; - timed_costmaps_.front().updateOrigin(new_origin_x, new_origin_y); + for(costmap_2d::Costmap2D& costmap : timed_costmaps_) + costmap.updateOrigin(new_origin_x, new_origin_y); } if (plugins_.size() == 0) @@ -122,8 +123,9 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - if(!(*plugin)->isEnabled()) + if(!(*plugin)->isEnabled() || (*plugin)->getName() == "local_costmap/dynamic_obstacle") continue; + double prev_minx = minx_; double prev_miny = miny_; double prev_maxx = maxx_; @@ -139,8 +141,10 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) } } + // To-Do: if timed layers use costmap converter to paint into costmap and costmap converter only sees front() + // that means the bounds don't need to be updated for the timed layers? we can just take the same bounds as front()? int x0, xn, y0, yn; - timed_costmaps_.front().worldToMapEnforceBounds(minx_, miny_, x0, y0); + timed_costmaps_.front().worldToMapEnforceBounds(minx_, miny_, x0, y0); timed_costmaps_.front().worldToMapEnforceBounds(maxx_, maxy_, xn, yn); x0 = std::max(0, x0); @@ -151,14 +155,14 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); if (xn < x0 || yn < y0) - return; + return; // To-Do: Do we need to change this? - timed_costmaps_.front().resetMap(x0, y0, xn, yn); + timed_costmaps_.front().resetMap(x0, y0, xn, yn); // To-Do: Reset all Maps here or no? for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - if((*plugin)->isEnabled()) - (*plugin)->updateCosts(timed_costmaps_.front(), x0, y0, xn, yn); // Add time here + if((*plugin)->isEnabled() && (*plugin)->getName() != "local_costmap/dynamic_obstacle") + (*plugin)->updateCosts(timed_costmaps_.front(), x0, y0, xn, yn); } bx0_ = x0; @@ -171,9 +175,12 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) // To-Do: Only compute timed layers inside the loop, non timed layers won't change and thus need to be computed only once for(costmap_2d::Costmap2D& costmap : timed_costmaps_) { - t += timestep_; + if(t == 0) + { + t += timestep_; + continue; // Put above code here? + } boost::unique_lock lock(*(costmap.getMutex())); - minx_ = miny_ = 1e30; maxx_ = maxy_ = -1e30; @@ -210,24 +217,25 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); if (xn < x0 || yn < y0) - return; + continue; costmap.resetMap(x0, y0, xn, yn); for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { if((*plugin)->getName() == "local_costmap/dynamic_obstacle") - (*plugin)->updateCosts(costmap, x0, y0, xn, yn, t); // Add time here + (*plugin)->updateCosts(costmap, x0, y0, xn, yn, t); // Time not used here rn... } bx0_ = x0; bxn_ = xn; by0_ = y0; byn_ = yn; + + t += timestep_; } initialized_ = true; - } bool LayeredCostmap::isCurrent() From d0972afd6a2ab20dea028e3f452d707f82b03853 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Thu, 1 Jun 2023 18:57:00 +0900 Subject: [PATCH 12/25] clean up and make work with and without time --- .../include/costmap_2d/costmap_2d_ros.h | 1 + .../include/costmap_2d/layered_costmap.h | 2 +- costmap_2d/src/costmap_2d_ros.cpp | 11 ++++++- costmap_2d/src/layered_costmap.cpp | 31 +++++++++++++------ 4 files changed, 33 insertions(+), 12 deletions(-) diff --git a/costmap_2d/include/costmap_2d/costmap_2d_ros.h b/costmap_2d/include/costmap_2d/costmap_2d_ros.h index 8503b69a35..0b020f89fc 100644 --- a/costmap_2d/include/costmap_2d/costmap_2d_ros.h +++ b/costmap_2d/include/costmap_2d/costmap_2d_ros.h @@ -268,6 +268,7 @@ class Costmap2DROS geometry_msgs::PoseStamped old_pose_; Costmap2DPublisher* publisher_; Costmap2DPublisher* timed_publisher_; + double timestep_, prediction_time_; dynamic_reconfigure::Server *dsrv_; diff --git a/costmap_2d/include/costmap_2d/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index 36e342a6f9..60a0b0ae4e 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -58,7 +58,7 @@ class LayeredCostmap /** * @brief Constructor for a 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 diff --git a/costmap_2d/src/costmap_2d_ros.cpp b/costmap_2d/src/costmap_2d_ros.cpp index da880c037f..9f68e387bf 100644 --- a/costmap_2d/src/costmap_2d_ros.cpp +++ b/costmap_2d/src/costmap_2d_ros.cpp @@ -89,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_ && !timestep_) + { + 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 @@ -113,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")) { diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index ae39055b92..0498968718 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -47,9 +47,9 @@ using std::vector; namespace costmap_2d { -LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) : - timestep_(0.1), - prediction_time_(1.2), +LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown, double prediction_time, double timestep) : + timestep_(timestep), + prediction_time_(prediction_time), global_frame_(global_frame), rolling_window_(rolling_window), current_(false), @@ -65,9 +65,12 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo size_locked_(false), circumscribed_radius_(1.0), inscribed_radius_(0.1) - { - timed_costmaps_.resize(ceil(prediction_time_/timestep_)); + if (!timestep_ || !prediction_time_) + timed_costmaps_.resize(1); + else + timed_costmaps_.resize(ceil(prediction_time_/timestep_)); + for(auto& costmap : timed_costmaps_) { if (track_unknown) @@ -172,7 +175,12 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) double t = 0; - // To-Do: Only compute timed layers inside the loop, non timed layers won't change and thus need to be computed only once + // Idea for costmap converter implementation: + // Each update step move timed_costmaps one down, so timed_costmap(t=2) becomes timed_costmap(t=1)... + // Then update costs keeping noise from previous predictions. + // Maybe implement a function like resetMap() but instead of setting to 0 subtract from previous cost? + // Or make some function cost depends on prev_cost, new_cost & t ???? + // Influence of previous prediction should be higher the further in the future the timed_costmap is. for(costmap_2d::Costmap2D& costmap : timed_costmaps_) { if(t == 0) @@ -187,14 +195,15 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - if((*plugin)->getName() != "local_costmap/dynamic_obstacle") // ask for all timed plugins instead + // To-Do: plugin->isTimed() instead + if((*plugin)->getName() != "local_costmap/dynamic_obstacle") 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_, t); // Add time here + (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_, t); if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " @@ -223,8 +232,9 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { + // To-Do: plugin->isTimed() instead if((*plugin)->getName() == "local_costmap/dynamic_obstacle") - (*plugin)->updateCosts(costmap, x0, y0, xn, yn, t); // Time not used here rn... + (*plugin)->updateCosts(costmap, x0, y0, xn, yn, t); // Time is not used here atm... } bx0_ = x0; @@ -254,7 +264,8 @@ costmap_2d::Costmap2D* LayeredCostmap::getCostmap(double t) { if (timed_costmaps_.empty()) return nullptr; - + if(!timestep_) + return &timed_costmaps_.front(); int n = std::min((int)timed_costmaps_.size()-1, int(t/timestep_)); return &timed_costmaps_[n]; } From 038d235462759a6b604813bc477eda2efd1e3856 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Mon, 5 Jun 2023 17:56:45 +0900 Subject: [PATCH 13/25] wip costmap converter --- costmap_2d/src/layered_costmap.cpp | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index 0498968718..963a1a58ee 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -123,9 +123,15 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) minx_ = miny_ = 1e30; maxx_ = maxy_ = -1e30; + // {1} To-Do: Maybe better to use static layers also for timed costmap and only switch obstacle layer for dynamic obstacle layer? + // Changes could be made in obstacle layer itself where if t > 0 we use the logic of current dynamic_obstacle layer... + // Then we can first update the static layers in ALL timed_costmaps and later only update the timed layers... + // See below comments with {1} for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { + // {1} calculate bounds for first costmap but ALL layers, default obstacle layer updateBounds with t = 0) + // or only calculate bounds if (!plugin->isTimed()) and add obstacle bounds in time loop if(!(*plugin)->isEnabled() || (*plugin)->getName() == "local_costmap/dynamic_obstacle") continue; @@ -133,7 +139,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) double prev_miny = miny_; double prev_maxx = maxx_; double prev_maxy = maxy_; - (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); // Add time here + (*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) { ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " @@ -158,12 +164,16 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); if (xn < x0 || yn < y0) + { + // ROS_ERROR_STREAM("WHEN DOES THIS HAPPEN??"); return; // To-Do: Do we need to change this? - - timed_costmaps_.front().resetMap(x0, y0, xn, yn); // To-Do: Reset all Maps here or no? + } + + timed_costmaps_.front().resetMap(x0, y0, xn, yn); // {1} Reset all Maps here? for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { + // {1} if (!plugin->isTimed() loop throught timed_costmaps and updateCosts everywhere... if((*plugin)->isEnabled() && (*plugin)->getName() != "local_costmap/dynamic_obstacle") (*plugin)->updateCosts(timed_costmaps_.front(), x0, y0, xn, yn); } @@ -173,14 +183,16 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) by0_ = y0; byn_ = yn; - double t = 0; - // Idea for costmap converter implementation: // Each update step move timed_costmaps one down, so timed_costmap(t=2) becomes timed_costmap(t=1)... // Then update costs keeping noise from previous predictions. // Maybe implement a function like resetMap() but instead of setting to 0 subtract from previous cost? // Or make some function cost depends on prev_cost, new_cost & t ???? // Influence of previous prediction should be higher the further in the future the timed_costmap is. + // ok but this is probably unneccessary... + + double t = 0; + for(costmap_2d::Costmap2D& costmap : timed_costmaps_) { if(t == 0) @@ -195,7 +207,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - // To-Do: plugin->isTimed() instead + // {1} plugin->isTimed() instead if((*plugin)->getName() != "local_costmap/dynamic_obstacle") continue; From a65890b11e8f2f1cc84d5166624d5b6701288585 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Fri, 9 Jun 2023 18:41:07 +0900 Subject: [PATCH 14/25] wip --- costmap_2d/include/costmap_2d/layer.h | 7 ++++++- costmap_2d/src/layered_costmap.cpp | 17 +++++------------ 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/costmap_2d/include/costmap_2d/layer.h b/costmap_2d/include/costmap_2d/layer.h index dda2e1e5ad..69e8519dbd 100644 --- a/costmap_2d/include/costmap_2d/layer.h +++ b/costmap_2d/include/costmap_2d/layer.h @@ -73,10 +73,15 @@ class Layer * calculated during UpdateBounds(). */ virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {} - + 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);} + virtual bool isTimed() const + { + return false; + } + /** @brief Stop publishers. */ virtual void deactivate() {} diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index 963a1a58ee..b0542e10da 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -123,16 +123,13 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) minx_ = miny_ = 1e30; maxx_ = maxy_ = -1e30; - // {1} To-Do: Maybe better to use static layers also for timed costmap and only switch obstacle layer for dynamic obstacle layer? - // Changes could be made in obstacle layer itself where if t > 0 we use the logic of current dynamic_obstacle layer... - // Then we can first update the static layers in ALL timed_costmaps and later only update the timed layers... - // See below comments with {1} + // To-Do: Maybe store all static layers in a costmap object then for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { // {1} calculate bounds for first costmap but ALL layers, default obstacle layer updateBounds with t = 0) // or only calculate bounds if (!plugin->isTimed()) and add obstacle bounds in time loop - if(!(*plugin)->isEnabled() || (*plugin)->getName() == "local_costmap/dynamic_obstacle") + if(!(*plugin)->isEnabled() || (*plugin)->isTimed()) continue; double prev_minx = minx_; @@ -165,7 +162,6 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) if (xn < x0 || yn < y0) { - // ROS_ERROR_STREAM("WHEN DOES THIS HAPPEN??"); return; // To-Do: Do we need to change this? } @@ -173,8 +169,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - // {1} if (!plugin->isTimed() loop throught timed_costmaps and updateCosts everywhere... - if((*plugin)->isEnabled() && (*plugin)->getName() != "local_costmap/dynamic_obstacle") + if((*plugin)->isEnabled() && !(*plugin)->isTimed()) (*plugin)->updateCosts(timed_costmaps_.front(), x0, y0, xn, yn); } @@ -207,8 +202,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - // {1} plugin->isTimed() instead - if((*plugin)->getName() != "local_costmap/dynamic_obstacle") + if((*plugin)->isTimed()) continue; double prev_minx = minx_; @@ -244,8 +238,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - // To-Do: plugin->isTimed() instead - if((*plugin)->getName() == "local_costmap/dynamic_obstacle") + if((*plugin)->isTimed()) (*plugin)->updateCosts(costmap, x0, y0, xn, yn, t); // Time is not used here atm... } From da1433cd43922b84cc582856a02ea81af686dceb Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Mon, 12 Jun 2023 17:42:45 +0900 Subject: [PATCH 15/25] add isTimedFront() and fix timed bounds (?) --- costmap_2d/include/costmap_2d/layer.h | 6 ++ .../include/costmap_2d/layered_costmap.h | 3 +- .../include/costmap_2d/obstacle_layer.h | 6 ++ costmap_2d/src/costmap_2d_ros.cpp | 2 +- costmap_2d/src/layered_costmap.cpp | 88 +++++++++++-------- 5 files changed, 65 insertions(+), 40 deletions(-) diff --git a/costmap_2d/include/costmap_2d/layer.h b/costmap_2d/include/costmap_2d/layer.h index 69e8519dbd..bd33365eb8 100644 --- a/costmap_2d/include/costmap_2d/layer.h +++ b/costmap_2d/include/costmap_2d/layer.h @@ -82,6 +82,12 @@ class Layer return false; } + // Layers that are neither part of the static nor the timed maps (like obstacle layer) will be only painted in the first timed costmap but + // not in the static costmap + virtual bool isTimedFront() const + { + return false; + } /** @brief Stop publishers. */ virtual void deactivate() {} diff --git a/costmap_2d/include/costmap_2d/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index 60a0b0ae4e..78a0d46754 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -155,12 +155,13 @@ class LayeredCostmap private: std::vector timed_costmaps_; + 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_; + double minx_, miny_, maxx_, maxy_, timed_minx_, timed_miny_, timed_maxx_, timed_maxy_; unsigned int bx0_, bxn_, by0_, byn_; // To-Do: Make parameters (ideally should be same as planner sim_time and sim_granularity?) diff --git a/costmap_2d/include/costmap_2d/obstacle_layer.h b/costmap_2d/include/costmap_2d/obstacle_layer.h index ea5a8db784..731ab6d78e 100644 --- a/costmap_2d/include/costmap_2d/obstacle_layer.h +++ b/costmap_2d/include/costmap_2d/obstacle_layer.h @@ -77,6 +77,12 @@ class ObstacleLayer : public CostmapLayer virtual void deactivate(); virtual void reset(); + + virtual bool isTimedFront() const override + { + return true; + } + /** * @brief A callback to handle buffering LaserScan messages * @param message The message returned from a message notifier diff --git a/costmap_2d/src/costmap_2d_ros.cpp b/costmap_2d/src/costmap_2d_ros.cpp index 9f68e387bf..29040f215f 100644 --- a/costmap_2d/src/costmap_2d_ros.cpp +++ b/costmap_2d/src/costmap_2d_ros.cpp @@ -173,7 +173,7 @@ Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf) : always_send_full_costmap); // Publish future timed costmap for debugging timed costmap (just for visualizing in rviz...) - timed_publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(3), global_frame_, "timed_costmap", + 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; diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index b0542e10da..c9397f2660 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -57,6 +57,10 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo miny_(0.0), maxx_(0.0), maxy_(0.0), + timed_minx_(0.0), + timed_miny_(0.0), + timed_maxx_(0.0), + timed_maxy_(0.0), bx0_(0), bxn_(0), by0_(0), @@ -78,6 +82,7 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo else costmap.setDefaultValue(FREE_SPACE); } + static_costmap_ = timed_costmaps_.front(); } LayeredCostmap::~LayeredCostmap() @@ -97,6 +102,7 @@ void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double boost::unique_lock lock(*(costmap.getMutex())); costmap.resizeMap(size_x, size_y, resolution, origin_x, origin_y); } + static_costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { @@ -106,8 +112,6 @@ void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { - boost::unique_lock lock(*(timed_costmaps_.front().getMutex())); - // if we're using a rolling buffer costmap_... we need to update the origin using the robot's position if (rolling_window_) { @@ -115,7 +119,9 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) double new_origin_y = robot_y - timed_costmaps_.front().getSizeInMetersY() / 2; for(costmap_2d::Costmap2D& costmap : timed_costmaps_) costmap.updateOrigin(new_origin_x, new_origin_y); + static_costmap_.updateOrigin(new_origin_x, new_origin_y); } + if (plugins_.size() == 0) return; @@ -123,13 +129,13 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) minx_ = miny_ = 1e30; maxx_ = maxy_ = -1e30; - // To-Do: Maybe store all static layers in a costmap object then + // To-Do: Maybe store all static layers in a static_costmap object? for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { // {1} calculate bounds for first costmap but ALL layers, default obstacle layer updateBounds with t = 0) // or only calculate bounds if (!plugin->isTimed()) and add obstacle bounds in time loop - if(!(*plugin)->isEnabled() || (*plugin)->isTimed()) + if(!(*plugin)->isEnabled() || (*plugin)->isTimed() || (*plugin)->isTimedFront()) continue; double prev_minx = minx_; @@ -150,13 +156,13 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) // To-Do: if timed layers use costmap converter to paint into costmap and costmap converter only sees front() // that means the bounds don't need to be updated for the timed layers? we can just take the same bounds as front()? int x0, xn, y0, yn; - timed_costmaps_.front().worldToMapEnforceBounds(minx_, miny_, x0, y0); - timed_costmaps_.front().worldToMapEnforceBounds(maxx_, maxy_, xn, yn); + static_costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0); + static_costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn); x0 = std::max(0, x0); - xn = std::min(int(timed_costmaps_.front().getSizeInCellsX()), xn + 1); + xn = std::min(int(static_costmap_.getSizeInCellsX()), xn + 1); y0 = std::max(0, y0); - yn = std::min(int(timed_costmaps_.front().getSizeInCellsY()), yn + 1); + yn = std::min(int(static_costmap_.getSizeInCellsY()), yn + 1); ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); @@ -165,12 +171,14 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) return; // To-Do: Do we need to change this? } - timed_costmaps_.front().resetMap(x0, y0, xn, yn); // {1} Reset all Maps here? + static_costmap_.resetMap(x0, y0, xn, yn); // Reset all Maps here? for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - if((*plugin)->isEnabled() && !(*plugin)->isTimed()) - (*plugin)->updateCosts(timed_costmaps_.front(), x0, y0, xn, yn); + if((*plugin)->isEnabled() && !(*plugin)->isTimed() && !(*plugin)->isTimedFront()) + { + (*plugin)->updateCosts(static_costmap_, x0, y0, xn, yn); + } } bx0_ = x0; @@ -178,51 +186,52 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) by0_ = y0; byn_ = yn; - // Idea for costmap converter implementation: - // Each update step move timed_costmaps one down, so timed_costmap(t=2) becomes timed_costmap(t=1)... - // Then update costs keeping noise from previous predictions. - // Maybe implement a function like resetMap() but instead of setting to 0 subtract from previous cost? - // Or make some function cost depends on prev_cost, new_cost & t ???? - // Influence of previous prediction should be higher the further in the future the timed_costmap is. - // ok but this is probably unneccessary... + // To-Do: Add inflation also on timed layers? double t = 0; for(costmap_2d::Costmap2D& costmap : timed_costmaps_) { - if(t == 0) - { - t += timestep_; - continue; // Put above code here? - } + timed_minx_ = minx_; + timed_miny_ = miny_; + timed_maxx_ = maxx_; + timed_maxy_ = maxy_; boost::unique_lock lock(*(costmap.getMutex())); - minx_ = miny_ = 1e30; - maxx_ = maxy_ = -1e30; + + // To-Do: instead of copying the costmap it might be more efficient (and make more sense) + // to add the timed costmaps in costmap2D class and do setCost(x,y) for every costmap in the timed vector..... + // But this causes some other issues...... + // costmap = static_costmap_; // Copy static costmap + + // timed_minx_ = timed_miny_ = 1e30; + // timed_maxx_ = timed_maxy_ = -1e30; for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - if((*plugin)->isTimed()) + if ((t == 0 && !(*plugin)->isTimedFront()) || (t > 0 && !(*plugin)->isTimed())) 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_, t); - if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) + double prev_minx = timed_minx_; + double prev_miny = timed_miny_; + double prev_maxx = timed_maxx_; + double prev_maxy = timed_maxy_; + double minx, miny, maxx, maxy; + // To-Do: save static bounds and compare against those, rather than previous timed costmaps bounds... + (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &timed_minx_, &timed_miny_, &timed_maxx_, &timed_maxy_, t); + if (timed_minx_ > prev_minx || timed_miny_ > prev_miny || timed_maxx_ < prev_maxx || timed_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_, + timed_minx_, timed_miny_, timed_maxx_ , timed_maxy_, (*plugin)->getName().c_str()); } } int x0, xn, y0, yn; - costmap.worldToMapEnforceBounds(minx_, miny_, x0, y0); - costmap.worldToMapEnforceBounds(maxx_, maxy_, xn, yn); + costmap.worldToMapEnforceBounds(timed_minx_, timed_miny_, x0, y0); + costmap.worldToMapEnforceBounds(timed_maxx_, timed_maxy_, xn, yn); x0 = std::max(0, x0); xn = std::min(int(costmap.getSizeInCellsX()), xn + 1); @@ -234,12 +243,15 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) if (xn < x0 || yn < y0) continue; - costmap.resetMap(x0, y0, xn, yn); + // costmap.resetMap(x0, y0, xn, yn); + costmap = static_costmap_; for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - if((*plugin)->isTimed()) - (*plugin)->updateCosts(costmap, x0, y0, xn, yn, t); // Time is not used here atm... + if(t == 0 && (*plugin)->isTimedFront() || t > 0 && (*plugin)->isTimed()) + { + (*plugin)->updateCosts(costmap, x0, y0, xn, yn, t); // t is not used here atm... + } } bx0_ = x0; From 0829bccebdf11512e7787793dbde9b944b60c740 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Tue, 13 Jun 2023 17:44:33 +0900 Subject: [PATCH 16/25] create static costmap and copy into timed costmaps... --- costmap_2d/src/layered_costmap.cpp | 86 +++++++++++++++++------------- 1 file changed, 50 insertions(+), 36 deletions(-) diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index c9397f2660..618fd5253e 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -48,6 +48,7 @@ namespace costmap_2d { 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), @@ -75,14 +76,18 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo else timed_costmaps_.resize(ceil(prediction_time_/timestep_)); - for(auto& costmap : timed_costmaps_) + if (track_unknown) { - if (track_unknown) + static_costmap_.setDefaultValue(NO_INFORMATION); + for(auto& costmap : timed_costmaps_) costmap.setDefaultValue(NO_INFORMATION); - else + } + else + { + static_costmap_.setDefaultValue(FREE_SPACE); + for(auto& costmap : timed_costmaps_) costmap.setDefaultValue(FREE_SPACE); } - static_costmap_ = timed_costmaps_.front(); } LayeredCostmap::~LayeredCostmap() @@ -97,12 +102,14 @@ void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double double origin_y, bool size_locked) { size_locked_ = size_locked; - for(costmap_2d::Costmap2D& costmap : timed_costmaps_) + + boost::unique_lock lock(*(static_costmap_.getMutex())); + 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); } - static_costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { @@ -112,14 +119,16 @@ void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { + boost::unique_lock lock(*(static_costmap_.getMutex())); + // 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 - 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_) costmap.updateOrigin(new_origin_x, new_origin_y); - static_costmap_.updateOrigin(new_origin_x, new_origin_y); } @@ -166,37 +175,41 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); - if (xn < x0 || yn < y0) + if (xn > x0 && yn > y0) // ???? { - return; // To-Do: Do we need to change this? - } - - static_costmap_.resetMap(x0, y0, xn, yn); // Reset all Maps here? - for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); - ++plugin) - { - if((*plugin)->isEnabled() && !(*plugin)->isTimed() && !(*plugin)->isTimedFront()) + static_costmap_.resetMap(x0, y0, xn, yn); // Reset all Maps here? + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) { - (*plugin)->updateCosts(static_costmap_, x0, y0, xn, yn); + // To-Do: To keep order of layers, use all Plugins until the first timed one... + if((*plugin)->isEnabled() && !(*plugin)->isTimed() && !(*plugin)->isTimedFront()) + { + (*plugin)->updateCosts(static_costmap_, x0, y0, xn, yn); + } } - } - - bx0_ = x0; - bxn_ = xn; - by0_ = y0; - byn_ = yn; + bx0_ = x0; + bxn_ = xn; + by0_ = y0; + byn_ = yn; + } // To-Do: Add inflation also on timed layers? double t = 0; - for(costmap_2d::Costmap2D& costmap : timed_costmaps_) + for(Costmap2D& costmap : timed_costmaps_) { timed_minx_ = minx_; timed_miny_ = miny_; timed_maxx_ = maxx_; timed_maxy_ = maxy_; + double prev_minx = timed_minx_; + double prev_miny = timed_miny_; + double prev_maxx = timed_maxx_; + double prev_maxy = timed_maxy_; boost::unique_lock lock(*(costmap.getMutex())); + // costmap.resetMap(x0, y0, xn, yn); + // costmap = static_costmap_; // To-Do: instead of copying the costmap it might be more efficient (and make more sense) // to add the timed costmaps in costmap2D class and do setCost(x,y) for every costmap in the timed vector..... @@ -205,6 +218,8 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) // timed_minx_ = timed_miny_ = 1e30; // timed_maxx_ = timed_maxy_ = -1e30; + costmap.resizeMap(static_costmap_.getSizeInCellsX(), static_costmap_.getSizeInCellsY(), static_costmap_.getResolution(), static_costmap_.getOriginX(), static_costmap_.getOriginY()); + costmap = static_costmap_; for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) @@ -212,10 +227,6 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) if ((t == 0 && !(*plugin)->isTimedFront()) || (t > 0 && !(*plugin)->isTimed())) continue; - double prev_minx = timed_minx_; - double prev_miny = timed_miny_; - double prev_maxx = timed_maxx_; - double prev_maxy = timed_maxy_; double minx, miny, maxx, maxy; // To-Do: save static bounds and compare against those, rather than previous timed costmaps bounds... (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &timed_minx_, &timed_miny_, &timed_maxx_, &timed_maxy_, t); @@ -227,6 +238,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) timed_minx_, timed_miny_, timed_maxx_ , timed_maxy_, (*plugin)->getName().c_str()); } + } int x0, xn, y0, yn; @@ -238,26 +250,28 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) y0 = std::max(0, y0); yn = std::min(int(costmap.getSizeInCellsY()), yn + 1); - ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); + ROS_ERROR("Updating area x: [%d, %d] y: [%d, %d], t: %f ", x0, xn, y0, yn, t); if (xn < x0 || yn < y0) continue; // costmap.resetMap(x0, y0, xn, yn); - costmap = static_costmap_; + // costmap.copyCostmapWindow(static_costmap_, 0, 0, static_costmap_.getSizeInMetersX(), static_costmap_.getSizeInMetersY()); + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - if(t == 0 && (*plugin)->isTimedFront() || t > 0 && (*plugin)->isTimed()) + if((t == 0 && (*plugin)->isTimedFront()) || (t > 0 && (*plugin)->isTimed())) { - (*plugin)->updateCosts(costmap, x0, y0, xn, yn, t); // t is not used here atm... + ROS_INFO_STREAM((*plugin)->getName() << " " << t); + (*plugin)->updateCosts(costmap, x0, y0, xn, yn); // t is not used here atm... } } - bx0_ = x0; - bxn_ = xn; - by0_ = y0; - byn_ = yn; + // bx0_ = x0; ?? + // bxn_ = xn; + // by0_ = y0; + // byn_ = yn; t += timestep_; } From 549a61ee21ae9022d04a1bab3ba1a2f7779f0544 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Thu, 15 Jun 2023 11:46:58 +0900 Subject: [PATCH 17/25] remove debugging logs --- costmap_2d/src/layered_costmap.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index 618fd5253e..f93edcca96 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -250,8 +250,6 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) y0 = std::max(0, y0); yn = std::min(int(costmap.getSizeInCellsY()), yn + 1); - ROS_ERROR("Updating area x: [%d, %d] y: [%d, %d], t: %f ", x0, xn, y0, yn, t); - if (xn < x0 || yn < y0) continue; @@ -263,7 +261,6 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { if((t == 0 && (*plugin)->isTimedFront()) || (t > 0 && (*plugin)->isTimed())) { - ROS_INFO_STREAM((*plugin)->getName() << " " << t); (*plugin)->updateCosts(costmap, x0, y0, xn, yn); // t is not used here atm... } } From 3efb8cff50acfcbd9da6b15bf6d02c16148855d4 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Thu, 15 Jun 2023 16:57:22 +0900 Subject: [PATCH 18/25] Fix isTimed() logic to work with inflation layer etc. --- costmap_2d/include/costmap_2d/layer.h | 5 +- .../include/costmap_2d/obstacle_layer.h | 4 ++ costmap_2d/src/layered_costmap.cpp | 50 +++++++++++-------- 3 files changed, 37 insertions(+), 22 deletions(-) diff --git a/costmap_2d/include/costmap_2d/layer.h b/costmap_2d/include/costmap_2d/layer.h index bd33365eb8..a7498445cf 100644 --- a/costmap_2d/include/costmap_2d/layer.h +++ b/costmap_2d/include/costmap_2d/layer.h @@ -82,8 +82,9 @@ class Layer return false; } - // Layers that are neither part of the static nor the timed maps (like obstacle layer) will be only painted in the first timed costmap but - // not in the static costmap + // Layers that are time dependent but should only be painted in the costmap with time = 0, like obstacle layer or stvl layer + // -> these layers have no timed logic, but the here painted observations might include dynamic obstacles and are thus timed. + // To-Do: Find better solution for this... virtual bool isTimedFront() const { return false; diff --git a/costmap_2d/include/costmap_2d/obstacle_layer.h b/costmap_2d/include/costmap_2d/obstacle_layer.h index 731ab6d78e..4da45c9887 100644 --- a/costmap_2d/include/costmap_2d/obstacle_layer.h +++ b/costmap_2d/include/costmap_2d/obstacle_layer.h @@ -77,6 +77,10 @@ class ObstacleLayer : public CostmapLayer virtual void deactivate(); virtual void reset(); + virtual bool isTimed() const override + { + return true; + } virtual bool isTimedFront() const override { diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index f93edcca96..c4330c4f6c 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -138,14 +138,18 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) minx_ = miny_ = 1e30; maxx_ = maxy_ = -1e30; - // To-Do: Maybe store all static layers in a static_costmap object? + // In this first loop we create a 'static costmap' that we later copy into the timed costmaps, so + // we don't recompute costs that don't change over time... for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - // {1} calculate bounds for first costmap but ALL layers, default obstacle layer updateBounds with t = 0) - // or only calculate bounds if (!plugin->isTimed()) and add obstacle bounds in time loop - if(!(*plugin)->isEnabled() || (*plugin)->isTimed() || (*plugin)->isTimedFront()) + + // We can't just skip timed plugins, since later plugins costs depend on previous ones, so a layers + // cost could change over timed even if it is itself not timed (e.g. inflation layer) + if(!(*plugin)->isEnabled()) continue; + else if((*plugin)->isTimed()) + break; double prev_minx = minx_; double prev_miny = miny_; @@ -181,11 +185,12 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - // To-Do: To keep order of layers, use all Plugins until the first timed one... - if((*plugin)->isEnabled() && !(*plugin)->isTimed() && !(*plugin)->isTimedFront()) - { + if(!(*plugin)->isEnabled()) + continue; + else if((*plugin)->isTimed()) + break; + else (*plugin)->updateCosts(static_costmap_, x0, y0, xn, yn); - } } bx0_ = x0; @@ -211,22 +216,23 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) // costmap.resetMap(x0, y0, xn, yn); // costmap = static_costmap_; - // To-Do: instead of copying the costmap it might be more efficient (and make more sense) - // to add the timed costmaps in costmap2D class and do setCost(x,y) for every costmap in the timed vector..... - // But this causes some other issues...... - // costmap = static_costmap_; // Copy static costmap - // timed_minx_ = timed_miny_ = 1e30; // timed_maxx_ = timed_maxy_ = -1e30; costmap.resizeMap(static_costmap_.getSizeInCellsX(), static_costmap_.getSizeInCellsY(), static_costmap_.getResolution(), static_costmap_.getOriginX(), static_costmap_.getOriginY()); costmap = static_costmap_; + bool plugins_are_time_dependent = false; for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - if ((t == 0 && !(*plugin)->isTimedFront()) || (t > 0 && !(*plugin)->isTimed())) - continue; + // if ((t == 0 && !(*plugin)->isTimedFront()) || (t > 0 && !(*plugin)->isTimed())) + // continue; + if (!(*plugin)->isTimed() && !plugins_are_time_dependent || (t > 0 && (*plugin)->isTimedFront())) + continue; + + plugins_are_time_dependent = true; // After the first timed plugin, all plugins are time dependent... + double minx, miny, maxx, maxy; // To-Do: save static bounds and compare against those, rather than previous timed costmaps bounds... (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &timed_minx_, &timed_miny_, &timed_maxx_, &timed_maxy_, t); @@ -250,19 +256,23 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) y0 = std::max(0, y0); yn = std::min(int(costmap.getSizeInCellsY()), yn + 1); + // ROS_ERROR("Updating area x: [%d, %d] y: [%d, %d], t: %f ", x0, xn, y0, yn, t); + if (xn < x0 || yn < y0) continue; // costmap.resetMap(x0, y0, xn, yn); // costmap.copyCostmapWindow(static_costmap_, 0, 0, static_costmap_.getSizeInMetersX(), static_costmap_.getSizeInMetersY()); - + plugins_are_time_dependent = false; for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - if((t == 0 && (*plugin)->isTimedFront()) || (t > 0 && (*plugin)->isTimed())) - { - (*plugin)->updateCosts(costmap, x0, y0, xn, yn); // t is not used here atm... - } + if (!(*plugin)->isTimed() && !plugins_are_time_dependent || (t > 0 && (*plugin)->isTimedFront())) + continue; + + plugins_are_time_dependent = true; + // ROS_INFO_STREAM((*plugin)->getName() << " " << t); + (*plugin)->updateCosts(costmap, x0, y0, xn, yn); // t is not used here atm... } // bx0_ = x0; ?? From 0a3f667774f250a38116cd3c02f1c27b913de827 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Mon, 19 Jun 2023 16:07:23 +0900 Subject: [PATCH 19/25] Fix logic with static and timed loop --- .../include/costmap_2d/layered_costmap.h | 80 +++++-- costmap_2d/src/layered_costmap.cpp | 209 +++++++++--------- 2 files changed, 175 insertions(+), 114 deletions(-) diff --git a/costmap_2d/include/costmap_2d/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index 78a0d46754..8831e3207c 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -79,12 +79,17 @@ 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_; + int n; + if (!timestep_) + n = 0; + else + n = std::min((int)timed_bounds_.size()-1, int(t/timestep_)); + minx = timed_bounds_[n].minx; + miny = timed_bounds_[n].miny; + maxx = timed_bounds_[n].maxx; + maxy = timed_bounds_[n].maxy; } bool isCurrent(); @@ -118,12 +123,17 @@ 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_; + int n; + if (!timestep_) + n = 0; + else + n = std::min((int)timed_bounds_.size()-1, int(t/timestep_)); + *x0 = timed_bounds_[n].bx0; + *xn = timed_bounds_[n].bxn; + *y0 = timed_bounds_[n].by0; + *yn = timed_bounds_[n].byn; } bool isInitialized() @@ -153,18 +163,62 @@ class LayeredCostmap * This is updated by setFootprint(). */ double getInscribedRadius() { return inscribed_radius_; } +protected: + /** + * Copied from Costmap2D + * To-Do: Modify function for use here... + * @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: + + // Struct to store bounds for the different timed costmaps + struct Costmap2DBounds + { + double minx, miny, maxx, maxy; + 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_, timed_minx_, timed_miny_, timed_maxx_, timed_maxy_; - unsigned int bx0_, bxn_, by0_, byn_; - // To-Do: Make parameters (ideally should be same as planner sim_time and sim_granularity?) + double static_minx_, static_miny_, static_maxx_, static_maxy_; + unsigned int static_bx0_, static_bxn_, static_by0_, static_byn_; double timestep_, prediction_time_; std::vector > plugins_; diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index c4330c4f6c..ceb8b60a0e 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -54,18 +54,14 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo global_frame_(global_frame), rolling_window_(rolling_window), current_(false), - minx_(0.0), - miny_(0.0), - maxx_(0.0), - maxy_(0.0), - timed_minx_(0.0), - timed_miny_(0.0), - timed_maxx_(0.0), - timed_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), @@ -76,6 +72,13 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo 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) { static_costmap_.setDefaultValue(NO_INFORMATION); @@ -119,7 +122,7 @@ void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { - boost::unique_lock lock(*(static_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_) @@ -131,57 +134,65 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) 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 recompute costs that don't change over time... + // 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) { + current_plugin = plugin; // Save the last plugin we looked at to know where to start the timed loop - // We can't just skip timed plugins, since later plugins costs depend on previous ones, so a layers - // cost could change over timed even if it is itself not timed (e.g. inflation layer) if(!(*plugin)->isEnabled()) + { continue; - else if((*plugin)->isTimed()) + } + // We can't just skip timed plugins, since later plugins costs depend on previous ones, so a layers + // cost could change over timed even if it is itself not timed (e.g. inflation layer) -> break. + else if((*plugin)->isTimed()) + { break; + } - 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) + 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_INFO_STREAM((*plugin)->getName()); + // ROS_ERROR("Updating area x: [%f, %f] y: [%f, %f] ", static_minx_, static_maxx_, static_miny_, static_maxy_); + } - // To-Do: if timed layers use costmap converter to paint into costmap and costmap converter only sees front() - // that means the bounds don't need to be updated for the timed layers? we can just take the same bounds as front()? - int x0, xn, y0, yn; - static_costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0); - static_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(static_costmap_.getSizeInCellsX()), xn + 1); - y0 = std::max(0, y0); - yn = std::min(int(static_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); + // ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); + // ROS_ERROR("Updating area x: [%f, %f] y: [%f, %f] ", static_minx_, static_maxx_, static_miny_, static_maxy_); - if (xn > x0 && yn > y0) // ???? + if (static_xn > static_x0 && static_yn > static_y0) { - static_costmap_.resetMap(x0, y0, xn, yn); // Reset all Maps here? + 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) { @@ -190,97 +201,93 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) else if((*plugin)->isTimed()) break; else - (*plugin)->updateCosts(static_costmap_, x0, y0, xn, yn); + (*plugin)->updateCosts(static_costmap_, static_x0, static_y0, static_xn, static_yn); } - - bx0_ = x0; - bxn_ = xn; - by0_ = y0; - byn_ = yn; } - // To-Do: Add inflation also on timed layers? - - double t = 0; - for(Costmap2D& costmap : timed_costmaps_) + // 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 the time index i given by the time i = t / timestep. + for (size_t i = 0; i < timed_costmaps_.size(); ++i) { - timed_minx_ = minx_; - timed_miny_ = miny_; - timed_maxx_ = maxx_; - timed_maxy_ = maxy_; - double prev_minx = timed_minx_; - double prev_miny = timed_miny_; - double prev_maxx = timed_maxx_; - double prev_maxy = timed_maxy_; - boost::unique_lock lock(*(costmap.getMutex())); - // costmap.resetMap(x0, y0, xn, yn); - // costmap = static_costmap_; - - // timed_minx_ = timed_miny_ = 1e30; - // timed_maxx_ = timed_maxy_ = -1e30; - costmap.resizeMap(static_costmap_.getSizeInCellsX(), static_costmap_.getSizeInCellsY(), static_costmap_.getResolution(), static_costmap_.getOriginX(), static_costmap_.getOriginY()); - costmap = static_costmap_; + Costmap2D& costmap = timed_costmaps_[i]; + Costmap2DBounds& bounds = timed_bounds_[i]; + + // We set the timed bounds to the static bounds to include changes in 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; - bool plugins_are_time_dependent = false; - for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + boost::unique_lock lock(*(costmap.getMutex())); + + for (vector >::iterator plugin = current_plugin; plugin != plugins_.end(); ++plugin) { - // if ((t == 0 && !(*plugin)->isTimedFront()) || (t > 0 && !(*plugin)->isTimed())) - // continue; - - if (!(*plugin)->isTimed() && !plugins_are_time_dependent || (t > 0 && (*plugin)->isTimedFront())) + // 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. + // A proper implementation of this depends on the logic of the perception component..... + if ((i > 0 && (*plugin)->isTimedFront())) continue; - - plugins_are_time_dependent = true; // After the first timed plugin, all plugins are time dependent... - double minx, miny, maxx, maxy; - // To-Do: save static bounds and compare against those, rather than previous timed costmaps bounds... - (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &timed_minx_, &timed_miny_, &timed_maxx_, &timed_maxy_, t); - if (timed_minx_ > prev_minx || timed_miny_ > prev_miny || timed_maxx_ < prev_maxx || timed_maxy_ < prev_maxy) + (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, + &bounds.minx, &bounds.miny, + &bounds.maxx, &bounds.maxy); + 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, - timed_minx_, timed_miny_, timed_maxx_ , timed_maxy_, + bounds.minx, bounds.miny, + bounds.maxx , bounds.maxy, (*plugin)->getName().c_str()); } - } + // ROS_INFO_STREAM((*plugin)->getName()); + // ROS_INFO_STREAM("Updating area x: [%f, %f] y: [%f, %f], i: %d i: %f ", bounds.minx, bounds.maxx, bounds.miny, bounds.maxy, i, i*timestep_); + } int x0, xn, y0, yn; - costmap.worldToMapEnforceBounds(timed_minx_, timed_miny_, x0, y0); - costmap.worldToMapEnforceBounds(timed_maxx_, timed_maxy_, xn, 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_ERROR("Updating area x: [%d, %d] y: [%d, %d], t: %f ", x0, xn, y0, yn, t); + // ROS_ERROR("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); - // costmap.resetMap(x0, y0, xn, yn); - // costmap.copyCostmapWindow(static_costmap_, 0, 0, static_costmap_.getSizeInMetersX(), static_costmap_.getSizeInMetersY()); - plugins_are_time_dependent = false; - for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + for (vector >::iterator plugin = current_plugin; plugin != plugins_.end(); ++plugin) { - if (!(*plugin)->isTimed() && !plugins_are_time_dependent || (t > 0 && (*plugin)->isTimedFront())) + if ((i > 0 && (*plugin)->isTimedFront())) continue; - - plugins_are_time_dependent = true; - // ROS_INFO_STREAM((*plugin)->getName() << " " << t); - (*plugin)->updateCosts(costmap, x0, y0, xn, yn); // t is not used here atm... + + // ROS_INFO_STREAM((*plugin)->getName() << " " << i); + (*plugin)->updateCosts(costmap, x0, y0, xn, yn); // i is not used here atm... } - // bx0_ = x0; ?? - // bxn_ = xn; - // by0_ = y0; - // byn_ = yn; - - t += timestep_; + bounds.bx0 = x0; + bounds.bxn = xn; + bounds.by0 = y0; + bounds.byn = yn; } initialized_ = true; @@ -302,10 +309,10 @@ costmap_2d::Costmap2D* LayeredCostmap::getCostmap(double t) { if (timed_costmaps_.empty()) return nullptr; - if(!timestep_) + else if(!timestep_) return &timed_costmaps_.front(); - int n = std::min((int)timed_costmaps_.size()-1, int(t/timestep_)); - return &timed_costmaps_[n]; + int i = std::min((int)timed_costmaps_.size()-1, int(t/timestep_)); + return &timed_costmaps_[i]; } // we're not using this rn... From 480fcf0105e98e43791268c2f9db1c49a980d39f Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Tue, 20 Jun 2023 17:15:17 +0900 Subject: [PATCH 20/25] Clean up and add time again --- costmap_2d/include/costmap_2d/layer.h | 5 +++-- costmap_2d/include/costmap_2d/layered_costmap.h | 4 ++-- costmap_2d/include/costmap_2d/obstacle_layer.h | 10 ---------- costmap_2d/plugins/obstacle_layer.cpp | 2 ++ costmap_2d/src/layer.cpp | 2 ++ costmap_2d/src/layered_costmap.cpp | 10 +++++++--- 6 files changed, 16 insertions(+), 17 deletions(-) diff --git a/costmap_2d/include/costmap_2d/layer.h b/costmap_2d/include/costmap_2d/layer.h index a7498445cf..90f9884703 100644 --- a/costmap_2d/include/costmap_2d/layer.h +++ b/costmap_2d/include/costmap_2d/layer.h @@ -79,7 +79,7 @@ class Layer virtual bool isTimed() const { - return false; + return timed_; } // Layers that are time dependent but should only be painted in the costmap with time = 0, like obstacle layer or stvl layer @@ -87,7 +87,7 @@ class Layer // To-Do: Find better solution for this... virtual bool isTimedFront() const { - return false; + return timed_front_; } /** @brief Stop publishers. */ @@ -159,6 +159,7 @@ class Layer LayeredCostmap* layered_costmap_; bool current_; bool enabled_; + bool timed_, timed_front_; 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 8831e3207c..db0119d0d1 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -166,7 +166,7 @@ class LayeredCostmap protected: /** * Copied from Costmap2D - * To-Do: Modify function for use here... + * 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 @@ -200,7 +200,7 @@ class LayeredCostmap private: - // Struct to store bounds for the different timed costmaps + // Struct to store bounds for each timed costmaps struct Costmap2DBounds { double minx, miny, maxx, maxy; diff --git a/costmap_2d/include/costmap_2d/obstacle_layer.h b/costmap_2d/include/costmap_2d/obstacle_layer.h index 4da45c9887..ea5a8db784 100644 --- a/costmap_2d/include/costmap_2d/obstacle_layer.h +++ b/costmap_2d/include/costmap_2d/obstacle_layer.h @@ -77,16 +77,6 @@ class ObstacleLayer : public CostmapLayer virtual void deactivate(); virtual void reset(); - virtual bool isTimed() const override - { - return true; - } - - virtual bool isTimedFront() const override - { - return true; - } - /** * @brief A callback to handle buffering LaserScan messages * @param message The message returned from a message notifier diff --git a/costmap_2d/plugins/obstacle_layer.cpp b/costmap_2d/plugins/obstacle_layer.cpp index 76e883e673..757fbda4e1 100644 --- a/costmap_2d/plugins/obstacle_layer.cpp +++ b/costmap_2d/plugins/obstacle_layer.cpp @@ -68,6 +68,8 @@ void ObstacleLayer::onInitialize() ObstacleLayer::matchSize(); current_ = true; + timed_ = true; + timed_front_ = true; global_frame_ = layered_costmap_->getGlobalFrameID(); double transform_tolerance; diff --git a/costmap_2d/src/layer.cpp b/costmap_2d/src/layer.cpp index 003c18ed41..3fbe7c30be 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) + , timed_front_(false) , name_() , tf_(NULL) {} diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index ceb8b60a0e..ef510976d0 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -159,6 +159,8 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { 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 merge obstacle and dynamic obstacle layer... double prev_minx = static_minx_; double prev_miny = static_miny_; @@ -234,13 +236,15 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) // 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)->isTimedFront())) 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); + &bounds.maxx, &bounds.maxy, i*timestep_); if (bounds.minx > prev_minx || bounds.miny > prev_miny || bounds.maxx < prev_maxx || bounds.maxy < prev_maxy) { @@ -281,7 +285,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) continue; // ROS_INFO_STREAM((*plugin)->getName() << " " << i); - (*plugin)->updateCosts(costmap, x0, y0, xn, yn); // i is not used here atm... + (*plugin)->updateCosts(costmap, x0, y0, xn, yn, i*timestep_); // i is not used here atm... } bounds.bx0 = x0; From 8dfd96355ed2f5ee38f79dd96699a778f16e5b06 Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Tue, 20 Jun 2023 18:00:23 +0900 Subject: [PATCH 21/25] Clean up and add comments --- .../base_local_planner/local_planner_util.h | 3 +- .../obstacle_cost_function.h | 2 -- base_local_planner/src/local_planner_util.cpp | 1 - .../src/obstacle_cost_function.cpp | 1 - .../include/costmap_2d/costmap_2d_ros.h | 1 - costmap_2d/include/costmap_2d/layer.h | 24 ++++++++++--- .../include/costmap_2d/layered_costmap.h | 8 ++--- costmap_2d/plugins/obstacle_layer.cpp | 6 ++++ costmap_2d/src/costmap_2d_ros.cpp | 4 +-- costmap_2d/src/layered_costmap.cpp | 35 ++++++++----------- dwa_local_planner/src/dwa_planner_ros.cpp | 2 -- 11 files changed, 47 insertions(+), 40 deletions(-) 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 b2b9783eba..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 @@ -62,7 +62,6 @@ class LocalPlannerUtil { std::string global_frame_; costmap_2d::LayeredCostmap* layered_costmap_; - tf2_ros::Buffer* tf_; @@ -99,7 +98,7 @@ class LocalPlannerUtil { bool getLocalPlan(const geometry_msgs::PoseStamped& global_pose, std::vector& transformed_plan); 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 0519680b40..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 @@ -44,7 +44,6 @@ #include #include - namespace base_local_planner { /** @@ -68,7 +67,6 @@ class ObstacleCostFunction : public TrajectoryCostFunction { // helper functions, made static for easy unit testing static double getScalingFactor(const Trajectory &traj, double scaling_speed, double max_trans_vel); - double footprintCost( const double& x, const double& y, diff --git a/base_local_planner/src/local_planner_util.cpp b/base_local_planner/src/local_planner_util.cpp index 44ea9ec723..b4e1a53ea8 100644 --- a/base_local_planner/src/local_planner_util.cpp +++ b/base_local_planner/src/local_planner_util.cpp @@ -45,7 +45,6 @@ void LocalPlannerUtil::initialize( tf2_ros::Buffer* tf, costmap_2d::LayeredCostmap* layered_costmap, std::string global_frame) { - if(!initialized_) { tf_ = tf; layered_costmap_ = layered_costmap; diff --git a/base_local_planner/src/obstacle_cost_function.cpp b/base_local_planner/src/obstacle_cost_function.cpp index b18abcd7ed..3dca21e0c3 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -94,7 +94,6 @@ ExePathOutcome ObstacleCostFunction::prepare(const geometry_msgs::PoseStamped& c double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) { double cost = 0; - double px, py, pth; if (footprint_spec_.size() == 0) { // Bug, should never happen diff --git a/costmap_2d/include/costmap_2d/costmap_2d_ros.h b/costmap_2d/include/costmap_2d/costmap_2d_ros.h index 0b020f89fc..580722b731 100644 --- a/costmap_2d/include/costmap_2d/costmap_2d_ros.h +++ b/costmap_2d/include/costmap_2d/costmap_2d_ros.h @@ -269,7 +269,6 @@ class Costmap2DROS 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 90f9884703..4a017c82bd 100644 --- a/costmap_2d/include/costmap_2d/layer.h +++ b/costmap_2d/include/costmap_2d/layer.h @@ -64,6 +64,10 @@ 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);} @@ -73,18 +77,30 @@ class Layer * calculated during UpdateBounds(). */ virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {} - + + /** + * @brief Modified updateBCosts() 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. + */ virtual bool isTimed() const { return timed_; } - // Layers that are time dependent but should only be painted in the costmap with time = 0, like obstacle layer or stvl layer - // -> these layers have no timed logic, but the here painted observations might include dynamic obstacles and are thus timed. - // To-Do: Find better solution for this... + /** + * @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. + */ virtual bool isTimedFront() const { return timed_front_; diff --git a/costmap_2d/include/costmap_2d/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index db0119d0d1..ef206eecbd 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -56,7 +56,7 @@ 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, double prediction_time = 0.0, double timestep = 0.0); @@ -165,7 +165,7 @@ class LayeredCostmap protected: /** - * Copied from Costmap2D + * ! 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 @@ -200,7 +200,7 @@ class LayeredCostmap private: - // Struct to store bounds for each timed costmaps + // Struct to store the bounds for each timed costmaps struct Costmap2DBounds { double minx, miny, maxx, maxy; @@ -217,7 +217,7 @@ class LayeredCostmap bool current_; - double static_minx_, static_miny_, static_maxx_, static_maxy_; + double static_minx_, static_miny_, static_maxx_, static_maxy_; // Bounds for static costmap unsigned int static_bx0_, static_bxn_, static_by0_, static_byn_; double timestep_, prediction_time_; diff --git a/costmap_2d/plugins/obstacle_layer.cpp b/costmap_2d/plugins/obstacle_layer.cpp index 757fbda4e1..8b28e6373d 100644 --- a/costmap_2d/plugins/obstacle_layer.cpp +++ b/costmap_2d/plugins/obstacle_layer.cpp @@ -68,6 +68,12 @@ 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; timed_front_ = true; diff --git a/costmap_2d/src/costmap_2d_ros.cpp b/costmap_2d/src/costmap_2d_ros.cpp index 29040f215f..33669cd241 100644 --- a/costmap_2d/src/costmap_2d_ros.cpp +++ b/costmap_2d/src/costmap_2d_ros.cpp @@ -172,7 +172,7 @@ 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 for debugging timed costmap (just for visualizing in rviz...) + // 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 @@ -207,7 +207,7 @@ Costmap2DROS::~Costmap2DROS() } if (publisher_ != NULL) delete publisher_; - + if (timed_publisher_ != NULL) delete timed_publisher_; diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index ef510976d0..ed8793e8e5 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -103,10 +103,9 @@ LayeredCostmap::~LayeredCostmap() void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked) -{ - size_locked_ = size_locked; - +{ boost::unique_lock lock(*(static_costmap_.getMutex())); + size_locked_ = size_locked; static_costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); for(Costmap2D& costmap : timed_costmaps_) { @@ -124,7 +123,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { 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 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 - timed_costmaps_.front().getSizeInMetersX() / 2; @@ -133,7 +132,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for(costmap_2d::Costmap2D& costmap : timed_costmaps_) costmap.updateOrigin(new_origin_x, new_origin_y); } - + if (plugins_.size() == 0) return; @@ -154,13 +153,13 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) continue; } // We can't just skip timed plugins, since later plugins costs depend on previous ones, so a layers - // cost could change over timed even if it is itself not timed (e.g. inflation layer) -> break. + // cost could change over time even if it is itself not timed (e.g. inflation layer) -> break. else 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 merge obstacle and dynamic obstacle layer... + // 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_; @@ -175,9 +174,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) static_minx_, static_miny_, static_maxx_ , static_maxy_, (*plugin)->getName().c_str()); } - // ROS_INFO_STREAM((*plugin)->getName()); - // ROS_ERROR("Updating area x: [%f, %f] y: [%f, %f] ", static_minx_, static_maxx_, static_miny_, static_maxy_); - + // ROS_DEBUG("%s is updating area x: [%f, %f] y: [%f, %f] ", (*plugin)->getName(), static_minx_, static_maxx_, static_miny_, static_maxy_); } int static_x0, static_xn, static_y0, static_yn; @@ -189,8 +186,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) 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); - // ROS_ERROR("Updating area x: [%f, %f] y: [%f, %f] ", static_minx_, static_maxx_, static_miny_, static_maxy_); + // ROS_DEBUG("Updating area x: [%f, %f] y: [%f, %f] ", static_minx_, static_maxx_, static_miny_, static_maxy_); if (static_xn > static_x0 && static_yn > static_y0) { @@ -209,13 +205,13 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) // 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 the time index i given by the time i = t / timestep. + // 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 in the static layers + // 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_; @@ -256,9 +252,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) (*plugin)->getName().c_str()); } - // ROS_INFO_STREAM((*plugin)->getName()); - // ROS_INFO_STREAM("Updating area x: [%f, %f] y: [%f, %f], i: %d i: %f ", bounds.minx, bounds.maxx, bounds.miny, bounds.maxy, i, i*timestep_); - + // 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); @@ -269,7 +263,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) y0 = std::max(0, y0); yn = std::min(int(costmap.getSizeInCellsY()), yn + 1); - // ROS_ERROR("Updating area x: [%d, %d] y: [%d, %d], i: %d i: %f ", x0, xn, y0, yn, i, i*timestep_); + // 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; @@ -283,8 +277,8 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { if ((i > 0 && (*plugin)->isTimedFront())) continue; - - // ROS_INFO_STREAM((*plugin)->getName() << " " << i); + + // Now we update the costs of the timed layers... (*plugin)->updateCosts(costmap, x0, y0, xn, yn, i*timestep_); // i is not used here atm... } @@ -325,7 +319,6 @@ double LayeredCostmap::getTimestep() const return timestep_; } - void LayeredCostmap::setFootprint(const std::vector& footprint_spec) { footprint_ = footprint_spec; diff --git a/dwa_local_planner/src/dwa_planner_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index 1d15ac37aa..971e2eef9a 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -129,7 +129,6 @@ namespace dwa_local_planner { // make sure to update the costmap we'll use for this cycle costmap_2d::LayeredCostmap* layered_costmap = costmap_ros_->getLayeredCostmap(); - 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 @@ -395,7 +394,6 @@ namespace dwa_local_planner { uint32_t DWAPlannerROS::computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, geometry_msgs::TwistStamped& cmd_vel, std::string& message) { - // dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal if ( ! costmap_ros_->getRobotPose(current_pose_)) { message = "Could not get robot pose"; From ca966ffeb6bb1a57434e2893f1c298745ddcc53a Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Fri, 23 Jun 2023 16:31:24 +0900 Subject: [PATCH 22/25] Fix format & style --- .../src/obstacle_cost_function.cpp | 5 ++- costmap_2d/include/costmap_2d/layer.h | 23 ++++++++----- .../include/costmap_2d/layered_costmap.h | 34 +++++++++---------- costmap_2d/plugins/obstacle_layer.cpp | 2 +- costmap_2d/src/costmap_2d_ros.cpp | 2 +- costmap_2d/src/layer.cpp | 2 +- costmap_2d/src/layered_costmap.cpp | 8 ++--- 7 files changed, 40 insertions(+), 36 deletions(-) diff --git a/base_local_planner/src/obstacle_cost_function.cpp b/base_local_planner/src/obstacle_cost_function.cpp index 3dca21e0c3..215bb8513d 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -150,9 +150,8 @@ double ObstacleCostFunction::footprintCost ( return -10.0; // What to return ??????????? } - std::unique_ptr world_model = std::make_unique(*costmap); - - double footprint_cost = world_model->footprintCost(x, y, th, scaled_footprint); + 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/layer.h b/costmap_2d/include/costmap_2d/layer.h index 4a017c82bd..1b8f37a07e 100644 --- a/costmap_2d/include/costmap_2d/layer.h +++ b/costmap_2d/include/costmap_2d/layer.h @@ -69,8 +69,10 @@ class Layer * 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);} + 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 @@ -79,16 +81,18 @@ class Layer virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {} /** - * @brief Modified updateBCosts() to use time argument t for timed layers. + * @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);} + 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. */ - virtual bool isTimed() const + bool isTimed() const { return timed_; } @@ -100,10 +104,11 @@ class Layer * 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) */ - virtual bool isTimedFront() const + bool paintOnlyCurrentTime() const { - return timed_front_; + return paint_only_current_time_; } /** @brief Stop publishers. */ @@ -175,7 +180,7 @@ class Layer LayeredCostmap* layered_costmap_; bool current_; bool enabled_; - bool timed_, timed_front_; + 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 ef206eecbd..a46d590d25 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -81,15 +81,15 @@ class LayeredCostmap void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy, double t = 0) { - int n; - if (!timestep_) - n = 0; + int i; + if (timestep_ == 0) + i = 0; else - n = std::min((int)timed_bounds_.size()-1, int(t/timestep_)); - minx = timed_bounds_[n].minx; - miny = timed_bounds_[n].miny; - maxx = timed_bounds_[n].maxx; - maxy = timed_bounds_[n].maxy; + i = std::min((int)timed_bounds_.size()-1, int(t/timestep_)); + minx = timed_bounds_[i].minx; + miny = timed_bounds_[i].miny; + maxx = timed_bounds_[i].maxx; + maxy = timed_bounds_[i].maxy; } bool isCurrent(); @@ -125,15 +125,15 @@ class LayeredCostmap void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn, double t = 0.0) { - int n; - if (!timestep_) - n = 0; + int i; + if (timestep_ == 0) + i = 0; else - n = std::min((int)timed_bounds_.size()-1, int(t/timestep_)); - *x0 = timed_bounds_[n].bx0; - *xn = timed_bounds_[n].bxn; - *y0 = timed_bounds_[n].by0; - *yn = timed_bounds_[n].byn; + i = std::min((int)timed_bounds_.size()-1, int(t/timestep_)); + *x0 = timed_bounds_[i].bx0; + *xn = timed_bounds_[i].bxn; + *y0 = timed_bounds_[i].by0; + *yn = timed_bounds_[i].byn; } bool isInitialized() @@ -218,7 +218,7 @@ class LayeredCostmap bool current_; double static_minx_, static_miny_, static_maxx_, static_maxy_; // Bounds for static costmap - unsigned int static_bx0_, static_bxn_, static_by0_, static_byn_; + 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 8b28e6373d..147d825b67 100644 --- a/costmap_2d/plugins/obstacle_layer.cpp +++ b/costmap_2d/plugins/obstacle_layer.cpp @@ -75,7 +75,7 @@ void ObstacleLayer::onInitialize() // To-Do: Add timed logic (--> Dynamic Obstacle Layer) // How to implement this depends on how perception component provides information... timed_ = true; - timed_front_ = true; + paint_only_current_time_ = true; global_frame_ = layered_costmap_->getGlobalFrameID(); double transform_tolerance; diff --git a/costmap_2d/src/costmap_2d_ros.cpp b/costmap_2d/src/costmap_2d_ros.cpp index 33669cd241..78d4c17415 100644 --- a/costmap_2d/src/costmap_2d_ros.cpp +++ b/costmap_2d/src/costmap_2d_ros.cpp @@ -92,7 +92,7 @@ Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf) : // Get params for timed_costmap private_nh.param("prediction_time", prediction_time_, 0.0); private_nh.param("timestep", timestep_, 0.0); - if(prediction_time_ && !timestep_) + 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_); diff --git a/costmap_2d/src/layer.cpp b/costmap_2d/src/layer.cpp index 3fbe7c30be..acf752f40f 100644 --- a/costmap_2d/src/layer.cpp +++ b/costmap_2d/src/layer.cpp @@ -37,7 +37,7 @@ Layer::Layer() , current_(false) , enabled_(false) , timed_(false) - , timed_front_(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 ed8793e8e5..38af0cd195 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -67,7 +67,7 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo circumscribed_radius_(1.0), inscribed_radius_(0.1) { - if (!timestep_ || !prediction_time_) + if (timestep_ == 0 || prediction_time_ == 0) timed_costmaps_.resize(1); else timed_costmaps_.resize(ceil(prediction_time_/timestep_)); @@ -234,7 +234,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) // 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)->isTimedFront())) + if ((i > 0 && (*plugin)->paintOnlyCurrentTime())) continue; // To-Do: Calculate Bounds only for first and last costmap? @@ -275,7 +275,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) for (vector >::iterator plugin = current_plugin; plugin != plugins_.end(); ++plugin) { - if ((i > 0 && (*plugin)->isTimedFront())) + if ((i > 0 && (*plugin)->paintOnlyCurrentTime())) continue; // Now we update the costs of the timed layers... @@ -307,7 +307,7 @@ costmap_2d::Costmap2D* LayeredCostmap::getCostmap(double t) { if (timed_costmaps_.empty()) return nullptr; - else if(!timestep_) + else if(timestep_ == 0) return &timed_costmaps_.front(); int i = std::min((int)timed_costmaps_.size()-1, int(t/timestep_)); return &timed_costmaps_[i]; From 07ecb92425a4e627ad5a099f0b67bd676ec5b63e Mon Sep 17 00:00:00 2001 From: Leonie Gadner Date: Wed, 5 Jul 2023 16:59:35 +0900 Subject: [PATCH 23/25] create function to get time index and adress comments --- .../include/costmap_2d/layered_costmap.h | 24 +++++++++---------- costmap_2d/src/layered_costmap.cpp | 19 ++++++--------- 2 files changed, 19 insertions(+), 24 deletions(-) diff --git a/costmap_2d/include/costmap_2d/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index a46d590d25..47a0387de3 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/costmap_2d/include/costmap_2d/layered_costmap.h @@ -81,11 +81,7 @@ class LayeredCostmap void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy, double t = 0) { - int i; - if (timestep_ == 0) - i = 0; - else - i = std::min((int)timed_bounds_.size()-1, int(t/timestep_)); + unsigned int i = getTimeIndex(t); minx = timed_bounds_[i].minx; miny = timed_bounds_[i].miny; maxx = timed_bounds_[i].maxx; @@ -96,7 +92,15 @@ class LayeredCostmap costmap_2d::Costmap2D* getCostmap(double t = 0.0); - double getTimestep() const; + unsigned int getTimeIndex(double t) + { + 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() { @@ -125,11 +129,7 @@ class LayeredCostmap void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn, double t = 0.0) { - int i; - if (timestep_ == 0) - i = 0; - else - i = std::min((int)timed_bounds_.size()-1, int(t/timestep_)); + unsigned int i = getTimeIndex(t); *x0 = timed_bounds_[i].bx0; *xn = timed_bounds_[i].bxn; *y0 = timed_bounds_[i].by0; @@ -204,7 +204,7 @@ class LayeredCostmap struct Costmap2DBounds { double minx, miny, maxx, maxy; - int bx0, bxn, by0, byn; + unsigned int bx0, bxn, by0, byn; }; std::vector timed_costmaps_; diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index 38af0cd195..6931a943e4 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/costmap_2d/src/layered_costmap.cpp @@ -67,13 +67,13 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo circumscribed_radius_(1.0), inscribed_radius_(0.1) { - if (timestep_ == 0 || prediction_time_ == 0) + 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_) + for (auto& bounds : timed_bounds_) { bounds.minx = bounds.maxx = bounds.miny = bounds.maxy = 0.0; bounds.bx0 = bounds.bxn = bounds.by0 = bounds.byn = 0; @@ -130,7 +130,10 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) 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) @@ -154,7 +157,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) } // 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. - else if((*plugin)->isTimed()) + if((*plugin)->isTimed()) { break; } @@ -307,18 +310,10 @@ costmap_2d::Costmap2D* LayeredCostmap::getCostmap(double t) { if (timed_costmaps_.empty()) return nullptr; - else if(timestep_ == 0) - return &timed_costmaps_.front(); - int i = std::min((int)timed_costmaps_.size()-1, int(t/timestep_)); + unsigned int i = getTimeIndex(t); return &timed_costmaps_[i]; } -// we're not using this rn... -double LayeredCostmap::getTimestep() const -{ - return timestep_; -} - void LayeredCostmap::setFootprint(const std::vector& footprint_spec) { footprint_ = footprint_spec; From ec4a05b1c0f3b44c69b21a391893cada1c4b23ff Mon Sep 17 00:00:00 2001 From: Cynthia Li Date: Wed, 16 Aug 2023 15:02:51 +0900 Subject: [PATCH 24/25] visualize footprints of the dwa trajectory --- .../dwa_local_planner/dwa_planner_ros.h | 15 ++++-- dwa_local_planner/src/dwa_planner_ros.cpp | 51 ++++++++++++++++++- 2 files changed, 62 insertions(+), 4 deletions(-) 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..91890e2699 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 publishFootprints(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_, 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 diff --git a/dwa_local_planner/src/dwa_planner_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index 971e2eef9a..b1410ffd6f 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,10 +123,13 @@ 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); + fp_pub_ = private_nh.advertise("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::LayeredCostmap* layered_costmap = costmap_ros_->getLayeredCostmap(); @@ -261,6 +265,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 +303,47 @@ namespace dwa_local_planner { return latched_inner_goal_ || bypassed_goal || oscillating_; } + void DWAPlannerROS::publishFootprints(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); + } + + 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 +436,7 @@ namespace dwa_local_planner { //publish information to the visualizer publishScaledFootprint(global_pose, path); publishLocalPlan(local_plan); + publishFootprints(costmap_ros_->getRobotFootprint(), path); return mbf_msgs::ExePathResult::SUCCESS; } From 6273c10bcff3639ab946d911d2e4b71c950a6df6 Mon Sep 17 00:00:00 2001 From: Cynthia Li Date: Thu, 28 Sep 2023 12:40:02 +0900 Subject: [PATCH 25/25] added paramter to enable/disable thisviz feature --- .../include/dwa_local_planner/dwa_planner_ros.h | 6 ++++-- dwa_local_planner/src/dwa_planner_ros.cpp | 13 +++++++++---- 2 files changed, 13 insertions(+), 6 deletions(-) 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 91890e2699..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 @@ -186,7 +186,7 @@ namespace dwa_local_planner { /** * @brief Publish robot footprint of even points on the chosen trajectory */ - void publishFootprints(const std::vector& footprint, + void publishProjectedFootprints(const std::vector& footprint, const base_local_planner::Trajectory& traj) const; bool finishedBestEffort(); @@ -196,7 +196,7 @@ namespace dwa_local_planner { tf2_ros::Buffer* tf_; ///< @brief Used for transforming point clouds // for visualisation, publishers of global plan, local plan, and also footprints - ros::Publisher g_plan_pub_, l_plan_pub_, scaled_fp_pub_, fp_pub_; + ros::Publisher g_plan_pub_, l_plan_pub_, scaled_fp_pub_, projected_fp_pub_; base_local_planner::LocalPlannerUtil planner_util_; @@ -217,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_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index b1410ffd6f..a29db9421e 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -123,7 +123,7 @@ 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); - fp_pub_ = private_nh.advertise("footprints", 1); + projected_fp_pub_ = private_nh.advertise("projected_footprints", 1); tf_ = tf; costmap_ros_ = costmap_ros; costmap_ros_->getRobotPose(current_pose_); @@ -145,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"); @@ -303,7 +305,7 @@ namespace dwa_local_planner { return latched_inner_goal_ || bypassed_goal || oscillating_; } - void DWAPlannerROS::publishFootprints(const std::vector& footprint, + void DWAPlannerROS::publishProjectedFootprints(const std::vector& footprint, const base_local_planner::Trajectory& traj) const { if (footprint.empty()) @@ -341,7 +343,7 @@ namespace dwa_local_planner { footprint_marker.markers.push_back(vertex_marker); } - fp_pub_.publish(footprint_marker); + projected_fp_pub_.publish(footprint_marker); } uint32_t DWAPlannerROS::dwaComputeVelocityCommands(geometry_msgs::PoseStamped& global_pose, @@ -436,7 +438,10 @@ namespace dwa_local_planner { //publish information to the visualizer publishScaledFootprint(global_pose, path); publishLocalPlan(local_plan); - publishFootprints(costmap_ros_->getRobotFootprint(), path); + if (publish_projected_fp_) + { + publishProjectedFootprints(costmap_ros_->getRobotFootprint(), path); + } return mbf_msgs::ExePathResult::SUCCESS; }