Пример #1
0
def pub_vel(v, dtheta):
    ss = v * 2
    mm = dtheta
    tr = (ss + mm) / 2
    tl = (ss - mm) / 2
    wcv = WheelCmdVel()
    wcv.desiredWV_R = tr
    wcv.desiredWV_L = tl
    velcmd_pub.publish(wcv)
Пример #2
0
 def move(self):
     #msg=RacecarDriveStamped(drive=RacecarDrive())
     #msg_debug=Float64MultiArray()
     if self.rotation:
         self.rot()
         return
     wcv = WheelCmdVel()
     self.vel_desired = 0.1
     debug = None
     self.stop_margin = 0.05
     if self.laser_data:
         alpha, distance, debug = find_direction(self.laser_data,
                                                 margin=0.3)
         alpha_rot, distance_rot, debug_rot = find_direction_rot(
             self.laser_data, margin=0.4)
         if distance <= self.stop_margin and distance_rot <= self.stop_margin:
             desiredWV_R, desiredWV_L = (0, 0)
         elif distance >= distance_rot / 5. or abs(
                 alpha_rot) <= 5. / 180. * np.pi:
             desiredWV_R, desiredWV_L = alpha_to_w(alpha, self.vel_desired)
         else:
             self.rotation = True
             self.rot_rad_left = alpha_rot
             self.rot()
             return
         wcv.desiredWV_R = desiredWV_R
         wcv.desiredWV_L = desiredWV_L
         self.velcmd_pub.publish(wcv)
         #servo=0.78
         #servo=servo
         #speed=0.0
         #msg_debug.data=debug
     else:
         # servo=0
         # speed=0
         wcv.desiredWV_R = 0.0
         wcv.desiredWV_L = 0.0
         self.velcmd_pub.publish(wcv)
     if self.laser_data:
         print alpha
         print desiredWV_R, desiredWV_L
         print debug
     return
Пример #3
0
def constant_vel_loop():
    velcmd_pub = rospy.Publisher('/cmdvel', WheelCmdVel, queue_size=1)
    rate = rospy.Rate(100)  # 100hz
    wcv = WheelCmdVel()
    while not rospy.is_shutdown():
        wcv.desiredWV_R = 0.1
        wcv.desiredWV_L = 0.2

        velcmd_pub.publish(wcv)

        rate.sleep()
Пример #4
0
 def rot(self):
     dt = 0.1
     wcv = WheelCmdVel()
     if self.rot_rad_left < 0:
         desiredWV_R = 0.1
         desiredWV_L = -0.1
     else:
         desiredWV_R = -0.1
         desiredWV_L = 0.1
     self.rot_rad_left -= np.sign(self.rot_rad_left) * 0.1 / robot_b * 0.1
     if abs(self.rot_rad_left) < 0.1 / robot_b * dt:
         self.rotation = False
         self.rot_rad_left = 0
     wcv.desiredWV_R = desiredWV_R
     wcv.desiredWV_L = desiredWV_L
     self.velcmd_pub.publish(wcv)
Пример #5
0
def constant_vel_loop():
    velcmd_pub = rospy.Publisher('/cmdvel', WheelCmdVel, queue_size=1)
    rate = rospy.Rate(100)  # 100hz

    while not rospy.is_shutdown():
        # lab 3 Task2.2
        # last modified at Oct15,2017
        # create a WheelCmdVel msg
        wcv = WheelCmdVel()
        wcv.desiredWV_R = 0.1
        wcv.desiredWV_L = 0.2
        velcmd_pub.publish(wcv)
        ##wcv = WheelCmdVel()
        ##wcv.desiredWV_R = ???
        ##wcv.desiredWV_L = ???
        ##velcmd_pub.publish(???)
        rate.sleep()
Пример #6
0
def dwa_distance(dis, vel_desired=0.3, angle_list=None, stop_margin=0.01):
    encoder0 = state.encoder_distance
    while True:
        if state.laser_data:
            break
    while True:
        print state.encoder_distance - encoder0
        if state.encoder_distance - encoder0 >= dis:
            pub_vel(0, 0)
            break

        alpha, distance, debug = find_direction(state.laser_data,
                                                angle_list=angle_list,
                                                margin=0.25)
        #alpha_rot,distance_rot,debug_rot=find_direction_rot(self.laser_data,margin=0.4)
        if distance <= stop_margin:
            pub_vel(0, 0)
            #break
        wcv = WheelCmdVel()
        desiredWV_R, desiredWV_L = alpha_to_w(alpha, vel_desired)
        wcv.desiredWV_R = desiredWV_R
        wcv.desiredWV_L = desiredWV_L
        velcmd_pub.publish(wcv)
        rospy.sleep(0.1)
