def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        self.scene = ExtendedPlanningSceneInterface()
        """
        :type self.robot :moveit_commander.RobotCommander
        """
        self.robot = moveit_commander.RobotCommander()

        #model_rec_manager for all the objects in the enviornment
        self.model_manager = ModelRecManager()
        #a cache of all of the object names in the enviorment, for use with remove_all_objects
        self.body_name_cache = []

        self.add_box_server = rospy.Service('moveit_trajectory_planner/add_box',
                                            BoxInfo,
                                            self.handle_add_box)

        self.add_autoscaled_mesh_server = rospy.Service('moveit_trajectory_planner/add_autoscaled_mesh',
                                                        MeshInfo,
                                                        self.handle_add_autoscaled_mesh)

        self.remove_object_server = rospy.Service('moveit_trajectory_planner/remove_object',
                                                  ObjectName,
                                                  self.handle_remove_object)

        self.refresh_model_list_server = rospy.Service('model_manager/refresh_model_list',
                                                       Empty,
                                                       self.refresh_model_list)

        self.reload_model_list_server = rospy.Service('model_manager/reload_model_list',
                                                      Empty,
                                                      self.return_current_model_list)
        self.planning_scene_service = rospy.ServiceProxy("/get_planning_scene", moveit_msgs.srv.GetPlanningScene)
        self.update_planning_scene_from_moveit()
        self.force_use_moveit = rospy.get_param("use_moveit", 1)

        self._run_recognition_as = actionlib.SimpleActionServer("recognize_objects_action",
                                                                graspit_msgs.msg.RunObjectRecognitionAction,
                                                                execute_cb=self._run_recognition_as_cb,
                                                                auto_start=False)
        self._run_recognition_as.start()
Beispiel #2
0
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)

        self.NEW_MODEL_REC = True
        self.scene = ExtendedPlanningSceneInterface()
        self.robot = moveit_commander.RobotCommander()

        self.model_manager = ModelRecManager(self.NEW_MODEL_REC)

        self.planning_scene_service_proxy = rospy.ServiceProxy("/get_planning_scene", moveit_msgs.srv.GetPlanningScene)

        self._run_recognition_as = actionlib.SimpleActionServer("recognize_objects_action",
                                                                graspit_msgs.msg.RunObjectRecognitionAction,
                                                                execute_cb=self._run_recognition_as_cb,
                                                                auto_start=False)
        self._run_recognition_as.start()
        rospy.loginfo("World Manager Node is Up and Running")
