Example #1
0
 def rotate(self, angle, tolerance):
     (self.position, self.rotation) = self.get_odom()
     move_cmd = Twist()
     # Set the movement command to a rotation
     if angle > 0:
         move_cmd.angular.z = self.angular_speed
     else:
         move_cmd.angular.z = -1*self.angular_speed
         angle = -1*angle
         
     # Track the last angle measured
     last_angle = self.rotation
     
     # Track how far we have turned
     turn_angle = 0
     while abs(turn_angle + tolerance) < abs(angle) and not rospy.is_shutdown():
         # Publish the Twist message and sleep 1 cycle         
         self.cmd_vel.publish(move_cmd)
         self.r.sleep()
         
         # Get the current rotation
         (self.position, self.rotation) = self.get_odom()
         
         # Compute the amount of rotation since the last loop
         delta_angle = normalize_angle(self.rotation - last_angle)
         
         # Add to the running total
         turn_angle += delta_angle
         last_angle = self.rotation
Example #2
0
def rotate(sign):
    # Initialize the movement command
    move_cmd = Twist()

    # Track the last angle measured
    last_angle = get_odom()[1]
    # Track how far we have turned
    turn_angle = 0

    # Turn right
    move_cmd.angular.z = angular_speed * sign
    while (
        abs(turn_angle + angular_tolerance) < abs(goal_angle)
        and not rospy.is_shutdown()
    ):
        cmd_vel_pub.publish(move_cmd)
        r.sleep()
        # Get the current rotation
        rotation = get_odom()[1]

        # Compute the amount of rotation since the last loop
        delta_angle = normalize_angle(rotation - last_angle)

        # Add to the running total
        turn_angle += delta_angle
        last_angle = rotation

    rotation = get_odom()[1]
    print("Rotating robot...:", rotation)
Example #3
0
 def old_rotate(self, angle):
     last_angle = self.rot
     turn_angle = 0
     while abs(turn_angle + self.angular_tolerance) < abs(
             angle) and not rospy.is_shutdown():
         self.rotate_inc()
         self.update_odom()
         delta_angle = normalize_angle(self.rot - last_angle)
         turn_angle += delta_angle
         last_angle = self.rot
     self.cmd_vel.publish(Twist())
Example #4
0
    def turn(self, val):
        # How fast will we update the robot's movement?
        rate = 20
        
        # Set the equivalent ROS rate variable
        r = rospy.Rate(rate)
        
        # Initialize the position variable as a Point type
        position = Point()
            
        # Loop once for each leg of the trip
        #############
        # Initialize the movement command
        move_cmd = Twist()
            
        # Set the movement command to forward motion
        move_cmd.linear.x = self.linear_speed
        # Set the movement command to a rotation
        move_cmd.angular.z = self.angular_speed
        
        # Get the starting position values     
        (position, rotation) = self.get_odom()
                        
        x_start = position.x
        y_start = position.y

        # Track the last angle measured
        last_angle = rotation

        # Track how far we have turned
        turn_angle = 0

        while abs(turn_angle + self.angular_tolerance) < abs(self.goal_angle) and not rospy.is_shutdown():
            
            # Publish the Twist message and sleep 1 cycle         
            self.cmd_vel.publish(move_cmd)
            r.sleep()
                
            # Get the current rotation
            (position, rotation) = self.get_odom()
                
            # Compute the amount of rotation since the last loop
            delta_angle = normalize_angle(rotation - last_angle)
                
            # Add to the running total
            turn_angle += delta_angle
            last_angle = rotation
            
        # Stop the robot for good
        self.cmd_vel.publish(Twist())

        return True
    def execute(self, userdata):
        rospy.loginfo("Executing Rotate Class...")

        # Get the starting position values
        (position, rotation) = self.get_odom()
        last_angle = rotation
        turn_angle = 0

        rotate_command = Twist()
        rotate_command.angular.z = self.angular_speed
        #and not userdata.detectTag_Flag
        while abs(turn_angle + self.angular_tolerance) < abs(
                self.goal_angle) and not rospy.is_shutdown():

            if self.preempt_requested():
                rospy.loginfo("State Rotate is being preempted!!!")
                self.service_preempt()

                return 'not_full_rotate'

            # Publish the Twist message and sleep 1 cycle
            self.cmd_vel.publish(rotate_command)
            self.r.sleep()

            # Get the current rotation
            (position, rotation) = self.get_odom()

            # Compute the amount of rotation since the last loop
            delta_angle = normalize_angle(rotation - last_angle)

            # Add to the running total
            turn_angle += delta_angle
            last_angle = rotation

        # Stop the robot before the next leg
        rotate_command = Twist()
        self.cmd_vel.publish(rotate_command)
        rospy.sleep(1)

        return 'full_rotate'
Example #6
0
    def rotate_radians(self, goal_angle):

        angular_speed = 0.5
        angular_tolerance = 0.02

        (position, rotation) = self.get_odom()

        move_cmd = Twist()
        self.cmd_vel.publish(move_cmd)
        rospy.sleep(1)

        move_cmd.angular.z = angular_speed
        if goal_angle < 0:
            move_cmd.angular.z = -1 * angular_speed

        # Track the last angle measured
        last_angle = rotation

        # Track how far we have turned
        turn_angle = 0

        while abs(turn_angle + angular_tolerance) < abs(
                goal_angle) and not rospy.is_shutdown():
            self.cmd_vel.publish(move_cmd)
            self.r.sleep()

            (position, rotation) = self.get_odom()

            delta_angle = normalize_angle(rotation - last_angle)

            turn_angle += delta_angle
            last_angle = rotation

        move_cmd = Twist()
        self.cmd_vel.publish(move_cmd)
        rospy.sleep(1)

        (position, rotation) = self.get_odom()
