From e35d0c0757864379e457f629db4bf915dfbf1af6 Mon Sep 17 00:00:00 2001
From: anshulraje <88229267+anshulraje@users.noreply.github.com>
Date: Sat, 12 Feb 2022 20:04:22 +0530
Subject: [PATCH 1/3] PID testing

---
 rain_interface/scripts/controller_copy.py | 131 ++++++++++++++++++++++
 rain_interface/scripts/test.py            |  36 ++++++
 2 files changed, 167 insertions(+)
 create mode 100644 rain_interface/scripts/controller_copy.py
 create mode 100644 rain_interface/scripts/test.py

diff --git a/rain_interface/scripts/controller_copy.py b/rain_interface/scripts/controller_copy.py
new file mode 100644
index 0000000..079a286
--- /dev/null
+++ b/rain_interface/scripts/controller_copy.py
@@ -0,0 +1,131 @@
+#!/usr/bin/env python3	
+
+from codecs import strict_errors
+import rospy
+from std_msgs.msg import String
+from geometry_msgs.msg import PoseStamped, Twist, Point, Quaternion
+from nav_msgs.msg import Odometry
+from math import sqrt
+
+class Controller():
+    
+    def __init__(self, path, odom_topic):
+        # initialize variables
+        self.path = path
+        self.current_pose = PoseStamped()
+        self.desired_pose = PoseStamped()
+        
+        self.odom_subscriber = rospy.Subscriber(odom_topic, Odometry, callback=self.odom_callback)
+
+        self.e_x = 0.0
+        self.e_i_x = 0.0
+        self.e_d_x = 0.0
+        self.e_prev_x = 0.0
+
+        self.e_y = 0.0
+        self.e_i_y = 0.0
+        self.e_d_y = 0.0
+        self.e_prev_y = 0.0
+
+        self.kP_x = 0.0099
+        self.kI_x = 0.00000075
+        self.kD_x = 0.5
+
+        self.kP_y = 0.0099
+        self.kI_y = 0.00000075
+        self.kD_y = 0.5
+
+        self.miny = 0.01
+        self.minx = 0.01
+        self.tolerance = 0.1
+        self.prevent_divide_by_zero = 0.000000000000000000000001
+        # rospy.spin()
+        # self.error = 1000     # Define in controller/map from imported controller 
+
+    def get_velocity(self, desired_pose: PoseStamped):
+        self.desired_pose = desired_pose
+        velocity = Twist()
+        desired_x = self.desired_pose.pose.position.x
+        desired_y = self.desired_pose.pose.position.y
+        curr_x = self.current_pose.pose.position.x
+        curr_y = self.current_pose.pose.position.y
+
+        if (abs(desired_x)-abs(curr_x))<0.01:
+            x_ratio = 0.0
+        else:
+            x_ratio = (abs(desired_x)-abs(curr_x))/(abs(desired_x)-abs(curr_x)+abs(desired_y)-abs(curr_y))
+            print(x_ratio)
+        if (abs(desired_y)-abs(curr_y))<0.01:
+            y_ratio = 0.0
+        else:
+            y_ratio = (abs(desired_y)-abs(curr_y))/(abs(desired_x)-abs(curr_x)+abs(desired_y)-abs(curr_y))
+        # print(y_ratio)
+
+        if desired_x-curr_x<0:
+            desired_xvel = x_ratio*0.5*(-1)#*(desired_x-curr_x)/(abs(desired_x)+abs(curr_x)+self.prevent_divide_by_zero)
+        else:
+            desired_xvel = x_ratio*0.5
+        if desired_y-curr_y<0:
+            desired_yvel = y_ratio*0.5*(-1)
+        else:
+            desired_yvel = y_ratio*0.5#*(desired_y-curr_y)/(abs(desired_y)+abs(curr_y)+self.prevent_divide_by_zero)
+        #print("desired_xvel: {}".format(desired_xvel))
+
+        self.e_x = self.x_error()
+        self.e_i_x += self.e_x
+        self.e_d_x = self.e_x - self.e_prev_x
+        calc_x = (self.kP_x*self.e_x + self.kI_x*self.e_i_x + self.kD_x*self.e_d_x)
+        #print("calc_x: {}".format(calc_x))
+        
+        self.e_y = self.y_error()
+        self.e_i_y += self.e_y
+        self.e_d_y = self.e_y - self.e_prev_y
+        calc_y = (self.kP_y*self.e_y + self.kI_y*self.e_i_y + self.kD_y*self.e_d_y)
+
+        if calc_x < abs(desired_xvel):
+            calc_x = calc_x*desired_xvel/(abs(desired_xvel)+self.prevent_divide_by_zero)
+            print("1")
+        elif calc_x < self.minx:
+            calc_x = self.minx*desired_xvel/(abs(desired_xvel)+self.prevent_divide_by_zero)
+            print("2")
+        elif calc_x > abs(desired_xvel):
+            calc_x = desired_xvel
+            print("3")
+
+        if (calc_y > self.miny) and (calc_y < abs(desired_yvel)):
+            calc_y = calc_y*desired_yvel/(abs(desired_yvel)+self.prevent_divide_by_zero)
+        elif calc_y < self.miny:
+            calc_y = self.miny*desired_yvel/(abs(desired_yvel)+self.prevent_divide_by_zero)
+        elif calc_y > abs(desired_yvel):
+            calc_y = desired_yvel
+            
+        velocity.linear.x = calc_x
+        velocity.linear.y = calc_y
+
+        self.e_prev_x = self.e_x
+        self.e_prev_y = self.e_y
+
+        return velocity
+        
+    def odom_callback(self, msg: Odometry):
+        # store odometry data in class level variable
+        self.current_pose.pose = msg.pose.pose
+
+    def reached_intermediate_goal(self):
+        # return true if any point in the path other than goal is reached
+        error = self.error_calculation() 
+        return (error < self.tolerance)
+
+    def get_current_status(self):
+        # only for testing
+        return String("Status: No errors")
+
+    def error_calculation(self):
+        error = sqrt((self.desired_pose.pose.position.x - self.current_pose.pose.position.x)**2 + (self.desired_pose.pose.position.y - self.current_pose.pose.position.y)**2)
+        return error
+
+    def x_error(self):
+        return abs(self.desired_pose.pose.position.x - self.current_pose.pose.position.x)
+
+    def y_error(self):
+        return abs(self.desired_pose.pose.position.y - self.current_pose.pose.position.y)
\ No newline at end of file
diff --git a/rain_interface/scripts/test.py b/rain_interface/scripts/test.py
new file mode 100644
index 0000000..8a5c17a
--- /dev/null
+++ b/rain_interface/scripts/test.py
@@ -0,0 +1,36 @@
+#!/usr/bin/env python3
+
+import rospy
+from nav_msgs.msg import Path, Odometry
+from geometry_msgs.msg import PoseStamped, Point, Quaternion, Twist
+from controller_copy import Controller
+
+class Test():
+    def __init__(self):
+        self.odom_topic = "/odom"
+
+        self.target_path = Path()
+        self.target_path.poses.append(PoseStamped())
+        self.goal = PoseStamped()                # for testing
+        self.goal.pose.position.x = 3.0
+        self.goal.pose.position.y = 2.0
+        # self.target_path.poses.append(goal)
+        
+        self.controller = Controller(self.target_path, odom_topic=self.odom_topic)
+        
+if __name__ == "__main__":
+    rospy.init_node("test")
+    test = Test()
+    velocity = Twist()
+    pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
+    while not rospy.is_shutdown():
+        velocity = test.controller.get_velocity(test.goal)
+        pub.publish(velocity)
+        if test.controller.reached_intermediate_goal():
+            velocity.linear.x = 0.0
+            velocity.linear.y = 0.0
+            pub.publish(velocity)
+            print("goal reached")
+            break
+        else:
+            continue
\ No newline at end of file

