lab2.py

#!/usr/bin/env python2

import rospy
import math
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Twist
from tf.transformations import euler_from_quaternion

class Lab2:

	sleepyTime = 0.05
	tolerance = 0.05

	def __init__(self):
		"""
	    Class constructor
	    """

		### REQUIRED CREDIT
		### Initialize node, name it 'lab2'
		rospy.init_node('lab2', anonymous=True)
		### Tell ROS that this node publishes Twist messages on the '/cmd_vel' topic
		self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
		### Tell ROS that this node subscribes to Odometry messages on the '/odom' topic
		### When a message is received, call self.update_odometry
		rospy.Subscriber('/odom', Odometry , self.update_odometry)
		### Tell ROS that this node subscribes to PoseStamped messages on the'/move_base_simple/goal' topic
		### When a message is received, call self.go_to
		rospy.Subscriber('/move_base_simple/goal',PoseStamped, self.go_to)

		self.px = 0
		self.py = 0
		self.pth = 0

		hasOdomUpdated = False
		rospy.sleep(2.0)




	def send_speed(self, linear_speed, angular_speed):
		"""
	    Sends the speeds to the motors.
	    :param linear_speed  [float] [m/s]   The forward linear speed.
	    :param angular_speed [float] [rad/s] The angular speed for rotating around the body center.
	    """
		### REQUIRED CREDIT
		### Make a new Twist message
		msg_cmd_vel = Twist()
		### Publish the message
		msg_cmd_vel.linear.x = linear_speed
		msg_cmd_vel.linear.y = 0
		msg_cmd_vel.linear.z = 0

		msg_cmd_vel.angular.x = 0
		msg_cmd_vel.angular.y = 0
		msg_cmd_vel.angular.z = angular_speed

		self.pub.publish(msg_cmd_vel)



	def drive(self, distance, linear_speed):
		"""
	    Drives the robot in a straight line.
	    :param distance     [float] [m]   The distance to cover.
	    :param linear_speed [float] [m/s] The forward linear speed.
	    """
		start_pos = (self.px, self.py)
		curr_distance = math.sqrt((self.px - start_pos[0])**2+(self.py - start_pos[1])**2)
		### Mom are we there yet
		self.send_speed(linear_speed, 0)
		while curr_distance < distance - self.tolerance:
			curr_distance = math.sqrt((self.px - start_pos[0])**2+(self.py - start_pos[1])**2)
			rospy.sleep(self.sleepyTime)
		self.send_speed(0, 0)

		# time = distance/linear_speed
		# self.send_speed(linear_speed, 0)
		# rospy.sleep(time)
		# self.send_speed(linear_speed, 0)



	# Helper function for calc shortest diff of absolute angle
	def get_error_abs(self, set_point, current_point):
		error = set_point - current_point

		if(abs(error) > (2*math.pi - 0) / 2):	#if this false, we done
			if(error > 0):
				error = error - 2*math.pi + 0
			else:
				error = error + 2*math.pi - 0

		return error



	def rotate(self, angle, aspeed):
		"""
	    Rotates the robot around the body center by the given angle.
	    :param angle         [float] [rad]   The distance to cover.
	    :param angular_speed [float] [rad/s] The angular speed.
	    """

		curr_angle = self.pth

		# which way should the robot rotate to reach the desired angle in the shortest distance
		direction = math.copysign(1, self.get_error_abs(angle, curr_angle))

		self.send_speed(0, direction*aspeed)

		### Mom are we there yet
#		self.send_speed(0, aspeed)		replaced by smart get_error_abs
		while abs(angle - curr_angle) > self.tolerance:
			curr_angle = self.pth
			rospy.sleep(self.sleepyTime)
		self.send_speed(0, 0)


	# Helper function
	def get_angle(self, msg):
		self.px = msg.pose.position.x
		self.py = msg.pose.position.y
		quat_orig = msg.pose.orientation
		quat_list = [quat_orig.x, quat_orig.y, quat_orig.z, quat_orig.w]
		(roll, pitch, yaw) = euler_from_quaternion(quat_list)

		return yaw


	def go_to(self, msg):
		"""
	    Calls rotate(), drive(), and rotate() to attain a given pose.
	    This method is a callback bound to a Subscriber.
	    :param msg [PoseStamped] The target pose.
	    """
		### REQUIRED CREDIT
		x1 = self.px
		y1 = self.py
		x2 = msg.pose.position.x
		y2 = msg.pose.position.y

		yT = y2 - y1
		xT = x2 - x1

		theta = math.atan2(yT,xT)
		d = math.sqrt(yT**2 + xT**2)

		self.rotate(theta,0.75)
		self.drive(d,0.1)

		# Mamaaaa we were bad plz forgive us
		# self.px = msg.pose.position.x
		# self.py = msg.pose.position.y
		# quat_orig = msg.pose.orientation
		# quat_list = [quat_orig.x, quat_orig.y, quat_orig.z, quat_orig.w]
		# (roll, pitch, yaw) = euler_from_quaternion(quat_list)

		rotate_by = self.get_angle(msg)
		self.rotate(rotate_by, 0.75)




	def update_odometry(self, msg):
		"""
	    Updates the current pose of the robot.
	    This method is a callback bound to a Subscriber.
	    :param msg [Odometry] The current odometry information.
	    """
		### REQUIRED CREDIT
		self.px = msg.pose.pose.position.x
		self.py = msg.pose.pose.position.y
		quat_orig = msg.pose.pose.orientation
		quat_list = [quat_orig.x, quat_orig.y, quat_orig.z, quat_orig.w]
		(roll, pitch, yaw) = euler_from_quaternion(quat_list)
		self.pth = yaw



	def arc_to(self, position):
		"""
	    Drives to a given position in an arc.
	    :param msg [PoseStamped] The target pose.
	    """
		### EXTRA CREDIT
		# TODO
		pass # delete this when you implement your code



	def smooth_drive(self, distance, linear_speed):
		"""
	    Drives the robot in a straight line by changing the actual speed smoothly.
	    :param distance     [float] [m]   The distance to cover.
	    :param linear_speed [float] [m/s] The maximum forward linear speed.
	    """
		### EXTRA CREDIT
		# TODO
		pass # delete this when you implement your code



	def run(self):
		rospy.spin()

if __name__ == '__main__':
	Lab2().run()