pathPlanner.cpp

Go to the documentation of this file.

#include <youbot_overhead_localization/pathPlanner.h>

using namespace std;
using namespace costmap_2d;
using namespace navfn;

pathPlanner::pathPlanner(std::string name)
  :as(n, name, boost::bind(&pathPlanner::executePathPlanning, this, _1), false), actionName(name), cmObj(NULL), navFnObj(NULL)
{
  ROS_INFO("Starting pathPlanner");
  string moveCmdTopicName;

  cmInitialized = false;
  navFnInitialized = false;

  n.getParam("/planner_map_server/map_file_location", mapFileName);
  n.getParam("/planner_map_server/map_save_debug_location", mapDebugLocation);
  if(!n.getParam("/path_planner/moveCommandTopic", moveCmdTopicName))
    baseCommandPublisher = n.advertise<geometry_msgs::Twist>("/cmd_vel", -1);
  else
    baseCommandPublisher = n.advertise<geometry_msgs::Twist>(moveCmdTopicName, -1);
  //initialize publishers and subscribers
  mapSubscriber = n.subscribe("/map", 0, &pathPlanner::mapserverCallback, this);
  localizationSubscriber = n.subscribe("base_localizer/pose_2d", 1, &pathPlanner::localizationCallback, this);
  coordConversionClient = n.serviceClient<youbot_overhead_vision::CoordConversion>("coord_conversion");
  costPublisher = n.advertise<youbot_overhead_localization::CostmapCost>("base_localizer/cost", 1);\
  costServer = n.advertiseService("/path_planner/get_cost",  &pathPlanner::getCost, this);

  //action lib
  as.start();

  //localization data
  currentPose.x = 0.0;
  currentPose.y = 0.0;
  currentPose.theta = 0.0;
  n.getParam("/path_planner/run_from_simulation", useSim);

  //plan
  vector<float> xVec;
  vector<float> yVec;
  vector<float> dst;
  plan.pathLength = 0;
  plan.pathx = xVec;
  plan.pathy = yVec;
  plan.distances = dst;
  plan.remainingDistance = 0;
  goalx = 2.0;
  goaly = 1.4;

  //costmap parameters
  inRadius = 0.24; //inner circle radius of robot base
  circumRadius = 0.4; //outer circle radius of robot base (not including arm)
  inflationRadius = 0.4;
  costScalingFactor = 1.0;
  obstacleRange = 2.5;
  raytraceRange = 3.0;
  maxObstacleHeight = .8;  //youbot height with arm fully extended
  lethalCostThreshold = 100;
  trackUnknownSpace = false;
  unknownCostValue = 0;

  //NavFn parameters
  allowUnknown = true;  //allow planning through unknown occupancy spaces

  //default position initialization, should be updated by localizer
  currentPose.x = 0;
  currentPose.y = 0;
  currentPose.theta = 0;

  //Trajectory error tracking
  prevTurnError = 0.0;
  prevError = 0.0;
  totalTurnError = 0.0;
  totalError = 0.0;
}

pathPlanner::~pathPlanner()
{
  if (cmObj)
    delete cmObj;

  if (navFnObj)
    delete navFnObj;
}

