Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
264 lines (208 sloc) 8.9 KB
<?xml version="1.0"?>
<robot name="gribot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find gribot)/urdf/wheel.urdf.xacro" />
<!-- Defining the colors used in this robot -->
<material name="Black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="Red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<material name="Blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<!-- Property list -->
<!-- All units in m-kg-s-radians unit system -->
<xacro:property name="M_PI" value="3.1415926535897931" />
<xacro:property name="M_PI_2" value="1.570796327" />
<xacro:property name="DEG_TO_RAD" value="0.017453293" />
<!-- Main body length height width -->
<xacro:property name="base_height" value="0.25" /> <!-- in meter -->
<xacro:property name="base_width" value="0.52" /> <!-- in meter -->
<xacro:property name="base_lenght" value="0.8" /> <!-- in meter -->
<xacro:property name="base_mass" value="20.0" /> <!-- in kg -->
<!-- Macro for calculating inertia of cylinder -->
<xacro:macro name="cylinder_inertia" params="m r h">
<inertia ixx="${ m * ( 3 * r*r + h*h ) / 12}" ixy="0" ixz="0"
iyy="${ m * ( 3 * r*r + h*h ) / 12}" iyz="0"
izz="${ m * r / 2}" />
</xacro:macro>
<!-- Macro for calculating inertia of a box -->
<xacro:macro name="box_inertia" params="m x y z">
<inertia ixx="${0.0833333 * m * (y*y + z*z)}" ixy="0.0" ixz="0.0"
iyy="${0.0833333 * m * (x*x + z*z)}" iyz="0.0"
izz="${0.0833333 * m * (x*x + y*y)}" />
</xacro:macro>
<!-- World link -->
<!-- base_footprint is a fictious link(frame) that is on the ground right below base_link origin -->
<!-- It is necessary as gazebo does not support inertia for the root link -->
<!-- Do not call it world as it is reserved for robot that must be fixed to the world -->
<link name="base_link">
</link>
<gazebo reference="base_link">
<turnGravityOff>false</turnGravityOff>
</gazebo>
<joint name="glue_robot_to_world" type="fixed">
<origin xyz="0 0 0.1" rpy="0 0 0" />
<parent link="base_link" />
<child link="chassis" />
</joint>
<!-- Base link -->
<!-- Actual body/chassis of the robot -->
<link name="chassis">
<inertial>
<mass value="${base_mass}" />
<!-- the3x3 rotational inertia matrix -->
<box_inertia m="${base_mass}" x="${base_lenght}" y="${base_width}" z="${base_height}" />
<origin xyz="0 0 0" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${base_lenght} ${base_width} ${base_height}" />
</geometry>
<material name="Blue" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${base_lenght} ${base_width} ${base_height}" />
</geometry>
</collision>
</link>
<gazebo reference="chassis">
<material>Gazebo/White</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<!-- Skid Steering Drive for 4 wheel robot for simulation -->
<gazebo>
<plugin name="drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<updateRate>100</updateRate>
<robotNameSpace>/</robotNameSpace>
<leftFrontJoint>front_left_wheel_joint</leftFrontJoint>
<rightFrontJoint>front_right_wheel_joint</rightFrontJoint>
<leftRearJoint>back_left_wheel_joint</leftRearJoint>
<rightRearJoint>back_right_wheel_joint</rightRearJoint>
<wheelSeparation>0.4</wheelSeparation>
<wheelDiameter>0.32</wheelDiameter>
<robotBaseFrame>base_link</robotBaseFrame>
<torque>10</torque>
<topicName>cmd_vel</topicName>
<broadcastTF>true</broadcastTF> <!-- Must be true or gmapping does not work in simulation environment -->
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<covariance_x>0.00100</covariance_x>
<covariance_y>0.00100</covariance_y>
<covariance_yaw>0.010000</covariance_yaw>
</plugin>
</gazebo>
<!-- Wheel Definition for simulation -->
<wheel fb="front" lr="right" parent="chassis" translateX="0.2" translateY="${-1*(base_width/2+0.04)}" translateZ="-0.065" flipY="1"/>
<wheel fb="front" lr="left" parent="chassis" translateX="0.2" translateY="${1*(base_width/2+0.04)}" translateZ="-0.065" flipY="1"/>
<wheel fb="back" lr="right" parent="chassis" translateX="-0.2" translateY="${-1*(base_width/2+0.04)}" translateZ="-0.065" flipY="1"/>
<wheel fb="back" lr="left" parent="chassis" translateX="-0.2" translateY="${1*(base_width/2+0.04)}" translateZ="-0.065" flipY="1"/>
<!-- Lidar definition for simulation -->
<link name="sweep_link" >
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.1 0.1 0.1" />
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.1" radius="0.05" />
</geometry>
<material name="black" />
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<joint name="lidar_to_base" type="fixed" >
<origin xyz="0.3 0 ${base_height/2+0.05}" rpy="0 0 0" />
<parent link="chassis" />
<child link="sweep_link" />
</joint>
<gazebo reference="sweep_link" >
<sensor type="ray" name="laser">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize> <!-- temporary for testing, but should be put to false -->
<update_rate>100</update_rate> <!-- rate at wich the sensor updates informations -->
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<!-- <min_angle>-1.570796</min_angle> -->
<!-- <max_angle>1.570796</max_angle> -->
<min_angle>-3.141592</min_angle>
<max_angle>3.141592</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type> <!-- Add parameters for Svcansee sweep -->
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="sweep_link" filename="libgazebo_ros_laser.so">
<topicName>base_scan</topicName>
<frameName>sweep_link</frameName>
</plugin>
</sensor>
</gazebo>
<!-- Publishing joint states from Gazebo simulator -->
<!-- fixed links are not published -->
<!-- this is required to publish wheel status -->
<gazebo>
<plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
<jointName>front_left_wheel_joint, front_right_wheel_joint, back_left_wheel_joint, back_right_wheel_joint</jointName>
<robotNameSpace>/</robotNameSpace>
<updateRate>100</updateRate>
<alwaysOn>true</alwaysOn>
</plugin>
</gazebo>
<!-- GPS simulator -->
<!-- <gazebo> -->
<!-- <plugin name="gps_sim" filename="libhector_gazebo_ros_gps.so"> -->
<!-- <alwaysOn>1</alwaysOn> -->
<!-- <updateRate>10.0</updateRate> -->
<!-- <bodyName>base_footprint</bodyName> -->
<!-- <topicName>fix</topicName> -->
<!-- <velocityTopicName>fix_velocity</velocityTopicName> -->
<!-- <drift>5.0 5.0 5.0</drift> -->
<!-- <gaussianNoise>0.1 0.1 0.1</gaussianNoise> -->
<!-- <velocityDrift>0 0 0</velocityDrift> -->
<!-- <velocityGaussianNoise>0.1 0.1 0.1</velocityGaussianNoise> -->
<!-- </plugin> -->
<!-- </gazebo> -->
<!-- <gazebo> -->
<!-- <plugin name="imu_plugin" filename="libgazebo_ros_imu.so"> -->
<!-- <alwaysOn>true</alwaysOn> -->
<!-- <bodyName>base_footprint</bodyName> -->
<!-- <topicName>imu</topicName> -->
<!-- <serviceName>imu_service</serviceName> -->
<!-- <gaussianNoise>0.0</gaussianNoise> -->
<!-- <updateRate>20.0</updateRate> -->
<!-- </plugin> -->
<!-- </gazebo> -->
<!-- <gazebo> -->
<!-- <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"> -->
<!-- <robotNamespace>/mybot</robotNamespace> -->
<!-- <legacyModeNS>true</legacyModeNS> -->
<!-- </plugin> -->
<!-- </gazebo> -->
</robot>
You can’t perform that action at this time.