Predefined global path for robot navigation

edit

Hi Peter, we made a new copy of navigation package. We did not make a overload function, just rewrite it :) Sure you can make the code more flexible with overload.

bool CarrotPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan)
  {

    if(!initialized_)
    {
      ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
      return false;
    }

    ROS_DEBUG("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();
    costmap_ros_->getCostmapCopy(costmap_);

    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;
    }

   bool done = false;

  int i,n;
  float *Coordinate_Array = NULL;
  char* Filename;
  Filename = "/home/p3dx/fuerte_workspace/navigation/carrot_planner/src/Coordinate.txt";
  FILE *fp = NULL;
  fp = fopen(Filename, "r");
  fscanf(fp, "%d", &n);

  Coordinate_Array = Readfile2D(Filename);

    for (i=0; i<n; i++)
    {
        geometry_msgs::PoseStamped first_start = start;
        geometry_msgs::PoseStamped first_goal = goal;

        first_start.pose.position.x=Coordinate_Array[i*2]-Coordinate_Array[0];//offset
        first_start.pose.position.y=Coordinate_Array[i*2+1]-Coordinate_Array[1];

        first_goal.pose.position.x=Coordinate_Array[i*2+2]-Coordinate_Array[0];
        first_goal.pose.position.y=Coordinate_Array[i*2+3]-Coordinate_Array[1];

        plan.push_back(first_start);

        tf::Stamped<tf::Pose> goal_tf;
        tf::Stamped<tf::Pose> start_tf;

        poseStampedMsgToTF(first_goal,goal_tf);
        poseStampedMsgToTF(first_start,start_tf);

        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);

        double goal_x = first_goal.pose.position.x;
        double goal_y = first_goal.pose.position.y;

        double target_x = goal_x;
        double target_y = goal_y;
        double target_yaw = goal_yaw;

        geometry_msgs::PoseStamped new_goal = first_goal;
        tf::Quaternion goal_quat = tf::createQuaternionFromYaw(target_yaw);

        new_goal.pose.position.x = target_x;
        new_goal.pose.position.y = target_y;

        new_goal.pose.orientation.x = goal_quat.x();
        new_goal.pose.orientation.y = goal_quat.y();
        new_goal.pose.orientation.z = goal_quat.z();
        new_goal.pose.orientation.w = goal_quat.w();

        plan.push_back(new_goal);


    }//end for i
    done = true;
   return (done);
  }//end of makePlan

This is the make_plan, we just make it as little change as we can. There is a function for reading predefined points written in a .txt file.

float* CarrotPlanner::Readfile2D (char* filename)
{
    int i ,n;
    float *array = NULL;
    char read[101];

    FILE *fp = NULL;
    fp = fopen(filename, "r");
    if(fp == NULL)
       ROS_INFO("Error in reading file");

    //get number of required elemenets for the array to store
    fscanf(fp, "%d", &n);
    fgets(read, 100, fp);

    array=(float*)calloc(2*n,sizeof(float));
    if(array==NULL)
        ROS_INFO("Error in dynamic memory allocation");

    for(i = 0; i < n; i ++)
    {
        //get the value
        fscanf(fp, "%s", &read); array[i*2] = (float)atof(read);
        fscanf(fp, "%s", &read); array[i*2+1] = (float)atof(read);

        //flush of comments
        fgets(read, 100, fp);
    }
    fclose(fp);
    return array;
}

Here I provide a sample .txt for you.. The first num in the .txt file is to indicate how many entries in this file, to allocate space for the array to store the data ... (more)