/**
* return a score for trajectory traj
*/
virtual double scoreTrajectory(Trajectory &traj)
{
if(traj.getPointsSize()==0)
return 0.0;
double px, py, pth;
traj.getPoint(0, px, py, pth);
double dist_to_goal = hypot(px-goal_x_, py-goal_y_);
if(dist_to_goal < .2){
return 0.0;
}
double start_diff;
if(!getAngleDiff(px,py,pth,&start_diff))
return -4.0;
if(fabs(start_diff) < max_trans_angle_){
return 0.0;
}
if(fabs(traj.xv_) > 0.0 || fabs(traj.yv_) > 0.0)
return -3.0;
else if( sign(start_diff) != sign(traj.thetav_) )
return -2.0;
traj.getPoint(traj.getPointsSize()-1, px, py, pth);
double end_diff;
if(!getAngleDiff(px,py,pth,&end_diff))
return -1.0;
return fabs(end_diff);
}
void CalibrateAction::visualize_trajectory(Trajectory &traj)
{
// publishing to rviz
geometry_msgs::PoseArray poses;
for(unsigned int i=0; i<traj.getPointsSize();i++)
{
geometry_msgs::Pose temp_pose;
double x_, y_,th_;
traj.getPoint(i, x_, y_, th_);
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
temp_pose.position.x = x_;
temp_pose.position.y = y_;
poses.poses.push_back(temp_pose);
//ROS_INFO("Pose: x: %f y: %f th: %f", x_, y_, th_);
}
geometry_msgs::Pose temp_pose, temp_pose2;
double x_, y_,th_;
traj.getPoint(0, x_, y_, th_);
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
temp_pose.position.x = x_;
temp_pose.position.y = y_;
traj.getEndpoint(x_, y_, th_);
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose2.orientation);
temp_pose2.position.x = x_;
temp_pose2.position.y = y_;
//ROS_INFO("Visualize trajectory with %i points. First Point (%2.1f,%2.1f), last Point (%2.1f,%2.1f)", traj.getPointsSize(),temp_pose.position.x,temp_pose.position.y,temp_pose2.position.x,temp_pose2.position.y);
poses.header.frame_id = cost_map->getGlobalFrameID();;
estTraj_pub.publish(poses);
}
double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
double cost = 0;
double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
double px, py, pth;
if (footprint_spec_.size() == 0) {
// Bug, should never happen
ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
return -9;
}
for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
traj.getPoint(i, px, py, pth);
double f_cost = footprintCost(px, py, pth,
scale, footprint_spec_,
costmap_, world_model_);
if(f_cost < 0){
return f_cost;
}
if(sum_scores_)
cost += f_cost;
else
cost = f_cost;
}
return cost;
}
double PathOrientationCostFunction::scoreTrajectory(Trajectory &traj) {
if(traj.getPointsSize()==0)
return 0.0;
double px, py, pth;
traj.getPoint(traj.getPointsSize()-1, px, py, pth);
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(px, py, cell_x, cell_y)) {
//we're off the map
ROS_WARN("Off Map %f, %f", px, py);
return -4.0;
}
unsigned int path_index = map_(cell_x, cell_y).index;
if(path_index>=yaws_.size())
return 0.0;
double diff = fabs(angles::shortest_angular_distance(pth, yaws_[path_index]));
if(diff > max_trans_angle_ && (traj.xv_ > 0.0 || traj.yv_ > 0.0))
return -1.0;
else
return diff;
}
bool CalibrateAction::checkTrajectory(Trajectory& traj)
{
unsigned int size = traj.getPointsSize();
float smallest_dist = INFINITY;
bool isValid;
for(unsigned int i = 0; i< size;i++)
{
geometry_msgs::Pose tempPose;
tf::Pose tfPose;
traj.getPoint(i,tempPose.position.x, tempPose.position.y, tempPose.orientation.z);
tf::poseMsgToTF(tempPose, tfPose);
float dist = getDistanceAtPose(tfPose, &isValid);
// to be able to inform about closest distance
if (dist < smallest_dist)
{
smallest_dist = dist;
}
if ((dist < traj_dist_threshold) && (dist != -INFINITY) && (dist != INFINITY)) // prevent negative infinity value from crashing us
{
ROS_INFO("Critical distance was %f at point %i, point valid: %i", dist, i, isValid);
return false;
}
}
//ROS_INFO("Trajectory check successful, smallest distance was: %f", smallest_dist);
return true;
/*
unsigned int size = traj.getPointsSize();
float smallest_dist = INFINITY;
for(unsigned int i = 0; i< size;i++)
{
geometry_msgs::Pose testPose;
traj.getPoint(i,testPose.position.x, testPose.position.y, testPose.orientation.z);
float dist = voronoi_.getDistance(testPose.position.x, testPose.position.y); // get closest distance from trajectory point to an obstacle
dist *= costmap_.getResolution(); // distance now in meters
if (dist < smallest_dist)
{
smallest_dist = dist;
}
if ((dist < traj_dist_threshold) && (dist != -INFINITY) && (dist != INFINITY)) // prevent negative infinity value from crashing us
{
ROS_INFO("Critical distance was %f at point %i", dist, i);
return false;
}
}
*/
}
double MapGridCostFunction::scoreTrajectory(Trajectory &traj) {
double cost = 0.0;
if (aggregationType_ == Product) {
cost = 1.0;
}
double px, py, pth;
double grid_dist;
for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
traj.getPoint(i, px, py, pth);
grid_dist = scoreCell(px, py, pth);
if(stop_on_failure_){
if (grid_dist == map_.obstacleCosts()) {
return -3.0;
} else if (grid_dist == map_.unreachableCellCosts()) {
return -2.0;
}
}
switch( aggregationType_ ) {
case Last:
cost = grid_dist;
break;
case Sum:
cost += grid_dist;
break;
case Product:
if (cost > 0) {
cost *= grid_dist;
}
break;
}
}
double factor = costmap_->getResolution() * 0.5;
return cost * factor;
}