Example #7
0
    def execute(self, userdata):
        rospy.loginfo("Rotating...")

        # Get the starting position values
        (position, rotation) = self.get_odom()
        last_angle = rotation
        turn_angle = 0

        rotate_command = Twist()
        rotate_command.angular.z = self.angular_speed

        while abs(turn_angle + self.angular_tolerance) < abs(self.goal_angle) and not rospy.is_shutdown():
            if self.preempt_requested():
                rospy.loginfo("State Rotate360 is being preempted!!!")
                self.service_preempt()
                return "not_full_rotate"

            # Publish the Twist message and sleep 1 cycle
            self.cmd_vel.publish(rotate_command)
            self.r.sleep()

            # Get the current rotation
            (position, rotation) = self.get_odom()

            # Compute the amount of rotation since the last loop
            delta_angle = normalize_angle(rotation - last_angle)

            # Add to the running total
            turn_angle += delta_angle
            last_angle = rotation

        # Stop the robot before the next leg
        rotate_command = Twist()
        self.cmd_vel.publish(rotate_command)
        rospy.sleep(1)

        return "full_rotate"
    def move_around(self, start_rotation, goal_radius=pi / 6):
        self._turn = PROCESS
        move_cmd = Twist()

        # Set the movement command to a rotation
        move_cmd.angular.z = self.angular_speed
        if goal_radius < 0:
            move_cmd.angular.z = -self.angular_speed
            goal_radius = -goal_radius

        # Track the last angle measured
        last_angle = start_rotation

        # Track how far we have turned
        turn_angle = 0

        while abs(turn_angle + self.angular_tolerance) < abs(
                goal_radius) and not rospy.is_shutdown():
            # Publish the Twist message and sleep 1 cycle
            self.cmd_vel.publish(move_cmd)
            self.r.sleep()

            # Get the current rotation
            (position, rotation) = self.get_odom()

            # Compute the amount of rotation since the last loop
            delta_angle = normalize_angle(rotation - last_angle)

            # Add to the running total
            turn_angle += delta_angle
            last_angle = rotation
        # Stop the robot
        move_cmd = Twist()
        self.cmd_vel.publish(move_cmd)
        rospy.sleep(1)
        print("Turn {} radius  end!".format(goal_radius))
        self._turn = FINISH
Example #9
0
    def __init__(self):
        # Give the node a name
        rospy.init_node('back_and_forth', anonymous=False)

        # Set rospy to execute a shutdown function when exiting       
        rospy.on_shutdown(self.shutdown)

        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        
        # How fast will we update the robot's movement?
        rate = 50
        
        # Set the equivalent ROS rate variable
        r = rospy.Rate(rate)
        
        # Set the forward linear speed to 0.15 meters per second 
        linear_speed = 0.2
        
        # Set the travel distance in meters
        goal_distance = 1.0

        # Set the rotation speed in radians per second
        angular_speed = 1
        
        # Set the angular tolerance in degrees converted to radians
        angular_tolerance = radians(1.0)
        
        # Set the rotation angle to Pi radians (180 degrees)
        goal_angle = pi/2 - ((pi/2)/20)

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()
        
        # Give tf some time to fill its buffer
        rospy.sleep(2)
        
        # Set the odom frame
        self.odom_frame = '/odom'
        
        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")  
        
        # Initialize the position variable as a Point type
        position = Point()
            
        # Loop once for each leg of the trip
        for i in range(4):
            # Initialize the movement command
            move_cmd = Twist()
            
            # Set the movement command to forward motion
            move_cmd.linear.x = linear_speed
            
            # Get the starting position values     
            (position, rotation) = self.get_odom()
                        
            x_start = position.x
            y_start = position.y
            
            # Keep track of the distance traveled
            distance = 0
            
            # Enter the loop to move along a side
            while distance < goal_distance and not rospy.is_shutdown():
                # Publish the Twist message and sleep 1 cycle         
                self.cmd_vel.publish(move_cmd)
                
                r.sleep()
        
                # Get the current position
                (position, rotation) = self.get_odom()
                
                # Compute the Euclidean distance from the start
                distance = sqrt(pow((position.x - x_start), 2) + 
                                pow((position.y - y_start), 2))

            # Stop the robot before the rotation
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
            
            # Set the movement command to a rotation
            move_cmd.angular.z = angular_speed
            
            # Track the last angle measured
            last_angle = rotation
            
            # Track how far we have turned
            turn_angle = 0
            
            while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
                # Publish the Twist message and sleep 1 cycle         
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                
                # Get the current rotation
                (position, rotation) = self.get_odom()
                
                # Compute the amount of rotation since the last loop
                delta_angle = normalize_angle(rotation - last_angle)
                
                # Add to the running total
                turn_angle += delta_angle
                last_angle = rotation
                
            # Stop the robot before the next leg
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
            
        # Stop the robot for good
        self.cmd_vel.publish(Twist())
    def __init__(self):
        # Give the node a name
        rospy.init_node('nav_square', anonymous=False)
        
        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)

        # How fast will we check the odometry values?
        rate = 20
        
        # Set the equivalent ROS rate variable
        r = rospy.Rate(rate)
        
        # Set the parameters for the target square
        goal_distance = rospy.get_param("~goal_distance", 1.0)      # meters
        goal_angle = rospy.get_param("~goal_angle", radians(90))    # degrees converted to radians
        linear_speed = rospy.get_param("~linear_speed", 0.2)        # meters per second
        angular_speed = rospy.get_param("~angular_speed", 0.7)      # radians per second
        angular_tolerance = rospy.get_param("~angular_tolerance", radians(2)) # degrees to radians
        
        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
         
        # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
        self.base_frame = rospy.get_param('~base_frame', '/base_link')

        # The odom frame is usually just /odom
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()
        
        # Give tf some time to fill its buffer
        rospy.sleep(2)
        
        # Set the odom frame
        self.odom_frame = '/odom'
        
        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")  
                
        # Initialize the position variable as a Point type
        position = Point()

        # Cycle through the four sides of the square
        for i in range(4):
            # Initialize the movement command
            move_cmd = Twist()
            
            # Set the movement command to forward motion
            move_cmd.linear.x = linear_speed
            
            # Get the starting position values     
            (position, rotation) = self.get_odom()
                        
            x_start = position.x
            y_start = position.y
            
            # Keep track of the distance traveled
            distance = 0
            
            # Enter the loop to move along a side
            while distance < goal_distance and not rospy.is_shutdown():
                # Publish the Twist message and sleep 1 cycle         
                self.cmd_vel.publish(move_cmd)
                
                r.sleep()
        
                # Get the current position
                (position, rotation) = self.get_odom()
                
                # Compute the Euclidean distance from the start
                distance = sqrt(pow((position.x - x_start), 2) + 
                                pow((position.y - y_start), 2))
                
            # Stop the robot before rotating
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1.0)
            
            # Set the movement command to a rotation
            move_cmd.angular.z = angular_speed
            
            # Track the last angle measured
            last_angle = rotation
            
            # Track how far we have turned
            turn_angle = 0
            
            # Begin the rotation
            while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
                # Publish the Twist message and sleep 1 cycle         
                self.cmd_vel.publish(move_cmd)
                
                r.sleep()
                
                # Get the current rotation
                (position, rotation) = self.get_odom()
                
                # Compute the amount of rotation since the last lopp
                delta_angle = normalize_angle(rotation - last_angle)
                
                turn_angle += delta_angle
                last_angle = rotation

            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1.0)
            
        # Stop the robot when we are done
        self.cmd_vel.publish(Twist())
    def __init__(self):
        # Give the node a name
        rospy.init_node('initial_odom_circle', anonymous=False)

        # Set rospy to execute a shutdown function when exiting       
        rospy.on_shutdown(self.shutdown)

        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size = 10)
        
        # How fast will we update the robot's movement?
        rate = 20
        
        # Set the equivalent ROS rate variable
        r = rospy.Rate(rate)
        
        # Set the forward linear speed to 0.2 meters per second 
        linear_speed = 0.0
        
        # Set the travel distance in meters
        goal_distance = 0.0

        # Set the rotation speed in radians per second
        angular_speed = 0.3
        
        # Set the angular tolerance in degrees converted to radians
        angular_tolerance = radians(2.5)
        
        # Set the rotation angle to Pi radians (180 degrees)
        goal_angle = 2*pi

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()
        
        # Give tf some time to fill its buffer
        rospy.sleep(2)
               
        
        # Set the odom frame
        self.odom_frame = '/odom'
        
        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")  
      
        # Initialize the position variable as a Point type
        position = Point()
            
        # Loop once for each leg of the trip

        # Initialize the movement command
        move_cmd = Twist()
            
        # Set the movement command to forward motion
        move_cmd.linear.x = linear_speed
        # Set the movement command to a rotation
        move_cmd.angular.z = angular_speed
        
        # Get the starting position values     
        (position, rotation) = self.get_odom()
                        
        x_start = position.x
        y_start = position.y

        # Track the last angle measured
        last_angle = rotation

        # Track how far we have turned
        turn_angle = 0

        while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
            
            # Publish the Twist message and sleep 1 cycle         
            self.cmd_vel.publish(move_cmd)
            r.sleep()
                
            # Get the current rotation
            (position, rotation) = self.get_odom()
                
            # Compute the amount of rotation since the last loop
            delta_angle = normalize_angle(rotation - last_angle)
                
            # Add to the running total
            turn_angle += delta_angle
            last_angle = rotation


            
        # Stop the robot for good
        self.cmd_vel.publish(Twist())
