Exemplo n.º 1
0
def build_turn_sm(forward_vel,turn_vel,turn_left=True):
    
    turn_sm = smach.Sequence(outcomes=["succeeded","aborted","preempted"],connector_outcome='succeeded')
    with turn_sm:
        smach.Sequence.add(
            "DRIVE_OUT_OF_ROW", 
            smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
                                        drive_forwardAction,
                                        goal=drive_forwardGoal(amount=2.5,vel=forward_vel))
        )
        if turn_left:
            smach.Sequence.add(
                "TURN_LEFT", 
                smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
                                            make_turnAction,
                                            goal=make_turnGoal(amount=math.pi/2,vel=turn_vel))
            )
        else:
            smach.Sequence.add(
                "TURN_RIGHT", 
                smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
                                            make_turnAction,
                                            goal=make_turnGoal(amount=-math.pi/2,vel=turn_vel))
            )
        smach.Sequence.add(
            "DRIVE_TO_NEXT_ROW", 
            smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
                                        drive_forwardAction,
                                        goal=drive_forwardGoal(amount=0.5,vel=forward_vel))
        )
        if turn_left:
            smach.Sequence.add(
                "TURN_LEFT_INTO_ROW", 
                smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
                                            make_turnAction,
                                            goal=make_turnGoal(amount=math.pi/2,vel=turn_vel)) 
            )
        else:
            smach.Sequence.add(
                "TURN_RIGHT_INTO_ROW", 
                smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
                                            make_turnAction,
                                            goal=make_turnGoal(amount=-math.pi/2,vel=turn_vel)) 
            )
        smach.Sequence.add(
            "DRIVE_INTO_ROW", 
            smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
                                        drive_forwardAction,
                                        goal=drive_forwardGoal(amount=1.5,vel=forward_vel))
        )
    return turn_sm
Exemplo n.º 2
0
def build_u_turn_sm(length_in, length_out, width, turn_radius , direction_l,vel_fw,vel_turn,fix_offset):
    """
    """
    # vel is in m/s turn radius is in meter
    if not direction_l:
        turn = vel_turn/turn_radius + 0.08
    else:
        turn = vel_turn/turn_radius
    
    if direction_l:
        lr_amount = 1.42
    else:
        lr_amount = -1.42
     
    uturn_sm = smach.StateMachine(outcomes=["succeeded","aborted","preempted"])
    
    with uturn_sm:
        smach.StateMachine.add("DRIVE_FW_IN", 
                               smach_ros.SimpleActionState("/fmExecutors/drive_forward", 
                                                           drive_forwardAction, 
                                                           goal = drive_forwardGoal(amount=(length_in-turn_radius), vel=vel_fw, fix_offset=fix_offset ) ), 
                               transitions={"succeeded":"TURN_1"}, 
                               )
        smach.StateMachine.add("TURN_1",
                               smach_ros.SimpleActionState("/fmExecutors/make_turn",
                                                           make_turnAction,
                                                           goal= make_turnGoal(amount = lr_amount, vel=turn, forward_vel=vel_turn )),
                               transitions={"succeeded":"DRIVE_FW_CROSS"}
                               )
        smach.StateMachine.add("DRIVE_FW_CROSS", 
                               smach_ros.SimpleActionState("/fmExecutors/drive_forward", 
                                                           drive_forwardAction, 
                                                           goal = drive_forwardGoal(amount=(width-turn_radius*2), vel=vel_fw, fix_offset=fix_offset ) ), 
                               transitions={"succeeded":"TURN_2"}, 
                               )
        smach.StateMachine.add("TURN_2",
                               smach_ros.SimpleActionState("/fmExecutors/make_turn",
                                                           make_turnAction,
                                                           goal= make_turnGoal(amount = lr_amount, vel=turn, forward_vel=vel_turn)),
                               transitions={"succeeded":"DRIVE_FW_OUT"}
                               )
        smach.StateMachine.add("DRIVE_FW_OUT",
                               smach_ros.SimpleActionState("/fmExecutors/drive_forward", 
                                                           drive_forwardAction, 
                                                           goal = drive_forwardGoal(amount=(length_out-turn_radius), vel=vel_fw, fix_offset=fix_offset ) ), 
                               transitions={"succeeded":"succeeded"}, 
                               )
    
    return uturn_sm
