|
| 1 | +<?xml version="1.0"?> |
| 2 | +<!-- |
| 3 | +Copied and modified from turtlebot3 example: |
| 4 | +https://github.com/ROBOTIS-GIT/turtlebot3/blob/kinetic-devel/turtlebot3_description/urdf/turtlebot3_waffle_pi_for_open_manipulator.gazebo.xacro |
| 5 | +--> |
| 6 | +<robot xmlns:xacro="http://ros.org/wiki/xacro"> |
| 7 | + |
| 8 | + <xacro:macro name="turtlebot3_waffle_pi_gazebo" params="prefix"> |
| 9 | + |
| 10 | + <xacro:property name="laser_visual" value="true"/> |
| 11 | + <xacro:property name="camera_visual" value="true"/> |
| 12 | + <xacro:property name="imu_visual" value="true"/> |
| 13 | + |
| 14 | + <gazebo reference="${prefix}base_link"> |
| 15 | + <material>Gazebo/DarkGrey</material> |
| 16 | + </gazebo> |
| 17 | + |
| 18 | + <gazebo reference="${prefix}wheel_left_link"> |
| 19 | + <mu1>10000</mu1> |
| 20 | + <mu2>10000</mu2> |
| 21 | + <kp>500000.0</kp> |
| 22 | + <kd>10.0</kd> |
| 23 | + <minDepth>0.001</minDepth> |
| 24 | + <maxVel>0.1</maxVel> |
| 25 | + <fdir1>1 0 0</fdir1> |
| 26 | + <material>Gazebo/FlatBlack</material> |
| 27 | + </gazebo> |
| 28 | + |
| 29 | + <gazebo reference="${prefix}wheel_right_link"> |
| 30 | + <mu1>10000</mu1> |
| 31 | + <mu2>10000</mu2> |
| 32 | + <kp>500000.0</kp> |
| 33 | + <kd>10.0</kd> |
| 34 | + <minDepth>0.001</minDepth> |
| 35 | + <maxVel>0.1</maxVel> |
| 36 | + <fdir1>1 0 0</fdir1> |
| 37 | + <material>Gazebo/FlatBlack</material> |
| 38 | + </gazebo> |
| 39 | + |
| 40 | + <gazebo reference="${prefix}caster_back_right_link"> |
| 41 | + <mu1>0.1</mu1> |
| 42 | + <mu2>0.1</mu2> |
| 43 | + <kp>1000000.0</kp> |
| 44 | + <kd>100.0</kd> |
| 45 | + <minDepth>0.001</minDepth> |
| 46 | + <maxVel>1.0</maxVel> |
| 47 | + <material>Gazebo/FlatBlack</material> |
| 48 | + </gazebo> |
| 49 | + |
| 50 | + <gazebo reference="${prefix}caster_back_left_link"> |
| 51 | + <mu1>0.1</mu1> |
| 52 | + <mu2>0.1</mu2> |
| 53 | + <kp>1000000.0</kp> |
| 54 | + <kd>100.0</kd> |
| 55 | + <minDepth>0.001</minDepth> |
| 56 | + <maxVel>1.0</maxVel> |
| 57 | + <material>Gazebo/FlatBlack</material> |
| 58 | + </gazebo> |
| 59 | + |
| 60 | + <gazebo reference="${prefix}imu_link"> |
| 61 | + <material>Gazebo/Grey</material> |
| 62 | + <sensor type="imu" name="imu"> |
| 63 | + <always_on>true</always_on> |
| 64 | + <update_rate>100.0</update_rate> |
| 65 | + <visualize>${imu_visual}</visualize> |
| 66 | + </sensor> |
| 67 | + </gazebo> |
| 68 | + |
| 69 | + <gazebo> |
| 70 | + <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'> |
| 71 | + <ros> |
| 72 | + <namespace></namespace> |
| 73 | + <remapping>cmd_vel:=cmd_vel</remapping> |
| 74 | + <remapping>odom:=odom</remapping> |
| 75 | + </ros> |
| 76 | + |
| 77 | + <left_joint>wheel_left_joint</left_joint> |
| 78 | + <right_joint>wheel_right_joint</right_joint> |
| 79 | + <wheel_separation>0.287</wheel_separation> |
| 80 | + <wheel_diameter>0.066</wheel_diameter> |
| 81 | + <max_wheel_torque>20</max_wheel_torque> |
| 82 | + <max_wheel_acceleration>1.0</max_wheel_acceleration> |
| 83 | + <publish_odom>true</publish_odom> |
| 84 | + <publish_odom_tf>true</publish_odom_tf> |
| 85 | + <publish_wheel_tf>false</publish_wheel_tf> |
| 86 | + <odometry_frame>odom</odometry_frame> |
| 87 | + <robot_base_frame>base_footprint</robot_base_frame> |
| 88 | + </plugin> |
| 89 | + </gazebo> |
| 90 | + |
| 91 | + <gazebo reference="${prefix}base_scan"> |
| 92 | + <material>Gazebo/FlatBlack</material> |
| 93 | + <sensor type="ray" name="lds_lfcd_sensor"> |
| 94 | + <pose>0 0 0 0 0 0</pose> |
| 95 | + <always_on>true</always_on> |
| 96 | + <update_rate>5.0</update_rate> |
| 97 | + <visualize>${laser_visual}</visualize> |
| 98 | + <ray> |
| 99 | + <scan> |
| 100 | + <horizontal> |
| 101 | + <samples>360</samples> |
| 102 | + <resolution>1</resolution> |
| 103 | + <min_angle>0.0</min_angle> |
| 104 | + <max_angle>6.28319</max_angle> |
| 105 | + </horizontal> |
| 106 | + </scan> |
| 107 | + <range> |
| 108 | + <min>0.120</min> |
| 109 | + <max>3.5</max> |
| 110 | + <resolution>0.015</resolution> |
| 111 | + </range> |
| 112 | + <noise> |
| 113 | + <type>gaussian</type> |
| 114 | + <mean>0.0</mean> |
| 115 | + <stddev>0.01</stddev> |
| 116 | + </noise> |
| 117 | + </ray> |
| 118 | + <plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_ray_sensor.so"> |
| 119 | + <ros> |
| 120 | + <namespace></namespace> |
| 121 | + <remapping>~/out:=scan</remapping> |
| 122 | + </ros> |
| 123 | + <output_type>sensor_msgs/LaserScan</output_type> |
| 124 | + </plugin> |
| 125 | + </sensor> |
| 126 | + </gazebo> |
| 127 | + |
| 128 | + <gazebo reference="${prefix}camera_rgb_frame"> |
| 129 | + <sensor type="camera" name="pi_camera"> |
| 130 | + <update_rate>30</update_rate> |
| 131 | + <always_on>true</always_on> |
| 132 | + <visualize>${camera_visual}</visualize> |
| 133 | + <camera> |
| 134 | + <horizontal_fov>1.085595</horizontal_fov> |
| 135 | + <image> |
| 136 | + <width>640</width> |
| 137 | + <height>480</height> |
| 138 | + <format>R8G8B8</format> |
| 139 | + </image> |
| 140 | + <clip> |
| 141 | + <near>0.03</near> |
| 142 | + <far>100</far> |
| 143 | + </clip> |
| 144 | + <noise> |
| 145 | + <type>gaussian</type> |
| 146 | + <!-- Noise is sampled independently per pixel on each frame. |
| 147 | + That pixel's noise value is added to each of its color |
| 148 | + channels, which at that point lie in the range [0,1]. --> |
| 149 | + <mean>0.0</mean> |
| 150 | + <stddev>0.007</stddev> |
| 151 | + </noise> |
| 152 | + </camera> |
| 153 | + <plugin name="camera_controller" filename="libgazebo_ros_camera.so"> |
| 154 | + <ros> |
| 155 | + <namespace></namespace> |
| 156 | + <remapping>image_raw:=image_raw</remapping> |
| 157 | + <remapping>camera_info:=camera_info</remapping> |
| 158 | + </ros> |
| 159 | + </plugin> |
| 160 | + </sensor> |
| 161 | + </gazebo> |
| 162 | + |
| 163 | + </xacro:macro> |
| 164 | + |
| 165 | +</robot> |
0 commit comments