def _init_global_localization(self):
        """
        Initialize global localization for amcl
        """

        service_name = '/global_localization'
        service_class = Empty
        utils.call_service(service_name, service_class)
Пример #2
0
    def unpause_sim(self):
        """
        Resume the phsics update of gazebo

        """

        service_name = '/gazebo/unpause_physics'
        service_class = Empty
        utils.call_service(service_name, service_class)
Пример #3
0
    def pause_sim(self):
        """
        Pause the physics updates of gazebo

        """

        service_name = '/gazebo/pause_physics'
        service_class = Empty
        utils.call_service(service_name, service_class)
Пример #4
0
    def reset_sim(self):
        """
        Using _reset_type variable's value the corresponding reset service
        of gazebo is called

        """

        rospy.logdebug('GazeboConnection.reset_sim() start')
        if self._reset_type == 'SIMULATION':
            # reset the entire simulation including the time
            service_name = '/gazebo/reset_simulation'
            service_class = Empty
        elif self._reset_type == 'WORLD':
            # reset the model's poses
            service_name = '/gazebo/reset_world'
            service_class = Empty
        else:
            # do nothing
            return
        utils.call_service(service_name, service_class)
    def _check_map_data_is_ready(self):
        """
        Checks map service is operational
        """

        rospy.logdebug(
            'TurtleBot3LocalizeEnv._check_map_data_is_ready() start')
        service_name = '/static_map'
        service_class = GetMap
        msg, _ = utils.call_service(service_name, service_class)

        if msg.map.header.frame_id != self._global_frame_id:
            rospy.logwarn('received map must be in the global frame')

        self._map_data = self.__process_map_msg(msg.map)

        # every time map is received perfrom following
        self._publish_rnd_init_pose()
        self._init_amcl(is_global=True)
Пример #6
0
    def set_model_state(self, model_state):
        """
        Sets the model state in gazebo through service call

        :param gazebo_msgs.msg._ModelState.ModelState model_state: gazebo model pose and twist
        """

        service_name = '/gazebo/set_model_state'
        service_class = SetModelState
        service_req = SetModelStateRequest()
        service_req.model_state = model_state

        response, is_successful = utils.call_service(service_name,
                                                     service_class,
                                                     service_req)
        if is_successful and response.success:
            pass  # do nothing
        else:
            rospy.logwarn(response.status_message)
Пример #7
0
    def spawn_sdf_model(self,
                        model_name: str,
                        initial_pose,
                        robot_namespace: str = '',
                        reference_frame: int = 'world'):
        """
        Spawns a model (*.sdf) to gazebo through service call

        :param str model_name: name of gazebo model to be spawn
               geometry_msgs.msg._Pose.Pose initial_pose: initial pose of model
               str robot_namespace: spawn model under this namespace
               str reference_frame: initial_pose is defined relative to the frame of this model,
                    default is gazebo world frame
        """

        if model_name in self.__current_models:
            rospy.logwarn(
                'model: %s already exists in gazebo so will be respawned',
                model_name)
            self.delete_model(model_name)

        model_path = rospkg.RosPack().get_path('indoor_layouts') + '/models/'
        file_xml = open(model_path + model_name + '/model.sdf')
        # this should be an urdf or gazebo xml
        model_xml = file_xml.read().replace('\n', '')

        service_name = '/gazebo/spawn_sdf_model'
        service_class = SpawnModel
        service_req = SpawnModelRequest()
        service_req.model_name = model_name
        service_req.model_xml = model_xml
        service_req.initial_pose = initial_pose
        service_req.robot_namespace = robot_namespace
        service_req.reference_frame = reference_frame

        response, is_successful = utils.call_service(service_name,
                                                     service_class,
                                                     service_req)
        if is_successful and response.success:
            # add model from tracking list
            self.__current_models.append(model_name)
        else:
            rospy.logwarn(response.status_message)
Пример #8
0
    def spawn_urdf_model(self,
                         model_name: str,
                         initial_pose,
                         robot_namespace: str = '',
                         reference_frame: str = 'world'):
        """
        Spawns a model (*.urdf) to gazebo through service call

        :param str model_name: name of gazebo model to be spawn
               geometry_msgs.msg._Pose.Pose initial_pose: initial pose of model
               str robot_namespace: spawn model under this namespace
               str reference_frame: initial_pose is defined relative to the frame of this model,
                    default is gazebo world frame
        """

        model_path = rospkg.RosPack().get_path(
            'turtlebot3_description') + '/urdf/'
        p = os.popen("rosrun xacro xacro " + model_path + model_name +
                     '_waffle.urdf.xacro')
        # this should be an urdf or gazebo xml
        model_xml = p.read().replace('\n', '')
        p.close()

        service_name = '/gazebo/spawn_urdf_model'
        service_class = SpawnModel
        service_req = SpawnModelRequest()
        service_req.model_name = model_name
        service_req.model_xml = model_xml
        service_req.initial_pose = initial_pose
        service_req.robot_namespace = robot_namespace
        service_req.reference_frame = reference_frame

        response, is_successful = utils.call_service(service_name,
                                                     service_class,
                                                     service_req)
        if is_successful and response.success:
            # add model from tracking list
            self.__current_models.append(model_name)
        else:
            rospy.logwarn(response.status_message)
Пример #9
0
    def get_model_state(self,
                        model_name: str,
                        relative_entity_name: str = 'world'):
        """
        Gets the model state from gazebo through service call

        :param str model_name: name of gazebo model
               str relative_entity_name: return pose and twist relative to this entity,
                    default is gazebo world frame
        :return response received from service call
        """

        service_name = '/gazebo/get_model_state'
        service_class = GetModelState
        service_req = GetModelStateRequest()
        service_req.model_name = model_name
        service_req.relative_entity_name = relative_entity_name

        response, is_successful = utils.call_service(service_name,
                                                     service_class,
                                                     service_req)
        return response
Пример #10
0
    def delete_model(self, model_name: str):
        """
        Delete a model from gazebo through service call

        :param str model_name: name of gazebo model to be deleted
        """

        service_name = '/gazebo/delete_model'
        service_class = DeleteModel
        service_req = DeleteModelRequest()
        service_req.model_name = model_name

        response, is_successful = utils.call_service(service_name,
                                                     service_class,
                                                     service_req)
        if is_successful and response.success:
            # remove model from tracking list
            if model_name in self.__current_models:
                self.__current_models.remove(model_name)
        else:
            rospy.logwarn(response.status_message)
        time.sleep(0.2)  # wait for some time