Example #12
0
    def __init__(self):
        # Creat a node named out_and_back
        rospy.init_node('nav_square', anonymous=False)
        # Set rospy to execute a shutdown function when exiting
        rospy.on_shutdown(self.shutdown)

        # variable definition
        rate = 20  # moving friquency ,How fast will we update the robot's movement?
        r = rospy.Rate(
            rate)  # friquency variable,   Set the equivalent ROS rate variable
        # Set the parameters for the target square  ~ private paramters
        goal_distance = rospy.get_param("~goal_distance", 1.0)  # meters
        goal_angle = radians(rospy.get_param(
            "~goal_angle", 90))  # degrees converted to radians
        linear_speed = rospy.get_param("~linear_speed",
                                       0.2)  # meters per second
        angular_speed = rospy.get_param("~angular_speed",
                                        0.7)  # radians per second
        angular_tolerance = radians(rospy.get_param("~angular_tolerance",
                                                    1))  # degrees to radians

        # Creat a Publisher, to control the robot's speed  topic  message_type  buffer_size
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
        self.base_frame = rospy.get_param('~base_frame', '/base_link')
        # The odom frame is usually just /odom
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')
        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()
        # Give tf some time to fill its buffer
        rospy.sleep(2)

        # Set the odom frame
        self.odom_frame = '/odom'  # world-fixed frame

        # Find the reference frame
        # Find out if the robot uses /base_link or /base_footprint
        # /base_footprint frame used by the TurtleBot
        # /base_link frame      used by Pi Robot and Maxwell
        try:
            self.tf_listener.waitForTransform(
                self.odom_frame, '/base_footprint', rospy.Time(),
                rospy.Duration(1.0))  #wait for tramsrform
            self.base_frame = '/base_footprint'  # the robot uses /base_footprint frame
        except (tf.Exception, tf.ConnectivityException,
                tf.LookupException):  # Exception = error state
            try:
                self.tf_listener.waitForTransform(self.odom_frame,
                                                  '/base_link', rospy.Time(),
                                                  rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException,
                    tf.LookupException):
                rospy.loginfo(
                    "Cannot find transform between /odom and /base_link or /base_footprint"
                )
                rospy.signal_shutdown("tf Exception")

        # Initialize the position variable as a Point type(comes from geometry_msgs.msg which has x,y,z)
        position = Point()

        # Cycle through the four sides of the square
        for i in range(4):
            # Straight forward
            # initialize the movement command with linear velocity
            move_cmd = Twist(
            )  # Initialize the movement command to zero , Twist:linear.x linear.y linear.z    Point: x,y,z
            move_cmd.linear.x = linear_speed  # Set the movement command to forward motion with desired speed
            #record the starting position
            (position,
             rotation) = self.get_odom()  # Get the starting position values
            x_start = position.x
            y_start = position.y
            distance = 0  # traveled  distance
            # travel with a linear  speed
            while distance < goal_distance and not rospy.is_shutdown(
            ):  # hasno't reached the desired distance
                self.cmd_vel.publish(
                    move_cmd
                )  # Publish the Twist message velocity comannd  until reached the destination
                r.sleep()
                (position,
                 rotation) = self.get_odom()  # Get the current position
                # Compute the Euclidean distance from the start
                distance = sqrt(
                    pow((position.x -
                         x_start), 2) +  # update the traveled  distance 
                    pow((position.y - y_start), 2))

        # Stop the robot before rotating
        # stop()
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1.0)

            # rotate
            move_cmd.angular.z = angular_speed  # Set the movement command to a rotation
            last_angle = rotation  # starting angle, odometry record  the last angle measured(starting angle)
            turn_angle = 0  # traveled angle
            while abs(turn_angle + angular_tolerance) < abs(
                    goal_angle) and not rospy.is_shutdown(
                    ):  # hasno't reached the desired goal_angle
                self.cmd_vel.publish(
                    move_cmd
                )  # Publish the Twist message velocity comannd angular_speed until reached the desired goal_angle
                r.sleep()  # sleep with rate
                (position,
                 rotation) = self.get_odom()  # Get the current rotation angle
                delta_angle = normalize_angle(
                    rotation - last_angle
                )  # compute the the delt_angel  constraint with in -pi to pi
                turn_angle += delta_angle  # update the traveled turn_angle
                last_angle = rotation  # update the last_angle

        # Stop the robot before the next leg
        # stop()
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1.0)

        # Stop the robot when we are done
        self.cmd_vel.publish(Twist())
