From e9d9e70ca73deb2cbe76fb084ac7980bf5dba543 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 24 Nov 2023 14:54:31 +0100 Subject: [PATCH 1/2] Added Vector3Stamped to ypr and imu_ypr topics in ROS1 --- .../fixposition_driver_ros1/data_to_ros1.hpp | 1 + .../src/fixposition_driver_node.cpp | 16 ++++++++++------ 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index 5ebd542..0bb259f 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -16,6 +16,7 @@ #define __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1__ /* ROS */ #include +#include #include #include #include diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index f834991..49a9225 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -46,8 +46,8 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para poiimu_pub_(nh_.advertise("/fixposition/poiimu", 100)), vrtk_pub_(nh_.advertise("/fixposition/vrtk", 100)), odometry_enu0_pub_(nh_.advertise("/fixposition/odometry_enu", 100)), - eul_pub_(nh_.advertise("/fixposition/ypr", 100)), - eul_imu_pub_(nh_.advertise("/fixposition/imu_ypr", 100)) { + eul_pub_(nh_.advertise("/fixposition/ypr", 100)), + eul_imu_pub_(nh_.advertise("/fixposition/imu_ypr", 100)) { ws_sub_ = nh_.subscribe(params_.customer_input.speed_topic, 100, &FixpositionDriverNode::WsCallback, this, ros::TransportHints().tcpNoDelay()); @@ -85,8 +85,10 @@ void FixpositionDriverNode::RegisterObservers() { vrtk_pub_.publish(vrtk); } if (eul_pub_.getNumSubscribers() > 0) { - geometry_msgs::Vector3 ypr; - tf::vectorEigenToMsg(data.eul, ypr); + geometry_msgs::Vector3Stamped ypr; + ypr.header.stamp = ros::Time::fromBoost(fixposition::times::GpsTimeToPtime(data.odometry.stamp)); + ypr.header.frame_id = "FP_POI"; + tf::vectorEigenToMsg(data.eul, ypr.vector); eul_pub_.publish(ypr); } @@ -142,8 +144,10 @@ void FixpositionDriverNode::RegisterObservers() { // Publish Pitch Roll based on IMU only Eigen::Vector3d imu_ypr_eigen = gnss_tf::QuatToEul(data.rotation); imu_ypr_eigen.x() = 0.0; // the yaw value is not observable using IMU alone - geometry_msgs::Vector3 imu_ypr; - tf::vectorEigenToMsg(imu_ypr_eigen, imu_ypr); + geometry_msgs::Vector3Stamped imu_ypr; + imu_ypr.header.stamp = tf.header.stamp; + imu_ypr.header.frame_id = "FP_POI"; + tf::vectorEigenToMsg(imu_ypr_eigen, imu_ypr.vector); eul_imu_pub_.publish(imu_ypr); } else { From 7c839771ea1cb4c132d4cb96688a527c09acbf3d Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 24 Nov 2023 16:07:03 +0100 Subject: [PATCH 2/2] Added ROS2 changes --- .../fixposition_driver_ros2/data_to_ros2.hpp | 1 + .../fixposition_driver_node.hpp | 4 ++-- .../src/fixposition_driver_node.cpp | 24 +++++++++++-------- 3 files changed, 17 insertions(+), 12 deletions(-) diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index e429924..0bcdc16 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 20a3875..609ca39 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -76,8 +76,8 @@ class FixpositionDriverNode : public FixpositionDriver { rclcpp::Publisher::SharedPtr poiimu_pub_; //!< Bias corrected IMU from ODOMETRY rclcpp::Publisher::SharedPtr vrtk_pub_; //!< VRTK message rclcpp::Publisher::SharedPtr odometry_enu0_pub_; //!< ENU0 Odometry - rclcpp::Publisher::SharedPtr eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU + rclcpp::Publisher::SharedPtr eul_imu_pub_; //!< Euler angles Pitch-Roll as estimated from the IMU in // local horizontal diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index edf25bd..0046534 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -47,8 +47,8 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr node, poiimu_pub_(node_->create_publisher("/fixposition/poiimu", 100)), vrtk_pub_(node_->create_publisher("/fixposition/vrtk", 100)), odometry_enu0_pub_(node_->create_publisher("/fixposition/odometry_enu", 100)), - eul_pub_(node_->create_publisher("/fixposition/ypr", 100)), - eul_imu_pub_(node_->create_publisher("/fixposition/imu_ypr", 100)), + eul_pub_(node_->create_publisher("/fixposition/ypr", 100)), + eul_imu_pub_(node_->create_publisher("/fixposition/imu_ypr", 100)), br_(std::make_shared(node_)), static_br_(std::make_shared(node_)) { ws_sub_ = node_->create_subscription( @@ -110,10 +110,12 @@ void FixpositionDriverNode::RegisterObservers() { vrtk_pub_->publish(vrtk); } if (eul_pub_->get_subscription_count() > 0) { - geometry_msgs::msg::Vector3 ypr; - ypr.set__x(data.eul.x()); - ypr.set__y(data.eul.y()); - ypr.set__z(data.eul.z()); + geometry_msgs::msg::Vector3Stamped ypr; + ypr.header.stamp = GpsTimeToMsgTime(data.odometry.stamp); + ypr.header.frame_id = "FP_POI"; + ypr.vector.set__x(data.eul.x()); + ypr.vector.set__y(data.eul.y()); + ypr.vector.set__z(data.eul.z()); eul_pub_->publish(ypr); } @@ -169,10 +171,12 @@ void FixpositionDriverNode::RegisterObservers() { // Publish Pitch Roll based on IMU only Eigen::Vector3d imu_ypr_eigen = gnss_tf::QuatToEul(data.rotation); imu_ypr_eigen.x() = 0.0; // the yaw value is not observable using IMU alone - geometry_msgs::msg::Vector3 imu_ypr; - imu_ypr.set__x(imu_ypr_eigen.x()); - imu_ypr.set__y(imu_ypr_eigen.y()); - imu_ypr.set__z(imu_ypr_eigen.z()); + geometry_msgs::msg::Vector3Stamped imu_ypr; + imu_ypr.header.stamp = tf.header.stamp; + imu_ypr.header.frame_id = "FP_POI"; + imu_ypr.vector.set__x(imu_ypr_eigen.x()); + imu_ypr.vector.set__y(imu_ypr_eigen.y()); + imu_ypr.vector.set__z(imu_ypr_eigen.z()); eul_imu_pub_->publish(imu_ypr); } else {