Пример #7
0
def navi_loop():
    velcmd_pub = rospy.Publisher("/cmdvel", WheelCmdVel, queue_size=1)
    target_pose2d = [0.25, 0, np.pi]
    rate = rospy.Rate(100)  # 100hz

    wcv = WheelCmdVel()

    arrived = False
    arrived_position = False

    while not rospy.is_shutdown():
        # 1. get robot pose
        robot_pose3d = lookupTransform(lr, '/map', '/robot_base')

        if robot_pose3d is None:
            print '1. Tag not in view, Stop'
            wcv.desiredWV_R = 0  # right, left
            wcv.desiredWV_L = 0
            velcmd_pub.publish(wcv)
            continue

        robot_position2d = robot_pose3d[0:2]
        target_position2d = target_pose2d[0:2]

        robot_yaw = tfm.euler_from_quaternion(robot_pose3d[3:7])[2]
        robot_pose2d = robot_position2d + [robot_yaw]

        # 2. navigation policy
        # 2.1 if       in the target, stop
        # 2.2 else if  close to target position, turn to the target orientation
        # 2.3 else if  in the correct heading, go straight to the target position,
        # 2.4 else     turn in the direction of the target position

        pos_delta = np.array(target_position2d) - np.array(robot_position2d)
        robot_heading_vec = np.array([np.cos(robot_yaw), np.sin(robot_yaw)])
        heading_err_cross = cross2d(robot_heading_vec,
                                    pos_delta / np.linalg.norm(pos_delta))

        # print 'robot_position2d', robot_position2d, 'target_position2d', target_position2d
        # print 'pos_delta', pos_delta
        # print 'robot_yaw', robot_yaw
        # print 'norm delta', np.linalg.norm( pos_delta ), 'diffrad', diffrad(robot_yaw, target_pose2d[2])
        # print 'heading_err_cross', heading_err_cross

        if arrived or (np.linalg.norm(pos_delta) < 0.08 and
                       np.fabs(diffrad(robot_yaw, target_pose2d[2])) < 0.05):
            print 'Case 2.1  Stop'
            wcv.desiredWV_R = 0
            wcv.desiredWV_L = 0
            arrived = True
        elif np.linalg.norm(pos_delta) < 0.08:
            arrived_position = True
            if diffrad(robot_yaw, target_pose2d[2]) > 0:
                print 'Case 2.2.1  Turn right slowly'
                wcv.desiredWV_R = -0.05
                wcv.desiredWV_L = 0.05
            else:
                print 'Case 2.2.2  Turn left slowly'
                wcv.desiredWV_R = 0.05
                wcv.desiredWV_L = -0.05

        elif arrived_position or np.fabs(heading_err_cross) < 0.2:
            print 'Case 2.3  Straight forward'
            wcv.desiredWV_R = 0.1
            wcv.desiredWV_L = 0.1
        else:
            if heading_err_cross < 0:
                print 'Case 2.4.1  Turn right'
                wcv.desiredWV_R = -0.1
                wcv.desiredWV_L = 0.1
            else:
                print 'Case 2.4.2  Turn left'
                wcv.desiredWV_R = 0.1
                wcv.desiredWV_L = -0.1

        velcmd_pub.publish(wcv)

        rate.sleep()
Пример #8
0
    def navi_loop(self):

        generic_target_pose2d = [0.4, 0, np.pi]

        #target_pose2d = [0.25, 0, np.pi]

        rate = rospy.Rate(100)  # 100hz

        wcv = WheelCmdVel()
        arrived = False
        arrived_position = False
        self.print_id = -1
        while not rospy.is_shutdown():
            if self.idx >= 10:
                break
            # 1. get robot pose
            robot_pose3d = lookupTransform(self.lr, '/map', '/robot_base')

            target_pose2d = generic_target_pose2d
            if self.idx == 6:
                print 'idx =', self.idx
                if self.idx6cnt < 4:
                    wcv.desiredWV_R = 0.1
                    wcv.desiredWV_L = 0.1
                    self.idx6cnt = self.idx6cnt + 1
                    self.velcmd_pub.publish(wcv)
                else:
                    self.idx = self.idx + 1
                    rospy.sleep(3.2)

            elif self.idx == 7:
                # close gripper

                self.idx = self.idx + 1
                rospy.sleep(5)

            elif self.idx == 8:
                print 'idx =', self.idx
                if self.idx6cnt > 0:
                    wcv.desiredWV_R = -0.1
                    wcv.desiredWV_L = -0.1
                    self.idx6cnt = self.idx6cnt - 1
                    self.velcmd_pub.publish(wcv)
                else:
                    self.idx = self.idx + 1
                    rospy.sleep(3.2)

            elif self.idx == 9:
                wcv.desiredWV_R = 0
                wcv.desiredWV_L = 0
                self.idx = self.idx + 1
                self.velcmd_pub.publish(wcv)

            elif self.idx == 5 and self.detection_id == 3:
                if robot_pose3d is None:
                    print_id = 0
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print '1. Tag not in view, Stop'
                    wcv.desiredWV_R = 0  # right, left
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rate.sleep()
                    continue

                robot_position2d = robot_pose3d[0:2]
                target_position2d = target_pose2d[0:2]

                robot_yaw = tfm.euler_from_quaternion(robot_pose3d[3:7])[2]
                robot_pose2d = robot_position2d + [robot_yaw]

                # 2. navigation policy
                # 2.1 if       in the target, stop
                # 2.2 else if  close to target position, turn to the target orientation
                # 2.3 else if  in the correct heading, go straight to the target position,
                # 2.4 else     turn in the direction of the target position

                pos_delta = np.array(target_position2d) - np.array(
                    robot_position2d)
                robot_heading_vec = np.array(
                    [np.cos(robot_yaw), np.sin(robot_yaw)])
                heading_err_cross = cross2d(
                    robot_heading_vec, pos_delta / np.linalg.norm(pos_delta))

                # print 'robot_position2d', robot_position2d, 'target_position2d', target_position2d
                # print 'pos_delta', pos_delta
                # print 'robot_yaw', robot_yaw
                # print 'norm delta', np.linalg.norm( pos_delta ), 'diffrad', diffrad(robot_yaw, target_pose2d[2])
                # print 'heading_err_cross', heading_err_cross

                if arrived or (np.linalg.norm(pos_delta) < 0.08 and np.fabs(
                        diffrad(robot_yaw, target_pose2d[2])) < 0.05):
                    print_id = 21
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Case 2.1  Stop'
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    arrived = True
                elif np.linalg.norm(pos_delta) < 0.08:
                    arrived_position = True
                    if diffrad(robot_yaw, target_pose2d[2]) > 0:
                        print_id = 221
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.2.1  Turn right slowly'
                        wcv.desiredWV_R = -0.05
                        wcv.desiredWV_L = 0.05
                    else:
                        print_id = 222
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.2.2  Turn left slowly'
                        wcv.desiredWV_R = 0.05
                        wcv.desiredWV_L = -0.05
                elif arrived_position or np.fabs(heading_err_cross) < 0.2:
                    print_id = 23
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Case 2.3  Straight forward'
                    wcv.desiredWV_R = 0.1
                    wcv.desiredWV_L = 0.1
                else:
                    if heading_err_cross < 0:
                        print_id = 241
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.4.1  Turn right'
                        wcv.desiredWV_R = -0.1
                        wcv.desiredWV_L = 0.1
                    else:
                        print_id = 242
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.4.2  Turn left'
                        wcv.desiredWV_R = 0.1
                        wcv.desiredWV_L = -0.1

                self.velcmd_pub.publish(wcv)

                if arrived:
                    print 'old idx', self.idx, 'new idx', self.idx + 1
                    self.idx = self.idx + 1
                    arrived = False
                    arrived_position = False
                    rospy.sleep(1)
            rate.sleep()
