double PreferStraightCostFunction::scoreTrajectory(base_local_planner::Trajectory & traj)
{
double score = 0.0;
double prev_th;
for (int i = 0; i < traj.getPointsSize(); ++i)
{
double x, y, th;
traj.getPoint(i, x, y, th);
if (i == 0)
{
prev_th = th;
continue;
}
score += fabs(th - prev_th);
prev_th = th;
}
}
double DirectionTrajectoryScorer::scoreTrajectory(base_local_planner::Trajectory& traj) {
obstacle_scorer_->prepare();
//ROS_WARN("Scorer After Prepare ROS Inscribed radius=%.4f", costmap_ros_->getInscribedRadius()); //sd1074
double obstacle_cost = obstacle_scorer_->scoreTrajectory(traj);
ROS_WARN("Obstacle cost: %f", obstacle_cost);
if (obstacle_cost < 0)
return obstacle_cost;
double px, py, pth;
traj.getPoint(traj.getPointsSize()-1, px, py, pth);
//ROS_WARN("tx=%f, dx=%f", px, desired_[0]);
double d2 = (px-desired_[0])*(px-desired_[0]) + (py-desired_[1])*(py-desired_[1]);
return obstacle_scale_*obstacle_cost + d2;
}
double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj)
{
double dist,x,y,theta,near_dist=DBL_MAX,heading_diff=0.0,diff=0.0;
int interval=traj.getPointsSize()/this->num_points;
unsigned int near_index=0,i,j;
for(j=0;j<this->num_points;j++)
{
traj.getPoint((j+1)*interval-1,x,y,theta);
// find the nearrest point on the path
for(i=1;i<this->global_plan.size();i++)
{
dist=sqrt((this->global_plan[i].pose.position.x-x)*(this->global_plan[i].pose.position.x-x)+
(this->global_plan[i].pose.position.y-y)*(this->global_plan[i].pose.position.y-y));
if(dist<near_dist)
{
near_dist=dist;
near_index=i;
}
}
double v1_x,v1_y;
v1_x = this->global_plan[near_index].pose.position.x - this->global_plan[near_index-1].pose.position.x;
v1_y = this->global_plan[near_index].pose.position.y - this->global_plan[near_index-1].pose.position.y;
double v2_x = cos(theta);
double v2_y = sin(theta);
double perp_dot = v1_x * v2_y - v1_y * v2_x;
double dot = v1_x * v2_x + v1_y * v2_y;
//get the signed angle
diff = fabs(atan2(perp_dot, dot));
if(diff>(3.14159/2.0))
diff=fabs(diff-3.14159);
heading_diff+=diff;
}
return heading_diff;
}
/**
* @param pos current position of robot
* @param vel desired velocity for sampling
*/
bool SimpleTrajectoryGenerator::generateTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f sample_target_vel,
base_local_planner::Trajectory& traj) {
double vmag = ::hypot(sample_target_vel[0], sample_target_vel[1]);
double eps = 1e-4;
traj.cost_ = -1.0; // placed here in case we return early
//trajectory might be reused so we'll make sure to reset it
traj.resetPoints();
// make sure that the robot would at least be moving with one of
// the required minimum velocities for translation and rotation (if set)
if ((limits_->min_trans_vel >= 0 && vmag + eps < limits_->min_trans_vel) &&
(limits_->min_rot_vel >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_rot_vel)) {
return false;
}
// make sure we do not exceed max diagonal (x+y) translational velocity (if set)
if (limits_->max_trans_vel >=0 && vmag - eps > limits_->max_trans_vel) {
return false;
}
int num_steps;
if (discretize_by_time_) {
num_steps = ceil(sim_time_ / sim_granularity_);
} else {
//compute the number of steps we must take along this trajectory to be "safe"
double sim_time_distance = vmag * sim_time_; // the distance the robot would travel in sim_time if it did not change velocity
double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_; // the angle the robot would rotate in sim_time
num_steps =
ceil(std::max(sim_time_distance / sim_granularity_,
sim_time_angle / angular_sim_granularity_));
}
//compute a timestep
double dt = sim_time_ / num_steps;
traj.time_delta_ = dt;
Eigen::Vector3f loop_vel;
if (continued_acceleration_) {
// assuming the velocity of the first cycle is the one we want to store in the trajectory object
loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
traj.xv_ = loop_vel[0];
traj.yv_ = loop_vel[1];
traj.thetav_ = loop_vel[2];
} else {
// assuming sample_vel is our target velocity within acc limits for one timestep
loop_vel = sample_target_vel;
traj.xv_ = sample_target_vel[0];
traj.yv_ = sample_target_vel[1];
traj.thetav_ = sample_target_vel[2];
}
//simulate the trajectory and check for collisions, updating costs along the way
for (int i = 0; i < num_steps; ++i) {
//add the point to the trajectory so we can draw it later if we want
traj.addPoint(pos[0], pos[1], pos[2]);
if (continued_acceleration_) {
//calculate velocities
loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
//ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
}
//update the position of the robot using the velocities passed in
pos = computeNewPositions(pos, loop_vel, dt);
} // end for simulation steps
return num_steps > 0; // true if trajectory has at least one point
}
void DWAPlanner::generateTrajectory(Eigen::Vector3f pos, const Eigen::Vector3f& vel, base_local_planner::Trajectory& traj, bool two_point_scoring, double sq_dist){
//ROS_ERROR("%.2f, %.2f, %.2f - %.2f %.2f", vel[0], vel[1], vel[2], sim_time_, sim_granularity_);
double impossible_cost = map_.map_.size();
double vmag = sqrt(vel[0] * vel[0] + vel[1] * vel[1]);
double eps = 1e-4;
//make sure that the robot is at least moving with one of the required velocities
if((vmag + eps < min_vel_trans_ && fabs(vel[2]) + eps < min_rot_vel_) ||
vmag - eps > max_vel_trans_ ||
oscillationCheck(vel)){
traj.cost_ = -1.0;
return;
}
//compute the number of steps we must take along this trajectory to be "safe"
int num_steps = ceil(std::max((vmag * sim_time_) / sim_granularity_, fabs(vel[2]) / sim_granularity_));
//compute a timestep
double dt = sim_time_ / num_steps;
double time = 0.0;
//initialize the costs for the trajectory
double path_dist = 0.0;
double goal_dist = 0.0;
double occ_cost = 0.0;
//we'll also be scoring a point infront of the robot
double front_path_dist = 0.0;
double front_goal_dist = 0.0;
//create a potential trajectory... it might be reused so we'll make sure to reset it
traj.resetPoints();
traj.xv_ = vel[0];
traj.yv_ = vel[1];
traj.thetav_ = vel[2];
traj.cost_ = -1.0;
//if we're not actualy going to simulate... we may as well just return now
if(num_steps == 0){
traj.cost_ = -1.0;
return;
}
//we want to check against the absolute value of the velocities for collisions later
//Eigen::Vector3f abs_vel = vel.array().abs();
//simulate the trajectory and check for collisions, updating costs along the way
for(int i = 0; i < num_steps; ++i){
//get the mapp coordinates of a point
unsigned int cell_x, cell_y;
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if(!costmap_.worldToMap(pos[0], pos[1], cell_x, cell_y)){
//we're off the map
traj.cost_ = -1.0;
return;
}
double front_x = pos[0] + forward_point_distance_ * cos(pos[2]);
double front_y = pos[1] + forward_point_distance_ * sin(pos[2]);
unsigned int front_cell_x, front_cell_y;
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if(!costmap_.worldToMap(front_x, front_y, front_cell_x, front_cell_y)){
//we're off the map
traj.cost_ = -1.0;
return;
}
//if we're over a certain speed threshold, we'll scale the robot's
//footprint to make it either slow down or stay further from walls
double scale = 1.0;
if(vmag > scaling_speed_){
//scale up to the max scaling factor linearly... this could be changed later
double ratio = (vmag - scaling_speed_) / (max_vel_trans_ - scaling_speed_);
scale = max_scaling_factor_ * ratio + 1.0;
}
//we want to find the cost of the footprint
double footprint_cost = footprintCost(pos, scale);
//if the footprint hits an obstacle... we'll check if we can stop before we hit it... given the time to get there
if(footprint_cost < 0){
traj.cost_ = -1.0;
return;
/* TODO: I'm not convinced this code is working properly
//we want to compute the max allowable speeds to be able to stop
//to be safe... we'll make sure we can stop some time before we actually hit
Eigen::Vector3f max_vel = getMaxSpeedToStopInTime(time - stop_time_buffer_);
//check if we can stop in time
if(abs_vel[0] < max_vel[0] && abs_vel[1] < max_vel[1] && abs_vel[2] < max_vel[2]){
//if we can, then we'll just break out of the loop here.. no point in checking future points
break;
}
else{
//if we can't... then this trajectory is invalid
traj.cost_ = -1.0;
return;
}
*/
}
//compute the costs for this point on the trajectory
occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
path_dist = map_(cell_x, cell_y).path_dist;
goal_dist = map_(cell_x, cell_y).goal_dist;
front_path_dist = front_map_(front_cell_x, front_cell_y).path_dist;
front_goal_dist = front_map_(front_cell_x, front_cell_y).goal_dist;
//if a point on this trajectory has no clear path to the goal... it is invalid
if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
traj.cost_ = -2.0; //-2.0 means that we were blocked because propagation failed
return;
}
//add the point to the trajectory so we can draw it later if we want
traj.addPoint(pos[0], pos[1], pos[2]);
//update the position of the robot using the velocities passed in
pos = computeNewPositions(pos, vel, dt);
time += dt;
}
double goal_heading = tf::getYaw(global_plan_.back().pose.orientation);
double heading_diff = fabs(angles::shortest_angular_distance(pos[2],goal_heading));
//ROS_ERROR("%.2f, %.2f", goal_heading, heading_diff);
double resolution = costmap_.getResolution();
//double sq_dist = squareDist(robot_pose, global_plan_.back());
double heading_scale_ = 0;
if (sq_dist < squared_dist_for_rotating_) {
heading_scale_ = heading_bias_;
}
//if we're not at the last point in the plan, then we can just score
if(two_point_scoring)
traj.cost_ = pdist_scale_ * resolution * ((front_path_dist + path_dist) / 2.0) + gdist_scale_ * resolution * ((front_goal_dist + goal_dist) / 2.0) + occdist_scale_ * occ_cost + heading_scale_ * heading_diff;
else
traj.cost_ = pdist_scale_ * resolution * path_dist + gdist_scale_ * resolution * goal_dist + occdist_scale_ * occ_cost + heading_scale_ * heading_diff;
//ROS_ERROR("%.2f, %.2f, %.2f, %.2f", vel[0], vel[1], vel[2], traj.cost_);
}