This tutorial is part of the Robotino Operation series.

This project will demonstrate how to create a controller to cause the robotino to navigate a potential field.

For more information and progress videos please visit the Robotino Operation Log

To download the completed ROS package: robotino_control_pfnav.zip

robotinoNavigator is the base class that deals with the mundane navigation aspects of the robotino. For example setting speed, setting arm position, retrieving odometer values. robotinoNavigator is designed to be inherited by other classes to run higher level navigation algorithms. It provides the following Member functions. It is based on the approach_grab class for more information see the Robotino Object Seeking tutorial.

robotinoNavigator.cpp

class robotinoNavigator{
    public:
        robotinoNavigator();
        void spin(){return;};						// designed to be inherited
        void setRobotinoVelocity(float vx, float vy, float omega);	//  sends a velocity message to robotino
        void setRobotinoGlobalVel(float vx,float vy, float omega);	// sets the velocity of the robotion in global reference frame
 
        struct pose{
            float array[6];
            bool gripperClosed;
        } BHAPose;
 
        void setBHAPose(pose myPose);   	// set the bhapose using a pose object 
        void setBHAPose(float x0,float x1,float x2,float x3,float x4,float x5, bool gripper); // set bha pose using primitives
        void sendPoseMsg();			// send the pose message.
 
        float targetPhi(float angle);		// returns a velocity to hold orientation at angle
 
        bool setOdom(float x, float y, float phi);
 
        // adjustable parameters
        float targetPhiGain;			// gain for proportional controll on in targetPhi()
        float minimumLinearVelocity;		// x or y velocity below this setting will be set to 0
        float minimumAngularVelocity;	// anguluar velocity below this setting will be set to 0
 
        potentialField::point getRobotinoPoint();
 
    private:
        /*
         * data
         */
        double x_,y_,phi_;			// the position of the robotino
 
        /*
         * callback functions
         */
        void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
 
 
        /*
         * publishers/subscribers
         */
        ros::NodeHandle n;
        ros::Publisher vel_pub;
        ros::Publisher bha_pub;
        ros::Publisher gripper_pub;
        ros::Subscriber odom_sub;
 
        /*
         * services
         */
 
        ros::ServiceClient odom_reset;  
};

The potentialField class is designed to represent the model of potential fields used in PF navigation. It allows the user to store a series of points, either repulsive or attractive and then provides functions useful in navigating the potential field. Below is a list of available functions.

The potential field is stored as a vector of attractive points and a vector of repulsive points. Attractive points have an x and y position and a weight. Repulsive points have an x and y position, a weight and max range of influence. A point is just used as a location to compare with attractive points.

potentialField.cpp

class potentialField{
    public:
        // diffenet points used
        struct point{
            float x;
            float y;
        };
 
        struct attractivePoint{
            float x;
            float y;
            float weight;
        };
 
        struct repulsivePoint{
            float x;
            float y;
            float weight;
            float maxRange;
        };
 
        void addPoint(repulsivePoint p);        // adds a repulsive point to the potential field
        void addPoint(attractivePoint p);       // adds a attractive point to the potential field
        void setPoint(unsigned int index, attractivePoint p);	// set a specific point at index to a new point
        void setPoint(unsigned int index, repulsivePoint p);	// set a specific point at index to a new point
        std::vector<float> attractiveGrad(point p);		// find the gradient of the attractiveField
        std::vector<float> repulsiveGrad(point p);			// find the gradient of the repulsiveField
        std::vector<float> netGrad(point p);			// find the total gradient
        float magnitude(std::vector<float> vect);			// find the magnitued of a vector
        std::vector<float> normalize(std::vector<float> vect);	// normalize a vector
        std::vector<float> netGradNormalized(point p);		// get the normaized gradient vector IE it's direction.
 
        float distFromAttractivePoint(unsigned int i,point p);	// get the distance of point p from an index in the attractiveField
 
        void printPotentialField();				// print out the field so we can take a look at it.
 
    private:
        double findDist(point p,repulsivePoint t);
        double findDist(point p,attractivePoint t);
 
        std::vector< attractivePoint > attractiveField;
        std::vector< repulsivePoint > repulsiveField;
};

The PFNav class extends the robotinoNavigator class. It adds the ability to move toward lower potential by following the gradient of the potential field. This class is quite simple because the majority of the work is done in the previous two classes. Note that the class's potentialField is public so users can edit it directly.

PFNav.cpp

class PFNav:public robotinoNavigator{
    public:
        PFNav();
        void setPotentialField(potentialField field);	// set the internal potential field
        void spin();						// run the class.  continuously move toward lower potential
        void moveTowardTarget();				// take one step toward target
 
        potentialField navField;
 
        float movementGain;
 
    private:
};

PFNavNode contains our main function. A few lines of pseudo code will explain how it works.

• create instances of potentialField and PFNav
• Initialize the potential field with sources and sinks
• set our PFNav's potential field to the one we just initialized.
• Tell our PFNav to spin!
• When it is done spinning reset the sink point to the origin
• update PFNav's potential field
• Tell PFNav to spin again

If you would like to see more about how the code works you can download the completed package and look through the source folder. It is well commented and should be easy to understand. robotino_control_pfnav.zip

 roslaunch robotino_node robotino_node.launch hostname:=<robotinoIP>

 rosrun robotino_control PFNavNode