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);
}
/**
* 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);
}
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;
}
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;
}
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;
}
void GovernorNode::processPlan(){
libTF::TFPose2D robot_pose, global_pose;
robot_pose.x = 0.0;
robot_pose.y = 0.0;
robot_pose.yaw = 0.0;
robot_pose.frame = "base";
robot_pose.time = 0;
try
{
global_pose = tf_.transformPose2D("map", robot_pose);
}
catch(libTF::TransformReference::LookupException& ex)
{
puts("no global->local Tx yet");
printf("%s\n", ex.what());
return;
}
catch(libTF::TransformReference::ConnectivityException& ex)
{
puts("no global->local Tx yet");
printf("%s\n", ex.what());
return;
}
catch(libTF::TransformReference::ExtrapolateException& ex)
{
// puts("extrapolation required");
// printf("%s\n", ex.what());
return;
}
libTF::TFPose2D robot_vel;
//we need robot_vel_ to compute global_vel so we'll lock
vel_lock.lock();
robot_vel = robot_vel_;
//give robot_vel_ back
vel_lock.unlock();
libTF::TFPose2D drive_cmds;
struct timeval start;
struct timeval end;
double start_t, end_t, t_diff;
//we need to lock the map while we process it
map_lock.lock();
ma_.updateOrigin(map_.origin_x, map_.origin_y);
ma_.updateResolution(map_.scale);
ma_.updateSize(map_.size_x_, map_.size_y_);
gettimeofday(&start,NULL);
Trajectory path = tc_.findBestPath(global_pose, robot_vel, drive_cmds);
gettimeofday(&end,NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
t_diff = end_t - start_t;
fprintf(stderr, "Cycle Time: %.3f\n", t_diff);
//give map_ back
map_lock.unlock();
printf("Robot Vel - vx: %.2f, vy: %.2f, vth: %.2f\n", robot_vel.x, robot_vel.y, robot_vel.yaw);
if(path.cost_ >= 0){
//let's print debug output to the screen
path_msg.set_points_size(tc_.num_steps_);
path_msg.color.r = 0;
path_msg.color.g = 0;
path_msg.color.b = 1.0;
path_msg.color.a = 0;
double x = 0.0;
double y = 0.0;
double th = 0.0;
for(int i = 0; i < tc_.num_steps_; ++i){
double pt_x, pt_y, pt_th;
path.getPoint(i, pt_x, pt_y, pt_th);
path_msg.points[i].x = pt_x;
path_msg.points[i].y = pt_y;
//so we can draw the footprint on the map
if(i == 0){
x = pt_x;
y = pt_y;
th = pt_th;
}
}
publish("local_path", path_msg);
printf("path msg\n");
vector<std_msgs::Point2DFloat32> footprint = tc_.drawFootprint(x, y, th);
//let's also draw the footprint of the robot for the last point on the selected trajectory
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
footprint_msg.color.g = 0;
footprint_msg.color.b = 0;
footprint_msg.color.a = 0;
for(unsigned int i = 0; i < footprint.size(); ++i){
footprint_msg.points[i].x = footprint[i].x;
footprint_msg.points[i].y = footprint[i].y;
//printf("(%.2f, %.2f)\n", footprint_msg.points[i].x, footprint_msg.points[i].y);
}
publish("robot_footprint", footprint_msg);
}
//drive the robot!
cmd_vel_msg_.vx = drive_cmds.x;
cmd_vel_msg_.vy = drive_cmds.y;
cmd_vel_msg_.vw = drive_cmds.yaw;
if(path.cost_ < 0)
printf("Local Plan Failed :(\n");
printf("Vel CMD - vx: %.2f, vy: %.2f, vt: %.2f\n", cmd_vel_msg_.vx, cmd_vel_msg_.vy, cmd_vel_msg_.vw);
publish("cmd_vel", cmd_vel_msg_);
}
