add apm

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/CHANGELOG.rst

@@ -0,0 +1,104 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mir_gazebo
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.1.7 (2023-01-20)
+------------------
+* Don't set cmake_policy CMP0048
+* Contributors: Martin Günther
+
+1.1.6 (2022-06-02)
+------------------
+* Add arg mir_type to launch files and urdfs
+* Rename mir_100 -> mir
+  This is in preparation of mir_250 support.
+* Contributors: Martin Günther
+
+1.1.5 (2022-02-11)
+------------------
+
+1.1.4 (2021-12-10)
+------------------
+* Remove outdated comment
+* 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)
+------------------
+* mir_gazebo: Add model_name arg
+* Move joint_state_publisher to mir_gazebo_common.launch
+* Add optional namespace to launch files
+* 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
+* Contributors: Martin Günther
+
+1.1.0 (2020-06-30)
+------------------
+* Initial release into noetic
+* Contributors: Martin Günther
+
+1.0.6 (2020-06-30)
+------------------
+* Set cmake_policy CMP0048 to fix warning
+* Contributors: Martin Günther
+
+1.0.5 (2020-05-01)
+------------------
+
+1.0.4 (2019-05-06)
+------------------
+* Fix gazebo launch file
+  Before this commit, the mobile base plugin couldn't initialize, because
+  subst_value didn't work.
+* Contributors: Martin Günther
+
+1.0.3 (2019-03-04)
+------------------
+* Add hector_mapping
+* fake_localization.launch: Add frame id args
+* 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
+* 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
+* Fix translation error in odom_comb (`#12 <https://github.com/DFKI-NI/mir_robot/issues/12>`_)
+  Previously, the ekf localization only computed a correct orientation, but the translation still followed the pure odometry data. This led to strange errors where the robot would move sideways (despite only having a diff drive).
+  This PR changes the ekf configuration to not use any position information from the odometry, but to integrate the velocities, which fixes this problem.
+* Split scan_rep117 topic into two separate topics
+  This fixes the problem that the back laser scanner was ignored in the
+  navigation costmap in Gazebo (probably because in Gazebo, both laser
+  scanners have the exact same timestamp).
+* Contributors: Martin Günther, Nils Niemann
+
+1.0.2 (2018-07-30)
+------------------
+* mir_gazebo: Install config directory
+* Contributors: Martin Günther
+
+1.0.1 (2018-07-17)
+------------------
+* gazebo: Replace robot_pose_ekf with robot_localization
+  robot_pose_ekf is deprecated, and has been removed from the navigation
+  stack starting in melodic.
+* gazebo: Adjust ekf.yaml
+* gazebo: Copy robot_localization/ekf_template.yaml
+  ... for modification.
+* Contributors: Martin Günther
+
+1.0.0 (2018-07-12)
+------------------
+* Initial release
+* Contributors: Martin Günther

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(cititruck_gazebo)
+
+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
+  maps
+  sdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+roslaunch_add_file_check(launch)

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/config/ekf.yaml

