A* path-finding algorithm for a robot

Path finding algorithm of a robot named “minik”, running The Robot Operating System (ROS)

It detects obstacles with its camera and takes the shortest route to its target, avoiding obstacles

rosThread.h

#include <ros/ros.h>
#include <tf/tf.h>
#include <QDebug>
#include <QVector>
#include <QObject>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Float32MultiArray.h>
#include <minik_ros_wrapper/minikSetVelocityMsg.h>
#include "math.h"

using namespace std;

struct Obstacle{
public:
    double x;
    double y;
    double r;
};

class RosThread:public QObject
{
    Q_OBJECT

public:
    RosThread();
    ~RosThread();

private:
    ros::NodeHandle n;
    ros::Publisher velPub;
    ros::Subscriber odomSub;
    ros::Subscriber targetSub;
    ros::Subscriber obstacleSub;

    // Define the global variables and prototype functions here

    static constexpr double wheelRad = 0.045; // in meters
    static constexpr double robotRadius = 0.1; // in meters

    double travelDistance; //total travelled distance

    double robotX;  // in meters
    double robotY;  // in meters
    double robotTh; // in radians

    double targetX; // in meters
    double targetY; // in meters
    double targetTh; // in radians
    
    int completed;

    std::vector<Obstacle> obstacles;

    void demoLoop();
    void odomHandler(const nav_msgs::OdometryConstPtr &odomMsg);
    void targetHandler(const geometry_msgs::PoseConstPtr &targetMsg);
    void obstacleHandler(const std_msgs::Float32MultiArrayConstPtr &obstacleMsg);

    void sendVelocityCommand(double leftWheel, double rightWheel); // meters per second
    static const int ticks_per_meter = 10610;

    double _lastX;
    double _lastY;

public slots:
     void work();

};

rosThread.cpp

#include "rosThread.h"

#include <iostream>

#include <cmath>

#include <vector>

#include <unistd.h>

#include <math.h>


using namespace std;
RosThread::RosThread() {
  robotX = 0;
  robotY = 0;
  robotTh = 0;

  targetX = 3;
  targetY = 3;

  travelDistance = 0;

  completed = 0;

  Obstacle temp1;

  temp1.x = 1;
  temp1.y = 1;
  temp1.r = 0.2;

  obstacles.push_back(temp1);

}

RosThread::~RosThread() {}

void RosThread::work() {

  velPub = n.advertise < minik_ros_wrapper::minikSetVelocityMsg > ("minik_ros_wrapper/minikSetVelocityMsg", 1);
  odomSub = n.subscribe("odom", 1, & RosThread::odomHandler, this);
  targetSub = n.subscribe("target", 1, & RosThread::targetHandler, this);
  obstacleSub = n.subscribe("obstacles", 1, & RosThread::obstacleHandler, this);

  ros::Rate loop(10);
  while (ros::ok()) {

    demoLoop();
    ros::spinOnce();
    loop.sleep();
  }

  qDebug() << "Quitting";
  ros::shutdown();
}

void RosThread::demoLoop() {
  sendVelocityCommand(0.2, -0.2);
  cout << "X: " << robotX << " \t Y: " << robotY << " \t Theta: " << robotTh << endl;
  if (obstacles.size() > 0) {
    cout << "oX: " << obstacles[0].x << " \t oY: " << obstacles[0].y << " \t oR: " << obstacles[0].r << endl;
  }
}

void RosThread::odomHandler(const nav_msgs::OdometryConstPtr & odomMsg) {

  //  ^ Y
  //  |				<-- Th
  //  |			      |
  //   -----> X   	
  robotX = odomMsg -> pose.pose.position.x;
  robotY = odomMsg -> pose.pose.position.y;

  tf::Quaternion q(odomMsg -> pose.pose.orientation.x, odomMsg -> pose.pose.orientation.y,
    odomMsg -> pose.pose.orientation.z, odomMsg -> pose.pose.orientation.w);
  tf::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);

  robotTh = yaw;

  travelDistance += sqrt(pow(robotX - _lastX, 2) + pow(robotY - _lastY, 2));
  _lastX = robotX;
  _lastY = robotY;
}

void RosThread::targetHandler(const geometry_msgs::PoseConstPtr & targetMsg) {

  targetX = targetMsg -> position.x;
  targetY = targetMsg -> position.y;

}

void RosThread::obstacleHandler(const std_msgs::Float32MultiArrayConstPtr & obstacleMsg) {

  obstacles.clear();

  for (int i = 0; i < obstacleMsg -> layout.dim[0].size; i++) {

    Obstacle temp;

    temp.x = obstacleMsg -> data[i * 3];
    temp.y = obstacleMsg -> data[i * 3 + 1];
    temp.r = obstacleMsg -> data[i * 3 + 2];

    obstacles.push_back(temp);
  }

}

void RosThread::sendVelocityCommand(double leftWheel, double rightWheel) {
  int leftTick = leftWheel * ticks_per_meter;
  int rightTick = rightWheel * ticks_per_meter;

  minik_ros_wrapper::minikSetVelocityMsg msg;

  vector < int > motorID;
  motorID.push_back(0);
  motorID.push_back(1);
  msg.motorID = motorID;

  vector < int > velocity;
  velocity.push_back(leftTick);
  velocity.push_back(rightTick);
  msg.velocity = velocity;

  velPub.publish(msg);
}

Robot Controller

#include <iostream>

#include <tuple>

#include <cmath>


const double PI = 3.141592653589793238463;

using namespace std;

class RobotController {
  float robotRadius = 0.1;

  float obstacleRadius = 0.2;

