Revision history [back]

CLosing the question as its solution is already available online. Below is snipped of my version of the function of a proportional controller, since I had defined inside a class I will be having self, self can be removed if defined outside the class:

while (not self.destination):
            robotPosition, currentOrientation = self.getOdom()
            errorX = x - robotPosition.x
            errorY = y - robotPosition.y
            self.distance = math.sqrt(errorX**2 + errorY**2)
            print(f"The distance error is {self.distance}")
            self.requiredPathAngle = math.atan2(errorY, errorX)
            self.errorOrientation = self.requiredPathAngle - currentOrientation 
            if abs(self.requiredPathAngle - currentOrientation) > 0.1 and self.distance > 0.2:
                if self.requiredPathAngle > currentOrientation:
                    self.speed.angular.z = self.kp*abs(self.errorOrientation)*self.CONSTANT_ANGULAR_SPEED
                else:
                    self.speed.angular.z = -self.kp*abs(self.errorOrientation)*self.CONSTANT_ANGULAR_SPEED
                self.speed.linear.x = 0.0
            else:
                self.speed.angular.z = 0.0
                if self.distance > 0.2:
                    self.speed.linear.x = self.kp*self.distance*self.CONSTANT_LINEAR_SPEED
                else:
                    self.speed.linear.x = 0.0
                    self.destination = True
                    print(f"Destination is reached and the final position value is {robotPosition.x}, {robotPosition.y}")
            self.velocityPublisher.publish(self.speed)
            rospy.sleep(rate)