Imu

docs/included_plugins/imu.rst

@@ -0,0 +1,55 @@
+.. image:: ../_static/flatland_logo2.png
+    :width: 250px
+    :align: right
+    :target: ../_static/flatland_logo2.png
+
+
+IMU
+==========
+This plugin provides a simple simulation of an IMU (three-axis accelerometer and three-axis gyroscope).
+
+* The reference frame of the IMU is x-forward, y-left, z-up. Because Flatland is 2D, only the x and y axes are populated with accelerations only the z axis is populated with an angular rate, and the orientation has components only in the z and w fields.
+
+* The measurements are modelled in a simple manner; they are simply the ground truth corrupted with zero-mean Gaussian noise.
+
+* Publishes a `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ message with the simulated measurements
+
+.. code-block:: yaml
+
+  plugins:
+
+      # required, specify Imu type to load the plugin
+    - type: Imu
+
+      # required, name of the plugin
+      name: turtlebot_imu
+
+      # required, body of a model to set location of simulated IMU
+      body: base
+
+      # optional, defaults to true, whether to publish the IMU data
+      enable_imu_pub: true
+
+      # optional, defaults to imu, TF frame name of the IMU
+      imu_frame_id: imu
+
+      # optional, defaults to imu/filtered, the topic on which the noisy imu data is published
+      imu_pub: imu/filtered
+
+      # optional, defaults to imu/ground_truth, the topic on which the true (no noise added) imu data is published
+      ground_truth_pub: imu/ground_truth
+
+      # optional, defaults to [0.0, 0.0, 0.0], the diagonal of the covariance matrix of the zero-mean noise added to the orientation . Note that because the simulation is 2D, only the third value (i.e., yaw noise) is used.
+      orientation_noise: [0.0, 0.0, 0.0]
+
+      # optional, defaults to [0.0, 0.0, 0.0], the diagonal of the covariance matrix of the zero-mean noise added to the angular velocity. Note that because the simulation is 2D, only the third value (i.e., yaw rate noise) is used.
+      angular_velocity_noise: [0.0, 0.0, 0.0]
+
+      # optional, defaults to [0.0, 0.0, 0.0], the diagonal of the covariance matrix of the zero-mean noise added to the linear acceleration. Note that because the simulation is 2D, only the first two values (i.e., x and y acceleration noise) are used.
+      linear_acceleration_noise: [0.0, 0.0, 0.0]
+
+      # optional, defaults to inifinity (i.e., as fast as the simulation updates), rate to publish IMU measurements, in Hz
+      update_rate: 40
+
+      # optional, defaults to true, whether to publish TF
+      broadcast_tf: true

flatland_plugins/CMakeLists.txt

@@ -74,6 +74,7 @@ add_library(flatland_plugins_lib
   src/tricycle_drive.cpp
   src/diff_drive.cpp
   src/dynamics_limits.cpp
+  src/imu.cpp
   src/model_tf_publisher.cpp
   src/update_timer.cpp
   src/bumper.cpp
@@ -153,6 +154,10 @@ if(CATKIN_ENABLE_TESTING)
                     test/diff_drive_test.cpp)
   target_link_libraries(diff_drive_test flatland_plugins_lib)
 
+  add_rostest_gtest(imu_test test/imu_test.test
+                    test/imu_test.cpp)
+  target_link_libraries(imu_test flatland_plugins_lib)
+
   add_rostest_gtest(bumper_test test/bumper_test.test
                     test/bumper_test.cpp)
   target_link_libraries(bumper_test flatland_plugins_lib)

flatland_plugins/flatland_plugins.xml

@@ -8,6 +8,9 @@
   <class type="flatland_plugins::DiffDrive" base_class_type="flatland_server::ModelPlugin">
     <description>Flatland differential drive plugin</description>
   </class>
+  <class type="flatland_plugins::Imu" base_class_type="flatland_server::ModelPlugin">
+    <description>Flatland imu plugin</description>
+  </class>
   <class type="flatland_plugins::ModelTfPublisher" base_class_type="flatland_server::ModelPlugin">
     <description>Publish body transformations</description>
   </class>

flatland_plugins/include/flatland_plugins/diff_drive.h

@@ -76,6 +76,9 @@ class DiffDrive : public flatland_server::ModelPlugin {
   bool enable_odom_pub_;            ///< YAML parameter to enable odom publishing
   bool enable_odom_tf_pub_;         ///< YAML parameter to enable odom tf publishing
   bool enable_twist_pub_;           ///< YAML parameter to enable twist publishing
+  bool twist_in_local_frame_;  ///< YAML parameter to publish velocity in local
+                               /// frame. Original diff drive plugin publishes
+                               /// local velocity wrt to odom frame
   DynamicsLimits angular_dynamics_; ///< Angular dynamics constraints
   DynamicsLimits linear_dynamics_;  ///< Linear dynamics constraints
   double angular_velocity_ = 0.0;

flatland_plugins/include/flatland_plugins/imu.h

@@ -0,0 +1,102 @@
+/*
+ *  ______                   __  __              __
+ * /\  _  \           __    /\ \/\ \            /\ \__
+ * \ \ \L\ \  __  __ /\_\   \_\ \ \ \____    ___\ \ ,_\   ____
+ *  \ \  __ \/\ \/\ \\/\ \  /'_` \ \ '__`\  / __`\ \ \/  /',__\
+ *   \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\
+ *    \ \_\ \_\ \___/  \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/
+ *     \/_/\/_/\/__/    \/_/\/__,_ /\/___/  \/___/  \/__/\/___/
+ * @copyright Copyright 2017 Avidbots Corp.
+ * @name	imu.h
+ * @brief   IMU plugin
+ * @author  Mike Brousseau
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Avidbots Corp.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *      copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Avidbots Corp. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <Box2D/Box2D.h>
+#include <flatland_plugins/update_timer.h>
+#include <flatland_server/model_plugin.h>
+#include <flatland_server/timekeeper.h>
+#include <geometry_msgs/Twist.h>
+#include <random>
+#include <sensor_msgs/Imu.h>
+#include <tf/transform_broadcaster.h>
+
+#ifndef FLATLAND_PLUGINS_IMU_H
+#define FLATLAND_PLUGINS_IMU_H
+
+using namespace flatland_server;
+
+namespace flatland_plugins {
+
+class Imu : public flatland_server::ModelPlugin {
+ public:
+  ros::Publisher imu_pub_;
+  ros::Publisher ground_truth_pub_;
+  Body* body_;
+  sensor_msgs::Imu imu_msg_;
+  sensor_msgs::Imu ground_truth_msg_;
+  UpdateTimer update_timer_;
+  bool enable_imu_pub_;  ///< YAML parameter to enable odom publishing
+
+  std::default_random_engine rng_;
+  std::array<std::normal_distribution<double>, 9> noise_gen_;
+  geometry_msgs::TransformStamped imu_tf_;   ///< tf from body to IMU frame
+  tf::TransformBroadcaster tf_broadcaster_;  ///< broadcast IMU frame
+  std::string imu_frame_id_;
+  bool broadcast_tf_;
+  b2Vec2 linear_vel_local_prev;
+  double pub_rate_;
+  /**
+   * @name          OnInitialize
+   * @brief         override the BeforePhysicsStep method
+   * @param[in]     config The plugin YAML node
+   */
+  void OnInitialize(const YAML::Node& config) override;
+  /**
+   * @name          AfterPhysicsStep
+   * @brief         override the AfterPhysicsStep method
+   * @param[in]     timekeeper Tracks time in flatland
+   */
+  void AfterPhysicsStep(const Timekeeper& timekeeper) override;
+  /**
+   * @name        TwistCallback
+   * @brief       callback to apply twist (velocity and omega)
+   * @param[in]   timestep how much the physics time will increment
+   */
+  void TwistCallback(const geometry_msgs::Twist& msg);
+};
+};
+
+#endif

