Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added Vector3Stamped to ypr and imu_ypr topics in ROS1 #42

Merged
merged 2 commits into from
Dec 6, 2023
Merged

Added Vector3Stamped to ypr and imu_ypr topics in ROS1 #42

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e9d9e70
Added Vector3Stamped to ypr and imu_ypr topics in ROS1
Nov 24, 2023
7c83977
Added ROS2 changes
Nov 24, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#define __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1__
/* ROS */
#include <eigen_conversions/eigen_msg.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
Expand Down
16 changes: 10 additions & 6 deletions fixposition_driver_ros1/src/fixposition_driver_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,8 +46,8 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
poiimu_pub_(nh_.advertise<sensor_msgs::Imu>("/fixposition/poiimu", 100)),
vrtk_pub_(nh_.advertise<fixposition_driver_ros1::VRTK>("/fixposition/vrtk", 100)),
odometry_enu0_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry_enu", 100)),
eul_pub_(nh_.advertise<geometry_msgs::Vector3>("/fixposition/ypr", 100)),
eul_imu_pub_(nh_.advertise<geometry_msgs::Vector3>("/fixposition/imu_ypr", 100)) {
eul_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/ypr", 100)),
eul_imu_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/imu_ypr", 100)) {
ws_sub_ = nh_.subscribe<fixposition_driver_ros1::Speed>(params_.customer_input.speed_topic, 100,
&FixpositionDriverNode::WsCallback, this,
ros::TransportHints().tcpNoDelay());
Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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 {
Expand Down
1 change: 1 addition & 0 deletions fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include <tf2_ros/transform_listener.h>

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/vector3_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
Expand Down
4 changes: 2 additions & 2 deletions fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,8 +76,8 @@ class FixpositionDriverNode : public FixpositionDriver {
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr poiimu_pub_; //!< Bias corrected IMU from ODOMETRY
rclcpp::Publisher<fixposition_driver_ros2::msg::VRTK>::SharedPtr vrtk_pub_; //!< VRTK message
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_enu0_pub_; //!< ENU0 Odometry
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr
rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU
rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr
eul_imu_pub_; //!< Euler angles Pitch-Roll as estimated from the IMU in
// local horizontal

Expand Down
24 changes: 14 additions & 10 deletions fixposition_driver_ros2/src/fixposition_driver_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,8 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr<rclcpp::Node> node,
poiimu_pub_(node_->create_publisher<sensor_msgs::msg::Imu>("/fixposition/poiimu", 100)),
vrtk_pub_(node_->create_publisher<fixposition_driver_ros2::msg::VRTK>("/fixposition/vrtk", 100)),
odometry_enu0_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("/fixposition/odometry_enu", 100)),
eul_pub_(node_->create_publisher<geometry_msgs::msg::Vector3>("/fixposition/ypr", 100)),
eul_imu_pub_(node_->create_publisher<geometry_msgs::msg::Vector3>("/fixposition/imu_ypr", 100)),
eul_pub_(node_->create_publisher<geometry_msgs::msg::Vector3Stamped>("/fixposition/ypr", 100)),
eul_imu_pub_(node_->create_publisher<geometry_msgs::msg::Vector3Stamped>("/fixposition/imu_ypr", 100)),
br_(std::make_shared<tf2_ros::TransformBroadcaster>(node_)),
static_br_(std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_)) {
ws_sub_ = node_->create_subscription<fixposition_driver_ros2::msg::Speed>(
Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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 {
Expand Down
Loading