Example #13
0
    def __init__(self):
        # Give the node a name
        rospy.init_node('move_robot', anonymous=False)

        # Set rospy to execute a shutdown function when exiting       
        rospy.on_shutdown(self.shutdown)

        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        #self.cmd_vel = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=5)
        
        # How fast will we update the robot's movement?
        rate = 20
        
        # Set the equivalent ROS rate variable
        r = rospy.Rate(rate)
        
        # Set the forward linear speed to 0.15 meters per second 
        linear_speed = 0.15

        # Set the rotation speed in radians per second
        angular_speed = 0.5
        
        # Set the angular tolerance in degrees converted to radians
        angular_tolerance = radians(1.0)
        
        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()
        
        # Give tf some time to fill its buffer
        #rospy.sleep(2)
        
        # Set the odom frame
        self.odom_frame = '/odom'
        
        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")  
        
        # Initialize the position variable as a Point type
        position = Point()
            

        # Following points in path
        for i in range(1, len(path)):
            # Set parameters for current target
            # Initialize the movement command
            move_cmd = Twist()
            
            # Get the starting position values     
            (position, rotation) = self.get_odom()
                        
            x_start = position.x
            y_start = position.y

            # Scale the points in path.txt by 0.01
            goal = Point(path[i][0]*0.01, path[i][1]*0.01, 0)
            goal_distance = sqrt((goal.x-x_start)**2 + (goal.y-y_start)**2)
            goal_angle = np.arctan2(goal.y-y_start, goal.x-x_start)
            goal_angle -= rotation
            if goal_angle < 0:
                goal_angle += 2*pi
            
            # Rotate before moving
            print("Moving towards ("+str(goal.x)+", "+str(goal.y)+")!")                        
            # Set the movement command to a rotation
            move_cmd.angular.z = angular_speed
            
            # Track the last angle measured
            last_angle = rotation
            
            # Track how far we have turned
            turn_angle = 0
            
            while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
                # Publish the Twist message and sleep 1 cycle         
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                
                # Get the current rotation
                (position, rotation) = self.get_odom()
                
                # Compute the amount of rotation since the last loop
                delta_angle = normalize_angle(rotation - last_angle)
                
                # Add to the running total
                turn_angle += delta_angle
                last_angle = rotation

            # Stop the robot before moving
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

            # Set the movement command to forward motion
            move_cmd.linear.x = linear_speed

            # Keep track of the distance traveled
            distance = 0
            
            # Enter the loop to move along a side
            while distance < goal_distance and not rospy.is_shutdown():
                print("Moving towards ("+str(goal.x)+", "+str(goal.y)+")!")

                # Publish the Twist message and sleep 1 cycle         
                self.cmd_vel.publish(move_cmd)          
                r.sleep()
        
                # Get the current position
                (position, rotation) = self.get_odom()

                
                # Compute the Euclidean distance from the start
                distance = sqrt(pow((position.x - x_start), 2) + 
                                pow((position.y - y_start), 2))
                print("Current position: (" + str(position.x) + ", " + str(position.y) + "), " + str(distance) + " away from position ("+str(goal.x)+", "+str(goal.y)+")!")
            
            
            print("Robot reaches ("+str(goal.x)+", "+str(goal.y)+")!")    
            # Stop the robot before the next edge
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
        
        print("Robot reaches final goal!")    
        # Stop the robot for good
        self.cmd_vel.publish(Twist())
Example #14
0
    def __init__(self):
        rospy.init_node('out_and_back', anonymous=True)

        rospy.on_shutdown(self.shutdown)
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist)
        rate = 20
        r = rospy.Rate(rate)
        #linear_speed=0.2
        linear_speed = 0.15
        goal_distance = 1.0
        #angular_speed=1.0
        angular_speed = 0.15
        angular_tolerance = radians(2.5)
        goal_angle = pi
        self.tf_listener = tf.TransformListener()
        rospy.sleep(2)
        self.odom_frame = '/odom'
        try:
            self.tf_listener.waitForTransform(self.odom_frame,
                                              '/base_footprint', rospy.Time(),
                                              rospy.Duration(1, 0))
            self.base_frame = '/base_footprint'
            print(self.base_frame)
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame,
                                                  '/base_link', rospy.Time(),
                                                  rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException,
                    tf.LookupException):
                rospy.loginfo(
                    'Cannot find transform between /odom and /base_link or /base_footprint'
                )
                rospy.signal_shutdown('tf Exception')
        position = Point()
        for i in range(2):
            move_cmd = Twist()
            move_cmd.linear.x = linear_speed
            (position, rotation) = self.get_odom()
            x_start = position.x
            y_start = position.y
            distance = 0
            while distance < goal_distance and not rospy.is_shutdown():
                self.cmd_vel.publish(move_cmd)

                r.sleep()
                (position, rotation) = self.get_odom()
                distance = sqrt(
                    pow((position.x - x_start), 2) +
                    pow((position.x - y_start), 2))
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
            move_cmd.angular.z = angular_speed
            last_angle = rotation
            turn_angle = 0
            while abs(turn_angle + angular_tolerance) < abs(
                    goal_angle) and not rospy.is_shutdown():
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                (position, rotation) = self.get_odom()
                delta_angle = normalize_angle(rotation - last_angle)
                turn_angle += delta_angle
                last_angle = rotation
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
        self.cmd_vel.publish(Twist())
