|
| 1 | +<?xml version="1.0"?> |
| 2 | +<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="OS0-32"> |
| 3 | + <xacro:property name="M_PI" value="3.1415926535897931" /> |
| 4 | + <xacro:macro name="OS0-32" params="*origin parent:=base_link name:=os0_sensor topic_points:=/os0_cloud_node/points topic_imu:=/os0_cloud_node/imu hz:=10 lasers:=32 samples:=512 min_range:=0.9 max_range:=75.0 noise:=0.008 min_angle:=-${M_PI} max_angle:=${M_PI} lidar_link:=os0_lidar imu_link:=os0_imu vfov_min:=-0.785398 vfov_max:=0.785398"> |
| 5 | + |
| 6 | + <joint name="${name}_mount_joint" type="fixed"> |
| 7 | + <xacro:insert_block name="origin" /> |
| 8 | + <parent link="${parent}"/> |
| 9 | + <child link="${name}_base_link"/> |
| 10 | + </joint> |
| 11 | + |
| 12 | + <link name="${name}_base_link" /> |
| 13 | + |
| 14 | + <joint name="${name}_base_to_baseplate" type="fixed"> |
| 15 | + <parent link="${name}_base_link"/> |
| 16 | + <child link="${name}_baseplate" /> |
| 17 | + <origin xyz="0 0 0.008"/> |
| 18 | + </joint> |
| 19 | + |
| 20 | + <link name="${name}_baseplate"> |
| 21 | + <visual> |
| 22 | + <geometry> |
| 23 | + <box size="0.112 0.112 0.016" /> |
| 24 | + </geometry> |
| 25 | + <material name="silver"> |
| 26 | + <color rgba="0.5 0.5 0.5 1.0" /> |
| 27 | + </material> |
| 28 | + </visual> |
| 29 | + <collision> |
| 30 | + <geometry> |
| 31 | + <box size="0.112 0.112 0.016" /> |
| 32 | + </geometry> |
| 33 | + </collision> |
| 34 | + </link> |
| 35 | + |
| 36 | + <joint name="${name}_baseplate_to_body" type="fixed"> |
| 37 | + <parent link="${name}_baseplate"/> |
| 38 | + <child link="${name}"/> |
| 39 | + <origin xyz="0 0 0.042" rpy="0 0 0" /> |
| 40 | + </joint> |
| 41 | + |
| 42 | + <link name="${name}"> |
| 43 | + <inertial> |
| 44 | + <mass value="0.33"/> |
| 45 | + <origin xyz="0 0 0.0365" rpy="0 0 0" /> |
| 46 | + <inertia ixx="0.000241148" ixy="0" ixz="0" |
| 47 | + iyy="0.000241148" iyz="0" izz="0.000264"/> |
| 48 | + </inertial> |
| 49 | + <collision name="base_collision"> |
| 50 | + <origin xyz="0 0 0.0365" rpy="0 0 0" /> |
| 51 | + <geometry> |
| 52 | + <cylinder radius="0.04" length="0.073"/> |
| 53 | + </geometry> |
| 54 | + </collision> |
| 55 | + <visual name="base_visual"> |
| 56 | + <origin xyz="0 0 0.0" rpy="0 0 1.5707" /> |
| 57 | + <geometry> |
| 58 | + <mesh filename="package://ouster_description/meshes/os1_64.dae" /> |
| 59 | + <!-- <cylinder length="0.073" radius="0.04" /> --> |
| 60 | + </geometry> |
| 61 | + </visual> |
| 62 | + </link> |
| 63 | + |
| 64 | + <link name="${imu_link}" /> |
| 65 | + |
| 66 | + <link name="${lidar_link}" /> |
| 67 | + |
| 68 | + |
| 69 | + <joint name="${name}_imu_link_joint" type="fixed"> |
| 70 | + <parent link="${name}" /> |
| 71 | + <child link="${imu_link}" /> |
| 72 | + <origin xyz="0.006253 -0.011775 0.007645" rpy="0 0 0" /> |
| 73 | + </joint> |
| 74 | + <gazebo reference="${imu_link}"> |
| 75 | + </gazebo> |
| 76 | + |
| 77 | + <joint name="${name}_lidar_link_joint" type="fixed"> |
| 78 | + <parent link="${name}" /> |
| 79 | + <child link="${lidar_link}" /> |
| 80 | + <origin xyz="0.0 0.0 0.03618" rpy="0 0 0" /> |
| 81 | + </joint> |
| 82 | + |
| 83 | + <!-- Gazebo requires the ouster_gazebo_plugins package --> |
| 84 | + <gazebo reference="${name}"> |
| 85 | + <sensor type="ray" name="${name}-OS0-32"> |
| 86 | + <pose>0 0 0 0 0 0</pose> |
| 87 | + <visualize>false</visualize> |
| 88 | + <update_rate>${hz}</update_rate> |
| 89 | + <ray> |
| 90 | + <scan> |
| 91 | + <horizontal> |
| 92 | + <samples>${samples}</samples> |
| 93 | + <resolution>1</resolution> |
| 94 | + <min_angle>${min_angle}</min_angle> |
| 95 | + <max_angle>${max_angle}</max_angle> |
| 96 | + </horizontal> |
| 97 | + <vertical> |
| 98 | + <samples>${lasers}</samples> |
| 99 | + <resolution>1</resolution> |
| 100 | + <min_angle>${vfov_min}</min_angle> |
| 101 | + <max_angle>${vfov_max}</max_angle> |
| 102 | + </vertical> |
| 103 | + </scan> |
| 104 | + <range> |
| 105 | + <min>${min_range}</min> |
| 106 | + <max>${max_range}</max> |
| 107 | + <resolution>0.03</resolution> |
| 108 | + </range> |
| 109 | + </ray> |
| 110 | + <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_ouster_laser.so"> |
| 111 | + <topicName>${topic_points}</topicName> |
| 112 | + <frameName>${lidar_link}</frameName> |
| 113 | + <min_range>${min_range}</min_range> |
| 114 | + <max_range>${max_range}</max_range> |
| 115 | + <gaussianNoise>${noise}</gaussianNoise> |
| 116 | + </plugin> |
| 117 | + </sensor> |
| 118 | + </gazebo> |
| 119 | + |
| 120 | + <!-- IMU --> |
| 121 | + <gazebo> |
| 122 | + <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so"> |
| 123 | + <robotNamespace>/</robotNamespace> |
| 124 | + <updateRate>100.0</updateRate> |
| 125 | + <bodyName>${imu_link}</bodyName> |
| 126 | + <topicName>${topic_imu}</topicName> |
| 127 | + <accelDrift>0.005 0.005 0.005</accelDrift> |
| 128 | + <accelGaussianNoise>0.005 0.005 0.005</accelGaussianNoise> |
| 129 | + <rateDrift>0.005 0.005 0.005 </rateDrift> |
| 130 | + <rateGaussianNoise>0.005 0.005 0.005 </rateGaussianNoise> |
| 131 | + <headingDrift>0.005</headingDrift> |
| 132 | + <headingGaussianNoise>0.005</headingGaussianNoise> |
| 133 | + </plugin> |
| 134 | + </gazebo> |
| 135 | + |
| 136 | + </xacro:macro> |
| 137 | +</robot> |
0 commit comments