DASL Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
====== Integrating Potential Field Navigation and SLAM with Robotino ======

**This tutorial is developed from the [[robotino_navigation|Robotic Navigation]] Series, an extension of the [[robotino_operation|Robotino Operation]] series.  It is recommended the reader familiarize themselves with that series before continuing**

===== Required to finish this tutorial =====

**Hardware:**

  * Hokuyo URG lidar

  * Robotino

**This tutorial is an extension of:**

  * [[robotino_slam|Robotino slam]]

  * [[robotino_pfnav_object_pickup|Robotino Potential Field Navigator With Object Pickup]]

===== Videos =====

{{youtube>kyWaatPnDps?large}}
{{youtube>ocUFSjQ-LYU?large}}\\ 

This video depicts the visualization of the potential field as viewed in RViz.   Green points z-location correspond to the value of the potential of that point on the map.  Red cubes correspond to obstacles plotted by the Gmapping slam package.  White points correspond to the raw lidar data.

===== Extending PFNavigator Class =====

In order to use map data generated by our SLAM node our PFNavigator must subscribe to gmapping's map topic.  However, we do not wish to modify the algorithm or disturb the current dependencies of PFNavigator.  Therefore, a child class PFSlam was created.  PFSlam inherits and extends the PFNavigator class without modify the navigation algorithm in any way.

The new child class, PFSlam, modifies PFNavigator by adding ROS subscribers and callback functions for both **map data** and **goal location**.  By subscribing to goal location it is possible to use RViz to enter goal locations for the robot.   The PFSlam class must also publish the obstacles and the goal location for easy visualization in RViz.

Below is a copy of the header file.

<code c++ PFNavSLAM.h>
#ifndef pfslam_h
#define pfslam_h
    
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include "PFNav.h"
#include <sensor_msgs/PointCloud.h>
  
class PFSlam:public PFNav{

    public:
  	PFSlam();
  	~PFSlam();
  	
    private:
    //  ROS subscribers and publishers
  	ros::NodeHandle nh_;
  
  	ros::Subscriber map_sub_;
  	
  	ros::Subscriber goal_sub_;
  	
  	ros::Publisher obstacle_pub_;
  	
  	ros::Publisher goal_pub_;
  	
  	void publishObstacles();
  	
  	void publishGoals();
  	
  	void mapCallback(const nav_msgs::OccupancyGrid& occupancyGrid);
  	
  	void goalCallback(const geometry_msgs::Pose& goalPose);
};
#endif
</code>

The publishers for the obstacles are simple point cloud publishers and are self explanatory.  However, the map callback does require some explanation.

The map callback must populate the PFNavigator's potential field using the map's occupancy grid.   Gmapping produces an occupancy grid that is a one dimensional array.   It is necessary to parse this array into x and y coordinates using the Map metadata provided in the map topic.  First 2 nested for loops must be created that run the width and the height of the map that you found from the map metadata.  

  for(int iy=0;iy< occupancyGrid.info.height;iy++){  // iy is the y index of the array
      for(int ix=0; ix<occupancyGrid.info.width;ix++){ 
      }
  }

This way in each iteration of the map nested loops we will have an x and a y index of the map data.  Those indices must then be multiplied by the map resolution and offset by the location of the origin as follows.

  tempPoint.x  =  ix*occupancyGrid.info.resolution+occupancyGrid.info.origin.position.x;  // calculate x and set to temp;
  tempPoint.y  =  iy*occupancyGrid.info.resolution+occupancyGrid.info.origin.position.y;  //

The tempPoint is then finished by adding it's max range and weight and added to the PFNavigator's potential field.  Just before the callback ends we publish out updated potential field obstacles and goals using the   **publishObstacles()**  and  **publishGoals()** member functions.  Below you can see the completed function.

<code c++ mapCallback.cpp>
void PFSlam:: mapCallback(const nav_msgs::OccupancyGrid& occupancyGrid)
{
    int i=0;
    potentialField::repulsivePoint tempPoint;
    potentialField tempField;
    // default values for the points created.  Likely to be updated later to be more dynamic
    float weight=10;
    float maxRange=.5;
    // create variables to store map metadata
    int height,width;
    float res;
    
    /*
     *   print out some data about the map when the callback is triggered
     */
    std::cout<<"callback!"<<std::endl;
    ROS_INFO("%i",occupancyGrid.data.size());
    ROS_INFO("width: %i",occupancyGrid.info.width);
    ROS_INFO("height: %i",occupancyGrid.info.height);
    ROS_INFO("width: %f",occupancyGrid.info.resolution);
    ROS_INFO("map position: %f,%f  orientation: %f",
  	   occupancyGrid.info.origin.position.x,
  	   occupancyGrid.info.origin.position.y,
  	   occupancyGrid.info.origin.orientation.w
    );
    
    
    //  2 for loops to traverse the occupancyGrid.
    for(int iy=0;iy< occupancyGrid.info.height;iy++){  // iy is the y index of the array
        for(int ix=0; ix<occupancyGrid.info.width;ix++){ 
            if(occupancyGrid.data[i]>0){
  	        // generate a point for any point in the occupancyGrid that has nonzero probability.
  	        tempPoint.x  =  ix*occupancyGrid.info.resolution+occupancyGrid.info.origin.position.x;  // calculate x and set to temp;
  	        tempPoint.y  =  iy*occupancyGrid.info.resolution+occupancyGrid.info.origin.position.y;  //
  	        tempPoint.weight= weight;
  	        tempPoint.maxRange=maxRange;
  	
  	        navField.addPoint(tempPoint);   // now that the point is prepared we add it to the potentialField
            }
        
            i++;
        }
    
    }
  
    // debugging printouts
    //navField.printPotentialField();
    //setPotentialField(tempField);
    publishObstacles();
    publishGoals();
}
</code>

===== Rviz Visuilization =====

Before further development could take place it was important to develop some visualization methods for debugging.   

First a point cloud is generated using PCL.  Each time the map data from the Gmapping node is updated the PFSlam class calculates the potential function at each point on the map.  That value is added to a **z** value of a point in the point cloud and the **x** and **y** values correspond to that of corresponding map point.  The point cloud is then broadcasted as a rostopic that can be viewed in Rviz.  Additionally each point has a color function.   Points above the current location of the Robotino are bright green, points at lower potential than the robotino are darker green.  This way the graph can be viewed easily from a top- down map view.


===== Modifying Potential Field navigator to overcome local minima =====

The previously discussed local minimum problem is severely limiting in a duct- work environment because many local minimum are formed. Several methods in the literature propose solutions. Yun and Tan [1997] proposes a method where the vehicle switches a wall following algorithm when a local minimum is detected. Due to the number of local minima in a ductwork assembly or a maze-like environment a more global method was desired. The method proposed by Zou and Zhu [2003] utilizes mul- tiple virtual targets to overcome the problem. When a local minimum is detected a virtual target is placed to navigate the vehicle clear of the minimum. Connolly et al. [1990] proposes a method in which generates a potential field free of local minima altogether by using solutions to the Laplace equation known as harmonic functions. Due to its ability to find solutions in any environment where the goal is achievable the harmonic potential function method was chosen to overcome the local minimum.
A numerical sover was developed to find solutions to the Laplace equation to use as the potential field.  The finite difference method was chosen to construct the field.

Below you can see a demonstration of the improved performance of the harmonic potential field.

{{youtube>kyWaatPnDps?large}}\\