├── trajectory_tracking
    ├── msg
    │   └── Traj.msg
    ├── src
    │   ├── obstacles.txt
    │   └── tracking_controller.cpp
    ├── launch
    │   └── main.launch
    ├── trajectories
    │   ├── linear.cpp
    │   ├── eight_shape.cpp
    │   └── circular.cpp
    ├── scripts
    │   └── plotter.py
    ├── package.xml
    └── CMakeLists.txt
└── README.md


/trajectory_tracking/msg/Traj.msg:
--------------------------------------------------------------------------------
1 | geometry_msgs/Pose2D pose
2 | geometry_msgs/Twist ref_vel
3 | 


--------------------------------------------------------------------------------
/trajectory_tracking/src/obstacles.txt:
--------------------------------------------------------------------------------
1 | 0.100696 2.88356 0.182349
2 | -2.34187 1.82967 0.163313
3 | -0.845611 -2.76587 0.192746
4 | 2.99339 -0.0522498 0.180914
5 | 


--------------------------------------------------------------------------------
/trajectory_tracking/launch/main.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <arg name="path" value="$(find trajectory_tracking)/src/obstacles.txt" />
 3 | 
 4 |   <node ns="circular" name="plotter" type="plotter.py" pkg="trajectory_tracking" output="screen">
 5 |     <remap from="/traj_topic" to="/circular" />
 6 |   </node>
 7 |   <node name="circular" type="circular" pkg="trajectory_tracking" output="screen">
 8 |     <param name="path" value="$(arg path)" />
 9 | 
10 |     <rosparam>
11 |       center_x: 0
12 |       center_y: 0
13 |       radius: 3
14 |       period: 150
15 |     </rosparam>
16 |   </node>
17 | 
18 |   <node name="robot" type="tracking_controller" pkg="trajectory_tracking" output="screen">
19 |     <param name="path" value="$(arg path)" />
20 |     <remap from="/traj_topic" to="/circular" />
21 |     <rosparam>
22 |       robot_radius: 0.089
23 |     </rosparam>
24 |   </node>
25 | 
26 |   <!-- node ns="linear" name="plotter" type="plotter.py" pkg="trajectory_tracking" output="screen">
27 |     <remap from="/traj_topic" to="/linear" />
28 |   </node>
29 |   <node name="linear" type="linear" pkg="trajectory_tracking" output="screen">
30 |     <rosparam>
31 |       x_start: 0
32 |       y_start: 0
33 |       gradient: 1
34 |       period: 60
35 |     </rosparam>
36 |   </node>
37 |   
38 |   <node ns="eight_shape" name="plotter" type="plotter.py" pkg="trajectory_tracking" output="screen">
39 |     <remap from="/traj_topic" to="/eight_shape" />
40 |   </node>
41 |   <node name="eight_shape" type="eight_shape" pkg="trajectory_tracking" output="screen">
42 |     <rosparam>
43 |       center_x: 0
44 |       center_y: 0
45 |       radius: 6
46 |       period: 60
47 |     </rosparam>
48 |   </node -->
49 | </launch>
50 | 


--------------------------------------------------------------------------------
/trajectory_tracking/trajectories/linear.cpp:
--------------------------------------------------------------------------------
 1 | #include "ros/ros.h"
 2 | #include "trajectory_tracking/Traj.h"
 3 | #include <cmath>
 4 | #include <ctime>
 5 | 
 6 | class LinearTrajectory{
 7 |     private:
 8 |         ros::NodeHandle nh;
 9 |         ros::Publisher traj_pub;
10 | 
11 |     public:
12 |         LinearTrajectory(){
13 |             traj_pub = nh.advertise<trajectory_tracking::Traj>("/linear", 10); // define publisher topic 
14 |         }
15 |         double period;
16 |         void publish_trajectory(double t){
17 |             trajectory_tracking::Traj traj;
18 | 
19 |             double x_start, y_start, gradient;  // declare line parameters
20 |             // get node name
21 |             std::string node_name = ros::this_node::getName();  
22 |             //get line parameters
23 |             nh.getParam(node_name + "/x_start", x_start);
24 |             nh.getParam(node_name + "/y_start", y_start);
25 |             nh.getParam(node_name + "/gradient", gradient);
26 |             nh.getParam(node_name + "/period", period);
27 | 
28 | 
29 |             double k = 6 * M_PI / period;  // define constant K = 6Pi/T
30 |             // define equations for positions x and y
31 |             traj.pose.x = x_start + (1/gradient) * (k * t - y_start);
32 |             traj.pose.y = y_start + gradient * (k * t - x_start);
33 |             // compute the first derivatives of the positions (velocity)
34 |             double x_dot = (1/gradient) * k;
35 |             double y_dot = gradient * k;
36 |             // compute theta
37 |             traj.pose.theta = atan2(y_dot, x_dot);
38 |             // compute the second derivatives of the positions (acceleration)
39 |             double x_dotdot = 0;
40 |             double y_dotdot = 0; 
41 |             // compute reference velocities
42 |             traj.ref_vel.linear.x = sqrt(pow(x_dot, 2) + pow(y_dot, 2));
43 |             traj.ref_vel.angular.z = (x_dot * y_dotdot + y_dot * x_dotdot) / (pow(x_dot, 2) + pow(y_dot, 2));
44 |             // publish trajectory information
45 |             traj_pub.publish(traj);
46 |         }
47 | };
48 | 
49 | int main(int argc, char** argv){
50 |     ros::init(argc, argv, "linear");
51 |     
52 |     LinearTrajectory linear_trajectory;
53 | 
54 |     ros::Rate loop_rate(10);
55 |     while (ros::ok()){
56 |         time_t begin, now;
57 |         time(&begin);
58 | 
59 |         double t = 0;
60 |         while(t <= linear_trajectory.period){
61 |             linear_trajectory.publish_trajectory(t);
62 |             t = time(&now) - begin;
63 |             loop_rate.sleep();
64 |         }
65 |     }
66 |     return 0;
67 | }
68 | 