Пример #9
0
    def navi_loop(self):
        
            
        generic_target_pose2d = [0.4, 0, np.pi]
                    
        #target_pose2d = [0.25, 0, np.pi]
                
        rate = rospy.Rate(100) # 100hz
        
        wcv = WheelCmdVel()
        wcv.desiredGripperPos = gripperOpen
        
        arrived = False
        arrived_position = False
        self.print_id = -1
        while not rospy.is_shutdown() :
            if self.idx >= 10:
                break
            # get robot pose
            robot_pose3d = lookupTransform(self.lr, '/map', '/robot_base')
            
            target_pose2d = generic_target_pose2d
            
            
            #7 close gripper
            if self.idx == 7:
                rospy.sleep(10)

                wcv.desiredGripperPos = gripperClose
                self.velcmd_pub.publish(wcv)  
                self.idx = self.idx + 1
            
            #8    
            elif self.idx == 8:
                # retreat
                print 'idx =', self.idx
                wcv.desiredWV_R = -0.1
                wcv.desiredWV_L = -0.1
                self.velcmd_pub.publish(wcv) 
                rospy.sleep(5.5)
                wcv.desiredWV_R = -0.3
                wcv.desiredWV_L = -0.1
                self.velcmd_pub.publish(wcv) 
                rospy.sleep(4.5) 
                wcv.desiredWV_R = 0.1
                wcv.desiredWV_L = 0.15
                self.velcmd_pub.publish(wcv) 
                rospy.sleep(1) 
                wcv.desiredWV_R = 0
                wcv.desiredWV_L = 0
                self.velcmd_pub.publish(wcv) 
                self.idx = self.idx + 1

            elif self.idx == 9:
                # go straight
                wcv.desiredWV_R = 0.1
                wcv.desiredWV_L = 0.1
                self.velcmd_pub.publish(wcv) 
                self.idx = self.idx + 1
                rospy.sleep(18)
                wcv.desiredWV_R = 0
                wcv.desiredWV_L = 0
                self.velcmd_pub.publish(wcv) 


            rate.sleep()