From 8d416017786e20da25b990866b75f77946a4dd27 Mon Sep 17 00:00:00 2001
From: anshulraje <88229267+anshulraje@users.noreply.github.com>
Date: Tue, 15 Feb 2022 17:28:59 +0530
Subject: [PATCH 2/3] deleted unnecessary files

---
 rain_interface/scripts/controller_copy.py | 131 ----------------------
 1 file changed, 131 deletions(-)
 delete mode 100644 rain_interface/scripts/controller_copy.py

diff --git a/rain_interface/scripts/controller_copy.py b/rain_interface/scripts/controller_copy.py
deleted file mode 100644
index 079a286..0000000
--- a/rain_interface/scripts/controller_copy.py
+++ /dev/null
@@ -1,131 +0,0 @@
-#!/usr/bin/env python3	
-
-from codecs import strict_errors
-import rospy
-from std_msgs.msg import String
-from geometry_msgs.msg import PoseStamped, Twist, Point, Quaternion
-from nav_msgs.msg import Odometry
-from math import sqrt
-
-class Controller():
-    
-    def __init__(self, path, odom_topic):
-        # initialize variables
-        self.path = path
-        self.current_pose = PoseStamped()
-        self.desired_pose = PoseStamped()
-        
-        self.odom_subscriber = rospy.Subscriber(odom_topic, Odometry, callback=self.odom_callback)
-
-        self.e_x = 0.0
-        self.e_i_x = 0.0
-        self.e_d_x = 0.0
-        self.e_prev_x = 0.0
-
-        self.e_y = 0.0
-        self.e_i_y = 0.0
-        self.e_d_y = 0.0
-        self.e_prev_y = 0.0
-
-        self.kP_x = 0.0099
-        self.kI_x = 0.00000075
-        self.kD_x = 0.5
-
-        self.kP_y = 0.0099
-        self.kI_y = 0.00000075
-        self.kD_y = 0.5
-
-        self.miny = 0.01
-        self.minx = 0.01
-        self.tolerance = 0.1
-        self.prevent_divide_by_zero = 0.000000000000000000000001
-        # rospy.spin()
-        # self.error = 1000     # Define in controller/map from imported controller 
-
-    def get_velocity(self, desired_pose: PoseStamped):
-        self.desired_pose = desired_pose
-        velocity = Twist()
-        desired_x = self.desired_pose.pose.position.x
-        desired_y = self.desired_pose.pose.position.y
-        curr_x = self.current_pose.pose.position.x
-        curr_y = self.current_pose.pose.position.y
-
-        if (abs(desired_x)-abs(curr_x))<0.01:
-            x_ratio = 0.0
-        else:
-            x_ratio = (abs(desired_x)-abs(curr_x))/(abs(desired_x)-abs(curr_x)+abs(desired_y)-abs(curr_y))
-            print(x_ratio)
-        if (abs(desired_y)-abs(curr_y))<0.01:
-            y_ratio = 0.0
-        else:
-            y_ratio = (abs(desired_y)-abs(curr_y))/(abs(desired_x)-abs(curr_x)+abs(desired_y)-abs(curr_y))
-        # print(y_ratio)
-
-        if desired_x-curr_x<0:
-            desired_xvel = x_ratio*0.5*(-1)#*(desired_x-curr_x)/(abs(desired_x)+abs(curr_x)+self.prevent_divide_by_zero)
-        else:
-            desired_xvel = x_ratio*0.5
-        if desired_y-curr_y<0:
-            desired_yvel = y_ratio*0.5*(-1)
-        else:
-            desired_yvel = y_ratio*0.5#*(desired_y-curr_y)/(abs(desired_y)+abs(curr_y)+self.prevent_divide_by_zero)
-        #print("desired_xvel: {}".format(desired_xvel))
-
-        self.e_x = self.x_error()
-        self.e_i_x += self.e_x
-        self.e_d_x = self.e_x - self.e_prev_x
-        calc_x = (self.kP_x*self.e_x + self.kI_x*self.e_i_x + self.kD_x*self.e_d_x)
-        #print("calc_x: {}".format(calc_x))
-        
-        self.e_y = self.y_error()
-        self.e_i_y += self.e_y
-        self.e_d_y = self.e_y - self.e_prev_y
-        calc_y = (self.kP_y*self.e_y + self.kI_y*self.e_i_y + self.kD_y*self.e_d_y)
-
-        if calc_x < abs(desired_xvel):
-            calc_x = calc_x*desired_xvel/(abs(desired_xvel)+self.prevent_divide_by_zero)
-            print("1")
-        elif calc_x < self.minx:
-            calc_x = self.minx*desired_xvel/(abs(desired_xvel)+self.prevent_divide_by_zero)
-            print("2")
-        elif calc_x > abs(desired_xvel):
-            calc_x = desired_xvel
-            print("3")
-
-        if (calc_y > self.miny) and (calc_y < abs(desired_yvel)):
-            calc_y = calc_y*desired_yvel/(abs(desired_yvel)+self.prevent_divide_by_zero)
-        elif calc_y < self.miny:
-            calc_y = self.miny*desired_yvel/(abs(desired_yvel)+self.prevent_divide_by_zero)
-        elif calc_y > abs(desired_yvel):
-            calc_y = desired_yvel
-            
-        velocity.linear.x = calc_x
-        velocity.linear.y = calc_y
-
-        self.e_prev_x = self.e_x
-        self.e_prev_y = self.e_y
-
-        return velocity
-        
-    def odom_callback(self, msg: Odometry):
-        # store odometry data in class level variable
-        self.current_pose.pose = msg.pose.pose
-
-    def reached_intermediate_goal(self):
-        # return true if any point in the path other than goal is reached
-        error = self.error_calculation() 
-        return (error < self.tolerance)
-
-    def get_current_status(self):
-        # only for testing
-        return String("Status: No errors")
-
-    def error_calculation(self):
-        error = sqrt((self.desired_pose.pose.position.x - self.current_pose.pose.position.x)**2 + (self.desired_pose.pose.position.y - self.current_pose.pose.position.y)**2)
-        return error
-
-    def x_error(self):
-        return abs(self.desired_pose.pose.position.x - self.current_pose.pose.position.x)
-
-    def y_error(self):
-        return abs(self.desired_pose.pose.position.y - self.current_pose.pose.position.y)
\ No newline at end of file