--------------------------------------------------------------------------------
/trajectory_tracking/trajectories/eight_shape.cpp:
--------------------------------------------------------------------------------
 1 | #include "ros/ros.h"
 2 | #include "trajectory_tracking/Traj.h"
 3 | #include <cmath>
 4 | #include <ctime>
 5 | 
 6 | class EightShapeTrajectory{
 7 |     private:
 8 |         ros::NodeHandle nh;
 9 |         ros::Publisher traj_pub;
10 | 
11 |     public:
12 |         EightShapeTrajectory(){
13 |             traj_pub = nh.advertise<trajectory_tracking::Traj>("/eight_shape", 10); // define publisher topic 
14 |         }
15 |         double period;
16 |         void publish_trajectory(double t){
17 |             trajectory_tracking::Traj traj;
18 | 
19 |             double center_x, center_y, radius;  // declare eight_shape parameters
20 |             // get node name
21 |             std::string node_name = ros::this_node::getName();  
22 |             //get eight_shape parameters
23 |             nh.getParam(node_name + "/center_x", center_x);
24 |             nh.getParam(node_name + "/center_y", center_y);
25 |             nh.getParam(node_name + "/radius", radius);
26 |             nh.getParam(node_name + "/period", period);
27 | 
28 | 
29 |             double k = 2 * M_PI / period;  // define constant K = 2Pi/T
30 |             // define equations for positions x and y
31 |             traj.pose.x = center_x + 1.5*radius * sin(k * t);
32 |             traj.pose.y = center_y + radius * sin(2*k * t);
33 |             // compute the first derivatives of the positions (velocity)
34 |             double x_dot = k * 1.5*radius * cos(k * t);
35 |             double y_dot = 2*k * radius * cos(2*k * t);
36 |             // compute theta
37 |             traj.pose.theta = atan2(y_dot, x_dot);
38 |             // compute the second derivatives of the positions (acceleration)
39 |             double x_dotdot = -pow(k, 2) * 1.5*radius * cos(k * t);
40 |             double y_dotdot = -pow(2*k, 2) * radius * sin(2*k * t); 
41 |             // compute reference velocities
42 |             traj.ref_vel.linear.x = sqrt(pow(x_dot, 2) + pow(y_dot, 2));
43 |             traj.ref_vel.angular.z = (x_dot * y_dotdot + y_dot * x_dotdot) / (pow(x_dot, 2) + pow(y_dot, 2));
44 |             // publish trajectory information
45 |             traj_pub.publish(traj);
46 |         }
47 | };
48 | 
49 | int main(int argc, char** argv){
50 |     ros::init(argc, argv, "eight_shape");
51 |     
52 |     EightShapeTrajectory eight_shape_trajectory;
53 | 
54 |     ros::Rate loop_rate(10);
55 |     while (ros::ok()){
56 |         time_t begin, now;
57 |         time(&begin);
58 | 
59 |         double t = 0;
60 |         while(t <= eight_shape_trajectory.period){
61 |             eight_shape_trajectory.publish_trajectory(t);
62 |             t = time(&now) - begin;
63 |             loop_rate.sleep();
64 |         }
65 |     }
66 |     return 0;
67 | }
68 | 


--------------------------------------------------------------------------------
/trajectory_tracking/scripts/plotter.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | # import ROS Libraries
 4 | import rospy
 5 | from nav_msgs.msg import Odometry
 6 | from trajectory_tracking.msg import Traj
 7 | 
 8 | import rospkg
 9 | 
