Cpp Programming Projects

A* path-finding algorithm for a robot

~18 mins read

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
#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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
#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();

}

🎰