diff --git a/rtt_ros2_tf2/CMakeLists.txt b/rtt_ros2_tf2/CMakeLists.txt new file mode 100644 index 0000000..4f0a43c --- /dev/null +++ b/rtt_ros2_tf2/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 3.5) +project(rtt_ros2_tf2) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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 -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rtt_ros2 REQUIRED) +find_package(rtt_ros2_node REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +# setup targets +include_directories(include/orocos) + +# Target: library +# message(AUTHOR_WARNING "The tf2 includes are in: ${tf2_INCLUDE_DIRS}") +# message(AUTHOR_WARNING "The rclcpp includes are in: ${rclcpp_INCLUDE_DIRS}") +# message(AUTHOR_WARNING "The tf2_msgs includes are in: ${tf2_msgs_INCLUDE_DIRS}") +orocos_library(rtt_ros2_tf2 + src/rtt_ros2_tf2.cpp + EXPORT ${PROJECT_NAME} + INCLUDES DESTINATION include/orocos/${PROJECT_NAME} +) +# target_link_libraries(rtt_ros2_tf2 tf2) +# target_include_directories(rtt_ros2_tf2 tf2) +ament_target_dependencies(rtt_ros2_tf2 + geometry_msgs + rclcpp + rtt_ros2 + rtt_ros2_node + tf2 + tf2_msgs + tf2_ros +) +target_link_libraries(rtt_ros2_tf2 + rtt_ros2_node::rtt_ros2_node +) + +# Target: orocos plugin +orocos_plugin(rtt_ros2_tf2_service + src/rtt_ros2_tf2_service.cpp +) +target_link_libraries(rtt_ros2_tf2_service rtt_ros2_tf2) + +# Target: orocos component +orocos_component(rtt_ros2_tf2_component + src/rtt_ros2_tf2_component.cpp +) +target_link_libraries(rtt_ros2_tf2_component rtt_ros2_tf2) + +# install +install( + DIRECTORY include/ + DESTINATION include +) + +# linters +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +# export information to downstream packages +# ament_export_dependencies(rtt_ros2) +ament_export_dependencies( + geometry_msgs + rclcpp + tf2_msgs + tf2_ros + tf2 +) +ament_export_include_directories(include/orocos) +if(COMMAND ament_export_targets) + ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +else() + ament_export_interfaces(${PROJECT_NAME} HAS_LIBRARY_TARGET) +endif() +rtt_ros2_export_plugin_depend(rtt_ros2_node) + +# orocos_generate_package( +# INCLUDE_DIRS include +# DEPENDS tf2 +# DEPENDS_TARGETS rtt_geometry_msgs rtt_tf2_msgs +# ) + +# Legacy from rtt_tf +# Tests +# if(CATKIN_ENABLE_TESTING) +# orocos_use_package(ocl-taskbrowser REQUIRED) +# orocos_use_package(ocl-deployment REQUIRED) + +# add_definitions(-DRTT_COMPONENT) +# orocos_executable(broadcaster_test tests/broadcaster_test.cpp) +# target_link_libraries(broadcaster_test ${catkin_LIBRARIES} ${USE_OROCOS_LIBRARIES}) +# orocos_executable(lookup_test tests/lookup_test.cpp) +# target_link_libraries(lookup_test ${catkin_LIBRARIES} ${USE_OROCOS_LIBRARIES}) +# endif() + +# must be called *after* the targets to check exported libraries etc. +ament_package() + +# orocos_generate_package() is deprecated for ROS 2. +# Prefer cmake target export and import instead, in combination with +# ament_export_interfaces() or ament_export_targets() when building with +# ament_cmake. +orocos_generate_package() diff --git a/rtt_ros2_tf2/README.md b/rtt_ros2_tf2/README.md new file mode 100644 index 0000000..d62cdac --- /dev/null +++ b/rtt_ros2_tf2/README.md @@ -0,0 +1,49 @@ +RTT ROS2 TF2 +============ + +This package provides an Orocos RTT service for utilizing the [TF2 rigid body +transform library](https://index.ros.org/doc/ros2/Tutorials/tf2) from within +Orocos. +This service has operations for requesting and broadcasting sing and +batch transforms. + +## Service description + +The service provided by the library exposes some operations to interact with TF. +The operations available are: +* `sendTransform()`: +* `sendTransforms()`: +* `sendStaticTransform()`: +* `sendStaticTransforms()`: +* `lookupTransform()`: +* `clear()`: + + +### Usage + +#### Scripting API (*.ops) + +The package can be loaded independently form other related `ros` packages, but it requires the `rosnode` service +to work. + +The service can be loaded globally by importing the package: + +``` +import("rtt_ros2") +ros.import("rtt_ros2_tf2") +``` +This will create a global service under the `ros` namespace. The operations run by this +service will be executed in the `ClientThread` and are NOT real-time safe. + +To load the service into a component, after importing the package, it can be loaded with: + +``` +loadService("", "tf2") +``` + +The component version will run the calls in the `OwnThread`. The component should not be +real time. + +### C++ API + +*ToDo* diff --git a/rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2.hpp b/rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2.hpp new file mode 100644 index 0000000..15c898f --- /dev/null +++ b/rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2.hpp @@ -0,0 +1,124 @@ +// Copyright 2020 Intermodalics BVBA +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_HPP_ +#define OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "rtt/InputPort.hpp" +#include "rtt/Service.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" +#include "tf2/buffer_core.h" + +namespace rtt_ros2_tf2 +{ + +class Tf2Service : public RTT::Service +{ +public: + typedef boost::shared_ptr shared_ptr; + + explicit Tf2Service(RTT::TaskContext * owner); + virtual ~Tf2Service(); + +protected: + ///! Operations + // rclcpp::Time getLatestCommonTime( + // const std::string & target, + // const std::string & source) const; + + bool canTransform( + const std::string & target, + const std::string & source) const; + + geometry_msgs::msg::TransformStamped lookupTransform( + const std::string & target, + const std::string & source) const; + + // geometry_msgs::msg::TransformStamped lookupTransformAtTime( + // const std::string & target, + // const std::string & source, + // const rclcpp::Time& common_time) const; + + void broadcastTransform( + const geometry_msgs::msg::TransformStamped & transform); + + void broadcastTransforms( + const std::vector & transforms); + + void broadcastStaticTransform( + const geometry_msgs::msg::TransformStamped & transform); + + void broadcastStaticTransforms( + const std::vector & transforms); + + void clear(); + + void addTf2Interface(RTT::Service::shared_ptr service); + +protected: + ///! Communication ports + // Input ports + RTT::InputPort ip_stamped_transform_; + RTT::InputPort ip_stamped_transform_static_; + // RTT::InputPort ip_tf_port_; + +private: + // Constant + static constexpr int kDefaultBufferSize = 100; + + // Clock + rclcpp::Node::SharedPtr rosnode; + boost::shared_ptr clock_; + boost::shared_ptr buffer_core_; + // boost::shared_ptr buffer_client_; + boost::shared_ptr transform_broadcaster_; + boost::shared_ptr transform_listener_; + boost::shared_ptr + static_transform_broadcaster_; + + // Example members + + int private_int_ = 42; + // tf2::BufferCore buffer_core_; + + // Deprecated? + std::string prop_tf_prefix_; + double prop_cache_time_; + double prop_buffer_size_; + +private: + // void internalUpdate( + // tf2_msgs::TFMessage & msg, + // RTT::InputPort & port, + // bool is_static); + tf2_msgs::msg::TFMessage tf_msgs_; + void stamped_message_callback(RTT::base::PortInterface * port); + void stamped_message_static_callback(RTT::base::PortInterface * port); + + bool rosReady(); +}; // class Tf2Service + +} // namespace rtt_ros2_tf2 + +#endif // OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_HPP_ diff --git a/rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp b/rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp new file mode 100644 index 0000000..c47b9a4 --- /dev/null +++ b/rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp @@ -0,0 +1,41 @@ +// Copyright 2020 Intermodalics BVBA +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_COMPONENT_HPP_ +#define OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_COMPONENT_HPP_ + +#include + +#include "rtt/RTT.hpp" +// #include "tf2_msgs/TFMessage.h" +// #include "tf2/buffer_core.h" + +namespace rtt_ros2_tf2 +{ +// Inherit from TaskContext +class TF2_Component : public RTT::TaskContext +{ +public: + explicit TF2_Component(std::string const & name); + + bool configureHook(); + + void updateHook(); + + void cleanupHook(); +}; + +} // namespace rtt_ros2_tf2 + +#endif // OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_COMPONENT_HPP_ diff --git a/rtt_ros2_tf2/package.xml b/rtt_ros2_tf2/package.xml new file mode 100644 index 0000000..f3ce481 --- /dev/null +++ b/rtt_ros2_tf2/package.xml @@ -0,0 +1,29 @@ + + + + rtt_ros2_tf2 + 0.0.0 + + This package provides an interface to use TF2 in Orocos context. + + Orocos Developers + Apache License 2.0 + + ament_cmake + + geometry_msgs + rclcpp + rtt_ros2_node + rtt_ros2 + tf2_msgs + tf2_ros + tf2 + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp new file mode 100644 index 0000000..1399054 --- /dev/null +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp @@ -0,0 +1,382 @@ +// Copyright 2020 Intermodalics BVBA +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rtt_ros2_tf2/rtt_ros2_tf2.hpp" + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rtt/TaskContext.hpp" +#include "rtt_ros2_node/rtt_ros2_node.hpp" + +#include "rtt/internal/GlobalService.hpp" + +// The main interface happens through tf2::BufferCore +// http://docs.ros2.org/foxy/api/tf2/ +// http://docs.ros2.org/foxy/api/tf2/classtf2_1_1BufferCore.html +namespace rtt_ros2_tf2 +{ + +Tf2Service::Tf2Service(RTT::TaskContext * owner) +: RTT::Service("tf2", owner), + ip_stamped_transform_("ip_stamped_transform"), + ip_stamped_transform_static_("ip_stamped_transform_static"), + // ip_tf_port_("pi_tf_port"), + rosnode(nullptr), +// clock_(boost::make_shared(rcl_clock_type_t::RCL_SYSTEM_TIME)), + buffer_core_(boost::make_shared()), + // buffer_client_(nullptr), + transform_broadcaster_(nullptr), + transform_listener_(nullptr), + static_transform_broadcaster_(nullptr) +{ + RTT::Logger::In in(getName()); + clock_ = boost::make_shared(rcl_clock_type_t::RCL_SYSTEM_TIME); + // Ports are only added if a TaskContext is provided (not in GlobalService) + if (owner != nullptr) { + // Add the interface to the own service + addTf2Interface(this->provides()); + RTT::log(RTT::Info) << + "TF2 service instantiated in: " << owner->getName() << + RTT::endlog(); + // Add operations + this->addOperation( + "broadcastTransform", + &Tf2Service::broadcastTransform, this, RTT::OwnThread) + .doc("Broadcasts a stamped transform as TF2"); + this->addOperation( + "broadcastTransforms", + &Tf2Service::broadcastTransforms, this, RTT::OwnThread) + .doc("Broadcasts a vector of stamped transforms as TF2"); + this->addOperation( + "broadcastStaticTransform", + &Tf2Service::broadcastStaticTransform, this, RTT::OwnThread) + .doc("Broadcasts a stamped transform as TF2 static"); + this->addOperation( + "broadcastStaticTransforms", + &Tf2Service::broadcastStaticTransforms, this, RTT::OwnThread) + .doc("Broadcasts a vector of stamped transforms as TF2 static"); + this->addOperation( + "lookupTransform", + &Tf2Service::lookupTransform, this, RTT::OwnThread) + .doc("Looks up for a TF2 transform"); + this->addOperation( + "clear", + &Tf2Service::clear, this, RTT::OwnThread) + .doc("Clears TF2 transforms"); + } else { + // addTf2Interface( + // RTT::internal::GlobalService::Instance()->provides("tf2")); + // Add operations + this->addOperation( + "broadcastTransform", + &Tf2Service::broadcastTransform, this, RTT::ClientThread) + .doc("Broadcasts a stamped transform as TF2"); + this->addOperation( + "broadcastTransforms", + &Tf2Service::broadcastTransforms, this, RTT::ClientThread) + .doc("Broadcasts a vector of stamped transforms as TF2"); + this->addOperation( + "broadcastStaticTransform", + &Tf2Service::broadcastStaticTransform, this, RTT::ClientThread) + .doc("Broadcasts a stamped transform as TF2 static"); + this->addOperation( + "broadcastStaticTransforms", + &Tf2Service::broadcastStaticTransforms, this, RTT::ClientThread) + .doc("Broadcasts a vector of stamped transforms as TF2 static"); + this->addOperation( + "lookupTransform", + &Tf2Service::lookupTransform, this, RTT::ClientThread) + .doc("Looks up for a TF2 transform"); + this->addOperation( + "clear", + &Tf2Service::clear, this, RTT::ClientThread) + .doc("Clears TF2 transforms"); + RTT::log(RTT::Info) << + "TF2 service instantiated standalone without interface" << + RTT::endlog(); + } +} + +Tf2Service::~Tf2Service() = default; + +// void Tf2Service::internalUpdate( +// tf2_msgs::TFMessage & msg, +// RTT::InputPort & port, +// bool is_static) { + +// } + +// rclcpp::Time Tf2Service::getLatestCommonTime( +// const std::string & target, +// const std::string & source) const { +// return clock_->now(); +// } + +void Tf2Service::clear() +{ + RTT::Logger::In in(getName()); + if (buffer_core_) { + buffer_core_->clear(); + } +} + +bool Tf2Service::canTransform( + const std::string & target, + const std::string & source) const +{ + std::string ret_error; + // const tf2::TimePoint time_point_now = tf2::TimePoint(clock_->now()); + try { + if (!buffer_core_->canTransform( + target, + source, tf2::TimePointZero, + &ret_error)) + { + RTT::log(RTT::Warning) << "Cannot transform from " << + source << " to " << target << RTT::endlog(); + return false; + } + } catch (std::exception e) { + RTT::log(RTT::Error) << "canTransform() produced an exception: " << + e.what() << RTT::endlog(); + return false; + } + return true; +} + +geometry_msgs::msg::TransformStamped Tf2Service::lookupTransform( + const std::string & target, + const std::string & source) const +{ + try { + return buffer_core_->lookupTransform(target, source, tf2::TimePointZero); + } catch (std::exception e) { + RTT::log(RTT::Error) << "lookupTransform() produced an exception: " << + e.what() << RTT::endlog(); + return geometry_msgs::msg::TransformStamped(); + } +} + +// geometry_msgs::msg::TransformStamped Tf2Service::lookupTransformAtTime( +// const std::string & target, +// const std::string & source, +// const rclcpp::Time & common_time) const { + +// } + +void Tf2Service::broadcastTransform( + const geometry_msgs::msg::TransformStamped & transform) +{ + RTT::Logger::In in(getName()); + if (!rosReady()) { + RTT::log(RTT::Error) << "Ros Node is not ready" << RTT::endlog(); + return; + } + // tf2_msgs::msg::TFMessage converted_tf2_msg; + try { + std::string authority_name; + if (nullptr != getOwner()) { + authority_name = getOwner()->getName(); + } else { + authority_name = getName(); + } + // RTT::log(RTT::Debug) << "Calling broadcast a transform and setting " + // "authority to: " << authority_name << " in the next line " + // "with pos.x = " << transform.transform.translation.x << RTT::endlog(); + buffer_core_->setTransform(transform, authority_name, false); + transform_broadcaster_->sendTransform(transform); + } catch (tf2::LookupException e) { + RTT::log(RTT::Error) << "Error when setting transform: " << + e.what() << RTT::endlog(); + } +} + +void Tf2Service::broadcastTransforms( + const std::vector & transforms) +{ + if (!rosReady()) { + return; + } + // for(const auto & transform : transforms) { + // broadcastTransform(transform); + // } + try { + std::string authority_name; + if (nullptr != getOwner()) { + authority_name = getOwner()->getName(); + } else { + authority_name = getName(); + } + for (const auto & transform : transforms) { + buffer_core_->setTransform(transform, authority_name, false); + } + // buffer_core_->setTransform(transform, "unknown_authority", false); + transform_broadcaster_->sendTransform(transforms); + } catch (std::exception e) { + RTT::log(RTT::Error) << "Error when setting transform: " << + e.what() << RTT::endlog(); + } +} + +void Tf2Service::broadcastStaticTransform( + const geometry_msgs::msg::TransformStamped & transform) +{ + if (!rosReady()) { + return; + } + try { + std::string authority_name; + if (nullptr != getOwner()) { + authority_name = getOwner()->getName(); + } else { + authority_name = getName(); + } + buffer_core_->setTransform(transform, authority_name, true); + static_transform_broadcaster_->sendTransform(transform); + } catch (tf2::LookupException e) { + RTT::log(RTT::Error) << "Error when setting static transform: " << + e.what() << RTT::endlog(); + } +} + +void Tf2Service::broadcastStaticTransforms( + const std::vector & transforms) +{ + if (!rosReady()) { + return; + } + // for(const auto & transform : transforms) { + // broadcastStaticTransform(transform); + // } + try { + std::string authority_name; + if (nullptr != getOwner()) { + authority_name = getOwner()->getName(); + } else { + authority_name = getName(); + } + for (const auto & transform : transforms) { + buffer_core_->setTransform(transform, authority_name, true); + } + static_transform_broadcaster_->sendTransform(transforms); + } catch (std::exception e) { + RTT::log(RTT::Error) << "Error when setting transform: " << + e.what() << RTT::endlog(); + } +} + +void Tf2Service::addTf2Interface(RTT::Service::shared_ptr service) +{ + // Ports cannot be added to the GlobalService + // Add ports + service->addEventPort( + ip_stamped_transform_, + boost::bind(&Tf2Service::stamped_message_callback, this, _1)) + .doc("Reception of stamped transform to broadcast through TF2"); + service->addEventPort( + ip_stamped_transform_static_, + boost::bind(&Tf2Service::stamped_message_static_callback, this, _1)) + .doc("Reception of stamped transform to broadcast through TF2 static"); +} + +void Tf2Service::stamped_message_callback(RTT::base::PortInterface * /*in_port*/) +{ + RTT::Logger::In in(getName()); + RTT::log(RTT::Warning) << "Callback called" << RTT::endlog(); + geometry_msgs::msg::TransformStamped in_msg; + if (ip_stamped_transform_.read(in_msg) != RTT::NewData) { + RTT::log(RTT::Warning) << "Received an event without new data" << + RTT::endlog(); + } else { + broadcastTransform(in_msg); + } +} + +void Tf2Service::stamped_message_static_callback( + RTT::base::PortInterface * /*in_port*/) +{ + RTT::Logger::In in(getName()); + RTT::log(RTT::Warning) << "Callback called" << RTT::endlog(); + geometry_msgs::msg::TransformStamped in_msg; + if (ip_stamped_transform_static_.read(in_msg) != RTT::NewData) { + RTT::log(RTT::Warning) << "Received an event without new data" << + RTT::endlog(); + } else { + broadcastStaticTransform(in_msg); + } +} + +bool Tf2Service::rosReady() +{ + if (!rosnode) { + rosnode = rtt_ros2_node::getNode(getOwner()); + } + if (!rosnode) { + RTT::log(RTT::Error) << + "No ROS node service from package rtt_ros2_node loaded into this " + "component or as a global service." << + RTT::endlog(); + return false; + } + RTT::log(RTT::Info) << "We have node: " << rosnode->get_name() << RTT::endlog(); + if ((nullptr != transform_broadcaster_) && + (nullptr != transform_listener_) && + (nullptr != static_transform_broadcaster_)) + { + return true; + } else { + // Initialize the interfaces to ros + // buffer_core_ = boost::make_shared(); + transform_broadcaster_ = boost::make_shared( + rosnode); + if (nullptr == transform_broadcaster_) { + RTT::log(RTT::Error) << "Could not initialize broadcaster" << + RTT::endlog(); + } + // ToDo: add parameter to decide spin_thread = spin_thread_ in nex call + if (nullptr == buffer_core_) { + RTT::log(RTT::Error) << "Buffer core is NULLPTR" << + RTT::endlog(); + } + transform_listener_ = boost::make_shared( + *buffer_core_, true); + if (nullptr == transform_listener_) { + RTT::log(RTT::Error) << "Could not initialize listener" << + RTT::endlog(); + } + static_transform_broadcaster_ = boost::make_shared< + tf2_ros::StaticTransformBroadcaster>( + rosnode); + if (nullptr == static_transform_broadcaster_) { + RTT::log(RTT::Error) << "Could not initialize static broadcaster" << + RTT::endlog(); + } + if ((nullptr == transform_broadcaster_) || + (nullptr == transform_listener_) || + (nullptr == static_transform_broadcaster_)) + { + transform_broadcaster_.reset(); + transform_listener_.reset(); + static_transform_broadcaster_.reset(); + RTT::log(RTT::Error) << "Some transform object could not get " + "initialized" << RTT::endlog(); + return false; + } + } + return true; +} + +} // namespace rtt_ros2_tf2 diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2_component.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2_component.cpp new file mode 100644 index 0000000..bcce3be --- /dev/null +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2_component.cpp @@ -0,0 +1,66 @@ +// Copyright 2020 Intermodalics BVBA +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rtt_ros2_tf2/rtt_ros2_tf2_component.hpp" + +#include + +#include "rtt/Component.hpp" + +namespace rtt_ros2_tf2 +{ + +TF2_Component::TF2_Component(const std::string & name) +: RTT::TaskContext(name, PreOperational) +{ + RTT::Logger::In(this->getName()); + RTT::log(RTT::Info) << "Constructing component!" << RTT::endlog(); +} + + +bool TF2_Component::configureHook() +{ + RTT::Logger::In(this->getName()); + + RTT::log(RTT::Info) << "Configuring component!" << RTT::endlog(); + return true; +} + + +void TF2_Component::updateHook() +{ + RTT::Logger::In(this->getName()); + RTT::log(RTT::Info) << "Running component!" << RTT::endlog(); +} + +void TF2_Component::cleanupHook() +{ + RTT::Logger::In(this->getName()); + RTT::log(RTT::Info) << "Cleaning up component!" << RTT::endlog(); +} +} // namespace rtt_ros2_tf2 + +/* + * Using this macro, only one component may live + * in one library *and* you may *not* link this library + * with another component library. Use + * ORO_CREATE_COMPONENT_TYPE() + * ORO_LIST_COMPONENT_TYPE(Rtt_tf) + * In case you want to link with another library that + * already contains components. + * + * If you have put your component class + * in a namespace, don't forget to add it here too: + */ +ORO_CREATE_COMPONENT(rtt_ros2_tf2::TF2_Component) diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp new file mode 100644 index 0000000..50d02dc --- /dev/null +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp @@ -0,0 +1,80 @@ +// Copyright 2020 Intermodalics BVBA +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +// #include "rtt/deployment/ComponentLoader.hpp" +#include "rtt/internal/GlobalService.hpp" +#include "rtt/plugin/ServicePlugin.hpp" + +#include "boost/make_shared.hpp" + +#include "rtt_ros2_tf2/rtt_ros2_tf2.hpp" + + +namespace rtt_ros2_tf2 +{ + +static bool loadROSServiceIntoTaskContext(RTT::TaskContext * tc) +{ + if (tc->provides()->hasService("tf2")) { + RTT::log(RTT::Error) << + "Another rosparam interface was already instantiated for component " << + tc->getName() << "." << RTT::endlog(); + return false; + } + + auto tf_service = boost::make_shared(tc); + // tf_service->addTf2Interface(tc->provides()); + tc->provides()->addService(std::move(tf_service)); + return true; +} + +static bool loadGlobalROSService() +{ + RTT::Service::shared_ptr ros = + RTT::internal::GlobalService::Instance()->provides("ros"); + ros->doc("ROS operations and services"); + rtt_ros2_tf2::Tf2Service::shared_ptr tf_service = + boost::make_shared(nullptr); + + // tf_service->addTf2Interface(tf_service); + if (!ros->addService(std::move(tf_service))) { + // addService() can fail if a service of the same name already exists in ros + RTT::log(RTT::Error) << "The global ROS service could not load tf2 " + "support" << RTT::endlog(); + return false; + } + + RTT::log(RTT::Info) << + "Initializing interface to ROS TF2" << + RTT::endlog(); + return true; +} + +extern "C" { +bool loadRTTPlugin(RTT::TaskContext * tc) +{ + if (tc == nullptr) { + return loadGlobalROSService(); + } else { + return loadROSServiceIntoTaskContext(tc); + } +} +std::string getRTTPluginName() {return "tf2";} +std::string getRTTTargetName() {return OROCOS_TARGET_NAME;} +} // extern "C" + +} // namespace rtt_ros2_tf2 diff --git a/tests/rtt_ros2_tf2_tests/CMakeLists.txt b/tests/rtt_ros2_tf2_tests/CMakeLists.txt new file mode 100644 index 0000000..a413dda --- /dev/null +++ b/tests/rtt_ros2_tf2_tests/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.5) +project(rtt_ros2_tf2_tests) + +# Skip building this package if BUILD_TESTING is OFF +option(BUILD_TESTING "Build the testing tree." ON) +if(NOT BUILD_TESTING) + install(CODE + "message(STATUS \"Skipping installation of package ${PROJECT_NAME} because BUILD_TESTING is OFF.\")" + ) + return() +endif() + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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 -Wpedantic) +# endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rtt_ros2 REQUIRED) +find_package(rtt_ros2_node REQUIRED) +find_package(rtt_ros2_tf2 REQUIRED) +# find_package(rtt_ros2_geometry_msgs REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() + +# unit tests +find_package(ament_cmake_gtest REQUIRED) + +# The test_ros_tf2 fails with +# +# double free or corruption (fasttop) +# +# during destruction in ROS dashing. This seems to be a known problem: +# https://gitlab.com/ApexAI/performance_test/-/issues/87 +# ==> workaround: disable the test for dashing +# +set(ROS_DISTRO $ENV{ROS_DISTRO}) +if(ROS_DISTRO STREQUAL "dashing") + return() +endif() + +ament_add_gtest(test_ros_tf2 test/test_ros_tf2.cpp) +orocos_configure_executable(test_ros_tf2) +target_link_libraries(test_ros_tf2 + rtt_ros2::rtt_ros2 + rtt_ros2_node::rtt_ros2_node +# rtt_ros2_tf2::rtt_ros2_tf2 +) +ament_target_dependencies(test_ros_tf2 + rtt_ros2 rtt_ros2_node rtt_ros2_tf2 geometry_msgs +) \ No newline at end of file diff --git a/tests/rtt_ros2_tf2_tests/package.xml b/tests/rtt_ros2_tf2_tests/package.xml new file mode 100644 index 0000000..5e9b724 --- /dev/null +++ b/tests/rtt_ros2_tf2_tests/package.xml @@ -0,0 +1,26 @@ + + + + rtt_ros2_tf2_tests + 0.0.0 + + Unit tests for rtt_ros2_tf2 + + Orocos Developers + Apache License 2.0 + + ament_cmake + + geometry_msgs + rtt_ros2 + rtt_ros2_node + rtt_ros2_tf2 + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp b/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp new file mode 100644 index 0000000..20362d7 --- /dev/null +++ b/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp @@ -0,0 +1,366 @@ +// Copyright 2020 Intermodalics BVBA +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rtt/RTT.hpp" +#include "rtt/OperationCaller.hpp" +#include "rtt/internal/GlobalService.hpp" +#include "rtt/internal/GlobalEngine.hpp" +#include "rtt/os/startstop.h" + +#include "rtt_ros2/rtt_ros2.hpp" +#include "rtt_ros2_node/rtt_ros2_node.hpp" +#include "rtt_ros2_tf2/rtt_ros2_tf2.hpp" + + +// #include "geometry_msgs/typekit/Types.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" + +#include "gtest/gtest.h" + +class TestRosTf2Environment + : public ::testing::Environment +{ + void SetUp() override + { + EXPECT_TRUE(rtt_ros2::import("rtt_ros2_tf2")); + } +}; + +class TestRosTf2 + : public RTT::TaskContext, + public ::testing::Test +{ +public: + TestRosTf2() + : RTT::TaskContext("TestRosTf2") + { + this->addProperty("int_property", int_member_); + this->addProperty("double_property", double_member_); + this->addProperty("bool_property", bool_member_); + } + +protected: + int int_member_ = 42; + bool bool_member_ = true; + double double_member_ = 3.14159; + std::string string_member_ = std::string("answer to life and universe"); +}; + +TEST_F(TestRosTf2, TestGlobalNodeTf2) +{ + RTT::Logger::In in(getName()); + + // Create a global (process-wide) ROS node + RTT::Service::shared_ptr global_ros = + RTT::internal::GlobalService::Instance()->getService("ros"); + ASSERT_TRUE(global_ros); + RTT::OperationCaller create_node = + global_ros->getOperation("create_node"); + ASSERT_TRUE(create_node.ready()); + ASSERT_TRUE(create_node()); + ASSERT_TRUE(rtt_ros2_node::getNodeService(this) != nullptr); + ASSERT_TRUE(rtt_ros2_node::getNode(this) != nullptr); + + // Create a global (process-wide) TF2 service + // ASSERT_TRUE(RTT::internal::GlobalService::Instance()->provides("tf2")); + RTT::Service::shared_ptr global_tf2 = + global_ros->getService("tf2"); + ASSERT_TRUE(nullptr != global_tf2); + + // Check operations + RTT::OperationCaller + broadcast_transform_operation = global_tf2->getOperation("broadcastTransform"); + ASSERT_TRUE(broadcast_transform_operation.ready()); + + RTT::OperationCaller)> + broadcast_transforms_operation = global_tf2->getOperation("broadcastTransforms"); + ASSERT_TRUE(broadcast_transforms_operation.ready()); + + RTT::OperationCaller + broadcast_static_transform_operation = + global_tf2->getOperation("broadcastStaticTransform"); + ASSERT_TRUE(broadcast_static_transform_operation.ready()); + + RTT::OperationCaller)> + broadcast_static_transforms_operation = + global_tf2->getOperation("broadcastStaticTransforms"); + ASSERT_TRUE(broadcast_static_transforms_operation.ready()); + + // RTT::OperationCaller + // can_transform_operation = + // global_tf2->getOperation("canTransform"); + // ASSERT_TRUE(can_transform_operation.ready()); + + RTT::OperationCaller + lookup_transform_operation = + global_tf2->getOperation("lookupTransform"); + ASSERT_TRUE(lookup_transform_operation.ready()); + + RTT::OperationCaller + clear_operation = + global_tf2->getOperation("clear"); + ASSERT_TRUE(clear_operation.ready()); + + // Check broadcastTransform / lookupTransform + { + geometry_msgs::msg::TransformStamped test_transform_out, test_transform_in; + test_transform_out.child_frame_id = std::string("gbase0"); + test_transform_out.header.frame_id = std::string("world"); + test_transform_out.transform.translation.x = 2.0; + test_transform_out.transform.rotation.y = 5.3; + test_transform_in = lookup_transform_operation.call("world", "gbase0"); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + EXPECT_EQ(test_transform_in, geometry_msgs::msg::TransformStamped()); + broadcast_transform_operation.call(test_transform_out); + // RTT::log(RTT::Debug) << "Out: " << + // test_transform_out.transform.translation.x << RTT::endlog(); + test_transform_in = lookup_transform_operation.call("world", "gbase0"); + EXPECT_NE(test_transform_in, geometry_msgs::msg::TransformStamped()); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + } + // Check broadcastTransforms / lookupTransform + { + geometry_msgs::msg::TransformStamped test_transform_out_1, test_transform_out_2, + test_transform_in; + std::vector transforms; + test_transform_out_1.child_frame_id = std::string("gbase1"); + test_transform_out_1.header.frame_id = std::string("world"); + test_transform_out_1.transform.translation.x = 2.0; + test_transform_out_1.transform.rotation.y = 5.3; + transforms.push_back(test_transform_out_1); + test_transform_out_2.child_frame_id = std::string("gbase2"); + test_transform_out_2.header.frame_id = std::string("world"); + test_transform_out_2.transform.translation.x = 3.0; + test_transform_out_2.transform.rotation.y = 2.3; + transforms.push_back(test_transform_out_2); + test_transform_in = lookup_transform_operation.call("gbase1", "gbase2"); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + EXPECT_EQ(test_transform_in, geometry_msgs::msg::TransformStamped()); + broadcast_transforms_operation.call(transforms); + // RTT::log(RTT::Debug) << "Out: " << + // test_transform_out.transform.translation.x << RTT::endlog(); + test_transform_in = lookup_transform_operation.call("gbase1", "gbase2"); + EXPECT_NE(test_transform_in, geometry_msgs::msg::TransformStamped()); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + } + // Check broadcastStaticTransform / lookupTransform + { + geometry_msgs::msg::TransformStamped test_transform_out, test_transform_in; + test_transform_out.child_frame_id = std::string("gbase3"); + test_transform_out.header.frame_id = std::string("world"); + test_transform_out.transform.translation.x = 2.0; + test_transform_out.transform.rotation.y = 5.3; + test_transform_in = lookup_transform_operation.call("world", "gbase3"); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + EXPECT_EQ(test_transform_in, geometry_msgs::msg::TransformStamped()); + broadcast_static_transform_operation.call(test_transform_out); + // RTT::log(RTT::Debug) << "Out: " << + // test_transform_out.transform.translation.x << RTT::endlog(); + test_transform_in = lookup_transform_operation.call("world", "gbase3"); + EXPECT_NE(test_transform_in, geometry_msgs::msg::TransformStamped()); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + } + // Check broadcastStaticTransforms / lookupTransform + { + geometry_msgs::msg::TransformStamped test_transform_out_1, test_transform_out_2, + test_transform_in; + std::vector transforms; + test_transform_out_1.child_frame_id = std::string("gbase4"); + test_transform_out_1.header.frame_id = std::string("world"); + test_transform_out_1.transform.translation.x = 2.0; + test_transform_out_1.transform.rotation.y = 5.3; + transforms.push_back(test_transform_out_1); + test_transform_out_2.child_frame_id = std::string("gbase5"); + test_transform_out_2.header.frame_id = std::string("world"); + test_transform_out_2.transform.translation.x = 3.0; + test_transform_out_2.transform.rotation.y = 2.3; + transforms.push_back(test_transform_out_2); + test_transform_in = lookup_transform_operation.call("gbase4", "gbase5"); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + EXPECT_EQ(test_transform_in, geometry_msgs::msg::TransformStamped()); + broadcast_static_transforms_operation.call(transforms); + // RTT::log(RTT::Debug) << "Out: " << + // test_transform_out.transform.translation.x << RTT::endlog(); + test_transform_in = lookup_transform_operation.call("gbase4", "gbase5"); + EXPECT_NE(test_transform_in, geometry_msgs::msg::TransformStamped()); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + } +} + +TEST_F(TestRosTf2, TestComponentNodeTf2) +{ + RTT::Logger::In in(getName()); + + // Create a component-local ROS node + ASSERT_TRUE(loadService("rosnode")); + ASSERT_TRUE(rtt_ros2_node::getNodeService(this) != nullptr); + ASSERT_TRUE(rtt_ros2_node::getNode(this) != nullptr); + + // Get local service + ASSERT_TRUE(this->loadService("tf2")); + RTT::Service::shared_ptr local_tf2 = + this->provides("tf2"); + ASSERT_TRUE(nullptr != local_tf2); + + // Check operations + RTT::OperationCaller + broadcast_transform_operation = local_tf2->getOperation("broadcastTransform"); + broadcast_transform_operation.setCaller(RTT::internal::GlobalEngine::Instance()); + ASSERT_TRUE(broadcast_transform_operation.ready()); + + RTT::OperationCaller)> + broadcast_transforms_operation = local_tf2->getOperation("broadcastTransforms"); + broadcast_transforms_operation.setCaller(RTT::internal::GlobalEngine::Instance()); + ASSERT_TRUE(broadcast_transforms_operation.ready()); + + RTT::OperationCaller + broadcast_static_transform_operation = + local_tf2->getOperation("broadcastStaticTransform"); + broadcast_static_transform_operation.setCaller(RTT::internal::GlobalEngine::Instance()); + ASSERT_TRUE(broadcast_static_transform_operation.ready()); + + RTT::OperationCaller)> + broadcast_static_transforms_operation = + local_tf2->getOperation("broadcastStaticTransforms"); + broadcast_static_transforms_operation.setCaller(RTT::internal::GlobalEngine::Instance()); + ASSERT_TRUE(broadcast_static_transforms_operation.ready()); + + // RTT::OperationCaller + // can_transform_operation = + // global_tf2->getOperation("canTransform"); + // ASSERT_TRUE(can_transform_operation.ready()); + + RTT::OperationCaller + lookup_transform_operation = + local_tf2->getOperation("lookupTransform"); + lookup_transform_operation.setCaller(RTT::internal::GlobalEngine::Instance()); + ASSERT_FALSE(RTT::internal::GlobalEngine::Instance()->isSelf()); + ASSERT_TRUE(lookup_transform_operation.ready()); + + RTT::OperationCaller + clear_operation = + local_tf2->getOperation("clear"); + clear_operation.setCaller(RTT::internal::GlobalEngine::Instance()); + ASSERT_TRUE(clear_operation.ready()); + + // Check broadcastTransform / lookupTransform + { + geometry_msgs::msg::TransformStamped test_transform_out, test_transform_in; + test_transform_out.child_frame_id = std::string("base0"); + test_transform_out.header.frame_id = std::string("world"); + test_transform_out.transform.translation.x = 2.0; + test_transform_out.transform.rotation.y = 5.3; + test_transform_in = lookup_transform_operation.call("world", "base0"); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + EXPECT_EQ(test_transform_in, geometry_msgs::msg::TransformStamped()); + broadcast_transform_operation.call(test_transform_out); + // RTT::log(RTT::Debug) << "Out: " << + // test_transform_out.transform.translation.x << RTT::endlog(); + test_transform_in = lookup_transform_operation.call("world", "base0"); + EXPECT_NE(test_transform_in, geometry_msgs::msg::TransformStamped()); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + } + // Check broadcastTransforms / lookupTransform + { + geometry_msgs::msg::TransformStamped test_transform_out_1, test_transform_out_2, + test_transform_in; + std::vector transforms; + test_transform_out_1.child_frame_id = std::string("base1"); + test_transform_out_1.header.frame_id = std::string("world"); + test_transform_out_1.transform.translation.x = 2.0; + test_transform_out_1.transform.rotation.y = 5.3; + transforms.push_back(test_transform_out_1); + test_transform_out_2.child_frame_id = std::string("base2"); + test_transform_out_2.header.frame_id = std::string("world"); + test_transform_out_2.transform.translation.x = 3.0; + test_transform_out_2.transform.rotation.y = 2.3; + transforms.push_back(test_transform_out_2); + test_transform_in = lookup_transform_operation.call("base1", "base2"); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + EXPECT_EQ(test_transform_in, geometry_msgs::msg::TransformStamped()); + broadcast_transforms_operation.call(transforms); + // RTT::log(RTT::Debug) << "Out: " << + // test_transform_out.transform.translation.x << RTT::endlog(); + test_transform_in = lookup_transform_operation.call("base1", "base2"); + EXPECT_NE(test_transform_in, geometry_msgs::msg::TransformStamped()); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + } + // Check broadcastStaticTransform / lookupTransform + { + geometry_msgs::msg::TransformStamped test_transform_out, test_transform_in; + test_transform_out.child_frame_id = std::string("base3"); + test_transform_out.header.frame_id = std::string("world"); + test_transform_out.transform.translation.x = 2.0; + test_transform_out.transform.rotation.y = 5.3; + test_transform_in = lookup_transform_operation.call("world", "base3"); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + EXPECT_EQ(test_transform_in, geometry_msgs::msg::TransformStamped()); + broadcast_static_transform_operation.call(test_transform_out); + // RTT::log(RTT::Debug) << "Out: " << + // test_transform_out.transform.translation.x << RTT::endlog(); + test_transform_in = lookup_transform_operation.call("world", "base3"); + EXPECT_NE(test_transform_in, geometry_msgs::msg::TransformStamped()); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + } + // Check broadcastStaticTransforms / lookupTransform + { + geometry_msgs::msg::TransformStamped test_transform_out_1, test_transform_out_2, + test_transform_in; + std::vector transforms; + test_transform_out_1.child_frame_id = std::string("base4"); + test_transform_out_1.header.frame_id = std::string("world"); + test_transform_out_1.transform.translation.x = 2.0; + test_transform_out_1.transform.rotation.y = 5.3; + transforms.push_back(test_transform_out_1); + test_transform_out_2.child_frame_id = std::string("base5"); + test_transform_out_2.header.frame_id = std::string("world"); + test_transform_out_2.transform.translation.x = 3.0; + test_transform_out_2.transform.rotation.y = 2.3; + transforms.push_back(test_transform_out_2); + test_transform_in = lookup_transform_operation.call("base4", "base5"); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + EXPECT_EQ(test_transform_in, geometry_msgs::msg::TransformStamped()); + broadcast_static_transforms_operation.call(transforms); + // RTT::log(RTT::Debug) << "Out: " << + // test_transform_out.transform.translation.x << RTT::endlog(); + test_transform_in = lookup_transform_operation.call("base4", "base5"); + EXPECT_NE(test_transform_in, geometry_msgs::msg::TransformStamped()); + // RTT::log(RTT::Debug) << "In: " << test_transform_in.transform.translation.x << RTT::endlog(); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + // __os_init() must be called after testing::InitGoogleTest(&argc, argv) + // because this function removes Google Test flags from argc/argv. + __os_init(argc, argv); + + const auto env = ::testing::AddGlobalTestEnvironment( + new TestRosTf2Environment); + int ret = RUN_ALL_TESTS(); + + __os_exit(); + return ret; +}