void pathPlanner::executePathPlanning(const youbot_overhead_localization::PlanPathGoalConstPtr& goal)
{
  ros::Rate r(30);
  bool success = true;
  youbot_overhead_vision::CoordConversion srv;

  if (goal->topic_name.compare("/logitech_9000_camera1/image_raw") == 0)
  {
    if (useSim)
    {
      srv.request.x = goal->x + SIM_TOP_CAMERA_OFFSET;
    }
    else
    {
      srv.request.x = goal->x + REAL_TOP_CAMERA_OFFSET;
    }
  }
  else
  {
    srv.request.x = goal->x;
  }
  srv.request.y = goal->y;
  srv.request.type = 1;  //convert from pixels to meters
  coordConversionClient.call(srv);
  goalx = srv.response.xConverted;
  goaly = srv.response.yConverted;

  int returnValue = makePlanAstar();

  //path found successfully
  if (returnValue == 1)
  {
    asFeedback.pathLength = plan.remainingDistance;
    as.publishFeedback(asFeedback);

    //Set initial time for path following
    clock_gettime(CLOCK_REALTIME, &prevPointTime);

    while (plan.pathLength > 0)
    {
      //Check for timeout
      timespec currentTime;
      clock_gettime(CLOCK_REALTIME, &currentTime);
      float elapsedTime = (currentTime.tv_sec + currentTime.tv_nsec / 1000000000.0) - (prevPointTime.tv_sec + prevPointTime.tv_nsec / 1000000000.0);
      //ROS_INFO("Time since last point was reached: %f", elapsedTime);

      if (as.isPreemptRequested() || !ros::ok() || elapsedTime > PATH_TIMEOUT)
      {
        ROS_INFO("%s: Preempted", actionName.c_str());
        baseCommand.linear.x = 0.0;
        baseCommand.linear.y = 0.0;
        baseCommand.angular.z = 0.0;
        baseCommandPublisher.publish(baseCommand);
        as.setPreempted();
        success = false;
        break;
      }

      followPath();
      
      r.sleep();
    }

    if (success)
    {
      asResult.posx = currentPose.x;
      asResult.posy = currentPose.y;
      ROS_INFO("%s: Succeeded", actionName.c_str());
      as.setSucceeded(asResult);
    }
  }
  else
  {
    ROS_INFO("%s: Preempted, no path found...", actionName.c_str());
    as.setPreempted();
  }
}

void pathPlanner::followPath()
{
  float currentx = currentPose.x;
  float currenty = currentPose.y;
  float currentHeading = currentPose.theta;

  float targetx = plan.pathx.front();
  float targety = plan.pathy.front();
  float targetHeading = atan2(targety - currenty, targetx - currentx);

  float forwardVel = 0.0;
  float leftVel = 0.0;
  float turnVel = 0.0;

  float turnError = targetHeading - currentHeading;
  while (turnError < -PI)
  {
    turnError += 2*PI;
  }
  while (turnError > PI)
  {
    turnError -= 2*PI;
  }

  float forwardError = sqrt(pow(targetx - currentx, 2) + pow(targety - currenty, 2));

  //Goal point reached
  if (forwardError < .1)
  {
    totalTurnError = 0;
    totalError = 0;
    prevTurnError = 0;
    turnError = 0;

    //shift to the next point on the path
    plan.pathx.erase(plan.pathx.begin());
    plan.pathy.erase(plan.pathy.begin());
    if (plan.distances.size() > 0)
      plan.distances.erase(plan.distances.begin());
    plan.pathLength --;
    if (plan.pathLength > 0)
      plan.remainingDistance = sqrt(pow(plan.pathx.front() - currentx, 2) + pow(plan.pathy.front() - currenty, 2));

    //Reset time for timeout check
    clock_gettime(CLOCK_REALTIME, &prevPointTime);
  }
  //Goal point close
  else if (forwardError < .5)
  {
    float angleToGoal = atan2(targety - currenty, targetx - currentx)
      - (currentHeading - PI/2.0);

    //Adjust position
    forwardVel = KP_LIN*forwardError + KI_LIN*totalError
      + KD_LIN*(forwardError - prevError);
    leftVel = -1 * forwardVel * cos(angleToGoal);
    forwardVel = forwardVel * sin(angleToGoal);
    totalError += forwardError;
    prevError = forwardError;

    plan.remainingDistance = forwardError;
  }
  //Goal point far
  else
  {
    //turn towards next point
    turnVel = KP_ANG*turnError + KI_ANG*totalTurnError
      + KD_ANG*(turnError - prevTurnError);
    totalTurnError += turnError;
    prevTurnError = turnError;

    if (abs(turnError) < .08727)  //angle is within 5 degrees, move towards next point
    {
      totalTurnError = 0;

      forwardVel = KP_LIN*forwardError + KI_LIN*totalError
        + KD_LIN*(forwardError - prevError);
      totalError += forwardError;
      prevError = forwardError;
    }

    plan.remainingDistance = forwardError;
  }

  //calculate remaining path distance
  for (unsigned int i = 0; i < plan.distances.size(); i++)
  {
    plan.remainingDistance += plan.distances[i];
  }

  //reduce the speed
  if (forwardVel > 1)
    forwardVel = 1;
  else if (forwardVel < -1)
    forwardVel = -1;
  if (leftVel > 1)
    leftVel = 1;
  else if (leftVel < -1)
    leftVel = -1;
  if (turnVel > 1)
    turnVel = 1;
  else if (turnVel < -1)
    turnVel = -1;



  forwardVel =  forwardVel * MAX_SPEED;
  leftVel =   leftVel * MAX_SPEED;
  turnVel = turnVel * MAX_SPEED;
  
  ROS_INFO("Forward vel %f, Left %f, turn %f", forwardVel, leftVel, turnVel);
 
  
  //move command for the robot base
  baseCommand.linear.x = forwardVel;
  baseCommand.linear.y = leftVel;
  baseCommand.angular.z = turnVel;
  baseCommandPublisher.publish(baseCommand);

  asFeedback.pathLength = plan.remainingDistance;
  as.publishFeedback(asFeedback);
}