Example #15
0
    def __init__(self):

        # Initializes a Node
        rospy.init_node('odom_out_and_back', anonymous=False)

        rospy.on_shutdown(self.shutdown)

        rospy.loginfo("Robot Started")

        # Initializes a topic to publish velocity of the robot
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist)

        # Rate at which the velocity would be published
        rate = 20

        r = rospy.Rate(rate)

        # Intended Linear Speed of the Robot
        linear_speed = 0.2

        # Distance of the Goal from the Start Poisition
        goal_distance = 5.0

        # Intended Angular Speed of the Robot
        angular_speed = 1.0

        # Angle of the Goal
        goal_angle = pi

        angular_tolerance = radians(2.5)

        self.tflistener = tf.TransformListener()

        rospy.sleep(2)

        self.odom_frame = '/odom'

        # Check whether transform is on /base_link frame or /base_footprint frame
        try:
            self.tflistener.waitForTransform(self.odom_frame,
                                             '/base_footprint', rospy.Time(),
                                             rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tflistener.waitForTransform(self.odom_frame, '/base_link',
                                                 rospy.Time(),
                                                 rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException,
                    tf.LookupException):
                rospy.loginfo("No Base Frame Found")
                rospy.signal_shutdown("tf exception")

        position = Point()

        # Loop to move the robot forward and then back again
        for i in range(2):

            move_cmd = Twist()

            move_cmd.linear.x = linear_speed

            (position, rotation) = self.get_odom()
            rospy.loginfo("here")
            x_start = position.x
            y_start = position.y

            distance = 0

            # Check whether the robot reached its goal
            while (distance < goal_distance) and not rospy.is_shutdown():

                self.cmd_vel.publish(move_cmd)

                r.sleep()

                (position, rotation) = self.get_odom()

                distance = sqrt(
                    pow((position.x - x_start), 2) +
                    pow((position.y - y_start), 2))

            rospy.loginfo("Complete 1st move")
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

            move_cmd.angular.z = angular_speed

            last_angle = rotation

            turn_angle = 0

            # Check whether the robot has turned as much as the goal angle
            while (abs(turn_angle + angular_tolerance) < abs(goal_angle)
                   and not rospy.is_shutdown()):

                self.cmd_vel.publish(move_cmd)

                r.sleep()

                (position, rotation) = self.get_odom()

                delta_angle = normalize_angle(rotation - last_angle)

                turn_angle += delta_angle

                last_angle = rotation

            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
Example #16
0
	def __init__(self):
		rospy.init_node("out_and_back",anonymous = False)

		rospy.on_shutdown(self.shutdown)
		self.cmd_vel = rospy.Publisher("/cmd_vel",Twist,queue_size = 5)
		rate = 20
		
		r = rospy.Rate(rate)
		linear_speed = 0.2
		goal_distance = 1.0
		angular_speed = 1.0
		goal_tolerance = radians(2.5)
		goal_angle = pi

		self.tf_listener = tf.TransformListener()
		rospy.sleep(2)
		self.odom_frame = "/odom"
		
		try:
			self.tf_listener.waitForTransform(self.odom_frame,'/base_footprint',rospy.Time(),rospy.Duration(1.0))
			self.base_frame = "/base_footprint"
		except (tf.Exception,tf.ConnectivityException,tf.LookupException):
			try:
				self.tf_listener.waitFotTransform(self.odom_frame,"/base_link",rospy.Time(),rospy.Duration(1.0))
				self.base_frame = "/base_link"
			except (tf.Exception,tf.ConnectivityException,tf.LoopupException):
				rospy.loginfo("can not find transform between /odom and /base_link and /base_footprint")
				rospy.signal_shutdown("tf Exception")
		position = Point()

		for i in range(2):
			move_cmd = Twist()
			move_cmd.linear.x = linear_speed

			(position,rotation) = self.get_odom(0)
			x_start = position.x
			y_start = position.y
	
			distance = 0
		
			while distance < goal_distance and not rospy.is_shutdown():
				self.cmd_vel.publish(move_cmd)
				r.sleep()
				(position,rotation) = self.get_odom()
				distance = sqrt(pow((position.x-x_start),2)+pow((position.y-y_start),2))
			move_cmd = Twist()
			self.cmd_vel.publish(move_cmd)
			rospy.sleep(1)
			
			move_cmd.angular.z = angular_speed
	    		last_angle = rotation
			turn_angle = 0 
			while abs(turn_angle + angle_tolerance) < abs(angular_goal) and not rospuy.is_shutdown():
				self.cmd_vel.publish(move_cmd)
				r.sleep()
				(position,rotation) = self.get_odom()
				delta_angle = normalize_angle(rotation - last_angle)
				turn_angle += delta_angle
				last_angle = rotation
 				
			move_cmd = Twist()
			self.cmd_vel.publish(move_cmd)
			rospy.sleep(1)
		self.cmd_vel.publish(Twist())
    def __init__(self):
        # Give the node a name
        rospy.init_node('calibrate_angular', anonymous=False)

        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)

        # How fast will we check the odometry values?
        self.rate = rospy.get_param('~rate', 20)
        r = rospy.Rate(self.rate)

        # The test angle is 360 degrees
        self.test_angle = radians(rospy.get_param('~test_angle', 360.0))

        self.speed = rospy.get_param('~speed', 0.5)  # radians per second
        self.tolerance = radians(rospy.get_param(
            'tolerance', 1))  # degrees converted to radians
        self.odom_angular_scale_correction = rospy.get_param(
            '~odom_angular_scale_correction', 1.0)
        self.start_test = rospy.get_param('~start_test', True)

        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

        # Fire up the dynamic_reconfigure server
        dyn_server = Server(CalibrateAngularConfig,
                            self.dynamic_reconfigure_callback)

        # Connect to the dynamic_reconfigure server
        dyn_client = dynamic_reconfigure.client.Client("calibrate_angular",
                                                       timeout=60)

        # The base frame is usually base_link or base_footprint
        self.base_frame = rospy.get_param('~base_frame', '/base_link')

        # The odom frame is usually just /odom
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()

        # Give tf some time to fill its buffer
        rospy.sleep(2)

        # Make sure we see the odom and base frames
        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame,
                                          rospy.Time(), rospy.Duration(60.0))

        rospy.loginfo("Bring up rqt_reconfigure to control the test.")

        reverse = 1

        while not rospy.is_shutdown():
            if self.start_test:
                # Get the current rotation angle from tf
                self.odom_angle = self.get_odom_angle()

                last_angle = self.odom_angle
                turn_angle = 0
                self.test_angle *= reverse
                error = self.test_angle - turn_angle

                # Alternate directions between tests
                reverse = -reverse

                while abs(error) > self.tolerance and self.start_test:
                    if rospy.is_shutdown():
                        return

                    # Rotate the robot to reduce the error
                    move_cmd = Twist()
                    move_cmd.angular.z = copysign(self.speed, error)
                    self.cmd_vel.publish(move_cmd)
                    r.sleep()

                    # Get the current rotation angle from tf
                    self.odom_angle = self.get_odom_angle()

                    # Compute how far we have gone since the last measurement
                    delta_angle = self.odom_angular_scale_correction * normalize_angle(
                        self.odom_angle - last_angle)

                    # Add to our total angle so far
                    turn_angle += delta_angle
                    error = self.test_angle - turn_angle
                    last_angle = self.odom_angle

                # Stop the robot
                self.cmd_vel.publish(Twist())

                # Update the status flag
                self.start_test = False
                params = {'start_test': False}
                dyn_client.update_configuration(params)

            rospy.sleep(0.5)

        # Stop the robot
        self.cmd_vel.publish(Twist())