flatland_plugins/src/diff_drive.cpp

@@ -49,6 +49,7 @@
 #include <flatland_server/debug_visualization.h>
 #include <flatland_server/model_plugin.h>
 #include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/TwistWithCovarianceStamped.h>
 #include <pluginlib/class_list_macros.h>
 #include <ros/ros.h>
 #include <tf/tf.h>
@@ -64,6 +65,8 @@ void DiffDrive::OnInitialize(const YAML::Node& config) {
   enable_odom_pub_ = reader.Get<bool>("enable_odom_pub", true);
   enable_odom_tf_pub_ = reader.Get<bool>("enable_odom_tf_pub", true);
   enable_twist_pub_ = reader.Get<bool>("enable_twist_pub", true);
+  twist_in_local_frame_ = reader.Get<bool>("twist_in_local_frame", true);
+
   std::string body_name = reader.Get<std::string>("body");
   std::string odom_frame_id = reader.Get<std::string>("odom_frame_id", "odom");
   std::string ground_truth_frame_id =
@@ -126,7 +129,8 @@ void DiffDrive::OnInitialize(const YAML::Node& config) {
   }
 
   if (enable_twist_pub_) {
-    twist_pub_ = nh_.advertise<geometry_msgs::TwistStamped>(twist_pub_topic, 1);
+    twist_pub_ = nh_.advertise<geometry_msgs::TwistWithCovarianceStamped>(
+        twist_pub_topic, 1);
   }
 
   // init the values for the messages
@@ -220,12 +224,22 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
     ground_truth_msg_.pose.pose.position.z = 0;
     ground_truth_msg_.pose.pose.orientation =
         tf::createQuaternionMsgFromYaw(angle);
-    ground_truth_msg_.twist.twist.linear.x = linear_vel_local.x;
-    ground_truth_msg_.twist.twist.linear.y = linear_vel_local.y;
+
     ground_truth_msg_.twist.twist.linear.z = 0;
     ground_truth_msg_.twist.twist.angular.x = 0;
     ground_truth_msg_.twist.twist.angular.y = 0;
-    ground_truth_msg_.twist.twist.angular.z = angular_vel;
+    if (twist_in_local_frame_) {
+      // change frame of velocity
+      ground_truth_msg_.twist.twist.linear.x =
+          cos(-angle) * linear_vel_local.x - sin(-angle) * linear_vel_local.y;
+      ground_truth_msg_.twist.twist.linear.y =
+          sin(-angle) * linear_vel_local.x + cos(-angle) * linear_vel_local.y;
+      ground_truth_msg_.twist.twist.angular.z = angular_vel;
+    } else {
+      ground_truth_msg_.twist.twist.linear.x = linear_vel_local.x;
+      ground_truth_msg_.twist.twist.linear.y = linear_vel_local.y;
+      ground_truth_msg_.twist.twist.angular.z = angular_vel;
+    }
 
     // add the noise to odom messages
     odom_msg_.header.stamp = timekeeper.GetSimTime();
@@ -247,17 +261,20 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
     if (enable_twist_pub_) {
       // Transform global frame velocity into local frame to simulate encoder
       // readings
-      geometry_msgs::TwistStamped twist_pub_msg;
+      geometry_msgs::TwistWithCovarianceStamped twist_pub_msg;
       twist_pub_msg.header.stamp = timekeeper.GetSimTime();
       twist_pub_msg.header.frame_id = odom_msg_.child_frame_id;
 
       // Forward velocity in twist.linear.x
-      twist_pub_msg.twist.linear.x = cos(angle) * linear_vel_local.x +
-                                     sin(angle) * linear_vel_local.y +
-                                     noise_gen_[3](rng_);
+      twist_pub_msg.twist.twist.linear.x = cos(angle) * linear_vel_local.x +
+                                           sin(angle) * linear_vel_local.y +
+                                           noise_gen_[3](rng_);
 
       // Angular velocity in twist.angular.z
-      twist_pub_msg.twist.angular.z = angular_vel + noise_gen_[5](rng_);
+      twist_pub_msg.twist.twist.angular.z = angular_vel + noise_gen_[5](rng_);
+
+      twist_pub_msg.twist.covariance = odom_msg_.twist.covariance;
+
       twist_pub_.publish(twist_pub_msg);
     }
 
@@ -278,4 +295,4 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
 }
 
 PLUGINLIB_EXPORT_CLASS(flatland_plugins::DiffDrive,
-                       flatland_server::ModelPlugin)
+                       flatland_server::ModelPlugin)