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
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
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
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