Skip to content

Commit

Permalink
Partial ros2 IMU plugin port
Browse files Browse the repository at this point in the history
  • Loading branch information
josephduchesne committed Nov 15, 2024
1 parent d4e9a68 commit 28091d8
Show file tree
Hide file tree
Showing 5 changed files with 65 additions and 64 deletions.
21 changes: 11 additions & 10 deletions flatland_plugins/include/flatland_plugins/imu.h
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,11 @@
#include <flatland_plugins/update_timer.h>
#include <flatland_server/model_plugin.h>
#include <flatland_server/timekeeper.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/msg/twist.hpp>
#include <random>
#include <sensor_msgs/Imu.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/msg/imu.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <rclcpp/rclcpp.hpp>

#ifndef FLATLAND_PLUGINS_IMU_H
#define FLATLAND_PLUGINS_IMU_H
Expand All @@ -62,18 +63,18 @@ namespace flatland_plugins {

class Imu : public flatland_server::ModelPlugin {
public:
ros::Publisher imu_pub_;
ros::Publisher ground_truth_pub_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr ground_truth_pub_;
Body* body_;
sensor_msgs::Imu imu_msg_;
sensor_msgs::Imu ground_truth_msg_;
sensor_msgs::msg::Imu imu_msg_;
sensor_msgs::msg::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
geometry_msgs::msg::TransformStamped imu_tf_; ///< tf from body to IMU frame
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_; ///< broadcast GPS frame
std::string imu_frame_id_;
bool broadcast_tf_;
b2Vec2 linear_vel_local_prev;
Expand All @@ -95,7 +96,7 @@ class Imu : public flatland_server::ModelPlugin {
* @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);
void TwistCallback(const geometry_msgs::msg::Twist& msg);
};
};

Expand Down
3 changes: 2 additions & 1 deletion flatland_plugins/src/diff_drive.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,8 @@
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <geometry_msgs/msg/TwistWithCovarianceStamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <pluginlib/class_list_macros.hpp>
Expand Down
74 changes: 40 additions & 34 deletions flatland_plugins/src/imu.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,15 +48,14 @@
#include <flatland_plugins/imu.h>
#include <flatland_server/debug_visualization.h>
#include <flatland_server/model_plugin.h>
#include <geometry_msgs/TransformStamped.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <pluginlib/class_list_macros.hpp>
#include <tf2/LinearMath/Quaternion.h>

namespace flatland_plugins {

void Imu::OnInitialize(const YAML::Node& config) {
YamlReader reader(config);
YamlReader reader(node_, config);
enable_imu_pub_ = reader.Get<bool>("enable_imu_pub", true);

std::string body_name = reader.Get<std::string>("body");
Expand Down Expand Up @@ -114,12 +113,12 @@ void Imu::OnInitialize(const YAML::Node& config) {
throw YAMLException("Body with name " + Q(body_name) + " does not exist");
}

imu_pub_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 1);
ground_truth_pub_ = nh_.advertise<sensor_msgs::Imu>(ground_truth_topic, 1);
imu_pub_ = node_->create_publisher<sensor_msgs::msg::Imu>(imu_topic, 1);
ground_truth_pub_ = node_->create_publisher<sensor_msgs::msg::Imu>(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_));
// 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);
Expand Down Expand Up @@ -151,10 +150,8 @@ void Imu::OnInitialize(const YAML::Node& config) {
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_.header.frame_id = GetModel()->NameSpaceTF(body_->GetName()); // Todo: parent_tf param
imu_tf_.child_frame_id = 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;
Expand All @@ -163,19 +160,19 @@ void Imu::OnInitialize(const YAML::Node& config) {
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_);
RCLCPP_DEBUG(
rclcpp::get_logger("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) {
Expand All @@ -192,8 +189,13 @@ void Imu::AfterPhysicsStep(const Timekeeper& timekeeper) {
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_.header.stamp = timekeeper.GetSimTime();
tf2::Quaternion q;
q.setRPY(0, 0, angle);
ground_truth_msg_.orientation.x = q.x();
ground_truth_msg_.orientation.y = q.y();
ground_truth_msg_.orientation.z = q.z();
ground_truth_msg_.orientation.w = q.w();
ground_truth_msg_.angular_velocity.z = angular_vel;

double global_acceleration_x =
Expand All @@ -210,10 +212,14 @@ void Imu::AfterPhysicsStep(const Timekeeper& timekeeper) {
// 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_.header.stamp = timekeeper.GetSimTime();

imu_msg_.orientation =
tf::createQuaternionMsgFromYaw(angle + noise_gen_[2](rng_));
tf2::Quaternion q2;
q2.setRPY(0, 0, angle + noise_gen_[2](rng_));
imu_msg_.orientation.x = q.x();
imu_msg_.orientation.y = q.y();
imu_msg_.orientation.z = q.z();
imu_msg_.orientation.w = q.w();

imu_msg_.angular_velocity = ground_truth_msg_.angular_velocity;
imu_msg_.angular_velocity.z += noise_gen_[5](rng_);
Expand All @@ -223,15 +229,15 @@ void Imu::AfterPhysicsStep(const Timekeeper& timekeeper) {
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_);
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_);
imu_tf_.header.stamp = timekeeper.GetSimTime();
tf_broadcaster_->sendTransform(imu_tf_);
}
}
}
Expand Down
22 changes: 12 additions & 10 deletions flatland_plugins/test/imu_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
* \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/
* @copyright Copyright 2017 Avidbots Corp.
* @name imu_test.cpp
* @brief test diff drive plugin
* @brief imu plugin load test
* @author Chunshang Li
*
* Software License Agreement (BSD License)
Expand Down Expand Up @@ -44,21 +44,23 @@
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <flatland_plugins/imu.h>
#include <flatland_server/model_plugin.h>
#include <gtest/gtest.h>
#include <pluginlib/class_loader.h>
#include <ros/ros.h>

TEST(ImuPluginTest, load_test) {
#include <pluginlib/class_loader.hpp>
#include <rclcpp/rclcpp.hpp>

TEST(ImuPluginTest, load_test)
{
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("test_imu_plugin");
pluginlib::ClassLoader<flatland_server::ModelPlugin> loader(
"flatland_server", "flatland_server::ModelPlugin");
"flatland_server", "flatland_server::ModelPlugin");

try {
boost::shared_ptr<flatland_server::ModelPlugin> plugin =
loader.createInstance("flatland_plugins::Imu");
} catch (pluginlib::PluginlibException& e) {
FAIL() << "Failed to load imu plugin. " << e.what();
std::shared_ptr<flatland_server::ModelPlugin> plugin =
loader.createSharedInstance("flatland_plugins::Imu");
} catch (pluginlib::PluginlibException & e) {
FAIL() << "Failed to load GPS plugin. " << e.what();
}
}

Expand Down
9 changes: 0 additions & 9 deletions flatland_plugins/test/imu_test.test

This file was deleted.

0 comments on commit 28091d8

Please sign in to comment.