Example #18
0
    def __init__(self):
        # 给出节点的名字
        rospy.init_node('out_and_back', anonymous=False)

        # 设置rospy在程序退出时执行的关机函数
        rospy.on_shutdown(self.shutdown)

        # Publisher(发布器):发布机器人运动的速度
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

        # 我们将用多快的速度更新控制机器人运动的命令?
        rate = 20

        # 设定相同的值给rospy.Rate()
        r = rospy.Rate(rate)

        # 设定前进的线速度为0.2m/s
        linear_speed = 0.2

        # 设定行程距离为1.0m
        goal_distance = 1.0

        # 设定转动速度1.0rad/s .即大约6秒转一圈
        angular_speed = 1.0

        # Set the angular tolerance in degrees converted to radians
        angular_tolerance = radians(2.5)

        # 设定转动弧度: Pi 弧度 (180 度)
        goal_angle = pi

        # 初始化这个tf监听器
        self.tf_listener = tf.TransformListener()

        # 给tf一些时间让它填补它的缓冲区
        rospy.sleep(2)

        # 设置odom坐标系
        self.odom_frame = '/odom'

        # 询问机器人使用的是/base_link坐标系还是/base_footprint坐标系
        try:
            self.tf_listener.waitForTransform(self.odom_frame,
                                              '/base_footprint', rospy.Time(),
                                              rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame,
                                                  '/base_link', rospy.Time(),
                                                  rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException,
                    tf.LookupException):
                rospy.loginfo(
                    "Cannot find transform between /odom and /base_link or /base_footprint"
                )
                rospy.signal_shutdown("tf Exception")

        # 初始化了一个Point类型的变量
        position = Point()

        # 往返2次行程
        for i in range(2):
            # 初始化运动命令
            move_cmd = Twist()

            # 设定前进速度
            move_cmd.linear.x = linear_speed

            # 得到开始的姿态信息(位置和转角)
            (position, rotation) = self.get_odom()

            x_start = position.x
            y_start = position.y

            # 随时掌控机器人行驶的距离
            distance = 0

            # 进入循环,沿着一边移动
            while distance < goal_distance and not rospy.is_shutdown():
                # 发布一次Twist消息 和 sleep 1秒
                self.cmd_vel.publish(move_cmd)

                r.sleep()

                # 给出正确的姿态信息(位置和转角)
                (position, rotation) = self.get_odom()

                # 计算相对于开始位置的欧几里得距离(即位移)
                distance = sqrt(
                    pow((position.x - x_start), 2) +
                    pow((position.y - y_start), 2))

            # 在转动机器人前,停止它
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

            # 给旋转配置运动命令
            move_cmd.angular.z = angular_speed

            # 跟踪记录最后的角度
            last_angle = rotation

            # 跟踪我们已经转动了多少角度
            turn_angle = 0

            while abs(turn_angle + angular_tolerance) < abs(
                    goal_angle) and not rospy.is_shutdown():
                # 发布一次Twist消息 和 sleep 1秒
                self.cmd_vel.publish(move_cmd)
                r.sleep()

                # 给出正确的姿态信息(位置和转角)
                (position, rotation) = self.get_odom()

                # 计算自每次的旋转量
                delta_angle = normalize_angle(rotation - last_angle)

                # 添加到正在运行的合计里
                turn_angle += delta_angle
                last_angle = rotation

            # 下一航段之前停止机器人
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

        # 为了机器人好,停止它
        self.cmd_vel.publish(Twist())
    def __init__(self):
        # Give the node a name
        rospy.init_node("out_and_back", anonymous=False)

        # Set rospy to execute a shutdown function when exiting
        rospy.on_shutdown(self.shutdown)

        # Publisher to control the robot's speed
        # self.cmd_vel = rospy.Publisher('/cmd_vel', Twist)  #for simulator
        self.cmd_vel = rospy.Publisher("/mobile_base/commands/velocity", Twist)  # FOR TURTLE BOT

        # How fast will we update the robot's movement?
        rate = 20

        # Set the equivalent ROS rate variable
        r = rospy.Rate(rate)

        # Set the forward linear speed to 0.2 meters per second
        linear_speed = 0.2

        # Set the travel distance in meters
        goal_distance = 3.0

        # Set the rotation speed in radians per second
        angular_speed = 1.0

        # Set the angular tolerance in degrees converted to radians
        angular_tolerance = radians(2.5)

        # Set the rotation angle to Pi radians (180 degrees)
        goal_angle = pi

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()

        # Give tf some time to fill its buffer
        rospy.sleep(2)

        # Set the odom frame
        self.odom_frame = "/odom"

        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(self.odom_frame, "/base_footprint", rospy.Time(), rospy.Duration(1.0))
            self.base_frame = "/base_footprint"
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, "/base_link", rospy.Time(), rospy.Duration(1.0))
                self.base_frame = "/base_link"
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")

        # Initialize the position variable as a Point type
        position = Point()

        # Loop once for each leg of the trip
        for i in range(2):
            # Initialize the movement command
            move_cmd = Twist()

            # Set the movement command to forward motion
            move_cmd.linear.x = linear_speed

            # Get the starting position values
            (position, rotation) = self.get_odom()

            x_start = position.x
            y_start = position.y

            # Keep track of the distance traveled
            distance = 0

            # Enter the loop to move along a side
            while distance < goal_distance and not rospy.is_shutdown():
                # Publish the Twist message and sleep 1 cycle
                self.cmd_vel.publish(move_cmd)

                r.sleep()

                # Get the current position
                (position, rotation) = self.get_odom()

                # Compute the Euclidean distance from the start
                distance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2))

            # Stop the robot before the rotation
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

            # Set the movement command to a rotation
            move_cmd.angular.z = angular_speed

            # Track the last angle measured
            last_angle = rotation

            # Track how far we have turned
            turn_angle = 0

            while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
                # Publish the Twist message and sleep 1 cycle
                self.cmd_vel.publish(move_cmd)
                r.sleep()

                # Get the current rotation
                (position, rotation) = self.get_odom()

                # Compute the amount of rotation since the last loop
                delta_angle = normalize_angle(rotation - last_angle)

                # Add to the running total
                turn_angle += delta_angle
                last_angle = rotation

            # Stop the robot before the next leg
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

        # Stop the robot for good
        self.cmd_vel.publish(Twist())
