From 79556c5b85296ebc9984f23d5d89c4c336f314c6 Mon Sep 17 00:00:00 2001 From: siferati <15384781+siferati@users.noreply.github.com> Date: Wed, 15 Nov 2023 12:40:54 +0900 Subject: [PATCH 1/4] discount factor and visualization of future footprints --- .../obstacle_cost_function.h | 6 ++- .../src/obstacle_cost_function.cpp | 13 ++--- dwa_local_planner/cfg/DWAPlanner.cfg | 1 + .../include/dwa_local_planner/dwa_planner.h | 3 +- .../dwa_local_planner/dwa_planner_ros.h | 2 +- dwa_local_planner/src/dwa_planner.cpp | 7 +-- dwa_local_planner/src/dwa_planner_ros.cpp | 50 +++++++++++++------ 7 files changed, 55 insertions(+), 27 deletions(-) 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..fae08af8a5 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 @@ -61,9 +61,10 @@ class ObstacleCostFunction : public TrajectoryCostFunction { void setSumScores(bool score_sums){ sum_scores_=score_sums; } - void setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation, double scaling_speed, bool occdist_use_footprint = true); + void setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation, + double scaling_speed, bool occdist_use_footprint = true, double scaling_discount_factor = 1.0); void setFootprint(std::vector footprint_spec); - std::vector getScaledFootprint(const Trajectory& traj) const; + std::vector getScaledFootprint(const Trajectory& traj, unsigned int index = 0) const; // helper functions, made static for easy unit testing static double getScalingFactor(const Trajectory &traj, double scaling_speed, double max_trans_vel); @@ -87,6 +88,7 @@ class ObstacleCostFunction : public TrajectoryCostFunction { ros::Subscriber sideward_inflation_scale_sub_; std::atomic sideward_inflation_scale_; bool occdist_use_footprint_; + double scaling_discount_factor_; }; } /* namespace base_local_planner */ diff --git a/base_local_planner/src/obstacle_cost_function.cpp b/base_local_planner/src/obstacle_cost_function.cpp index 8688f041f7..91990475fc 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -62,23 +62,25 @@ ObstacleCostFunction::~ObstacleCostFunction() { } } - -void ObstacleCostFunction::setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation, double scaling_speed, bool occdist_use_footprint) { +void ObstacleCostFunction::setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation, + double scaling_speed, bool occdist_use_footprint, double scaling_discount_factor) { // TODO: move this to prepare if possible max_trans_vel_ = max_trans_vel; max_forward_inflation_ = max_forward_inflation; max_sideward_inflation_ = max_sideward_inflation; scaling_speed_ = scaling_speed; occdist_use_footprint_ = occdist_use_footprint; + scaling_discount_factor_ = scaling_discount_factor; } void ObstacleCostFunction::setFootprint(std::vector footprint_spec) { footprint_spec_ = footprint_spec; } -std::vector ObstacleCostFunction::getScaledFootprint(const Trajectory& traj) const { +std::vector ObstacleCostFunction::getScaledFootprint(const Trajectory& traj, unsigned int index) const { + index = std::min(index, traj.getPointsSize() - 1); std::vector scaled_footprint = footprint_spec_; - const double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_); + const double scale = std::pow(scaling_discount_factor_, index) * getScalingFactor(traj, scaling_speed_, max_trans_vel_); if (scale != 0.0) { const bool fwd = traj.xv_ > 0; const double forward_inflation = scale * max_forward_inflation_; @@ -110,12 +112,11 @@ double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) { return -9; } - std::vector scaled_footprint = getScaledFootprint(traj); - const unsigned int point_size = traj.getPointsSize(); // ignore first trajectory point since it is the same for all trajectories, // unless trajectory only contains one point for (unsigned int i = point_size > 1 ? 1 : 0; i < point_size; ++i) { + const auto scaled_footprint = getScaledFootprint(traj, i); traj.getPoint(i, px, py, pth); double f_cost = footprintCost(px, py, pth, scaled_footprint, diff --git a/dwa_local_planner/cfg/DWAPlanner.cfg b/dwa_local_planner/cfg/DWAPlanner.cfg index 72988e32e6..53c8f23410 100755 --- a/dwa_local_planner/cfg/DWAPlanner.cfg +++ b/dwa_local_planner/cfg/DWAPlanner.cfg @@ -25,6 +25,7 @@ gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn b gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325) +gen.add("scaling_discount_factor", double_t, 0, "Discount factor to apply to the scaling of the footprint for future points in the trajectory", 1.0, 0.0, 1.0) gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0) gen.add("max_forward_inflation", double_t, 0, "The maximum length the robot's footprint is extended forward by", 0.2, 0) gen.add("max_sideward_inflation", double_t, 0, "The maximum length the robot's footprint is extended sidewards by", 0.2, 0) diff --git a/dwa_local_planner/include/dwa_local_planner/dwa_planner.h b/dwa_local_planner/include/dwa_local_planner/dwa_planner.h index 6507db0a50..1e9ad55e44 100644 --- a/dwa_local_planner/include/dwa_local_planner/dwa_planner.h +++ b/dwa_local_planner/include/dwa_local_planner/dwa_planner.h @@ -161,9 +161,10 @@ namespace dwa_local_planner { /** * @brief Gets the footprint of the robot scaled by the given trajectory * @param traj Trajectory to evaluate + * @param index Index of the point in the trajectory to evaluate * @return Scaled footprint */ - std::vector getScaledFootprint(const base_local_planner::Trajectory &traj) const; + std::vector getScaledFootprint(const base_local_planner::Trajectory& traj, unsigned int index = 0) const; private: /** 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 ca1399a360..ead0f46351 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 @@ -179,7 +179,7 @@ namespace dwa_local_planner { void publishGlobalPlan(std::vector& path); - void publishScaledFootprint(const geometry_msgs::PoseStamped& pose, const base_local_planner::Trajectory &traj) const; + void publishScaledFootprint(const base_local_planner::Trajectory& traj) const; bool finishedBestEffort(); diff --git a/dwa_local_planner/src/dwa_planner.cpp b/dwa_local_planner/src/dwa_planner.cpp index ef55c097e5..438b4903d6 100644 --- a/dwa_local_planner/src/dwa_planner.cpp +++ b/dwa_local_planner/src/dwa_planner.cpp @@ -81,7 +81,8 @@ namespace dwa_local_planner { alignment_costs_.setXShift(forward_point_distance_); // obstacle costs can vary due to scaling footprint feature - obstacle_costs_.setParams(config.max_vel_trans, config.max_forward_inflation, config.max_sideward_inflation, config.scaling_speed, config.occdist_use_footprint); + obstacle_costs_.setParams(config.max_vel_trans, config.max_forward_inflation, config.max_sideward_inflation, + config.scaling_speed, config.occdist_use_footprint, config.scaling_discount_factor); twirling_costs_.setScale(config.twirling_scale); @@ -225,8 +226,8 @@ namespace dwa_local_planner { return planner_util_->setPlan(orig_global_plan); } - std::vector DWAPlanner::getScaledFootprint(const base_local_planner::Trajectory &traj) const { - return obstacle_costs_.getScaledFootprint(traj); + std::vector DWAPlanner::getScaledFootprint(const base_local_planner::Trajectory& traj, unsigned int index) const { + return obstacle_costs_.getScaledFootprint(traj, index); } /** diff --git a/dwa_local_planner/src/dwa_planner_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index a192543978..a66701680c 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -121,7 +122,7 @@ namespace dwa_local_planner { ros::NodeHandle private_nh("~/" + name); 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); + scaled_fp_pub_ = private_nh.advertise("scaled_footprint", 1); tf_ = tf; costmap_ros_ = costmap_ros; costmap_ros_->getRobotPose(current_pose_); @@ -244,18 +245,39 @@ namespace dwa_local_planner { base_local_planner::publishPlan(path, g_plan_pub_); } - void DWAPlannerROS::publishScaledFootprint(const geometry_msgs::PoseStamped& pose, const base_local_planner::Trajectory &traj) const { - visualization_msgs::Marker marker; - marker.header.frame_id = pose.header.frame_id; - marker.header.stamp = ros::Time::now(); - marker.lifetime = ros::Duration(2 * dp_->getSimPeriod()); // double the sim period to avoid flickering - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.pose = pose.pose; - marker.scale.x = 0.01; - marker.color.g = marker.color.a = 1.0; // green - marker.points = dp_->getScaledFootprint(traj); - marker.points.push_back(marker.points.front()); // close the polygon - scaled_fp_pub_.publish(marker); + void DWAPlannerROS::publishScaledFootprint(const base_local_planner::Trajectory& traj) const { + visualization_msgs::MarkerArray markers; + + for (size_t i = 0; i < traj.getPointsSize(); ++i) { + visualization_msgs::Marker marker; + marker.header.frame_id = costmap_ros_->getGlobalFrameID(); + marker.header.stamp = ros::Time::now(); + marker.ns = i == 0 ? "current_footprint" : "future_footprints"; + marker.id = i; + marker.lifetime = ros::Duration(2 * dp_->getSimPeriod()); // double the sim period to avoid flickering + marker.type = visualization_msgs::Marker::LINE_STRIP; + + double yaw = 0; + traj.getPoint(i, marker.pose.position.x, marker.pose.position.y, yaw); + tf2::Quaternion quat_tf; + quat_tf.setRPY(0, 0, yaw); + marker.pose.orientation = tf2::toMsg(quat_tf); + + // gradient: green -> yellow -> red + // https://stackoverflow.com/a/7947812 + const double ratio = i / static_cast(traj.getPointsSize() - 1); + marker.color.a = 1.0; + marker.color.r = std::min(2 * ratio, 1.0); + marker.color.g = std::min(2 * (1.0 - ratio), 1.0); + marker.scale.x = 0.01; + + marker.points = dp_->getScaledFootprint(traj, i); + marker.points.push_back(marker.points.front()); // close the polygon + + markers.markers.push_back(marker); + } + + scaled_fp_pub_.publish(markers); } DWAPlannerROS::~DWAPlannerROS(){ @@ -386,7 +408,7 @@ namespace dwa_local_planner { } //publish information to the visualizer - publishScaledFootprint(global_pose, path); + publishScaledFootprint(path); publishLocalPlan(local_plan); return mbf_msgs::ExePathResult::SUCCESS; } From 6436ceb97f196bbe9733e29ab0d06d50017ae306 Mon Sep 17 00:00:00 2001 From: siferati <15384781+siferati@users.noreply.github.com> Date: Fri, 17 Nov 2023 09:57:43 +0900 Subject: [PATCH 2/4] swap arg order and return early --- .../include/base_local_planner/obstacle_cost_function.h | 2 +- base_local_planner/src/obstacle_cost_function.cpp | 2 +- dwa_local_planner/src/dwa_planner.cpp | 2 +- dwa_local_planner/src/dwa_planner_ros.cpp | 4 ++++ 4 files changed, 7 insertions(+), 3 deletions(-) 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 fae08af8a5..f1f6efebdc 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 @@ -62,7 +62,7 @@ class ObstacleCostFunction : public TrajectoryCostFunction { void setSumScores(bool score_sums){ sum_scores_=score_sums; } void setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation, - double scaling_speed, bool occdist_use_footprint = true, double scaling_discount_factor = 1.0); + double scaling_speed, double scaling_discount_factor = 1.0, bool occdist_use_footprint = true); void setFootprint(std::vector footprint_spec); std::vector getScaledFootprint(const Trajectory& traj, unsigned int index = 0) const; diff --git a/base_local_planner/src/obstacle_cost_function.cpp b/base_local_planner/src/obstacle_cost_function.cpp index 91990475fc..a9239885ef 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -63,7 +63,7 @@ ObstacleCostFunction::~ObstacleCostFunction() { } void ObstacleCostFunction::setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation, - double scaling_speed, bool occdist_use_footprint, double scaling_discount_factor) { + double scaling_speed, double scaling_discount_factor, bool occdist_use_footprint) { // TODO: move this to prepare if possible max_trans_vel_ = max_trans_vel; max_forward_inflation_ = max_forward_inflation; diff --git a/dwa_local_planner/src/dwa_planner.cpp b/dwa_local_planner/src/dwa_planner.cpp index 438b4903d6..9451ea9ecd 100644 --- a/dwa_local_planner/src/dwa_planner.cpp +++ b/dwa_local_planner/src/dwa_planner.cpp @@ -82,7 +82,7 @@ namespace dwa_local_planner { // obstacle costs can vary due to scaling footprint feature obstacle_costs_.setParams(config.max_vel_trans, config.max_forward_inflation, config.max_sideward_inflation, - config.scaling_speed, config.occdist_use_footprint, config.scaling_discount_factor); + config.scaling_speed, config.scaling_discount_factor, config.occdist_use_footprint); twirling_costs_.setScale(config.twirling_scale); diff --git a/dwa_local_planner/src/dwa_planner_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index a66701680c..b0de3151f9 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -246,6 +246,10 @@ namespace dwa_local_planner { } void DWAPlannerROS::publishScaledFootprint(const base_local_planner::Trajectory& traj) const { + if (scaled_fp_pub_.getNumSubscribers() == 0) { + return; + } + visualization_msgs::MarkerArray markers; for (size_t i = 0; i < traj.getPointsSize(); ++i) { From 44430eba981d615ea5b371b95c0cbbbe664ec721 Mon Sep 17 00:00:00 2001 From: siferati <15384781+siferati@users.noreply.github.com> Date: Wed, 22 Nov 2023 11:16:13 +0900 Subject: [PATCH 3/4] treat discount factor as last one --- base_local_planner/src/obstacle_cost_function.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/base_local_planner/src/obstacle_cost_function.cpp b/base_local_planner/src/obstacle_cost_function.cpp index a9239885ef..4891856718 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -80,7 +80,13 @@ void ObstacleCostFunction::setFootprint(std::vector footpr std::vector ObstacleCostFunction::getScaledFootprint(const Trajectory& traj, unsigned int index) const { index = std::min(index, traj.getPointsSize() - 1); std::vector scaled_footprint = footprint_spec_; - const double scale = std::pow(scaling_discount_factor_, index) * getScalingFactor(traj, scaling_speed_, max_trans_vel_); + + // scaling_discount_factor is the discount factor applied to the last point in the trajectory + // so we need to compute the discount factor gamma to apply to the given index + const double max_index = std::max(traj.getPointsSize() - 1, 1u); + const double gamma = std::pow(scaling_discount_factor_, index / max_index); + + const double scale = gamma * getScalingFactor(traj, scaling_speed_, max_trans_vel_); if (scale != 0.0) { const bool fwd = traj.xv_ > 0; const double forward_inflation = scale * max_forward_inflation_; From ce3f7165e1ecbd56287524d586b20bdd6dc7d280 Mon Sep 17 00:00:00 2001 From: siferati <15384781+siferati@users.noreply.github.com> Date: Mon, 27 Nov 2023 09:51:17 +0900 Subject: [PATCH 4/4] desc --- dwa_local_planner/cfg/DWAPlanner.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dwa_local_planner/cfg/DWAPlanner.cfg b/dwa_local_planner/cfg/DWAPlanner.cfg index 53c8f23410..70e46ff193 100755 --- a/dwa_local_planner/cfg/DWAPlanner.cfg +++ b/dwa_local_planner/cfg/DWAPlanner.cfg @@ -25,7 +25,7 @@ gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn b gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325) -gen.add("scaling_discount_factor", double_t, 0, "Discount factor to apply to the scaling of the footprint for future points in the trajectory", 1.0, 0.0, 1.0) +gen.add("scaling_discount_factor", double_t, 0, "Discount factor to apply to the scaling of the footprint in the last point in the trajectory. Discount factor for other footprints is automatically computed", 1.0, 0.0, 1.0) gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0) gen.add("max_forward_inflation", double_t, 0, "The maximum length the robot's footprint is extended forward by", 0.2, 0) gen.add("max_sideward_inflation", double_t, 0, "The maximum length the robot's footprint is extended sidewards by", 0.2, 0)