<?xml version="1.0"?>
<robot name="cyber_dog" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:arg name="ROBOT" default="cyber_dog" />
    <xacro:arg name="USE_LIDAR" default="true" />
    <xacro:include filename="const.xacro" />
    <xacro:include filename="leg.xacro" />
    <xacro:include filename="gazebo.xacro" />

    <link name="base_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="0.001 0.001 0.001" />
            </geometry>
        </visual>
    </link>

    <joint name="floating_base" type="fixed">
        <origin rpy="0 0 0" xyz="0 0 0" />
        <parent link="base_link" />
        <child link="body" />
    </joint>

    <link name="body">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="file://$(find cyberdog_description)/meshes/body.dae" scale="1 1 1" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="${body_length} ${body_width} ${body_height}" />
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="${body_com_x} ${body_com_y} ${body_com_z}" />
            <mass value="${body_mass}" />
            <inertia ixx="${body_ixx}" ixy="${body_ixy}" ixz="${body_ixz}" iyy="${body_iyy}" iyz="${body_iyz}" izz="${body_izz}" />
        </inertial>
    </link>

    <joint name="imu_joint" type="fixed">
        <parent link="body" />
        <child link="imu_link" />
        <origin rpy="0 0 0" xyz="33.4e-3 -17.2765e-3 51.0469e-3" />
    </joint>

    <link name="imu_link">
        <inertial>
            <mass value="0.001" />
            <origin rpy="0 0 0" xyz="0 0 0" />
            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="0.001 0.001 0.001" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size=".001 .001 .001" />
            </geometry>
        </collision>
    </link>

    <joint name="scan_joint" type="fixed">
        <parent link="body" />
        <child link="lidar_link" />
        <origin rpy="0 0 0" xyz="0.21425 0 0.0908" />
    </joint>

    <link name="lidar_link">    
      <inertial>
            <mass value="0.001" />
            <origin rpy="0 0 0" xyz="0 0 0" />
            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
      </inertial>
    </link>




    <gazebo reference="lidar_link">
    <!-- 添加激光雷达传感器 -->
    <sensor name="realsense" type="ray">
    
        <always_on>true</always_on>
        <visualize>true</visualize>
        <pose>0.0 0 0.0 0 0 0</pose>  <!-- 设置传感器相对位置 -->
        <ray>
            <scan>
                <horizontal>
                    <samples>100</samples> <!-- 水平扫描的样本数 -->
                    <resolution>1.000000</resolution> <!-- 扫描分辨率 -->
                    <min_angle>-1.5700</min_angle> <!-- 最小扫描角度 -->
                    <max_angle>1.5700</max_angle> <!-- 最大扫描角度 -->
                </horizontal>
            </scan>
            <range>
                <min>0.1</min> <!-- 激光雷达的最小测距范围 -->
                <max>12.0</max> <!-- 激光雷达的最大测距范围 -->
                <resolution>0.015000</resolution> <!-- 距离分辨率 -->
            </range>
        

        <noise>
            <type>gaussian</type>
            <mean>0.0</mean>  <!-- 噪声的均值，通常为 0 -->
            <stddev>0.015</stddev>  <!-- 噪声的标准差，值越大，噪声越大 -->
        </noise>
        </ray>


        <plugin name="Cyberdog_laserscan" filename="libgazebo_ros_ray_sensor.so">
            <ros>
                <remapping>~/out:=scan</remapping>
            </ros>

            <output_type>sensor_msg/LaserScan</output_type>
            <frame_name>lidar_link</frame_name>
        </plugin>


    </sensor>
    </gazebo>

    



    <joint name="D435_camera_joint" type="fixed">
        <parent link="body" />
        <child link="D435_camera_link" />
        <origin rpy="0 0 0" xyz="271.994e-3 25e-3 114.912e-3" />
    </joint>

    <link name="D435_camera_link">
        <inertial>
            <mass value="0.001" />
            <origin rpy="0 0 0" xyz="0 0 0" />
            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="0.001 0.001 0.001" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size=".001 .001 .001" />
            </geometry>
        </collision>
    </link>

    <joint name="RGB_camera_joint" type="fixed">
        <parent link="body" />
        <child link="RGB_camera_link" />
        <origin rpy="0 0 0" xyz="275.76e-3 0 125.794e-3" />
    </joint>

    <link name="RGB_camera_link">
        <inertial>
            <mass value="0.001" />
            <origin rpy="0 0 0" xyz="0 0 0" />
            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="0.001 0.001 0.001" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size=".001 .001 .001" />
            </geometry>
        </collision>
    </link>

    <!-- 定义一个Gazebo模型中的相机传感器，连接到名为"RGB_camera_link"的连杆上 -->
    <gazebo reference="RGB_camera_link">
        <!-- 传感器定义：类型为相机，名称为"rgb camera" -->
        <sensor type="camera" name="rgb camera">
            <!-- 设置传感器始终开启（即使没有订阅者也会发布数据） -->
            <always_on>true</always_on>
            <!-- 设置传感器更新频率为15Hz（每秒15帧） -->
            <update_rate>15.0</update_rate>
            
            <!-- 相机具体参数配置 -->
            <camera name="rgb_camera">
                <!-- 设置水平视场角为1.46608弧度（约84度） -->
                <horizontal_fov>1.46608</horizontal_fov>
                
                <!-- 图像参数配置 -->
                <image>
                    <!-- 图像分辨率：宽度320像素 -->
                    <width>1920</width>
                    <!-- 图像分辨率：高度180像素 -->
                    <height>1080</height>
                    <!-- 图像格式：RGB三通道，每通道8位 -->
                    <format>R8G8B8</format>
                </image>
                
                <!-- 镜头畸变参数配置 -->
                <distortion>
                    <!-- 径向畸变系数k1 -->
                    <k1>0.0</k1>
                    <!-- 径向畸变系数k2 -->
                    <k2>0.0</k2>
                    <!-- 径向畸变系数k3 -->
                    <k3>0.0</k3>
                    <!-- 切向畸变系数p1 -->
                    <p1>0.0</p1>
                    <!-- 切向畸变系数p2 -->
                    <p2>0.0</p2>
                    <!-- 畸变中心坐标（归一化坐标，0.5表示图像中心） -->
                    <center>0.5 0.5</center>
                </distortion>
            </camera>
            
            <!-- Gazebo ROS相机插件配置 -->
            <plugin name="rgb_camera_plugin" filename="libgazebo_ros_camera.so">
                <!-- ROS相关配置 -->
                <ros>
                    <!-- 可选的命名空间配置（当前被注释掉） -->
                    <!-- <namespace>stereo</namespace> -->
                    
                    <!-- 重映射话题名称 -->
                    <remapping>~/image_raw:=image_raw</remapping>  <!-- 原始图像数据话题 -->
                    <remapping>~/camera_info:=camera_info</remapping>  <!-- 相机信息话题 -->
                </ros>
                
                <!-- 设置相机名称（ROS中使用），如果为空则默认使用传感器名称 -->
                <camera_name>rgb_camera</camera_name>
                
                <!-- 设置TF坐标系名称，如果为空则默认使用link名称 -->
                <frame_name>RGB_camera_link</frame_name>
                
                <!-- 用于立体相机的基线距离（单位：米），单目相机通常不需要 -->
                <hack_baseline>0.2</hack_baseline>
            </plugin>
        </sensor>
    </gazebo>









    <joint name="AI_camera_joint" type="fixed">
        <parent link="body" />
        <child link="AI_camera_link" />
        <origin rpy="0 0 0" xyz="290.228e-3 0 147.420e-3" />
    </joint>

    <link name="AI_camera_link">
        <inertial>
            <mass value="0.001" />
            <origin rpy="0 0 0" xyz="0 0 0" />
            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size="0.001 0.001 0.001" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <box size=".001 .001 .001" />
            </geometry>
        </collision>
    </link>

    <xacro:leg name="FR" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True">
        <origin rpy="0 0 0" xyz="${abad_offset_x} ${-abad_offset_y} 0" />
    </xacro:leg>

    <xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
        <origin rpy="0 0 0" xyz="${abad_offset_x} ${abad_offset_y} 0" />
    </xacro:leg>

    <xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
        <origin rpy="0 0 0" xyz="${-abad_offset_x} ${-abad_offset_y} 0" />
    </xacro:leg>

    <xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
        <origin rpy="0 0 0" xyz="${-abad_offset_x} ${abad_offset_y} 0" />
    </xacro:leg>

    <!-- This link is only for head collision -->
    <joint name="head_joint" type="fixed">
        <parent link="body" />
        <child link="head" />
        <origin rpy="0 0 0" xyz="0 0 0" />
    </joint>
    <link name="head">
        <collision>
            <origin rpy="0 0 0" xyz="0.256 0 0.120" />
            <geometry>
                <box size="0.076 0.060 0.040" />
            </geometry>
        </collision>
        <collision>
            <origin rpy="0 0 0" xyz="0.225 0 0.150" />
            <geometry>
                <box size="0.020 0.080 0.100" />
            </geometry>
        </collision>
    </link>

</robot>