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

Jump to bottom

Porting tf2_sensor_msgs in C++ (#2) #75

Merged
merged 2 commits into from
Oct 26, 2018
Merged

Porting tf2_sensor_msgs in C++ (#2) #75

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
08cfff5
cherry pick over to ros2
SteveMacenski Oct 25, 2018
a1faa1c
adding new clock logic
SteveMacenski Oct 25, 2018
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
Empty file removed tf2_sensor_msgs/AMENT_IGNORE
View file
Open in desktop
Empty file.
71 changes: 39 additions & 32 deletions tf2_sensor_msgs/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,42 +1,49 @@
cmake_minimum_required(VERSION 3.5)
project(tf2_sensor_msgs)

find_package(catkin REQUIRED COMPONENTS cmake_modules sensor_msgs tf2_ros tf2)
find_package(Boost COMPONENTS thread REQUIRED)
find_package(Eigen)
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra)
endif()

catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS sensor_msgs tf2_ros tf2
DEPENDS Eigen
find_package(ament_cmake_auto REQUIRED)
set(required_dependencies
"sensor_msgs"
#"python_orocos_kdl"
"tf2"
"tf2_ros"
"Eigen3"
)

ament_auto_find_build_dependencies(REQUIRED ${required_dependencies})

include_directories(include
${catkin_INCLUDE_DIRS}
)

install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
${sensor_msgs_INCLUDE_DIRS}
${tf2_INCLUDE_DIRS}
${tf2_ros_INCLUDE_DIRS}
${Eigen3_INCLUDE_DIRS}
)

catkin_python_setup()

if(CATKIN_ENABLE_TESTING)
catkin_add_nosetests(test/test_tf2_sensor_msgs.py)

find_package(catkin REQUIRED COMPONENTS sensor_msgs rostest tf2_ros tf2)

include_directories(${EIGEN_INCLUDE_DIRS})

add_executable(test_tf2_sensor_msgs_cpp EXCLUDE_FROM_ALL test/test_tf2_sensor_msgs.cpp)
target_link_libraries(test_tf2_sensor_msgs_cpp ${catkin_LIBRARIES} ${GTEST_LIBRARIES})


if(TARGET tests)
add_dependencies(tests test_tf2_sensor_msgs_cpp)
endif()
add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch)


endif()
# TODO enable tests
#if(BUILD_TESTING)
# catkin_add_nosetests(test/test_tf2_sensor_msgs.py)

#find_package(catkin REQUIRED COMPONENTS
# sensor_msgs
# rostest
# tf2_ros
# tf2
#)
#include_directories(${EIGEN_INCLUDE_DIRS})
#add_executable(test_tf2_sensor_msgs_cpp EXCLUDE_FROM_ALL test/test_tf2_sensor_msgs.cpp)
#target_link_libraries(test_tf2_sensor_msgs_cpp ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
#if(TARGET tests)
# add_dependencies(tests test_tf2_sensor_msgs_cpp)
#endif()
#add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch)
#endif()

ament_auto_package()
27 changes: 14 additions & 13 deletions tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,9 @@
#define TF2_SENSOR_MSGS_H

#include <tf2/convert.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <tf2/time.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <Eigen/Eigen>
#include <Eigen/Geometry>

Expand All @@ -46,17 +47,17 @@ namespace tf2
// method to extract timestamp from object
template <>
inline
tf2::TimePoint getTimestamp(const sensor_msgs::PointCloud2& p) {return p.header.stamp;}
tf2::TimePoint getTimestamp(const sensor_msgs::msg::PointCloud2& p) {return tf2_ros::fromMsg(p.header.stamp);}

// method to extract frame id from object
template <>
inline
std::string getFrameId(const sensor_msgs::PointCloud2 &p) {return p.header.frame_id;}
std::string getFrameId(const sensor_msgs::msg::PointCloud2 &p) {return p.header.frame_id;}

// this method needs to be implemented by client library developers
template <>
inline
void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in)
void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::PointCloud2 &p_out, const geometry_msgs::msg::TransformStamped& t_in)
{
p_out = p_in;
p_out.header = t_in.header;
Expand All @@ -65,13 +66,13 @@ void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2
t_in.transform.rotation.w, t_in.transform.rotation.x,
t_in.transform.rotation.y, t_in.transform.rotation.z);

sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, "x");
sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, "y");
sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, "z");
sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, std::string("x"));
sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, std::string("y"));
sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, std::string("z"));