void pathPlanner::localizationCallback(const youbot_overhead_localization::Pose2d& newPose)
{
  currentPose.x = newPose.x;
  currentPose.y = newPose.y;
  currentPose.theta = newPose.theta;

  if (cmInitialized)
  {
    unsigned int costmapx, costmapy;
    youbot_overhead_localization::CostmapCost currentCost;
    if (!cmObj->worldToMap(currentPose.y, currentPose.x, costmapx, costmapy))
      currentCost.cost = 255;  //set cost high if robot is off map
    else
      currentCost.cost = cmObj->getCost(costmapx, costmapy);
    costPublisher.publish(currentCost);
  }
}

void pathPlanner::mapserverCallback(const nav_msgs::OccupancyGridConstPtr& newMap)
{
  mapWidth = newMap->info.width;
  mapHeight = newMap->info.height;
  mapResolution = newMap->info.resolution;
  mapOriginx = newMap->info.origin.position.x;
  mapOriginy = newMap->info.origin.position.y;
  unsigned int mapCellCount = mapWidth * mapHeight;
  mapData.clear();
  mapData.reserve(mapCellCount);
  for(unsigned int i = 0; i < mapCellCount; i++)
    mapData.push_back((unsigned char)newMap->data[i]);
  if (!cmInitialized)
  {
    cmObj = new Costmap2D(mapWidth, mapHeight,
      mapResolution, mapOriginx, mapOriginy, inRadius, circumRadius,
      inflationRadius, obstacleRange, maxObstacleHeight, raytraceRange,
      costScalingFactor, mapData, lethalCostThreshold, trackUnknownSpace,
      unknownCostValue);
    cmInitialized = true;
  }
  else
  {
    cmObj->replaceFullMap(mapOriginx, mapOriginy, mapWidth, mapHeight, mapData);
  }
  unsigned int costmapx, costmapy;
  youbot_overhead_localization::CostmapCost currentCost;
  //x and y must be switched going in and out of the planning algorithm because
  //the planner map image uses a (row, col) coordinate system instead of (x, y)
  if (!cmObj->worldToMap(currentPose.y, currentPose.x, costmapx, costmapy))
    currentCost.cost = 255; //set cost high if robot is off map
  else
    currentCost.cost = cmObj->getCost(costmapx, costmapy);
  costPublisher.publish(currentCost);

  //TODO: Debugging stuff
  //printOccupancyGrid();
  //printCostmap();
  //ROS_INFO("Position: %d, %d", costmapx, costmapy);
  //ROS_INFO("Cost: %d", cmObj->getCost(costmapx, costmapy));
  //TODO: End of debugging stuff

  //initialize navfn
  if (!navFnInitialized)
  {
    navFnObj = new NavFn(cmObj->getSizeInCellsX(), cmObj->getSizeInCellsY());
    navFnInitialized = true;
  }
    navFnObj->setCostmap(cmObj->getCharMap(), true, allowUnknown);
}

