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..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 @@ -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, double scaling_discount_factor = 1.0, bool occdist_use_footprint = true); 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..4891856718 100644 --- a/base_local_planner/src/obstacle_cost_function.cpp +++ b/base_local_planner/src/obstacle_cost_function.cpp @@ -62,23 +62,31 @@ 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, 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; 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_); + + // 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_; @@ -110,12 +118,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..70e46ff193 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 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) 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..9451ea9ecd 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.scaling_discount_factor, config.occdist_use_footprint); 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..b0de3151f9 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,43 @@ 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 { + if (scaled_fp_pub_.getNumSubscribers() == 0) { + return; + } + + 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 +412,7 @@ namespace dwa_local_planner { } //publish information to the visualizer - publishScaledFootprint(global_pose, path); + publishScaledFootprint(path); publishLocalPlan(local_plan); return mbf_msgs::ExePathResult::SUCCESS; }