git commit -m "first commit"

This commit is contained in:
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions

View File

@@ -0,0 +1,133 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package mir_description
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.1.7 (2023-01-20)
------------------
* Don't set cmake_policy CMP0048
* Contributors: Martin Günther
1.1.6 (2022-06-02)
------------------
* URDF: Downsize inertia box, move to lower back
* URDF: Pull out inertia properties
* URDF: Update masses according to data sheet
* URDF: Add mir_250
* Add arg mir_type to launch files and urdfs
* Add mir_250 meshes
* URDF: Make wheels black
* Add mir_100_v1.urdf.xacro for backwards compatibility
* Rename mir_100 -> mir
* Refactor URDF to prepare for MiR250 support
* Gazebo: Don't manually specify wheel params for diffdrive controller
* Simplify mir_100 collision mesh further
* Contributors: Martin Günther
1.1.5 (2022-02-11)
------------------
* Remove xacro comment to work around xacro bug
Since xacro 1.14.11, xacro now also evaluates expressions in comments
and throws an error if the substition argument is undefined. In xacro
1.14.12, this error was changed to a warning.
This commit removes that warning.
Workaround for https://github.com/ros/xacro/issues/309 .
* xacro: drop --inorder option
In-order processing became default in ROS Melodic.
* Add gazebo_plugins to dependency list (`#103 <https://github.com/DFKI-NI/mir_robot/issues/103>`_)
This is needed for the ground truth pose via p3d plugin.
* Contributors: Martin Günther, moooeeeep
1.1.4 (2021-12-10)
------------------
* Replace gazebo_plugins IMU with hector_gazebo_plugins
* Use cylinders instead of STLs for wheel collision geometries
Fixes `#99 <https://github.com/DFKI-NI/mir_robot/issues/99>`_.
* mir_debug_urdf.launch: Fix GUI display
* Contributors: Martin Günther
1.1.3 (2021-06-11)
------------------
* Merge branch 'melodic-2.8' into noetic
* Rename tf frame and topic 'odom_comb' -> 'odom'
This is how they are called on the real MiR since MiR software 2.0.
* Contributors: Martin Günther
1.1.2 (2021-05-12)
------------------
* Fix laser scan frame_id with gazebo_plugins 2.9.2
* Contributors: Martin Günther
1.1.1 (2021-02-11)
------------------
* Add prepend_prefix_to_laser_frame to URDF and launch files
Fixes `#65 <https://github.com/DFKI-NI/mir_robot/issues/65>`_.
* Add tf_prefix to URDF and launch files
* Fix typo in robot_namespace
* Add missing 'xacro:' xml namespace prefixes
Macro calls without 'xacro:' prefix are deprecated in Melodic and will
be forbidden in Noetic.
* Contributors: Martin Günther
1.1.0 (2020-06-30)
------------------
* Initial release into noetic
* Contributors: Martin Günther
1.0.6 (2020-06-30)
------------------
* Update to non-deprecated robot_state_publisher node
* Set cmake_policy CMP0048 to fix warning
* Contributors: Martin Günther
1.0.5 (2020-05-01)
------------------
* Switch from Gazebo GPU laser to normal laser plugin
The GPU laser plugin has caused multiple people problems before, because
it is not compatible with all GPUS: `#1 <https://github.com/DFKI-NI/mir_robot/issues/1>`_
`#32 <https://github.com/DFKI-NI/mir_robot/issues/32>`_
`#46 <https://github.com/DFKI-NI/mir_robot/issues/46>`_
`#52 <https://github.com/DFKI-NI/mir_robot/issues/52>`_
The normal laser plugin directly uses the physics engine, so it doesn't
depend on any specific GPU. Also, it doesn't slow down the simulation
noticeably (maybe 1-2%).
* Contributors: Martin Günther
1.0.4 (2019-05-06)
------------------
* Add legacyModeNS param to gazebo_ros_control plugin
This enables the new behavior of the plugin (pid_gains parameter are now
in the proper namespace).
* re-added gazebo friction parameters for the wheels (`#19 <https://github.com/DFKI-NI/mir_robot/issues/19>`_)
* Contributors: Martin Günther, niniemann
1.0.3 (2019-03-04)
------------------
* Merge pull request `#16 <https://github.com/DFKI-NI/mir_robot/issues/16>`_ from niniemann/add-prefix-argument-to-configs
Add prefix argument to configs
* removed prefix from plugin frameName in sick urdf
The gazebo plugins automatically use tf_prefix, even if none is set
(in that case it defaults to the robot namespace). That's why we can
remove the prefix from the plugins configuration, assuming that the
robot namespace will be equal to the prefix.
* adds $(arg prefix) to a lot of configs
This is an important step to be able to re-parameterize move base,
the diffdrive controller, ekf, amcl and the costmaps for adding a
tf prefix to the robots links
* workaround eval in xacro for indigo support
* adds tf_prefix argument to imu.gazebo.urdf.xacro
* Add TFs for ultrasound sensors
* Contributors: Martin Günther, Nils Niemann
1.0.2 (2018-07-30)
------------------
1.0.1 (2018-07-17)
------------------
* gazebo: Remove leading slashes in TF frames
TF2 doesn't like it (e.g., robot_localization).
* Contributors: Martin Günther
1.0.0 (2018-07-12)
------------------
* Initial release
* Contributors: Martin Günther

