Pull in 3rd party enhancement: IMU plugin

flatland_plugins/CMakeLists.txt

@@ -72,6 +72,7 @@ add_library(flatland_plugins_lib
   src/laser.cpp
   src/tricycle_drive.cpp
   src/diff_drive.cpp
+  src/imu.cpp
   src/model_tf_publisher.cpp
   src/update_timer.cpp
   src/bumper.cpp
@@ -148,6 +149,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

@@ -71,8 +71,12 @@ class DiffDrive : public flatland_server::ModelPlugin {
   nav_msgs::Odometry ground_truth_msg_;
   UpdateTimer update_timer_;
   tf::TransformBroadcaster tf_broadcaster;  ///< For publish ROS TF
-  bool enable_odom_pub_;   ///< YAML parameter to enable odom publishing
-  bool enable_twist_pub_;  ///< YAML parameter to enable twist publishing
+  bool enable_odom_pub_;       ///< YAML parameter to enable odom publishing
+  bool enable_twist_pub_;      ///< YAML parameter to enable twist publishing
+  bool broadcast_tf_;          ///< YAML parameter to enable tf broadcasting
+  bool twist_in_local_frame_;  ///< YAML parameter to publish velocity in local
+                               /// frame. Original diff drive plugin publishes
+  /// local velocity wrt to odom frame
 
   std::default_random_engine rng_;
   std::array<std::normal_distribution<double>, 6> noise_gen_;
@@ -98,4 +102,4 @@ class DiffDrive : public flatland_server::ModelPlugin {
 };
 };
 
-#endif
+#endif

flatland_plugins/include/flatland_plugins/imu.h

@@ -0,0 +1,101 @@
+/*
+ *  ______                   __  __              __
+ * /\  _  \           __    /\ \/\ \            /\ \__
+ * \ \ \L\ \  __  __ /\_\   \_\ \ \ \____    ___\ \ ,_\   ____
+ *  \ \  __ \/\ \/\ \\/\ \  /'_` \ \ '__`\  / __`\ \ \/  /',__\
+ *   \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\
+ *    \ \_\ \_\ \___/  \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/
+ *     \/_/\/_/\/__/    \/_/\/__,_ /\/___/  \/___/  \/__/\/___/
+ * @copyright Copyright 2017 Avidbots Corp.
+ * @name	diff_drive.h
+ * @brief   Diff drive 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 <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 laser frame
+  tf::TransformBroadcaster tf_broadcaster_;  ///< broadcast laser 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          BeforePhysicsStep
+   * @brief         override the BeforePhysicsStep method
+   * @param[in]     config The plugin YAML node
+   */
+  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>
@@ -63,6 +64,9 @@ void DiffDrive::OnInitialize(const YAML::Node& config) {
   YamlReader reader(config);
   enable_odom_pub_ = reader.Get<bool>("enable_odom_pub", true);
   enable_twist_pub_ = reader.Get<bool>("enable_twist_pub", true);
+  broadcast_tf_ = reader.Get<bool>("broadcast_tf", 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");
 
@@ -117,11 +121,13 @@ 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
-  ground_truth_msg_.header.frame_id = odom_frame_id;
+  ground_truth_msg_.header.frame_id =
+      tf::resolve("", GetModel()->NameSpaceTF(odom_frame_id));
   ground_truth_msg_.child_frame_id =
       tf::resolve("", GetModel()->NameSpaceTF(body_->name_));
   ground_truth_msg_.twist.covariance.fill(0);
@@ -170,8 +176,8 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
 
   if (publish) {
     // get the state of the body and publish the data
-    b2Vec2 linear_vel_local =
-        b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0));
+    b2Vec2 linear_vel_local = b2body->GetLinearVelocityFromLocalPoint(
+        b2Vec2(0, 0));  // local velocity is in global frame!
     float angular_vel = b2body->GetAngularVelocity();
 
     ground_truth_msg_.header.stamp = ros::Time::now();
@@ -180,12 +186,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_ == true) {
+      // 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 = ros::Time::now();
@@ -207,29 +223,34 @@ 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 = ros::Time::now();
       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);
     }
 
     // publish odom tf
-    geometry_msgs::TransformStamped odom_tf;
-    odom_tf.header = odom_msg_.header;
-    odom_tf.child_frame_id = odom_msg_.child_frame_id;
-    odom_tf.transform.translation.x = odom_msg_.pose.pose.position.x;
-    odom_tf.transform.translation.y = odom_msg_.pose.pose.position.y;
-    odom_tf.transform.translation.z = 0;
-    odom_tf.transform.rotation = odom_msg_.pose.pose.orientation;
-    tf_broadcaster.sendTransform(odom_tf);
+    if (broadcast_tf_) {
+      geometry_msgs::TransformStamped odom_tf;
+      odom_tf.header = odom_msg_.header;
+      odom_tf.child_frame_id = odom_msg_.child_frame_id;
+      odom_tf.transform.translation.x = odom_msg_.pose.pose.position.x;
+      odom_tf.transform.translation.y = odom_msg_.pose.pose.position.y;
+      odom_tf.transform.translation.z = 0;
+      odom_tf.transform.rotation = odom_msg_.pose.pose.orientation;
+      tf_broadcaster.sendTransform(odom_tf);
+    }
   }
 
   // we apply the twist velocities, this must be done every physics step to make
@@ -256,4 +277,4 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
 }
 
 PLUGINLIB_EXPORT_CLASS(flatland_plugins::DiffDrive,
-                       flatland_server::ModelPlugin)
+                       flatland_server::ModelPlugin)