#!/usr/bin/env python
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from turtlesim.srv import SetPen
from math import pow, atan2, sqrt
PI = 3.1415926535897

def rotate(s,a,c):
    #Starts a new node
    #rospy.init_node('robot_cleaner', anonymous=True)
    velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    vel_msg = Twist()

    # Receiveing the user's input
    #print("Let's rotate your robot")
    speed = s#input("Input your speed (degrees/sec):")
    angle = a#input("Type your distance (degrees):")
    clockwise = c#input("Clockwise?: ") #True or false

    #Converting from angles to radians
    angular_speed = speed*2*PI/360
    relative_angle = angle*2*PI/360

    #We wont use linear components
    vel_msg.linear.x=0
    vel_msg.linear.y=0
    vel_msg.linear.z=0
    vel_msg.angular.x = 0
    vel_msg.angular.y = 0

    # Checking if our movement is CW or CCW
    if clockwise:
        vel_msg.angular.z = -abs(angular_speed)
    else:
        vel_msg.angular.z = abs(angular_speed)
    # Setting the current time for distance calculus
    t0 = rospy.Time.now().to_sec()
    current_angle = 0

    while(current_angle < relative_angle):
        velocity_publisher.publish(vel_msg)
        t1 = rospy.Time.now().to_sec()
        current_angle = angular_speed*(t1-t0)


    #Forcing our robot to stop
    vel_msg.angular.z = 0
    velocity_publisher.publish(vel_msg)
    #rospy.spin()

def move(s,d,f):
    # Starts a new node
    #rospy.init_node('robot_cleaner', anonymous=True)
    velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    vel_msg = Twist()
    
    #Receiveing the user's input
    #print("Let's move your robot")
    speed = s#input("Input your speed:")
    distance = d#input("Type your distance:")
    isForward = f#input("Foward?: ")#True or False
    
    #Checking if the movement is forward or backwards
    if(isForward):
        vel_msg.linear.x = abs(speed)
    else:
        vel_msg.linear.x = -abs(speed)
    #Since we are moving just in x-axis
    vel_msg.linear.y = 0
    vel_msg.linear.z = 0
    vel_msg.angular.x = 0
    vel_msg.angular.y = 0
    vel_msg.angular.z = 0
    
    while not rospy.is_shutdown():

        #Setting the current time for distance calculus
        t0 = rospy.Time.now().to_sec()
        current_distance = 0

        #Loop to move the turtle in an specified distance
        while(current_distance < distance):
            #Publish the velocity
            velocity_publisher.publish(vel_msg)
            #Takes actual time to velocity calculus
            t1=rospy.Time.now().to_sec()
            #Calculates distancePoseStamped
            current_distance= speed*(t1-t0)
        #After the loop, stops the robot
        vel_msg.linear.x = 0
        #Force the robot to stop
        velocity_publisher.publish(vel_msg)
	break