View File

@@ -0,0 +1,31 @@
cmake_minimum_required(VERSION 3.5.1)
project(mir_description)
find_package(catkin REQUIRED COMPONENTS
roslaunch
)
###################################
## catkin specific configuration ##
###################################
catkin_package()
#############
## Install ##
#############
# Mark other files for installation (e.g. launch and bag files, etc.)
install(DIRECTORY
config
launch
meshes
rviz
urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
roslaunch_add_file_check(launch)

View File

@@ -0,0 +1,44 @@
# -----------------------------------
mobile_base_controller:
type : "diff_drive_controller/DiffDriveController"
left_wheel : '$(arg prefix)left_wheel_joint'
right_wheel : '$(arg prefix)right_wheel_joint'
publish_rate: 41.2 # this is what the real MiR platform publishes (default: 50)
# These covariances are exactly what the real MiR platform publishes
pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0]
enable_odom_tf: false
# Wheel separation and diameter. These are both optional.
# diff_drive_controller will attempt to read either one or both from the
# URDF if not specified as a parameter.
# We don't set the value here because it's different for each MiR type (100, 250, ...), and
# the plugin figures out the correct values.
#wheel_separation : 0.445208
#wheel_radius : 0.0625
# Wheel separation and radius multipliers
wheel_separation_multiplier: 1.0 # default: 1.0
wheel_radius_multiplier : 1.0 # default: 1.0
# Velocity commands timeout [s], default 0.5
cmd_vel_timeout: 0.5
# frame_ids (same as real MiR platform)
base_frame_id: $(arg prefix)base_footprint # default: base_link
odom_frame_id: $(arg prefix)odom # default: odom
# Velocity and acceleration limits
# Whenever a min_* is unspecified, default to -max_*
linear:
x:
has_velocity_limits : false
max_velocity : 2.0 # m/s; move_base max_vel_x: 0.8
has_acceleration_limits: true
max_acceleration : 3.0 # m/s^2; move_base acc_lim_x: 1.5
angular:
z:
has_velocity_limits : false
max_velocity : 2.0 # rad/s; move_base max_rot_vel: 1.0
has_acceleration_limits: true
max_acceleration : 2.0 # rad/s^2; move_base acc_lim_th: 2.0

View File

@@ -0,0 +1,4 @@
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50

View File

@@ -0,0 +1,18 @@
<?xml version="1.0" ?>
<launch>
<arg name="mir_type" default="mir_250" doc="The MiR variant. Can be 'mir_100' or 'mir_250' for now." />
<arg name="gui" default="true" />
<!-- load MiR URDF -->
<include file="$(find mir_description)/launch/upload_mir_urdf.launch">
<arg name="mir_type" value="$(arg mir_type)" />
</include>
<node if="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<node unless="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mir_description)/rviz/mir_description.rviz" required="true" />
</launch>

View File

@@ -0,0 +1,7 @@
<?xml version="1.0" ?>
<launch>
<arg name="mir_type" default="mir_250" doc="The MiR variant. Can be 'mir_100' or 'mir_250' for now." />
<arg name="tf_prefix" default="" doc="TF prefix to use for all of the MiR's TF frames"/>
<param name="robot_description" command="$(find xacro)/xacro $(find mir_description)/urdf/mir.urdf.xacro mir_type:=$(arg mir_type) tf_prefix:=$(arg tf_prefix)" />
</launch>

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