Exemplo n.º 3
0
def build_fish_tail(turn_vel,drive_vel):
    fish_tail = smach.Sequence(outcomes=['succeeded','aborted','preempted'], connector_outcome='succeeded')
    
    with fish_tail:
        smach.Sequence.add("TURN1", 
                           smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
                                        make_turnAction,
                                        goal=make_turnGoal(amount=0.5025,vel=turn_vel))
                           )
        smach.Sequence.add("DRIVE1",
                           smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
                                        drive_forwardAction,
                                        goal=drive_forwardGoal(amount=1,vel=drive_vel))
                           )
        smach.Sequence.add("TURN2", 
                           smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
                                        make_turnAction,
                                        goal=make_turnGoal(amount=1.05,vel=turn_vel))
                           )
        smach.Sequence.add("DRIVE_REV1",
                           smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
                                        drive_forwardAction,
                                        goal=drive_forwardGoal(amount=-1,vel=drive_vel))
                           )
        smach.Sequence.add("TURN_REV1", 
                           smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
                                        make_turnAction,
                                        goal=make_turnGoal(amount=1.05,vel=turn_vel))
                           )
        smach.Sequence.add("DRIVE_REV2",
                           smach_ros.SimpleActionState("/fmDecisionMakers/drive_forward",
                                        drive_forwardAction,
                                        goal=drive_forwardGoal(amount=1,vel=drive_vel))
                           )
        smach.Sequence.add("TURN_REV2", 
                           smach_ros.SimpleActionState("/fmDecisionMakers/make_turn",
                                        make_turnAction,
                                        goal=make_turnGoal(amount=0.5025,vel=turn_vel))
                           )
        
    return fish_tail