class TurtleBot:

    def __init__(self):
        # Creates a node with name 'turtlebot_controller' and make sure it is a
        # unique node (using anonymous=True).
        rospy.init_node('turtlebot_controller', anonymous=True)

        # Publisher which will publish to the topic '/turtle1/cmd_vel'.
        self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
                                                  Twist, queue_size=10)

        # A subscriber to the topic '/turtle1/pose'. self.update_pose is called
        # when a message of type Pose is received.
        self.pose_subscriber = rospy.Subscriber('/turtle1/pose',
                                                Pose, self.update_pose)

        self.pose = Pose()
        self.rate = rospy.Rate(10)

    def update_pose(self, data):
        """Callback function which is called when a new message of type Pose is
        received by the subscriber."""
        self.pose = data
        self.pose.x = round(self.pose.x, 4)
        self.pose.y = round(self.pose.y, 4)

    def euclidean_distance(self, goal_pose):
        """Euclidean distance between current pose and the goal."""
        return sqrt(pow((goal_pose.x - self.pose.x), 2) +
                    pow((goal_pose.y - self.pose.y), 2))

    def linear_vel(self, goal_pose, constant=1.5):
        """See video: https://www.youtube.com/watch?v=Qh15Nol5htM."""
        return constant * self.euclidean_distance(goal_pose)

    def steering_angle(self, goal_pose):
        """See video: https://www.youtube.com/watch?v=Qh15Nol5htM."""
        return atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x)

    def angular_vel(self, goal_pose, constant=6):
        """See video: https://www.youtube.com/watch?v=Qh15Nol5htM."""
        return constant * (self.steering_angle(goal_pose) - self.pose.theta)

    def move2goal(self,x,y):
        """Moves the turtle to the goal."""
        goal_pose = Pose()

        # Get the input from the user.
        goal_pose.x = x#input("Set your x goal: ")
        goal_pose.y = y#input("Set your y goal: ")

        # Please, insert a number slightly greater than 0 (e.g. 0.01).
        distance_tolerance = 0.5#input("Set your tolerance: ")

        vel_msg = Twist()

        while self.euclidean_distance(goal_pose) >= distance_tolerance:

            # Porportional controller.
            # https://en.wikipedia.org/wiki/Proportional_control

            # Linear velocity in the x-axis.
            vel_msg.linear.x = self.linear_vel(goal_pose)
            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 = self.angular_vel(goal_pose)

            # Publishing our vel_msg
            self.velocity_publisher.publish(vel_msg)

            # Publish at the desired rate.
            self.rate.sleep()

        # Stopping our robot after the movement is over.
        vel_msg.linear.x = 0
        vel_msg.angular.z = 0
        self.velocity_publisher.publish(vel_msg)

        # If we press control + C, the node will stop.
        #rospy.spin()

if __name__ == '__main__':
    try:
	turtle_set_pen = rospy.ServiceProxy('turtle1/set_pen', SetPen)
        x = TurtleBot()
	turtle_set_pen(0,0,0,0,1)
        x.move2goal(6,5.5)
	rotate(100,0,True)
	x.move2goal(1,8)
	turtle_set_pen(0,0,0,3,0)
	x.move2goal(1,6)
	x.move2goal(3,6)
	x.move2goal(3,8.5)
	turtle_set_pen(0,0,0,0,1)
	x.move2goal(4,6)
	turtle_set_pen(0,0,0,3,0)
	x.move2goal(5,6)
	x.move2goal(6,6)
	x.move2goal(5,7)
	x.move2goal(4,7.5)
	x.move2goal(5,9)
	x.move2goal(6,7)
	turtle_set_pen(0,0,0,0,1)
	x.move2goal(8,6)
	rotate(100,110,False)
	turtle_set_pen(0,0,0,3,0)
	x.move2goal(7.5,8.5)
	turtle_set_pen(0,0,0,0,1)
	x.move2goal(7.5,8.75)
	turtle_set_pen(0,0,0,3,0)
	x.move2goal(7.5,9)
	turtle_set_pen(0,0,0,0,1)
	x.move2goal(7.5,10)
    except rospy.ROSInterruptException:
        pass

#!/usr/bin/env python
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from turtlesim.srv import SetPen
from math import pow, atan2, sqrt
PI = 3.1415926535897

def rotate(s,a,c):
    #Starts a new node
    #rospy.init_node('robot_cleaner', anonymous=True)
    velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    vel_msg = Twist()

    # Receiveing the user's input
    #print("Let's rotate your robot")
    speed = s#input("Input your speed (degrees/sec):")
    angle = a#input("Type your distance (degrees):")
    clockwise = c#input("Clockwise?: ") #True or false

    #Converting from angles to radians
    angular_speed = speed*2*PI/360
    relative_angle = angle*2*PI/360

    #We wont use linear components
    vel_msg.linear.x=0
    vel_msg.linear.y=0
    vel_msg.linear.z=0
    vel_msg.angular.x = 0
    vel_msg.angular.y = 0

    # Checking if our movement is CW or CCW
    if clockwise:
        vel_msg.angular.z = -abs(angular_speed)
    else:
        vel_msg.angular.z = abs(angular_speed)
    # Setting the current time for distance calculus
    t0 = rospy.Time.now().to_sec()
    current_angle = 0

    while(current_angle < relative_angle):
        velocity_publisher.publish(vel_msg)
        t1 = rospy.Time.now().to_sec()
        current_angle = angular_speed*(t1-t0)


    #Forcing our robot to stop
    vel_msg.angular.z = 0
    velocity_publisher.publish(vel_msg)
    #rospy.spin()