class WorldManager:

    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        self.scene = ExtendedPlanningSceneInterface()
        """
        :type self.robot :moveit_commander.RobotCommander
        """
        self.robot = moveit_commander.RobotCommander()

        #model_rec_manager for all the objects in the enviornment
        self.model_manager = ModelRecManager()
        #a cache of all of the object names in the enviorment, for use with remove_all_objects
        self.body_name_cache = []

        self.add_box_server = rospy.Service('moveit_trajectory_planner/add_box',
                                            BoxInfo,
                                            self.handle_add_box)

        self.add_autoscaled_mesh_server = rospy.Service('moveit_trajectory_planner/add_autoscaled_mesh',
                                                        MeshInfo,
                                                        self.handle_add_autoscaled_mesh)

        self.remove_object_server = rospy.Service('moveit_trajectory_planner/remove_object',
                                                  ObjectName,
                                                  self.handle_remove_object)

        self.refresh_model_list_server = rospy.Service('model_manager/refresh_model_list',
                                                       Empty,
                                                       self.refresh_model_list)

        self.reload_model_list_server = rospy.Service('model_manager/reload_model_list',
                                                      Empty,
                                                      self.return_current_model_list)
        self.planning_scene_service = rospy.ServiceProxy("/get_planning_scene", moveit_msgs.srv.GetPlanningScene)
        self.update_planning_scene_from_moveit()
        self.force_use_moveit = rospy.get_param("use_moveit", 1)

        self._run_recognition_as = actionlib.SimpleActionServer("recognize_objects_action",
                                                                graspit_msgs.msg.RunObjectRecognitionAction,
                                                                execute_cb=self._run_recognition_as_cb,
                                                                auto_start=False)
        self._run_recognition_as.start()



    def update_planning_scene_from_moveit(self):
        """ Update the current cached scene directly from moveit.
            Returns true if it the service call succeeds. False implies moveit hasn't started correctly.
        """
        try:
            rospy.wait_for_service("/get_planning_scene", 5)
            components = PlanningSceneComponents(PlanningSceneComponents.WORLD_OBJECT_NAMES + PlanningSceneComponents.TRANSFORMS)
            """
            :type ps_response :moveit_msgs.srv.GetPlanningSceneResponse
            """
            ps_request = moveit_msgs.srv.GetPlanningSceneRequest(components=components)
            ps_response = self.planning_scene_service.call(ps_request)

            self.body_name_cache = [co.id for co in ps_response.scene.world.collision_objects]
            rospy.loginfo("body name cache:%s"%(', '.join(self.body_name_cache)))
            return True
        except Exception as e:

            raise e
            rospy.logerror("Failed to find service /get_planning_scene %s and force use moveit enabled"%(e))
            rospy.shutdown()
            rospy.logwarn("Failed to find service /get_planning_scene %s"%(e))
            return False

    def handle_add_box(self, req):
        box_dimensions = (req.sizeX, req.sizeY, req.sizeZ)
        self.scene.add_box(req.name, req.pose, box_dimensions)
        return BoxInfoResponse()

    def handle_add_autoscaled_mesh(self, req):
        """
        Adds a mesh, but makes sure that the mesh is scaled to meters if given a mesh that
        is in millimeters.
        """
        if os.path.isfile(req.filename):
            self.scene.add_mesh_autoscaled(req.name, req.pose, req.filename)
            rospy.loginfo(self.__class__.__name__ + '::handle_add_autoscaled_mesh::name -- %s filename -- %s'%(req.name, req.filename) )
        else:
            rospy.logwarn('File doesn\'t exist - object %s, filename %s'%(req.name, req.filename))

        return MeshInfoResponse()

    def handle_remove_object(self, req):
        self.scene.remove_world_object(req.name)
        return ObjectNameResponse()

    def refresh_model_list(self, empty_msg):

        self.model_manager.refresh()

        #self.remove_object_publisher.publish('ALL') #commented lines for graspit
        #self.publish_table_models()
        self.remove_all_objects_from_planner()
        self.add_all_objects_to_planner()
        #need to return [] for empty response
        return []

    def _run_recognition_as_cb(self, goal):
        rospy.loginfo("Received Run Recognition Goal")

        self.remove_all_objects_from_planner()
        self.model_manager.refresh()

        #self.remove_object_publisher.publish('ALL') #commented lines for graspit
        #self.publish_table_models()

        self.add_all_objects_to_planner()
        #need to return [] for empty response.
        _result = trajectory_planner_msgs.msg.RunObjectRecognitionResult()
        for model in self.model_manager.model_list:
            model_name = model.model_name
            object_name = model.object_name
            object_pose = model.get_world_pose()
            object_info = graspit_msgs.msg.ObjectInfo(object_name, model_name, object_pose)
            rospy.loginfo("Object Info: " + str(object_info))
            _result.object_info.append(object_info)

        self._run_recognition_as.set_succeeded(_result)
        return []


    def return_current_model_list(self, empty_msg):
        #self.model_manager.read()

        #self.remove_object_publisher.publish('ALL') #commented lines for graspit
        #self.publish_table_models()
        if not len(self.model_manager.model_list):
            self.refresh_model_list(empty_msg)
            return []
        self.remove_all_objects_from_planner()
        self.add_all_objects_to_planner()
        return []

    def get_model_list(self, empty_msg):
        self.world_manager.read()

    def remove_all_objects_from_planner(self):
        """
        @brief - Clears all models from moveit enivornment (from name cache)

        FIXME - Add additional obstacles for camera post and computers around robot
        and some way of annotating that they shouldn't be removed.
        """
        # update the body name cache
        self.update_planning_scene_from_moveit()

        for index in range(len(self.body_name_cache)):
            body_name = self.body_name_cache[index]
            rospy.logwarn("removing body: body_name %s"%(body_name))
            self.scene.remove_world_object(body_name)
            #del self.body_name_cache[index]
            #del body_name
        self.body_name_cache = []

        # read the obstacles such as the table and any safety walls
        self.add_obstacles()

    def add_all_objects_to_planner(self):
        """
        @brief - Adds all of the models in the model_rec_manager to moveit enviornment and adds names to cache
        """
        # Clear moveit's model list
        for model in self.model_manager.model_list:
            model_name = model.model_name.strip('/')
            filename = file_name_dict[model_name]
            rospy.logwarn('model name: {}, filename: {}'.format(model_name, filename))
            if os.path.isfile(filename):
                stampedModelPose = geometry_msgs.msg.PoseStamped()
                stampedModelPose.header.frame_id = "/world"  #"/camera_link" #self.robot.get_planning_frame()
                rospy.loginfo(self.__class__.__name__ +
                              ':: Adding model %s -- frame_id %s -- '%(model_name, stampedModelPose.header.frame_id) +
                              ' filename %s '%(filename))


                stampedModelPose.pose = model.get_world_pose()
                self.scene.add_mesh_autoscaled(model.object_name, stampedModelPose, filename)

                #self.scene.remove_world_object(model.object_name)
            else:
                rospy.logwarn('File doesn\'t exist - object %s, filename %s'%(model.object_name, filename))


    def add_table(self):

        time.sleep(1)
        rospy.wait_for_service('moveit_trajectory_planner/add_box')
        frame_id = "/world"
        rospy.loginfo("adding table in planning frame: " + str(frame_id))
        box_pose = geometry_msgs.msg.PoseStamped()
        box_pose.header.frame_id = frame_id
        table_x = rospy.get_param('/table_x', 1.45)
        table_y = rospy.get_param('/table_y', .74)
        table_z = rospy.get_param('/table_z', .05)
        table_world_x_offset = rospy.get_param('/table_world_x_offset', -.05)
        table_world_y_offset = rospy.get_param('/table_world_y_offset', -.05)
        table_world_z_offset = rospy.get_param('/table_world_z_offset', -.0525)
        box_pose.pose.position.x = table_world_x_offset
        box_pose.pose.position.y = table_world_y_offset
        box_pose.pose.position.z = table_world_z_offset
        box_pose.pose.orientation.x = 0
        box_pose.pose.orientation.y = 0
        box_pose.pose.orientation.z = 0
        box_pose.pose.orientation.w = 0
        box_dimensions = (table_x, table_y, table_z)

        self.scene.attach_box(world_manager.robot.get_link_names()[0], "table", box_pose, box_dimensions)
        rospy.loginfo("table added")


    def add_mico_base(self):

        time.sleep(1)
        rospy.wait_for_service('moveit_trajectory_planner/add_box')
        frame_id = "/root"
        rospy.loginfo("adding table in planning frame: " + str(frame_id))
        box_pose = geometry_msgs.msg.PoseStamped()
        box_pose.header.frame_id = frame_id
        base_x = rospy.get_param('/base_x', 0.2)
        base_y = rospy.get_param('/base_y', 0.15)
        base_z = rospy.get_param('/base_z', 0.01)
        base_robot_x_offset = rospy.get_param('/base_robot_x_offset', 0)
        base_robot_y_offset = rospy.get_param('/base_robot_y_offset', 0)
        base_robot_z_offset = rospy.get_param('/base_robot_z_offset', -0.02)
        box_pose.pose.position.x = base_robot_x_offset
        box_pose.pose.position.y = base_robot_y_offset
        box_pose.pose.position.z = base_robot_z_offset
        box_pose.pose.orientation.x = 0
        box_pose.pose.orientation.y = 0
        box_pose.pose.orientation.z = 0
        box_pose.pose.orientation.w = 0
        box_dimensions = (base_x, base_y, base_z)

        self.scene.attach_box(world_manager.robot.get_link_names()[1], "robot_base", box_pose, box_dimensions)
        rospy.loginfo("base added")

    def add_walls(self):

        back_wall_pose = geometry_msgs.msg.PoseStamped()
        back_wall_pose.header.frame_id = '/staubli_rx60l_link1'
        wall_dimensions = [0.92, 1.22, 0.05]
        back_wall_pose.pose.position = geometry_msgs.msg.Point(**{'x': 0.35, 'y': -0.09, 'z': -0.22})
        back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': -0.7128395185,
                                                                          'y': 0.0414512120285,
                                                                          'z': 0.699774446448,
                                                                          'w': 0.0213855555098})
        left_wall_pose = geometry_msgs.msg.PoseStamped()
        left_wall_pose.header.frame_id = '/staubli_rx60l_link1'
        left_wall_dimensions =  [0.92, 1.22, 0.05]
        left_wall_pose.pose.position = geometry_msgs.msg.Point(**{'x': -0.56,
                                                                  'y': -0.91,
                                                                  'z': -0.09})

        left_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': -0.482199130451,
                                                                          'y': 0.516804171535,
                                                                          'z': 0.487346836672,
                                                                          'w': 0.512728493124})

        right_wall_pose = geometry_msgs.msg.PoseStamped()
        right_wall_pose.header.frame_id = '/staubli_rx60l_link1'
        right_wall_dimensions =  [0.92, 1.22, 0.05]
        right_wall_pose.pose.position = geometry_msgs.msg.Point(**{'x': -0.6,
                                                                   'y': 0.68,
                                                                   'z': -0.31})

        right_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': -0.50728105048,
                                                                           'y': 0.487102614313,
                                                                           'z': 0.482034934632,
                                                                           'w': 0.522531626553})
        self.scene.add_box("left_wall", left_wall_pose, wall_dimensions)
        self.scene.add_box("right_wall", right_wall_pose, wall_dimensions)
        self.scene.add_box("back_wall", back_wall_pose, wall_dimensions)


    def add_obstacles(self):
        self.add_table()
        self.add_walls()
