frame_transforms.h/cpp library

[fbenti]

I would like to subscribe to the VehicleLocalPosition topic and automatically convert the coordinate from NED (px4) to ENU(ros) using the frame_transforms.

I created a new file within the listeners folder, named transform.cpp, where I have added #include <px4_ros_com/frame_transforms.h>

Then in the CMakeLists.txt, I create its respective executable with

add_executable(transform src/examples/listeners/transform.cpp)
ament_target_dependencies(transform rclcpp px4_msgs Eigen3 geometry_msgs sensor_msgs)
install(TARGETS transform DESTINATION lib/${PROJECT_NAME})

However, the colcon build returns an error:

Starting >>> px4_ros_com
--- stderr: px4_ros_com                             
/usr/bin/ld: CMakeFiles/transform.dir/src/examples/listeners/transform.cpp.o: in function `__static_initialization_and_destruction_0(int, int)':
transform.cpp:(.text+0x3ab): undefined reference to `px4_ros_com::frame_transforms::utils::quaternion::quaternion_from_euler(double, double, double)'
/usr/bin/ld: transform.cpp:(.text+0x3ce): undefined reference to `px4_ros_com::frame_transforms::utils::quaternion::quaternion_from_euler(double, double, double)'
collect2: error: ld returned 1 exit status
gmake[2]: *** [CMakeFiles/transform.dir/build.make:183: transform] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:277: CMakeFiles/transform.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< px4_ros_com [1.38s, exited with code 2]

Summary: 0 packages finished [1.59s]
  1 package failed: px4_ros_com
  1 package had stderr output: px4_ros_com

I haven't used cpp in a while, anyone can help?

[fbenti]

Temporary fixed with #169 (comment), is there another solution?

However now I am facing a new issue. I've created a node to subscribe to the /fmu/out/vehicle_local_position: but I cannot correctly pass the newly created Eigen::Vector to the transform_static_frame function.

subscription_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>("/fmu/out/vehicle_local_position", qos,
[this](const px4_msgs::msg::VehicleLocalPosition::UniquePtr msg) {
  std::cout << "x: " << msg->x  << std::endl;
  std::cout << "y: " << msg->y  << std::endl;
  std::cout << "z: " << msg->z  << std::endl;
  
  Eigen::Vector3d in(msg->x,msg->y,msg->z);
  Eigen::Vector3d out = px4_ros_com::frame_transforms::transform_static_frame(in, px4_ros_com::frame_transforms::StaticTF::NED_TO_ENU);
  std::cout << out   << std::endl;
});

Which give error:

Starting >>> px4_ros_com
--- stderr: px4_ros_com                                
/usr/bin/ld: CMakeFiles/transform.dir/src/examples/listeners/transform.cpp.o: in function `LocalPositionListener::LocalPositionListener()::{lambda(std::unique_ptr<px4_msgs::msg::VehicleLocalPosition_<std::allocator<void> >, std::default_delete<px4_msgs::msg::VehicleLocalPosition_<std::allocator<void> > > >)#1}::operator()(std::unique_ptr<px4_msgs::msg::VehicleLocalPosition_<std::allocator<void> >, std::default_delete<px4_msgs::msg::VehicleLocalPosition_<std::allocator<void> > > >) const':
transform.cpp:(.text._ZZN21LocalPositionListenerC4EvENKUlSt10unique_ptrIN8px4_msgs3msg21VehicleLocalPosition_ISaIvEEESt14default_deleteIS5_EEE_clES8_[_ZZN21LocalPositionListenerC4EvENKUlSt10unique_ptrIN8px4_msgs3msg21VehicleLocalPosition_ISaIvEEESt14default_deleteIS5_EEE_clES8_]+0x23b): undefined reference to `px4_ros_com::frame_transforms::transform_static_frame(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, px4_ros_com::frame_transforms::StaticTF)'
collect2: error: ld returned 1 exit status
gmake[2]: *** [CMakeFiles/transform.dir/build.make:183: transform] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:277: CMakeFiles/transform.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< px4_ros_com [15.3s, exited with code 2]

Summary: 0 packages finished [15.9s]
  1 package failed: px4_ros_com
  1 package had stderr output: px4_ros_com

Ideally, I would like to transform the x,y,z from the /fmu/out/vehicle_local_position and the quaternion from /fmu/out/vehicle_attitude from NED coordinate to ENU. Is there a single function that does it?

[beniaminopozzan]

Temporary fixed with #169 (comment), is there another solution?

That solution is for including a library created in package A in an executable (node) created in package B.
You want to do something different: using the library in a node which is itself in A.
You can use this as reference (credits to https://answers.ros.org/question/371562/linking-library-with-node-in-same-package/): https://github.com/ros-planning/moveit2/blob/main/moveit_ros/occupancy_map_monitor/CMakeLists.txt
In that cmake file the library is moveit_ros_occupancy_map_monitor while the executable is moveit_ros_occupancy_map_server

[fbenti]

Thanks that solved it.

What about

Ideally, I would like to transform the x,y,z from the /fmu/out/vehicle_local_position and the quaternion from /fmu/out/vehicle_attitude from NED coordinate to ENU. Is there a single function that does it?

Is

px4_ros_com/include/px4_ros_com/frame_transforms.h

Line 316 in 0bcf68b

template <class T> inline T px4_to_ros_orientation(const T &in)

the function that I need for the orientation and

px4_ros_com/include/px4_ros_com/frame_transforms.h

Line 338 in 0bcf68b

template <class T> inline T ned_to_enu_local_frame(const T &in)

the one for the translation part?

[beniaminopozzan]

Yes, that will do