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

Reenable sensor_msgs test #422

Merged
merged 10 commits into from
May 27, 2021
Merged

Reenable sensor_msgs test #422

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
33c0a39
Reenable sensor_msgs cpp test
gleichdick Jan 25, 2021
7eb81ba
Small cleanups.
clalancette May 26, 2021
283cc4b
Add static_casts for Windows compile warning.
clalancette May 26, 2021
86c30b5
Nitpicks (including cpplint)
gleichdick May 26, 2021
1d9c751
Enable linting
gleichdick May 26, 2021
b392269
Fix copyright
gleichdick May 26, 2021
9b9c170
Fix flake8
gleichdick May 26, 2021
778574f
fix cpp linting
gleichdick May 26, 2021
1f8065a
re-add tf2_ros_py dependency
gleichdick May 26, 2021
192d585
fix further flake8 complains
gleichdick May 27, 2021
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
41 changes: 22 additions & 19 deletions tf2_sensor_msgs/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,24 +24,27 @@ find_package(Eigen3 REQUIRED)

ament_export_dependencies(eigen3_cmake_module)
ament_export_dependencies(Eigen3)

# 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_export_dependencies(tf2)
ament_export_dependencies(tf2_ros)

if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)

# TODO(ros2/orocos_kinematics_dynamics): reenable when PyKDL is ready to use
#find_package(ament_cmake_pytest REQUIRED)
#ament_add_pytest_test(test_tf2_sensor_msgs_py test/test_tf2_sensor_msgs.py)

ament_add_gtest(test_tf2_sensor_msgs_cpp test/test_tf2_sensor_msgs.cpp)
if(TARGET test_tf2_sensor_msgs_cpp)
target_include_directories(test_tf2_sensor_msgs_cpp PUBLIC include)
ament_target_dependencies(test_tf2_sensor_msgs_cpp
"Eigen3"
"rclcpp"
"sensor_msgs"
"tf2"
"tf2_ros"
)
endif()
endif()

ament_auto_package()
19 changes: 15 additions & 4 deletions tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,10 +62,21 @@ void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::Po
{
p_out = p_in;
p_out.header = t_in.header;
Eigen::Transform<float,3,Eigen::Affine> t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y,
t_in.transform.translation.z) * Eigen::Quaternion<float>(
t_in.transform.rotation.w, t_in.transform.rotation.x,
t_in.transform.rotation.y, t_in.transform.rotation.z);
// FIXME(clalancette): The static casts to float aren't ideal; the incoming
// transform uses double, and hence may have values that are too large to represent
// in a float. But since this method implicitly returns a float PointCloud2, we don't
// have much choice here without subtly changing the API.
auto translation = Eigen::Translation3f(
static_cast<float>(t_in.transform.translation.x),
static_cast<float>(t_in.transform.translation.y),
static_cast<float>(t_in.transform.translation.z));
auto quaternion = Eigen::Quaternion<float>(
static_cast<float>(t_in.transform.rotation.w),
static_cast<float>(t_in.transform.rotation.x),
static_cast<float>(t_in.transform.rotation.y),
static_cast<float>(t_in.transform.rotation.z));

Eigen::Transform<float,3,Eigen::Affine> t = translation * quaternion;

sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, std::string("x"));
sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, std::string("y"));
Expand Down
4 changes: 2 additions & 2 deletions tf2_sensor_msgs/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,10 +25,10 @@
<depend>tf2_ros</depend>
<!-- <depend>python_orocos_kdl</depend> -->
<!-- <test_depend>rostest</test_depend> -->
Copy link
Contributor

Choose a reason for hiding this comment

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

Remove

Copy link
Contributor

Choose a reason for hiding this comment

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

I still think we should remove this. It's a ros1 dependency

Copy link
Contributor

Choose a reason for hiding this comment

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

@gleichdick I will relaunch CI when this line will be removed

Copy link
Contributor Author

Choose a reason for hiding this comment

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

I removed them, furhtermore some more flake8 issues are resolved which the CI found. Maybe we should add the flake8 modules mentioned here to the CI of this project (not the big one)?

Copy link
Contributor

Choose a reason for hiding this comment

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

While I agree we should eventually do that, I don't think we should do that here. The next step for this package in my opinion is to split it up into separate tf2_sensor_msgs and tf2_sensor_msgs_py packages, and actually get the Python stuff working (which it currently isn't). At that point, it would make sense to enable the automated flake8 tests.


<exec_depend>tf2_ros_py</exec_depend>
<!-- <exec_depend>tf2_ros_py</exec_depend> -->
Copy link
Contributor

Choose a reason for hiding this comment

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

remove


<test_depend>ament_cmake_gtest</test_depend>
<test_depend>rclcpp</test_depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
51 changes: 29 additions & 22 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 @@ -27,13 +27,16 @@
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <memory>

#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/transform_listener.h>
#include <ros/ros.h>
#include <builtin_interfaces/msg/time.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <gtest/gtest.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>

std::unique_ptr<tf2_ros::Buffer> tf_buffer = nullptr;
static const double EPS = 1e-3;
Expand All @@ -42,45 +45,48 @@ static const double EPS = 1e-3;
TEST(Tf2Sensor, PointCloud2)
{
sensor_msgs::msg::PointCloud2 cloud;
sensor_msgs::msg::PointCloud2Modifier modifier(cloud);
sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
modifier.resize(1);

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");
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");

*iter_x = 1;
*iter_y = 2;
*iter_z = 3;

cloud.header.stamp = builtin_interfaces::msg::Time(2);
cloud.header.stamp.sec = 2;
cloud.header.stamp.nanosec = 0;
Copy link
Contributor

Choose a reason for hiding this comment

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

same as below

cloud.header.frame_id = "A";

// simple api
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");
sensor_msgs::msg::PointCloud2 cloud_simple = tf_buffer->transform(
cloud, "B", tf2::durationFromSec(
2.0));
Copy link
Contributor

Choose a reason for hiding this comment

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

this looks wierd.

Suggested change
cloud, "B", tf2::durationFromSec(
2.0));
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");
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::msg::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", builtin_interfaces::msg::Time(2.0),
"A", tf2::durationFromSec(3.0));
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");
sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform(
cloud, "B", tf2::timeFromSec(2.0),
"A", tf2::durationFromSec(3.0));
Copy link
Contributor

Choose a reason for hiding this comment

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

Suggested change
sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform(
cloud, "B", tf2::timeFromSec(2.0),
"A", tf2::durationFromSec(3.0));
sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform(
cloud, "B", tf2::timeFromSec(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");
EXPECT_NEAR(*iter_x_advanced, -9, EPS);
EXPECT_NEAR(*iter_y_advanced, 18, EPS);
EXPECT_NEAR(*iter_z_advanced, 27, EPS);
}

int main(int argc, char **argv){
int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test");
ros::NodeHandle n;

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf_buffer = std::make_unique<tf2_ros::Buffer>(clock);
Expand All @@ -94,7 +100,8 @@ int main(int argc, char **argv){
t.transform.rotation.y = 0;
t.transform.rotation.z = 0;
t.transform.rotation.w = 0;
t.header.stamp = builtin_interfaces::msg::Time(2.0);
t.header.stamp.sec = 2;
t.header.stamp.nanosec = 0;
Copy link
Contributor

Choose a reason for hiding this comment

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

Did you try this?

Suggested change
t.header.stamp.sec = 2;
t.header.stamp.nanosec = 0;
t.header.stamp.sec = rclcpp::Time(2, 0);

t.header.frame_id = "A";
t.child_frame_id = "B";
tf_buffer->setTransform(t, "test");
Expand Down