Skip to content

Commit

Permalink
converted odometry_utils package to ROS2
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nareshp1
Nareshp1 committed Mar 25, 2024
1 parent 9b7e703 commit 4102536
Show file tree
Hide file tree
Showing 5 changed files with 171 additions and 139 deletions.
104 changes: 68 additions & 36 deletions mil_common/gnc/odometry_utils/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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()
14 changes: 0 additions & 14 deletions mil_common/gnc/odometry_utils/nodelet.xml
View file

This file was deleted.

36 changes: 15 additions & 21 deletions mil_common/gnc/odometry_utils/package.xml
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<?xml version="1.0"?>
<package>
<package format="2">
<name>odometry_utils</name>
<version>1.0.0</version>
<description>odometry_utils</description>
Expand All @@ -9,25 +9,19 @@
<!-- <url type="bugtracker"></url> -->
<author>Forrest Voight</author>
<!-- Dependencies which this package needs to build itself. -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Dependencies needed to compile this package. -->
<build_depend>eigen_conversions</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>odom_estimator</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>tf</build_depend>
<build_depend>tf_conversions</build_depend>
<!-- Dependencies needed after this package is compiled. -->
<run_depend>nav_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>eigen_conversions</run_depend>
<run_depend>tf</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>odom_estimator</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>tf_conversions</run_depend>
<buildtool_depend>ament_cmake</buildtool_depend>
<!-- Dependencies needed to compile this package and use at runtime. -->
<depend>eigen_conversions</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>nodelet</depend>
<depend>odom_estimator</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_eigen</depend>
<depend>tf2_geometry_msgs</depend>
<!-- Dependencies needed only for running tests. -->
<!-- <test_depend>nav_msgs</test_depend> -->
<!-- <test_depend>roscpp</test_depend> -->
Expand All @@ -38,6 +32,6 @@
<!-- <test_depend>nodelet</test_depend> -->
<!-- <test_depend>tf_conversions</test_depend> -->
<export>
<nodelet plugin="${prefix}/nodelet.xml"/>
<build_type>ament_cmake</build_type>
</export>
</package>
49 changes: 31 additions & 18 deletions mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,33 +1,44 @@
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf2/transform_datatypes.h>

#include <map>
#include <pluginlib/class_list_macros.hpp>
#include <string>

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

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<std::string, ros::Time> _last_tf_stamps;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_br_;
std::map<std::string, rclcpp::Time> _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:
Expand All @@ -37,10 +48,12 @@ class odometry_to_tf : public nodelet::Nodelet

virtual void onInit()
{
odom_sub =
getNodeHandle().subscribe<nav_msgs::Odometry>("odom", 10, boost::bind(&odometry_to_tf::handle_odom, this, _1));
tf_br_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
odom_sub = this->create_subscription<nav_msgs::msg::Odometry>("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 <rclcpp_components/register_node_macro.hpp> // Include macro to register the component
RCLCPP_COMPONENTS_REGISTER_NODE(odometry_utils::OdometryToTf) // Register the node as a component
Loading

0 comments on commit 4102536

Please sign in to comment.