How to remove object collision for the fetch demo

asked 2020-06-27 22:12:21 -0600

ung12345 gravatar image

updated 2020-06-29 04:06:01 -0600

I am attempting to use move_base as shown in the code below to move the fetch robot to the elevator world from the gazebo tutorial however when the robot moves too close to any object it uses an object collision detection method to attempt to move around it. Basically I copied the fetch gazebo demo and changed some files to allow it to run in the elevator world instead of the test_zone.

The distance is not close enough to activate the plugin to allow the elevator doors to open. I am unsure where I can disable the object collision I have attempted to look through the python file. Using the move_base function works as intended in an empty world. For example if I input

move_base.goto(2.0,0,0)

it will move the robot down 2 grid blocks on gazebo. However if i place a cube model in front at around (3,0,0) the robot would not be touching the cube however would curve around the cube around the 1.5 grid block mark.

    import copy
    import actionlib
    import rospy
    import time

    from math import sin, cos
    from moveit_python import (MoveGroupInterface,
                               PlanningSceneInterface,
                               PickPlaceInterface)
    from moveit_python.geometry import rotate_pose_msg_by_euler_angles

    from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
    from control_msgs.msg import PointHeadAction, PointHeadGoal
    from grasping_msgs.msg import FindGraspableObjectsAction, FindGraspableObjectsGoal
    from geometry_msgs.msg import PoseStamped
    from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
    from moveit_msgs.msg import PlaceLocation, MoveItErrorCodes
    from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

    # Move base using navigation stack
    class MoveBaseClient(object):

        def __init__(self):
            self.client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
            rospy.loginfo("Waiting for move_base...")
            self.client.wait_for_server()

        def goto(self, x, y, theta, frame="map"):
            move_goal = MoveBaseGoal()
            move_goal.target_pose.pose.position.x = x
            move_goal.target_pose.pose.position.y = y
            move_goal.target_pose.pose.orientation.z = sin(theta/2.0)
            move_goal.target_pose.pose.orientation.w = cos(theta/2.0)
            move_goal.target_pose.header.frame_id = frame
            move_goal.target_pose.header.stamp = rospy.Time.now()

            # TODO wait for things to work
            self.client.send_goal(move_goal)
            self.client.wait_for_result()


if __name__ == "__main__":
    # Create a node
    rospy.init_node("demo")

    # Make sure sim time is working
    while not rospy.Time.now():
        pass

    # Setup clients
    move_base = MoveBaseClient()
    """
    torso_action = FollowTrajectoryClient("torso_controller", ["torso_lift_joint"])
    head_action = PointHeadClient()
    grasping_client = GraspingClient()
    """
    # Move the base to be in front of the table
    # Demonstrates the use of the navigation stack
    rospy.loginfo("Moving to elevator...")
    time.sleep(1)
    move_base.goto(1.0,0,0)

Complete python file

edit retag flag offensive close merge delete