@@ -0,0 +1,34 @@
<?xml version="1.0"?>
<package format="2">
<name>mir_description</name>
<version>1.1.7</version>
<description>URDF description of the MiR robot</description>
<maintainer email="martin.guenther@dfki.de">Martin Günther</maintainer>
<author email="martin.guenther@dfki.de">Martin Günther</author>
<license>BSD</license>
<url type="website">https://github.com/DFKI-NI/mir_robot</url>
<url type="repository">https://github.com/DFKI-NI/mir_robot</url>
<url type="bugtracker">https://github.com/DFKI-NI/mir_robot/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roslaunch</build_depend>
<build_depend>diff_drive_controller</build_depend>
<exec_depend>diff_drive_controller</exec_depend>
<exec_depend>gazebo_plugins</exec_depend>
<exec_depend>gazebo_ros_control</exec_depend>
<exec_depend>hector_gazebo_plugins</exec_depend>
<exec_depend>joint_state_controller</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>joint_state_publisher_gui</exec_depend>
<exec_depend>position_controllers</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>rviz</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
</package>

View File

@@ -0,0 +1,210 @@
Panels:
- Class: rviz/Displays
Help Height: 81
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 535
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
back_laser_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
bl_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
bl_caster_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
br_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
br_caster_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_laser_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_frame:
Alpha: 1
Show Axes: false
Show Trail: false
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
left_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
surface:
Alpha: 1
Show Axes: false
Show Trail: false
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: base_footprint
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.20704937
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.023150593
Y: -0.0341755934
Z: -2.6775524e-09
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.515398085
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.490397513
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002a9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002a9000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000043000002a9000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000022300fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1200
X: 59
Y: 29

View File

@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="controller_plugin_gazebo" params="robot_namespace">
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<xacro:unless value="${robot_namespace == ''}">
<robotNamespace>${robot_namespace}</robotNamespace>
</xacro:unless>
<controlPeriod>0.001</controlPeriod>
<legacyModeNS>false</legacyModeNS>
</plugin>
</gazebo>
</xacro:macro>
</robot>

View File

@@ -0,0 +1,84 @@
<?xml version="1.0" ?>
<!--
Various useful properties such as constants and materials
-->
<robot name="xacro_properties" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- #################### RViz materials #################### -->
<xacro:property name="material_white">
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</xacro:property>
<xacro:property name="material_yellow">
<material name="yellow">
<color rgba="${255/255} ${226/255} ${0/255} 1"/>
</material>
</xacro:property>
<xacro:property name="material_almost_white">
<material name="almost_white">
<color rgba="0.9 0.9 0.9 1" />
</material>
</xacro:property>
<xacro:property name="material_grey">
<material name="grey">
<color rgba="0.5 0.5 0.5 1"/>
</material>
</xacro:property>
<xacro:property name="material_light_grey">
<material name="light_grey">
<color rgba="0.6 0.6 0.6 1"/>
</material>
</xacro:property>
<xacro:property name="material_dark_grey">
<material name="dark_grey">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</xacro:property>
<xacro:property name="material_black">
<material name="black">
<color rgba="0.1 0.1 0.1 1"/>
</material>
</xacro:property>
<xacro:property name="material_aluminum">
<material name="aluminum">
<color rgba="0.8 0.8 0.8 1" />
</material>
</xacro:property>
<xacro:property name="material_silver">
<material name="silver">
<color rgba="0.79 0.82 0.93 1" />
</material>
</xacro:property>
<!-- #################### inertials with origin #################### -->
<!-- see https://secure.wikimedia.org/wikipedia/en/wiki/List_of_moment_of_inertia_tensors -->
<xacro:macro name="sphere_inertial" params="radius mass *origin">
<inertial>
<mass value="${mass}" />
<xacro:insert_block name="origin" />
<inertia ixx="${0.4 * mass * radius * radius}" ixy="0.0" ixz="0.0"
iyy="${0.4 * mass * radius * radius}" iyz="0.0"
izz="${0.4 * mass * radius * radius}" />
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertial" params="radius length mass *origin">
<inertial>
<mass value="${mass}" />
<xacro:insert_block name="origin" />
<inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
izz="${0.5 * mass * radius * radius}" />
</inertial>
</xacro:macro>
<xacro:macro name="box_inertial" params="x y z mass *origin">
<inertial>
<mass value="${mass}" />
<xacro:insert_block name="origin" />
<inertia ixx="${0.0833333 * mass * (y*y + z*z)}" ixy="0.0" ixz="0.0"
iyy="${0.0833333 * mass * (x*x + z*z)}" iyz="0.0"
izz="${0.0833333 * mass * (x*x + y*y)}" />
</inertial>
</xacro:macro>
</robot>

View File

