def mb_execute_cb(msg):
mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg.target_pose), feedback_cb=mbf_feedback_cb)
rospy.logdebug("Relaying legacy move_base goal to mbf")
mbf_mb_ac.wait_for_result()
status = mbf_mb_ac.get_state()
result = mbf_mb_ac.get_result()
rospy.logdebug("MBF execution completed with result [%d]: %s", result.outcome, result.message)
if result.outcome == mbf_msgs.MoveBaseResult.SUCCESS:
mb_as.set_succeeded(mb_msgs.MoveBaseResult(), "Goal reached.")
else:
mb_as.set_aborted(mb_msgs.MoveBaseResult(), result.message)
def mb_execute_cb(msg):
max_replanning_attempts = 1
replanning_counter = 0
last_executed_lane_plan_length = 10000000
min_new_plan_lengh_difference = 3 # safety distance used to avoid loops in switching between planners
is_following_lane = True
rospy.loginfo("New goal received by move base flex script")
while True:
# Stop if a new goal is received or the goal is cancelled
if mb_as.is_new_goal_available() or mb_as.is_preempt_requested():
mbf_ep_ac.cancel_all_goals()
velocity_publisher.publish(Twist())
mb_as.set_preempted()
return
# Try to follow the lane
mbf_gp_ac.send_goal(
mbf_msgs.GetPathGoal(target_pose=msg.target_pose,
use_start_pose=False,
planner="WaypointGlobalPlanner",
concurrency_slot=1))
mbf_gp_ac.wait_for_result()
planning_result = mbf_gp_ac.get_result()
new_plan_length = getPathLength(planning_result.path, 0,
len(planning_result.path.poses) - 1)
loop_detected = (not is_following_lane) and (
new_plan_length >
last_executed_lane_plan_length - min_new_plan_lengh_difference)
if planning_result.outcome != mbf_msgs.GetPathResult.SUCCESS or loop_detected:
is_following_lane = False
# Try using SPBL to go to some of the following points on the lane
# Get the remaining path
mbf_gp_ac.send_goal(
mbf_msgs.GetPathGoal(
target_pose=msg.target_pose,
use_start_pose=False,
planner="NoObstaclesWaypointGlobalPlanner",
concurrency_slot=1))
mbf_gp_ac.wait_for_result()
short_plan_found = False
planning_result = mbf_gp_ac.get_result()
if planning_result.outcome == mbf_msgs.GetPathResult.SUCCESS:
path_poses = planning_result.path.poses
lane_path_length = getPathLength(
planning_result.path, 0,
len(planning_result.path.poses) - 1)
number_of_remaining_waypoints = len(path_poses)
step_size = 10 # in number of waypoints
first_waypoint_index = max(
0, min(number_of_remaining_waypoints - 1, 15))
last_waypoint_index = max(
0, min(number_of_remaining_waypoints - 1, 80))
for waypoint_index in range(first_waypoint_index,
last_waypoint_index, step_size):
mbf_gp_ac.send_goal(
mbf_msgs.GetPathGoal(
target_pose=path_poses[waypoint_index],
planner="SBPLLatticePlanner",
concurrency_slot=2))
while not mbf_gp_ac.wait_for_result(
rospy.Duration.from_sec(0.1)):
if mb_as.is_new_goal_available(
) or mb_as.is_preempt_requested():
mbf_ep_ac.cancel_all_goals()
mbf_gp_ac.cancel_all_goals()
velocity_publisher.publish(Twist())
mb_as.set_preempted()
return
planning_result = mbf_gp_ac.get_result()
if planning_result.outcome == mbf_msgs.GetPathResult.SUCCESS:
free_path_length = getPathLength(
planning_result.path, 0,
len(planning_result.path.poses) - 1)
if free_path_length > lane_path_length * 2:
# the robot would probably turn around the corridor, try the next waypoint
continue
short_plan_found = True
# Append the reamaining part of the plan
if waypoint_index < len(path_poses) - 1:
planning_result.path.poses += path_poses[
waypoint_index:]
break
# Try to use SBPL to go to the goal
if not short_plan_found:
mbf_gp_ac.send_goal(
mbf_msgs.GetPathGoal(target_pose=msg.target_pose,
planner="SBPLLatticePlanner",
concurrency_slot=4))
while not mbf_gp_ac.wait_for_result(
rospy.Duration.from_sec(0.1)):
if mb_as.is_new_goal_available(
) or mb_as.is_preempt_requested():
mbf_ep_ac.cancel_all_goals()
mbf_gp_ac.cancel_all_goals()
velocity_publisher.publish(Twist())
mb_as.set_preempted()
return
planning_result = mbf_gp_ac.get_result()
else:
last_executed_lane_plan_length = new_plan_length
is_following_lane = True
if planning_result.outcome != mbf_msgs.GetPathResult.SUCCESS:
# There is nothing left to try, abort
mbf_ep_ac.cancel_all_goals()
velocity_publisher.publish(Twist())
mb_as.set_aborted(mb_msgs.MoveBaseResult(),
planning_result.message)
return
# Execute the path
rospy.loginfo("Execute the plan")
mbf_ep_ac.send_goal(
mbf_msgs.ExePathGoal(path=planning_result.path,
concurrency_slot=3))
# Wait a bit
execution_completed = mbf_ep_ac.wait_for_result(
rospy.Duration.from_sec(0.1))
# If the execution of the global plan is completed, return
if execution_completed:
if mbf_ep_ac.get_result() == mbf_msgs.ExePathResult.SUCCESS:
mb_as.set_succeeded(mb_msgs.MoveBaseResult(), "Goal reached.")
return
else:
velocity_publisher.publish(Twist())
if replanning_counter >= max_replanning_attempts:
mb_as.set_aborted(mb_msgs.MoveBaseResult(),
mbf_ep_ac.get_result().message)
return
else:
replanning_counter += 1
else:
replanning_counter = 0