gazebo_world_manager/gazebo_ctrl.py

import rospy
from gazebo_msgs.srv import (GetLinkProperties, GetModelProperties,
                             GetModelPropertiesResponse, GetModelState,
                             GetModelStateResponse, GetWorldProperties)
from std_srvs.srv import Empty


class GazeboROSController:
    def __init__(self):
        rospy.init_node('gazebo_ros_controller', anonymous=True)
        self.model_info = None

    def get_model_names(self) -> list:
        rospy.wait_for_service('/gazebo/get_world_properties')
        try:
            get_world_properties = rospy.ServiceProxy(
                '/gazebo/get_world_properties', GetWorldProperties)
            world_properties = get_world_properties()
            return world_properties.model_names
        except rospy.ServiceException as e:
            rospy.logerr(f"Service call failed: {e}")
            return []

    def get_model_state(self, model_names: list = None) -> dict:
        """
        获取模型的状态。
        如果model_names为空，返回所有模型的状态，否则返回指定模型的状态。
        """
        if not model_names or len(model_names) == 0:
            model_names = self.get_model_names()
        print(f"Getting state for models: {model_names}")
        model_states = {}
        for name in model_names:
            rospy.wait_for_service('/gazebo/get_model_state')
            try:
                get_model_state = rospy.ServiceProxy(
                    '/gazebo/get_model_state', GetModelState)
                state = get_model_state(name, '')
                model_states[name] = state
            except rospy.ServiceException as e:
                rospy.logerr(f"Service call failed for model {name}: {e}")
                model_states[name] = None
        return model_states

    def get_model_properties(self, model_names: list = None) -> dict:
        """
        获取模型的属性。
        如果model_names为空，返回所有模型的属性，否则返回指定模型的属性。
        """
        if not model_names or len(model_names) == 0:
            model_names = self.get_model_names()
        print(f"Getting properties for models: {model_names}")
        model_properties = {}
        for name in model_names:
            rospy.wait_for_service('/gazebo/get_model_properties')
            try:
                get_model_properties = rospy.ServiceProxy(
                    '/gazebo/get_model_properties', GetModelProperties)
                properties = get_model_properties(name)
                model_properties[name] = properties
            except rospy.ServiceException as e:
                rospy.logerr(f"Service call failed for model {name}: {e}")
                model_properties[name] = None
        return model_properties

    def get_link_properties(self, link_full_name: str) -> dict:
        """
        获取指定链接的属性。
        """
        rospy.wait_for_service('/gazebo/get_link_properties')
        try:
            get_link_properties = rospy.ServiceProxy(
                '/gazebo/get_link_properties', GetLinkProperties)
            properties = get_link_properties(link_full_name)
            return properties
        except rospy.ServiceException as e:
            rospy.logerr(f"Service call failed for link {link_full_name}: {e}")
            return None

    def format_model_state(self, state: GetModelStateResponse) -> dict:
        return {
            'pose': {
                'position': {
                    'x': state.pose.position.x,
                    'y': state.pose.position.y,
                    'z': state.pose.position.z
                },
                'orientation': {
                    'x': state.pose.orientation.x,
                    'y': state.pose.orientation.y,
                    'z': state.pose.orientation.z,
                    'w': state.pose.orientation.w
                }
            },
            'twist': {
                'linear': {
                    'x': state.twist.linear.x,
                    'y': state.twist.linear.y,
                    'z': state.twist.linear.z
                },
                'angular': {
                    'x': state.twist.angular.x,
                    'y': state.twist.angular.y,
                    'z': state.twist.angular.z
                }
            }}

    def format_model_properties(self, properties: GetModelPropertiesResponse) -> dict:
        return {
            'parent_model_name': properties.parent_model_name,
            'canonical_body_name': properties.canonical_body_name,
            'body_names': properties.body_names,
            'geom_names': properties.geom_names,
            'joint_names': properties.joint_names,
            'child_model_names': properties.child_model_names,
            'is_static': properties.is_static
        }

    def format_model_info(self, model_name: str, state: GetModelStateResponse, properties: GetModelPropertiesResponse) -> dict:
        return {
            'name': model_name,
            'state': self.format_model_state(state),
            'properties': self.format_model_properties(properties)
        }

    def get_model_info_f(self, model_names: list = None) -> list:
        """
        获取模型的详细信息，包括状态和属性。
        如果model_names为空，返回所有模型的信息，否则返回指定模型的信息。
        返回值已经格式化为list-dict形式。
        """
        if not model_names or len(model_names) == 0:
            model_names = self.get_model_names()
        print(f"Getting info for models: {model_names}")
        model_info = []
        states = self.get_model_state(model_names)
        properties = self.get_model_properties(model_names)
        for name in model_names:
            m_state = states[name]
            m_properties = properties[name]
            info = self.format_model_info(name, m_state, m_properties)
            model_info.append(info)
        return model_info

    def get_model_state_f(self, model_names: list = None) -> list:
        """
        获取模型的状态。
        如果model_names为空，返回所有模型的信息，否则返回指定模型的信息。
        返回值已经格式化为list-dict形式。
        """
        if not model_names or len(model_names) == 0:
            model_names = self.get_model_names()
        print(f"Getting info for models: {model_names}")
        model_info = []
        states = self.get_model_state(model_names)
        for name in model_names:
            m_state = states[name]
            info = self.format_model_state(m_state)
            info['name'] = name
            # 获取箱子质量
            if name == 'block':
                info['mass'] = self.get_block_mass()
            model_info.append(info)
        return model_info

if __name__ == '__main__':
    controller = GazeboROSController()
    import json
    print(json.dumps(controller.get_model_state_f(['block'])))