std::pair<std::vector<double>, std::vector<double>> TrajectoryGenerator::getOptimalTrajectory() {
std::pair<int, double> lane_speed = fsm.getLaneSpeed();
auto trajectory = generateTrajectory(lane_speed.first, lane_speed.second, 30);
auto collision = checkCollision(trajectory.first, trajectory.second);
bool collision_detected = std::get<0>(collision);
double time_to_collision = std::get<2>(collision);
if (collision_detected && time_to_collision < 2.) {
double agent_speed = std::get<1>(collision);
double target_speed = std::min(agent_speed, lane_speed.second);
return generateTrajectory(lane_speed.first, target_speed, 5);
}
return trajectory;
}
// checks if a path is clear from current position for given speed, goal speed, time and accel
bool CalibrateAction::checkPath(double vx, double vy, double vtheta, double sim_time_, double vx_samp, double vy_samp, double vtheta_samp,
double acc_x, double acc_y, double acc_theta)
{
Trajectory traj;
tf::StampedTransform transform;
try
{
// shift lookup in the past by 100ms to ensure cache is not empty
listener->lookupTransform(cost_map->getGlobalFrameID(),robotFrame,ros::Time(0.0), transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("Nope! %s", ex.what());
return false;
}
//ROS_INFO("gen. Traj with pos x: %f, y: %f, th: %f, vel: x: %f, y:%f, th: %f, dur: %f", transform.getOrigin().getX(), transform.getOrigin().getY(), tf::getYaw(transform.getRotation()), vx, vy, vtheta, sim_time_);
// generate a trajectory for the given goal speed, rotation and time. No acceleration needed here
generateTrajectory(transform.getOrigin().getX(), transform.getOrigin().getY(), tf::getYaw(transform.getRotation()),
vx, vy, vtheta, sim_time_, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, traj);
visualize_trajectory(traj);
return checkTrajectory(traj);
}
bool TrajectoryGenerator::createpPreviewTraj(int type)
{
float fXstart, fXend, fYstart, fYend;
if(type == 0) {
//XZ
fXstart = - m_pScanPatt->m_fXRangeV / 2 + m_pScanPatt->m_fXOffsetV;
fXend = m_pScanPatt->m_fXRangeV / 2 + m_pScanPatt->m_fXOffsetV;
fYstart = m_pScanPatt->m_fYOffsetV;
fYend = m_pScanPatt->m_fYOffsetV;
m_bXPriority = true;
}else{
//YZ
fXstart = - m_pScanPatt->m_fYRangeV / 2 + m_pScanPatt->m_fYOffsetV;
fXend = m_pScanPatt->m_fYRangeV / 2 + m_pScanPatt->m_fYOffsetV;
fYstart = m_pScanPatt->m_fXOffsetV;
fYend = m_pScanPatt->m_fXOffsetV;
m_bXPriority = false;
}
//printf("fXstart: %f, fYstart: %f, fXend: %f, fYend: %f.\n", fXstart, fYstart, fXend, fYend);
m_uiNumAscan = m_pTrajCali->m_iPreviewAScan;
m_uiNumBscan = 1;
bool re = generateTrajectory(fXstart, fXend, fYstart, fYend, 0);
return re;
}
// checks if a path is clear from a given position for given speed, goal speed, time and accel
bool CalibrateAction::checkPath(double xPos, double yPos, double thetaPos, double vx, double vy, double vtheta, double sim_time_, double vx_samp, double vy_samp, double vtheta_samp,
double acc_x, double acc_y, double acc_theta)
{
Trajectory traj;
// generate a trajectory for the given goal speed, rotation and time. No acceleration needed here
generateTrajectory(xPos, yPos, thetaPos, vx, vy, vtheta, sim_time_, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, traj);
visualize_trajectory(traj);
return checkTrajectory(traj);
}
bool DWAPlanner::checkTrajectory(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel){
resetOscillationFlags();
base_local_planner::Trajectory t;
generateTrajectory(pos, vel, t, false, 0);
//if the trajectory is a legal one... the check passes
if(t.cost_ >= 0)
return true;
//otherwise the check fails
return false;
}
bool TrajectoryGenerator::createFundusViewTraj()
{
float fXstart = - m_pScanPatt->m_fXRangeV / 2 + m_pTrajCali->m_fFundusXOffsetV;
float fXend = m_pScanPatt->m_fXRangeV / 2 + m_pTrajCali->m_fFundusXOffsetV;
float fYstart = - m_pScanPatt->m_fYRangeV / 2 + m_pTrajCali->m_fFundusYOffsetV;
float fYend = m_pScanPatt->m_fYRangeV / 2 + m_pTrajCali->m_fFundusYOffsetV;
m_uiNumAscan = m_pTrajCali->m_iFundusViewAScan;
m_uiNumBscan = m_pTrajCali->m_iFundusViewAScan;
bool re = generateTrajectory(fXstart, fXend, fYstart, fYend, 0);
return re;
}
bool TrajectoryGenerator::createpBackgroundTraj()
{
float fXstart = - m_pScanPatt->m_fXRangeV / 2 + m_pScanPatt->m_fXOffsetV;
float fXend = m_pScanPatt->m_fXRangeV / 2 + m_pScanPatt->m_fXOffsetV;
float fYstart = - m_pTrajCali->m_fYMaxV;
float fYend = - m_pTrajCali->m_fYMaxV;
m_uiNumAscan = m_pTrajCali->m_iBackgroundAScan;
m_uiNumBscan = 1;
m_bXPriority = true;
bool re = generateTrajectory(fXstart, fXend, fYstart, fYend, 0);
return re;
}
/**
* Create and return the next sample trajectory
*/
bool SimpleTrajectoryGenerator::nextTrajectory(Trajectory &comp_traj) {
bool result = false;
if (hasMoreTrajectories()) {
if (generateTrajectory(
pos_,
vel_,
sample_params_[next_sample_index_],
comp_traj)) {
result = true;
}
}
next_sample_index_++;
return result;
}
bool TrajectoryGenerator::createAcquireTraj()
{
float fXstart = - m_pScanPatt->m_fXRangeV / 2 + m_pScanPatt->m_fXOffsetV;
float fXend = m_pScanPatt->m_fXRangeV / 2 + m_pScanPatt->m_fXOffsetV;
float fYstart = - m_pScanPatt->m_fYRangeV / 2 + m_pScanPatt->m_fYOffsetV;
float fYend = m_pScanPatt->m_fYRangeV / 2 + m_pScanPatt->m_fYOffsetV;
m_uiNumAscan = m_pScanPatt->m_uiNumAscan;
m_uiNumBscan = m_pScanPatt->m_uiNumBscan;
//generateTrajectory(fXstart, fXend, fYstart, fYend);
bool re = generateTrajectory(fXstart, fXend, fYstart, fYend, m_pScanPatt->m_uiNumBscanRepeat);
return re;
}
void singlePlanner::executeTrajectory(vector<Vector> &_bestTraj,
vector<Vector> &_bestTrajRoot,
const string &color)
{
updatePlanner();
_bestTraj = generateTrajectory();
_bestTrajRoot = getBestTrajRoot();
printTrajectory();
if (running_mode=="single")
{
displayPlan(color);
logTrajectory();
}
}
// checks path when starting from zero speed with unknown speed up time
bool CalibrateAction::checkPath(double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp,
double acc_x, double acc_y, double acc_theta, Trajectory *resultTraj)
{
Trajectory traj;
tf::StampedTransform transform;
try
{
// shift lookup in the past by 100ms to ensure cache is not empty
listener->lookupTransform(cost_map->getGlobalFrameID(),robotFrame,ros::Time(0.0), transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("Nope! %s", ex.what());
return false;
}
// getting maximum needed sim_time_, absolute value, as time can't be negative, but speed can be
double sim_time_x = fabs((vx_samp - vx)/acc_x);
double sim_time_y = fabs((vy_samp - vy)/acc_y);
double sim_time_theta = fabs((vtheta_samp - vtheta)/acc_theta);
double sim_time_ = std::max(sim_time_x,sim_time_y);
sim_time_ = std::max(sim_time_,sim_time_theta);
sim_time_ = std::max(sim_time_, min_time_clear); // always check ahead at least 'min_time_clear' seconds
//ROS_INFO("gen. Traj with pos x: %f, y: %f, th: %f, vel: x: %f, y:%f, th: %f, dur: %f", transform.getOrigin().getX(), transform.getOrigin().getY(), tf::getYaw(transform.getRotation()), vx, vy, vtheta, sim_time_);
// generate a trajectory for the given goal speed, rotation and time. No acceleration needed here
generateTrajectory(transform.getOrigin().getX(), transform.getOrigin().getY(), tf::getYaw(transform.getRotation()),
vx, vy, vtheta, sim_time_, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, traj);
visualize_trajectory(traj);
if(resultTraj != NULL)
{
*resultTraj = traj;
}
return checkTrajectory(traj);
}
AlphaList
MCGS_improve( AlphaList **projections, PomdpSolveParams param ) {
Trajectory trajectory, t;
int trajectory_count = 0;
AlphaList old_vector_list;
AlphaList new_vector_list;
double gen_time, prune_time;
double tot_gen_time = 0.0, tot_prune_time = 0.0;
int m, num_new_points = 0;
/* FIXME: Need to rethink how this works before making it
available. */
Abort( "MCGS method not yet available. Sorry.");
if ( param->opts->verbose == POMDP_SOLVE_OPTS_Verbose_mcgs ) {
fprintf( param->report_file,
"<<Monte Carlo Gauss Seidel Approximation>>\n" );
} /* if verbose */
/* Use an old and a new and periodically merge them. */
old_vector_list = newAlphaList();
new_vector_list = newAlphaList();
trajectory = newTrajectory( param->opts->mcgs_traj_length );
if ( param->opts->verbose == POMDP_SOLVE_OPTS_Verbose_mcgs )
fprintf( param->report_file,
"Using trajectory length %d.\n",
param->opts->mcgs_traj_length );
while ( (! gInterrupt)
&& param->opts->mcgs_num_traj > trajectory_count ) {
/* Generate a trajectory of length N from starting state,
accumulating a set of N points. Uses random selection for the
policy (the NULL parameter specifies this). */
generateTrajectory( trajectory, gInitialBelief, NULL );
trajectory_count++;
if ( param->opts->verbose == POMDP_SOLVE_OPTS_Verbose_mcgs )
fprintf( param->report_file,
"Trajectory %d.\n", trajectory_count );
/* Loop over this set of points M times. */
for( m = 0; m < param->opts->mcgs_traj_iter_count; m++ ) {
/* Loop over the points from the trajectory. */
for ( t = trajectory; t != NULL; t = t->next ) {
if ( gInterrupt )
goto END_MCGS;
/* For each point, and the current alpha vector list, generate the
vector for this point. This routine also adds the vector to
the list (if it is not already in the list. */
makeAlphaVector( new_vector_list,
projections,
t->belief,
SMALLEST_PRECISION );
/* FIXME: On review, this does not look right to me. Seems
like we would only want to increment *if* the new vector is
added, and further, this is a count of new *value function
facets* not points. Also, seems like we can just look at
the size of the new list to determine whether it is time to
merge and prune. */
num_new_points++;
if ( param->opts->mcgs_prune_freq == num_new_points ) {
num_new_points = 0;
if ( param->opts->verbose == POMDP_SOLVE_OPTS_Verbose_mcgs ) {
fprintf( param->report_file,
"Changing value function..." );
fflush( param->report_file );
} /* if versboe */
/* First merge the two sets... */
unionTwoAlphaLists( old_vector_list, new_vector_list );
/* ... create a new list ... */
new_vector_list = newAlphaList();
if ( param->opts->verbose == POMDP_SOLVE_OPTS_Verbose_mcgs )
fprintf( param->report_file,
"%d vectors -> ", sizeAlphaList( old_vector_list ) );
/* ... then prune this set */
normalPrune( old_vector_list, param );
/* Clear the old projection set ... */
clearAllProjections( projections );
/* ... and create the new projection set */
setAllProjections( projections, old_vector_list );
if ( gVerbose[V_APPROX_MCGS] )
fprintf( param->report_file,
"%d vectors.\n", sizeAlphaList( old_vector_list ) );
} /* if time to prune */
} /* for n */
} /* for m */
} /* while not done */
END_MCGS:
destroyAlphaList( new_vector_list );
freeTrajectory( trajectory );
freeAllProjections( projections );
return ( old_vector_list );
} /* MCGS_improve */
base_local_planner::Trajectory DWAPlanner::computeTrajectories(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel){
tf::Stamped<tf::Pose> robot_pose_tf;
geometry_msgs::PoseStamped robot_pose;
//compute the distance between the robot and the last point on the global_plan
costmap_ros_->getRobotPose(robot_pose_tf);
tf::poseStampedTFToMsg(robot_pose_tf, robot_pose);
double sq_dist = squareDist(robot_pose, global_plan_.back());
bool two_point_scoring = true;
if(sq_dist < forward_point_distance_ * forward_point_distance_)
ROS_INFO("single points scoring");
two_point_scoring = false;
//compute the feasible velocity space based on the rate at which we run
Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
max_vel[0] = std::min(max_vel_x_, vel[0] + acc_lim_[0] * sim_period_);
max_vel[1] = std::min(max_vel_y_, vel[1] + acc_lim_[1] * sim_period_);
max_vel[2] = std::min(max_vel_th_, vel[2] + acc_lim_[2] * sim_period_);
Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
min_vel[0] = std::max(min_vel_x_, vel[0] - dec_lim_[0] * sim_period_);
min_vel[1] = std::max(min_vel_y_, vel[1] - dec_lim_[1] * sim_period_);
min_vel[2] = std::max(min_vel_th_, vel[2] - dec_lim_[2] * sim_period_);
Eigen::Vector3f dv = Eigen::Vector3f::Zero();
//we want to sample the velocity space regularly
for(unsigned int i = 0; i < 3; ++i){
dv[i] = (max_vel[i] - min_vel[i]) / (std::max(1.0, double(vsamples_[i]) - 1));
}
//keep track of the best trajectory seen so far... we'll re-use two memeber vars for efficiency
base_local_planner::Trajectory* best_traj = &traj_one_;
best_traj->cost_ = -1.0;
base_local_planner::Trajectory* comp_traj = &traj_two_;
comp_traj->cost_ = -1.0;
Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
//ROS_ERROR("x(%.2f, %.2f), y(%.2f, %.2f), th(%.2f, %.2f)", min_vel[0], max_vel[0], min_vel[1], max_vel[1], min_vel[2], max_vel[2]);
//ROS_ERROR("x(%.2f, %.2f), y(%.2f, %.2f), th(%.2f, %.2f)", min_vel_x_, max_vel_x_, min_vel_y_, max_vel_y_, min_vel_th_, max_vel_th_);
//ROS_ERROR("dv %.2f %.2f %.2f", dv[0], dv[1], dv[2]);
for(VelocityIterator x_it(min_vel[0], max_vel[0], dv[0]); !x_it.isFinished(); x_it++){
vel_samp[0] = x_it.getVelocity();
for(VelocityIterator y_it(min_vel[1], max_vel[1], dv[1]); !y_it.isFinished(); y_it++){
vel_samp[1] = y_it.getVelocity();
for(VelocityIterator th_it(min_vel[2], max_vel[2], dv[2]); !th_it.isFinished(); th_it++){
vel_samp[2] = th_it.getVelocity();
generateTrajectory(pos, vel_samp, *comp_traj, two_point_scoring, sq_dist);
selectBestTrajectory(best_traj, comp_traj);
}
}
}
ROS_DEBUG_NAMED("oscillation_flags", "forward_pos_only: %d, forward_neg_only: %d, strafe_pos_only: %d, strafe_neg_only: %d, rot_pos_only: %d, rot_neg_only: %d",
forward_pos_only_, forward_neg_only_, strafe_pos_only_, strafe_neg_only_, rot_pos_only_, rot_neg_only_);
//ok... now we have our best trajectory
if(best_traj->cost_ >= 0){
//we want to check if we need to set any oscillation flags
if(setOscillationFlags(best_traj)){
prev_stationary_pos_ = pos;
}
//if we've got restrictions... check if we can reset any oscillation flags
if(forward_pos_only_ || forward_neg_only_
|| strafe_pos_only_ || strafe_neg_only_
|| rot_pos_only_ || rot_neg_only_){
resetOscillationFlagsIfPossible(pos, prev_stationary_pos_);
}
}
//TODO: Think about whether we want to try to do things like back up when a valid trajectory is not found
return *best_traj;
}