Exemplo n.º 4
0
def build_sm():
    """
        Construct the state machine executing the selected behaviours
    """
    row_navigator = smach.StateMachine(outcomes=["succeeded","aborted","preempted"])
    
    row_goal = navigate_in_row_simpleGoal()
    
    row_goal.desired_offset_from_row = 1.1
    row_goal.distance_scale = -0.3
    row_goal.forward_velcoity = 0.8
    row_goal.headland_timeout = 0
    row_goal.P = 1.2
    
    turn_goal = make_turnGoal()
    turn_goal.amount = math.pi
    turn_goal.vel = 0.6
    
    wiggle_goal = make_turnGoal()
    wiggle_goal.amount = 0.1
    wiggle_goal.vel = 0.5
    
    fw_goal = drive_forwardGoal()
    fw_goal.amount = 2
    fw_goal.vel = 0.6
    
    find_row_timeout_sm = smach.Concurrence(outcomes=['succeeded','aborted','preempted'], 
                           default_outcome='aborted',
                           outcome_map={
                                        "aborted":{'TIMEOUT':'succeeded','FIND_ROW':'preempted'}, 
                                        "succeeded":{'FIND_ROW':'invalid'}},
                           child_termination_cb=force_preempt)
    
    with find_row_timeout_sm:
        smach.Concurrence.add("FIND_ROW", smach_ros.MonitorState("/fmExtractors/rows", row, on_rows, -1))
        smach.Concurrence.add("TIMEOUT" , behaviours.WaitState(2))
        
    with row_navigator:
        smach.StateMachine.add("FIND_ROW_WITH_TIMEOUT", 
                               find_row_timeout_sm,
                               transitions={'succeeded':'NAVIGATE_IN_ROW','aborted':'NAVIGATE_IN_ROW'}
                               )
        
        smach.StateMachine.add("NAVIGATE_IN_ROW", 
                               smach_ros.SimpleActionState("/navigate_in_row_simple", navigate_in_row_simpleAction,row_goal),
                               transitions={'succeeded':'aborted'}
                               )
    
    laser_state = smach.Concurrence(outcomes=['yellow_active','red_active'], 
                                    outcome_map={'yellow_active':{'WAIT_FOR_YELLOW':'invalid','WAIT_FOR_RED':'preempted'},
                                                 'red_active'   :{'WAIT_FOR_RED':'invalid'}}
                                    ,default_outcome='red_active',child_termination_cb=force_preempt)
    
    red_active = smach_ros.MonitorState("/fmExtractors/safety_zone",lidar_safety_zone,on_safety_red, max_checks=-1)
    red_not_active = smach_ros.MonitorState("/fmExtractors/safety_zone",lidar_safety_zone,on_safety_red_neg, max_checks=-1)
    
    yellow_active = smach_ros.MonitorState("/fmExtractors/safety_zone",lidar_safety_zone,on_safety_yellow, max_checks=-1)
    yellow_not_active = smach_ros.MonitorState("/fmExtractors/safety_zone",lidar_safety_zone,on_safety_yellow_neg, max_checks=-1)
    with laser_state:
        smach.Concurrence.add("WAIT_FOR_YELLOW",
                               yellow_active
                               )
        smach.Concurrence.add("WAIT_FOR_RED",
                              red_active
                              )

    
    control_state = smach.Concurrence(outcomes=['stop','retract','aborted'],
                                      outcome_map={'stop':{'MONITOR_LASER':'yellow_active'},
                                                   'retract':{'MONITOR_LASER':'red_active'},
                                                   'aborted':{'NAVIGATE_IN_ROW':'aborted'}}, 
                                      default_outcome='stop',
                                      child_termination_cb=force_preempt)

    with control_state:
        smach.Concurrence.add("MONITOR_LASER", laser_state)
        smach.Concurrence.add("NAVIGATE_IN_ROW", row_navigator)
       
       
        
    stop_state = smach.Concurrence(outcomes=['continue','retreat'],
                                      outcome_map={'continue':{'MONITOR_YELLOW':'invalid','MONITOR_RED':'preempted'},
                                                   'retreat':{'MONITOR_RED':'invalid'}
                                                   },
                                      default_outcome='continue',
                                      child_termination_cb=force_preempt)
    with stop_state:
        smach.Concurrence.add("MONITOR_RED", red_active)
        smach.Concurrence.add("MONITOR_YELLOW",yellow_not_active)
    
    
    retreat_state = smach.StateMachine(outcomes=['clear','aborted','preempted'])
    
    with retreat_state:
        smach.StateMachine.add("BACK_UP",
                               smach_ros.SimpleActionState("/drive_forward",drive_forwardAction,drive_forwardGoal(amount=-0.3,vel=0.3)),
                               transitions ={'succeeded':'MONITOR_RED','aborted':'aborted','preempted':'preempted'})
        smach.StateMachine.add("MONITOR_RED",
                               red_not_active,
                               transitions={'valid':'BACK_UP','invalid':'clear'})
        
    
    master = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
        
    with master:
        smach.StateMachine.add("DRIVE_IN_ROW",
                               control_state,
                               transitions={'stop':'STOP_STATE','retract':'RETREAT_STATE','aborted':'TURN_180'})
        smach.StateMachine.add("STOP_STATE",
                               stop_state,
                               transitions={'continue':'DRIVE_IN_ROW','retreat':'RETREAT_STATE'})
        smach.StateMachine.add("RETREAT_STATE",
                               retreat_state,
                               transitions={'clear':'TURN_180','aborted':'aborted'})
        smach.StateMachine.add("TURN_180",
                               smach_ros.SimpleActionState("/make_turn", make_turnAction, goal=make_turnGoal(amount=math.pi-0.01,vel=0.6)),
                               transitions={'succeeded':'DRIVE_IN_ROW'})
        
    return master