Merge pull request #2 from Juan-Carlos-TV/comentarios_control

ianyan11 · web-flow · commit 5bfc744efefe · 2022-05-03T11:39:53.000-05:00
Comentarios control 03-05-2022
diff --git a/src/control.py b/src/control.py
@@ -6,65 +6,80 @@
 from geometry_msgs.msg import Pose, Point, Twist, Vector3, Quaternion, TransformStamped
 from tf.transformations import euler_from_quaternion
 
-
+#Clase control que ejecuta las acciones de contol y contiene las variables involucradas
 class Control:
+    #Init pose object
     pose = Pose()
+    #Topic we are going to publish 
     posPub = rospy.Publisher('cmd_vel', Twist, queue_size = 10)
+    #The message is going to be a Twist class objects
     msg = Twist()
+    #Sintonization variables
     kpl = 0.8
     kpt = 0.5
+
     def __init__(self):
         self.tf_buffer = Buffer()
+        #Start tf listener
         self.listener = TransformListener(self.tf_buffer)
 
+    #Method to set the position
     def setDesiredPosition(self, data):
+        #Send the position using the broadcast method
         self.broadcastTransform(data)
         x=True
         while(x):
             try:
+                #Keep broadcasting the data
                 self.broadcastTransform(data)
+                #Update x with the return value of control method wich is
+                #   True if we finished the control or false if not
                 x = (not self.startControl())
             except:
                 None
-            
+        #Set velocities to 0
         self.setVelocities(Twist())
 
-
+    #Set the pose
     def setPosition(self, data):
         self.pose = data
 
-
+    #Method which execute control
     def startControl(self):
-        
+        #Look for the transform between robot and points
         transformObject = self.tf_buffer.lookup_transform("robot", "point", rospy.Time())
         rospy.loginfo(transformObject)
-        
+        #Extract translation
         trans = transformObject.transform.translation
+        #Compute PID
         self.pidCalculation(trans)
+        #Return true if robot reached the desired position
         return(abs(trans.x)<0.01 and abs(trans.y)<0.01)
         
-        
-
+    #Compute PID method
     def pidCalculation(self, error):
+        #Compute and Set linear and angular speed on msg
         self.msg.linear.x = self.kpl*error.x
-
         yaw = atan2(error.y,error.x)
         self.msg.angular.z = self.kpt*yaw
+        #Publish velocities
         self.setVelocities(self.msg)
 
         
-
+    #Publish the message for control propourses
     def setVelocities(self, speed):
         self.posPub.publish(speed)
-        
+    
     def broadcastTransform(self, data):
+        #Init transform broadcaster
         br = TransformBroadcaster()
         t = TransformStamped()
+        #Fill the transform with time, frames, translation and orientation data
         t.header.stamp = rospy.Time.now()
         t.header.frame_id = "world"
         t.child_frame_id = "point"
-        t.transform.translation = Vector3(data.x, data.y, data.z)
-        t.transform.rotation = Quaternion(0,0,0,1)
+        t.transform.translation = Vector3(data.x, data.y, data.z) #Translation
+        t.transform.rotation = Quaternion(0,0,0,1) #Orientation
         br.sendTransform(t)
 		
 
diff --git a/src/odometry.py b/src/odometry.py
@@ -9,65 +9,88 @@
 from geometry_msgs.msg import Pose, TransformStamped, Vector3
 import numpy as np
 
+#Class which has some characteristics of the robot and 
+#	has the odometry variables
 class Odom:
+	#Initialize pose class
 	p = Pose()
+	#Speed and delta time
 	wl = 0
 	wr = 0
 	dt = 0.01
 	#posPub = rospy.Publisher('Position', Pose, queue_size = 10)
 	r = 0.05 # Wheel radius, 0.05m
 	l = 0.19 # Distance between robot wheels 0.19m
 
+	#Get the speed of left wheel
 	def lSpeed(self, data):
 		self.wl = data.data
-	
+	#Get the speed of rigth wheel
 	def rSpeed(self, data):
 		self.wr = data.data
 	
+	#Calculate position based on the integration of velocities
 	def calculate(self):
 		euler = euler_from_quaternion((self.p.orientation.x,self.p.orientation.y,self.p.orientation.z,self.p.orientation.w))
 		yaw = euler[2]
 
+		#Computing and updating the position on the pose object
 		self.p.position.x += (self.r*(self.wr+self.wl)/2*self.dt*cos(yaw))
 		self.p.position.y += (self.r*(self.wr+self.wl)/2*self.dt*sin(yaw))
 
 		if(yaw>pi or yaw<-pi):
 			yaw*=-1
-			
+		
+		#Update yaw
 		yaw+=self.r*(self.wr-self.wl)/self.l*self.dt
+		#Send yaw value to ros log
 		rospy.loginfo(yaw)
+		#Compute the new quaternion with new yaw value
 		quat = quaternion_from_euler(0, 0, yaw)
+		#Update orientation 
 		self.p.orientation.x = quat[0]
 		self.p.orientation.y = quat[1]
 		self.p.orientation.z = quat[2]
 		self.p.orientation.w = quat[3]
 		#self.posPub.publish(self.p)
 
+	#Send transformation using broadcaster
 	def broadcastTransform(self):
+		#Initialize broadcaster
 		br = TransformBroadcaster()
 		t = TransformStamped()
+		#Fill the transform with the position and orientations
 		t.header.stamp = rospy.Time.now()
+		#Frame names
 		t.header.frame_id = "world"
 		t.child_frame_id = "robot"
 		t.transform.translation = Vector3(self.p.position.x, self.p.position.y,self.p.position.z)
 		t.transform.rotation = self.p.orientation
+		#Send transform
 		br.sendTransform(t)
-		
+	
+	#Restart pose
 	def restart(self,_):
 		self.p = Pose()
 
+	#Get the pose directly
 	def getPose(self):
 		return self.p
 
 
 def main():
+	#Init the of odometry
 	rospy.init_node('Odometry', anonymous=True)
+	#Set rospy rate
 	r = rospy.Rate(100)
+	#Iniatilize class
 	odometria = Odom()
+	#Get speed from topic using the Odometry class methods
 	rospy.Subscriber("/wl", Float32, odometria.lSpeed)
 	rospy.Subscriber("/wr", Float32, odometria.rSpeed)
 	rospy.Subscriber("position/restart",Empty, odometria.restart)
 
+	#Keep calculating the position and sending the transform
 	while not rospy.is_shutdown():
 		odometria.calculate()
 		odometria.broadcastTransform()
