/*
* given the current state of the robot, find a good trajectory
*/
base_local_planner::Trajectory DWAPlanner::findBestPath(
tf::Stamped<tf::Pose> global_pose,
tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities,
std::vector<geometry_msgs::Point> footprint_spec) {
obstacle_costs_.setFootprint(footprint_spec);
//make sure that our configuration doesn't change mid-run
boost::mutex::scoped_lock l(configuration_mutex_);
Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
// prepare cost functions and generators for this run
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);
result_traj_.cost_ = -7;
// find best trajectory by sampling and scoring the samples
scored_sampling_planner_.findBestTrajectory(result_traj_, NULL);
// verbose publishing of point clouds
if (publish_cost_grid_pc_) {
//we'll publish the visualization of the costs to rviz before returning our best trajectory
map_viz_.publishCostCloud(*(planner_util_->getCostmap()));
}
// debrief stateful scoring functions
oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_trans_vel);
//if we don't have a legal trajectory, we'll just command zero
if (result_traj_.cost_ < 0) {
drive_velocities.setIdentity();
} else {
tf::Vector3 start(result_traj_.xv_, result_traj_.yv_, 0);
drive_velocities.setOrigin(start);
tf::Matrix3x3 matrix;
matrix.setRotation(tf::createQuaternionFromYaw(result_traj_.thetav_));
drive_velocities.setBasis(matrix);
}
return result_traj_;
}
//given the current state of the robot, find a good trajectory
base_local_planner::Trajectory DWAPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities){
//make sure that our configuration doesn't change mid-run
boost::mutex::scoped_lock l(configuration_mutex_);
//make sure to get an updated copy of the costmap before computing trajectories
costmap_ros_->getCostmapCopy(costmap_);
Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
//reset the map for new operations
map_.resetPathDist();
front_map_.resetPathDist();
//make sure that we update our path based on the global plan and compute costs
map_.setPathCells(costmap_, global_plan_);
std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x + forward_point_distance_ * cos(tf::getYaw(front_global_plan.back().pose.orientation));
front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ * sin(tf::getYaw(front_global_plan.back().pose.orientation));
front_map_.setPathCells(costmap_, front_global_plan);
ROS_DEBUG_NAMED("dwa_local_planner", "Path/Goal distance computed");
//rollout trajectories and find the minimum cost one
base_local_planner::Trajectory best = computeTrajectories(pos, vel);
ROS_DEBUG_NAMED("dwa_local_planner", "Trajectories created");
//if we don't have a legal trajectory, we'll just command zero
if(best.cost_ < 0){
drive_velocities.setIdentity();
}
else{
tf::Vector3 start(best.xv_, best.yv_, 0);
drive_velocities.setOrigin(start);
tf::Matrix3x3 matrix;
matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
drive_velocities.setBasis(matrix);
}
//we'll publish the visualization of the costs to rviz before returning our best trajectory
map_viz_.publishCostCloud();
return best;
}
bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
global_pose.setIdentity();
tf::Stamped < tf::Pose > robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
robot_pose.stamp_ = ros::Time();
ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
//get the global pose of the robot
try
{
tf_.transformPose(global_frame_, robot_pose, global_pose);
}
catch (tf::LookupException& ex)
{
ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ConnectivityException& ex)
{
ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ExtrapolationException& ex)
{
ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
return false;
}
// check global_pose timeout
if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_)
{
ROS_WARN_THROTTLE(1.0,
"Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_);
return false;
}
return true;
}
bool BallPickerLayer::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
global_pose.setIdentity();
tf::Stamped <tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
robot_pose.stamp_ = ros::Time();
try
{
tfl.transformPose(global_frame_, robot_pose, global_pose);
}
catch (tf::TransformException& ex)
{
ROS_ERROR_THROTTLE(1.0, "TF exception looking up robot pose: %s/n", ex.what());
return false;
}
return true;
}
/*
* given the current state of the robot, find a good trajectory
*/
base_local_planner::Trajectory EDWAPlanner::findBestPath(
tf::Stamped<tf::Pose> global_pose,
tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities,
std::vector<geometry_msgs::Point> footprint_spec) {
obstacle_costs_.setFootprint(footprint_spec);
//make sure that our configuration doesn't change mid-run
boost::mutex::scoped_lock l(configuration_mutex_);
Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
// prepare cost functions and generators for this run
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);
energy_costs_.setLastSpeeds(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
result_traj_.cost_ = -7;
// find best trajectory by sampling and scoring the samples
std::vector<base_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);
if(publish_traj_pc_)
{
base_local_planner::MapGridCostPoint pt;
traj_cloud_->points.clear();
traj_cloud_->width = 0;
traj_cloud_->height = 0;
std_msgs::Header header;
pcl_conversions::fromPCL(traj_cloud_->header, header);
header.stamp = ros::Time::now();
traj_cloud_->header = pcl_conversions::toPCL(header);
for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
{
if(t->cost_<0)
continue;
// Fill out the plan
for(unsigned int i = 0; i < t->getPointsSize(); ++i) {
double p_x, p_y, p_th;
t->getPoint(i, p_x, p_y, p_th);
pt.x=p_x;
pt.y=p_y;
pt.z=0;
pt.path_cost=p_th;
pt.total_cost=t->cost_;
traj_cloud_->push_back(pt);
}
}
traj_cloud_pub_.publish(*traj_cloud_);
}
// verbose publishing of point clouds
if (publish_cost_grid_pc_) {
//we'll publish the visualization of the costs to rviz before returning our best trajectory
map_viz_.publishCostCloud(planner_util_->getCostmap());
}
// debrief stateful scoring functions
oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_trans_vel);
//if we don't have a legal trajectory, we'll just command zero
if (result_traj_.cost_ < 0) {
drive_velocities.setIdentity();
} else {
tf::Vector3 start(result_traj_.xv_, result_traj_.yv_, 0);
drive_velocities.setOrigin(start);
tf::Matrix3x3 matrix;
matrix.setRotation(tf::createQuaternionFromYaw(result_traj_.thetav_));
drive_velocities.setBasis(matrix);
}
return result_traj_;
}
//given the current state of the robot, find a good trajectory
Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities){
Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
//reset the map for new operations
path_map_.resetPathDist();
goal_map_.resetPathDist();
//temporarily remove obstacles that are within the footprint of the robot
std::vector<base_local_planner::Position2DInt> footprint_list =
footprint_helper_.getFootprintCells(
pos,
footprint_spec_,
costmap_,
true);
//mark cells within the initial footprint of the robot
for (unsigned int i = 0; i < footprint_list.size(); ++i) {
path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
}
//make sure that we update our path based on the global plan and compute costs
path_map_.setTargetCells(costmap_, global_plan_);
goal_map_.setLocalGoal(costmap_, global_plan_);
ROS_DEBUG("Path/Goal distance computed");
//rollout trajectories and find the minimum cost one
Trajectory best = createTrajectories(pos[0], pos[1], pos[2],
vel[0], vel[1], vel[2],
acc_lim_x_, acc_lim_y_, acc_lim_theta_);
ROS_DEBUG("Trajectories created");
/*
//If we want to print a ppm file to draw goal dist
char buf[4096];
sprintf(buf, "base_local_planner.ppm");
FILE *fp = fopen(buf, "w");
if(fp){
fprintf(fp, "P3\n");
fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
fprintf(fp, "255\n");
for(int j = map_.size_y_ - 1; j >= 0; --j){
for(unsigned int i = 0; i < map_.size_x_; ++i){
int g_dist = 255 - int(map_(i, j).goal_dist);
int p_dist = 255 - int(map_(i, j).path_dist);
if(g_dist < 0)
g_dist = 0;
if(p_dist < 0)
p_dist = 0;
fprintf(fp, "%d 0 %d ", g_dist, 0);
}
fprintf(fp, "\n");
}
fclose(fp);
}
*/
if(best.cost_ < 0){
drive_velocities.setIdentity();
}
else{
tf::Vector3 start(best.xv_, best.yv_, 0);
drive_velocities.setOrigin(start);
tf::Matrix3x3 matrix;
matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
drive_velocities.setBasis(matrix);
}
return best;
}