====== Robotino Potential Field Navigation ======
**This tutorial is part of the [[robotino_operation|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|Robotino Operation Log]]
To download the completed ROS package: {{dylanw:robotino_control_PFNav.zip}}
{{youtube>87D3sv-VHUI?large}}\\
===== Program Setup =====
{{dylanw:objects.png}}\\
=== robotinoNavigator ===
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|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;
};
=== potentialField ===
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 attractiveGrad(point p); // find the gradient of the attractiveField
std::vector repulsiveGrad(point p); // find the gradient of the repulsiveField
std::vector netGrad(point p); // find the total gradient
float magnitude(std::vector vect); // find the magnitued of a vector
std::vector normalize(std::vector vect); // normalize a vector
std::vector 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;
};
=== PFNav ===
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 ===
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. {{dylanw:robotino_control_PFNav.zip}}
===== Running the PFNavNode =====
roslaunch robotino_node robotino_node.launch hostname:=
rosrun robotino_control PFNavNode