ulvis.paste.net

Paste Search Dynamic
Recent pastes
angle
  1. #!/usr/bin/env python
  2. import rospy
  3. import math
  4. from math import sqrt, cos
  5. import copy
  6. import numpy as np
  7. from geometry_msgs.msg import Twist, Pose, PoseStamped, PoseWithCovariance, Quaternion
  8. from nav_msgs.msg import Odometry
  9. import tf
  10. from tf.transformations import euler_from_quaternion
  11. PI = 3.14159265359
  12.  
  13.  
  14. class Robot:
  15.  
  16.     def __init__(self):
  17.     # creates the subscriber for the odometry data and rviz goal data and the publisher for cmd_vel
  18.     self.odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
  19.     self.drive_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
  20.     self.nav_sub = rospy.Subscriber("/goal", PoseStamped, self.nav_to_pose)
  21.     # creates global variables that will be used in multiple functions
  22.     self.vel_msg = Twist()
  23.     self.current_pos = Pose()
  24.     self.roll, self.pitch, self.yaw = 0,0,0
  25.    
  26.    
  27.     def nav_to_pose(self, goal):
  28.         # type: (PoseStamped) -> None
  29.         """
  30.        This is a callback function. It should extract data from goal, drive in a straight line to reach the goal and
  31.        then spin to match the goal orientation.
  32.        :param goal: PoseStamped
  33.        :return:
  34.        """
  35.     speed = 0.3 # m/s
  36.     x0 = self.current_pos.position.x
  37.     y0 = self.current_pos.position.y
  38.     xf = goal.pose.position.x
  39.     yf = goal.pose.position.y
  40.     dist = sqrt((pow(x0-xf,2)) + (pow(y0-yf,2)))
  41.     x_dist = xf-x0 # x-distance between the two points
  42.     y_dist = yf-y0 # y-distance between the two points
  43.     if(x_dist < 0 and y_dist > 0): # determines angle within a 360 deg circle
  44.         angle = (cos(x_dist/dist) * (180/PI)) + 90
  45.     elif(x_dist < 0 and y_dist < 0):
  46.         angle = (cos(x_dist/dist) * (180/PI)) + 180
  47.     elif(x_dist > 0 and y_dist < 0):
  48.         angle = (cos(x_dist/dist) * (180/PI)) + 270
  49.     else:
  50.         angle = cos(x_dist/dist) * (180/PI)
  51.     self.rotate(angle)
  52.     self.drive_straight(speed, dist)
  53.  
  54.  
  55.     def drive_straight(self, speed, distance):
  56.     """
  57.        Make the turtlebot drive straight
  58.        :type speed: float
  59.        :type distance: float
  60.        :param speed: speed to drive
  61.        :param distance: distance to drive
  62.        :return:
  63.        """
  64.     current_dist = 0
  65.     #vel_msg = Twist()
  66.     # sets the direction/movement only in x-direction (straight)
  67.     self.vel_msg.linear.x = abs(speed)
  68.     self.vel_msg.linear.y = 0
  69.     self.vel_msg.linear.z = 0
  70.     self.vel_msg.angular.x = 0
  71.     self.vel_msg.angular.y = 0
  72.     self.vel_msg.angular.z = 0
  73.     while(current_dist < distance):
  74.         self.drive_pub.publish(self.vel_msg) # publish velocity
  75.         current_dist = ((self.current_pos.position.x)**2) + ((self.current_pos.position.y)**2) # current_dist is the magnitude of the distance traveled
  76.     # when distance traveled = specified distance --> stops robot via publishing to cmd_vel
  77.     self.vel_msg.linear.x = 0
  78.     self.drive_pub.publish(self.vel_msg)  
  79.        
  80.  
  81.  
  82.  
  83.  
  84.     def rotate(self, deg_angle):
  85.         """
  86.        Rotate in place
  87.        :param angle: angle to rotate in rad
  88.        :return:
  89.        """
  90.     rad_angle = deg_angle*2*PI/360 # translates angle from degrees to radians (can be taken out -- used for visual thinking)
  91.     if(rad_angle > 3.14): # turtlebot3 reads angles from 0 to PI and -PI to 0
  92.         rad_angle = rad_angle - (PI*2) # if angle is greater than PI, finds the corresponding negative angle
  93.     current_orient = self.yaw
  94.     angular_speed = 0.523599 # rad/sec (30 deg/sec)
  95.     self.vel_msg.linear.x = 0
  96.     self.vel_msg.linear.y = 0
  97.     self.vel_msg.linear.z = 0
  98.     self.vel_msg.angular.x = 0
  99.     self.vel_msg.angular.y = 0
  100.     if(rad_angle > 0): # if angle is positive, rotate counter-clockwise
  101.         while(current_orient <= rad_angle):
  102.             self.vel_msg.angular.z = angular_speed
  103.             self.drive_pub.publish(self.vel_msg)
  104.             current_orient = self.yaw
  105.     elif(rad_angle < 0): # if angle is negative, rotate clockwise to angle
  106.         while(current_orient >= rad_angle):
  107.             self.vel_msg.angular.z = -angular_speed
  108.             self.drive_pub.publish(self.vel_msg)
  109.             current_orient = self.yaw
  110.     self.vel_msg.angular.z = 0 # when angle has been reached, set angular velocity to 0 (stop moving)
  111.     self.drive_pub.publish(self.vel_msg)
  112.        
  113.    
  114.  
  115.  
  116.     def odom_callback(self, msg):
  117.     """
  118.        update the state of the robot
  119.    pulls the odometry data and finds the x position, y position and angular displacement of robot
  120.        :type msg: Odom
  121.        :return:
  122.        """
  123.     self.current_pos.position.x = msg.pose.pose.position.x
  124.     self.current_pos.position.y = msg.pose.pose.position.y
  125.     quat = msg.pose.pose.orientation
  126.     q = [quat.x, quat.y, quat.z, quat.w]
  127.     self.roll, self.pitch, self.yaw = euler_from_quaternion(q)
  128.        
  129.  
  130.  
  131.    
  132.  
  133. if __name__ == '__main__':
  134.     rospy.init_node('driving_node', anonymous=true)
  135.     turtleBot = Robot() #initialize robot object
  136.     #turtleBot.drive_straight(0.4, 1)
  137.     #turtleBot.rotate(300)
  138.     #rospy.spin()
  139.     #turtleBot.nav_to_pose()
  140.     while not rospy.is_shutdown():
  141.         pass
Parsed in 0.035 seconds