10 | # import Matplotlib
11 | import matplotlib.pyplot as plt
12 | import matplotlib.animation as animation
13 | import time
14 | import math
15 | import random
16 | 
17 | # global variables decalration 
18 | global traj, robot_x, robot_y
19 | 
20 | traj = Traj()
21 | 
22 | xd = []
23 | yd = [] 
24 | xrobot = []
25 | yrobot = []
26 | colors = [
27 |     (0.078, 0.804, 0.784, 1), (0.000, 0.412, 0.573, 1), (0.671, 0.718, 0.718, 1),
28 |     (0.463, 0.365, 0.412, 1), (1.000, 0.580, 0.439, 1), (0.424, 0.478, 0.537, 1),
29 |     (0.686, 0.255, 0.329, 1), (0.945, 0.353, 0.133, 1), (0.431, 0.188, 0.294, 1),
30 |     (0.635, 0.871, 0.817, 1)
31 | ]
32 | 
33 | def obstacles(path):
34 |     global obsts
35 |     with open(path + '/src/obstacles.txt') as f:
36 |         obsts = f.readlines()
37 |     obsts = [obst.strip() for obst in obsts]
38 |     for i in range(len(obsts)):
39 |         obsts[i] = [float(x) for x in obsts[i].split()]
40 | 
41 | # callback function for odom data
42 | def odom_callbk(msg):
43 |     global robot_x, robot_y
44 | 
45 |     robot_x = msg.pose.pose.position.x
46 |     robot_y = msg.pose.pose.position.y
47 | 
48 | # callback function to get trajectory
49 | def circular_traj_callbk(msg):
50 |     global traj
51 |     
52 |     traj.pose.x = msg.pose.x
53 |     traj.pose.y = msg.pose.y
54 | 
55 | # animate function to plot data
56 | def animate1(i):
57 |        
58 |     xd.append(float(traj.pose.x))
59 |     yd.append(float(traj.pose.y))
60 |     ax1.plot(xd,yd, 'r',linestyle='solid',label="reference trajectory",lw=1)  
61 | 
62 |     xrobot.append(float(robot_x))
63 |     yrobot.append(float(robot_y))
64 |     ax1.plot(xrobot, yrobot, 'k', linestyle='dashdot', label="robot's trajectory", lw=1)
65 |     
66 |     for obst in obsts:
67 |         ax1.add_artist( plt.Circle((obst[0], obst[1]), obst[2], color=colors[random.randint(0, 9)], alpha=0.9))
68 |     
69 |     ax1.grid(True)
70 |     ax1.legend()
71 |     ax1.axis("equal")    # comment this line out if you'd like to set limits for x and y axis
72 | 
73 |     
74 | if __name__ == '__main__':
75 | 
76 |     # initialization of ROS node
77 |     rospy.init_node('plotter', anonymous=True) #make node
78 |     rospy.Subscriber("/traj_topic", Traj, circular_traj_callbk, queue_size=10)
79 |     rospy.Subscriber("/odom", Odometry, odom_callbk, queue_size=10)
80 | 
81 |     fig1 = plt.figure()
82 |     ax1 = fig1.add_subplot(111)
83 | 
84 |     rospack = rospkg.RosPack()
85 |     path = rospack.get_path('trajectory_tracking')
86 |     obstacles(path)
87 | 
88 |     # uncomment to set axis limits
89 |     # ax1.set_xlim([-17, 17])
90 |     # ax1.set_ylim([-17, 17])
91 |     
92 |     time.sleep(0.1)
93 |     ani1 = animation.FuncAnimation(fig1, animate1, interval=100)
94 |     plt.show()
95 |     rospy.spin()
96 | 


--------------------------------------------------------------------------------
/trajectory_tracking/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>trajectory_tracking</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The trajectory_tracking package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="yusah@todo.todo">yusah</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/trajectory_tracking</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <exec_depend>message_runtime</exec_depend>
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>roscpp</build_depend>
53 |   <build_depend>rospy</build_depend>
54 |   <build_depend>std_msgs</build_depend>
55 |   <build_depend>geometry_msgs</build_depend>
56 |   <build_export_depend>roscpp</build_export_depend>
57 |   <build_export_depend>rospy</build_export_depend>
58 |   <build_export_depend>std_msgs</build_export_depend>
59 |   <build_export_depend>geometry_msgs</build_export_depend>
60 |   <exec_depend>roscpp</exec_depend>
61 |   <exec_depend>rospy</exec_depend>
62 |   <exec_depend>std_msgs</exec_depend>
63 |   <exec_depend>geometry_msgs</exec_depend>
64 | 
65 |   <!-- The export tag contains other, unspecified, tags -->
66 |   <export>
67 |     <!-- Other tools can request additional information be placed here -->
68 | 
69 |   </export>
70 | </package>
71 | 