@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<!-- If tf_prefix is given, use "frame tf_prefix/imu_frame", else "imu_frame" -->
<xacro:arg name="tf_prefix" default="" />
<xacro:property name="tf_prefix" value="$(arg tf_prefix)" />
<xacro:if value="${tf_prefix == ''}">
<xacro:property name="imu_frame" value="imu_frame" />
</xacro:if>
<xacro:unless value="${tf_prefix == ''}">
<xacro:property name="imu_frame" value="$(arg tf_prefix)/imu_frame" />
</xacro:unless>
<xacro:macro name="imu_gazebo" params="link imu_topic update_rate">
<gazebo>
<plugin name="imu_plugin" filename="libhector_gazebo_ros_imu.so">
<updateRate>${update_rate}</updateRate>
<bodyName>${link}</bodyName>
<frameId>${imu_frame}</frameId> <!-- from real MiR -->
<topicName>${imu_topic}</topicName>
<accelDrift>0.0 0.0 0.0</accelDrift>
<accelGaussianNoise>${sqrt(5e-05)} ${sqrt(0.0001)} ${sqrt(0.00013)}</accelGaussianNoise> <!-- real MiR linear_acceleration_covariance: [5e-05, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.00013] -->
<rateDrift>0.0 0.0 0.0</rateDrift>
<rateGaussianNoise>${sqrt(8e-06)} ${sqrt(8e-06)} ${sqrt(3e-07)}</rateGaussianNoise> <!-- real MiR angular_velocity_covariance: [8e-06, 0.0, 0.0, 0.0, 8e-06, 0.0, 0.0, 0.0, 3e-07] -->
<yawDrift>0.0</yawDrift>
<yawGaussianNoise>${sqrt(0.1)}</yawGaussianNoise> <!-- real MiR orientation_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1] -->
</plugin>
</gazebo>
</xacro:macro>
</robot>

View File

@@ -0,0 +1,62 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="diff_controller_plugin_gazebo" params="prefix left_wheel_joint right_wheel_joint wheel_separation wheel_radius">
<gazebo>
<plugin name="diff_drive_controller" filename="libgazebo_ros_diff_drive.so">
<legacyMode>false</legacyMode>
<alwaysOn>true</alwaysOn>
<updateRate>1000.0</updateRate>
<leftJoint>${left_wheel_joint}</leftJoint>
<rightJoint>${right_wheel_joint}</rightJoint>
<wheelSeparation>${wheel_separation}</wheelSeparation>
<wheelDiameter>${2*wheel_radius}</wheelDiameter>
<wheelTorque>10</wheelTorque>
<publishTf>1</publishTf>
<odometryFrame>map</odometryFrame>
<commandTopic>mobile_base_controller/cmd_vel</commandTopic>
<odometryTopic>mobile_base_controller/odom</odometryTopic>
<robotBaseFrame>base_footprint</robotBaseFrame>
<wheelAcceleration>2.8</wheelAcceleration>
<publishWheelJointState>true</publishWheelJointState>
<publishWheelTF>false</publishWheelTF>
<odometrySource>world</odometrySource>
<rosDebugLevel>Debug</rosDebugLevel>
</plugin>
</gazebo>
</xacro:macro>
<xacro:macro name="set_wheel_friction" params="link friction">
<gazebo reference="${link}">
<mu1 value="${friction}"/>
<mu2 value="${friction}"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<minDepth>0.01</minDepth>
</gazebo>
</xacro:macro>
<xacro:macro name="set_all_wheel_frictions" params="prefix">
<xacro:set_wheel_friction link="${prefix}left_wheel_link" friction="200"/>
<xacro:set_wheel_friction link="${prefix}right_wheel_link" friction="200"/>
<xacro:set_wheel_friction link="${prefix}fl_caster_wheel_link" friction="1"/>
<xacro:set_wheel_friction link="${prefix}fr_caster_wheel_link" friction="1"/>
<xacro:set_wheel_friction link="${prefix}bl_caster_wheel_link" friction="1"/>
<xacro:set_wheel_friction link="${prefix}br_caster_wheel_link" friction="1"/>
</xacro:macro>
<xacro:macro name="p3d_base_controller" params="prefix">
<gazebo>
<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
<alwaysOn>true</alwaysOn>
<updateRate>50.0</updateRate>
<bodyName>${prefix}base_footprint</bodyName>
<topicName>base_pose_ground_truth</topicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>0 0 0</xyzOffsets>
<rpyOffsets>0 0 0</rpyOffsets>
</plugin>
</gazebo>
</xacro:macro>
</robot>

View File