@@ -0,0 +1,217 @@
+# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+# computation until it receives at least one message from one of the inputs. It will then run continuously at the
+# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+frequency: 40
+
+# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
+# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
+# filter will generate new output. Defaults to 1 / frequency if not specified.
+sensor_timeout: 0.1
+
+# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+# by, for example, an IMU. Defaults to false if unspecified.
+two_d_mode: true
+
+# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
+# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
+# unspecified.
+transform_time_offset: 0.0
+
+# Use this parameter to specify how long the tf listener should wait for a transform to become available.
+# Defaults to 0.0 if unspecified.
+transform_timeout: 0.0
+
+# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
+# unhappy with any settings or data.
+print_diagnostics: true
+
+# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
+# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
+# effects on the performance of the node. Defaults to false if unspecified.
+debug: false
+
+# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
+debug_out_file: /path/to/debug/file.txt
+
+# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+publish_tf: true
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+publish_acceleration: false
+
+# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
+# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
+# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
+# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
+# Here is how to use the following settings:
+# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
+#         odom_frame.
+# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
+#   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
+# from landmark observations) then:
+#     3a. Set your "world_frame" to your map_frame value
+#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
+#         estimation node from robot_localization! However, that instance should *not* fuse the global data.
+map_frame: map                                   # Defaults to "map" if unspecified
+odom_frame: $(arg tf_prefix)odom                 # Defaults to "odom" if unspecified
+base_link_frame: $(arg tf_prefix)base_footprint  # Defaults to "base_link" if unspecified
+world_frame: $(arg tf_prefix)odom                # Defaults to the value of odom_frame if unspecified
+
+# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
+# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
+# default values, and must be specified.
+odom0: odom
+
+# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
+# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
+# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
+# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
+# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
+# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
+# if unspecified, effectively making this parameter required for each sensor.
+# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
+odom0_config: [false, false, false,   # x y z
+               false, false, false,   # roll pitch yaw
+               true,  true,  false,   # vx vy vz
+               false, false, true,    # vroll vpitch vyaw
+               false, false, false]   # ax ay az
+
+# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
+# the size of the subscription queue so that more measurements are fused.
+odom0_queue_size: 10
+
+# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
+# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
+# algorithm.
+odom0_nodelay: false
+
+# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
+# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
+# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
+# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
+# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
+# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
+# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
+# for twist measurements has no effect.
+odom0_differential: false
+
+# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
+# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
+# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
+# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
+odom0_relative: false
+
+# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
+# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
+# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
+# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
+# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
+# the thresholds.
+#odom0_pose_rejection_threshold: 5
+#odom0_twist_rejection_threshold: 1
+
+# Further input parameter examples
+# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
+imu0: imu_data
+imu0_config: [false, false, false,   # x y z
+              false, false, true,    # roll pitch yaw
+              false, false, false,   # vx vy vz
+              false, false, true,    # vroll vpitch vyaw
+              true,  false, false]    # ax ay az
+imu0_nodelay: false
+imu0_differential: false
+imu0_relative: true
+imu0_queue_size: 10
+#imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
+#imu0_twist_rejection_threshold: 0.8                #
+#imu0_linear_acceleration_rejection_threshold: 0.8  #
+
+# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
+# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
+imu0_remove_gravitational_acceleration: false
+
+# [ADVANCED]  The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
+# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
+# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
+# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
+# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
+# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
+# for the velocity variable in question, or decrease the  variance of the variable in question in the measurement
+# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
+# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
+# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
+# inputs, the control term will be ignored.
+# Whether or not we use the control input during predicition. Defaults to false.
+use_control: false
+# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
+# false.
+stamped_control: false
+# The last issued control command will be used in prediction for this period. Defaults to 0.2.
+control_timeout: 0.2
+# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
+control_config: [true, false, false, false, false, true]
+# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
+acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+# Acceleration and deceleration limits are not always the same for robots.
+deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
+# gains
+acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
+# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
+# gains
+deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+
+# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
+# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
+# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
+# However, if users find that a given variable is slow to converge, one approach is to increase the
+# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
+# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
+# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
+# unspecified.
+process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]
+
+# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
+# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
+# question. Users should take care not to use large values for variables that will not be measured directly. The values
+# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
+#if unspecified.
+initial_estimate_covariance: [100.0,0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    100.0,0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    1.0,  0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     10.0, 0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    10.0, 0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/cititruck_empty_world.launch

@@ -0,0 +1,64 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="world_name" default="worlds/empty.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable, but can also be an absolute path -->
+
+  <arg name="robot_x"   default="0.0" />
+  <arg name="robot_y"   default="0.0" />
+  <arg name="robot_yaw" default="0.0" />
+
+  <arg name="robot_type" default="cititruck-01" />
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+
+  <arg name="namespace" default="$(arg tf_prefix)" doc="Namespace to push all topics into."/>
+
+  <group if="$(eval namespace != '')">
+    <group>
+      <remap from="$(arg namespace)/joint_states"                   to="$(arg namespace)/cititruck/joint_states" />
+      <remap from="$(arg namespace)/mobile_base_controller/cmd_vel" to="$(arg namespace)/cmd_vel" />
+      <remap from="$(arg namespace)/mobile_base_controller/odom"    to="$(arg namespace)/odom" />
+
+      <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" value="true" />
+        <arg name="gui" value="$(arg gui)" />
+      </include>
+    </group>
+
+    <group ns="$(arg namespace)">
+      <!-- spawn robot and bring up controllers etc. -->
+      <include file="$(find cititruck_gazebo)/launch/cititruck_gazebo_common.launch">
+        <arg name="robot_x"   value="$(arg robot_x)" />
+        <arg name="robot_y"   value="$(arg robot_y)" />
+        <arg name="robot_yaw" value="$(arg robot_yaw)" />
+        <arg name="robot_type" value="$(arg robot_type)" />
+        <arg name="tf_prefix" value="$(arg tf_prefix)" />
+      </include>
+    </group>
+  </group>
+
+  <!-- Duplicate of the above in case namespace is empty. This is necessary to
+       avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
+  <group unless="$(eval namespace != '')">
+    <group>
+      <remap from="joint_states"                   to="cititruck/joint_states" />
+      <remap from="mobile_base_controller/cmd_vel" to="cmd_vel" />
+      <remap from="mobile_base_controller/odom"    to="odom" />
+
+      <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" value="true" />
+        <arg name="gui" value="$(arg gui)" />
+      </include>
+    </group>
+
+    <!-- spawn robot and bring up controllers etc. -->
+    <include file="$(find cititruck_gazebo)/launch/cititruck_gazebo_common.launch">
+      <arg name="robot_x"   value="$(arg robot_x)" />
+      <arg name="robot_y"   value="$(arg robot_y)" />
+      <arg name="robot_yaw" value="$(arg robot_yaw)" />
+      <arg name="robot_type" value="$(arg robot_type)" />
+      <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    </include>
+  </group>
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/cititruck_gazebo_common.launch

@@ -0,0 +1,71 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot_x"   default="0.0" />
+  <arg name="robot_y"   default="0.0" />
+  <arg name="robot_yaw" default="0.0" />
+
+  <arg name="robot_type" default="cititruck-01" />
+  
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+
+  <arg name="prefix" value="$(arg tf_prefix)/" if="$(eval tf_prefix != '')" /> <!-- $(arg prefix) is used in all the config files! TODO: For multiple robots, create groups when loading the parameters to overwrite the arg? -->
+  <arg name="prefix" value=""                  unless="$(eval tf_prefix != '')" />
+
+  <arg name="model_name" default="cititruck" doc="Name of the Gazebo robot model (needs to be different for each robot)" />
+
+
+  <!-- Load URDF -->
+  <include file="$(find cititruck_description)/launch/upload_cititruck_urdf.launch">
+    <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    <arg name="robot_type" value="$(arg robot_type)" />
+  </include>
+
+  <!-- Spawn the robot into Gazebo -->
+  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model $(arg model_name)
+    -x $(arg robot_x) -y $(arg robot_y) -Y $(arg robot_yaw) " respawn="false"/>
+
+<!-- Load ros_control controller configurations -->
+  <rosparam file="$(find cititruck_description)/config/joint_state_controller.yaml" command="load" />
+  <rosparam file="$(find cititruck_description)/config/steerdrive_controller.yaml" command="load" subst_value="true" />
+
+  <!-- Start the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen"
+    args="joint_state_controller mobile_base_controller"/>
+
+  <!-- EKF -->
+  <include file="$(find cititruck_gazebo)/launch/includes/ekf.launch.xml">
+      <arg name="tf_prefix" value="$(arg prefix)" />
+  </include>
+
+  <!-- Add passive + mimic joints to joint_states topic -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" output="screen" >
+    <rosparam param="source_list">[cititruck/joint_states]</rosparam>
+    <param name="rate" value="200.0" />
+  </node>
+
+  <!-- Robot state publisher -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />
+
+  <!-- Load teleop -->
+  <node name="rqt_robot_steering" pkg="rqt_robot_steering" type="rqt_robot_steering">
+    <param name="default_topic" value="cmd_vel"/>
+    <param name="default_vx_max" value="1.0" />
+    <param name="default_vx_min" value="-1.0" />
+    <param name="default_vw_max" value="1.57079" />
+    <param name="default_vw_min" value="-1.57079" />
+  </node>
+
+  <!-- create combined scan topic (like on real cititruck) -->
+  <node pkg="topic_tools" type="relay" name="l_scan_relay" args="l_scan scan"/>
+  <node pkg="topic_tools" type="relay" name="r_scan_relay" args="r_scan scan"/>
+
+  <node name="l_rep117_laser_filter" pkg="cititruck_driver" type="rep117_filter.py" output="screen">
+    <remap from="scan" to="l_scan" />
+    <remap from="scan_filtered" to="l_scan_rep117" />
+  </node>
+
+  <node name="r_rep117_laser_filter" pkg="cititruck_driver" type="rep117_filter.py" output="screen">
+    <remap from="scan" to="r_scan" />
+    <remap from="scan_filtered" to="r_scan_rep117" />
+  </node>
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/cititruck_maze_world.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="robot_type" default="cititruck-01" />
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+  <arg name="robot_x"   default="0.0" />
+  <arg name="robot_y"   default="0.0" />
+  <arg name="robot_yaw" default="0.0" />
+
+  <include file="$(find cititruck_gazebo)/launch/cititruck_empty_world.launch">
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="robot_type" value="$(arg robot_type)" />
+    <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    <arg name="robot_x"   value="$(arg robot_x)" />
+    <arg name="robot_y"   value="$(arg robot_y)" />
+    <arg name="robot_yaw" value="$(arg robot_yaw)" />
+  </include>
+
+  <include file="$(find cititruck_gazebo)/launch/includes/spawn_maze.launch.xml" />
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/fake_localization.launch

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+
+<launch>
+  <arg name="delta_x" default="0.0" />
+  <arg name="delta_y" default="0.0" />
+  <arg name="delta_yaw" default="0.0" />
+  <arg name="odom_frame_id" default="odom"/>
+  <arg name="base_frame_id" default="base_footprint"/>
+
+  <node name="fake_localization" pkg="fake_localization" type="fake_localization" output="screen">
+    <param name="odom_frame_id" value="$(arg odom_frame_id)"/>
+    <param name="base_frame_id" value="$(arg base_frame_id)"/>
+    <param name="delta_x"       value="$(arg delta_x)" />
+    <param name="delta_y"       value="$(arg delta_y)" />
+    <param name="delta_yaw"     value="$(arg delta_yaw)" />
+  </node>
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/includes/ekf.launch.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="tf_prefix" default="" />
+  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node" clear_params="true" output="screen">
+    <rosparam command="load" file="$(find cititruck_gazebo)/config/ekf.yaml" subst_value="true" />
+  </node>
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/launch/includes/spawn_maze.launch.xml

@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<launch>
+    <node name="spawn_maze" pkg="gazebo_ros" type="spawn_model" args="-sdf
+      -file $(find cititruck_gazebo)/sdf/maze/model.sdf -model walls" output="screen" />
+</launch>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/maps_new/maze.yaml

@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/maps_new/maze_virtual_walls.yaml

@@ -0,0 +1,6 @@
+image: maze_virtual_walls.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/maze.yaml

@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/maze_virtual_walls.yaml

@@ -0,0 +1,6 @@
+image: maze_virtual_walls.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/critical_zones.yaml

@@ -0,0 +1,6 @@
+image: critical_zones.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/direction_zones.yaml

@@ -0,0 +1,6 @@
+image: direction_zones.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/map_empty.yaml

@@ -0,0 +1,6 @@
+image: map_empty.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/maze.yaml

@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [-13.0, -2.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/preferred_zones.yaml

@@ -0,0 +1,6 @@
+image: preferred_zones.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/unpreferred_zones.yaml

@@ -0,0 +1,6 @@
+image: unpreferred_zones.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/maps/warehouse/virtual_walls.yaml

@@ -0,0 +1,6 @@
+image: virtual_walls.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>cititruck_gazebo</name>
+  <version>1.1.7</version>
+  <description>Simulation specific launch and configuration files for the cititruck 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/cititruck_robot</url>
+  <url type="repository">https://github.com/DFKI-NI/cititruck_robot</url>
+  <url type="bugtracker">https://github.com/DFKI-NI/cititruck_robot/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roslaunch</build_depend>
+
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>fake_localization</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>cititruck_description</exec_depend>
+  <exec_depend>cititruck_driver</exec_depend>
+  <exec_depend>robot_localization</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rostopic</exec_depend>
+  <exec_depend>rqt_robot_steering</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
+</package>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/sdf/maze/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>maze</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name>Martin Günther</name>
+        <email>martin.guenther@dfki.de</email>
+    </author>
+    <description></description>
+</model>

Controllers/Packages/robot_gazebo_plugins/cititruck_gazebo/sdf/maze/model.sdf

@@ -0,0 +1,345 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='maze'>
+    <pose frame=''>-0.078283 0.098984 0 0 -0 0</pose>
+    <link name='Wall_0'>
+      <collision name='Wall_0_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_0_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>0.030536 9.925 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_1'>
+      <collision name='Wall_1_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_1_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>9.95554 0 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_2'>
+      <collision name='Wall_2_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_2_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>0.030536 -9.925 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_24'>
+      <collision name='Wall_24_Collision'>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_24_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>5.35089 3.21906 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_25'>
+      <collision name='Wall_25_Collision'>
+        <geometry>
+          <box>
+            <size>5.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_25_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>4.67589 5.76906 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_27'>
+      <collision name='Wall_27_Collision'>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_27_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>7.10914 4.73454 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_28'>
+      <collision name='Wall_28_Collision'>
+        <geometry>
+          <box>
+            <size>3 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_28_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>3 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>8.53414 2.05954 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_3'>
+      <collision name='Wall_3_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_3_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-9.89446 0 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_30'>
+      <collision name='Wall_30_Collision'>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_30_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-4.35914 -2.82889 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_31'>
+      <collision name='Wall_31_Collision'>
+        <geometry>
+          <box>
+            <size>5.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_31_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-7.15914 -5.50389 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_5'>
+      <collision name='Wall_5_Collision'>
+        <geometry>
+          <box>
+            <size>16 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_5_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>16 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-1.89911 1.86906 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_6'>
+      <collision name='Wall_6_Collision'>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_6_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>6.02589 2.54406 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_8'>
+      <collision name='Wall_8_Collision'>
+        <geometry>
+          <box>
+            <size>0.15 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_8_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>6.02589 3.21906 0 0 -0 0</pose>
+    </link>
+    <static>1</static>
+  </model>
+</sdf>