def move(s,d,f):
    # Starts a new node
    #rospy.init_node('robot_cleaner', anonymous=True)
    velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    vel_msg = Twist()
    
    #Receiveing the user's input
    #print("Let's move your robot")
    speed = s#input("Input your speed:")
    distance = d#input("Type your distance:")
    isForward = f#input("Foward?: ")#True or False
    
    #Checking if the movement is forward or backwards
    if(isForward):
        vel_msg.linear.x = abs(speed)
    else:
        vel_msg.linear.x = -abs(speed)
    #Since we are moving just in x-axis
    vel_msg.linear.y = 0
    vel_msg.linear.z = 0
    vel_msg.angular.x = 0
    vel_msg.angular.y = 0
    vel_msg.angular.z = 0
    
    while not rospy.is_shutdown():

        #Setting the current time for distance calculus
        t0 = rospy.Time.now().to_sec()
        current_distance = 0

        #Loop to move the turtle in an specified distance
        while(current_distance < distance):
            #Publish the velocity
            velocity_publisher.publish(vel_msg)
            #Takes actual time to velocity calculus
            t1=rospy.Time.now().to_sec()
            #Calculates distancePoseStamped
            current_distance= speed*(t1-t0)
        #After the loop, stops the robot
        vel_msg.linear.x = 0
        #Force the robot to stop
        velocity_publisher.publish(vel_msg)
	break


class TurtleBot:

    def __init__(self):
        # Creates a node with name 'turtlebot_controller' and make sure it is a
        # unique node (using anonymous=True).
        rospy.init_node('turtlebot_controller', anonymous=True)

        # Publisher which will publish to the topic '/turtle1/cmd_vel'.
        self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
                                                  Twist, queue_size=10)

        # A subscriber to the topic '/turtle1/pose'. self.update_pose is called
        # when a message of type Pose is received.
        self.pose_subscriber = rospy.Subscriber('/turtle1/pose',
                                                Pose, self.update_pose)

        self.pose = Pose()
        self.rate = rospy.Rate(10)

    def update_pose(self, data):
        """Callback function which is called when a new message of type Pose is
        received by the subscriber."""
        self.pose = data
        self.pose.x = round(self.pose.x, 4)
        self.pose.y = round(self.pose.y, 4)

    def euclidean_distance(self, goal_pose):
        """Euclidean distance between current pose and the goal."""
        return sqrt(pow((goal_pose.x - self.pose.x), 2) +
                    pow((goal_pose.y - self.pose.y), 2))

    def linear_vel(self, goal_pose, constant=1.5):
        """See video: https://www.youtube.com/watch?v=Qh15Nol5htM."""
        return constant * self.euclidean_distance(goal_pose)

    def steering_angle(self, goal_pose):
        """See video: https://www.youtube.com/watch?v=Qh15Nol5htM."""
        return atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x)

    def angular_vel(self, goal_pose, constant=6):
        """See video: https://www.youtube.com/watch?v=Qh15Nol5htM."""
        return constant * (self.steering_angle(goal_pose) - self.pose.theta)

    def move2goal(self,x,y):
        """Moves the turtle to the goal."""
        goal_pose = Pose()

        # Get the input from the user.
        goal_pose.x = x#input("Set your x goal: ")
        goal_pose.y = y#input("Set your y goal: ")

        # Please, insert a number slightly greater than 0 (e.g. 0.01).
        distance_tolerance = 0.5#input("Set your tolerance: ")

        vel_msg = Twist()

        while self.euclidean_distance(goal_pose) >= distance_tolerance:

            # Porportional controller.
            # https://en.wikipedia.org/wiki/Proportional_control

            # Linear velocity in the x-axis.
            vel_msg.linear.x = self.linear_vel(goal_pose)
            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 = self.angular_vel(goal_pose)

            # Publishing our vel_msg
            self.velocity_publisher.publish(vel_msg)

            # Publish at the desired rate.
            self.rate.sleep()

        # Stopping our robot after the movement is over.
        vel_msg.linear.x = 0
        vel_msg.angular.z = 0
        self.velocity_publisher.publish(vel_msg)

        # If we press control + C, the node will stop.
        #rospy.spin()