sensor_msgs::PointCloud2Iterator<float> x_out(p_out, "x");
sensor_msgs::PointCloud2Iterator<float> y_out(p_out, "y");
sensor_msgs::PointCloud2Iterator<float> z_out(p_out, "z");
sensor_msgs::PointCloud2Iterator<float> x_out(p_out, std::string("x"));
sensor_msgs::PointCloud2Iterator<float> y_out(p_out, std::string("y"));
sensor_msgs::PointCloud2Iterator<float> z_out(p_out, std::string("z"));

Eigen::Vector3f point;
for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
Expand All @@ -82,12 +83,12 @@ void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2
}
}
inline
sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in)
sensor_msgs::msg::PointCloud2 toMsg(const sensor_msgs::msg::PointCloud2 &in)
{
return in;
}
inline
void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out)
void fromMsg(const sensor_msgs::msg::PointCloud2 &msg, sensor_msgs::msg::PointCloud2 &out)
{
out = msg;
}
Expand Down
29 changes: 13 additions & 16 deletions tf2_sensor_msgs/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<package>
<package format="2">
<name>tf2_sensor_msgs</name>
<version>0.9.1</version>
<description>
Expand All @@ -9,23 +9,20 @@
<license>BSD</license>

<url type="website">http://www.ros.org/wiki/tf2_ros</url>

<buildtool_depend version_gte="0.5.6">catkin</buildtool_depend>

<build_depend>cmake_modules</build_depend>
<build_depend>eigen</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf2</build_depend>
<build_depend>tf2_ros</build_depend>
<buildtool_depend>ament_cmake_auto</buildtool_depend>

<run_depend>cmake_modules</run_depend>
<run_depend>eigen</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>tf2_ros</run_depend>
<run_depend>tf2</run_depend>
<run_depend>python_orocos_kdl</run_depend>
<depend>cmake_modules</depend>
<depend>Eigen3</depend>
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure capital Eigen3 is defined in rosdistro.

ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
tf2_sensor_msgs: Cannot locate rosdep definition for [Eigen3]

@clalancette , was this migration intentional?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ruffsl I think you're right, I looked at rosdistro and it looks like eigen leads to eigen3 debians

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

See #76

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I actually don't remember at all, sorry. However, this package is released into Bouncy, so I'm pretty sure what is currently in there works one way or another.

<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<!-- <depend>python_orocos_kdl</depend> -->
<!-- <test_depend>rostest</test_depend> -->

<test_depend>rostest</test_depend>
<test_depend>ament_cmake_gtest</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>

31 changes: 16 additions & 15 deletions tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,14 +41,14 @@ static const double EPS = 1e-3;

TEST(Tf2Sensor, PointCloud2)
{
sensor_msgs::PointCloud2 cloud;
sensor_msgs::PointCloud2Modifier modifier(cloud);
sensor_msgs::msg::PointCloud2 cloud;
sensor_msgs::msg::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
modifier.resize(1);

sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
sensor_msgs::msg::PointCloud2Iterator<float> iter_x(cloud, "x");
sensor_msgs::msg::PointCloud2Iterator<float> iter_y(cloud, "y");
sensor_msgs::msg::PointCloud2Iterator<float> iter_z(cloud, "z");

*iter_x = 1;
*iter_y = 2;
Expand All @@ -58,20 +58,20 @@ TEST(Tf2Sensor, PointCloud2)
cloud.header.frame_id = "A";

// simple api
sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", tf2::durationFromSec(2.0));
sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
sensor_msgs::msg::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", tf2::durationFromSec(2.0));
sensor_msgs::msg::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
sensor_msgs::msg::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
sensor_msgs::msg::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
EXPECT_NEAR(*iter_x_after, -9, EPS);
EXPECT_NEAR(*iter_y_after, 18, EPS);
EXPECT_NEAR(*iter_z_after, 27, EPS);

// advanced api
sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", builtin_interfaces::msg::Time(2.0),
sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", builtin_interfaces::msg::Time(2.0),
"A", tf2::durationFromSec(3.0));
sensor_msgs::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z");
sensor_msgs::msg::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
sensor_msgs::msg::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
sensor_msgs::msg::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z");
EXPECT_NEAR(*iter_x_advanced, -9, EPS);
EXPECT_NEAR(*iter_y_advanced, 18, EPS);
EXPECT_NEAR(*iter_z_advanced, 27, EPS);
Expand All @@ -82,10 +82,11 @@ int main(int argc, char **argv){
ros::init(argc, argv, "test");
ros::NodeHandle n;

tf_buffer = new tf2_ros::Buffer();
rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf_buffer = new tf2_ros::Buffer(clock);

// populate buffer
geometry_msgs::TransformStamped t;
geometry_msgs::msg::TransformStamped t;
t.transform.translation.x = 10;
t.transform.translation.y = 20;
t.transform.translation.z = 30;
Expand Down