Пример #10
0
    def navi_loop_0(self):
        first_turn_point_id6 = [0.95, 0.28, -3.00] # apriltag id = 6
        stop_first_turn_lo = [2.38, 0.84, -2.75] # apriltag id = 4
        stop_first_turn_up = [2.30, 0.86, -3.03] # apriltag id = 4
        generic_target_pose2d = [0.4, 0, np.pi]
                    
        #target_pose2d = [0.25, 0, np.pi]
                
        rate = rospy.Rate(100) # 100hz
        
        wcv = WheelCmdVel()
        arrived = False
        arrived_position = False
        self.print_id = -1
        while not rospy.is_shutdown() :
            print 'self.idx',self.idx
            if self.idx >= 6:
                break 
                
            # 0. set target
            if self.idx == 1:
                target_pose2d = first_turn_point_id6
            elif self.idx == 2:
                target_pose2d = stop_first_turn_lo
            else:
                target_pose2d = generic_target_pose2d
            
            # 1. get robot pose
            robot_pose3d = lookupTransform(self.lr, '/map', '/robot_base')


            if self.idx == 1 and self.detection_id == 6:
                if robot_pose3d is None:
                    print_id = 0
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print '1. Tag not in view, Stop'
                    wcv.desiredWV_R = 0  # right, left
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)  
                    rate.sleep()
                    continue
                robot_position2d  = robot_pose3d[0:2]
                target_position2d = target_pose2d[0:2]
                
                robot_yaw    = tfm.euler_from_quaternion(robot_pose3d[3:7]) [2]
                robot_pose2d = robot_position2d + [robot_yaw]

                if robot_pose2d[0] > target_pose2d[0]:
                    wcv.desiredWV_R = 0.1
                    wcv.desiredWV_L = 0.1
                else:
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    arrived = True

                self.velcmd_pub.publish(wcv)  

                if arrived:
                    self.idx = self.idx + 1
                    arrived = False
                    arrived_position = False
                    rate.sleep()
                    

            elif (self.idx == 3 and self.detection_id == 4) or (self.idx == 5 and self.detection_id == 3):
                if robot_pose3d is None:
                    print_id = 0
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print '1. Tag not in view, Stop'
                    wcv.desiredWV_R = 0  # right, left
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)  
                    rate.sleep()
                    continue
                
                robot_position2d  = robot_pose3d[0:2]
                target_position2d = target_pose2d[0:2]
                
                robot_yaw    = tfm.euler_from_quaternion(robot_pose3d[3:7]) [2]
                robot_pose2d = robot_position2d + [robot_yaw]
                
                # 2. navigation policy
                # 2.1 if       in the target, stop
                # 2.2 else if  close to target position, turn to the target orientation
                # 2.3 else if  in the correct heading, go straight to the target position,
                # 2.4 else     turn in the direction of the target position
                
                pos_delta         = np.array(target_position2d) - np.array(robot_position2d)
                robot_heading_vec = np.array([np.cos(robot_yaw), np.sin(robot_yaw)])
                heading_err_cross = cross2d( robot_heading_vec, pos_delta / np.linalg.norm(pos_delta) )
                
                # print 'robot_position2d', robot_position2d, 'target_position2d', target_position2d
                # print 'pos_delta', pos_delta
                # print 'robot_yaw', robot_yaw
                # print 'norm delta', np.linalg.norm( pos_delta ), 'diffrad', diffrad(robot_yaw, target_pose2d[2])
                # print 'heading_err_cross', heading_err_cross
            
                if arrived or (np.linalg.norm( pos_delta ) < 0.08 and np.fabs(diffrad(robot_yaw, target_pose2d[2]))<0.05) :
                    print_id = 21
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Case 2.1  Stop'
                    wcv.desiredWV_R = 0  
                    wcv.desiredWV_L = 0
                    arrived = True
                elif np.linalg.norm( pos_delta ) < 0.08:
                    arrived_position = True
                    if diffrad(robot_yaw, target_pose2d[2]) > 0:
                        print_id = 221
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.2.1  Turn right slowly'      
                        wcv.desiredWV_R = -0.05 
                        wcv.desiredWV_L = 0.05
                    else:
                        print_id = 222
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.2.2  Turn left slowly'
                        wcv.desiredWV_R = 0.05  
                        wcv.desiredWV_L = -0.05
                elif arrived_position or np.fabs( heading_err_cross ) < 0.2:
                    print_id = 23
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Case 2.3  Straight forward'  
                    wcv.desiredWV_R = 0.1
                    wcv.desiredWV_L = 0.1
                else:
                    if heading_err_cross < 0:
                        print_id = 241
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.4.1  Turn right'
                        wcv.desiredWV_R = -0.1
                        wcv.desiredWV_L = 0.1
                    else:
                        print_id = 242
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.4.2  Turn left'
                        wcv.desiredWV_R = 0.1
                        wcv.desiredWV_L = -0.1
                        
                self.velcmd_pub.publish(wcv)  
                
                if arrived:
                    print 'old idx', self.idx, 'new idx', self.idx + 1
                    self.idx = self.idx + 1
                    arrived = False
                    arrived_position = False
                    rate.sleep()
                    rate.sleep()
                    

                
                
            elif self.idx == 2:
                if robot_pose3d is not None:
                    robot_position2d  = robot_pose3d[0:2]
                    target_position2d = target_pose2d[0:2]
                    
                    robot_yaw    = tfm.euler_from_quaternion(robot_pose3d[3:7]) [2]
                    robot_pose2d = robot_position2d + [robot_yaw]
                if robot_pose3d is None or robot_yaw > stop_first_turn_lo[2] or self.detection_id != 4:
                    print_id = 2001
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Turn right slowly'
                    wcv.desiredWV_R = -0.05 
                    wcv.desiredWV_L = 0.05
                    '''
                elif robot_yaw < stop_first_turn_up[2]:
                    print_id = 2002
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Turn left slowly'
                    wcv.desiredWV_R = 0.05 
                    wcv.desiredWV_L = -0.05
                    '''
                else:
                    print_id = 2003
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'idx2 finished'
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    arrived = True
                    
                self.velcmd_pub.publish(wcv)  
                if arrived:
                    self.idx = self.idx + 1
                    arrived = False
                    arrived_position = False
                    rate.sleep()
            
            elif self.idx == 4:
                if self.detection_id == 3:
                    print_id = 4002
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'idx4 finished'
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    arrived = True
                else:
                    print_id = 4001
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Turn left slowly'
                    wcv.desiredWV_R = 0.05 
                    wcv.desiredWV_L = -0.05

                self.velcmd_pub.publish(wcv)  

                if arrived:
                    self.idx = self.idx + 1
                    arrived = False
                    arrived_position = False
                    rate.sleep()
                
            rate.sleep()