@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="mir_wheel_transmission" params="prefix locationprefix">
<transmission name="${prefix}${locationprefix}_wheel_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}${locationprefix}_wheel_joint">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix}${locationprefix}_wheel_motor">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<xacro:macro name="mir_wheel_transmissions" params="prefix">
<xacro:mir_wheel_transmission prefix="${prefix}" locationprefix="left"/>
<xacro:mir_wheel_transmission prefix="${prefix}" locationprefix="right"/>
</xacro:macro>
</robot>

View File

@@ -0,0 +1,12 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro"
name="mir_100" >
<!-- This file is only for backward compatibility before the "mir_100" to "mir" rename. -->
<xacro:include filename="$(find mir_description)/urdf/include/mir_v1.urdf.xacro" />
<xacro:macro name="mir_100" params="prefix">
<xacro:mir prefix="${prefix}" />
</xacro:macro>
</robot>

View File

@@ -0,0 +1,308 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:arg name="mir_type" default="mir_250" /> <!-- The MiR variant. Can be "mir_100" or "mir_250" for now.-->
<xacro:property name="mir_type" value="$(arg mir_type)" /> <!-- necessary because args cannot be accessed inside ${} expressions -->
<xacro:include filename="$(find mir_description)/urdf/include/common_properties.urdf.xacro" />
<xacro:include filename="$(find mir_description)/urdf/include/imu.gazebo.urdf.xacro" />
<xacro:include filename="$(find mir_description)/urdf/include/mir.gazebo.xacro" />
<xacro:include filename="$(find mir_description)/urdf/include/mir.transmission.xacro" />
<xacro:include filename="$(find mir_description)/urdf/include/sick_s300.urdf.xacro" />
<xacro:property name="deg_to_rad" value="0.017453293" />
<!-- The inertia for the MiR platform is intentionally chosen to be smaller than
the bounding box and also shifted a bit to the back, because most of the mass
is in the lower center back (because of the batteries). -->
<xacro:property name="mir_base_inertial_x" value="-0.05" />
<xacro:property name="mir_base_inertial_y" value="0.0" />
<xacro:property name="mir_base_inertial_z" value="0.15" />
<xacro:property name="mir_base_inertial_x_length" value="0.50" />
<xacro:property name="mir_base_inertial_y_length" value="0.30" />
<xacro:property name="mir_base_inertial_z_length" value="0.20" />
<xacro:if value="${mir_type == 'mir_100'}">
<xacro:property name="mir_base_mass" value="77.0" />
<xacro:property name="mir_act_wheel_radius" value="0.0625" />
<xacro:property name="mir_act_wheel_width" value="0.032" />
<xacro:property name="mir_act_wheel_mass" value="1.0" />
<xacro:property name="mir_act_wheel_dx" value="0.037646" />
<xacro:property name="mir_act_wheel_dy" value="0.222604" />
</xacro:if>
<xacro:if value="${mir_type == 'mir_250'}">
<xacro:property name="mir_base_mass" value="97.0" />
<xacro:property name="mir_act_wheel_radius" value="0.100" />
<xacro:property name="mir_act_wheel_width" value="0.038" />
<xacro:property name="mir_act_wheel_mass" value="1.0" />
<xacro:property name="mir_act_wheel_dx" value="-0.004485" />
<xacro:property name="mir_act_wheel_dy" value="0.2015" />
</xacro:if>
<xacro:property name="mir_caster_wheel_radius" value="0.0625" />
<xacro:property name="mir_caster_wheel_width" value="0.032" />
<xacro:property name="mir_caster_wheel_mass" value="1.0" />
<xacro:property name="mir_caster_wheel_dx" value="-0.0382" />
<xacro:property name="mir_caster_wheel_dy" value="0" />
<xacro:property name="mir_caster_wheel_dz" value="-0.094" />
<xacro:property name="mir_front_caster_wheel_base_dx" value="0.3037" />
<xacro:if value="${mir_type == 'mir_100'}">
<xacro:property name="mir_back_caster_wheel_base_dx" value="0.3078" />
<xacro:property name="mir_caster_wheel_base_dy" value="0.203" />
</xacro:if>
<xacro:if value="${mir_type == 'mir_250'}">
<xacro:property name="mir_back_caster_wheel_base_dx" value="0.296" />
<xacro:property name="mir_caster_wheel_base_dy" value="0.188" />
</xacro:if>
<xacro:property name="mir_caster_wheel_base_dz" value="${mir_caster_wheel_radius - mir_caster_wheel_dz}" />
<!-- from visually matching up the meshes of the MiR and the laser scanner -->
<xacro:if value="${mir_type == 'mir_100'}">
<xacro:property name="laser_dx" value="0.392" />
<xacro:property name="laser_dy" value="0.2358" />
<xacro:property name="laser_dz" value="0.1914" />
</xacro:if>
<xacro:if value="${mir_type == 'mir_250'}">
<xacro:property name="laser_dx" value="0.315" />
<xacro:property name="laser_dy" value="0.205" />
<xacro:property name="laser_dz" value="0.1914" />
</xacro:if>
<xacro:macro name="actuated_wheel" params="prefix locationprefix locationright">
<joint name="${prefix}${locationprefix}_wheel_joint" type="continuous">
<origin xyz="0.0 ${-mir_act_wheel_dy * locationright} ${mir_act_wheel_radius}" rpy="0 0 0" />
<parent link="${prefix}base_link" />
<child link="${prefix}${locationprefix}_wheel_link" />
<axis xyz="0 1 0" />
<limit effort="100" velocity="20.0" />
</joint>
<link name="${prefix}${locationprefix}_wheel_link">
<xacro:cylinder_inertial mass="${mir_act_wheel_mass}" radius="${mir_act_wheel_radius}" length="${mir_act_wheel_width}">
<origin xyz="0 0 0" rpy="${0.5 * pi} 0 0" />
</xacro:cylinder_inertial>
<visual>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<geometry>
<cylinder radius="${mir_act_wheel_radius}" length="${mir_act_wheel_width}" />
</geometry>
<xacro:insert_block name="material_black" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<geometry>
<cylinder radius="${mir_act_wheel_radius}" length="${mir_act_wheel_width}" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}${locationprefix}_wheel_link">
<material>Gazebo/FlatBlack</material>
</gazebo>
</xacro:macro>
<xacro:macro name="caster_wheel" params="prefix locationprefix locationright wheel_base_dx">
<!-- caster hub -->
<joint name="${prefix}${locationprefix}_caster_rotation_joint" type="continuous">
<origin xyz="${wheel_base_dx} ${-mir_caster_wheel_base_dy * locationright} ${mir_caster_wheel_base_dz}" rpy="0 0 0" />
<parent link="${prefix}base_link" />
<child link="${prefix}${locationprefix}_caster_rotation_link" />
<axis xyz="0 0 1" />
<dynamics damping="0.01" friction="0.0"/>
</joint>
<link name="${prefix}${locationprefix}_caster_rotation_link">
<inertial>
<!-- <origin xyz="0 0 -0.042500000044" rpy="${0.5 * pi} ${24 * deg_to_rad} ${1.5 * pi}" /> -->
<origin xyz="0 0 -0.042500000044" rpy="${24 * deg_to_rad} 0 ${0.5 * pi} " />
<mass value="0.3097539019" />
<inertia
ixx="0.0005844517978"
ixy="0"
ixz="0"
iyy="0.00052872551237"
iyz="0"
izz="0.00017923555074" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mir_description/meshes/visual/caster_wheel_base.stl" />
</geometry>
<xacro:if value="${mir_type == 'mir_100'}">
<xacro:insert_block name="material_silver" />
</xacro:if>
<xacro:if value="${mir_type == 'mir_250'}">
<xacro:insert_block name="material_black" />
</xacro:if>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mir_description/meshes/collision/caster_wheel_base.stl" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}${locationprefix}_caster_rotation_link">
<xacro:if value="${mir_type == 'mir_100'}">
<material>Gazebo/Grey</material>
</xacro:if>
<xacro:if value="${mir_type == 'mir_250'}">
<material>Gazebo/FlatBlack</material>
</xacro:if>
</gazebo>
<!-- caster wheel -->
<joint name="${prefix}${locationprefix}_caster_wheel_joint" type="continuous">
<origin xyz="${mir_caster_wheel_dx} ${-mir_caster_wheel_dy * locationright} ${mir_caster_wheel_dz}" rpy="0 0 0" />
<parent link="${prefix}${locationprefix}_caster_rotation_link" />
<child link="${prefix}${locationprefix}_caster_wheel_link" />
<axis xyz="0 1 0" />
</joint>
<link name="${prefix}${locationprefix}_caster_wheel_link">
<xacro:cylinder_inertial mass="${mir_caster_wheel_mass}" radius="${mir_caster_wheel_radius}" length="${mir_caster_wheel_width}">
<origin xyz="0 0 0" rpy="${0.5 * pi} 0 0" />
</xacro:cylinder_inertial>
<visual>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<geometry>
<cylinder radius="${mir_caster_wheel_radius}" length="${mir_caster_wheel_width}" />
</geometry>
<xacro:insert_block name="material_black" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<geometry>
<cylinder radius="${mir_caster_wheel_radius}" length="${mir_caster_wheel_width}" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}${locationprefix}_caster_wheel_link">
<material>Gazebo/FlatBlack</material>
</gazebo>
</xacro:macro>
<xacro:macro name="mir" params="prefix">
<link name="${prefix}base_footprint" />
<joint name="${prefix}base_joint" type="fixed">
<parent link="${prefix}base_footprint" />
<child link="${prefix}base_link" />
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<link name="${prefix}base_link">
<xacro:box_inertial mass="${mir_base_mass}" x="${mir_base_inertial_x_length}" y="${mir_base_inertial_y_length}" z="${mir_base_inertial_z_length}">
<origin xyz="${mir_base_inertial_x + mir_act_wheel_dx} ${mir_base_inertial_y} ${mir_base_inertial_z}" rpy="0 0 0" />
</xacro:box_inertial>
<visual>
<origin xyz="${mir_act_wheel_dx} 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mir_description/meshes/visual/${mir_type}_base.stl" />
</geometry>
<xacro:if value="${mir_type == 'mir_100'}">
<xacro:insert_block name="material_white" />
</xacro:if>
<xacro:if value="${mir_type == 'mir_250'}">
<xacro:insert_block name="material_dark_grey" />
</xacro:if>
</visual>
<collision>
<origin xyz="${mir_act_wheel_dx} 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mir_description/meshes/collision/${mir_type}_base.stl" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}base_link">
<xacro:if value="${mir_type == 'mir_100'}">
<material>Gazebo/White</material>
</xacro:if>
<xacro:if value="${mir_type == 'mir_250'}">
<material>Gazebo/DarkGrey</material>
</xacro:if>
</gazebo>
<!-- IMU -->
<joint name="${prefix}base_link_to_imu_joint" type="fixed">
<parent link="${prefix}base_link" />
<child link="${prefix}imu_link" />
<origin xyz="0.0 0.0 0.25" rpy="0 0 0" /> <!-- same as real MiR -->
</joint>
<link name="${prefix}imu_link" />
<xacro:imu_gazebo link="${prefix}imu_link" imu_topic="imu_data" update_rate="50.0" />
<!-- Create an alias for imu_link. This is necessary because the real MiR's
TF has imu_link, but the imu_data topic is published in the imu_frame
frame. -->
<joint name="${prefix}imu_link_to_imu_frame_joint" type="fixed">
<parent link="${prefix}imu_link" />
<child link="${prefix}imu_frame" />
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<link name="${prefix}imu_frame" />
<!-- Laser scanners -->
<joint name="${prefix}base_link_to_front_laser_joint" type="fixed">
<parent link="${prefix}base_link" />
<child link="${prefix}front_laser_link" />
<origin xyz="${laser_dx + mir_act_wheel_dx} ${laser_dy} ${laser_dz}" rpy="0.0 0.0 ${0.25 * pi}" />
</joint>
<xacro:sick_s300 prefix="${prefix}" link="front_laser_link" topic="f_scan" />
<joint name="${prefix}base_link_to_back_laser_joint" type="fixed">
<parent link="${prefix}base_link" />
<child link="${prefix}back_laser_link" />
<origin xyz="${-laser_dx + mir_act_wheel_dx} ${-laser_dy} ${laser_dz}" rpy="0.0 0.0 ${-0.75 * pi}" />
</joint>
<xacro:sick_s300 prefix="${prefix}" link="back_laser_link" topic="b_scan" />
<!-- Ultrasound sensors -->
<joint name="${prefix}us_1_joint" type="fixed"> <!-- right ultrasound -->
<parent link="${prefix}base_link" />
<child link="${prefix}us_1_frame" />
<origin xyz="0.45 -0.12 0.16 " rpy="0 0 0" /> <!-- from visually matching to the mesh of the MiR -->
</joint>
<link name="${prefix}us_1_frame" />
<joint name="${prefix}us_2_joint" type="fixed"> <!-- left ultrasound -->
<parent link="${prefix}base_link" />
<child link="${prefix}us_2_frame" />
<origin xyz="0.45 0.12 0.16 " rpy="0 0 0" /> <!-- from visually matching to the mesh of the MiR -->
</joint>
<link name="${prefix}us_2_frame" />
<!-- wheels -->
<xacro:actuated_wheel prefix="${prefix}" locationprefix="left" locationright="-1"/>
<xacro:actuated_wheel prefix="${prefix}" locationprefix="right" locationright="1"/>
<xacro:caster_wheel prefix="${prefix}" locationprefix="fl" locationright="-1" wheel_base_dx="${mir_front_caster_wheel_base_dx + mir_act_wheel_dx}"/>
<xacro:caster_wheel prefix="${prefix}" locationprefix="fr" locationright="1" wheel_base_dx="${mir_front_caster_wheel_base_dx + mir_act_wheel_dx}"/>
<xacro:caster_wheel prefix="${prefix}" locationprefix="bl" locationright="-1" wheel_base_dx="${-mir_back_caster_wheel_base_dx + mir_act_wheel_dx}"/>
<xacro:caster_wheel prefix="${prefix}" locationprefix="br" locationright="1" wheel_base_dx="${-mir_back_caster_wheel_base_dx + mir_act_wheel_dx}"/>
<joint name="${prefix}base_link_surface_joint" type="fixed">
<origin xyz="${mir_act_wheel_dx} 0 0.352" rpy="0 0 0" />
<parent link="${prefix}base_link" />
<child link="${prefix}surface" />
<axis xyz="0 0 1" />
</joint>
<link name="${prefix}surface"/>
<xacro:mir_wheel_transmissions prefix="${prefix}"/>
<!-- set the gazebo friction parameters for the wheels -->
<xacro:set_all_wheel_frictions prefix="${prefix}"/>
<xacro:p3d_base_controller prefix="${prefix}" />
</xacro:macro>
</robot>