int pathPlanner::makePlanAstar()
{
  if (!(cmInitialized && navFnInitialized))
  {
    ROS_INFO("Costmap or planner not initialized. Unable to plan.");
    return -1;
  }

  //x and y must be switched going in and out of the planning algorithm because
  //the planner map image uses a (row, col) coordinate system instead of (x, y)
  unsigned int mapStartx, mapStarty;
  if (!cmObj->worldToMap(currentPose.y, currentPose.x, mapStartx, mapStarty))
  {
    ROS_INFO("Robot start position is outside map area.");
    return -1;
  }

  unsigned int mapGoalx, mapGoaly;
  if (!cmObj->worldToMap(goaly, goalx, mapGoalx, mapGoaly))
  {
    ROS_INFO("Robot goal position is outside map area.");
    return -1;
  }

  plan.pathx.clear();
  plan.pathy.clear();
  plan.distances.clear();

  int mapStart[2];
  mapStart[0] = mapStartx;
  mapStart[1] = mapStarty;

  int mapGoal[2];
  mapGoal[0] = mapGoalx;
  mapGoal[1] = mapGoaly;

  navFnObj->setStart(mapStart);
  navFnObj->setGoal(mapGoal);

  //calculate plan
  navFnObj->calcNavFnAstar();
  navFnObj->calcPath(cmObj->getSizeInCellsX() * 4);

  //extract the plan
  int len = navFnObj->getPathLen();
  float *tempPathy = navFnObj->getPathX();
  float *tempPathx = navFnObj->getPathY();

  if (len <= 1)
  {
    ROS_INFO("Goal is unreachable");
    return -1;
  }

  //filter path
  float xfilt[len];
  float yfilt[len];

  for(int i=0; i < len; i++)
  {
    xfilt[i] = tempPathx[i];
    yfilt[i] = tempPathy[i];
  }

  //First order Butterworth filter
  if (len > 1)
  {
    for (int i=1; i < len; i++)
    {
      xfilt[i] = (B1*tempPathx[i] + B2*tempPathx[(i-1)] - A*xfilt[(i-1)]);
      yfilt[i] = (B1*tempPathy[i] + B2*tempPathy[(i-1)] - A*yfilt[(i-1)]);
    }
  }

  //Significant point extraction
  int i;
  int k = 5;  //test interval
  float c[len];
  for (i=0; i < len; i++)
  {
    float ax, ay, bx, by;
    if (i+k < len)
    {
      ax = xfilt[i] - xfilt[i+k];
      ay = yfilt[i] - yfilt[i+k];
    }
    else
    {
      ax = xfilt[i] - xfilt[len-1];
      ay = yfilt[i] - yfilt[len-1];
    }
    if (i-k > 0)
    {
      bx = xfilt[i] - xfilt[i-k];
      by = yfilt[i] - yfilt[i-k];
    }
    else
    {
      bx = xfilt[i] - xfilt[0];
      by = yfilt[i] - yfilt[0];
    }
    c[i] = (ax*bx + ay*by) / (sqrt(ax*ax+ay*ay)*sqrt(bx*bx+by*by));
  }

  float sigx[len];
  float sigy[len];
  int numSig = 0;
  for (i=1; i<len-1; i++)
  {
    if (c[i] > c[i-1] && c[i] > c[i+1] && c[i] > SIG_THRESH)
    {
      sigx[numSig] = xfilt[i];
      sigy[numSig] = yfilt[i];
      numSig ++;
    }
  }

  //Add endpoint as a significant path point
  sigx[numSig] = xfilt[len-1];
  sigy[numSig] = yfilt[len-1];
  numSig ++;

  //Convert path from map coordinates to world coordinates
  for (i=0; i < numSig; i++)
  {
    youbot_overhead_vision::CoordConversion srv;
    srv.request.x = sigx[i];
    srv.request.y = sigy[i];
    srv.request.type = 3;  //convert from occupancy grid pixels to meters
    coordConversionClient.call(srv);

    plan.pathx.push_back(srv.response.xConverted);
    plan.pathy.push_back(srv.response.yConverted);
  }

  plan.pathLength = numSig;

  //Fill in distances for remaining path distance calculation
  for (i=0; i < plan.pathLength - 1; i++)
  {
    plan.distances.push_back(sqrt(pow(plan.pathx[i+1] - plan.pathx[i], 2) + pow(plan.pathy[i+1] - plan.pathy[i], 2)));
  }
  if (plan.pathLength > 0)
  {
    plan.remainingDistance = sqrt(pow(plan.pathx[0] - currentPose.x, 2) + pow(plan.pathy[0] - currentPose.y, 2));
    for (unsigned int j=0; j < plan.distances.size(); j++)
    {
      plan.remainingDistance += plan.distances[j];
    }
  }
  else
    plan.remainingDistance = 0;

  return 1;
}