Пример #11
0
def cmd_vel_loop():
    velcmd_pub = rospy.Publisher('/cmdvel', WheelCmdVel, queue_size=1)
    rate = rospy.Rate(100)  # 100hz

    while not rospy.is_shutdown():
        wcv = WheelCmdVel()
        cmd = raw_input()

        #cmd='w'
        #print 'test'
        if cmd == 'w':
            # self.run_dir=1.
            # self.turn_direction=0
            # self.turn_cd=0
            wcv.desiredWV_R = 0.1
            wcv.desiredWV_L = 0.1
            #print 'haha'
        elif cmd == 'a':
            # self.turn_direction=-1
            # self.turn_cd=4
            wcv.desiredWV_R = 0.1
            wcv.desiredWV_L = -0.1

        elif cmd == 'd':
            # self.turn_direction=1
            # self.turn_cd=4
            wcv.desiredWV_R = -0.1
            wcv.desiredWV_L = 0.1

        elif cmd == 's':
            #self.run_dir=-1.
            wcv.desiredWV_R = -0.1
            wcv.desiredWV_L = -0.1

        else:
            wcv.desiredWV_R = 0.
            wcv.desiredWV_L = 0.
            self.run_dir = 0.

        # if self.turn_cd>0:
        #     if self.turn_direction==-1:
        #         wcv.desiredWV_R = 0.2*self.run_dir
        #         wcv.desiredWV_L = 0.1*self.run_dir
        #     elif self.turn_direction==1:
        #         wcv.desiredWV_R = 0.1*self.run_dir
        #         wcv.desiredWV_L = 0.2*self.run_dir
        #     else:
        #         wcv.desiredWV_R = 0.
        #         wcv.desiredWV_L = 0.
        #     self.turn_cd-=1
        # elif self.run_dir>0:
        #     wcv.desiredWV_R = 0.1
        #     wcv.desiredWV_L = 0.1
        # elif self.run_dir<0:
        #     wcv.desiredWV_R = 0.
        #     wcv.desiredWV_L = 0.
        # else:
        #     wcv.desiredWV_R = -0.1
        #     wcv.desiredWV_L = -0.1

        print wcv
        velcmd_pub.publish(wcv)

        rate.sleep()