Beispiel #4
0
class WorldManager:
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)

        self.NEW_MODEL_REC = True
        self.scene = ExtendedPlanningSceneInterface()
        self.robot = moveit_commander.RobotCommander()

        self.model_manager = ModelRecManager(self.NEW_MODEL_REC)

        self.planning_scene_service_proxy = rospy.ServiceProxy("/get_planning_scene", moveit_msgs.srv.GetPlanningScene)

        self._run_recognition_as = actionlib.SimpleActionServer("recognize_objects_action",
                                                                graspit_msgs.msg.RunObjectRecognitionAction,
                                                                execute_cb=self._run_recognition_as_cb,
                                                                auto_start=False)
        self._run_recognition_as.start()
        rospy.loginfo("World Manager Node is Up and Running")

    def _run_recognition_as_cb(self, goal):
        print("_run_recognition_as_cb")

        print("about to remove_all_objects_from_planner()")
        self.remove_all_objects_from_planner()
        print("finished remove_all_objects_from_planner()")

        self.model_manager.refresh()

        print("about to add_all_objects_to_planner()")
        self.add_all_objects_to_planner()
        print("finished add_all_objects_to_planner()")

        _result = graspit_msgs.msg.RunObjectRecognitionResult()
        print("graspit_msgs.msg.RunObjectRecognitionResult()")

        for model in self.model_manager.model_list:
            object_info = graspit_msgs.msg.ObjectInfo(model.object_name, model.model_name, model.get_world_pose())
            _result.object_info.append(object_info)
        print("finished for loop")

        self._run_recognition_as.set_succeeded(_result)
        return []

    def get_body_names_from_planner(self):
        rospy.wait_for_service("/get_planning_scene", 5)
        components = PlanningSceneComponents(
            PlanningSceneComponents.WORLD_OBJECT_NAMES + PlanningSceneComponents.TRANSFORMS)

        ps_request = moveit_msgs.srv.GetPlanningSceneRequest(components=components)
        ps_response = self.planning_scene_service_proxy.call(ps_request)

        body_names = [co.id for co in ps_response.scene.world.collision_objects]

        return body_names

    def remove_all_objects_from_planner(self):

        body_names = self.get_body_names_from_planner()

        while len(body_names) > 0:
            print("removing bodies from the planner, this can potentially take several tries")
            for body_name in body_names:
                self.scene.remove_world_object(body_name)

            body_names = self.get_body_names_from_planner()

    def add_all_objects_to_planner(self):
        self.add_obstacles()
        for model in self.model_manager.model_list:
            model_name = model.model_name.strip('/')
            print "Adding " + str(model_name) + "To Moveit"
            filename = file_name_dict[model_name]
            if os.path.isfile(filename):

                stamped_model_pose = geometry_msgs.msg.PoseStamped()
                stamped_model_pose.header.frame_id = "/world"
                stamped_model_pose.pose = model.get_world_pose()

                self.scene.add_mesh_autoscaled(model.object_name, stamped_model_pose, filename)

            else:
                rospy.logwarn('File doesn\'t exist - object %s, filename %s' % (model.object_name, filename))

    def add_table(self):

        frame_id = "/root"
        rospy.loginfo("adding table in planning frame: " + str(frame_id))
        box_pose = geometry_msgs.msg.PoseStamped()
        box_pose.header.frame_id = frame_id
        table_x = rospy.get_param('/table_x', 1.45)
        table_y = rospy.get_param('/table_y', .74)
        table_z = rospy.get_param('/table_z', .05)
        table_world_x_offset = rospy.get_param('/table_world_x_offset', -.05)
        table_world_y_offset = rospy.get_param('/table_world_y_offset', -.05)
        table_world_z_offset = rospy.get_param('/table_world_z_offset', -.0525)
        box_pose.pose.position.x = table_world_x_offset
        box_pose.pose.position.y = table_world_y_offset
        box_pose.pose.position.z = table_world_z_offset
        box_pose.pose.orientation.x = 0
        box_pose.pose.orientation.y = 0
        box_pose.pose.orientation.z = 0
        box_pose.pose.orientation.w = 0
        box_dimensions = (table_x, table_y, table_z)

        self.scene.attach_box(world_manager.robot.get_link_names()[0], "table", box_pose, box_dimensions)
        rospy.loginfo("table added")

    def add_base(self):

        time.sleep(1)

        frame_id = "/root"
        rospy.loginfo("adding table in planning frame: " + str(frame_id))
        box_pose = geometry_msgs.msg.PoseStamped()
        box_pose.header.frame_id = frame_id
        base_x = rospy.get_param('/base_x', 0.2)
        base_y = rospy.get_param('/base_y', 0.15)
        base_z = rospy.get_param('/base_z', 0.01)
        base_robot_x_offset = rospy.get_param('/base_robot_x_offset', 0)
        base_robot_y_offset = rospy.get_param('/base_robot_y_offset', 0)
        base_robot_z_offset = rospy.get_param('/base_robot_z_offset', -0.02)
        box_pose.pose.position.x = base_robot_x_offset
        box_pose.pose.position.y = base_robot_y_offset
        box_pose.pose.position.z = base_robot_z_offset
        box_pose.pose.orientation.x = 0
        box_pose.pose.orientation.y = 0
        box_pose.pose.orientation.z = 0
        box_pose.pose.orientation.w = 0
        box_dimensions = (base_x, base_y, base_z)

        self.scene.attach_box(world_manager.robot.get_link_names()[1], "robot_base", box_pose, box_dimensions)
        rospy.loginfo("base added")

    def add_walls(self):

        back_wall_pose = geometry_msgs.msg.PoseStamped()
        back_wall_pose.header.frame_id = '/world'
        wall_dimensions = [1.45, .05, .5]
        back_wall_pose.pose.position = geometry_msgs.msg.Point(**{'x': .35, 'y': 0.2, 'z': 0.28})
        back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': 0,
                                                                          'y': 0,
                                                                          'z': 0,
                                                                          'w': 0})

        self.scene.add_box("back_wall", back_wall_pose, wall_dimensions)

    def add_bin_wall(self, w, d, h, x, y, z, wall_name):
        time.sleep(1)
        bin_pose = geometry_msgs.msg.PoseStamped()
        bin_pose.header.frame_id = '/root'
        bin_dimensions = [w, d, h]
        bin_pose.pose.position = geometry_msgs.msg.Point(**{'x': x, 'y': y, 'z': z})
        bin_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': 0,
                                                                         'y': 0,
                                                                         'z': 0,
                                                                         'w': 0})
        self.scene.add_box(wall_name, bin_pose, bin_dimensions)

    def add_bin(self):

        # back_bin_pose = geometry_msgs.msg.PoseStamped()
        # back_bin_pose.header.frame_id = '/world'
        # back_bin_dimensions = [0.5, .01, .2]
        # back_bin_pose.pose.position = geometry_msgs.msg.Point(**{'x': .62, 'y': -0.29, 'z': 0.16})
        # back_bin_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': 0,
        #                                                                  'y': 0,
        #                                                                  'z': 0,
        #                                                                  'w': 0})
        # right_bin_pose = geometry_msgs.msg.PoseStamped()
        # right_bin_pose.header.frame_id = '/world'
        # right_bin_dimensions = [0.01, .25, .2]
        # right_bin_pose.pose.position = geometry_msgs.msg.Point(**{'x': .36, 'y': -0.415, 'z': 0.16})
        # right_bin_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': 0,
        #                                                                   'y': 0,
        #                                                                   'z': 0,
        #                                                                   'w': 0})

        # self.scene.add_box("back_bin_wall", back_bin_pose, back_bin_dimensions)
        # self.scene.add_box("right_bin_wall", right_bin_pose, right_bin_dimensions)
        h = 0.085
        z = h/2
        l_1 = 0.537
        l_2 = 0.254
        thickness = 0.01
        x = -0.1
        y = -l_1 / 2

        self.add_bin_wall(thickness, l_2, h,
                         y, x - l_2/2, z, "left_bin_wall")
        self.add_bin_wall(thickness, l_2, h,
                          y + l_1, x - l_2/2, z, "right_bin_wall")
        self.add_bin_wall(l_1, thickness, h,
                          y + l_1/2, x - l_2 - thickness, z, "top_bin_wall")
        self.add_bin_wall(l_1, thickness, h,
                          y + l_1/2, x + thickness, z, "bottom_bin_wall")

        self.add_bin_wall(thickness, 0.254, 0.152,
                         y + l_1/2,  x - l_2/2, 0.076, "center_bin_wall")

    def add_obstacles(self):
        self.add_table()
        experiment_type = rospy.get_param('~experiment_type')
        if experiment_type == "block":
            self.add_bin()