mobot.gazebo
<?xml version="1.0"?>
<robot>
<!--skid_steer_drive_controller-->
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_diff_drive.so">
<ros>
<namespace>/mobot</namespace>
<argument>/cmd_vel:=mr_cme_vel</argument>
<argument>/odom:=odom</argument>
</ros>
<update_rate>100.0</update_rate>
<num_wheel_pairs>2</num_wheel_pairs>
<odometry_frame>odom</odometry_frame>
<left_joint>front_left_wheel_joint</left_joint>
<left_joint>back_left_wheel_joint</left_joint>
<wheel_separation>0.4</wheel_separation>
<wheel_diameter>0.2</wheel_diameter>
<right_joint>front_right_wheel_joint</right_joint>
<right_joint>back_right_wheel_joint</right_joint>
<wheel_separation>0.4</wheel_separation>
<wheel_diameter>0.2</wheel_diameter>
<robot_base_frame>robot_footprint</robot_base_frame>
<max_wheel_torque>20</max_wheel_torque>
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<odometry_source>world</odometry_source>
</plugin>
</gazebo>
<!-- camera -->
<gazebo reference="camera">
<sensor type="camera" name="camera_sensor">
<update_rate>30.0</update_rate>
<always_on>1</always_on>
<camera name="camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<distortion>
<k1>0.0</k1>
<k2>0.0</k2>
<k3>0.0</k3>
<p1>0.0</p1>
<p2>0.0</p2>
<center>0.5 0.5</center>
</distortion>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<ros>
<namespace>/mobot</namespace>
<argument>camera/image_raw:=/mobot/camera/image</argument>
<argument>camera/camera_info:=/mobot/camera/image/camera_info</argument>
</ros>
<camera_name>camera</camera_name>
<frame_name>camera</frame_name>
<hack_baseline>0.07</hack_baseline>
</plugin>
</sensor>
</gazebo>
<!-- hokuyo -->
<gazebo reference="hokuyo">
<sensor type="ray" name="head_hokuyo_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>30</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<!-- Noise parameters based on published spec for Hokuyo laser
achieving "+-30mm" accuracy at range < 10m. A mean of 0.0m and
stddev of 0.01m will put 99.7% of samples within 0.03m of the true
reading. -->
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<update_rate>30</update_rate>
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_ray_sensor.so">
<ros>
<namespace>/mobot</namespace>
<argument>~/out:=scan</argument>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
</plugin>
</sensor>
</gazebo>
</robot>
mobot.urdf.xacro
<?xml version='1.0'?>
<robot name="mobot"
xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="mobot.gazebo" />
<link name="robot_footprint"></link>
<joint name="robot_footprint_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="robot_footprint"/>
<child link="chassis" />
</joint>
<link name='chassis'>
<pose>0 0 0.1 0 0 0</pose>
<inertial>
<mass value="15.0"/>
<origin xyz="0.0 0 0" rpy=" 0 0 0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1" />
</inertial>
<collision name='collision'>
<origin xyz="0 0 0" rpy=" 0 0 0"/>
<geometry>
<box size=".4 .2 .1"/>
</geometry>
</collision>
<visual name='chassis_visual'>
<origin xyz="0 0 0" rpy=" 0 0 0"/>
<geometry>
<box size=".4 .2 .1"/>
</geometry>
</visual>
</link>
<link name="front_left_wheel">
<inertial>
<mass value="5.0"/>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1" />
</inertial>
<visual>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<geometry>
<cylinder length="0.05" radius="0.06"/>
</geometry>
</visual>
<collision>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<geometry>
<cylinder length="0.05" radius="0.06"/>
</geometry>
</collision>
</link>
<link name="back_left_wheel">
<inertial>
<mass value="5.0"/>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1" />
</inertial>
<visual>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<geometry>
<cylinder length="0.05" radius="0.06"/>
</geometry>
</visual>
<collision>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<geometry>
<cylinder length="0.05" radius="0.06"/>
</geometry>
</collision>
</link>
<link name="front_right_wheel">
<inertial>
<mass value="5.0"/>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1" />
</inertial>
<visual>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<geometry>
<cylinder length="0.05" radius="0.06"/>
</geometry>
</visual>
<collision>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<geometry>
<cylinder length="0.05" radius="0.06"/>
</geometry>
</collision>
</link>
<link name="back_right_wheel">
<inertial>
<mass value="5.0"/>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1" />
</inertial>
<visual>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<geometry>
<cylinder length="0.05" radius="0.06"/>
</geometry>
</visual>
<collision>
<origin xyz="0.0 0 0" rpy=" 0 1.5707 1.5707"/>
<geometry>
<cylinder length="0.05" radius="0.06"/>
</geometry>
</collision>
</link>
<link name="camera">
<inertial>
<mass value="0.1"/>
<origin xyz="0.05 0.05 0.05" rpy=" 0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
<visual name="camera_visual">
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</visual>
<collision name="camera_colision">
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
</link>
<link name="hokuyo">
<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>
<visual name="hokuyo_visual">
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<!-- <mesh filename="$(find mobot)/meshes/hokuyo.dae"/> -->
<box size="0.05 0.05 0.05"/>
</geometry>
</visual>
<collision name="hokuyo_colision">
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
</link>
<joint type="continuous" name="front_left_wheel_joint">
<origin xyz="0.12 0.15 0" rpy="0 0 0"/>
<child link="front_left_wheel"/>
<parent link="chassis"/>
<axis xyz="0 1 0" rpy="0 0 0"/>
<limit effort="10000" velocity="1000"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<joint type="continuous" name="front_right_wheel_joint">
<origin xyz="0.12 -0.15 0" rpy="0 0 0"/>
<child link="front_right_wheel"/>
<parent link="chassis"/>
<axis xyz="0 1 0" rpy="0 0 0"/>
<limit effort="10000" velocity="1000"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<joint type="continuous" name="back_left_wheel_joint">
<origin xyz="-0.12 0.15 0" rpy="0 0 0"/>
<child link="back_left_wheel"/>
<parent link="chassis"/>
<axis xyz="0 1 0" rpy="0 0 0"/>
<limit effort="10000" velocity="1000"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<joint type="continuous" name="back_right_wheel_joint">
<origin xyz="-0.12 -0.15 0" rpy="0 0 0"/>
<child link="back_right_wheel"/>
<parent link="chassis"/>
<axis xyz="0 1 0" rpy="0 0 0"/>
<limit effort="10000" velocity="1000"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<joint type="fixed" name="camera_joint">
<origin xyz="0.2 0 0" rpy="0 0 0"/>
<child link="camera"/>
<parent link="chassis"/>
<axis xyz="0 1 0" rpy="0 0 0"/>
</joint>
<joint type="fixed" name="hokuyo_joint">
<origin xyz="0.15 0 0.1" rpy="0 0 0"/>
<child link="hokuyo"/>
<parent link="chassis"/>
<axis xyz="0 1 0" rpy="0 0 0"/>
</joint>
<gazebo reference="chassis">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="camera">
<material>Gazebo/Red</material>
</gazebo>
<gazebo reference="hokuyo">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="front_left_wheel">
<material>Gazebo/Gray</material>
</gazebo>
<gazebo reference="back_left_wheel">
<material>Gazebo/Gray</material>
</gazebo>
<gazebo reference="front_right_wheel">
<material>Gazebo/Gray</material>
</gazebo>
<gazebo reference="back_right_wheel">
<material>Gazebo/Gray</material>
</gazebo>
</robot>