--------------------------------------------------------------------------------
/trajectory_tracking/trajectories/circular.cpp:
--------------------------------------------------------------------------------
  1 | #include "ros/ros.h"
  2 | #include "ros/package.h"
  3 | #include "trajectory_tracking/Traj.h"
  4 | #include <cmath>
  5 | #include <ctime>
  6 | #include <fstream>
  7 | #include <cstdlib>
  8 | #include <iostream>
  9 | #include <vector>
 10 | 
 11 | class CircularTrajectory{
 12 |     private:
 13 |         ros::NodeHandle nh;
 14 |         ros::Publisher traj_pub;
 15 |         
 16 |     public:
 17 |         CircularTrajectory(){
 18 |             traj_pub = nh.advertise<trajectory_tracking::Traj>("/circular", 10); // define publisher topic 
 19 |         }
 20 |         double center_x, center_y, radius;  // declare circle parameters
 21 |         double period;
 22 |         void publish_trajectory(double t){
 23 |             trajectory_tracking::Traj traj;
 24 | 
 25 |             // get node name
 26 |             std::string node_name = ros::this_node::getName();  
 27 |             //get circle parameters
 28 |             nh.getParam(node_name + "/center_x", center_x);
 29 |             nh.getParam(node_name + "/center_y", center_y);
 30 |             nh.getParam(node_name + "/radius", radius);
 31 |             nh.getParam(node_name + "/period", period);
 32 | 
 33 |             double k = 2 * M_PI / period;  // define constant K = 2Pi/T
 34 |             // define equations for positions x and y
 35 |             traj.pose.x = center_x + radius * cos(k * t);
 36 |             traj.pose.y = center_y + radius * sin(k * t);
 37 |             // compute the first derivatives of the positions (velocity)
 38 |             double x_dot = -k * radius * sin(k * t);
 39 |             double y_dot = k * radius * cos(k * t);
 40 |             // compute theta
 41 |             traj.pose.theta = atan2(y_dot, x_dot);
 42 |             // compute the second derivatives of the positions (acceleration)
 43 |             double x_dotdot = -pow(k, 2) * radius * cos(k * t);
 44 |             double y_dotdot = -pow(k, 2) * radius * sin(k * t); 
 45 |             // compute reference velocities
 46 |             traj.ref_vel.linear.x = sqrt(pow(x_dot, 2) + pow(y_dot, 2));
 47 |             traj.ref_vel.angular.z = (x_dot * y_dotdot + y_dot * x_dotdot) / (pow(x_dot, 2) + pow(y_dot, 2));
 48 |             // publish trajectory information
 49 |             traj_pub.publish(traj);
 50 |         }
 51 | 
 52 |         void random_obstacles(std::string path){
 53 |             std::vector<int> angles;
 54 |             srand(time(0));
 55 |             angles.push_back(rand()%(90-50) + 50);
 56 |             angles.push_back(rand()%(180-140) + 140);
 57 |             angles.push_back(rand()%(270-230) + 230);
 58 |             angles.push_back(rand()%(360-320) + 320);
 59 |             
 60 |             std::ofstream file;
 61 |             file.open(path);
 62 |             for(auto i = 0; i != angles.size(); ++i){
 63 |                 radius = ((0.26) * rand()) / RAND_MAX + radius - 0.15;
 64 | 
 65 |                 double x = center_x + radius * cos(angles[i]*M_PI/180);
 66 |                 double y = center_y + radius * sin(angles[i]*M_PI/180);
 67 |                 double r = ((0.22 - 0.12) * rand()) / RAND_MAX + 0.12;
 68 | 
 69 |                 file << x << " " << y << " " << r << "\n";
 70 |             }
 71 |             file.close();
 72 |         }
 73 | };
 74 | 
 75 | int main(int argc, char** argv){
 76 |     ros::init(argc, argv, "circular");
 77 |     ros::NodeHandle private_node_handle("~");
 78 | 
 79 |     std::string path = private_node_handle.param<std::string>("path", "default");
 80 | 
 81 |     CircularTrajectory circular_trajectory;
 82 | 
 83 |     ros::Rate loop_rate(10);
 84 |     
 85 |     while (ros::ok()){
 86 |         time_t begin, now;
 87 |         time(&begin);
 88 |         
 89 |         double t = 0;
 90 |         circular_trajectory.publish_trajectory(t);
 91 |         circular_trajectory.random_obstacles(path);
 92 |         while(t <= circular_trajectory.period){
 93 |             circular_trajectory.publish_trajectory(t);
 94 |             t = time(&now) - begin;
 95 |             loop_rate.sleep();
 96 |         }
 97 |     }
 98 |     return 0;
 99 | }
