This method works. Run the tutorial smacc program twice, so there will be two seperate statemachine node. And whenever you publish a new goal, one of them will work to reach the goal and the other one will be preempted!
I reviesed the program slightly as below:
#!/usr/bin/env python3
import rospy
import smach
import smach_ros
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
from mbf_msgs.msg import ExePathAction
from mbf_msgs.msg import GetPathAction
from mbf_msgs.msg import RecoveryAction
import threading
def main():
rospy.init_node('mbf_state_machine',anonymous=True)
# Create SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Define userdata
sm.userdata.goal = None
sm.userdata.path = None
sm.userdata.error = None
sm.userdata.clear_costmap_flag = False
sm.userdata.error_status = None
with sm:
# Goal callback for state WAIT_FOR_GOAL
def goal_cb(userdata, msg):
userdata.goal = msg
return False
# Monitor topic to get MeshGoal from RViz plugin
smach.StateMachine.add(
'WAIT_FOR_GOAL',
smach_ros.MonitorState(
'/move_base_simple/goal',
PoseStamped,
goal_cb,
output_keys=['goal']
),
transitions={
'invalid': 'GET_PATH',
'valid': 'WAIT_FOR_GOAL',
'preempted': 'preempted'
}
)
# Get path
smach.StateMachine.add(
'GET_PATH',
smach_ros.SimpleActionState(
'/move_base/get_path',
GetPathAction,
goal_slots=['target_pose'],
result_slots=['path']
),
transitions={
'succeeded': 'EXE_PATH',
'aborted': 'WAIT_FOR_GOAL',
'preempted': 'preempted'
},
remapping={
'target_pose': 'goal'
}
)
# Execute path
smach.StateMachine.add(
'EXE_PATH',
smach_ros.SimpleActionState(
'/move_base/exe_path',
ExePathAction,
goal_slots=['path']
),
transitions={
'succeeded': 'WAIT_FOR_GOAL',
'aborted': 'RECOVERY',
#'preempted': 'preempted'
'preempted': 'WAIT_FOR_GOAL'
}
)
# Goal callback for state RECOVERY
def recovery_path_goal_cb(userdata, goal):
if userdata.clear_costmap_flag == False:
goal.behavior = 'clear_costmap'
userdata.clear_costmap_flag = True
else:
goal.behavior = 'straf_recovery'
userdata.clear_costmap_flag = False
# Recovery
smach.StateMachine.add(
'RECOVERY',
smach_ros.SimpleActionState(
'move_base/recovery',
RecoveryAction,
goal_cb=recovery_path_goal_cb,
input_keys=["error", "clear_costmap_flag"],
output_keys = ["error_status", 'clear_costmap_flag']
),
transitions={
#'succeeded': 'GET_PATH',
'succeeded': 'WAIT_FOR_GOAL',
#'aborted': 'aborted',
'aborted': 'WAIT_FOR_GOAL',
#'preempted': 'preempted'
'preempted': 'WAIT_FOR_GOAL'
}
)
# Create and start introspection server
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
sm_t = threading.Thread(target=sm.execute)
sm_t.start()
# sm.execute()
# Wait for interrupt and stop introspection server
rospy.spin()
sis.stop()
sm.preempt_requested()
rospy.loginfo("wait sm exit!")
sm.request_preempt()
sm_t.join()
if __name__=="__main__":
main()
Found the following issue #145 which pretty much answers my own question. Any further details/insight in this is still appreciated!
Also refer to issues #70 and #64.
@Tider's method uses the node reuse approach(very ROSy?!) to run GetPath and ExePath simultaneously. Plus, as he points out the code segments only stop the Controller when cancelling goal, not when updating the Goal as discussed in issues #64, #70 and #145.
TO SUMMARIZE: Another state needs to be running in parallel that Gets an updated path while the ExePath is working for the path to be updated. This doesn't need calling ExePath again as long as the same controller and concurrency slot is used.