From 3698fc9ddbcc35bc62c0b2ab7557cd3a4ce72d41 Mon Sep 17 00:00:00 2001
From: anshulraje <88229267+anshulraje@users.noreply.github.com>
Date: Tue, 15 Feb 2022 19:03:41 +0530
Subject: [PATCH 3/3] added PID controller

---
 rain_interface/scripts/controller.py | 57 ++++++++++++++++++++++++----
 1 file changed, 49 insertions(+), 8 deletions(-)

diff --git a/rain_interface/scripts/controller.py b/rain_interface/scripts/controller.py
index a4faf94..1940ba4 100755
--- a/rain_interface/scripts/controller.py
+++ b/rain_interface/scripts/controller.py
@@ -5,6 +5,7 @@
 from std_msgs.msg import String
 from geometry_msgs.msg import PoseStamped, Twist, Point, Quaternion
 from nav_msgs.msg import Odometry
+from math import sqrt, atan2, sin, cos
 
 class Controller():
     
@@ -16,25 +17,65 @@ def __init__(self, path, odom_topic):
         
         self.odom_subscriber = rospy.Subscriber(odom_topic, Odometry, callback=self.odom_callback)
 
+        self.e = 0.0
+        self.e_i = 0.0
+        self.e_d = 0.0
+        self.e_prev = 0.0
+
+        self.kP = 0.3
+        self.kI = 0.00000025
+        self.kD = 0.0
+
+        self.max_vel = 0.5
+        self.min_vel = 0.05
         self.tolerance = 0.1
