I do not understand why getPath keeps aborting ones moved to in other scenarios a fine position.
I have a monitor state (WAIT_CANCEL) running concurrently to the getpath, recovery, exePath state-machine that on an empty message ends the concurrent state and moves to a wait_goal state. This state on receiving a goal starts the concurrent state machine with the WAIT_CANCEL state and [getpath, recovery, exePath] state-machine.
If I cancel it by sending to WAIT_CANCEL and then send the exact same goal Getpath succeeds. if I have the concurrent state end after recovery and go to wait_goal with the same goal on it's own, GetPath fails.
It keeps telling me the planner patience is exceeded. If I send to the WAIT_CANCEL, it tells me it canceled the planner. but not otherwise.
with self.sm:
# WAIT_MSG Concurrence 1
#Concurrence terminiation when
def wait_msg_child_term_cb(outcome_map):
if outcome_map['WAIT_WAYPOINT'] == 'invalid':
return True
elif outcome_map['WAIT_RUN'] == 'invalid':
return True
elif outcome_map['WAIT_RESET'] == 'invalid':
return True
else:
return False
#Concurrence output
def wait_msg_out_cb(outcome_map):
if outcome_map['WAIT_WAYPOINT'] == 'invalid':
return 'new_waypoint'
elif outcome_map['WAIT_RUN'] == 'invalid':
return 'run'
elif outcome_map['WAIT_RESET'] == 'invalid':
return 'reset'
else:
return 'nothing'
Wait_message = smach.Concurrence(outcomes=['new_waypoint', 'run', 'reset', 'nothing'],
default_outcome='nothing',
input_keys = ['goals'],
output_keys = ['goals'],
child_termination_cb=wait_msg_child_term_cb,
outcome_cb=wait_msg_out_cb)
with Wait_message:
def goal_cb(userdata, msg):
userdata.goals.append(msg)
return False
def reset_cb(userdata, msg):
userdata.goals = []
return False
def monitor_cb(ud, msg):
return False
smach.Concurrence.add('WAIT_WAYPOINT', smach_ros.MonitorState(
GOAL_TOPIC,
PoseStamped,
goal_cb,
input_keys=['goals'],
output_keys=['goals']
))
smach.Concurrence.add('WAIT_RUN', smach_ros.MonitorState(
RUN_TOPIC,
Empty,
monitor_cb
))
smach.Concurrence.add('WAIT_RESET', smach_ros.MonitorState(
RESET_TOPIC,
Empty,
reset_cb,
input_keys=['goals'],
output_keys=['goals']
))
smach.StateMachine.add(
'WAIT_MSG',
Wait_message,
transitions={
'new_waypoint': 'WAIT_MSG',
'run': 'RUN_PATH',
'reset': 'WAIT_MSG',
'nothing': 'WAIT_MSG'
}
)
#RUN_PATH Concurrence 2
#Concurrence termination when
def run_path_child_term_cb(outcome_map):
if outcome_map['WAIT_CANCEL'] == 'invalid':
return True
elif outcome_map['WAIT_RESTART'] == 'invalid':
return True
elif outcome_map['EXE_SUB'] == 'succeeded' or outcome_map['EXE_SUB'] == 'aborted' or outcome_map['EXE_SUB'] == 'preempted':
return True
else:
return False
#Concurrence output
def run_path_out_cb(outcome_map):
if outcome_map['WAIT_CANCEL'] == 'invalid':
return 'aborted'
elif outcome_map['EXE_SUB'] == 'succeeded':
return 'succeeded'
elif outcome_map['EXE_SUB'] == 'aborted':
return 'aborted'
elif outcome_map['EXE_SUB'] == 'preempted':
return 'preempted'
else:
return 'succeeded'
run_path = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
input_keys = ['goals'],
output_keys = ['goals'],
child_termination_cb=run_path_child_term_cb,
outcome_cb=run_path_out_cb)
with run_path:
def cancel_cb(ud, msg):
return False
smach.Concurrence.add('WAIT_CANCEL', smach_ros.MonitorState(
CANCEL_TOPIC,
Empty,
cancel_cb
))
# Create the sub SMACH state machine
exe_sub = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys = ['goals'],
output_keys = ['goals'])
exe_sub.userdata.goal = None
exe_sub.userdata.path = None
exe_sub.userdata.error = None
exe_sub.userdata.error_status = None
# Open the container
with exe_sub:
class set_goal(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['success','aborted'])
def execute(self, userdata):
#Set new goal
if len(exe_sub.userdata.goals)==0:
print('no_waypoints')
return 'aborted'
else:
exe_sub.userdata.goal = exe_sub.userdata.goals[0]
return 'success'
# Set goal from waypoint list
smach.StateMachine.add(
'SET_GOAL',
set_goal(),
transitions={
'success': 'GET_PATH'
}
)
# Get path
smach.StateMachine.add(
'GET_PATH',
smach_ros.SimpleActionState(
'/move_base_flex/get_path',
GetPathAction,
goal_slots=['target_pose'],
result_slots=['path']
),
transitions={
'succeeded': 'EXE_PATH',
'aborted': 'RECOVERY',
'preempted': 'preempted'
},
remapping={
'target_pose': 'goal'
}
)
# EXE_PATH
smach.StateMachine.add(
'EXE_PATH',
smach_ros.SimpleActionState(
'/move_base_flex/exe_path',
ExePathAction,
goal_slots=['path']
),
transitions={
'succeeded': 'CHECK_GOALS',
'aborted': 'RECOVERY',
'preempted': 'preempted'
}
)
class check_goals(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['success','continue'])
def execute(self, userdata):
del exe_sub.userdata.goals[0]
if len(exe_sub.userdata.goals)==0:
return 'success'
else:
return 'continue'
# move waypoint list along
smach.StateMachine.add(
'CHECK_GOALS',
check_goals(),
transitions={
'success': 'succeeded',
'continue': 'SET_GOAL'
}
)
# Goal callback for state RECOVERY
def recovery_path_goal_cb(userdata, goal):
if self.fdist > self.bdist and self.fdist > 1.0:
goal.behavior = 'twist_recovery_forwards'
elif self.bdist > 1.0:
goal.behavior = 'twist_recovery_backwards'
else:
goal.behavior = 'do_nothing_5_seconds'
return
smach.StateMachine.add(
'RECOVERY',
smach_ros.SimpleActionState(
'move_base_flex/recovery',
RecoveryAction,
goal_cb=recovery_path_goal_cb,
input_keys=["error"],
output_keys = ["error_status"]
),
transitions={
'succeeded': 'SET_GOAL',
'aborted': 'aborted',
'preempted': 'preempted'
}
)
smach.Concurrence.add('EXE_SUB', exe_sub)
# Execute path
smach.StateMachine.add('RUN_PATH',
run_path,
transitions={
'succeeded': 'WAIT_MSG',
'aborted': 'ABORTED_GOALS',
'preempted': 'preempted'
}
)
class aborted_goals(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['success'])
def execute(self, userdata):
msg = String()
msg.data = "failed_reaching_waypoint"
pubGUI.publish(msg.data)
return 'success'
smach.StateMachine.add(
'ABORTED_GOALS',
aborted_goals(),
transitions={
'success': 'WAIT_MSG'
}
)
-ROS noetic
-Smach
Using a smach setup similar to: http://wiki.ros.org/move_base_flex/Tutorials/SimpleSmachForMoveBaseFlex
I move the robot to an impossible location, inside an obstacle, as due to dynamic object this scenario can sometimes occur for us.
I do not understand why getPath keeps aborting ones moved to in other scenarios a fine position.
I have a monitor state (WAIT_CANCEL) running concurrently to the getpath, recovery, exePath state-machine that on an empty message ends the concurrent state and moves to a wait_goal state. This state on receiving a goal starts the concurrent state machine with the WAIT_CANCEL state and [getpath, recovery, exePath] state-machine.
If I cancel it by sending to WAIT_CANCEL and then send the exact same goal Getpath succeeds. if I have the concurrent state end after recovery and go to wait_goal with the same goal on it's own, GetPath fails.
It keeps telling me the planner patience is exceeded. If I send to the WAIT_CANCEL, it tells me it canceled the planner. but not otherwise.
Below the state_machine