Пример #12
0
    def navi_loop(self):
        first_turn_point_id6_route0 = [0.95, 0.28, -3.00]  # apriltag id = 6
        first_turn_point_id6_route1 = [0.38, 0.09, 3.076]  # apriltag id = 6
        stop_first_turn_lo_route0 = [2.38, 0.84, -2.75]  # apriltag id = 4
        stop_first_turn_up_route0 = [2.30, 0.86, -3.03]  # apriltag id = 4
        stop_first_turn_lo_route1 = [2.14, -0.45, 2.95]  # apriltag id = 4
        stop_first_turn_up_route1 = [2.01, 0.34, 3.10]  # apriltag id = 4
        generic_target_pose2d = [0.4, 0, np.pi]

        #target_pose2d = [0.25, 0, np.pi]

        rate = rospy.Rate(100)  # 100hz

        wcv = WheelCmdVel()
        arrived = False
        arrived_position = False
        self.print_id = -1

        self.idx6cnt = 0
        while not rospy.is_shutdown():
            # print 'self.idx',self.idx
            if self.idx >= 7:
                break
            if self.idx <= 0:  # haven't found obstacle
                rospy.sleep(2)
                continue

            # 0. set target
            if self.idx == 1:
                if self.route_idx == 0:
                    target_pose2d = first_turn_point_id6_route0
                else:
                    target_pose2d = first_turn_point_id6_route1
            elif self.idx == 2:
                target_pose2d = stop_first_turn_lo_route0
            else:
                target_pose2d = generic_target_pose2d

            # 1. get robot pose
            robot_pose3d = lookupTransform(self.lr, '/map', '/robot_base')

            if self.idx == 6:
                if self.idx6cnt < 4:
                    wcv.desiredWV_R = 0.1
                    wcv.desiredWV_L = 0.1
                    self.idx6cnt = self.idx6cnt + 1
                else:
                    self.idx = self.idx + 1

            # idx == 1, start from initial position
            if self.idx == 1 and self.detection_id == 6:
                if robot_pose3d is None:
                    print_id = 0
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print '1. Tag not in view, Stop'
                    wcv.desiredWV_R = 0  # right, left
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rate.sleep()
                    continue
                robot_position2d = robot_pose3d[0:2]
                target_position2d = target_pose2d[0:2]

                robot_yaw = tfm.euler_from_quaternion(robot_pose3d[3:7])[2]
                robot_pose2d = robot_position2d + [robot_yaw]

                if robot_pose2d[0] > target_pose2d[0]:
                    wcv.desiredWV_R = 0.1
                    wcv.desiredWV_L = 0.1
                else:
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    arrived = True

                self.velcmd_pub.publish(wcv)

                if arrived:
                    self.idx = self.idx + 1
                    arrived = False
                    arrived_position = False
                    rospy.sleep(1)

            # idx == 3 or 5, go to human or target
            elif (self.idx == 3
                  and self.detection_id == 4) or (self.idx == 5
                                                  and self.detection_id == 3):
                if robot_pose3d is None:
                    print_id = 0
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print '1. Tag not in view, Stop'
                    wcv.desiredWV_R = 0  # right, left
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rate.sleep()
                    continue

                robot_position2d = robot_pose3d[0:2]
                target_position2d = target_pose2d[0:2]

                robot_yaw = tfm.euler_from_quaternion(robot_pose3d[3:7])[2]
                robot_pose2d = robot_position2d + [robot_yaw]

                # 2. navigation policy
                # 2.1 if       in the target, stop
                # 2.2 else if  close to target position, turn to the target orientation
                # 2.3 else if  in the correct heading, go straight to the target position,
                # 2.4 else     turn in the direction of the target position

                pos_delta = np.array(target_position2d) - np.array(
                    robot_position2d)
                robot_heading_vec = np.array(
                    [np.cos(robot_yaw), np.sin(robot_yaw)])
                heading_err_cross = cross2d(
                    robot_heading_vec, pos_delta / np.linalg.norm(pos_delta))

                # print 'robot_position2d', robot_position2d, 'target_position2d', target_position2d
                # print 'pos_delta', pos_delta
                # print 'robot_yaw', robot_yaw
                # print 'norm delta', np.linalg.norm( pos_delta ), 'diffrad', diffrad(robot_yaw, target_pose2d[2])
                # print 'heading_err_cross', heading_err_cross

                if arrived or (np.linalg.norm(pos_delta) < 0.08 and np.fabs(
                        diffrad(robot_yaw, target_pose2d[2])) < 0.05):
                    print_id = 21
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Case 2.1  Stop'
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    arrived = True
                elif np.linalg.norm(pos_delta) < 0.08:
                    arrived_position = True
                    if diffrad(robot_yaw, target_pose2d[2]) > 0:
                        print_id = 221
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.2.1  Turn right slowly'
                        wcv.desiredWV_R = -0.05
                        wcv.desiredWV_L = 0.05
                    else:
                        print_id = 222
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.2.2  Turn left slowly'
                        wcv.desiredWV_R = 0.05
                        wcv.desiredWV_L = -0.05
                elif arrived_position or np.fabs(heading_err_cross) < 0.2:
                    print_id = 23
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Case 2.3  Straight forward'
                    wcv.desiredWV_R = 0.1
                    wcv.desiredWV_L = 0.1
                else:
                    if heading_err_cross < 0:
                        print_id = 241
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.4.1  Turn right'
                        wcv.desiredWV_R = -0.05
                        wcv.desiredWV_L = 0.05
                    else:
                        print_id = 242
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.4.2  Turn left'
                        wcv.desiredWV_R = 0.05
                        wcv.desiredWV_L = -0.05

                self.velcmd_pub.publish(wcv)

                if arrived:
                    print 'old idx', self.idx, 'new idx', self.idx + 1
                    self.idx = self.idx + 1
                    arrived = False
                    arrived_position = False
                    rospy.sleep(3)

            # idx == 2, turn right and go to human
            elif self.idx == 2:
                if self.route_idx == 0:  # route 0: turn right early, smaller degrees
                    if robot_pose3d is not None:
                        robot_position2d = robot_pose3d[0:2]
                        target_position2d = target_pose2d[0:2]

                        robot_yaw = tfm.euler_from_quaternion(
                            robot_pose3d[3:7])[2]
                        robot_pose2d = robot_position2d + [robot_yaw]
                    if (robot_pose3d is None) or (self.detection_id != 4) or (
                            self.detection_id == 4
                            and robot_yaw > stop_first_turn_lo_route0[2]):
                        print_id = 2001
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Turn right slowly'
                        wcv.desiredWV_R = -0.05
                        wcv.desiredWV_L = 0.05
                        '''
                    elif robot_yaw < stop_first_turn_up[2]:
                        print_id = 2002
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Turn left slowly'
                        wcv.desiredWV_R = 0.05 
                        wcv.desiredWV_L = -0.05
                        '''
                    else:
                        print_id = 2003
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'idx2 finished'
                        wcv.desiredWV_R = 0
                        wcv.desiredWV_L = 0
                        arrived = True
                else:  # route 1: turn right late, larger degrees, near 90 degrees.
                    if robot_pose3d is not None:
                        robot_position2d = robot_pose3d[0:2]
                        target_position2d = target_pose2d[0:2]

                        robot_yaw = tfm.euler_from_quaternion(
                            robot_pose3d[3:7])[2]
                        robot_pose2d = robot_position2d + [robot_yaw]
                    if (robot_pose3d is None) or (self.detection_id != 4) or (
                            self.detection_id == 4 and
                        (robot_yaw < stop_first_turn_lo_route1[2]
                         or robot_yaw > stop_first_turn_up_route1[2])):
                        # make it between 2.95 and 3.10
                        print_id = 2001
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Turn right slowly'
                        wcv.desiredWV_R = -0.05
                        wcv.desiredWV_L = 0.05
                        '''
                    elif robot_yaw < stop_first_turn_up[2]:
                        print_id = 2002
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Turn left slowly'
                        wcv.desiredWV_R = 0.05 
                        wcv.desiredWV_L = -0.05
                        '''
                    else:
                        print_id = 2003
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'idx2 finished'
                        wcv.desiredWV_R = 0
                        wcv.desiredWV_L = 0
                        arrived = True

                self.velcmd_pub.publish(wcv)
                if arrived:
                    self.idx = self.idx + 1
                    arrived = False
                    arrived_position = False
                    rospy.sleep(1)

            # idx == 4, turn left and go to target
            elif self.idx == 4:
                if self.detection_id == 3:
                    print_id = 4002
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'idx4 finished'
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    arrived = True
                else:
                    print_id = 4001
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Turn left slowly'
                    wcv.desiredWV_R = 0.05
                    wcv.desiredWV_L = -0.05

                self.velcmd_pub.publish(wcv)

                if arrived:
                    self.idx = self.idx + 1
                    arrived = False
                    arrived_position = False
                    rospy.sleep(1)

            rate.sleep()