bool pathPlanner::getCost(youbot_overhead_localization::GetCost::Request &req, youbot_overhead_localization::GetCost::Response &res)
{
  unsigned int mapPosx, mapPosy;
  //x and y must be switched going in and out of the planning algorithm because
  //the planner map image uses a (row, col) coordinate system instead of (x, y)
  if (!cmObj->worldToMap(req.y, req.x, mapPosx, mapPosy))
  {
    ROS_INFO("Requested cost position is outside map area.");
    return false;
  }

  res.cost = cmObj->getCost(mapPosx, mapPosy);

  return true;
}

void pathPlanner::printOccupancyGrid()
{
  FILE *fp = fopen(mapDebugLocation.c_str(), "w");

  if(!fp)
  {
    ROS_INFO("Can't open file %s.", mapDebugLocation.c_str());
  }
  else
  {
    fprintf(fp, "P2\n%d\n%d\n%d\n", cmObj->getSizeInCellsX(), cmObj->getSizeInCellsY(), 0xff);
    for(unsigned int iy = 0; iy < cmObj->getSizeInCellsY(); iy++) {
      for(unsigned int ix = 0; ix < cmObj->getSizeInCellsX(); ix++) {
        unsigned char cost = cmObj->getCost(ix,iy);
        if (cost == LETHAL_OBSTACLE) {
          fprintf(fp, "255 ");
        }
        else if (cost == NO_INFORMATION){
          fprintf(fp, "180 ");
        }
        else if (cost == INSCRIBED_INFLATED_OBSTACLE ) {
          fprintf(fp, "128 ");
        }
        else if (cost > 0){
          fprintf(fp, "50 ");
        }
        else {
          fprintf(fp, "0 ");
        }
      }
      fprintf(fp, "\n");
    }
    fclose(fp);
  }
}

void pathPlanner::printCostmap()
{
  FILE *fp = fopen(mapDebugLocation.c_str(), "w");

  if(!fp)
  {
    ROS_INFO("Can't open file %s.", mapDebugLocation.c_str());
  }
  else
  {
    fprintf(fp, "P2\n%d\n%d\n%d\n", cmObj->getSizeInCellsX(), cmObj->getSizeInCellsY(), 0xff);
    for(unsigned int iy = 0; iy < cmObj->getSizeInCellsY(); iy++) {
      for(unsigned int ix = 0; ix < cmObj->getSizeInCellsX(); ix++) {
        unsigned char cost = cmObj->getCost(ix,iy);
        fprintf(fp, "%d ", cost);
      }
      fprintf(fp, "\n");
    }
    fclose(fp);
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "path_planner");
  pathPlanner planner2d(ros::this_node::getName());
  ros::spin();

  return 0;
}