if __name__ == '__main__':
    try:
	turtle_set_pen = rospy.ServiceProxy('turtle1/set_pen', SetPen)
        x = TurtleBot()
	turtle_set_pen(0,0,0,0,1)
        x.move2goal(6,5.5)
	rotate(100,0,True)
	x.move2goal(1,8)
	turtle_set_pen(0,0,0,3,0)
	x.move2goal(1,6)
	x.move2goal(3,6)
	x.move2goal(3,8.5)
	turtle_set_pen(0,0,0,0,1)
	x.move2goal(4,6)
	turtle_set_pen(0,0,0,3,0)
	x.move2goal(5,6)
	x.move2goal(6,6)
	x.move2goal(5,7)
	x.move2goal(4,7.5)
	x.move2goal(5,9)
	x.move2goal(6,7)
	turtle_set_pen(0,0,0,0,1)
	x.move2goal(8,6)
	rotate(100,110,False)
	turtle_set_pen(0,0,0,3,0)
	x.move2goal(7.5,8.5)
	turtle_set_pen(0,0,0,0,1)
	x.move2goal(7.5,8.75)
	turtle_set_pen(0,0,0,3,0)
	x.move2goal(7.5,9)
	turtle_set_pen(0,0,0,0,1)
	x.move2goal(7.5,10)
    except rospy.ROSInterruptException:
        pass

#!/usr/bin/env python #!/usr/bin/env python #Starts a new node #rospy.init_node('robot_cleaner', anonymous=True) # Receiveing the user's input #print("Let's rotate your robot") #input("Input your speed (degrees/sec):") #input("Type your distance (degrees):") #input("Clockwise?: ") #True or false #Converting from angles to radians #We wont use linear components # Checking if our movement is CW or CCW # Setting the current time for distance calculus #Forcing our robot to stop #rospy.spin() # Starts a new node #rospy.init_node('robot_cleaner', anonymous=True) #Receiveing the user's input #print("Let's move your robot") #input("Input your speed:") #input("Type your distance:") #input("Foward?: ")#True or False #Checking if the movement is forward or backwards #Since we are moving just in x-axis #Setting the current time for distance calculus #Loop to move the turtle in an specified distance #Publish the velocity #Takes actual time to velocity calculus #Calculates distancePoseStamped #After the loop, stops the robot #Force the robot to stop # Creates a node with name 'turtlebot_controller' and make sure it is a # unique node (using anonymous=True). # Publisher which will publish to the topic '/turtle1/cmd_vel'. # A subscriber to the topic '/turtle1/pose'. self.update_pose is called # when a message of type Pose is received. Callback function which is called when a new message of type Pose is         received by the subscriber. Euclidean distance between current pose and the goal. See video: https://www.youtube.com/watch?v=Qh15Nol5htM. See video: https://www.youtube.com/watch?v=Qh15Nol5htM. See video: https://www.youtube.com/watch?v=Qh15Nol5htM. Moves the turtle to the goal. # Get the input from the user. #input("Set your x goal: ") #input("Set your y goal: ") # Please, insert a number slightly greater than 0 (e.g. 0.01). #input("Set your tolerance: ") # Porportional controller. # https://en.wikipedia.org/wiki/Proportional_control # Linear velocity in the x-axis. # Angular velocity in the z-axis. # Publishing our vel_msg # Publish at the desired rate. # Stopping our robot after the movement is over. # If we press control + C, the node will stop. #rospy.spin()