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?

asked 2021-12-03 00:35:09 -0500

mingyue gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

@ osilva I reposted the question

mingyue gravatar image mingyue  ( 2021-12-03 00:36:04 -0500 )edit