From 4e1d3d12bc89e2790dc3d29c74002c473276582b Mon Sep 17 00:00:00 2001 From: marbosjo Date: Thu, 4 Jan 2018 16:12:25 +0100 Subject: [PATCH 01/10] added initial version of imu plugin --- flatland_plugins/CMakeLists.txt | 5 + flatland_plugins/flatland_plugins.xml | 3 + .../include/flatland_plugins/imu.h | 101 ++++++++ flatland_plugins/src/imu.cpp | 239 ++++++++++++++++++ flatland_plugins/test/imu_test.cpp | 70 +++++ flatland_plugins/test/imu_test.test | 9 + 6 files changed, 427 insertions(+) create mode 100644 flatland_plugins/include/flatland_plugins/imu.h create mode 100644 flatland_plugins/src/imu.cpp create mode 100644 flatland_plugins/test/imu_test.cpp create mode 100644 flatland_plugins/test/imu_test.test diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index d177b6cd..d1e78b0d 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/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) diff --git a/flatland_plugins/flatland_plugins.xml b/flatland_plugins/flatland_plugins.xml index 7b9f985a..d3d26571 100644 --- a/flatland_plugins/flatland_plugins.xml +++ b/flatland_plugins/flatland_plugins.xml @@ -8,6 +8,9 @@ Flatland differential drive plugin + + Flatland imu plugin + Publish body transformations diff --git a/flatland_plugins/include/flatland_plugins/imu.h b/flatland_plugins/include/flatland_plugins/imu.h new file mode 100644 index 00000000..040bdeab --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +#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, 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 diff --git a/flatland_plugins/src/imu.cpp b/flatland_plugins/src/imu.cpp new file mode 100644 index 00000000..e5ea270f --- /dev/null +++ b/flatland_plugins/src/imu.cpp @@ -0,0 +1,239 @@ +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name Imu.cpp + * @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 +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_plugins { + +void Imu::OnInitialize(const YAML::Node& config) { + YamlReader reader(config); + enable_imu_pub_ = reader.Get("enable_imu_pub", true); + + std::string body_name = reader.Get("body"); + imu_frame_id_ = reader.Get("imu_frame_id", "imu"); + + std::string imu_topic = reader.Get("imu_pub", "imu/filtered"); + std::string ground_truth_topic = + reader.Get("ground_truth_pub", "imu/ground_truth"); + + // noise are in the form of linear x, linear y, angular variances + std::vector orientation_noise = + reader.GetList("orientation_noise", {0, 0, 0}, 3, 3); + std::vector angular_velocity_noise = + reader.GetList("angular_velocity_noise", {0, 0, 0}, 3, 3); + std::vector linear_acceleration_noise = + reader.GetList("linear_acceleration_noise", {0, 0, 0}, 3, 3); + + pub_rate_ = + reader.Get("pub_rate", std::numeric_limits::infinity()); + update_timer_.SetRate(pub_rate_); + + broadcast_tf_ = true; + + // by default the covariance diagonal is the variance of actual noise + // generated, non-diagonal elements are zero assuming the noises are + // independent, we also don't care about linear z, angular x, and angular y + std::array orientation_covar_default = {0}; + orientation_covar_default[0] = orientation_noise[0]; + orientation_covar_default[4] = orientation_noise[1]; + orientation_covar_default[8] = orientation_noise[2]; + + std::array angular_velocity_covar_default = {0}; + angular_velocity_covar_default[0] = angular_velocity_noise[0]; + angular_velocity_covar_default[4] = angular_velocity_noise[1]; + angular_velocity_covar_default[8] = angular_velocity_noise[2]; + + std::array linear_acceleration_covar_default = {0}; + linear_acceleration_covar_default[0] = linear_acceleration_noise[0]; + linear_acceleration_covar_default[4] = linear_acceleration_noise[1]; + linear_acceleration_covar_default[8] = linear_acceleration_noise[2]; + + auto orientation_covar = reader.GetArray( + "orientation_covariance", orientation_covar_default); + + auto angular_velocity_covar = reader.GetArray( + "angular_velocity_covariance", angular_velocity_covar_default); + + auto linear_acceleration_covar = reader.GetArray( + "linear_acceleration_covariance", linear_acceleration_covar_default); + + reader.EnsureAccessedAllKeys(); + + body_ = GetModel()->GetBody(body_name); + if (body_ == nullptr) { + throw YAMLException("Body with name " + Q(body_name) + " does not exist"); + } + + imu_pub_ = nh_.advertise(imu_topic, 1); + ground_truth_pub_ = nh_.advertise(ground_truth_topic, 1); + + // init the values for the messages + ground_truth_msg_.header.frame_id = imu_frame_id_; + tf::resolve("", GetModel()->NameSpaceTF(body_->name_)); + ground_truth_msg_.orientation_covariance.fill(0); + ground_truth_msg_.angular_velocity_covariance.fill(0); + ground_truth_msg_.linear_acceleration_covariance.fill(0); + imu_msg_ = ground_truth_msg_; + + // copy from std::array to boost array + for (int i = 0; i < 9; i++) { + imu_msg_.orientation_covariance[i] = orientation_covar[i]; + imu_msg_.angular_velocity_covariance[i] = angular_velocity_covar[i]; + imu_msg_.linear_acceleration_covariance[i] = linear_acceleration_covar[i]; + } + + // init the random number generators + std::random_device rd; + rng_ = std::default_random_engine(rd()); + for (int i = 0; i < 3; i++) { + // variance is standard deviation squared + noise_gen_[i] = + std::normal_distribution(0.0, sqrt(orientation_noise[i])); + } + for (int i = 0; i < 3; i++) { + // variance is standard deviation squared + noise_gen_[i + 3] = + std::normal_distribution(0.0, sqrt(angular_velocity_noise[i])); + } + for (int i = 0; i < 3; i++) { + // variance is standard deviation squared + noise_gen_[i + 6] = std::normal_distribution( + 0.0, sqrt(linear_acceleration_noise[i])); + } + + imu_tf_.header.frame_id = tf::resolve( + "", GetModel()->NameSpaceTF(body_->GetName())); // Todo: parent_tf param + imu_tf_.child_frame_id = + tf::resolve("", GetModel()->NameSpaceTF(imu_frame_id_)); + imu_tf_.transform.translation.x = 0; // origin_.x; TODO: read position + imu_tf_.transform.translation.y = 0; // origin_.y; + imu_tf_.transform.translation.z = 0; + imu_tf_.transform.rotation.x = 0; // q.x(); + imu_tf_.transform.rotation.y = 0; // q.y(); + imu_tf_.transform.rotation.z = 0; // q.z(); + imu_tf_.transform.rotation.w = 1; // q.w(); + + ROS_DEBUG_NAMED( + "Imu", + "Initialized with params body(%p %s) imu_frame_id(%s) " + "imu_pub(%s) ground_truth_pub(%s) " + "orientation_noise({%f,%f,%f}) angular_velocity_noise({%f,%f,%f}) " + "linear_acceleration_velocity({%f,%f,%f}) " + "pub_rate(%f)\n", + body_, body_->name_.c_str(), imu_frame_id_.c_str(), imu_topic.c_str(), + ground_truth_topic.c_str(), orientation_noise[0], orientation_noise[1], + orientation_noise[2], angular_velocity_noise[0], + angular_velocity_noise[1], angular_velocity_noise[2], + linear_acceleration_noise[0], linear_acceleration_noise[1], + linear_acceleration_noise[2], pub_rate_); +} + +void Imu::AfterPhysicsStep(const Timekeeper& timekeeper) { + bool publish = update_timer_.CheckUpdate(timekeeper); + + b2Body* b2body = body_->physics_body_; + + b2Vec2 position = b2body->GetPosition(); + float angle = b2body->GetAngle(); + b2Vec2 linear_vel_local = + b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0)); + float angular_vel = b2body->GetAngularVelocity(); + + if (publish) { + // get the state of the body and publish the data + + ground_truth_msg_.header.stamp = ros::Time::now(); + ground_truth_msg_.orientation = tf::createQuaternionMsgFromYaw(angle); + ground_truth_msg_.angular_velocity.z = angular_vel; + + double global_acceleration_x = + (linear_vel_local.x - linear_vel_local_prev.x) * pub_rate_; + double global_acceleration_y = + (linear_vel_local.y - linear_vel_local_prev.y) * pub_rate_; + + ground_truth_msg_.linear_acceleration.x = + cos(angle) * global_acceleration_x + sin(angle) * global_acceleration_y; + ground_truth_msg_.linear_acceleration.y = + sin(angle) * global_acceleration_x + cos(angle) * global_acceleration_y; + + // ROS_INFO_STREAM_THROTTLE( + // 1, "" << linear_vel_local.x << " " << linear_vel_local_prev.x << " " + //<< pub_rate_); + // add the noise to odom messages + imu_msg_.header.stamp = ros::Time::now(); + + imu_msg_.orientation = + tf::createQuaternionMsgFromYaw(angle + noise_gen_[2](rng_)); + + imu_msg_.angular_velocity = ground_truth_msg_.angular_velocity; + imu_msg_.angular_velocity.z += noise_gen_[5](rng_); + + imu_msg_.linear_acceleration = ground_truth_msg_.linear_acceleration; + imu_msg_.linear_acceleration.x += noise_gen_[6](rng_); + imu_msg_.linear_acceleration.y += noise_gen_[7](rng_); + + if (enable_imu_pub_) { + ground_truth_pub_.publish(ground_truth_msg_); + imu_pub_.publish(imu_msg_); + } + linear_vel_local_prev = linear_vel_local; + } + + if (broadcast_tf_) { + imu_tf_.header.stamp = ros::Time::now(); + tf_broadcaster_.sendTransform(imu_tf_); + } +} +} + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::Imu, flatland_server::ModelPlugin) diff --git a/flatland_plugins/test/imu_test.cpp b/flatland_plugins/test/imu_test.cpp new file mode 100644 index 00000000..c4004c72 --- /dev/null +++ b/flatland_plugins/test/imu_test.cpp @@ -0,0 +1,70 @@ +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name imu_test.cpp + * @brief test diff drive plugin + * @author Chunshang Li + * + * 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 +#include +#include +#include +#include + +TEST(ImuPluginTest, load_test) { + pluginlib::ClassLoader loader( + "flatland_server", "flatland_server::ModelPlugin"); + + try { + boost::shared_ptr plugin = + loader.createInstance("flatland_plugins::Imu"); + } catch (pluginlib::PluginlibException& e) { + FAIL() << "Failed to load imu plugin. " << e.what(); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "imu_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_plugins/test/imu_test.test b/flatland_plugins/test/imu_test.test new file mode 100644 index 00000000..857fcd53 --- /dev/null +++ b/flatland_plugins/test/imu_test.test @@ -0,0 +1,9 @@ + + + + From 3b48d67dddc46d40ec40940e1f5b642b5419540f Mon Sep 17 00:00:00 2001 From: marbosjo Date: Mon, 21 May 2018 14:43:51 +0200 Subject: [PATCH 02/10] publish twist with covariance --- flatland_plugins/src/diff_drive.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/flatland_plugins/src/diff_drive.cpp b/flatland_plugins/src/diff_drive.cpp index c81ec869..e3cbd5e8 100644 --- a/flatland_plugins/src/diff_drive.cpp +++ b/flatland_plugins/src/diff_drive.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -117,7 +118,8 @@ void DiffDrive::OnInitialize(const YAML::Node& config) { } if (enable_twist_pub_) { - twist_pub_ = nh_.advertise(twist_pub_topic, 1); + twist_pub_ = nh_.advertise( + twist_pub_topic, 1); } // init the values for the messages @@ -207,17 +209,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 = 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); } From dc96bbf56b203578e65b45b97ae6d1bce49b4840 Mon Sep 17 00:00:00 2001 From: marbosjo Date: Mon, 21 May 2018 14:44:55 +0200 Subject: [PATCH 03/10] diff_drive: added parameter broadcast_tf to enable tf broadcast from odom to base --- .../include/flatland_plugins/diff_drive.h | 3 ++- flatland_plugins/src/diff_drive.cpp | 21 +++++++++++-------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/flatland_plugins/include/flatland_plugins/diff_drive.h b/flatland_plugins/include/flatland_plugins/diff_drive.h index 832663bc..bf04d87a 100644 --- a/flatland_plugins/include/flatland_plugins/diff_drive.h +++ b/flatland_plugins/include/flatland_plugins/diff_drive.h @@ -73,6 +73,7 @@ class DiffDrive : public flatland_server::ModelPlugin { 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 broadcast_tf_; std::default_random_engine rng_; std::array, 6> noise_gen_; @@ -98,4 +99,4 @@ class DiffDrive : public flatland_server::ModelPlugin { }; }; -#endif \ No newline at end of file +#endif diff --git a/flatland_plugins/src/diff_drive.cpp b/flatland_plugins/src/diff_drive.cpp index e3cbd5e8..20eb005e 100644 --- a/flatland_plugins/src/diff_drive.cpp +++ b/flatland_plugins/src/diff_drive.cpp @@ -64,6 +64,7 @@ void DiffDrive::OnInitialize(const YAML::Node& config) { YamlReader reader(config); enable_odom_pub_ = reader.Get("enable_odom_pub", true); enable_twist_pub_ = reader.Get("enable_twist_pub", true); + broadcast_tf_ = reader.Get("broadcast_tf", true); std::string body_name = reader.Get("body"); std::string odom_frame_id = reader.Get("odom_frame_id", "odom"); @@ -227,14 +228,16 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { } // 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 @@ -261,4 +264,4 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { } PLUGINLIB_EXPORT_CLASS(flatland_plugins::DiffDrive, - flatland_server::ModelPlugin) \ No newline at end of file + flatland_server::ModelPlugin) From 66a1e0abf0a698ab2119ef937ec01bcc0128822a Mon Sep 17 00:00:00 2001 From: marbosjo Date: Mon, 28 May 2018 16:55:37 +0200 Subject: [PATCH 04/10] imu: now reads parameter for broadcasting tf --- flatland_plugins/src/imu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flatland_plugins/src/imu.cpp b/flatland_plugins/src/imu.cpp index e5ea270f..75cb994b 100644 --- a/flatland_plugins/src/imu.cpp +++ b/flatland_plugins/src/imu.cpp @@ -78,7 +78,7 @@ void Imu::OnInitialize(const YAML::Node& config) { reader.Get("pub_rate", std::numeric_limits::infinity()); update_timer_.SetRate(pub_rate_); - broadcast_tf_ = true; + broadcast_tf_ = reader.Get("broadcast_tf", true); // by default the covariance diagonal is the variance of actual noise // generated, non-diagonal elements are zero assuming the noises are From 09c62e1aa7aafaabfd75acc7a8279e8727a91ea5 Mon Sep 17 00:00:00 2001 From: marbosjo Date: Mon, 28 May 2018 16:56:04 +0200 Subject: [PATCH 05/10] diff_drive: added flatland tf resolving for odom frame --- flatland_plugins/src/diff_drive.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flatland_plugins/src/diff_drive.cpp b/flatland_plugins/src/diff_drive.cpp index 20eb005e..10950dac 100644 --- a/flatland_plugins/src/diff_drive.cpp +++ b/flatland_plugins/src/diff_drive.cpp @@ -124,7 +124,8 @@ void DiffDrive::OnInitialize(const YAML::Node& config) { } // 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); From 760131501c0b346b88ef9e374e0f59f444e737da Mon Sep 17 00:00:00 2001 From: marbosjo Date: Tue, 5 Jun 2018 14:14:55 +0200 Subject: [PATCH 06/10] diff_drive: added parameter to publish twist in robot frame --- .../include/flatland_plugins/diff_drive.h | 9 +++++--- flatland_plugins/src/diff_drive.cpp | 22 ++++++++++++++----- 2 files changed, 23 insertions(+), 8 deletions(-) diff --git a/flatland_plugins/include/flatland_plugins/diff_drive.h b/flatland_plugins/include/flatland_plugins/diff_drive.h index bf04d87a..418b72df 100644 --- a/flatland_plugins/include/flatland_plugins/diff_drive.h +++ b/flatland_plugins/include/flatland_plugins/diff_drive.h @@ -71,9 +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 broadcast_tf_; + 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, 6> noise_gen_; diff --git a/flatland_plugins/src/diff_drive.cpp b/flatland_plugins/src/diff_drive.cpp index d7aa787b..6b01f4e2 100644 --- a/flatland_plugins/src/diff_drive.cpp +++ b/flatland_plugins/src/diff_drive.cpp @@ -65,6 +65,8 @@ void DiffDrive::OnInitialize(const YAML::Node& config) { enable_odom_pub_ = reader.Get("enable_odom_pub", true); enable_twist_pub_ = reader.Get("enable_twist_pub", true); broadcast_tf_ = reader.Get("broadcast_tf", true); + twist_in_local_frame_ = reader.Get("twist_in_local_frame", true); + std::string body_name = reader.Get("body"); std::string odom_frame_id = reader.Get("odom_frame_id", "odom"); @@ -174,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(); @@ -184,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(); From 27773a680cb36fdf23823815648183462bdacb6a Mon Sep 17 00:00:00 2001 From: Marc Gallant Date: Tue, 21 Nov 2023 14:25:40 -0500 Subject: [PATCH 07/10] Adding missing header. --- flatland_plugins/include/flatland_plugins/imu.h | 1 + 1 file changed, 1 insertion(+) diff --git a/flatland_plugins/include/flatland_plugins/imu.h b/flatland_plugins/include/flatland_plugins/imu.h index 040bdeab..cc70f7a3 100644 --- a/flatland_plugins/include/flatland_plugins/imu.h +++ b/flatland_plugins/include/flatland_plugins/imu.h @@ -49,6 +49,7 @@ #include #include #include +#include #include #include From 89af75f3cc31bf4697b8ac003a83d359b67c8da9 Mon Sep 17 00:00:00 2001 From: Marc Gallant Date: Wed, 22 Nov 2023 09:38:15 -0500 Subject: [PATCH 08/10] Fixed documentation and comment errors. --- flatland_plugins/include/flatland_plugins/imu.h | 14 +++++++------- flatland_plugins/test/imu_test.test | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/flatland_plugins/include/flatland_plugins/imu.h b/flatland_plugins/include/flatland_plugins/imu.h index cc70f7a3..fa90143a 100644 --- a/flatland_plugins/include/flatland_plugins/imu.h +++ b/flatland_plugins/include/flatland_plugins/imu.h @@ -7,8 +7,8 @@ * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ * @copyright Copyright 2017 Avidbots Corp. - * @name diff_drive.h - * @brief Diff drive plugin + * @name imu.h + * @brief IMU plugin * @author Mike Brousseau * * Software License Agreement (BSD License) @@ -72,8 +72,8 @@ class Imu : public flatland_server::ModelPlugin { std::default_random_engine rng_; std::array, 9> noise_gen_; - geometry_msgs::TransformStamped imu_tf_; ///< tf from body to laser frame - tf::TransformBroadcaster tf_broadcaster_; ///< broadcast laser frame + 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; @@ -85,9 +85,9 @@ class Imu : public flatland_server::ModelPlugin { */ void OnInitialize(const YAML::Node& config) override; /** - * @name BeforePhysicsStep - * @brief override the BeforePhysicsStep method - * @param[in] config The plugin YAML node + * @name AfterPhysicsStep + * @brief override the AfterPhysicsStep method + * @param[in] timekeeper Tracks time in flatland */ void AfterPhysicsStep(const Timekeeper& timekeeper) override; /** diff --git a/flatland_plugins/test/imu_test.test b/flatland_plugins/test/imu_test.test index 857fcd53..c3fc942f 100644 --- a/flatland_plugins/test/imu_test.test +++ b/flatland_plugins/test/imu_test.test @@ -1,5 +1,5 @@