  float vMax = 0.2;
  float wMax = PI / 2;

  float epsilon = 0.01;
  int initial_degree = 0;

  float axleLength = 0.2;
  float wheelRadius = 0.045;
  float wheelCircumference = 2 * PI * wheelRadius;
  float topViewRobotCircumference = PI * axleLength;

  float obstaclePositions[3][3];
  float robotPosition[3] = {
    0,
    0,
    0
  };
  float robotOrientation[3];

  bool leftCameraDetectedObstacle;
  float distanceLeft;
  float leftPosition[];

  bool rightCameraDetectedObstacle;
  float distanceRight;
  float rightPosition[];

  tuple < float, float > subtract_arrays(float x[], float y[]) {
    float dx = x[0] - y[0];
    float dy = x[1] - y[1];
    return make_tuple(dx, dy);
  }

  float euclidean_distance(float dx, float dy) {
    return dx * dx + dy * dy;
  }

  float get_distance(float currentPosition[], float distantPosition[]) {
    float dx, dy;
    tie(dx, dy) = subtract_arrays(currentPosition, distantPosition);
    float distance = euclidean_distance(dx, dy);
    return distance;
  }

  public: void go() {
    bool is_first_obstacle = true;
    if (initial_degree < 360) {
      setRobotSpeed(0, 3);
      initial_degree = initial_degree + 6;

      if (leftCameraDetectedObstacle == true and is_first_obstacle == true) {
        obstaclePositions[0][0] = leftPosition[0] + robotPosition[0];
        obstaclePositions[0][1] = leftPosition[1] + robotPosition[1];
        obstaclePositions[0][2] = leftPosition[2] + robotPosition[2];

        is_first_obstacle = false;
      }

    } else {

      float robotTheta = robotOrientation[3] + PI / 2;

      // Get from cameras
      float goalPosition[3];
      float obstaclePositions[3][3];

      float distance_to_goal = get_distance(robotPosition, goalPosition);

      float position_of_middle_of_obstacles[3];
      float distance_from_middle_of_obstacles = get_distance(robotPosition, position_of_middle_of_obstacles);

      bool any_obstacle = true;
      float Fx, Fy;

      if (any_obstacle == true) {
        tie(Fx, Fy) = calculateGradient(position_of_middle_of_obstacles, robotPosition, obstaclePositions);

      } else {
        tie(Fx, Fy) = calculateGradient(goalPosition, robotPosition, obstaclePositions);
      }

      float v = 0;
      float w = 0;

      float Fmag = sqrt(Fx * Fx + Fy * Fy);
      float Fth = atan2(Fy, Fx);

      float th = Fth - robotTheta;

      v = vMax * cos(th);
      w = wMax * sin(th);

      if (distance_from_middle_of_obstacles < epsilon) {
        any_obstacle = false;
      }

      if (distance_to_goal < epsilon) {
        setRobotSpeed(0, 0);
      } else {
        setRobotSpeed(v, w);
      }

    }

  }

  bool sign(int x) {
    return (x > 0 and 1) or(x < 0 and - 1) or 0;
  }

  tuple < float, float > calculateGradient(float goalPosition[], float robotPosition[], float obstaclePositions[3][3]) {
    float Fx = 0;
    float Fy = 0;

    float dgx, dgy, do1x, do1y, do2x, do2y, do3x, do3y;

    tie(dgx, dgy) = subtract_arrays(robotPosition, goalPosition);

    tie(do1x, do1y) = subtract_arrays(robotPosition, obstaclePositions[0]);

    tie(do2x, do2y) = subtract_arrays(robotPosition, obstaclePositions[1]);

    tie(do3x, do3y) = subtract_arrays(robotPosition, obstaclePositions[2]);

    float gamma = dgx * dgx + dgy * dgy;

    float B1 = do1x * do1x + do1y * do1y - (robotRadius + obstacleRadius) * (robotRadius + obstacleRadius);
    float B2 = do2x * do2x + do2y * do2y - (robotRadius + obstacleRadius) * (robotRadius + obstacleRadius);
    float B3 = do3x * do3x + do3y * do3y - (robotRadius + obstacleRadius) * (robotRadius + obstacleRadius);

    float B = B1 * B2 * B3;

    int k = 5;
    Fx = (k * pow(gamma, k - 1) * 2 * dgx * B - pow(gamma, k) * (2 * do1x * B2 * B3 + 2 * do2x * B1 * B3 + 2 * do3x * B2 * B1)) / (B * B);
    Fy = (k * pow(gamma, k - 1) * 2 * dgy * B - pow(gamma, k) * (2 * do1y * B2 * B3 + 2 * do2y * B1 * B3 + 2 * do3y * B2 * B1)) / (B * B);

    return make_tuple(-Fx, -Fy);

  }

  void setRobotSpeed(float transVel, float rotVel) {
    // Convert speed to rad/sec
    transVel = transVel / wheelCircumference * 2 * PI;
    rotVel = rotVel * (topViewRobotCircumference / wheelCircumference);

    // Give speed to both left and right motor
    float leftMotorSpeed = transVel - rotVel;
    float rightMotorSpeed = -transVel - rotVel;

    int tau_max = 5;

    if (abs(leftMotorSpeed) > tau_max) {
      leftMotorSpeed = sign(leftMotorSpeed) * tau_max;
    }

    if (abs(rightMotorSpeed) > tau_max) {
      rightMotorSpeed = sign(rightMotorSpeed) * tau_max;
    }
  }

};

int main() {
  std::cout << "Hello World!\n";
  RobotController controller = RobotController();
  controller.go();

}