-        self.error = 1000     # Define in controller/map from imported controller 
+        
+        # rospy.spin()
+        # self.error = 1000     # Define in controller/map from imported controller 
 
     def get_velocity(self, desired_pose: PoseStamped):
         self.desired_pose = desired_pose
         velocity = Twist()
-        velocity.linear.x = 0.1     # only for testing
-        # implement controller
-        # velocity, error = controller_function()
+        desired_x = self.desired_pose.pose.position.x
+        desired_y = self.desired_pose.pose.position.y
+        curr_x = self.current_pose.pose.position.x
+        curr_y = self.current_pose.pose.position.y
+
+        self.e = self.error_calculation()
+        self.e_i += self.e
+        self.e_d = self.e - self.e_prev
+        desired_vel = (self.kP*self.e + self.kI*self.e_i + self.kD*self.e_d)
+
+        self.e_prev = self.e
+
+        if desired_vel > self.max_vel:
+            desired_vel = self.max_vel
+        elif desired_vel < self.min_vel:
+            desired_vel = self.min_vel
+
+        slope = atan2(desired_y-curr_y,desired_x-curr_x)
+        
+        x_vel = cos(slope)*desired_vel
+        y_vel = sin(slope)*desired_vel
+        print(x_vel)
+        velocity.linear.x = x_vel
+        velocity.linear.y = y_vel
+
         return velocity
         
     def odom_callback(self, msg: Odometry):
         # store odometry data in class level variable
-        self.current_pose = msg.pose.pose
+        self.current_pose.pose = msg.pose.pose
 
     def reached_intermediate_goal(self):
-        # return true if any point in the path other than goal is reached 
-        return (self.error < self.tolerance)
+        # return true if any point in the path other than goal is reached
+        error = self.error_calculation() 
+        return (error < self.tolerance)
 
     def get_current_status(self):
         # only for testing
-        return String("Status: No errors")
\ No newline at end of file
+        return String("Status: No errors")
+
+    def error_calculation(self):
+        error = sqrt((self.desired_pose.pose.position.x - self.current_pose.pose.position.x)**2 + (self.desired_pose.pose.position.y - self.current_pose.pose.position.y)**2)
+        return error
\ No newline at end of file