void CartesianPositionController::commandCB_cartesian(const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
{
ROS_WARN_STREAM("CartesianPositionController: Pose Command Recieved");
// convert message to transform
tf::Stamped<tf::Pose> pose_stamped;
poseStampedMsgToTF(*pose_msg, pose_stamped);
// convert to reference frame of root link of the controller chain
// tf_.transformPose(root_name_, pose_stamped, pose_stamped);
tf::PoseTFToKDL(pose_stamped, pose_desired_);
joint_command_lock_ = true;
}
bool PoseFollower::transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
transformed_plan.clear();
try{
if (!global_plan.size() > 0)
{
ROS_ERROR("Recieved plan with zero length");
return false;
}
tf::StampedTransform transform;
tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, transform);
tf::Stamped<tf::Pose> tf_pose;
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
for(unsigned int i = 0; i < global_plan.size(); ++i){
const geometry_msgs::PoseStamped& pose = global_plan[i];
poseStampedMsgToTF(pose, tf_pose);
tf_pose.setData(transform * tf_pose);
tf_pose.stamp_ = transform.stamp_;
tf_pose.frame_id_ = global_frame;
poseStampedTFToMsg(tf_pose, newer_pose);
transformed_plan.push_back(newer_pose);
}
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
bool getGoalPose (const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame, tf::Stamped<tf::Pose>& goal_pose)
{
if (global_plan.empty())
{
ROS_ERROR ("Received plan with zero length");
return false;
}
const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
try
{
tf::StampedTransform transform;
tf.waitForTransform (global_frame, ros::Time::now(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, ros::Duration (0.5));
tf.lookupTransform (global_frame, ros::Time(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, transform);
poseStampedMsgToTF (plan_goal_pose, goal_pose);
goal_pose.setData (transform * goal_pose);
goal_pose.stamp_ = transform.stamp_;
goal_pose.frame_id_ = global_frame;
}
catch (tf::LookupException& ex)
{
ROS_ERROR ("No Transform available Error: %s\n", ex.what());
return false;
}
catch (tf::ConnectivityException& ex)
{
ROS_ERROR ("Connectivity Error: %s\n", ex.what());
return false;
}
catch (tf::ExtrapolationException& ex)
{
ROS_ERROR ("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR ("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int) global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
bool
BKPlanner::poseToGlobalFrame(const PoseStamped& pose, PoseStamped& transformed)
{
std::string global_frame = planner_costmap_->getGlobalFrameID();
tf::Stamped<tf::Pose> pose_tf, global_tf;
poseStampedMsgToTF(pose, pose_tf);
try {
tf_.transformPose(global_frame, ros::Time::now()-ros::Duration(0.0), pose_tf, global_frame, global_tf);
}
catch(tf::TransformException& ex) {
//(target_frame, target_time, pin, fixed_frame, pout)
ROS_ERROR("[poseToGlobalFrame] Failed to transform goal pose from \"%s\" to \"%s\" frame: %s",
pose_tf.frame_id_.c_str(), global_frame.c_str(), ex.what());
return false;
}
tf::poseStampedTFToMsg(global_tf, transformed);
return true;
}
bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg,
geometry_msgs::PoseStamped &pose_msg_out,
const std::string &root_frame,
tf::TransformListener& tf)
{
geometry_msgs::PoseStamped pose_msg_in = pose_msg;
ROS_DEBUG("Request:\nframe_id: %s\nPosition: %f %f %f\n:Orientation: %f %f %f %f\n",
pose_msg_in.header.frame_id.c_str(),
pose_msg_in.pose.position.x,
pose_msg_in.pose.position.y,
pose_msg_in.pose.position.z,
pose_msg_in.pose.orientation.x,
pose_msg_in.pose.orientation.y,
pose_msg_in.pose.orientation.z,
pose_msg_in.pose.orientation.w);
pose_msg_out = pose_msg;
tf::Stamped<tf::Pose> pose_stamped;
poseStampedMsgToTF(pose_msg_in, pose_stamped);
if (!tf.canTransform(root_frame, pose_stamped.frame_id_, pose_stamped.stamp_))
{
std::string err;
if (tf.getLatestCommonTime(pose_stamped.frame_id_, root_frame, pose_stamped.stamp_, &err) != tf::NO_ERROR)
{
ROS_ERROR("pr2_arm_ik:: Cannot transform from '%s' to '%s'. TF said: %s",pose_stamped.frame_id_.c_str(),root_frame.c_str(), err.c_str());
return false;
}
}
try
{
tf.transformPose(root_frame, pose_stamped, pose_stamped);
}
catch(...)
{
ROS_ERROR("pr2_arm_ik:: Cannot transform from '%s' to '%s'",pose_stamped.frame_id_.c_str(),root_frame.c_str());
return false;
}
tf::poseStampedTFToMsg(pose_stamped,pose_msg_out);
return true;
}
/** Set new target pose as given in the goal message.
*
* Checks if the orientation provided in the target pose is valid.
* Publishes the goal pose for the visualization purposes.
*
* @return true if the goal was accepted. */
bool setNewGoal(const amr_msgs::MoveToGoalConstPtr& new_goal)
{
if (!isQuaternionValid(new_goal->target_pose.pose.orientation))
{
ROS_WARN("Aborted. Target pose has invalid quaternion.");
move_to_server_->setAborted(amr_msgs::MoveToResult(), "Aborted. Target pose has invalid quaternion.");
return false;
}
else
{
double x = new_goal->target_pose.pose.position.x;
double y = new_goal->target_pose.pose.position.y;
double yaw = tf::getYaw(new_goal->target_pose.pose.orientation);
Pose pose(x, y, yaw);
velocity_controller_->setTargetPose(pose);
poseStampedMsgToTF(new_goal->target_pose, target_pose_);
target_pose_.frame_id_ = "odom";
current_goal_publisher_.publish(new_goal->target_pose);
ROS_INFO_STREAM("New target pose: " << pose);
return true;
}
}
geometry_msgs::PoseStamped LogicalCore::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg)
{
std::string global_frame = "map";
tf::Stamped<tf::Pose> goal_pose, global_pose;
poseStampedMsgToTF(goal_pose_msg, goal_pose);
//just get the latest available transform... for accuracy they should send
//goals in the frame of the planner
goal_pose.stamp_ = ros::Time();
try{
tf_.transformPose(global_frame, goal_pose, global_pose);
}
catch(tf::TransformException& ex){
ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
goal_pose.frame_id_.c_str(), global_frame.c_str(), ex.what());
return goal_pose_msg;
}
geometry_msgs::PoseStamped global_pose_msg;
tf::poseStampedTFToMsg(global_pose, global_pose_msg);
return global_pose_msg;
}
bool transformGlobalPlan(
const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const tf::Stamped<tf::Pose>& global_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
transformed_plan.clear();
try {
if (!global_plan.size() > 0) {
ROS_ERROR("Received plan with zero length");
return false;
}
// get plan_to_global_transform from plan frame to global_frame
tf::StampedTransform plan_to_global_transform;
tf.waitForTransform(global_frame, ros::Time::now(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, ros::Duration(0.5));
tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, plan_to_global_transform);
//let's get the pose of the robot in the frame of the plan
tf::Stamped<tf::Pose> robot_pose;
tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose);
//we'll discard points on the plan that are outside the local costmap
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
unsigned int i = 0;
double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = 0;
//we need to loop to a point on the plan that is within a certain distance of the robot
while(i < (unsigned int)global_plan.size()) {
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
if (sq_dist <= sq_dist_threshold) {
break;
}
++i;
}
tf::Stamped<tf::Pose> tf_pose;
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
const geometry_msgs::PoseStamped& pose = global_plan[i];
poseStampedMsgToTF(pose, tf_pose);
tf_pose.setData(plan_to_global_transform * tf_pose);
tf_pose.stamp_ = plan_to_global_transform.stamp_;
tf_pose.frame_id_ = global_frame;
poseStampedTFToMsg(tf_pose, newer_pose);
transformed_plan.push_back(newer_pose);
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
++i;
}
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan, std::vector<int>& start_end_counts)
{
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
// initiate refernce variables
transformed_plan.clear();
try
{
if (!global_plan.size() > 0)
{
ROS_ERROR("Recieved plan with zero length");
return false;
}
tf::StampedTransform transform;
tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, transform);
//let's get the pose of the robot in the frame of the plan
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = costmap.getBaseFrameID();
robot_pose.stamp_ = ros::Time();
tf.transformPose(plan_pose.header.frame_id, robot_pose, robot_pose);
//we'll keep points on the plan that are within the window that we're looking at
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
unsigned int i = 0;
double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = DBL_MAX;
// --- start - modification w.r.t. base_local_planner
// initiate start_end_count
std::vector<int> start_end_count;
start_end_count.assign(2, 0);
// we know only one direction and that is forward! - initiate search with previous start_end_counts
// this is neccesserry to work with the sampling based planners - path may severall time enter and leave moving window
ROS_ASSERT( (start_end_counts.at(0) > 0) && (start_end_counts.at(0) <= (int) global_plan.size()) );
i = (unsigned int) global_plan.size() - (unsigned int) start_end_counts.at(0);
// --- end - modification w.r.t. base_local_planner
//we need to loop to a point on the plan that is within a certain distance of the robot
while(i < (unsigned int)global_plan.size() && sq_dist > sq_dist_threshold)
{
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
// --- start - modification w.r.t. base_local_planner
// not yet in reach - get next frame
if( sq_dist > sq_dist_threshold )
++i;
else
// set counter for start of transformed intervall - from back as beginning of plan might be prunned
start_end_count.at(0) = (int) (((unsigned int) global_plan.size()) - i);
// --- end - modification w.r.t. base_local_planner
}
tf::Stamped<tf::Pose> tf_pose;
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
while(i < (unsigned int)global_plan.size() && sq_dist < sq_dist_threshold)
{
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
const geometry_msgs::PoseStamped& pose = global_plan[i];
poseStampedMsgToTF(pose, tf_pose);
tf_pose.setData(transform * tf_pose);
tf_pose.stamp_ = transform.stamp_;
tf_pose.frame_id_ = global_frame;
poseStampedTFToMsg(tf_pose, newer_pose);
transformed_plan.push_back(newer_pose);
// --- start - modification w.r.t. base_local_planner
// set counter for end of transformed intervall - from back as beginning of plan might be prunned
start_end_count.at(1) = int (((unsigned int) global_plan.size()) - i);
// --- end - modification w.r.t. base_local_planner
++i;
}
// --- start - modification w.r.t. base_local_planner
// write to reference variable
start_end_counts = start_end_count;
// --- end - modification w.r.t. base_local_planner
}
catch(tf::LookupException& ex)
{
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex)
{
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex)
{
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
//función llamada por move_base para obtener el plan.
bool MyastarPlanner::makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
//***********************************************************
// Inicio de gestion de ROS, mejor no tocar nada en esta parte
//***********************************************************
if(!initialized_){
ROS_ERROR("The astar planner has not been initialized, please call initialize() to use the planner");
return false;
}
ROS_INFO("MyastarPlanner: Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
plan.clear();
//obtenemos el costmap global que está publicado por move_base.
costmap_ = costmap_ros_->getCostmap();
//Obligamos a que el marco de coordenadas del goal enviado y del costmap sea el mismo.
//esto es importante para evitar errores de transformaciones de coordenadas.
if(goal.header.frame_id != costmap_ros_->getGlobalFrameID()){
ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",
costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
return false;
}
tf::Stamped<tf::Pose> goal_tf;
tf::Stamped<tf::Pose> start_tf;
poseStampedMsgToTF(goal,goal_tf);
poseStampedMsgToTF(start,start_tf);
//obtenemos la orientación start y goal en start_yaw y goal_yaw.
double useless_pitch, useless_roll, goal_yaw, start_yaw;
start_tf.getBasis().getEulerYPR(start_yaw, useless_pitch, useless_roll);
goal_tf.getBasis().getEulerYPR(goal_yaw, useless_pitch, useless_roll);
/**************************************************************************/
/*************** HASTA AQUÍ GESTIÓN DE ROS *********************************/
/****************************************************************************/
//pasamos el goal y start a un nodo (estructura coupleOfCells)
coupleOfCells cpstart, cpgoal;
double goal_x = goal.pose.position.x;
double goal_y = goal.pose.position.y;
unsigned int mgoal_x, mgoal_y;
costmap_->worldToMap(goal_x,goal_y,mgoal_x, mgoal_y);
cpgoal.index = MyastarPlanner::costmap_->getIndex(mgoal_x, mgoal_y);
cpgoal.parent=0;
cpgoal.gCost=0;
cpgoal.hCost=0;
cpgoal.fCost=0;
double start_x = start.pose.position.x;
double start_y = start.pose.position.y;
unsigned int mstart_x, mstart_y;
costmap_->worldToMap(start_x,start_y, mstart_x, mstart_y);
cpstart.index = MyastarPlanner::costmap_->getIndex(mstart_x, mstart_y);
cpstart.parent =cpstart.index;
cpstart.gCost = 0;
cpstart.hCost = MyastarPlanner::calculateHCost(cpstart.index,cpgoal.index);
double maxdistance=sqrt(costmap_->getSizeInMetersY()*costmap_->getSizeInMetersY()+costmap_->getSizeInMetersX()*costmap_->getSizeInMetersX());
//insertamos el nodo inicial en abiertos
MyastarPlanner::openList.insert(cpstart);
ROS_INFO("Inserto en Abiertos: %d", cpstart.index );
ROS_INFO("Index del goal: %d", cpgoal.index );
unsigned int explorados = 0;
unsigned int currentIndex = cpstart.index;
while (!MyastarPlanner::openList.empty()) //while the open list is not empty continuie the search
{
//escoger el nodo (coupleOfCells) de abiertos que tiene el valor más pequeño de f.
coupleOfCells COfCells=*(openList.begin());
currentIndex=COfCells.index;
//vamos a insertar ese nodo en cerrados
//obtenemos un iterador a ese nodo en la lista de abiertos
set<coupleOfCells>::iterator it=getPositionInList(openList,currentIndex);
//copiamos el contenido de ese nodo a una variable nodo auxiliar
cpstart.index=currentIndex;
cpstart.parent=(*it).parent;
cpstart.gCost=(*it).gCost;
cpstart.hCost=(*it).hCost;
cpstart.fCost=(*it).fCost;
//y esa variable la insertamos en cerrados
MyastarPlanner::closedList.insert(cpstart);
//ROS_INFO("Inserto en CERRADOS: %d", (*it).index );
ROS_INFO("G: %f, H: %f, F: %f", (*it).gCost, (*it).hCost, (*it).fCost);
ROS_INFO("Index: %d Parent: %d", (*it).index, (*it).parent);
// Si el nodo recién insertado es el goal, ¡plan encontrado!
if(currentIndex==cpgoal.index || explorados == 2000)
{
//el plan lo construimos partiendo del goal, del parent del goal y saltando en cerrados "de parent en parent"
//y vamos insertando al final los waypoints (los nodos de cerrados)
ROS_INFO("PLAN ENCONTRADO!!!");
//convertimos goal a poseStamped nueva
geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = goal.header.frame_id;//debe tener el mismo frame que el goal pasado por parámetro
pose.pose.position.x = goal_x;
pose.pose.position.y = goal_y;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
//lo añadimos al plan%
plan.push_back(pose);
ROS_INFO("Inserta en Plan: %f, %f", pose.pose.position.x, pose.pose.position.y);
coupleOfCells currentCouple = cpstart;
unsigned int currentParent = cpstart.parent;
while (currentCouple.index != currentParent) //e.d. mientras no lleguemos al nodo start
{
//encontramos la posición de currentParent en cerrados
set<coupleOfCells>::iterator it=getPositionInList(closedList,currentParent);
//hacemos esa posición que sea el currentCouple
currentCouple.index=currentParent;
currentCouple.parent=(*it).parent;
currentCouple.gCost=(*it).gCost;
currentCouple.hCost=(*it).hCost;
currentCouple.fCost=(*it).fCost;
//creamos una PoseStamped con la información de currentCouple.index
//primero hay que convertir el currentCouple.index a world coordinates
unsigned int mpose_x, mpose_y;
double wpose_x, wpose_y;
costmap_->indexToCells(currentCouple.index, mpose_x, mpose_y);
costmap_->mapToWorld(mpose_x, mpose_y, wpose_x, wpose_y);
//después creamos la pose
geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = goal.header.frame_id;//debe tener el mismo frame que el de la entrada
pose.pose.position.x = wpose_x;
pose.pose.position.y = wpose_y;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
//insertamos la pose en el plan
plan.push_back(pose);
ROS_INFO("Inserta en Plan: %f, %f", pose.pose.position.x, pose.pose.position.y);
//hacemos que currentParent sea el parent de currentCouple
currentParent = currentCouple.parent;
}
ROS_INFO("Sale del bucle de generación del plan.");
std::reverse(plan.begin(),plan.end());
//lo publica en el topic "planTotal"
publishPlan(plan);
return true;
}
//Si no hemos encontrado plan aún eliminamos el nodo insertado de ABIERTOS.
openList.erase(openList.begin());
//Buscamos en el costmap las celdas adyacentes a la actual
vector <unsigned int> neighborCells=findFreeNeighborCell(currentIndex);
//Ignoramos las celdas que ya existen en CERRADOS
for(int i = 0; i < neighborCells.size(); i++){
if( isContains(closedList, neighborCells[i]) ){
neighborCells.erase(neighborCells.begin()+i);
i--;
}
}
//Determinamos las celdas que ya están en ABIERTOS y las que no están en ABIERTOS
//Para los nodos que ya están en abiertos, comprobar en cerrados su coste y actualizarlo si fuera necesario
//Añadimos a ABIERTOS las celdas que todavía no están en ABIERTO, marcando el nodo actual como su padre
//ver la función addNeighborCellsToOpenList(openList, neighborsNotInOpenList, currentIndex, coste_del_nodo_actual, indice_del_nodo_goal);
addNeighborCellsToOpenList(openList, neighborCells, currentIndex, cpstart.gCost, cpgoal.index, maxdistance);
explorados++;
}
if(openList.empty()) // if the openList is empty: then failure to find a path
{
ROS_INFO("Failure to find a path !");
return false;
// exit(1);
}
};
bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan) {
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
transformed_plan.clear();
try {
if (!global_plan.size() > 0)
{
ROS_ERROR("Recieved plan with zero length");
return false;
}
tf::StampedTransform transform;
tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, transform);
//let's get the pose of the robot in the frame of the plan
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = costmap.getBaseFrameID();
robot_pose.stamp_ = ros::Time();
tf.transformPose(plan_pose.header.frame_id, robot_pose, robot_pose);
//we'll keep points on the plan that are within the window that we're looking at
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
unsigned int i = 0;
double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = DBL_MAX;
//we need to loop to a point on the plan that is within a certain distance of the robot
while(i < (unsigned int)global_plan.size() && sq_dist > sq_dist_threshold) {
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
++i;
}
//make sure not to count the first point that is too far away
if(i > 0)
--i;
tf::Stamped<tf::Pose> tf_pose;
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
while(i < (unsigned int)global_plan.size() && sq_dist < sq_dist_threshold) {
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
const geometry_msgs::PoseStamped& pose = global_plan[i];
poseStampedMsgToTF(pose, tf_pose);
tf_pose.setData(transform * tf_pose);
tf_pose.stamp_ = transform.stamp_;
tf_pose.frame_id_ = global_frame;
poseStampedTFToMsg(tf_pose, newer_pose);
transformed_plan.push_back(newer_pose);
++i;
}
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
bool isGoalReached(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2DROS& costmap_ros, const std::string& global_frame,
const nav_msgs::Odometry& base_odom, double rot_stopped_vel, double trans_stopped_vel,
double xy_goal_tolerance, double yaw_goal_tolerance) {
const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
tf::Stamped<tf::Pose> goal_pose;
try {
if (!global_plan.size() > 0)
{
ROS_ERROR("Recieved plan with zero length");
return false;
}
tf::StampedTransform transform;
tf.lookupTransform(global_frame, ros::Time(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, transform);
poseStampedMsgToTF(plan_goal_pose, goal_pose);
goal_pose.setData(transform * goal_pose);
goal_pose.stamp_ = transform.stamp_;
goal_pose.frame_id_ = global_frame;
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
//we assume the global goal is the last point in the global plan
double goal_x = goal_pose.getOrigin().getX();
double goal_y = goal_pose.getOrigin().getY();
double yaw = tf::getYaw(goal_pose.getRotation());
double goal_th = yaw;
tf::Stamped<tf::Pose> global_pose;
if(!costmap_ros.getRobotPose(global_pose))
return false;
//check to see if we've reached the goal position
if(goalPositionReached(global_pose, goal_x, goal_y, xy_goal_tolerance)) {
//check to see if the goal orientation has been reached
if(goalOrientationReached(global_pose, goal_th, yaw_goal_tolerance)) {
//make sure that we're actually stopped before returning success
if(stopped(base_odom, rot_stopped_vel, trans_stopped_vel))
return true;
}
}
return false;
}