// 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);
}
// 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 UpperBodyPlanner::previewPathPlan(const std::vector<geometry_msgs::Pose>& pose_sequence, move_group_interface::MoveGroup::Plan &plan) {
std::cout << "Adding " << pose_sequence.size() << " points." << std::endl;
moveit_msgs::RobotTrajectory trajectory;
moveit::core::robotStateToRobotStateMsg(*kinematic_state, plan.start_state_);
double fraction = this->computeCartesianPath(pose_sequence, path_step, 0.0, trajectory);
std::cout << "Fraction is: " << fraction << std::endl;
std::cout << "Current position tolerance is: " << this->getGoalPositionTolerance() << std::endl;
std::cout << "Current orientation tolerance is: " << this->getGoalOrientationTolerance() << std::endl;
if (fraction != 1.0) ROS_WARN("The plan is not perfect. Current fraction isn't equal to 1.0");
checkTrajectory(trajectory, plan.trajectory_);
if (singularity_check == false) ROS_WARN("The plan didn't pass singularity check. Suggest to replan.");
ROS_INFO("Preview planning...");
if (fraction != 1.0 || singularity_check == false) return false;
return true;
}
// 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);
}
