Ardupilot_Multiagent_Simulation/src/multiagent_simulation/models/lidar/model.xacro at main · aau-cns/Ardupilot_Multiagent_Simulation · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
<?xml version="1.0"?>
<sdf version="1.9" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <model name="lidars">
    <static>true</static>
    <link name="lidars_link">
      <pose>0 0 0 0 0 0</pose>
      <kinematic>true</kinematic>
      <gravity>false</gravity>
      <inertial>
        <mass>0.0</mass>
        <inertia>
          <ixx>0.0</ixx>
          <ixy>0.0</ixy>
          <iyy>0.0</iyy>
          <iyz>0.0</iyz>
          <izz>0.0</izz>
        </inertia>
      </inertial>
    </link>

    <!-- Macro Definition for LiDAR -->
    <xacro:macro name="lidar_sensor"
      params="name pose horizontal_fov vertical_fov horizontal_samples vertical_samples update_rate">
      <link name="${name}_link">
        <pose>${pose}</pose>
        <kinematic>true</kinematic>
        <gravity>false</gravity>

        <inertial>
          <mass>0.0</mass>
          <inertia>
            <ixx>0.0</ixx>
            <ixy>0.0</ixy>
            <iyy>0.0</iyy>
            <iyz>0.0</iyz>
            <izz>0.0</izz>
          </inertia>
        </inertial>

        <visual name="visual">
          <geometry>
            <box>
              <size>0.05 0.05 0.05</size>
            </box>
          </geometry>
        </visual>

        <visual name="landmark">
          <geometry>
            <cylinder>
              <radius>0.01</radius>
              <length>0.005</length>
            </cylinder>
          </geometry>
          <pose>0.03 0 0 0 1.5708 0</pose>
          <material>
            <ambient>1 0 0 1</ambient>
            <diffuse>1 0 0 1</diffuse>
          </material>
        </visual>

        <sensor name="${name}" type="gpu_lidar">
          <gz_frame_id>${name}_link</gz_frame_id>
          <pose>0 0 0 0 0 0</pose>
          <update_rate>${update_rate}</update_rate>
          <lidar>
            <scan>
              <horizontal>
                <samples>${horizontal_samples}</samples>
                <resolution>1</resolution>
                <min_angle>-${horizontal_fov}</min_angle>
                <max_angle>${horizontal_fov}</max_angle>
              </horizontal>
              <vertical>
                <samples>${vertical_samples}</samples>
                <resolution>1</resolution>
                <min_angle>-${vertical_fov}</min_angle>
                <max_angle>${vertical_fov}</max_angle>
              </vertical>
            </scan>
            <range>
              <min>0.45</min>
              <max>50.0</max> <!--100m range is in ideal. In reality more like 50 m for most surfaces-->
              <resolution>0.001</resolution>
            </range>
            <noise>
              <type>gaussian</type>
              <mean>0.0</mean>
              <stddev>0.01</stddev>
              <bias_mean>0.0</bias_mean>
              <bias_stddev>0.001</bias_stddev>
            </noise>
          </lidar>
          <visualize>false</visualize>
        </sensor>
      </link>

      <joint name="${name}_joint" type="fixed">
        <parent>lidars_link</parent>
        <child>${name}_link</child>
        <preserve_fixed_joint>true</preserve_fixed_joint>
      </joint>
    </xacro:macro>

    <!-- Add LiDAR sensors -->
    <xacro:lidar_sensor name="lidar_1" pose="0.12 0.02 0.0 0 1.57 0" horizontal_fov="3.14159"
      vertical_fov="0.785398" horizontal_samples="512" vertical_samples="128" update_rate="20" />
  </model>
</sdf>