100 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Trajectory tracking with obstacle avoidance
 2 | 
 3 | ## 1. Overview
 4 | This ROS package uses the tracking controller proposed in this [paper](https://www.sciencedirect.com/science/article/abs/pii/S0005109897000551) coupled with a new variant of the limit-cycle obstacle avoidance strategy to implement trajectory tracking with obstacle avoidance for nonholonomic constrained mobile robots. It uses hierarchical action selection to activate the avoidance controller when a disturbing obstacle is detected, and then switch to tracking controller in an obstacle free location.
 5 | 
 6 | ## 2. Build
 7 | * Clone the repo to the src folder of your catkin workspace
 8 |     ```
 9 |     $ git clone https://github.com/yabdulra/Trajectory-tracking-with-obstacle-avoidance.git
10 |     ```
11 |     To use the tracking controller without obstacle avoidance, clone the repo using:
12 |     ```
13 |     $ git clone -b v1.0 https://github.com/yabdulra/Trajectory-tracking-with-obstacle-avoidance.git
14 |     ```
15 | 
16 | * Change directory to `catkin_ws` and build.
17 |     ```
18 |     $ cd ..
19 |     $ catkin_make
20 |     ```
21 | 
22 | ## 3. Configuration
23 | A default launch file for the tracking controller, reference trajectory, and the plotter can be found in the `trajectory_tracking` package directory. The launch file contains the following configurable parameters:  
24 | * path: lets you define where to store obstacles' data. The avoidance controller in the robot node uses this data to determine the obstacle the robot must avoid, while the plotter node uses it to plot a representation of the obstacle.
25 | * center_x, center_y, radius, period: used to define parameters for the circular trajectory.
26 | * robot_radius: the avoidance controller uses this along with other predefined parameters to define safe margin for collision avoidance.
27 | 
28 |   Below is the sample configuration which can be modified in the launch file.
29 |   ```
30 |   <launch>
31 |     <arg name="path" value="$(find trajectory_tracking)/src/obstacles.txt" />
32 | 
33 |     <node ns="circular" name="plotter" type="plotter.py" pkg="trajectory_tracking" output="screen">
34 |       <remap from="/traj_topic" to="/circular" />
35 |     </node>
36 |     <node name="circular" type="circular" pkg="trajectory_tracking" output="screen">
37 |       <param name="path" value="$(arg path)" />
38 | 
39 |       <rosparam>
40 |         center_x: 0
41 |         center_y: 0
42 |         radius: 3
43 |         period: 150
44 |       </rosparam>
45 |     </node>
46 | 
47 |     <node name="robot" type="tracking_controller" pkg="trajectory_tracking" output="screen">
48 |       <param name="path" value="$(arg path)" />
49 |       <remap from="/traj_topic" to="/circular" />
50 |       <rosparam>
51 |         robot_radius: 0.089
52 |       </rosparam>
53 |     </node>
54 |   </launch>
55 |   ```
56 | 
57 | ## 4. Nodes
58 | * robot:
59 |   - subscribed topics:
60 |     * traj_topic (trajectory_tracking/Traj): The custom message contains the 2D pose of the trajectory and a referecne linear and angular velocities. This topic is remapped to subscribe to desired trajectory topic (circular, linear, 8_shape) of the same message type. The tracking controller uses this message to calculate appropriate control inputs to the robot.
61 |     * odom ([nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html)): Contains robot's pose data used by both controllers to calculate appropriate control inputs to the robot.
62 |   - published topics:
63 |     * cmd_vel ([geometry_msgs/Twist](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Twist.html)): the node uses this topic to publish velocity commands to drive the robot.
64 | * circular:
65 |   - published topics:
66 |     * circular (trajectory_tracking/Traj): contains pose and reference velocity of the circular trajectory.
67 | * plotter:
68 |   - sbscribed topics:
69 |     * odom ([nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html)): The node uses this data to plot the robot's trajectory.
70 |     * traj_topic (trajectory_tracking/Traj): The node uses thid data to plot the reference trajectory. This topic is remapped to subscribe to desired trajectory topic (circular, linear, 8_shape) of the same message type.
71 | ## 5. Launch
72 | * Source your workspace and launch your robot.
73 | 
74 | * In a new terminal, source your workspace and launch the trajectory_tracking package.
75 |     ```
76 |     $ source devel/setup.bash
77 |     $ roslaunch trajectory_tracking main.launch
78 |     ```
79 | 
80 | ## 6. Visualization
81 | Once the nodes are launched, you can visualize the simulated obstacles on the plotter as well as the robot's and reference trajectories. You can see the robot deviating from the referecne trajectory to avoid any obstacle that can obstruct its motion, and then switching back to tracking the trajectory when the obstacle is completely avoided.  
82 | Below are sample views of a robot tracking a circular trajectory of radius 3m centered at the origin. The plots depict the robot avoiding some randomly generated disturbing obstacles.  
83 | ![2](https://user-images.githubusercontent.com/61895971/189817321-9b8f6c19-1881-4799-a94b-edbdc2ef8585.png)
84 | ![6](https://user-images.githubusercontent.com/61895971/189817360-e0195ace-19bd-4e0b-be98-bf6d855599e1.png)
85 | 


--------------------------------------------------------------------------------
/trajectory_tracking/src/tracking_controller.cpp:
--------------------------------------------------------------------------------
  1 | #include <ros/ros.h>
  2 | #include <ros/package.h>
  3 | #include <tf/tf.h>
  4 | #include "trajectory_tracking/Traj.h"
  5 | #include <geometry_msgs/Twist.h>
  6 | #include <nav_msgs/Odometry.h>
  7 | 
  8 | #include <cmath>
  9 | #include <fstream>
 10 | #include <cstdlib>
 11 | #include <ctime>
 12 | #include <vector>
 13 | #include <algorithm>
 14 | using std::vector;
 15 | 
 16 | class Trajectory{
 17 |     private:
 18 |         ros::NodeHandle n;
 19 |         ros::Subscriber traj;
 20 |     public:
 21 |         Trajectory(){
 22 |             traj = n.subscribe("/traj_topic", 10, &Trajectory::traj_callback, this);
 23 |         }
 24 |         double x_traj, y_traj, theta_traj, v_r, w_r;
 25 | 
 26 |         void traj_callback(const trajectory_tracking::Traj &msg){
 27 |             // get trajectory info
 28 |             x_traj = msg.pose.x;
 29 |             y_traj = msg.pose.y;
 30 |             theta_traj = msg.pose.theta;
 31 |             v_r = msg.ref_vel.linear.x;
 32 |             w_r = msg.ref_vel.angular.z;
 33 |         }
 34 | };
 35 | 
 36 | class Robot{
 37 |     private:
 38 |         ros::NodeHandle nh;
 39 |         ros::Publisher vel_pub;
 40 |         ros::Subscriber odom;
 41 |         double x_pos = 0.0, y_pos = 0.0, theta = 0.0, r_radius, xs = 0.0, ys = 0.0;    
 42 | 
 43 |     public:
 44 |         Robot(){
 45 |             vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
 46 |             odom = nh.subscribe("odom", 10, &Robot::odom_callback, this);
 47 |             std::string node_name = ros::this_node::getName();
 48 |             nh.getParam(node_name + "/robot_radius", r_radius);
 49 |         }
 50 |         vector<int> sign_xs, sign_ys;
 51 |         double R_i;
 52 |         bool avoidance_suceeded = false;
 53 | 
 54 |         double get_robot_radius(){
 55 |             return r_radius;
 56 |         }
 57 | 
 58 |         void odom_callback(const nav_msgs::Odometry::ConstPtr &msg){
 59 |             // get robot's x and y position
 60 |             x_pos = msg->pose.pose.position.x;
 61 |             y_pos = msg->pose.pose.position.y;
 62 |             // convert theta from Quaternion to Euler and get robot's yaw
 63 |             tf::Quaternion quat;
 64 |             tf::quaternionMsgToTF(msg->pose.pose.orientation, quat);
 65 |             double roll, pitch, yaw;
 66 |             tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
 67 |             theta = yaw;
 68 |         }
 69 | 
 70 |         void tracking_controller(const Trajectory &t){
 71 |             geometry_msgs::Twist vel;
 72 |             // error dynamics
 73 |             double e_x = (t.x_traj - x_pos) * cos(theta) + (t.y_traj - y_pos) * sin(theta);
 74 |             double e_y = -(t.x_traj - x_pos) * sin(theta) + (t.y_traj - y_pos) * cos(theta);
 75 |             double e_theta = t.theta_traj - theta;
 76 |             e_theta = atan2(sin(e_theta), cos(e_theta));  // normalize theta between -PI and PI
 77 |             // controller parameters
 78 |             double ep = 0.9, b = 18;
 79 |             double a = sqrt(pow(t.v_r, 2) + pow(t.w_r, 2));
 80 |             double kx = 2 * ep * a, ky = b * abs(t.v_r), kz = kx;
 81 |             // compute control inputs
 82 |             vel.linear.x = t.v_r * cos(e_theta) + kx * e_x;
 83 |             vel.angular.z = t.w_r + (ky * e_y * sin(e_theta) / e_theta) + kx * e_theta;
 84 |             vel_pub.publish(vel);
 85 |         }
 86 |         // takes in a vector of obstacles and compute the distance of each obstacle from the robot
 87 |         // returns a vector of an obstacle with the least distance to the robot
 88 |         vector<double> closest_obstacle(vector<vector<double> > &obstacles){
 89 |             vector<double> dist_to_robot(obstacles.size());
 90 |             vector<double> obstacle;
 91 |             for(auto i = 0; i != obstacles.size(); ++i ){
 92 |                 dist_to_robot[i] = sqrt(pow((obstacles[i][0] - x_pos), 2) + pow((obstacles[i][1] - y_pos), 2));
 93 |             }
 94 | 
 95 |             int index = std::min_element(dist_to_robot.begin(), dist_to_robot.end()) - dist_to_robot.begin();
 96 |             obstacle.push_back(obstacles[index][0]);
 97 |             obstacle.push_back(obstacles[index][1]);
 98 |             obstacle.push_back(obstacles[index][2]);
 99 |             obstacle.push_back(dist_to_robot[index]);
100 |             
101 |             return obstacle;   // {x_obst, y_obst, r_obst, distance to robot}
102 |         }
103 | 
104 |         void escape_critetion(){
105 |             auto i = sign_xs.size(), j = sign_ys.size();
106 |             // checks if the vectors are not empty, track sign changes and set correct boolean for
107 |             // whether an obstacle if successfully avoided or not.
108 |             if(i > 0 && j > 0){
109 |                 if((sign_xs[i-1] != sign_xs[0]) && (sign_ys[j-1] != sign_ys[0])){
110 |                     avoidance_suceeded = true;
111 |                 }
112 |                 else{
113 |                     avoidance_suceeded = false;
114 |                 }
115 |             }
116 |             (std::signbit(xs) == false)? sign_xs.push_back(1) : sign_xs.push_back(-1);
117 |             (std::signbit(ys) == false)? sign_ys.push_back(1) : sign_ys.push_back(-1);
118 |         }
119 | 
120 |         void avoidance_controller(vector<double> & obst){
121 |             geometry_msgs::Twist vel;
122 |             double k = 1.8;   //controller parameter 1.8
123 |             double Rc = R_i - 0.2;  // radius of limit-cycle
124 |             // define robot position with respect to the limit-cycle
125 |             xs = x_pos - obst[0];
126 |             ys = y_pos - obst[1];
127 |             
128 |             double xsdot = -ys + xs * (pow(Rc, 2) - pow(xs, 2) - pow(ys, 2));
129 |             double ysdot = xs + ys * (pow(Rc, 2) - pow(xs, 2) - pow(ys, 2));
130 |             double thetadot = atan2(ysdot, xsdot);
131 |             double thetaerr = thetadot - theta;
132 |             thetaerr = atan2(sin(thetaerr), cos(thetaerr));
133 |             
134 |             double xsdotdot = -ysdot - 2 * xs * (xs * xsdot + ys * ysdot) + xsdot * (pow(Rc, 2) - pow(xs, 2) - pow(ys, 2));
135 |             double ysdotdot = xsdot - 2 * ys * (xs * xsdot + ys * ysdot) + ysdot * (pow(Rc, 2) - pow(xs, 2) - pow(ys, 2));
136 |             double thetadotdot = (xsdot * ysdotdot - ysdot * xsdotdot) / (pow(xsdot, 2) + pow(ysdot, 2));
137 | 
138 |             vel.linear.x = 0.14;
139 |             vel.angular.z = 1 * (thetadotdot + k * thetaerr);
140 |             vel_pub.publish(vel);
141 | 
142 |             this->escape_critetion();
143 |         }
144 | };
145 | // take in path to obstacles' txt file and return a vector of obstacles
146 | // each obstacle is defined by its x and y coordinates and radius.
147 | vector<vector<double> > obstacles(std::string path){
148 |     double x, y, radius;
149 |     vector<vector<double> > obsts;
150 |     std::ifstream file(path);
151 |     while(file >> x >> y >> radius){
152 |         obsts.push_back({x, y, radius});
153 |     }
154 |     
155 |     return obsts;
156 | }
157 | 
158 | 
159 | int main(int argc, char** argv){
160 |     ros::init(argc, argv, "robot");
161 |     ros::NodeHandle private_node_handle("~");
162 | 
163 |     Trajectory trajectory;
164 |     Robot robot;
165 |     ros::Rate loop_rate(10);
166 |     
167 |     std::string path = private_node_handle.param<std::string>("path", "default");
168 | 
169 |     vector<vector<double> > obsts = obstacles(path);
170 | 
171 |     vector<double> obst;
172 |     while(ros::ok()){
173 |         obst = robot.closest_obstacle(obsts);
174 |         
175 |         robot.R_i = obst[2] + robot.get_robot_radius() + 0.6;
176 |         if(obst[3] <= robot.R_i){
177 |             if (robot.avoidance_suceeded == false){
178 |                 robot.avoidance_controller(obst);
179 |             }
180 |             else{
181 |                 robot.tracking_controller(trajectory);
182 |             }    
183 |         }
184 |         else{
185 |             robot.tracking_controller(trajectory);
186 |             robot.avoidance_suceeded = false;
187 |             robot.sign_xs.clear();
188 |             robot.sign_ys.clear();
189 |         }
190 | 
191 |         ros::spinOnce();
192 |         loop_rate.sleep();
193 |     }
194 | 
195 |     return 0;
196 | }


--------------------------------------------------------------------------------
/trajectory_tracking/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 3.0.2)
  2 | project(trajectory_tracking)
  3 | 
  4 | ## Compile as C++11, supported in ROS Kinetic and newer
  5 | add_compile_options(-std=c++11)
  6 | 
  7 | ## Find catkin macros and libraries
  8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
  9 | ## is used, also find other catkin packages
 10 | find_package(catkin REQUIRED COMPONENTS
 11 |   roscpp
 12 |   rospy
 13 |   std_msgs
 14 |   geometry_msgs
 15 |   message_generation
 16 | )
 17 | 
 18 | ## System dependencies are found with CMake's conventions
 19 | # find_package(Boost REQUIRED COMPONENTS system)
 20 | 
 21 | 
 22 | ## Uncomment this if the package has a setup.py. This macro ensures
 23 | ## modules and global scripts declared therein get installed
 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 25 | # catkin_python_setup()
 26 | 
 27 | ################################################
 28 | ## Declare ROS messages, services and actions ##
 29 | ################################################
 30 | 
 31 | ## To declare and build messages, services or actions from within this
 32 | ## package, follow these steps:
 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in
 34 | ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
 35 | ## * In the file package.xml:
 36 | ##   * add a build_depend tag for "message_generation"
 37 | ##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
 38 | ##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
 39 | ##     but can be declared for certainty nonetheless:
 40 | ##     * add a exec_depend tag for "message_runtime"
 41 | ## * In this file (CMakeLists.txt):
 42 | ##   * add "message_generation" and every package in MSG_DEP_SET to
 43 | ##     find_package(catkin REQUIRED COMPONENTS ...)
 44 | ##   * add "message_runtime" and every package in MSG_DEP_SET to
 45 | ##     catkin_package(CATKIN_DEPENDS ...)
 46 | ##   * uncomment the add_*_files sections below as needed
 47 | ##     and list every .msg/.srv/.action file to be processed
 48 | ##   * uncomment the generate_messages entry below
 49 | ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 50 | 
 51 | ## Generate messages in the 'msg' folder
 52 | add_message_files(
 53 |    FILES
 54 |    Traj.msg
 55 | )
 56 | 
 57 | ## Generate services in the 'srv' folder
 58 | # add_service_files(
 59 | #   FILES
 60 | #   Service1.srv
 61 | #   Service2.srv
 62 | # )
 63 | 
 64 | ## Generate actions in the 'action' folder
 65 | # add_action_files(
 66 | #   FILES
 67 | #   Action1.action
 68 | #   Action2.action
 69 | # )
 70 | 
 71 | ## Generate added messages and services with any dependencies listed here
 72 | generate_messages(
 73 |    DEPENDENCIES
 74 |    std_msgs
 75 |    geometry_msgs
 76 | )
 77 | 
 78 | ################################################
 79 | ## Declare ROS dynamic reconfigure parameters ##
 80 | ################################################
 81 | 
 82 | ## To declare and build dynamic reconfigure parameters within this
 83 | ## package, follow these steps:
 84 | ## * In the file package.xml:
 85 | ##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
 86 | ## * In this file (CMakeLists.txt):
 87 | ##   * add "dynamic_reconfigure" to
 88 | ##     find_package(catkin REQUIRED COMPONENTS ...)
 89 | ##   * uncomment the "generate_dynamic_reconfigure_options" section below
 90 | ##     and list every .cfg file to be processed
 91 | 
 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder
 93 | # generate_dynamic_reconfigure_options(
 94 | #   cfg/DynReconf1.cfg
 95 | #   cfg/DynReconf2.cfg
 96 | # )
 97 | 
 98 | ###################################
 99 | ## catkin specific configuration ##
100 | ###################################
101 | ## The catkin_package macro generates cmake config files for your package
102 | ## Declare things to be passed to dependent projects
103 | ## INCLUDE_DIRS: uncomment this if your package contains header files
104 | ## LIBRARIES: libraries you create in this project that dependent projects also need
105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
106 | ## DEPENDS: system dependencies of this project that dependent projects also need
107 | 
108 | catkin_package(
109 |   CATKIN_DEPENDS message_runtime
110 | )
111 | 
112 | catkin_package(
113 | #  INCLUDE_DIRS include
114 | #  LIBRARIES trajectory_tracking
115 | #  CATKIN_DEPENDS roscpp rospy std_msgs
116 | #  DEPENDS system_lib
117 | )
118 | 
119 | ###########
120 | ## Build ##
121 | ###########
122 | 
123 | ## Specify additional locations of header files
124 | ## Your package locations should be listed before other locations
125 | include_directories(
126 | include
127 |   ${catkin_INCLUDE_DIRS}
128 | )
129 | 
130 | ## Declare a C++ library
131 | # add_library(${PROJECT_NAME}
132 | #   src/${PROJECT_NAME}/trajectory_tracking.cpp
133 | # )
134 | 
135 | ## Add cmake target dependencies of the library
136 | ## as an example, code may need to be generated before libraries
137 | ## either from message generation or dynamic reconfigure
138 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
139 | 
140 | ## Declare a C++ executable
141 | ## With catkin_make all packages are built within a single CMake context
142 | ## The recommended prefix ensures that target names across packages don't collide
143 | # add_executable(${PROJECT_NAME}_node src/trajectory_tracking_node.cpp)
144 | 
145 | ## Rename C++ executable without prefix
146 | ## The above recommended prefix causes long target names, the following renames the
147 | ## target back to the shorter version for ease of user use
148 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
149 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
150 | 
151 | ## Add cmake target dependencies of the executable
152 | ## same as for the library above
153 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
154 | 
155 | ## Specify libraries to link a library or executable target against
156 | # target_link_libraries(${PROJECT_NAME}_node
157 | #   ${catkin_LIBRARIES}
158 | # )
159 | 
160 | #############
161 | ## Install ##
162 | #############
163 | 
164 | # all install targets should use catkin DESTINATION variables
165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
166 | 
167 | ## Mark executable scripts (Python etc.) for installation
168 | ## in contrast to setup.py, you can choose the destination
169 | # catkin_install_python(PROGRAMS
170 | #   scripts/my_python_script
171 | #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
172 | # )
173 | 
174 | ## Mark executables for installation
175 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
176 | # install(TARGETS ${PROJECT_NAME}_node
177 | #   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
178 | # )
179 | 
180 | ## Mark libraries for installation
181 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
182 | # install(TARGETS ${PROJECT_NAME}
183 | #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
184 | #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
185 | #   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
186 | # )
187 | 
188 | ## Mark cpp header files for installation
189 | # install(DIRECTORY include/${PROJECT_NAME}/
190 | #   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
191 | #   FILES_MATCHING PATTERN "*.h"
192 | #   PATTERN ".svn" EXCLUDE
193 | # )
194 | 
195 | ## Mark other files for installation (e.g. launch and bag files, etc.)
196 | # install(FILES
197 | #   # myfile1
198 | #   # myfile2
199 | #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
200 | # )
201 | 
202 | #############
203 | ## Testing ##
204 | #############
205 | 
206 | ## Add gtest based cpp test target and link libraries
207 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_trajectory_tracking.cpp)
208 | # if(TARGET ${PROJECT_NAME}-test)
209 | #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
210 | # endif()
211 | 
212 | ## Add folders to be run by python nosetests
213 | # catkin_add_nosetests(test)
214 | 
215 | 
216 | add_executable(circular trajectories/circular.cpp)
217 | target_link_libraries(circular ${catkin_LIBRARIES})
218 | add_dependencies(circular trajectory_tracking_generate_messages_cpp)
219 | 
220 | add_executable(linear trajectories/linear.cpp)
221 | target_link_libraries(linear ${catkin_LIBRARIES})
222 | add_dependencies(linear trajectory_tracking_generate_messages_cpp)
223 | 
224 | add_executable(eight_shape trajectories/eight_shape.cpp)
225 | target_link_libraries(eight_shape ${catkin_LIBRARIES})
226 | add_dependencies(eight_shape trajectory_tracking_generate_messages_cpp)
227 | 
228 | add_executable(tracking_controller src/tracking_controller.cpp)
229 | target_link_libraries(tracking_controller ${catkin_LIBRARIES})
230 | add_dependencies(tracking_controller trajectory_tracking_generate_messages_cpp)
231 | 
232 | 
233 | 
234 | 
235 | 


--------------------------------------------------------------------------------