From 4102536e0b45cb936fc61e908e8fa1e3f2c911cc Mon Sep 17 00:00:00 2001 From: Naresh Panchal Date: Mon, 25 Mar 2024 11:11:48 -0400 Subject: [PATCH 1/3] converted odometry_utils package to ROS2 --- mil_common/gnc/odometry_utils/CMakeLists.txt | 104 +++++++++++------ mil_common/gnc/odometry_utils/nodelet.xml | 14 --- mil_common/gnc/odometry_utils/package.xml | 36 +++--- .../gnc/odometry_utils/src/odometry_to_tf.cpp | 49 +++++--- .../odometry_utils/src/transform_odometry.cpp | 107 ++++++++++-------- 5 files changed, 171 insertions(+), 139 deletions(-) delete mode 100644 mil_common/gnc/odometry_utils/nodelet.xml diff --git a/mil_common/gnc/odometry_utils/CMakeLists.txt b/mil_common/gnc/odometry_utils/CMakeLists.txt index 8b56afe02..49ff1f4bd 100644 --- a/mil_common/gnc/odometry_utils/CMakeLists.txt +++ b/mil_common/gnc/odometry_utils/CMakeLists.txt @@ -1,44 +1,76 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(odometry_utils) -find_package(catkin - REQUIRED COMPONENTS - nav_msgs - roscpp - eigen_conversions - odom_estimator - geometry_msgs - tf - nodelet - tf_conversions -) +# Use C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -catkin_package( - DEPENDS - CATKIN_DEPENDS - nav_msgs - roscpp - eigen_conversions - odom_estimator - geometry_msgs - tf - nodelet - tf_conversions - INCLUDE_DIRS - LIBRARIES -) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(eigen_conversions REQUIRED) +find_package(odom_estimator REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package() +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) -include_directories( - ${Boost_INCLUDE_DIR} - ${catkin_INCLUDE_DIRS} -) +include_directories(include ${Boost_INCLUDE_DIRS}) add_library(transform_odometry src/transform_odometry.cpp) -target_link_libraries(transform_odometry ${catkin_LIBRARIES}) -add_dependencies(transform_odometry ${catkin_EXPORTED_TARGETS}) -set_target_properties(transform_odometry PROPERTIES COMPILE_FLAGS "-O3 -std=c++11") +ament_target_dependencies(transform_odometry + rclcpp + geometry_msgs + odom_estimator + eigen_conversions + nav_msgs + tf2 + tf2_ros + tf2_eigen + tf2_geometry_msgs) +rclcpp_components_register_nodes(transform_odometry "odometry_utils::TransformOdometry") +set_target_properties(transform_odometry PROPERTIES COMPILE_FLAGS "-O3 -std=c++17") add_library(odometry_to_tf src/odometry_to_tf.cpp) -target_link_libraries(odometry_to_tf ${catkin_LIBRARIES}) -add_dependencies(odometry_to_tf ${catkin_EXPORTED_TARGETS}) -set_target_properties(odometry_to_tf PROPERTIES COMPILE_FLAGS "-O3 -std=c++11") +ament_target_dependencies(odometry_to_tf + rclcpp + geometry_msgs + odom_estimator + eigen_conversions + nav_msgs + tf2 + tf2_ros + tf2_eigen + tf2_geometry_msgs) +rclcpp_components_register_nodes(odometry_to_tf "odometry_utils::OdometryToTf") +set_target_properties(odometry_to_tf PROPERTIES COMPILE_FLAGS "-O3 -std=c++17") + +# Install libraries +install(TARGETS transform_odometry odometry_to_tf + DESTINATION lib/${PROJECT_NAME} +) + +# Install launch files, config files, etc. +# install(DIRECTORY +# launch +# config +# DESTINATION share/${PROJECT_NAME} +# ) + +# Export dependencies +ament_export_dependencies(rclcpp geometry_msgs nav_msgs odom_estimator eigen_conversions tf2 tf2_ros tf2_eigen tf2_geometry_msgs) +ament_export_include_directories(include) +ament_export_libraries(transform_odometry odometry_to_tf) + +# Add linters if desired +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() + +ament_package() diff --git a/mil_common/gnc/odometry_utils/nodelet.xml b/mil_common/gnc/odometry_utils/nodelet.xml deleted file mode 100644 index 355afcd02..000000000 --- a/mil_common/gnc/odometry_utils/nodelet.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - odometry frame converter - - - - - - - odometry frame converter - - - diff --git a/mil_common/gnc/odometry_utils/package.xml b/mil_common/gnc/odometry_utils/package.xml index 6b160fc8c..d012766d6 100644 --- a/mil_common/gnc/odometry_utils/package.xml +++ b/mil_common/gnc/odometry_utils/package.xml @@ -1,5 +1,5 @@ - + odometry_utils 1.0.0 odometry_utils @@ -9,25 +9,19 @@ Forrest Voight - catkin - - eigen_conversions - geometry_msgs - nav_msgs - nodelet - odom_estimator - roscpp - tf - tf_conversions - - nav_msgs - roscpp - eigen_conversions - tf - geometry_msgs - odom_estimator - nodelet - tf_conversions + ament_cmake + + eigen_conversions + geometry_msgs + nav_msgs + nodelet + odom_estimator + rclcpp + rclcpp_components + tf2 + tf2_ros + tf2_eigen + tf2_geometry_msgs @@ -38,6 +32,6 @@ - + ament_cmake diff --git a/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp b/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp index 6c2794453..c91a5e82b 100644 --- a/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp +++ b/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp @@ -1,33 +1,44 @@ #include -#include -#include -#include -#include +#include #include -#include #include +#include +#include +#include +#include +#include + namespace odometry_utils { -class odometry_to_tf : public nodelet::Nodelet +class odometry_to_tf : public rclcpp::Node { private: - ros::Subscriber odom_sub; - tf::TransformBroadcaster tf_br; - std::map _last_tf_stamps; + rclcpp::Subscription::SharedPtr odom_sub; + std::unique_ptr tf_br_; + std::map _last_tf_stamps; - void handle_odom(const nav_msgs::Odometry::ConstPtr& msg) + void handle_odom(const nav_msgs::msg::Odometry::ConstPtr& msg) { - tf::Transform transform; - poseMsgToTF(msg->pose.pose, transform); + geometry_msgs::msg::TransformStamped stamped_transform; + + stamped_transform.header.stamp = msg->header.stamp; + stamped_transform.header.frame_id = msg->header.frame_id; + stamped_transform.child_frame_id = msg->child_frame_id; + + stamped_transform.transform.translation.x = msg->pose.pose.position.x; + stamped_transform.transform.translation.y = msg->pose.pose.position.y; + stamped_transform.transform.translation.z = msg->pose.pose.position.z; + stamped_transform.transform.rotation = msg->pose.pose.orientation; + if (_last_tf_stamps.count(msg->header.frame_id) && _last_tf_stamps[msg->header.frame_id] == msg->header.stamp) { return; } _last_tf_stamps[msg->header.frame_id] = msg->header.stamp; - tf::StampedTransform stamped_transform(transform, msg->header.stamp, msg->header.frame_id, msg->child_frame_id); - tf_br.sendTransform(stamped_transform); + + tf_br_->sendTransform(stamped_transform); } public: @@ -37,10 +48,12 @@ class odometry_to_tf : public nodelet::Nodelet virtual void onInit() { - odom_sub = - getNodeHandle().subscribe("odom", 10, boost::bind(&odometry_to_tf::handle_odom, this, _1)); + tf_br_ = std::make_unique(this); + odom_sub = this->create_subscription("odom", 10, std::bind(&OdometryToTf::handle_odom, this, std::placeholders::_1)); + } }; -PLUGINLIB_EXPORT_CLASS(odometry_utils::odometry_to_tf, nodelet::Nodelet); -} // namespace odometry_utils +} +#include // Include macro to register the component +RCLCPP_COMPONENTS_REGISTER_NODE(odometry_utils::OdometryToTf) // Register the node as a component diff --git a/mil_common/gnc/odometry_utils/src/transform_odometry.cpp b/mil_common/gnc/odometry_utils/src/transform_odometry.cpp index 568f8cd95..a2d932973 100644 --- a/mil_common/gnc/odometry_utils/src/transform_odometry.cpp +++ b/mil_common/gnc/odometry_utils/src/transform_odometry.cpp @@ -1,13 +1,16 @@ #include -#include +#include #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include @@ -17,84 +20,88 @@ using namespace odom_estimator; using namespace Eigen; -class transform_odometry : public nodelet::Nodelet +class transform_odometry : public rclcpp::Node { private: std::string frame_id; std::string child_frame_id; - tf::TransformListener listener; + tf2_ros::Buffer tf_buffer_; + std::shared_ptr tf_listener_; - ros::Subscriber sub; - ros::Publisher pub; + rclcpp::Subscription::SharedPtr sub; + rclcpp::Publisher::SharedPtr pub; - void handle(const nav_msgs::Odometry::ConstPtr &msgp) + void handle(const nav_msgs::msg::Odometry::SharedPtr msgp) { - tf::StampedTransform lefttransform; + geometry_msgs::msg::TransformStamped lefttransform; try { - listener.lookupTransform(frame_id, msgp->header.frame_id, ros::Time(0), lefttransform); + lefttransform = tf_buffer_.lookupTransform(frame_id, msgp->header.frame_id, tf2::TimePointZero); } - catch (tf::TransformException ex) + catch (tf2::TransformException &ex) { - ROS_ERROR_THROTTLE(10.0, "%s", ex.what()); + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 10.0, "%s", ex.what()); return; } - tf::StampedTransform righttransform; + geometry_msgs::msg::TransformStamped righttransform; try { - listener.lookupTransform(msgp->child_frame_id, child_frame_id, ros::Time(0), righttransform); + righttransform = tf_buffer_.lookupTransform(msgp->child_frame_id, child_frame_id, tf2::TimePointZero); } - catch (tf::TransformException ex) + catch (tf2::TransformException &ex) { - ROS_ERROR_THROTTLE(10.0, "%s", ex.what()); + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 10.0, "%s", ex.what()); return; } - Vector3d left_p; - tf::vectorTFToEigen(lefttransform.getOrigin(), left_p); - Quaterniond left_q; - tf::quaternionTFToEigen(lefttransform.getRotation(), left_q); - Vector3d right_p; - tf::vectorTFToEigen(righttransform.getOrigin(), right_p); - Quaterniond right_q; - tf::quaternionTFToEigen(righttransform.getRotation(), right_q); + Eigen::Vector3d left_p; + tf2::fromMsg(transformStamped.transform.translation, left_p); + Eigen::Quaterniond left_q; + tf2::fromMsg(transformStamped.transform.rotation, left_q); + + Eigen::Vector3d right_p; + tf2::fromMsg(righttransform.transform.translation, right_p); + Eigen::Quaterniond right_q; + tf2::fromMsg(righttransform.transform.rotation, right_q); EasyDistributionFunction > transformer( - [this, &left_p, &left_q, &right_p, &right_q](Odom const &odom, Vec<0> const &extra) - { - return Odom(odom.stamp, frame_id, child_frame_id, - left_p + left_q._transformVector(odom.pos + odom.orient._transformVector(right_p)), - left_q * odom.orient * right_q, - right_q.inverse()._transformVector(odom.vel - right_p.cross(odom.ang_vel)), - right_q.inverse()._transformVector(odom.ang_vel)); - }, - GaussianDistribution >(Vec<0>(), SqMat<0>())); + [this, &left_p, &left_q, &right_p, &right_q](Odom const &odom, Vec<0> const &extra) + { + return Odom(odom.stamp, frame_id, child_frame_id, + left_p + left_q * (odom.pos + odom.orient * right_p), + left_q * odom.orient * right_q, + right_q.inverse() * (odom.vel - right_p.cross(odom.ang_vel)), + right_q.inverse() * odom.ang_vel); + }, + GaussianDistribution >(Vec<0>(), SqMat<0>())); pub.publish(msg_from_odom(transformer(odom_from_msg(*msgp)))); } public: - transform_odometry() + transform_odometry() : Node("transform_odometry"), tf_buffer_(this->get_clock()), tf_listener_(std::make_shared(tf_buffer_)) { - } + this->declare_parameter("frame_id", "default_frame_id"); // Default value as example + this->declare_parameter("child_frame_id", "default_child_frame_id"); // Default value as example - virtual void onInit() - { - if (!getPrivateNodeHandle().getParam("frame_id", frame_id)) + if (!this->get_parameter("frame_id", frame_id)) { - throw std::runtime_error("param frame_id required"); + throw std::runtime_error("param 'frame_id' required"); } - if (!getPrivateNodeHandle().getParam("child_frame_id", child_frame_id)) + if (!this->get_parameter("child_frame_id", child_frame_id)) { - throw std::runtime_error("param child_frame_id required"); + throw std::runtime_error("param 'child_frame_id' required"); } - sub = getNodeHandle().subscribe("orig_odom", 10, - boost::bind(&transform_odometry::handle, this, _1)); - pub = getNodeHandle().advertise("odom", 10); - } + sub = this->create_subscription( + "orig_odom", 10, + std::bind(&TransformOdometry::handle, this, std::placeholders::_1)); + + pub = this->create_publisher("odom", 10); }; -PLUGINLIB_EXPORT_CLASS(odometry_utils::transform_odometry, nodelet::Nodelet); -} // namespace odometry_utils + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(odometry_utils::TransformOdometry) +} From 49a0620c178a86a702c4062c4b569fddf2f97655 Mon Sep 17 00:00:00 2001 From: Naresh Panchal Date: Mon, 25 Mar 2024 11:26:31 -0400 Subject: [PATCH 2/3] needed plugin.xml file for ROS component update from nodelet --- mil_common/gnc/odometry_utils/plugin.xml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 mil_common/gnc/odometry_utils/plugin.xml diff --git a/mil_common/gnc/odometry_utils/plugin.xml b/mil_common/gnc/odometry_utils/plugin.xml new file mode 100644 index 000000000..91051b8ad --- /dev/null +++ b/mil_common/gnc/odometry_utils/plugin.xml @@ -0,0 +1,14 @@ + + + + odometry frame converter + + + + + + + odometry frame converter + + + From 917a46332f8dfd8badaf84de26face6b7ff87339 Mon Sep 17 00:00:00 2001 From: Naresh Panchal Date: Wed, 27 Mar 2024 11:26:58 -0400 Subject: [PATCH 3/3] refactored .cpp files for clang-format --- .../gnc/odometry_utils/src/odometry_to_tf.cpp | 21 +++++----- .../odometry_utils/src/transform_odometry.cpp | 42 +++++++++---------- 2 files changed, 31 insertions(+), 32 deletions(-) diff --git a/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp b/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp index c91a5e82b..777dc9a0c 100644 --- a/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp +++ b/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp @@ -1,14 +1,13 @@ #include +#include #include +#include +#include #include -#include - -#include #include -#include -#include -#include +#include +#include namespace odometry_utils { @@ -49,11 +48,11 @@ class odometry_to_tf : public rclcpp::Node virtual void onInit() { tf_br_ = std::make_unique(this); - odom_sub = this->create_subscription("odom", 10, std::bind(&OdometryToTf::handle_odom, this, std::placeholders::_1)); - + odom_sub = this->create_subscription( + "odom", 10, std::bind(&OdometryToTf::handle_odom, this, std::placeholders::_1)); } }; -} -#include // Include macro to register the component -RCLCPP_COMPONENTS_REGISTER_NODE(odometry_utils::OdometryToTf) // Register the node as a component +} // namespace odometry_utils +#include // Include macro to register the component +RCLCPP_COMPONENTS_REGISTER_NODE(odometry_utils::OdometryToTf) // Register the node as a component diff --git a/mil_common/gnc/odometry_utils/src/transform_odometry.cpp b/mil_common/gnc/odometry_utils/src/transform_odometry.cpp index a2d932973..43d08736b 100644 --- a/mil_common/gnc/odometry_utils/src/transform_odometry.cpp +++ b/mil_common/gnc/odometry_utils/src/transform_odometry.cpp @@ -1,18 +1,18 @@ #include -#include #include #include #include #include -#include #include -#include -#include #include +#include #include -#include +#include +#include +#include #include +#include namespace odometry_utils { @@ -67,24 +67,25 @@ class transform_odometry : public rclcpp::Node tf2::fromMsg(righttransform.transform.rotation, right_q); EasyDistributionFunction > transformer( - [this, &left_p, &left_q, &right_p, &right_q](Odom const &odom, Vec<0> const &extra) - { - return Odom(odom.stamp, frame_id, child_frame_id, - left_p + left_q * (odom.pos + odom.orient * right_p), - left_q * odom.orient * right_q, - right_q.inverse() * (odom.vel - right_p.cross(odom.ang_vel)), - right_q.inverse() * odom.ang_vel); - }, - GaussianDistribution >(Vec<0>(), SqMat<0>())); + [this, &left_p, &left_q, &right_p, &right_q](Odom const &odom, Vec<0> const &extra) + { + return Odom(odom.stamp, frame_id, child_frame_id, left_p + left_q * (odom.pos + odom.orient * right_p), + left_q * odom.orient * right_q, right_q.inverse() * (odom.vel - right_p.cross(odom.ang_vel)), + right_q.inverse() * odom.ang_vel); + }, + GaussianDistribution >(Vec<0>(), SqMat<0>())); pub.publish(msg_from_odom(transformer(odom_from_msg(*msgp)))); } public: - transform_odometry() : Node("transform_odometry"), tf_buffer_(this->get_clock()), tf_listener_(std::make_shared(tf_buffer_)) + transform_odometry() + : Node("transform_odometry") + , tf_buffer_(this->get_clock()) + , tf_listener_(std::make_shared(tf_buffer_)) { - this->declare_parameter("frame_id", "default_frame_id"); // Default value as example - this->declare_parameter("child_frame_id", "default_child_frame_id"); // Default value as example + this->declare_parameter("frame_id", "default_frame_id"); // Default value as example + this->declare_parameter("child_frame_id", "default_child_frame_id"); // Default value as example if (!this->get_parameter("frame_id", frame_id)) { @@ -96,12 +97,11 @@ class transform_odometry : public rclcpp::Node } sub = this->create_subscription( - "orig_odom", 10, - std::bind(&TransformOdometry::handle, this, std::placeholders::_1)); + "orig_odom", 10, std::bind(&TransformOdometry::handle, this, std::placeholders::_1)); pub = this->create_publisher("odom", 10); -}; + }; #include -RCLCPP_COMPONENTS_REGISTER_NODE(odometry_utils::TransformOdometry) + RCLCPP_COMPONENTS_REGISTER_NODE(odometry_utils::TransformOdometry) }