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.
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.
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.
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.
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