move_base_flex adds waypoints to the robot, and the recovery function, as well as the problem that the obstacles in the map are not cleared in time?
I am using ros melodic, and there is still another question at the moment, how should I configure it so that the robot can distinguish between the path point and the target point?Below is my startup file:
<launch>
<node pkg="mbf_costmap_nav" name="move_base_flex" type="mbf_costmap_nav" respawn="false" output="screen">
<rosparam file="$(find motor-ctrl)/config/base5/costmap_common_params.yaml" command="load" ns="global_costmap"/>-->
<rosparam file="$(find motor-ctrl)/config/base5/costmap_common_params.yaml" command="load" ns="local_costmap"/>-->
<rosparam file="$(find motor-ctrl)/config/base5/local_costmap_params.yaml" command="load" />
<rosparam file="$(find motor-ctrl)/config/base5/global_costmap_params.yaml" command="load"/>
<rosparam file="$(find motor-ctrl)/config/base5/planners.yaml" command="load"/>
<rosparam file="$(find motor-ctrl)/config/base5/controllers.yaml" command="load"/>
<rosparam file="$(find motor-ctrl)/config/base5/recovery_behaviors.yaml" command="load"/>
<remap from="/move_base_flex/GlobalPlanner/plan" to="path"/>-->
<remap from="cmd_vel" to="cmd_vel"/>
</node>
<node name="navigation_sm" pkg="mbf_costmap_nav" type="navigation_sm.py">
<param name="base_global_planner" value="GlobalPlanner" />
<param name="base_local_planner" value="TrackingPidLocalPlanner" />
</node>
</launch>
recovery_behavious.yaml
recovery_behaviors:
- name: 'rotate_recovery'
type: 'rotate_recovery/RotateRecovery'
- name: 'clear_costmap'
type: 'clear_costmap_recovery/ClearCostmapRecovery'
- name: 'moveback_recovery'
type: 'moveback_recovery/MoveBackRecovery'
moveback_recovery:
linear_vel_back: -0.1 # default -0.3
step_back_length: 0.3 # default 1.0
step_back_timeout: 5.0 # default 15.0
navigation_sm.py
import rospy
import smach
import smach_ros
from mbf_msgs.msg import ExePathAction, ExePathResult
from mbf_msgs.msg import GetPathAction, GetPathResult
from mbf_msgs.msg import RecoveryAction, RecoveryResult
from wait_for_goal import WaitForGoal
class Control(smach.StateMachine):
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted', 'failed'],
input_keys=['path'],
output_keys=[
'outcome', 'message',
'final_pose', 'dist_to_goal', 'angle_to_goal',
'recovery_behavior']
)
with self:
self.userdata.recovery_behavior = None
smach.StateMachine.add(
'EXE_PATH',
smach_ros.SimpleActionState(
'move_base_flex/exe_path',
ExePathAction,
goal_cb=Control.controller_goal_cb,
result_cb=Control.controller_result_cb),
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'aborted',
'failed': 'failed',
}
)
@staticmethod
@smach.cb_interface(input_keys=['path'])
def controller_goal_cb(user_data, goal):
goal.path = user_data.path
goal.controller = 'PathTrackingPID'
@staticmethod
@smach.cb_interface(
output_keys=['path', 'outcome', 'message', 'final_pose', 'dist_to_goal', 'angle_to_goal', 'recovery_behavior'],
outcomes=['succeeded', 'aborted', 'failed', 'preempted'])
def controller_result_cb(user_data, status, result):
outcome_map = {
ExePathResult.COLLISION: 'COLLISION',
ExePathResult.CANCELED: 'CANCELED',
ExePathResult.BLOCKED_PATH: 'BLOCKED_PATH',
ExePathResult.FAILURE: 'FAILURE',
ExePathResult.INTERNAL_ERROR: 'INTERNAL_ERROR',
ExePathResult.INVALID_PATH: 'INVALID_PATH',
ExePathResult.MISSED_GOAL: 'MISSED_GOAL',
ExePathResult.INVALID_PLUGIN: 'INVALID_PLUGIN',
ExePathResult.MISSED_PATH: 'MISSED_PATH',
ExePathResult.NO_VALID_CMD: 'NO_VALID_CMD',
ExePathResult.NOT_INITIALIZED: 'NOT_INITIALIZED',
ExePathResult.OSCILLATION: 'OSCILLATION',
ExePathResult.PAT_EXCEEDED: 'PAT_EXCEEDED',
ExePathResult.ROBOT_STUCK: 'ROBOT_SUCK',
ExePathResult.TF_ERROR: 'TF_ERROR',
ExePathResult.SUCCESS: 'SUCCESS',
}
controller_aborted_map = [
ExePathResult.TF_ERROR,
ExePathResult.INTERNAL_ERROR,
ExePathResult.INVALID_PATH,
ExePathResult.NOT_INITIALIZED,
]
controller_failed_map = [
ExePathResult.PAT_EXCEEDED,
ExePathResult.BLOCKED_PATH,
ExePathResult.FAILURE,
ExePathResult.MISSED_PATH,
ExePathResult.MISSED_GOAL,
ExePathResult.NO_VALID_CMD,
ExePathResult.OSCILLATION,
ExePathResult.ROBOT_STUCK,
]
user_data.outcome = result.outcome
user_data.message = result.message
user_data.final_pose = result.final_pose
user_data.dist_to_goal = result.dist_to_goal
user_data.angle_to_goal = result.angle_to_goal
recovery_behavior = 'clear_costmap'
if result.outcome == ExePathResult.SUCCESS:
p = result.final_pose.pose.position
rospy.loginfo("Controller arrived at goal: (%s), %s, %s",
str(p), outcome_map[result.outcome], result.message)
return 'succeeded'
elif result.outcome == ExePathResult.CANCELED:
rospy.loginfo("Controller has been canceled.")
return 'preempted'
elif result.outcome in controller_failed_map:
rospy.logwarn("Controller failed: %s, %s", outcome_map[result.outcome], result.message)
user_data.recovery_behavior = recovery_behavior
rospy.loginfo("Set recovery behavior to %s", recovery_behavior ...
@ osilva I reposted the question