View File

@@ -0,0 +1,72 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find mir_description)/urdf/include/common_properties.urdf.xacro" />
<xacro:property name="laser_x" value="0.156" />
<xacro:property name="laser_y" value="0.155" />
<xacro:property name="laser_z" value="0.185" />
<xacro:property name="laser_mass" value="1.2" />
<xacro:macro name="sick_s300" params="link topic prefix">
<link name="${prefix}${link}">
<visual>
<origin xyz="0.0 0.0 0.0" rpy="${pi} 0 0" />
<geometry>
<mesh filename="package://mir_description/meshes/visual/sick_lms-100.stl" />
</geometry>
<!-- <xacro:insert_block name="material_yellow" /> -->
<xacro:insert_block name="material_black" />
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="${pi} 0 0" />
<geometry>
<mesh filename="package://mir_description/meshes/visual/sick_lms-100.stl" />
</geometry>
</collision>
<xacro:box_inertial x="${laser_x}" y="${laser_y}" z="${laser_z}" mass="${laser_mass}">
<origin xyz="0 0 0" />
</xacro:box_inertial>
</link>
<gazebo reference="${prefix}${link}">
<!-- <material value="Gazebo/Yellow" /> -->
<material value="Gazebo/FlatBlack" />
<sensor type="ray" name="${prefix}${link}">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>12.5</update_rate>
<ray>
<scan>
<horizontal>
<samples>541</samples>
<resolution>1</resolution> <!-- has to be 1; actual resolution will be computed from number of samples + min_angle/max_angle -->
<!-- <min_angle>-2.35619449615</min_angle>
<max_angle>2.35619449615</max_angle> -->
<min_angle>-2.094395102</min_angle>
<max_angle>2.181661565</max_angle>
</horizontal>
</scan>
<range>
<min>0.05</min>
<max>29.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<!-- Noise parameters based on published spec for S300 achieving
"+-29mm" accuracy at range < 3m (~0.01 of the range) at
1 sigma. -->
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_${link}_controller" filename="libgazebo_ros_laser.so">
<frameName>${prefix}${link}</frameName>
<topicName>${topic}</topicName>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>

View File

@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro"
name="mir" >
<!-- MiR base -->
<xacro:include filename="$(find mir_description)/urdf/include/mir_v1.urdf.xacro" />
<xacro:include filename="$(find mir_description)/urdf/include/common.gazebo.xacro" />
<xacro:arg name="tf_prefix" default="" />
<xacro:property name="tf_prefix_" value="$(arg tf_prefix)" />
<xacro:if value="${tf_prefix_ == ''}">
<xacro:property name="prefix" value="" />
</xacro:if>
<xacro:unless value="${tf_prefix_ == ''}">
<xacro:property name="prefix" value="${tf_prefix_}/" />
</xacro:unless>
<xacro:mir prefix="${prefix}" />
<xacro:controller_plugin_gazebo robot_namespace=""/>
<!-- instead of the controller_plugin_gazebo, you can use the diffdrive controller like this: -->
<!-- <xacro:diff_controller_plugin_gazebo prefix=""
left_wheel_joint="left_wheel_joint"
right_wheel_joint="right_wheel_joint"
wheel_separation="${2*mir_act_wheel_dy}"
wheel_radius="${mir_act_wheel_radius}"/> -->
</robot>