Add files via upload

Author: FilipMatic

gotoXY_NEW.py

@@ -17,8 +17,8 @@ def __init__(self):
         self.rate = rospy.Rate(5)
 
     orientation = 0
-
-    yaw = 0
+    global yaw
+    yaw = 0.0
 
     def convertQtoEuler():
         quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
@@ -30,40 +30,48 @@ def convertQtoEuler():
     #Callback function implementing the pose value received
     def callback(self, data):
         self.pose = data
-        self.pose.pose.position.x = round(self.pose.pose.position.x, 4)
-        self.pose.pose.position.y = round(self.pose.pose.position.y, 4)
+        self.pose.pose.pose.position.x = round(self.pose.pose.pose.position.x, 4)
+        self.pose.pose.pose.position.y = round(self.pose.pose.pose.position.y, 4)
 
     def get_distance(self, goal_x, goal_y):
-        distance = sqrt(pow((goal_x - self.pose.x), 2) + pow((goal_y - self.pose.y), 2))
+        distance = sqrt(pow((goal_x - self.pose.pose.pose.positionx), 2) + pow((goal_y - self.pose.pose.pose.position.y), 2))
         return distance
 
     def move2goal(self):
-        #goal_pose = Pose()
         goal_pose = Odometry()
-        goal_pose.pose.position.x = input("Set your x goal: ")
-        goal_pose.pose.position.y = input("Set your y goal: ")
-	#goal_pose.theta = input("Set your final orientation: ")
+        print(goal_pose)
+        goal_pose.pose.pose.position.x = input("Set your x goal: ")
+        goal_pose.pose.pose.position.y = input("Set your y goal: ")
         angle = input("Set your final orientation: ")
-	#orientation = goal_pose.theta
         distance_tolerance = input("Set your tolerance: ")
         vel_msg = Twist()
 
-        while sqrt(pow((goal_pose.pose.position.x - self.pose.pose.position.x), 2) + pow((goal_pose.pose.position.y - self.pose.pose.position.y), 2)) >= distance_tolerance:
+        while sqrt(pow((goal_pose.pose.pose.position.x - self.pose.pose.pose.position.x), 2) + pow((goal_pose.pose.pose.position.y - self.pose.pose.pose.position.y), 2)) >= distance_tolerance:
 
             #Porportional Controller
+            
             #linear velocity in the x-axis:
-            vel_msg.linear.x = 1.5 * sqrt(pow((goal_pose.pose.position.x - self.pose.pose.position.x), 2) + pow((goal_pose.pose.position.y - self.pose.pose.position.y), 2))
+            v_x = 1.5 * sqrt(pow((goal_pose.pose.pose.position.x - self.pose.pose.pose.position.x), 2) + pow((goal_pose.pose.pose.position.y - self.pose.pose.pose.position.y), 2))
+            if(v_x > 0.5):
+                v_x = 0.5
+            vel_msg.linear.x = v_x
             vel_msg.linear.y = 0
             vel_msg.linear.z = 0
 
             #angular velocity in the z-axis:
             vel_msg.angular.x = 0
             vel_msg.angular.y = 0
-            vel_msg.angular.z = 4 * (atan2(goal_pose.pose.position.y - self.pose.pose.position.y, goal_pose.pose.position.x - self.pose.pose.position.x) - yaw)
-
+            v_z = 4 * (atan2(goal_pose.pose.pose.position.y - self.pose.pose.pose.position.y, goal_pose.pose.pose.position.x - self.pose.pose.pose.position.x) - yaw)
+            if(v_z < -4.25):
+                v_z = -4.25
+            if(v_z > 4.25):
+                v_z = 4.25
+            vel_msg.angular.z = v_z
+            
             #Publishing our vel_msg
             self.velocity_publisher.publish(vel_msg)
             self.rate.sleep()
+            
         #Stopping our robot after the movement is over
         vel_msg.linear.x = 0
         vel_msg.angular.z = orientation - yaw