Example #20
0
def __init__(self):

    rospy.init_node('calibration', anonymous=False)   
    rospy.on_shutdown(self.shutdown)

    # Publisher to control robot's speed
    self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

    # Robot movement update speed
    rate = 20
    r = rospy.Rate(rate)

    # Forward linear speed (m/s)
    linear_speed = 0.1
    goal_distance = 1.0

    # Rotation speed (rads/s)
    angular_speed = 1.0
    angular_tolerance = radians(2.5)
    # Rotation angle (360 degrees)
    goal_angle = 2*pi


    self.tf_listener = tf.TransformListener()
    rospy.sleep(2)
    self.odom_frame = '/odom'

    # Look if robot uses /base_link or /base_footprint
    try:
        self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
        self.base_frame = '/base_footprint'
    except (tf.Exception, tf.ConnectivityException, tf.LookupException):
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_link'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
            rospy.signal_shutdown("tf Exception")  


    # Initialize position variable as a Point type
    position = Point()
    # Loop once for each leg of the trip
    for i in range(2):
        # Initialize the movement command
        move_cmd = Twist()
        # Set the movement command to forward motion
        move_cmd.linear.x = linear_speed

        # Get initial position     
        (position, rotation) = self.get_odom()
        x_start = position.x
        y_start = position.y

        # Keep track of the distance traveled
        # distance = 0
        # # Enter the loop to move along a side
        # while distance < goal_distance and not rospy.is_shutdown():
        #     # Publish the Twist message and sleep 1 cycle         
        #     self.cmd_vel.publish(move_cmd)

        #     r.sleep()

        #     # Get the current position
        #     (position, rotation) = self.get_odom()

        #     # Compute the Euclidean distance from the start
        #     distance = sqrt(pow((position.x - x_start), 2) + 
        #                     pow((position.y - y_start), 2))


        ### ROTATION
        # Stop the robot before the rotation
        move_cmd = Twist()
        self.cmd_vel.publish(move_cmd)
        rospy.sleep(1)

        # Set the movement command to a rotation
        move_cmd.angular.z = angular_speed
        # Track the last angle measured
        last_angle = rotation
        # Track how far we have turned
        turn_angle = 0

        while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
            # Publish the Twist message and sleep 1 cycle         
            self.cmd_vel.publish(move_cmd)
            r.sleep()
            # Get current rotation
            (position, rotation) = self.get_odom()
            delta_angle = normalize_angle(rotation - last_angle)

            turn_angle += delta_angle
            last_angle = rotation

        # Stop the robot before the next leg
        move_cmd = Twist()
        self.cmd_vel.publish(move_cmd)
        rospy.sleep(1)

    # Stop the robot
    self.cmd_vel.publish(Twist())
    def __init__(self):
        # Give the node a name
        rospy.init_node('calibrate_angular', anonymous=False)
        
        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)
        
        # How fast will we check the odometry values?
        self.rate = 30
        r = rospy.Rate(self.rate)
        
        # The test angle is 360 degrees
        self.test_angle = radians(rospy.get_param('~test_angle', 360.0))

        self.speed = rospy.get_param('~speed', 0.7) # radians per second
        self.tolerance = rospy.get_param('tolerance', radians(5)) # degrees converted to radians
        self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0)
        self.start_test = rospy.get_param('~start_test', True)
        
        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=10)
        
        # Fire up the dynamic_reconfigure server
        dyn_server = Server(CalibrateAngularConfig, self.dynamic_reconfigure_callback)
        
        # Connect to the dynamic_reconfigure server
        dyn_client = dynamic_reconfigure.client.Client("calibrate_angular", timeout=60)
        
        # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
        self.base_frame = rospy.get_param('~base_frame', '/base_link')

        # The odom frame is usually just /odom
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()
        
        # Give tf some time to fill its buffer
        rospy.sleep(2)
        
        # Make sure we see the odom and base frames
        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))
            
        rospy.loginfo("Bring up rqt_reconfigure to control the test.")
        
        reverse = 1
        
        while not rospy.is_shutdown():
            # Execute the rotation

            if self.start_test:
                # Get the current rotation angle from tf
                self.odom_angle = self.get_odom_angle()
                
                last_angle = self.odom_angle
                turn_angle = 0
                
                # Alternate directions between tests
                reverse = -reverse
                angular_speed  = reverse * self.speed
                
                while abs(turn_angle) < abs(self.test_angle):
                    if rospy.is_shutdown():
                        return
                    move_cmd = Twist()
                    move_cmd.angular.z = angular_speed
                    self.cmd_vel.publish(move_cmd)
                    r.sleep()
                 
                    # Get the current rotation angle from tf                   
                    self.odom_angle = self.get_odom_angle()
                    
                    # Compute how far we have gone since the last measurement
                    delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle)
                    
                    # Add to our total angle so far
                    turn_angle += delta_angle
                    last_angle = self.odom_angle
                
                # Stop the robot
                self.cmd_vel.publish(Twist())
                
                # Update the status flag
                self.start_test = False
                params = {'start_test': False}
                dyn_client.update_configuration(params)
                
            rospy.sleep(0.5) 
                    
        # Stop the robot
        self.cmd_vel.publish(Twist())
