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

Ros migration naresh #1171

Merged
merged 3 commits into from
Mar 30, 2024
Merged

Ros migration naresh #1171

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
4102536
converted odometry_utils package to ROS2
Nareshp1 Mar 25, 2024
49a0620
needed plugin.xml file for ROS component update from nodelet
Nareshp1 Mar 25, 2024
917a463
refactored .cpp files for clang-format
Nareshp1 Mar 27, 2024
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
104 changes: 68 additions & 36 deletions mil_common/gnc/odometry_utils/CMakeLists.txt
View file
Open in desktop
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()
36 changes: 15 additions & 21 deletions mil_common/gnc/odometry_utils/package.xml
View file
Open in desktop
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>
4 changes: 2 additions & 2 deletions mil_common/gnc/odometry_utils/nodelet.xml → mil_common/gnc/odometry_utils/plugin.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
<library path="lib/libtransform_odometry">
<class name="odometry_utils/transform_odometry" type="odometry_utils::transform_odometry" base_class_type="nodelet::Nodelet">
<class name="odometry_utils/transform_odometry" type="odometry_utils::transform_odometry" base_class_type="rclcpp::Node">
<description>
odometry frame converter
</description>
</class>
</library>
<library path="lib/libodometry_to_tf">
<class name="odometry_utils/odometry_to_tf" type="odometry_utils::odometry_to_tf" base_class_type="nodelet::Nodelet">
<class name="odometry_utils/odometry_to_tf" type="odometry_utils::odometry_to_tf" base_class_type="rclcpp::Node">
<description>
odometry frame converter
</description>
Expand Down
46 changes: 29 additions & 17 deletions mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,33 +1,43 @@
#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/LinearMath/Quaternion.h>
#include <tf2/transform_datatypes.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_broadcaster.h>

#include <map>
#include <pluginlib/class_list_macros.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string>

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 +47,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
Loading