Пример #13
0
    def navi_loop(self):
        first_turn_point_id6_route0 = [0.95, 0.28, -3.00]  # apriltag id = 6
        first_turn_point_id6_route1 = [0.38, 0.09, 3.076]  # apriltag id = 6
        stop_first_turn_lo_route0 = [2.38, 0.84, -2.75]  # apriltag id = 4
        stop_first_turn_up_route0 = [2.30, 0.86, -3.03]  # apriltag id = 4
        stop_first_turn_lo_route1 = [2.14, -0.45, 2.95]  # apriltag id = 4
        stop_first_turn_up_route1 = [2.01, 0.34, 3.10]  # apriltag id = 4
        generic_target_pose2d = [0.4, 0, np.pi]

        #target_pose2d = [0.25, 0, np.pi]

        rate = rospy.Rate(100)  # 100hz

        wcv = WheelCmdVel()
        wcv.desiredGripperPos = gripperOpen
        arrived = False
        arrived_position = False
        self.print_id = -1

        while not rospy.is_shutdown():
            print 'self.idx', self.idx
            if self.idx >= 11:
                break
            if self.idx <= 0:  # haven't found obstacle
                rospy.sleep(2)
                continue

            # 0. set target
            target_pose2d = generic_target_pose2d

            # 1. get robot pose
            robot_pose3d = lookupTransform(self.lr, '/map', '/robot_base')

            # idx == 1, start from initial position
            if self.idx == 1:
                if self.route_idx == 0:
                    self.idx = self.idx + 1
                    # go straight
                    wcv.desiredWV_R = 0.2
                    wcv.desiredWV_L = 0.2
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(1)

                    # stop
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(1)

                    # turn right a few degrees
                    wcv.desiredWV_R = 0.1
                    wcv.desiredWV_L = 0.2
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(5.5)

                    # stop
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(1)

                    # go straight
                    wcv.desiredWV_R = 0.5
                    wcv.desiredWV_L = 0.5
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(4)
                    # stop
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(1)

                elif self.route_idx == 1:
                    self.idx = self.idx + 1
                    # go straight
                    wcv.desiredWV_R = 0.3
                    wcv.desiredWV_L = 0.3
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(4.5)

                    # stop
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(1)

                    # turn right a few degrees
                    wcv.desiredWV_R = -0.1
                    wcv.desiredWV_L = 0.1
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(3)

                    # stop
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(1)

                    # go straight
                    wcv.desiredWV_R = 0.5
                    wcv.desiredWV_L = 0.5
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(3)  #was3

                    # stop
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(1)

            elif self.idx == 2:
                self.idx = self.idx + 1

            # idx == 2 or 5, go to human or target
            elif (self.idx == 5 and self.detection_id == 3) \
                or (self.idx == 10 and self.detection_id == 4):
                if robot_pose3d is None:
                    print_id = 0
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print '1. Tag not in view, Stop'
                    wcv.desiredWV_R = 0  # right, left
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rate.sleep()
                    continue

                robot_position2d = robot_pose3d[0:2]
                target_position2d = target_pose2d[0:2]

                robot_yaw = tfm.euler_from_quaternion(robot_pose3d[3:7])[2]
                robot_pose2d = robot_position2d + [robot_yaw]

                # 2. navigation policy
                # 2.1 if       in the target, stop
                # 2.2 else if  close to target position, turn to the target orientation
                # 2.3 else if  in the correct heading, go straight to the target position,
                # 2.4 else     turn in the direction of the target position

                pos_delta = np.array(target_position2d) - np.array(
                    robot_position2d)
                robot_heading_vec = np.array(
                    [np.cos(robot_yaw), np.sin(robot_yaw)])
                heading_err_cross = cross2d(
                    robot_heading_vec, pos_delta / np.linalg.norm(pos_delta))

                # print 'robot_position2d', robot_position2d, 'target_position2d', target_position2d
                # print 'pos_delta', pos_delta
                # print 'robot_yaw', robot_yaw
                # print 'norm delta', np.linalg.norm( pos_delta ), 'diffrad', diffrad(robot_yaw, target_pose2d[2])
                # print 'heading_err_cross', heading_err_cross

                if arrived or (np.linalg.norm(pos_delta) < 0.08 and np.fabs(
                        diffrad(robot_yaw, target_pose2d[2])) < 0.05):
                    print_id = 21
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Case 2.1  Stop'
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    arrived = True
                elif np.linalg.norm(pos_delta) < 0.08:
                    arrived_position = True
                    if diffrad(robot_yaw, target_pose2d[2]) > 0:
                        print_id = 221
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.2.1  Turn right slowly'
                        wcv.desiredWV_R = -0.05
                        wcv.desiredWV_L = 0.05
                    else:
                        print_id = 222
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.2.2  Turn left slowly'
                        wcv.desiredWV_R = 0.05
                        wcv.desiredWV_L = -0.05
                elif arrived_position or np.fabs(heading_err_cross) < 0.2:
                    print_id = 23
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Case 2.3  Straight forward'
                    wcv.desiredWV_R = 0.1
                    wcv.desiredWV_L = 0.1
                else:
                    if heading_err_cross < 0:
                        print_id = 241
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.4.1  Turn right'
                        wcv.desiredWV_R = -0.05
                        wcv.desiredWV_L = 0.05
                    else:
                        print_id = 242
                        if print_id != self.print_id:
                            self.print_id = print_id
                            print 'Case 2.4.2  Turn left'
                        wcv.desiredWV_R = 0.05
                        wcv.desiredWV_L = -0.05

                self.velcmd_pub.publish(wcv)

                if arrived:
                    print 'old idx', self.idx, 'new idx', self.idx + 1
                    self.idx = self.idx + 1
                    arrived = False
                    arrived_position = False
                    rospy.sleep(3)

            # idx == 3, wait for human signal
            # apriltag id == 12: left
            # apriltag_id == 14: middle
            # apriltag_id == 16: right
            elif self.idx == 3:
                if self.detection_id == 12:
                    self.bottle = 0
                    self.idx = self.idx + 1
                elif self.detection_id == 14:
                    self.bottle = 1
                    self.idx = self.idx + 1
                elif self.detection_id == 16:
                    self.bottle = 2
                    self.idx = self.idx + 1

            # idx == 4, turn left and go to target
            elif self.idx == 4:
                if self.detection_id == 3:
                    print_id = 4002
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'idx4 finished'
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    arrived = True
                else:
                    print_id = 4001
                    if print_id != self.print_id:
                        self.print_id = print_id
                        print 'Turn left slowly'
                    wcv.desiredWV_R = 0.05
                    wcv.desiredWV_L = -0.05

                self.velcmd_pub.publish(wcv)

                if arrived:
                    self.idx = self.idx + 1
                    arrived = False
                    arrived_position = False
                    rospy.sleep(1)

            # 6 go to bottle
            elif self.idx == 6:
                print 'idx =', self.idx
                if self.bottle == 0:  #left
                    wcv.desiredWV_R = -0.3
                    wcv.desiredWV_L = -0.1
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(2)
                    wcv.desiredWV_R = 0.3
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(1.8)
                    '''
                    wcv.desiredWV_R  = 0.05
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)  
                    rospy.sleep(1)
                    '''
                    wcv.desiredWV_R = 0.1
                    wcv.desiredWV_L = 0.1
                    self.velcmd_pub.publish(wcv)

                    rospy.sleep(3.8)
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    self.idx = self.idx + 1
                elif self.bottle == 1:
                    # middle
                    wcv.desiredWV_R = 0.05
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                    rospy.sleep(3)
                    wcv.desiredWV_R = 0.05
                    wcv.desiredWV_L = 0.05
                    self.velcmd_pub.publish(wcv)
                    self.idx = self.idx + 1
                    rospy.sleep(3.6)
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                elif self.bottle == 2:  # right
                    wcv.desiredWV_R = 0.05
                    wcv.desiredWV_L = 0.05
                    self.velcmd_pub.publish(wcv)
                    self.idx = self.idx + 1
                    rospy.sleep(6)
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                # left
            #7 close gripper
            elif self.idx == 7:
                wcv.desiredGripperPos = girpperClose
                self.velcmd_pub.publish(wcv)
                self.idx = self.idx + 1
                rospy.sleep(2)
                self.idx6cnt = 0

            #8
            elif self.idx == 8:
                # retreat
                print 'idx =', self.idx
                if self.bottle == 0:  #left
                    if self.route_idx == 0:
                        wcv.desiredWV_R = -0.3
                        wcv.desiredWV_L = 0
                        self.velcmd_pub.publish(wcv)
                        rospy.sleep(2)
                        self.idx = self.idx + 1
                        wcv.desiredWV_R = -0.1
                        wcv.desiredWV_L = -0.1
                        #self.velcmd_pub.publish(wcv)
                        rospy.sleep(2)  # left
                        wcv.desiredWV_R = 0
                        wcv.desiredWV_L = 0
                        self.velcmd_pub.publish(wcv)
                    elif self.route_idx == 1:
                        wcv.desiredWV_R = -0.3
                        wcv.desiredWV_L = 0
                        self.velcmd_pub.publish(wcv)
                        rospy.sleep(2)
                        self.idx = self.idx + 1
                        wcv.desiredWV_R = -0.1
                        wcv.desiredWV_L = -0.1
                        #self.velcmd_pub.publish(wcv)
                        rospy.sleep(1)  # left
                        wcv.desiredWV_R = 0
                        wcv.desiredWV_L = 0
                        self.velcmd_pub.publish(wcv)
                elif self.bottle == 1:  #middle
                    wcv.desiredWV_R = -0.3
                    wcv.desiredWV_L = -0.1
                    self.velcmd_pub.publish(wcv)
                    self.idx = self.idx + 1
                    rospy.sleep(5)  #middle
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                elif self.bottle == 2:  #right
                    wcv.desiredWV_R = -0.3
                    wcv.desiredWV_L = -0.1
                    self.velcmd_pub.publish(wcv)
                    self.idx = self.idx + 1
                    rospy.sleep(4)
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)

            elif self.idx == 9:
                # turn right
                print 'idx =', self.idx
                if self.bottle == 0:  #left
                    if self.route_idx == 0:
                        wcv.desiredWV_R = 0
                        wcv.desiredWV_L = 0.2
                        self.velcmd_pub.publish(wcv)
                        self.idx = self.idx + 1
                        rospy.sleep(3)  # left
                        wcv.desiredWV_R = 0
                        wcv.desiredWV_L = 0
                        self.velcmd_pub.publish(wcv)
                    elif self.route_idx == 1:
                        wcv.desiredWV_R = 0
                        wcv.desiredWV_L = 0.2
                        self.velcmd_pub.publish(wcv)
                        rospy.sleep(3)  # left
                        if self.detection_id != 4:
                            wcv.desiredWV_R = 0
                            wcv.desiredWV_L = 0.05
                            self.velcmd_pub.publish(wcv)
                        else:
                            self.idx = self.idx + 1
                elif self.bottle == 1:  #middle
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0.2
                    self.velcmd_pub.publish(wcv)
                    self.idx = self.idx + 1
                    rospy.sleep(5.5)  # middle
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
                elif self.bottle == 2:  #right
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0.2
                    self.velcmd_pub.publish(wcv)
                    self.idx = self.idx + 1
                    rospy.sleep(5)
                    wcv.desiredWV_R = 0
                    wcv.desiredWV_L = 0
                    self.velcmd_pub.publish(wcv)
            '''
            elif self.idx == 10:
                # go straight
                
                print 'idx =', self.idx
                wcv.desiredWV_R = 0.3
                wcv.desiredWV_L = 0.3
                self.velcmd_pub.publish(wcv) 
                self.idx = self.idx + 1
                rospy.sleep(4.5)
                wcv.desiredWV_R = 0
                wcv.desiredWV_L = 0
                self.velcmd_pub.publish(wcv) 
            '''

            rate.sleep()