Example #22
0
    def __init__(self):
        # Creat a node named out_and_back
        rospy.init_node('out_and_back', anonymous=False)
        # Set rospy to execute a shutdown function when exiting
        rospy.on_shutdown(self.shutdown)
        # Creat a Publisher, to control the robot's speed  topic  message_type  buffer_size
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

        # variable definition
        rate = 30  # moving friquency ,How fast will we update the robot's movement?
        r = rospy.Rate(
            rate)  # friquency variable,   Set the equivalent ROS rate variable
        linear_speed = 0.15  # forward linear speed, Set the forward linear speed to  0.15 m/s
        goal_distance = 2.0  # travelled distance,   Set the travel distance in meters
        angular_speed = 0.5  # rotation speed,       Set the rotation speed in radians per second
        angular_tolerance = radians(
            2.5)  # Set the angular tolerance in degrees converted to radians
        goal_angle = pi  # rotation angle,       Set the rotation angle to Pi radians (180 degrees)

        # Creat a tf listener
        self.tf_listener = tf.TransformListener()
        # let ROS rest for 2 seconds,so can give tf some time to fill its buffer
        rospy.sleep(2)
        self.odom_frame = '/odom'  # Set the robot's odometery frame   # world-fixed frame

        # Find the reference frame
        # Find out if the robot uses /base_link or /base_footprint
        # /base_footprint frame used by the TurtleBot
        # /base_link frame      used by Pi Robot and Maxwell
        try:
            self.tf_listener.waitForTransform(
                self.odom_frame, '/base_footprint', rospy.Time(),
                rospy.Duration(1.0))  #wait for tramsrform
            self.base_frame = '/base_footprint'  # the robot uses /base_footprint frame
        except (tf.Exception, tf.ConnectivityException,
                tf.LookupException):  # Exception = error state
            try:
                self.tf_listener.waitForTransform(self.odom_frame,
                                                  '/base_link', rospy.Time(),
                                                  rospy.Duration(1.0))
                self.base_frame = '/base_link'  # the robot uses /base_footprint frame
            except (tf.Exception, tf.ConnectivityException,
                    tf.LookupException):
                rospy.loginfo(
                    "Cannot find transform between /odom and /base_link or /base_footprint"
                )
                rospy.signal_shutdown("tf Exception")

        # Initialize the position variable as a Point type(comes from geometry_msgs.msg which has x,y,z)
        position = Point()

        # Loop once for each leg of the trip, back and forth
        for i in range(2):
            # Straight forward
            # initialize the movement command with linear velocity
            move_cmd = Twist(
            )  # Initialize the movement command to zero , Twist:linear.x linear.y linear.z    Point: x,y,z
            move_cmd.linear.x = linear_speed  # Set the movement command to forward motion with desired speed
            #record the starting position(loop once and loop twice were different)
            (position,
             rotation) = self.get_odom()  # Get the starting position values
            x_start = position.x
            y_start = position.y
            distance = 0  # traveled  distance

            # travel with a linear  speed
            while distance < goal_distance and not rospy.is_shutdown(
            ):  # hasno't reached the desired distance
                self.cmd_vel.publish(
                    move_cmd
                )  # Publish the Twist message velocity comannd  until reached the destination
                r.sleep()
                (position,
                 rotation) = self.get_odom()  # Get the current position
                # Compute the Euclidean distance from the start
                distance = sqrt(
                    pow((position.x -
                         x_start), 2) +  # update the traveled  distance 
                    pow((position.y - y_start), 2))

            # Stop the robot before the rotation
            #stop()
            move_cmd = Twist()  # Initialize the movement command  to zero
            self.cmd_vel.publish(move_cmd)  # publish the stop movement command
            rospy.sleep(1)  # sleeping

            # rotate
            move_cmd.angular.z = angular_speed  # Set the movement command to a rotation
            last_angle = rotation  # starting angle, odometry record  the last angle measured(starting angle)
            turn_angle = 0  # traveled angle

            while abs(turn_angle + angular_tolerance) < abs(
                    goal_angle) and not rospy.is_shutdown(
                    ):  # hasno't reached the desired goal_angle
                self.cmd_vel.publish(
                    move_cmd
                )  # Publish the Twist message velocity comannd angular_speed until reached the desired goal_angle
                r.sleep()  # sleep with rate
                (position,
                 rotation) = self.get_odom()  # Get the current rotation angle
                delta_angle = normalize_angle(
                    rotation - last_angle
                )  # compute the the delt_angel  constraint with in -pi to pi
                turn_angle += delta_angle  # update the traveled turn_angle
                last_angle = rotation  # update the last_angle

            # Stop the robot before the next leg
            #stop()
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

        # Stop the robot for good
        self.cmd_vel.publish(Twist())