From 252ea307f390f4e053e14618c72cb89edc6f1b0a Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Wed, 15 Jul 2020 18:41:38 +0200 Subject: [PATCH 01/21] rtt_ros2_tf2: create the skeleton of new package rtt_ros2_tf2 This patch includes only the skeleton without functionality of the new package rtt_ros2_tf2. The package will provide Orocos support for TF2 in ROS 2. --- rtt_ros2_tf2/CMakeLists.txt | 125 ++++++++++++++++++ rtt_ros2_tf2/README.md | 22 +++ .../include/rtt_ros2_tf2/rtt_ros2_tf2.hpp | 113 ++++++++++++++++ .../rtt_ros2_tf2/rtt_ros2_tf2_component.hpp | 39 ++++++ rtt_ros2_tf2/package.xml | 30 +++++ rtt_ros2_tf2/src/rtt_ros2_tf2.cpp | 105 +++++++++++++++ rtt_ros2_tf2/src/rtt_ros2_tf2_component.cpp | 66 +++++++++ rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp | 51 +++++++ 8 files changed, 551 insertions(+) create mode 100644 rtt_ros2_tf2/CMakeLists.txt create mode 100644 rtt_ros2_tf2/README.md create mode 100644 rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp create mode 100644 rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp create mode 100644 rtt_ros2_tf2/package.xml create mode 100644 rtt_ros2_tf2/src/rtt_ros2_tf2.cpp create mode 100644 rtt_ros2_tf2/src/rtt_ros2_tf2_component.cpp create mode 100644 rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp diff --git a/rtt_ros2_tf2/CMakeLists.txt b/rtt_ros2_tf2/CMakeLists.txt new file mode 100644 index 0000000..511237c --- /dev/null +++ b/rtt_ros2_tf2/CMakeLists.txt @@ -0,0 +1,125 @@ +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() + +# Include RTT, this should include orocos_*() macros +find_package(OROCOS-RTT REQUIRED) +if(NOT OROCOS-RTT_FOUND) + message(FATAL_ERROR "\n RTT not found") +else() + include(${OROCOS-RTT_USE_FILE_PATH}/UseOROCOS-RTT.cmake) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(rtt_ros2_geometry_msgs REQUIRED) +# find_package(rtt_tf2_msgs REQUIRED) + +# Include rtt_ros2 for the macro: rtt_ros2_export_plugin_depend() +find_package(rtt_ros2 REQUIRED) + +# setup targets +include_directories(include) + +# Target: library +orocos_library(rtt_ros2_tf2 + src/rtt_ros2_tf2.cpp + EXPORT ${PROJECT_NAME} + INCLUDES DESTINATION include/${PROJECT_NAME} +) +# target_link_libraries(rtt_ros2_tf2 tf2) +ament_target_dependencies(rtt_ros2_tf2 + rclcpp tf2 rtt_ros2_geometry_msgs +) + +# 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) +ament_target_dependencies(rtt_ros2_tf2_component + OROCOS-RTT +) +# target_link_libraries(rtt_ros2_tf2_component ${catkin_LIBRARIES}) + +# Target: typekit (from tf2) +# ros_generate_rtt_typekit(tf2) + +# install +install( + DIRECTORY include/ + DESTINATION include +) + +# orocos_install_headers(include/rtt_ros2_tf2/rtt_ros2_tf2.hpp) +# orocos_install_headers(include/rtt_ros2_tf2/tf2_interface.h) +# orocos_install_headers(include/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp) + +# 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(rclcpp tf2) +ament_export_include_directories(include/${PROJECT_NAME}) +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(rclcpp) +rtt_ros2_export_plugin_depend(tf2) +rtt_ros2_export_plugin_depend(rtt_ros2_tf2) +# rtt_ros2_export_plugin_depend(rtt_ros2_tf2_msgs) # If typekit is generated + +# 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..cf9bf24 --- /dev/null +++ b/rtt_ros2_tf2/README.md @@ -0,0 +1,22 @@ +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. + +### ToDo + +Implement it. + +### Usage + +#### Scripting API (*.ops) + +TODO + +### C++ API + +TODO diff --git a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp new file mode 100644 index 0000000..807809a --- /dev/null +++ b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp @@ -0,0 +1,113 @@ +// 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 "rtt/Service.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "tf2/buffer_core.h" +#include "geometry_msgs/typekit/msg/transform_stamped_Types.hpp" + +namespace rtt_ros2_tf2 +{ + +class RTT_TF2 : public RTT::Service, protected tf2::BufferCore +{ + + +public: + typedef boost::shared_ptr shared_ptr; + + explicit RTT_TF2(RTT::TaskContext * owner); + virtual ~RTT_TF2(); + +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; + + bool canTransformAtTime( + const std::string & target, + const std::string & source, + const rclcpp::Time & common_time) const; + + // geometry_msgs::TransformStamped lookupTransform( + // const std::string & target, + // const std::string & source) const; + + // geometry_msgs::TransformStamped lookupTransformAtTime( + // const std::string & target, + // const std::string & source, + // const rclcpp::Time& common_time) const; + + void broadcastTransform( + const geometry_msgs::msg::TransformStamped & tform); + + void broadcastTransforms( + const std::vector & tforms); + + void broadcastStaticTransform( + const geometry_msgs::msg::TransformStamped & tform); + + void broadcastStaticTransforms( + const std::vector & tforms); + + void addTFOperations(RTT::Service::shared_ptr service); + + ///! Communication ports + // RTT::InputPort port_tf_in; + // RTT::InputPort port_tf_static_in; + // RTT::OutputPort port_tf_out; + // RTT::OutputPort port_tf_static_out; + +private: + // Constant + static constexpr int kDefaultBufferSize = 100; + + // Clock + boost::shared_ptr clock_; + + // 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); + +}; // class RTT_TF2 + +} // namespace rtt_ros2_tf2 + +#endif // OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_HPP_ diff --git a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp new file mode 100644 index 0000000..4355631 --- /dev/null +++ b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp @@ -0,0 +1,39 @@ +// 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_COMPONENT_HPP +#define OROCOS_RTT_ROS2_TF2_COMPONENT_HPP + +#include +// #include +// #include + +namespace rtt_ros2_tf2 +{ + // Inherit from TaskContext + class TF2_Component: public RTT::TaskContext + { + public: + TF2_Component(std::string const& name); + + bool configureHook(); + + void updateHook(); + + void cleanupHook(); + }; + +}//namespace rtt_ros2_tf2 + +#endif //OROCOS_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..2a6c458 --- /dev/null +++ b/rtt_ros2_tf2/package.xml @@ -0,0 +1,30 @@ + + + + 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 + + ament_index_cpp + rtt + + rclcpp + tf2 + rtt_ros2 + rtt_ros2_geometry_msgs + + + 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..23a4471 --- /dev/null +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp @@ -0,0 +1,105 @@ +// 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 "rclcpp/rclcpp.hpp" + +#include "rtt/internal/GlobalService.hpp" + +namespace rtt_ros2_tf2 +{ + +RTT_TF2::RTT_TF2(RTT::TaskContext * owner) +: RTT::Service("TF2", owner), + clock_(boost::make_shared(RCL_SYSTEM_TIME)) +{ + RTT::Logger::In in(getName()); + + RTT::log(RTT::Info) << + "TF2 service instantiated! in " << getName() << + RTT::endlog(); +} + +RTT_TF2::~RTT_TF2() = default; + +// void RTT_TF2::internalUpdate( +// tf2_msgs::TFMessage & msg, +// RTT::InputPort & port, +// bool is_static) { + +// } + +rclcpp::Time RTT_TF2::getLatestCommonTime( + const std::string & target, + const std::string & source) const { + + return clock_->now(); +} + +bool RTT_TF2::canTransform( + const std::string & target, + const std::string & source) const { + + return false; +} + +bool RTT_TF2::canTransformAtTime( + const std::string& target, + const std::string& source, + const rclcpp::Time& common_time) const { + + return false; +} + +// geometry_msgs::TransformStamped RTT_TF2::lookupTransform( +// const std::string & target, +// const std::string & source) const { + +// } + +// geometry_msgs::TransformStamped RTT_TF2::lookupTransformAtTime( +// const std::string & target, +// const std::string & source, +// const rclcpp::Time & common_time) const { + +// } + +void RTT_TF2::broadcastTransform( + const geometry_msgs::msg::TransformStamped & tform) { + +} + +void RTT_TF2::broadcastTransforms( + const std::vector & tforms) { + +} + +void RTT_TF2::broadcastStaticTransform( + const geometry_msgs::msg::TransformStamped & tform) { + +} + +void RTT_TF2::broadcastStaticTransforms( + const std::vector & tforms) { + +} + +void RTT_TF2::addTFOperations(RTT::Service::shared_ptr service) { + +} + +} // 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..d7059f8 --- /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 + +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..8309004 --- /dev/null +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp @@ -0,0 +1,51 @@ +// 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) { + return true; +} + +static bool loadGlobalROSService() { + 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 From fdc20c105885770acaf746e0e5fa0bc4b9078983 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Thu, 16 Jul 2020 18:10:48 +0200 Subject: [PATCH 02/21] rtt_ros2_tf2: implement basic functionality This patch provides the basic operations and ports from broadcasting TF2 with Orocos in ROS 2. --- rtt_ros2_tf2/CMakeLists.txt | 14 ++- .../include/rtt_ros2_tf2/rtt_ros2_tf2.hpp | 27 +++-- rtt_ros2_tf2/package.xml | 4 +- rtt_ros2_tf2/src/rtt_ros2_tf2.cpp | 101 +++++++++++++++--- 4 files changed, 118 insertions(+), 28 deletions(-) diff --git a/rtt_ros2_tf2/CMakeLists.txt b/rtt_ros2_tf2/CMakeLists.txt index 511237c..88e7928 100644 --- a/rtt_ros2_tf2/CMakeLists.txt +++ b/rtt_ros2_tf2/CMakeLists.txt @@ -29,20 +29,27 @@ find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) find_package(rtt_ros2_geometry_msgs REQUIRED) -# find_package(rtt_tf2_msgs REQUIRED) +find_package(rtt_ros2_interfaces REQUIRED) +# find_package(rtt_tf2_msgs REQUIRED) # Will be generated here # Include rtt_ros2 for the macro: rtt_ros2_export_plugin_depend() find_package(rtt_ros2 REQUIRED) # setup targets include_directories(include) +include_directories(include/orocos) # Target: library orocos_library(rtt_ros2_tf2 src/rtt_ros2_tf2.cpp EXPORT ${PROJECT_NAME} - INCLUDES DESTINATION include/${PROJECT_NAME} + INCLUDES DESTINATION include/orocos/${PROJECT_NAME} ) +target_include_directories(rtt_ros2_tf2 PRIVATE + $ + $ # /include/mylib +) + # target_link_libraries(rtt_ros2_tf2 tf2) ament_target_dependencies(rtt_ros2_tf2 rclcpp tf2 rtt_ros2_geometry_msgs @@ -57,13 +64,14 @@ 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) -ament_target_dependencies(rtt_ros2_tf2_component + ament_target_dependencies(rtt_ros2_tf2_component OROCOS-RTT ) # target_link_libraries(rtt_ros2_tf2_component ${catkin_LIBRARIES}) # Target: typekit (from tf2) # ros_generate_rtt_typekit(tf2) +rtt_ros2_generate_typekit_and_transports(tf2_msgs) # install install( diff --git a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp index 807809a..bc66a2b 100644 --- a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp +++ b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp @@ -23,7 +23,11 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" #include "tf2/buffer_core.h" -#include "geometry_msgs/typekit/msg/transform_stamped_Types.hpp" +// #include "geometry_msgs/typekit/msg/transform_stamped_Types.hpp" +#include "geometry_msgs/typekit/Types.hpp" +// #include "tf2_msgs/typekit/msg/tf_message_Types.hpp" +// #include "tf2_msgs/typekit/Types.hpp" +#include "tf2_msgs/msg/tf_message.hpp" namespace rtt_ros2_tf2 { @@ -64,24 +68,24 @@ class RTT_TF2 : public RTT::Service, protected tf2::BufferCore // const rclcpp::Time& common_time) const; void broadcastTransform( - const geometry_msgs::msg::TransformStamped & tform); + const geometry_msgs::msg::TransformStamped & transform); void broadcastTransforms( - const std::vector & tforms); + const std::vector & transforms); void broadcastStaticTransform( - const geometry_msgs::msg::TransformStamped & tform); + const geometry_msgs::msg::TransformStamped & transform); void broadcastStaticTransforms( - const std::vector & tforms); + const std::vector & transforms); - void addTFOperations(RTT::Service::shared_ptr service); + void addTf2Interface(RTT::Service::shared_ptr service); +protected: ///! Communication ports - // RTT::InputPort port_tf_in; - // RTT::InputPort port_tf_static_in; - // RTT::OutputPort port_tf_out; - // RTT::OutputPort port_tf_static_out; + // Input ports + RTT::InputPort ip_stamped_transform_; + RTT::InputPort ip_stamped_transform_static_; private: // Constant @@ -105,6 +109,9 @@ class RTT_TF2 : public RTT::Service, protected tf2::BufferCore // 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); }; // class RTT_TF2 diff --git a/rtt_ros2_tf2/package.xml b/rtt_ros2_tf2/package.xml index 2a6c458..3b627d0 100644 --- a/rtt_ros2_tf2/package.xml +++ b/rtt_ros2_tf2/package.xml @@ -13,12 +13,14 @@ ament_index_cpp rtt + rtt_ros2_interfaces + tf2_msgs rclcpp tf2 rtt_ros2 rtt_ros2_geometry_msgs - + ament_lint_auto ament_lint_common diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp index 23a4471..d8c18fe 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp @@ -17,21 +17,35 @@ #include #include "rclcpp/rclcpp.hpp" +#include #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 { RTT_TF2::RTT_TF2(RTT::TaskContext * owner) : RTT::Service("TF2", owner), - clock_(boost::make_shared(RCL_SYSTEM_TIME)) + ip_stamped_transform_("ip_stamped_transform"), + ip_stamped_transform_static_("ip_stamped_transform_static"), + clock_(boost::make_shared(rcl_clock_type_t::RCL_SYSTEM_TIME)) { RTT::Logger::In in(getName()); - RTT::log(RTT::Info) << - "TF2 service instantiated! in " << getName() << - RTT::endlog(); + if (owner != nullptr) { + // Add the interface to the own service + addTf2Interface(this->provides("TF2")); + RTT::log(RTT::Info) << + "TF2 service instantiated in: " << owner->getName() << + RTT::endlog(); + } else { + RTT::log(RTT::Info) << + "TF2 service instantiated standalone without interface" << + RTT::endlog(); + } } RTT_TF2::~RTT_TF2() = default; @@ -46,14 +60,19 @@ RTT_TF2::~RTT_TF2() = default; rclcpp::Time RTT_TF2::getLatestCommonTime( const std::string & target, const std::string & source) const { - return clock_->now(); } bool RTT_TF2::canTransform( const std::string & target, const std::string & source) const { - + std::string ret_error; + // const tf2::TimePoint time_point_now = tf2::TimePoint(clock_->now()); + // if (!tf2::BufferCore::canTransform(target, source, time_point_now, ret_error)) + // { + // RTT::log(RTT::Warning) << "Cannot transform from " << + // source << " to " << target << RTT::endlog(); + // } return false; } @@ -79,27 +98,81 @@ bool RTT_TF2::canTransformAtTime( // } void RTT_TF2::broadcastTransform( - const geometry_msgs::msg::TransformStamped & tform) { - + const geometry_msgs::msg::TransformStamped & transform) { + RTT::Logger::In in(getName()); + // tf2_msgs::msg::TFMessage converted_tf2_msg; + try { + setTransform(transform, "unknown_authority", false); + } catch (tf2::LookupException e) { + RTT::log(RTT::Error) << "Error when setting transform: " << + e.what() << RTT::endlog(); + } } void RTT_TF2::broadcastTransforms( - const std::vector & tforms) { - + const std::vector & transforms) { + for(const auto & transform : transforms) { + broadcastTransform(transform); + } } void RTT_TF2::broadcastStaticTransform( - const geometry_msgs::msg::TransformStamped & tform) { - + const geometry_msgs::msg::TransformStamped & transform) { + setTransform(transform, "unknown_authority", true); } void RTT_TF2::broadcastStaticTransforms( - const std::vector & tforms) { + const std::vector & transforms) { + for(const auto & transform : transforms) { + broadcastStaticTransform(transform); + } +} +void RTT_TF2::addTf2Interface(RTT::Service::shared_ptr service) { + // Add ports + service->addEventPort(ip_stamped_transform_, + boost::bind(&RTT_TF2::stamped_message_callback, this, _1)) + .doc("Reception of stamped transform to broadcast through TF2"); + service->addEventPort(ip_stamped_transform_static_, + boost::bind(&RTT_TF2::stamped_message_static_callback, this, _1)) + .doc("Reception of stamped transform to broadcast through TF2 static"); + // Add operations + service->addOperation("broadcastTransform", + &RTT_TF2::broadcastTransform, this, RTT::OwnThread) + .doc("Broadcasts a stamped transform as TF2"); + service->addOperation("broadcastTransforms", + &RTT_TF2::broadcastTransforms, this, RTT::OwnThread) + .doc("Broadcasts a vector of stamped transforms as TF2"); + service->addOperation("broadcastStaticTransform", + &RTT_TF2::broadcastStaticTransform, this, RTT::OwnThread) + .doc("Broadcasts a stamped transform as TF2 static"); + service->addOperation("broadcastStaticTransforms", + &RTT_TF2::broadcastStaticTransforms, this, RTT::OwnThread) + .doc("Broadcasts a vector of stamped transforms as TF2 static"); } -void RTT_TF2::addTFOperations(RTT::Service::shared_ptr service) { +void RTT_TF2::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 RTT_TF2::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); + } } } // namespace rtt_ros2_tf2 From 10a39ce2b823e4c149eec1c839ff6d58606bc3de Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Fri, 17 Jul 2020 12:50:16 +0200 Subject: [PATCH 03/21] rtt_ros2_tf2: add more functionality This patch adds lookup functionality for TF2. --- rtt_ros2_tf2/CMakeLists.txt | 38 ++++++---- .../include/rtt_ros2_tf2/rtt_ros2_tf2.hpp | 24 +++---- rtt_ros2_tf2/src/rtt_ros2_tf2.cpp | 69 ++++++++++++------- 3 files changed, 79 insertions(+), 52 deletions(-) diff --git a/rtt_ros2_tf2/CMakeLists.txt b/rtt_ros2_tf2/CMakeLists.txt index 88e7928..2b587f4 100644 --- a/rtt_ros2_tf2/CMakeLists.txt +++ b/rtt_ros2_tf2/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) +find_package(tf2_msgs REQUIRED) find_package(rtt_ros2_geometry_msgs REQUIRED) find_package(rtt_ros2_interfaces REQUIRED) # find_package(rtt_tf2_msgs REQUIRED) # Will be generated here @@ -40,25 +41,36 @@ include_directories(include) 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 rtt_ros2_geometry_msgs includes are in: ${rtt_ros2_geometry_msgs_INCLUDE_DIRS}") +# message(AUTHOR_WARNING "The rtt_ros2_interfaces includes are in: ${rtt_ros2_interfaces_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_include_directories(rtt_ros2_tf2 PRIVATE - $ - $ # /include/mylib -) - # target_link_libraries(rtt_ros2_tf2 tf2) +# target_include_directories(rtt_ros2_tf2 tf2) ament_target_dependencies(rtt_ros2_tf2 - rclcpp tf2 rtt_ros2_geometry_msgs + rclcpp tf2 tf2_msgs rtt_ros2_geometry_msgs +) + +# Extra include_directories to find the own generated typekits from inside +target_include_directories(rtt_ros2_tf2 PRIVATE + $ + $ +) +target_link_libraries(rtt_ros2_tf2 + rtt_tf2_msgs_typekit # typekit generated in this package (target name) ) # 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 @@ -68,21 +80,21 @@ orocos_component(rtt_ros2_tf2_component OROCOS-RTT ) # target_link_libraries(rtt_ros2_tf2_component ${catkin_LIBRARIES}) +# Now made with ament_target_dependencies() -# Target: typekit (from tf2) -# ros_generate_rtt_typekit(tf2) +# Target: typekit (from tf2_msgs) +# ros_generate_rtt_typekit(tf2_msgs) rtt_ros2_generate_typekit_and_transports(tf2_msgs) # install +# orocos_install_headers(include/rtt_ros2_tf2/rtt_ros2_tf2.hpp) +# orocos_install_headers(include/rtt_ros2_tf2/tf2_interface.h) +# orocos_install_headers(include/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp) install( DIRECTORY include/ DESTINATION include ) -# orocos_install_headers(include/rtt_ros2_tf2/rtt_ros2_tf2.hpp) -# orocos_install_headers(include/rtt_ros2_tf2/tf2_interface.h) -# orocos_install_headers(include/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp) - # linters if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -102,7 +114,7 @@ endif() rtt_ros2_export_plugin_depend(rclcpp) rtt_ros2_export_plugin_depend(tf2) rtt_ros2_export_plugin_depend(rtt_ros2_tf2) -# rtt_ros2_export_plugin_depend(rtt_ros2_tf2_msgs) # If typekit is generated +rtt_ros2_export_plugin_depend(rtt_tf2_msgs_typekit) # If typekit is generated and used # orocos_generate_package( # INCLUDE_DIRS include diff --git a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp index bc66a2b..ea6cd1d 100644 --- a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp +++ b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp @@ -26,7 +26,7 @@ // #include "geometry_msgs/typekit/msg/transform_stamped_Types.hpp" #include "geometry_msgs/typekit/Types.hpp" // #include "tf2_msgs/typekit/msg/tf_message_Types.hpp" -// #include "tf2_msgs/typekit/Types.hpp" +#include "tf2_msgs/typekit/Types.hpp" #include "tf2_msgs/msg/tf_message.hpp" namespace rtt_ros2_tf2 @@ -43,26 +43,21 @@ class RTT_TF2 : public RTT::Service, protected tf2::BufferCore virtual ~RTT_TF2(); protected: + using tf2::BufferCore::canTransform; ///! Operations - - rclcpp::Time getLatestCommonTime( - const std::string & target, - const std::string & source) const; + // rclcpp::Time getLatestCommonTime( + // const std::string & target, + // const std::string & source) const; bool canTransform( const std::string & target, const std::string & source) const; - bool canTransformAtTime( + geometry_msgs::msg::TransformStamped lookupTransform( const std::string & target, - const std::string & source, - const rclcpp::Time & common_time) const; - - // geometry_msgs::TransformStamped lookupTransform( - // const std::string & target, - // const std::string & source) const; + const std::string & source) const; - // geometry_msgs::TransformStamped lookupTransformAtTime( + // geometry_msgs::msg::TransformStamped lookupTransformAtTime( // const std::string & target, // const std::string & source, // const rclcpp::Time& common_time) const; @@ -79,6 +74,8 @@ class RTT_TF2 : public RTT::Service, protected tf2::BufferCore void broadcastStaticTransforms( const std::vector & transforms); + void clear(); + void addTf2Interface(RTT::Service::shared_ptr service); protected: @@ -86,6 +83,7 @@ class RTT_TF2 : public RTT::Service, protected tf2::BufferCore // Input ports RTT::InputPort ip_stamped_transform_; RTT::InputPort ip_stamped_transform_static_; + RTT::InputPort ip_tf_port_; private: // Constant diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp index d8c18fe..cd07407 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp @@ -31,6 +31,7 @@ RTT_TF2::RTT_TF2(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"), clock_(boost::make_shared(rcl_clock_type_t::RCL_SYSTEM_TIME)) { RTT::Logger::In in(getName()); @@ -57,10 +58,14 @@ RTT_TF2::~RTT_TF2() = default; // } -rclcpp::Time RTT_TF2::getLatestCommonTime( - const std::string & target, - const std::string & source) const { - return clock_->now(); +// rclcpp::Time RTT_TF2::getLatestCommonTime( +// const std::string & target, +// const std::string & source) const { +// return clock_->now(); +// } + +void RTT_TF2::clear() { + tf2::BufferCore::clear(); } bool RTT_TF2::canTransform( @@ -68,29 +73,36 @@ bool RTT_TF2::canTransform( const std::string & source) const { std::string ret_error; // const tf2::TimePoint time_point_now = tf2::TimePoint(clock_->now()); - // if (!tf2::BufferCore::canTransform(target, source, time_point_now, ret_error)) - // { - // RTT::log(RTT::Warning) << "Cannot transform from " << - // source << " to " << target << RTT::endlog(); - // } - return false; + try { + if (!tf2::BufferCore::canTransform(target, + source, tf2::TimePoint(), + &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; } -bool RTT_TF2::canTransformAtTime( - const std::string& target, - const std::string& source, - const rclcpp::Time& common_time) const { - - return false; +geometry_msgs::msg::TransformStamped RTT_TF2::lookupTransform( + const std::string & target, + const std::string & source) const { + try { + return tf2::BufferCore::lookupTransform(target, source, tf2::TimePoint()); + } catch (std::exception e) { + RTT::log(RTT::Error) << "lookupTransform() produced an exception: " << + e.what() << RTT::endlog(); + return geometry_msgs::msg::TransformStamped(); + } } -// geometry_msgs::TransformStamped RTT_TF2::lookupTransform( -// const std::string & target, -// const std::string & source) const { - -// } - -// geometry_msgs::TransformStamped RTT_TF2::lookupTransformAtTime( +// geometry_msgs::msg::TransformStamped RTT_TF2::lookupTransformAtTime( // const std::string & target, // const std::string & source, // const rclcpp::Time & common_time) const { @@ -118,7 +130,12 @@ void RTT_TF2::broadcastTransforms( void RTT_TF2::broadcastStaticTransform( const geometry_msgs::msg::TransformStamped & transform) { - setTransform(transform, "unknown_authority", true); + try { + setTransform(transform, "unknown_authority", true); + } catch (tf2::LookupException e) { + RTT::log(RTT::Error) << "Error when setting static transform: " << + e.what() << RTT::endlog(); + } } void RTT_TF2::broadcastStaticTransforms( @@ -151,7 +168,7 @@ void RTT_TF2::addTf2Interface(RTT::Service::shared_ptr service) { .doc("Broadcasts a vector of stamped transforms as TF2 static"); } -void RTT_TF2::stamped_message_callback(RTT::base::PortInterface * in_port) { +void RTT_TF2::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; @@ -163,7 +180,7 @@ void RTT_TF2::stamped_message_callback(RTT::base::PortInterface * in_port) { } } -void RTT_TF2::stamped_message_static_callback(RTT::base::PortInterface * in_port) { +void RTT_TF2::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; From 43d26e567ced527fcb61cb04f3f239a219ffe912 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Fri, 17 Jul 2020 15:06:02 +0200 Subject: [PATCH 04/21] rtt_ros2_tf2: refactor to use composition instead of inheritance The service is no longer also a tf2::BufferCore, but it uses a member object buffer_core_ --- rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp | 5 ++--- rtt_ros2_tf2/src/rtt_ros2_tf2.cpp | 11 ++++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp index ea6cd1d..f220d06 100644 --- a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp +++ b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp @@ -32,10 +32,9 @@ namespace rtt_ros2_tf2 { -class RTT_TF2 : public RTT::Service, protected tf2::BufferCore +class RTT_TF2 : public RTT::Service { - public: typedef boost::shared_ptr shared_ptr; @@ -43,11 +42,11 @@ class RTT_TF2 : public RTT::Service, protected tf2::BufferCore virtual ~RTT_TF2(); protected: - using tf2::BufferCore::canTransform; ///! Operations // rclcpp::Time getLatestCommonTime( // const std::string & target, // const std::string & source) const; + boost::shared_ptr buffer_core_; bool canTransform( const std::string & target, diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp index cd07407..10cb1fe 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp @@ -29,6 +29,7 @@ namespace rtt_ros2_tf2 RTT_TF2::RTT_TF2(RTT::TaskContext * owner) : RTT::Service("TF2", owner), + buffer_core_(boost::make_shared()), ip_stamped_transform_("ip_stamped_transform"), ip_stamped_transform_static_("ip_stamped_transform_static"), ip_tf_port_("pi_tf_port"), @@ -65,7 +66,7 @@ RTT_TF2::~RTT_TF2() = default; // } void RTT_TF2::clear() { - tf2::BufferCore::clear(); + buffer_core_->clear(); } bool RTT_TF2::canTransform( @@ -74,7 +75,7 @@ bool RTT_TF2::canTransform( std::string ret_error; // const tf2::TimePoint time_point_now = tf2::TimePoint(clock_->now()); try { - if (!tf2::BufferCore::canTransform(target, + if (!buffer_core_->canTransform(target, source, tf2::TimePoint(), &ret_error)) { @@ -94,7 +95,7 @@ geometry_msgs::msg::TransformStamped RTT_TF2::lookupTransform( const std::string & target, const std::string & source) const { try { - return tf2::BufferCore::lookupTransform(target, source, tf2::TimePoint()); + return buffer_core_->lookupTransform(target, source, tf2::TimePoint()); } catch (std::exception e) { RTT::log(RTT::Error) << "lookupTransform() produced an exception: " << e.what() << RTT::endlog(); @@ -114,7 +115,7 @@ void RTT_TF2::broadcastTransform( RTT::Logger::In in(getName()); // tf2_msgs::msg::TFMessage converted_tf2_msg; try { - setTransform(transform, "unknown_authority", false); + buffer_core_->setTransform(transform, "unknown_authority", false); } catch (tf2::LookupException e) { RTT::log(RTT::Error) << "Error when setting transform: " << e.what() << RTT::endlog(); @@ -131,7 +132,7 @@ void RTT_TF2::broadcastTransforms( void RTT_TF2::broadcastStaticTransform( const geometry_msgs::msg::TransformStamped & transform) { try { - setTransform(transform, "unknown_authority", true); + buffer_core_->setTransform(transform, "unknown_authority", true); } catch (tf2::LookupException e) { RTT::log(RTT::Error) << "Error when setting static transform: " << e.what() << RTT::endlog(); From 0febc6b6712594b627d630cb6ee6f43b85b92b47 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Fri, 17 Jul 2020 17:29:06 +0200 Subject: [PATCH 05/21] rtt_ros2_tf2: add the service as an Orocos plugin --- .../include/rtt_ros2_tf2/rtt_ros2_tf2.hpp | 6 ++- rtt_ros2_tf2/src/rtt_ros2_tf2.cpp | 46 +++++++++++-------- rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp | 29 +++++++++++- 3 files changed, 61 insertions(+), 20 deletions(-) diff --git a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp index f220d06..a6c32b9 100644 --- a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp +++ b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp @@ -46,7 +46,6 @@ class RTT_TF2 : public RTT::Service // rclcpp::Time getLatestCommonTime( // const std::string & target, // const std::string & source) const; - boost::shared_ptr buffer_core_; bool canTransform( const std::string & target, @@ -90,6 +89,11 @@ class RTT_TF2 : public RTT::Service // Clock boost::shared_ptr clock_; + boost::shared_ptr buffer_core_; + boost::shared_ptr buffer_client_; + boost::shared_ptr transform_broadcaster_; + boost::shared_ptr + static_transform_broadcaster_; // Example members diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp index 10cb1fe..6795db3 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp @@ -28,22 +28,44 @@ namespace rtt_ros2_tf2 { RTT_TF2::RTT_TF2(RTT::TaskContext * owner) -: RTT::Service("TF2", owner), +: RTT::Service("tf2", owner), buffer_core_(boost::make_shared()), + buffer_client_(nullptr), + transform_broadcaster_(nullptr), ip_stamped_transform_("ip_stamped_transform"), ip_stamped_transform_static_("ip_stamped_transform_static"), ip_tf_port_("pi_tf_port"), clock_(boost::make_shared(rcl_clock_type_t::RCL_SYSTEM_TIME)) { RTT::Logger::In in(getName()); - + // Add operations + this->addOperation("broadcastTransform", + &RTT_TF2::broadcastTransform, this, RTT::OwnThread) + .doc("Broadcasts a stamped transform as TF2"); + this->addOperation("broadcastTransforms", + &RTT_TF2::broadcastTransforms, this, RTT::OwnThread) + .doc("Broadcasts a vector of stamped transforms as TF2"); + this->addOperation("broadcastStaticTransform", + &RTT_TF2::broadcastStaticTransform, this, RTT::OwnThread) + .doc("Broadcasts a stamped transform as TF2 static"); + this->addOperation("broadcastStaticTransforms", + &RTT_TF2::broadcastStaticTransforms, this, RTT::OwnThread) + .doc("Broadcasts a vector of stamped transforms as TF2 static"); + this->addOperation("lookupTransform", + &RTT_TF2::lookupTransform, this, RTT::OwnThread) + .doc("Looks up for a TF2 transform"); + this->addOperation("clear", + &RTT_TF2::clear, this, RTT::OwnThread) + .doc("Clears TF2 transforms"); + // Ports will only be added if a TaskContext is provided (not in GlobalService) if (owner != nullptr) { // Add the interface to the own service - addTf2Interface(this->provides("TF2")); + addTf2Interface(this->provides()); RTT::log(RTT::Info) << "TF2 service instantiated in: " << owner->getName() << RTT::endlog(); } else { + // addTf2Interface(RTT::internal::GlobalService::Instance()->provides("tf2")); RTT::log(RTT::Info) << "TF2 service instantiated standalone without interface" << RTT::endlog(); @@ -76,7 +98,7 @@ bool RTT_TF2::canTransform( // const tf2::TimePoint time_point_now = tf2::TimePoint(clock_->now()); try { if (!buffer_core_->canTransform(target, - source, tf2::TimePoint(), + source, tf2::TimePointZero, &ret_error)) { RTT::log(RTT::Warning) << "Cannot transform from " << @@ -95,7 +117,7 @@ geometry_msgs::msg::TransformStamped RTT_TF2::lookupTransform( const std::string & target, const std::string & source) const { try { - return buffer_core_->lookupTransform(target, source, tf2::TimePoint()); + return buffer_core_->lookupTransform(target, source, tf2::TimePointZero); } catch (std::exception e) { RTT::log(RTT::Error) << "lookupTransform() produced an exception: " << e.what() << RTT::endlog(); @@ -147,6 +169,7 @@ void RTT_TF2::broadcastStaticTransforms( } void RTT_TF2::addTf2Interface(RTT::Service::shared_ptr service) { + // Ports cannot be added to the GlobalService // Add ports service->addEventPort(ip_stamped_transform_, boost::bind(&RTT_TF2::stamped_message_callback, this, _1)) @@ -154,19 +177,6 @@ void RTT_TF2::addTf2Interface(RTT::Service::shared_ptr service) { service->addEventPort(ip_stamped_transform_static_, boost::bind(&RTT_TF2::stamped_message_static_callback, this, _1)) .doc("Reception of stamped transform to broadcast through TF2 static"); - // Add operations - service->addOperation("broadcastTransform", - &RTT_TF2::broadcastTransform, this, RTT::OwnThread) - .doc("Broadcasts a stamped transform as TF2"); - service->addOperation("broadcastTransforms", - &RTT_TF2::broadcastTransforms, this, RTT::OwnThread) - .doc("Broadcasts a vector of stamped transforms as TF2"); - service->addOperation("broadcastStaticTransform", - &RTT_TF2::broadcastStaticTransform, this, RTT::OwnThread) - .doc("Broadcasts a stamped transform as TF2 static"); - service->addOperation("broadcastStaticTransforms", - &RTT_TF2::broadcastStaticTransforms, this, RTT::OwnThread) - .doc("Broadcasts a vector of stamped transforms as TF2 static"); } void RTT_TF2::stamped_message_callback(RTT::base::PortInterface * /*in_port*/) { diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp index 8309004..4e9c5af 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp @@ -15,7 +15,7 @@ #include #include -#include "rtt/deployment/ComponentLoader.hpp" +// #include "rtt/deployment/ComponentLoader.hpp" #include "rtt/internal/GlobalService.hpp" #include "rtt/plugin/ServicePlugin.hpp" @@ -28,10 +28,37 @@ 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::RTT_TF2::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; } From 7cb3c9ac6da694797b540fe4441bb87962c4b132 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Mon, 20 Jul 2020 12:11:33 +0200 Subject: [PATCH 06/21] rtt_ros2_tf2: use tf2_ros::TransformBroadcaster and other tf2_ros Change the TF classes to use tf2_ros, to allow the actual broadcasting and listening of transforms trhough ROS 2 interfaces. --- rtt_ros2_tf2/CMakeLists.txt | 4 +- .../include/rtt_ros2_tf2/rtt_ros2_tf2.hpp | 9 +- rtt_ros2_tf2/package.xml | 2 + rtt_ros2_tf2/src/rtt_ros2_tf2.cpp | 170 ++++++++++++++---- 4 files changed, 151 insertions(+), 34 deletions(-) diff --git a/rtt_ros2_tf2/CMakeLists.txt b/rtt_ros2_tf2/CMakeLists.txt index 2b587f4..f2ac6f2 100644 --- a/rtt_ros2_tf2/CMakeLists.txt +++ b/rtt_ros2_tf2/CMakeLists.txt @@ -28,7 +28,9 @@ find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) find_package(tf2_msgs REQUIRED) +find_package(rtt_ros2_node REQUIRED) find_package(rtt_ros2_geometry_msgs REQUIRED) find_package(rtt_ros2_interfaces REQUIRED) # find_package(rtt_tf2_msgs REQUIRED) # Will be generated here @@ -54,7 +56,7 @@ orocos_library(rtt_ros2_tf2 # target_link_libraries(rtt_ros2_tf2 tf2) # target_include_directories(rtt_ros2_tf2 tf2) ament_target_dependencies(rtt_ros2_tf2 - rclcpp tf2 tf2_msgs rtt_ros2_geometry_msgs + rclcpp tf2 tf2_ros tf2_msgs rtt_ros2_geometry_msgs ) # Extra include_directories to find the own generated typekits from inside diff --git a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp index a6c32b9..fbc1fd7 100644 --- a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp +++ b/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp @@ -23,6 +23,9 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" #include "tf2/buffer_core.h" +#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" // #include "geometry_msgs/typekit/msg/transform_stamped_Types.hpp" #include "geometry_msgs/typekit/Types.hpp" // #include "tf2_msgs/typekit/msg/tf_message_Types.hpp" @@ -88,10 +91,12 @@ class RTT_TF2 : public RTT::Service 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 buffer_client_; boost::shared_ptr transform_broadcaster_; + boost::shared_ptr transform_listener_; boost::shared_ptr static_transform_broadcaster_; @@ -114,6 +119,8 @@ class RTT_TF2 : public RTT::Service void stamped_message_callback(RTT::base::PortInterface * port); void stamped_message_static_callback(RTT::base::PortInterface * port); + bool rosReady(); + }; // class RTT_TF2 } // namespace rtt_ros2_tf2 diff --git a/rtt_ros2_tf2/package.xml b/rtt_ros2_tf2/package.xml index 3b627d0..90457b2 100644 --- a/rtt_ros2_tf2/package.xml +++ b/rtt_ros2_tf2/package.xml @@ -18,7 +18,9 @@ rclcpp tf2 + tf2_ros rtt_ros2 + rtt_ros2_node rtt_ros2_geometry_msgs diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp index 6795db3..5729876 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp @@ -18,6 +18,7 @@ #include "rclcpp/rclcpp.hpp" #include +#include #include "rtt/internal/GlobalService.hpp" @@ -29,43 +30,67 @@ namespace rtt_ros2_tf2 RTT_TF2::RTT_TF2(RTT::TaskContext * owner) : RTT::Service("tf2", owner), - buffer_core_(boost::make_shared()), - buffer_client_(nullptr), - transform_broadcaster_(nullptr), ip_stamped_transform_("ip_stamped_transform"), ip_stamped_transform_static_("ip_stamped_transform_static"), ip_tf_port_("pi_tf_port"), - clock_(boost::make_shared(rcl_clock_type_t::RCL_SYSTEM_TIME)) + 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()); - // Add operations - this->addOperation("broadcastTransform", - &RTT_TF2::broadcastTransform, this, RTT::OwnThread) - .doc("Broadcasts a stamped transform as TF2"); - this->addOperation("broadcastTransforms", - &RTT_TF2::broadcastTransforms, this, RTT::OwnThread) - .doc("Broadcasts a vector of stamped transforms as TF2"); - this->addOperation("broadcastStaticTransform", - &RTT_TF2::broadcastStaticTransform, this, RTT::OwnThread) - .doc("Broadcasts a stamped transform as TF2 static"); - this->addOperation("broadcastStaticTransforms", - &RTT_TF2::broadcastStaticTransforms, this, RTT::OwnThread) - .doc("Broadcasts a vector of stamped transforms as TF2 static"); - this->addOperation("lookupTransform", - &RTT_TF2::lookupTransform, this, RTT::OwnThread) - .doc("Looks up for a TF2 transform"); - this->addOperation("clear", - &RTT_TF2::clear, this, RTT::OwnThread) - .doc("Clears TF2 transforms"); - // Ports will only be added if a TaskContext is provided (not in GlobalService) + 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", + &RTT_TF2::broadcastTransform, this, RTT::OwnThread) + .doc("Broadcasts a stamped transform as TF2"); + this->addOperation("broadcastTransforms", + &RTT_TF2::broadcastTransforms, this, RTT::OwnThread) + .doc("Broadcasts a vector of stamped transforms as TF2"); + this->addOperation("broadcastStaticTransform", + &RTT_TF2::broadcastStaticTransform, this, RTT::OwnThread) + .doc("Broadcasts a stamped transform as TF2 static"); + this->addOperation("broadcastStaticTransforms", + &RTT_TF2::broadcastStaticTransforms, this, RTT::OwnThread) + .doc("Broadcasts a vector of stamped transforms as TF2 static"); + this->addOperation("lookupTransform", + &RTT_TF2::lookupTransform, this, RTT::OwnThread) + .doc("Looks up for a TF2 transform"); + this->addOperation("clear", + &RTT_TF2::clear, this, RTT::OwnThread) + .doc("Clears TF2 transforms"); } else { - // addTf2Interface(RTT::internal::GlobalService::Instance()->provides("tf2")); + // addTf2Interface( + // RTT::internal::GlobalService::Instance()->provides("tf2")); + // Add operations + this->addOperation("broadcastTransform", + &RTT_TF2::broadcastTransform, this, RTT::ClientThread) + .doc("Broadcasts a stamped transform as TF2"); + this->addOperation("broadcastTransforms", + &RTT_TF2::broadcastTransforms, this, RTT::ClientThread) + .doc("Broadcasts a vector of stamped transforms as TF2"); + this->addOperation("broadcastStaticTransform", + &RTT_TF2::broadcastStaticTransform, this, RTT::ClientThread) + .doc("Broadcasts a stamped transform as TF2 static"); + this->addOperation("broadcastStaticTransforms", + &RTT_TF2::broadcastStaticTransforms, this, RTT::ClientThread) + .doc("Broadcasts a vector of stamped transforms as TF2 static"); + this->addOperation("lookupTransform", + &RTT_TF2::lookupTransform, this, RTT::ClientThread) + .doc("Looks up for a TF2 transform"); + this->addOperation("clear", + &RTT_TF2::clear, this, RTT::ClientThread) + .doc("Clears TF2 transforms"); RTT::log(RTT::Info) << "TF2 service instantiated standalone without interface" << RTT::endlog(); @@ -135,9 +160,13 @@ geometry_msgs::msg::TransformStamped RTT_TF2::lookupTransform( void RTT_TF2::broadcastTransform( const geometry_msgs::msg::TransformStamped & transform) { RTT::Logger::In in(getName()); + if (!rosReady()) { + return; + } // tf2_msgs::msg::TFMessage converted_tf2_msg; try { - buffer_core_->setTransform(transform, "unknown_authority", false); + // buffer_core_->setTransform(transform, "unknown_authority", false); + transform_broadcaster_->sendTransform(transform); } catch (tf2::LookupException e) { RTT::log(RTT::Error) << "Error when setting transform: " << e.what() << RTT::endlog(); @@ -146,15 +175,29 @@ void RTT_TF2::broadcastTransform( void RTT_TF2::broadcastTransforms( const std::vector & transforms) { - for(const auto & transform : transforms) { - broadcastTransform(transform); + if (!rosReady()) { + return; + } + // for(const auto & transform : transforms) { + // broadcastTransform(transform); + // } + try { + // 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 RTT_TF2::broadcastStaticTransform( const geometry_msgs::msg::TransformStamped & transform) { + if (!rosReady()) { + return; + } try { - buffer_core_->setTransform(transform, "unknown_authority", true); + // buffer_core_->setTransform(transform, "unknown_authority", true); + static_transform_broadcaster_->sendTransform(transform); } catch (tf2::LookupException e) { RTT::log(RTT::Error) << "Error when setting static transform: " << e.what() << RTT::endlog(); @@ -163,8 +206,18 @@ void RTT_TF2::broadcastStaticTransform( void RTT_TF2::broadcastStaticTransforms( const std::vector & transforms) { - for(const auto & transform : transforms) { - broadcastStaticTransform(transform); + if (!rosReady()) { + return; + } + // for(const auto & transform : transforms) { + // broadcastStaticTransform(transform); + // } + try { + // buffer_core_->setTransform(transform, "unknown_authority", false); + static_transform_broadcaster_->sendTransform(transforms); + } catch (std::exception e) { + RTT::log(RTT::Error) << "Error when setting transform: " << + e.what() << RTT::endlog(); } } @@ -191,7 +244,8 @@ void RTT_TF2::stamped_message_callback(RTT::base::PortInterface * /*in_port*/) { } } -void RTT_TF2::stamped_message_static_callback(RTT::base::PortInterface * /*in_port*/) { +void RTT_TF2::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; @@ -203,4 +257,56 @@ void RTT_TF2::stamped_message_static_callback(RTT::base::PortInterface * /*in_po } } +bool RTT_TF2::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 + 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 + 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 From 604a24e828d081f08e81840bab58edc5cbe2c308 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Mon, 20 Jul 2020 13:49:14 +0200 Subject: [PATCH 07/21] rtt_ros2_tf2 comply with linting and other sanity checks --- .../rtt_ros2_tf2/rtt_ros2_tf2.hpp | 25 ++- .../rtt_ros2_tf2/rtt_ros2_tf2_component.hpp | 34 +-- rtt_ros2_tf2/src/rtt_ros2_tf2.cpp | 199 ++++++++++-------- rtt_ros2_tf2/src/rtt_ros2_tf2_component.cpp | 16 +- rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp | 10 +- 5 files changed, 158 insertions(+), 126 deletions(-) rename rtt_ros2_tf2/include/{ => orocos}/rtt_ros2_tf2/rtt_ros2_tf2.hpp (85%) rename rtt_ros2_tf2/include/{ => orocos}/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp (54%) diff --git a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp b/rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2.hpp similarity index 85% rename from rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp rename to rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2.hpp index fbc1fd7..aac3391 100644 --- a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2.hpp +++ b/rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rtt/Service.hpp" @@ -37,7 +38,6 @@ namespace rtt_ros2_tf2 class RTT_TF2 : public RTT::Service { - public: typedef boost::shared_ptr shared_ptr; @@ -51,12 +51,12 @@ class RTT_TF2 : public RTT::Service // const std::string & source) const; bool canTransform( - const std::string & target, - const std::string & source) const; + const std::string & target, + const std::string & source) const; geometry_msgs::msg::TransformStamped lookupTransform( - const std::string & target, - const std::string & source) const; + const std::string & target, + const std::string & source) const; // geometry_msgs::msg::TransformStamped lookupTransformAtTime( // const std::string & target, @@ -64,16 +64,16 @@ class RTT_TF2 : public RTT::Service // const rclcpp::Time& common_time) const; void broadcastTransform( - const geometry_msgs::msg::TransformStamped & transform); + const geometry_msgs::msg::TransformStamped & transform); void broadcastTransforms( - const std::vector & transforms); + const std::vector & transforms); void broadcastStaticTransform( - const geometry_msgs::msg::TransformStamped & transform); + const geometry_msgs::msg::TransformStamped & transform); void broadcastStaticTransforms( - const std::vector & transforms); + const std::vector & transforms); void clear(); @@ -98,7 +98,7 @@ class RTT_TF2 : public RTT::Service boost::shared_ptr transform_broadcaster_; boost::shared_ptr transform_listener_; boost::shared_ptr - static_transform_broadcaster_; + static_transform_broadcaster_; // Example members @@ -120,9 +120,8 @@ class RTT_TF2 : public RTT::Service void stamped_message_static_callback(RTT::base::PortInterface * port); bool rosReady(); - }; // class RTT_TF2 -} // namespace rtt_ros2_tf2 +} // namespace rtt_ros2_tf2 -#endif // OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_HPP_ +#endif // OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_HPP_ diff --git a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp b/rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp similarity index 54% rename from rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp rename to rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp index 4355631..c47b9a4 100644 --- a/rtt_ros2_tf2/include/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp +++ b/rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp @@ -12,28 +12,30 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OROCOS_RTT_ROS2_TF2_COMPONENT_HPP -#define OROCOS_RTT_ROS2_TF2_COMPONENT_HPP +#ifndef OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_COMPONENT_HPP_ +#define OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_COMPONENT_HPP_ -#include -// #include -// #include +#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: - TF2_Component(std::string const& name); +// Inherit from TaskContext +class TF2_Component : public RTT::TaskContext +{ +public: + explicit TF2_Component(std::string const & name); - bool configureHook(); + bool configureHook(); - void updateHook(); + void updateHook(); - void cleanupHook(); - }; + void cleanupHook(); +}; -}//namespace rtt_ros2_tf2 +} // namespace rtt_ros2_tf2 -#endif //OROCOS_RTT_ROS2_TF2_COMPONENT_HPP +#endif // OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_COMPONENT_HPP_ diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp index 5729876..091fb4c 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp @@ -15,10 +15,11 @@ #include "rtt_ros2_tf2/rtt_ros2_tf2.hpp" #include +#include #include "rclcpp/rclcpp.hpp" -#include -#include +#include "rtt/TaskContext.hpp" +#include "rtt_ros2_node/rtt_ros2_node.hpp" #include "rtt/internal/GlobalService.hpp" @@ -50,47 +51,59 @@ RTT_TF2::RTT_TF2(RTT::TaskContext * owner) RTT::log(RTT::Info) << "TF2 service instantiated in: " << owner->getName() << RTT::endlog(); - // Add operations - this->addOperation("broadcastTransform", + // Add operations + this->addOperation( + "broadcastTransform", &RTT_TF2::broadcastTransform, this, RTT::OwnThread) - .doc("Broadcasts a stamped transform as TF2"); - this->addOperation("broadcastTransforms", + .doc("Broadcasts a stamped transform as TF2"); + this->addOperation( + "broadcastTransforms", &RTT_TF2::broadcastTransforms, this, RTT::OwnThread) - .doc("Broadcasts a vector of stamped transforms as TF2"); - this->addOperation("broadcastStaticTransform", + .doc("Broadcasts a vector of stamped transforms as TF2"); + this->addOperation( + "broadcastStaticTransform", &RTT_TF2::broadcastStaticTransform, this, RTT::OwnThread) - .doc("Broadcasts a stamped transform as TF2 static"); - this->addOperation("broadcastStaticTransforms", + .doc("Broadcasts a stamped transform as TF2 static"); + this->addOperation( + "broadcastStaticTransforms", &RTT_TF2::broadcastStaticTransforms, this, RTT::OwnThread) - .doc("Broadcasts a vector of stamped transforms as TF2 static"); - this->addOperation("lookupTransform", + .doc("Broadcasts a vector of stamped transforms as TF2 static"); + this->addOperation( + "lookupTransform", &RTT_TF2::lookupTransform, this, RTT::OwnThread) - .doc("Looks up for a TF2 transform"); - this->addOperation("clear", + .doc("Looks up for a TF2 transform"); + this->addOperation( + "clear", &RTT_TF2::clear, this, RTT::OwnThread) - .doc("Clears TF2 transforms"); + .doc("Clears TF2 transforms"); } else { // addTf2Interface( // RTT::internal::GlobalService::Instance()->provides("tf2")); - // Add operations - this->addOperation("broadcastTransform", + // Add operations + this->addOperation( + "broadcastTransform", &RTT_TF2::broadcastTransform, this, RTT::ClientThread) - .doc("Broadcasts a stamped transform as TF2"); - this->addOperation("broadcastTransforms", + .doc("Broadcasts a stamped transform as TF2"); + this->addOperation( + "broadcastTransforms", &RTT_TF2::broadcastTransforms, this, RTT::ClientThread) - .doc("Broadcasts a vector of stamped transforms as TF2"); - this->addOperation("broadcastStaticTransform", + .doc("Broadcasts a vector of stamped transforms as TF2"); + this->addOperation( + "broadcastStaticTransform", &RTT_TF2::broadcastStaticTransform, this, RTT::ClientThread) - .doc("Broadcasts a stamped transform as TF2 static"); - this->addOperation("broadcastStaticTransforms", + .doc("Broadcasts a stamped transform as TF2 static"); + this->addOperation( + "broadcastStaticTransforms", &RTT_TF2::broadcastStaticTransforms, this, RTT::ClientThread) - .doc("Broadcasts a vector of stamped transforms as TF2 static"); - this->addOperation("lookupTransform", + .doc("Broadcasts a vector of stamped transforms as TF2 static"); + this->addOperation( + "lookupTransform", &RTT_TF2::lookupTransform, this, RTT::ClientThread) - .doc("Looks up for a TF2 transform"); - this->addOperation("clear", + .doc("Looks up for a TF2 transform"); + this->addOperation( + "clear", &RTT_TF2::clear, this, RTT::ClientThread) - .doc("Clears TF2 transforms"); + .doc("Clears TF2 transforms"); RTT::log(RTT::Info) << "TF2 service instantiated standalone without interface" << RTT::endlog(); @@ -112,17 +125,20 @@ RTT_TF2::~RTT_TF2() = default; // return clock_->now(); // } -void RTT_TF2::clear() { +void RTT_TF2::clear() +{ buffer_core_->clear(); } bool RTT_TF2::canTransform( - const std::string & target, - const std::string & source) const { + 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, + if (!buffer_core_->canTransform( + target, source, tf2::TimePointZero, &ret_error)) { @@ -132,20 +148,21 @@ bool RTT_TF2::canTransform( } } catch (std::exception e) { RTT::log(RTT::Error) << "canTransform() produced an exception: " << - e.what() << RTT::endlog(); - return false; + e.what() << RTT::endlog(); + return false; } return true; } geometry_msgs::msg::TransformStamped RTT_TF2::lookupTransform( - const std::string & target, - const std::string & source) const { + 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(); + e.what() << RTT::endlog(); return geometry_msgs::msg::TransformStamped(); } } @@ -158,7 +175,8 @@ geometry_msgs::msg::TransformStamped RTT_TF2::lookupTransform( // } void RTT_TF2::broadcastTransform( - const geometry_msgs::msg::TransformStamped & transform) { + const geometry_msgs::msg::TransformStamped & transform) +{ RTT::Logger::In in(getName()); if (!rosReady()) { return; @@ -174,7 +192,8 @@ void RTT_TF2::broadcastTransform( } void RTT_TF2::broadcastTransforms( - const std::vector & transforms) { + const std::vector & transforms) +{ if (!rosReady()) { return; } @@ -191,7 +210,8 @@ void RTT_TF2::broadcastTransforms( } void RTT_TF2::broadcastStaticTransform( - const geometry_msgs::msg::TransformStamped & transform) { + const geometry_msgs::msg::TransformStamped & transform) +{ if (!rosReady()) { return; } @@ -205,14 +225,15 @@ void RTT_TF2::broadcastStaticTransform( } void RTT_TF2::broadcastStaticTransforms( - const std::vector & transforms) { + const std::vector & transforms) +{ if (!rosReady()) { return; } // for(const auto & transform : transforms) { // broadcastStaticTransform(transform); // } - try { + try { // buffer_core_->setTransform(transform, "unknown_authority", false); static_transform_broadcaster_->sendTransform(transforms); } catch (std::exception e) { @@ -221,18 +242,22 @@ void RTT_TF2::broadcastStaticTransforms( } } -void RTT_TF2::addTf2Interface(RTT::Service::shared_ptr service) { +void RTT_TF2::addTf2Interface(RTT::Service::shared_ptr service) +{ // Ports cannot be added to the GlobalService // Add ports - service->addEventPort(ip_stamped_transform_, + service->addEventPort( + ip_stamped_transform_, boost::bind(&RTT_TF2::stamped_message_callback, this, _1)) - .doc("Reception of stamped transform to broadcast through TF2"); - service->addEventPort(ip_stamped_transform_static_, + .doc("Reception of stamped transform to broadcast through TF2"); + service->addEventPort( + ip_stamped_transform_static_, boost::bind(&RTT_TF2::stamped_message_static_callback, this, _1)) - .doc("Reception of stamped transform to broadcast through TF2 static"); + .doc("Reception of stamped transform to broadcast through TF2 static"); } -void RTT_TF2::stamped_message_callback(RTT::base::PortInterface * /*in_port*/) { +void RTT_TF2::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; @@ -245,7 +270,8 @@ void RTT_TF2::stamped_message_callback(RTT::base::PortInterface * /*in_port*/) { } void RTT_TF2::stamped_message_static_callback( - RTT::base::PortInterface * /*in_port*/) { + RTT::base::PortInterface * /*in_port*/) +{ RTT::Logger::In in(getName()); RTT::log(RTT::Warning) << "Callback called" << RTT::endlog(); geometry_msgs::msg::TransformStamped in_msg; @@ -257,7 +283,8 @@ void RTT_TF2::stamped_message_static_callback( } } -bool RTT_TF2::rosReady() { +bool RTT_TF2::rosReady() +{ if (!rosnode) { rosnode = rtt_ros2_node::getNode(getOwner()); } @@ -270,43 +297,45 @@ bool RTT_TF2::rosReady() { } RTT::log(RTT::Info) << "We have node: " << rosnode->get_name() << RTT::endlog(); if ((nullptr != transform_broadcaster_) && - (nullptr != transform_listener_) && - (nullptr != static_transform_broadcaster_)) { + (nullptr != transform_listener_) && + (nullptr != static_transform_broadcaster_)) + { return true; - } else { - // Initialize the interfaces to ros - 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 - 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; - } + } else { + // Initialize the interfaces to ros + 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 + 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 +} // 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 index d7059f8..bcce3be 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2_component.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2_component.cpp @@ -14,13 +14,15 @@ #include "rtt_ros2_tf2/rtt_ros2_tf2_component.hpp" -#include +#include + +#include "rtt/Component.hpp" namespace rtt_ros2_tf2 { - -TF2_Component::TF2_Component(const std::string& name) : - RTT::TaskContext(name, PreOperational) + +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(); @@ -40,7 +42,6 @@ void TF2_Component::updateHook() { RTT::Logger::In(this->getName()); RTT::log(RTT::Info) << "Running component!" << RTT::endlog(); - } void TF2_Component::cleanupHook() @@ -48,13 +49,12 @@ void TF2_Component::cleanupHook() RTT::Logger::In(this->getName()); RTT::log(RTT::Info) << "Cleaning up component!" << RTT::endlog(); } - -} //namespace rtt_ros2_tf2 +} // 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 + * 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 diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp index 4e9c5af..013827c 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp @@ -27,7 +27,8 @@ namespace rtt_ros2_tf2 { -static bool loadROSServiceIntoTaskContext(RTT::TaskContext * tc) { +static bool loadROSServiceIntoTaskContext(RTT::TaskContext * tc) +{ if (tc->provides()->hasService("tf2")) { RTT::log(RTT::Error) << "Another rosparam interface was already instantiated for component " << @@ -41,11 +42,12 @@ static bool loadROSServiceIntoTaskContext(RTT::TaskContext * tc) { return true; } -static bool loadGlobalROSService() { +static bool loadGlobalROSService() +{ RTT::Service::shared_ptr ros = RTT::internal::GlobalService::Instance()->provides("ros"); ros->doc("ROS operations and services"); - rtt_ros2_tf2::RTT_TF2::shared_ptr tf_service = + rtt_ros2_tf2::RTT_TF2::shared_ptr tf_service = boost::make_shared(nullptr); // tf_service->addTf2Interface(tf_service); @@ -75,4 +77,4 @@ std::string getRTTPluginName() {return "tf2";} std::string getRTTTargetName() {return OROCOS_TARGET_NAME;} } // extern "C" -} // namespace rtt_ros2_tf2 +} // namespace rtt_ros2_tf2 From 4947a72362c6f3f4805bb1ca156d344daafb2886 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Mon, 20 Jul 2020 13:59:30 +0200 Subject: [PATCH 08/21] rtt_ros2_tf2: adapt travis to include rtt_ros2_integration --- .travis.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index c02de7e..0b53932 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,11 +25,11 @@ branches: env: global: # global settings for all jobs - CCACHE_DIR=$HOME/.ccache # enables C/C++ caching in industrial_ci - - ROSDEP_SKIP_KEYS="log4cpp rtt ocl" # preinstalled in the docker images + - ROSDEP_SKIP_KEYS="log4cpp rtt ocl rtt_ros2_common_interfaces rtt_ros2_geometry_msgs" # preinstalled in the docker images matrix: # each line is a job - - ROS_DISTRO="dashing" DOCKER_IMAGE="orocos/ros2:dashing-ros-base-bionic" - - ROS_DISTRO="eloquent" DOCKER_IMAGE="orocos/ros2:eloquent-ros-base-bionic" - - ROS_DISTRO="foxy" DOCKER_IMAGE="orocos/ros2:foxy-ros-base-focal" + - ROS_DISTRO="dashing" DOCKER_IMAGE="orocos/ros2_integration:dashing-ros-base-bionic" + - ROS_DISTRO="eloquent" DOCKER_IMAGE="orocos/ros2_integration:eloquent-ros-base-bionic" + - ROS_DISTRO="foxy" DOCKER_IMAGE="orocos/ros2_integration:foxy-ros-base-focal" # allow failures, e.g. for unsupported distros # matrix: From 351316f635547a6e2c11c9eca5697625c1585ed8 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Mon, 20 Jul 2020 15:21:48 +0200 Subject: [PATCH 09/21] rtt_ros2_tf2: test tarvis with rtt_ros2_common_interfaces This patch adds the repository for rtt_ros2_common_interfaces in building time. --- .travis.yml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index 0b53932..f8f521d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,11 +25,11 @@ branches: env: global: # global settings for all jobs - CCACHE_DIR=$HOME/.ccache # enables C/C++ caching in industrial_ci - - ROSDEP_SKIP_KEYS="log4cpp rtt ocl rtt_ros2_common_interfaces rtt_ros2_geometry_msgs" # preinstalled in the docker images + - ROSDEP_SKIP_KEYS="log4cpp rtt ocl" # preinstalled in the docker images matrix: # each line is a job - - ROS_DISTRO="dashing" DOCKER_IMAGE="orocos/ros2_integration:dashing-ros-base-bionic" - - ROS_DISTRO="eloquent" DOCKER_IMAGE="orocos/ros2_integration:eloquent-ros-base-bionic" - - ROS_DISTRO="foxy" DOCKER_IMAGE="orocos/ros2_integration:foxy-ros-base-focal" + - ROS_DISTRO="dashing" DOCKER_IMAGE="orocos/ros2:dashing-ros-base-bionic" + - ROS_DISTRO="eloquent" DOCKER_IMAGE="orocos/ros2:eloquent-ros-base-bionic" + - ROS_DISTRO="foxy" DOCKER_IMAGE="orocos/ros2:foxy-ros-base-focal" # allow failures, e.g. for unsupported distros # matrix: @@ -39,5 +39,6 @@ env: # clone and run industrial_ci install: - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci -b master + - git clone --quiet --depth 50 https://github.com/orocos/rtt_ros2_common_interfaces.git src/rtt_ros2_common_interfaces -b master script: - .industrial_ci/travis.sh From 32ca0a5db7abe79c4d339e0b0ae55be722ab09c5 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Mon, 20 Jul 2020 16:01:39 +0200 Subject: [PATCH 10/21] rtt_ros2_tf2: remove internal dependency on typekit Remove in-package support for tf2_msgs typekit. --- rtt_ros2_tf2/CMakeLists.txt | 6 +++--- rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2.hpp | 4 ++-- rtt_ros2_tf2/src/rtt_ros2_tf2.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rtt_ros2_tf2/CMakeLists.txt b/rtt_ros2_tf2/CMakeLists.txt index f2ac6f2..9efa991 100644 --- a/rtt_ros2_tf2/CMakeLists.txt +++ b/rtt_ros2_tf2/CMakeLists.txt @@ -64,9 +64,9 @@ target_include_directories(rtt_ros2_tf2 PRIVATE $ $ ) -target_link_libraries(rtt_ros2_tf2 - rtt_tf2_msgs_typekit # typekit generated in this package (target name) -) +# target_link_libraries(rtt_ros2_tf2 +# rtt_tf2_msgs_typekit # typekit generated in this package (target name) +# ) # Target: orocos plugin orocos_plugin(rtt_ros2_tf2_service 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 index aac3391..2a42b00 100644 --- 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 @@ -30,7 +30,7 @@ // #include "geometry_msgs/typekit/msg/transform_stamped_Types.hpp" #include "geometry_msgs/typekit/Types.hpp" // #include "tf2_msgs/typekit/msg/tf_message_Types.hpp" -#include "tf2_msgs/typekit/Types.hpp" +// #include "tf2_msgs/typekit/Types.hpp" #include "tf2_msgs/msg/tf_message.hpp" namespace rtt_ros2_tf2 @@ -84,7 +84,7 @@ class RTT_TF2 : public RTT::Service // Input ports RTT::InputPort ip_stamped_transform_; RTT::InputPort ip_stamped_transform_static_; - RTT::InputPort ip_tf_port_; + // RTT::InputPort ip_tf_port_; private: // Constant diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp index 091fb4c..c05a502 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp @@ -33,7 +33,7 @@ RTT_TF2::RTT_TF2(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"), + // ip_tf_port_("pi_tf_port"), rosnode(nullptr), // clock_(boost::make_shared(rcl_clock_type_t::RCL_SYSTEM_TIME)), buffer_core_(boost::make_shared()), From 898813045d79c9e76fa72ae55f7a1baac1d10ae8 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Mon, 20 Jul 2020 17:19:32 +0200 Subject: [PATCH 11/21] rtt_ros2_tf2: link target agains typekits specifically This patch may solve the problem compiling for dashing and eloquent. --- rtt_ros2_tf2/CMakeLists.txt | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/rtt_ros2_tf2/CMakeLists.txt b/rtt_ros2_tf2/CMakeLists.txt index 9efa991..8e635eb 100644 --- a/rtt_ros2_tf2/CMakeLists.txt +++ b/rtt_ros2_tf2/CMakeLists.txt @@ -56,7 +56,10 @@ orocos_library(rtt_ros2_tf2 # target_link_libraries(rtt_ros2_tf2 tf2) # target_include_directories(rtt_ros2_tf2 tf2) ament_target_dependencies(rtt_ros2_tf2 - rclcpp tf2 tf2_ros tf2_msgs rtt_ros2_geometry_msgs + rclcpp tf2 tf2_ros tf2_msgs rtt_ros2_node +) +target_link_libraries(rtt_ros2_tf2 + rtt_ros2_geometry_msgs::rtt_geometry_msgs_typekit ) # Extra include_directories to find the own generated typekits from inside @@ -115,7 +118,10 @@ else() endif() rtt_ros2_export_plugin_depend(rclcpp) rtt_ros2_export_plugin_depend(tf2) +rtt_ros2_export_plugin_depend(tf2_msgs) rtt_ros2_export_plugin_depend(rtt_ros2_tf2) +rtt_ros2_export_plugin_depend(rtt_ros2_node) +rtt_ros2_export_plugin_depend(rtt_ros2_geometry_msgs) rtt_ros2_export_plugin_depend(rtt_tf2_msgs_typekit) # If typekit is generated and used # orocos_generate_package( From 6032213a892248df2e79f8b13e4b1d67ac06f888 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Wed, 22 Jul 2020 10:47:26 +0200 Subject: [PATCH 12/21] rtt_ros2_tf2: fix eloquent dashing dependencies --- rtt_ros2_tf2/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rtt_ros2_tf2/CMakeLists.txt b/rtt_ros2_tf2/CMakeLists.txt index 8e635eb..1af5673 100644 --- a/rtt_ros2_tf2/CMakeLists.txt +++ b/rtt_ros2_tf2/CMakeLists.txt @@ -56,10 +56,11 @@ orocos_library(rtt_ros2_tf2 # target_link_libraries(rtt_ros2_tf2 tf2) # target_include_directories(rtt_ros2_tf2 tf2) ament_target_dependencies(rtt_ros2_tf2 - rclcpp tf2 tf2_ros tf2_msgs rtt_ros2_node + rclcpp tf2 tf2_ros tf2_msgs rtt_ros2_node rtt_ros2_geometry_msgs ) target_link_libraries(rtt_ros2_tf2 rtt_ros2_geometry_msgs::rtt_geometry_msgs_typekit + rtt_ros2_node::rtt_ros2_node ) # Extra include_directories to find the own generated typekits from inside From d5e87aa53147b75107aca0247f51c0793281698e Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Wed, 22 Jul 2020 17:35:40 +0200 Subject: [PATCH 13/21] rtt_ros2_tf2: rename main class to Tf2Service Rename the main class. --- .travis.yml | 1 - rtt_ros2_tf2/CMakeLists.txt | 19 ++- .../orocos/rtt_ros2_tf2/rtt_ros2_tf2.hpp | 15 +-- rtt_ros2_tf2/package.xml | 10 +- rtt_ros2_tf2/src/rtt_ros2_tf2.cpp | 109 ++++++++++++------ rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp | 6 +- 6 files changed, 98 insertions(+), 62 deletions(-) diff --git a/.travis.yml b/.travis.yml index f8f521d..c02de7e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -39,6 +39,5 @@ env: # clone and run industrial_ci install: - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci -b master - - git clone --quiet --depth 50 https://github.com/orocos/rtt_ros2_common_interfaces.git src/rtt_ros2_common_interfaces -b master script: - .industrial_ci/travis.sh diff --git a/rtt_ros2_tf2/CMakeLists.txt b/rtt_ros2_tf2/CMakeLists.txt index 1af5673..d2f4133 100644 --- a/rtt_ros2_tf2/CMakeLists.txt +++ b/rtt_ros2_tf2/CMakeLists.txt @@ -27,13 +27,12 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_msgs REQUIRED) find_package(rtt_ros2_node REQUIRED) -find_package(rtt_ros2_geometry_msgs REQUIRED) find_package(rtt_ros2_interfaces REQUIRED) -# find_package(rtt_tf2_msgs REQUIRED) # Will be generated here # Include rtt_ros2 for the macro: rtt_ros2_export_plugin_depend() find_package(rtt_ros2 REQUIRED) @@ -45,7 +44,6 @@ 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 rtt_ros2_geometry_msgs includes are in: ${rtt_ros2_geometry_msgs_INCLUDE_DIRS}") # message(AUTHOR_WARNING "The rtt_ros2_interfaces includes are in: ${rtt_ros2_interfaces_INCLUDE_DIRS}") # message(AUTHOR_WARNING "The tf2_msgs includes are in: ${tf2_msgs_INCLUDE_DIRS}") orocos_library(rtt_ros2_tf2 @@ -56,10 +54,10 @@ orocos_library(rtt_ros2_tf2 # target_link_libraries(rtt_ros2_tf2 tf2) # target_include_directories(rtt_ros2_tf2 tf2) ament_target_dependencies(rtt_ros2_tf2 - rclcpp tf2 tf2_ros tf2_msgs rtt_ros2_node rtt_ros2_geometry_msgs + rclcpp tf2 tf2_ros tf2_msgs rtt_ros2_node geometry_msgs ) target_link_libraries(rtt_ros2_tf2 - rtt_ros2_geometry_msgs::rtt_geometry_msgs_typekit + # rtt_ros2_geometry_msgs::rtt_geometry_msgs_typekit rtt_ros2_node::rtt_ros2_node ) @@ -110,20 +108,17 @@ endif() # export information to downstream packages # ament_export_dependencies(rtt_ros2) -ament_export_dependencies(rclcpp tf2) +ament_export_dependencies(rclcpp + tf2 tf2_ros tf2_msgs + rtt_ros2_topics +) ament_export_include_directories(include/${PROJECT_NAME}) 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(rclcpp) -rtt_ros2_export_plugin_depend(tf2) -rtt_ros2_export_plugin_depend(tf2_msgs) -rtt_ros2_export_plugin_depend(rtt_ros2_tf2) rtt_ros2_export_plugin_depend(rtt_ros2_node) -rtt_ros2_export_plugin_depend(rtt_ros2_geometry_msgs) -rtt_ros2_export_plugin_depend(rtt_tf2_msgs_typekit) # If typekit is generated and used # orocos_generate_package( # INCLUDE_DIRS include 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 index 2a42b00..8670c5f 100644 --- 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 @@ -20,7 +20,7 @@ #include #include "rtt/Service.hpp" - +#include "rtt/InputPort.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" #include "tf2/buffer_core.h" @@ -28,7 +28,8 @@ #include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_listener.h" // #include "geometry_msgs/typekit/msg/transform_stamped_Types.hpp" -#include "geometry_msgs/typekit/Types.hpp" +// #include "geometry_msgs/typekit/Types.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" // #include "tf2_msgs/typekit/msg/tf_message_Types.hpp" // #include "tf2_msgs/typekit/Types.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -36,13 +37,13 @@ namespace rtt_ros2_tf2 { -class RTT_TF2 : public RTT::Service +class Tf2Service : public RTT::Service { public: - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; - explicit RTT_TF2(RTT::TaskContext * owner); - virtual ~RTT_TF2(); + explicit Tf2Service(RTT::TaskContext * owner); + virtual ~Tf2Service(); protected: ///! Operations @@ -120,7 +121,7 @@ class RTT_TF2 : public RTT::Service void stamped_message_static_callback(RTT::base::PortInterface * port); bool rosReady(); -}; // class RTT_TF2 +}; // class Tf2Service } // namespace rtt_ros2_tf2 diff --git a/rtt_ros2_tf2/package.xml b/rtt_ros2_tf2/package.xml index 90457b2..05f1c16 100644 --- a/rtt_ros2_tf2/package.xml +++ b/rtt_ros2_tf2/package.xml @@ -12,17 +12,17 @@ ament_cmake ament_index_cpp - rtt - rtt_ros2_interfaces - tf2_msgs rclcpp + geometry_msgs tf2 + tf2_msgs tf2_ros + rtt rtt_ros2 rtt_ros2_node - rtt_ros2_geometry_msgs - + rtt_ros2_interfaces + ament_lint_auto ament_lint_common diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp index c05a502..1399054 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2.cpp @@ -29,7 +29,7 @@ namespace rtt_ros2_tf2 { -RTT_TF2::RTT_TF2(RTT::TaskContext * owner) +Tf2Service::Tf2Service(RTT::TaskContext * owner) : RTT::Service("tf2", owner), ip_stamped_transform_("ip_stamped_transform"), ip_stamped_transform_static_("ip_stamped_transform_static"), @@ -54,27 +54,27 @@ RTT_TF2::RTT_TF2(RTT::TaskContext * owner) // Add operations this->addOperation( "broadcastTransform", - &RTT_TF2::broadcastTransform, this, RTT::OwnThread) + &Tf2Service::broadcastTransform, this, RTT::OwnThread) .doc("Broadcasts a stamped transform as TF2"); this->addOperation( "broadcastTransforms", - &RTT_TF2::broadcastTransforms, this, RTT::OwnThread) + &Tf2Service::broadcastTransforms, this, RTT::OwnThread) .doc("Broadcasts a vector of stamped transforms as TF2"); this->addOperation( "broadcastStaticTransform", - &RTT_TF2::broadcastStaticTransform, this, RTT::OwnThread) + &Tf2Service::broadcastStaticTransform, this, RTT::OwnThread) .doc("Broadcasts a stamped transform as TF2 static"); this->addOperation( "broadcastStaticTransforms", - &RTT_TF2::broadcastStaticTransforms, this, RTT::OwnThread) + &Tf2Service::broadcastStaticTransforms, this, RTT::OwnThread) .doc("Broadcasts a vector of stamped transforms as TF2 static"); this->addOperation( "lookupTransform", - &RTT_TF2::lookupTransform, this, RTT::OwnThread) + &Tf2Service::lookupTransform, this, RTT::OwnThread) .doc("Looks up for a TF2 transform"); this->addOperation( "clear", - &RTT_TF2::clear, this, RTT::OwnThread) + &Tf2Service::clear, this, RTT::OwnThread) .doc("Clears TF2 transforms"); } else { // addTf2Interface( @@ -82,27 +82,27 @@ RTT_TF2::RTT_TF2(RTT::TaskContext * owner) // Add operations this->addOperation( "broadcastTransform", - &RTT_TF2::broadcastTransform, this, RTT::ClientThread) + &Tf2Service::broadcastTransform, this, RTT::ClientThread) .doc("Broadcasts a stamped transform as TF2"); this->addOperation( "broadcastTransforms", - &RTT_TF2::broadcastTransforms, this, RTT::ClientThread) + &Tf2Service::broadcastTransforms, this, RTT::ClientThread) .doc("Broadcasts a vector of stamped transforms as TF2"); this->addOperation( "broadcastStaticTransform", - &RTT_TF2::broadcastStaticTransform, this, RTT::ClientThread) + &Tf2Service::broadcastStaticTransform, this, RTT::ClientThread) .doc("Broadcasts a stamped transform as TF2 static"); this->addOperation( "broadcastStaticTransforms", - &RTT_TF2::broadcastStaticTransforms, this, RTT::ClientThread) + &Tf2Service::broadcastStaticTransforms, this, RTT::ClientThread) .doc("Broadcasts a vector of stamped transforms as TF2 static"); this->addOperation( "lookupTransform", - &RTT_TF2::lookupTransform, this, RTT::ClientThread) + &Tf2Service::lookupTransform, this, RTT::ClientThread) .doc("Looks up for a TF2 transform"); this->addOperation( "clear", - &RTT_TF2::clear, this, RTT::ClientThread) + &Tf2Service::clear, this, RTT::ClientThread) .doc("Clears TF2 transforms"); RTT::log(RTT::Info) << "TF2 service instantiated standalone without interface" << @@ -110,27 +110,30 @@ RTT_TF2::RTT_TF2(RTT::TaskContext * owner) } } -RTT_TF2::~RTT_TF2() = default; +Tf2Service::~Tf2Service() = default; -// void RTT_TF2::internalUpdate( +// void Tf2Service::internalUpdate( // tf2_msgs::TFMessage & msg, // RTT::InputPort & port, // bool is_static) { // } -// rclcpp::Time RTT_TF2::getLatestCommonTime( +// rclcpp::Time Tf2Service::getLatestCommonTime( // const std::string & target, // const std::string & source) const { // return clock_->now(); // } -void RTT_TF2::clear() +void Tf2Service::clear() { - buffer_core_->clear(); + RTT::Logger::In in(getName()); + if (buffer_core_) { + buffer_core_->clear(); + } } -bool RTT_TF2::canTransform( +bool Tf2Service::canTransform( const std::string & target, const std::string & source) const { @@ -154,7 +157,7 @@ bool RTT_TF2::canTransform( return true; } -geometry_msgs::msg::TransformStamped RTT_TF2::lookupTransform( +geometry_msgs::msg::TransformStamped Tf2Service::lookupTransform( const std::string & target, const std::string & source) const { @@ -167,23 +170,33 @@ geometry_msgs::msg::TransformStamped RTT_TF2::lookupTransform( } } -// geometry_msgs::msg::TransformStamped RTT_TF2::lookupTransformAtTime( +// geometry_msgs::msg::TransformStamped Tf2Service::lookupTransformAtTime( // const std::string & target, // const std::string & source, // const rclcpp::Time & common_time) const { // } -void RTT_TF2::broadcastTransform( +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 { - // buffer_core_->setTransform(transform, "unknown_authority", false); + 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: " << @@ -191,7 +204,7 @@ void RTT_TF2::broadcastTransform( } } -void RTT_TF2::broadcastTransforms( +void Tf2Service::broadcastTransforms( const std::vector & transforms) { if (!rosReady()) { @@ -201,6 +214,15 @@ void RTT_TF2::broadcastTransforms( // 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) { @@ -209,14 +231,20 @@ void RTT_TF2::broadcastTransforms( } } -void RTT_TF2::broadcastStaticTransform( +void Tf2Service::broadcastStaticTransform( const geometry_msgs::msg::TransformStamped & transform) { if (!rosReady()) { return; } try { - // buffer_core_->setTransform(transform, "unknown_authority", true); + 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: " << @@ -224,7 +252,7 @@ void RTT_TF2::broadcastStaticTransform( } } -void RTT_TF2::broadcastStaticTransforms( +void Tf2Service::broadcastStaticTransforms( const std::vector & transforms) { if (!rosReady()) { @@ -234,7 +262,15 @@ void RTT_TF2::broadcastStaticTransforms( // broadcastStaticTransform(transform); // } try { - // buffer_core_->setTransform(transform, "unknown_authority", false); + 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: " << @@ -242,21 +278,21 @@ void RTT_TF2::broadcastStaticTransforms( } } -void RTT_TF2::addTf2Interface(RTT::Service::shared_ptr service) +void Tf2Service::addTf2Interface(RTT::Service::shared_ptr service) { // Ports cannot be added to the GlobalService // Add ports service->addEventPort( ip_stamped_transform_, - boost::bind(&RTT_TF2::stamped_message_callback, this, _1)) + 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(&RTT_TF2::stamped_message_static_callback, this, _1)) + boost::bind(&Tf2Service::stamped_message_static_callback, this, _1)) .doc("Reception of stamped transform to broadcast through TF2 static"); } -void RTT_TF2::stamped_message_callback(RTT::base::PortInterface * /*in_port*/) +void Tf2Service::stamped_message_callback(RTT::base::PortInterface * /*in_port*/) { RTT::Logger::In in(getName()); RTT::log(RTT::Warning) << "Callback called" << RTT::endlog(); @@ -269,7 +305,7 @@ void RTT_TF2::stamped_message_callback(RTT::base::PortInterface * /*in_port*/) } } -void RTT_TF2::stamped_message_static_callback( +void Tf2Service::stamped_message_static_callback( RTT::base::PortInterface * /*in_port*/) { RTT::Logger::In in(getName()); @@ -283,7 +319,7 @@ void RTT_TF2::stamped_message_static_callback( } } -bool RTT_TF2::rosReady() +bool Tf2Service::rosReady() { if (!rosnode) { rosnode = rtt_ros2_node::getNode(getOwner()); @@ -303,6 +339,7 @@ bool RTT_TF2::rosReady() return true; } else { // Initialize the interfaces to ros + // buffer_core_ = boost::make_shared(); transform_broadcaster_ = boost::make_shared( rosnode); if (nullptr == transform_broadcaster_) { @@ -310,6 +347,10 @@ bool RTT_TF2::rosReady() 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_) { diff --git a/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp b/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp index 013827c..50d02dc 100644 --- a/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp +++ b/rtt_ros2_tf2/src/rtt_ros2_tf2_service.cpp @@ -36,7 +36,7 @@ static bool loadROSServiceIntoTaskContext(RTT::TaskContext * tc) return false; } - auto tf_service = boost::make_shared(tc); + auto tf_service = boost::make_shared(tc); // tf_service->addTf2Interface(tc->provides()); tc->provides()->addService(std::move(tf_service)); return true; @@ -47,8 +47,8 @@ static bool loadGlobalROSService() RTT::Service::shared_ptr ros = RTT::internal::GlobalService::Instance()->provides("ros"); ros->doc("ROS operations and services"); - rtt_ros2_tf2::RTT_TF2::shared_ptr tf_service = - boost::make_shared(nullptr); + rtt_ros2_tf2::Tf2Service::shared_ptr tf_service = + boost::make_shared(nullptr); // tf_service->addTf2Interface(tf_service); if (!ros->addService(std::move(tf_service))) { From ad77c0020246c4f3614f2b10cf8ae4e7764284b7 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Wed, 22 Jul 2020 17:36:34 +0200 Subject: [PATCH 14/21] rtt_ros2_tf2: add unitest in rtt_ros2_tf2_tests This patch adds a new package called rtt_ros2_tf2_tests with unitests for the service Tf2Service. --- tests/rtt_ros2_tf2_tests/CMakeLists.txt | 64 +++ tests/rtt_ros2_tf2_tests/package.xml | 26 ++ .../rtt_ros2_tf2_tests/test/test_ros_tf2.cpp | 365 ++++++++++++++++++ 3 files changed, 455 insertions(+) create mode 100644 tests/rtt_ros2_tf2_tests/CMakeLists.txt create mode 100644 tests/rtt_ros2_tf2_tests/package.xml create mode 100644 tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp diff --git a/tests/rtt_ros2_tf2_tests/CMakeLists.txt b/tests/rtt_ros2_tf2_tests/CMakeLists.txt new file mode 100644 index 0000000..6456f9a --- /dev/null +++ b/tests/rtt_ros2_tf2_tests/CMakeLists.txt @@ -0,0 +1,64 @@ +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) + ament_package() + return() +endif() + +# The test_ros_transport 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() + +# 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(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) + +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 rtt_ros2_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..c51c3c9 --- /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 + + rtt_ros2 + rtt_ros2_node + rtt_ros2_tf2 + rtt_ros2_geometry_msgs + + 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..d2744ae --- /dev/null +++ b/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp @@ -0,0 +1,365 @@ +// 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/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 "gtest/gtest.h" + +class TestRosTf2Environment + : public ::testing::Environment +{ + void SetUp() override + { + EXPECT_TRUE(rtt_ros2::import("rtt_ros2_tf2")); + EXPECT_TRUE(rtt_ros2::import("rtt_ros2_geometry_msgs")); + } +}; + +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("ros2-node")); + 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; +} From 4a29fc681039d92cb65d5f1feb796f0a149988a7 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Wed, 22 Jul 2020 17:45:28 +0200 Subject: [PATCH 15/21] rtt_ros2_tf2_tests: remove rtt_ros2_geometry_msgs dependency --- tests/rtt_ros2_tf2_tests/CMakeLists.txt | 5 +++-- tests/rtt_ros2_tf2_tests/package.xml | 2 +- tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp | 4 +++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/tests/rtt_ros2_tf2_tests/CMakeLists.txt b/tests/rtt_ros2_tf2_tests/CMakeLists.txt index 6456f9a..77369f0 100644 --- a/tests/rtt_ros2_tf2_tests/CMakeLists.txt +++ b/tests/rtt_ros2_tf2_tests/CMakeLists.txt @@ -37,10 +37,11 @@ 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) +# find_package(rtt_ros2_geometry_msgs REQUIRED) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -60,5 +61,5 @@ orocos_configure_executable(test_ros_tf2) # rtt_ros2_tf2::rtt_ros2_tf2 # ) ament_target_dependencies(test_ros_tf2 - rtt_ros2 rtt_ros2_node rtt_ros2_tf2 rtt_ros2_geometry_msgs + 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 index c51c3c9..5e9b724 100644 --- a/tests/rtt_ros2_tf2_tests/package.xml +++ b/tests/rtt_ros2_tf2_tests/package.xml @@ -11,10 +11,10 @@ ament_cmake + geometry_msgs rtt_ros2 rtt_ros2_node rtt_ros2_tf2 - rtt_ros2_geometry_msgs ament_cmake_gtest ament_lint_auto diff --git a/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp b/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp index d2744ae..ab10b35 100644 --- a/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp +++ b/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp @@ -22,6 +22,7 @@ #include #include "rtt/RTT.hpp" +#include "rtt/OperationCaller.hpp" #include "rtt/internal/GlobalService.hpp" #include "rtt/internal/GlobalEngine.hpp" #include "rtt/os/startstop.h" @@ -31,7 +32,8 @@ #include "rtt_ros2_tf2/rtt_ros2_tf2.hpp" -#include "geometry_msgs/typekit/Types.hpp" +// #include "geometry_msgs/typekit/Types.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" #include "gtest/gtest.h" From 3d0d6ee0a57e015c602f0e6a71d17717ff82171d Mon Sep 17 00:00:00 2001 From: Sergio Portoles Diez Date: Fri, 24 Jul 2020 16:46:04 +0200 Subject: [PATCH 16/21] rtt_ros2_tf2: add documentation Add some documentation about the tf2 service. --- rtt_ros2_tf2/README.md | 35 +++++++++++++++++++++++++++++++---- 1 file changed, 31 insertions(+), 4 deletions(-) diff --git a/rtt_ros2_tf2/README.md b/rtt_ros2_tf2/README.md index cf9bf24..d62cdac 100644 --- a/rtt_ros2_tf2/README.md +++ b/rtt_ros2_tf2/README.md @@ -7,16 +7,43 @@ Orocos. This service has operations for requesting and broadcasting sing and batch transforms. -### ToDo +## 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()`: -Implement it. ### Usage #### Scripting API (*.ops) -TODO +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 +*ToDo* From a3da06755ac60a414052b5f4aa99e39cfe4cdab8 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Tue, 28 Jul 2020 23:16:26 +0200 Subject: [PATCH 17/21] rtt_ros2_tf2: fix ament_export_include_directories() and use new macro name rtt_ros2_generate_interfaces_plugins() --- rtt_ros2_tf2/CMakeLists.txt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rtt_ros2_tf2/CMakeLists.txt b/rtt_ros2_tf2/CMakeLists.txt index d2f4133..3bb2f81 100644 --- a/rtt_ros2_tf2/CMakeLists.txt +++ b/rtt_ros2_tf2/CMakeLists.txt @@ -87,8 +87,7 @@ orocos_component(rtt_ros2_tf2_component # Now made with ament_target_dependencies() # Target: typekit (from tf2_msgs) -# ros_generate_rtt_typekit(tf2_msgs) -rtt_ros2_generate_typekit_and_transports(tf2_msgs) +rtt_ros2_generate_interfaces_plugins(tf2_msgs) # install # orocos_install_headers(include/rtt_ros2_tf2/rtt_ros2_tf2.hpp) @@ -112,7 +111,7 @@ ament_export_dependencies(rclcpp tf2 tf2_ros tf2_msgs rtt_ros2_topics ) -ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_include_directories(include/orocos) if(COMMAND ament_export_targets) ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) else() From b89d2e9c12a30d2556a6696db53c6464cfb8da9c Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Tue, 28 Jul 2020 23:17:47 +0200 Subject: [PATCH 18/21] rtt_ros2_tf2_tests: match CMakeLists.txt with other unit test packages and fix linking to rtt_ros2_node and rtt_ros2_tf2 targets --- tests/rtt_ros2_tf2_tests/CMakeLists.txt | 38 +++++++++++++------------ 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/tests/rtt_ros2_tf2_tests/CMakeLists.txt b/tests/rtt_ros2_tf2_tests/CMakeLists.txt index 77369f0..a413dda 100644 --- a/tests/rtt_ros2_tf2_tests/CMakeLists.txt +++ b/tests/rtt_ros2_tf2_tests/CMakeLists.txt @@ -4,20 +4,9 @@ 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) - ament_package() - return() -endif() - -# The test_ros_transport 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") + install(CODE + "message(STATUS \"Skipping installation of package ${PROJECT_NAME} because BUILD_TESTING is OFF.\")" + ) return() endif() @@ -53,13 +42,26 @@ 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 +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 From f41ca47d0ef05728609fba7b0b99b3d806f05217 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Tue, 28 Jul 2020 23:38:16 +0200 Subject: [PATCH 19/21] rtt_ros2_tf2: cleanup CMakeLists.txt and dependencies in package.xml, drop tf2_msgs typekit --- rtt_ros2_tf2/CMakeLists.txt | 64 ++++++------------- .../orocos/rtt_ros2_tf2/rtt_ros2_tf2.hpp | 14 ++-- rtt_ros2_tf2/package.xml | 19 ++---- 3 files changed, 31 insertions(+), 66 deletions(-) diff --git a/rtt_ros2_tf2/CMakeLists.txt b/rtt_ros2_tf2/CMakeLists.txt index 3bb2f81..4f0a43c 100644 --- a/rtt_ros2_tf2/CMakeLists.txt +++ b/rtt_ros2_tf2/CMakeLists.txt @@ -15,36 +15,22 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# Include RTT, this should include orocos_*() macros -find_package(OROCOS-RTT REQUIRED) -if(NOT OROCOS-RTT_FOUND) - message(FATAL_ERROR "\n RTT not found") -else() - include(${OROCOS-RTT_USE_FILE_PATH}/UseOROCOS-RTT.cmake) -endif() - # find dependencies find_package(ament_cmake REQUIRED) -find_package(ament_index_cpp REQUIRED) -find_package(rclcpp 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_ros REQUIRED) find_package(tf2_msgs REQUIRED) -find_package(rtt_ros2_node REQUIRED) -find_package(rtt_ros2_interfaces REQUIRED) - -# Include rtt_ros2 for the macro: rtt_ros2_export_plugin_depend() -find_package(rtt_ros2 REQUIRED) +find_package(tf2_ros REQUIRED) # setup targets -include_directories(include) 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 rtt_ros2_interfaces includes are in: ${rtt_ros2_interfaces_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 @@ -54,45 +40,31 @@ orocos_library(rtt_ros2_tf2 # target_link_libraries(rtt_ros2_tf2 tf2) # target_include_directories(rtt_ros2_tf2 tf2) ament_target_dependencies(rtt_ros2_tf2 - rclcpp tf2 tf2_ros tf2_msgs rtt_ros2_node geometry_msgs + geometry_msgs + rclcpp + rtt_ros2 + rtt_ros2_node + tf2 + tf2_msgs + tf2_ros ) target_link_libraries(rtt_ros2_tf2 - # rtt_ros2_geometry_msgs::rtt_geometry_msgs_typekit rtt_ros2_node::rtt_ros2_node ) -# Extra include_directories to find the own generated typekits from inside -target_include_directories(rtt_ros2_tf2 PRIVATE - $ - $ -) -# target_link_libraries(rtt_ros2_tf2 -# rtt_tf2_msgs_typekit # typekit generated in this package (target name) -# ) - # 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) - ament_target_dependencies(rtt_ros2_tf2_component - OROCOS-RTT + src/rtt_ros2_tf2_component.cpp ) -# target_link_libraries(rtt_ros2_tf2_component ${catkin_LIBRARIES}) -# Now made with ament_target_dependencies() - -# Target: typekit (from tf2_msgs) -rtt_ros2_generate_interfaces_plugins(tf2_msgs) +target_link_libraries(rtt_ros2_tf2_component rtt_ros2_tf2) # install -# orocos_install_headers(include/rtt_ros2_tf2/rtt_ros2_tf2.hpp) -# orocos_install_headers(include/rtt_ros2_tf2/tf2_interface.h) -# orocos_install_headers(include/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp) install( DIRECTORY include/ DESTINATION include @@ -104,12 +76,14 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() - # export information to downstream packages # ament_export_dependencies(rtt_ros2) -ament_export_dependencies(rclcpp - tf2 tf2_ros tf2_msgs - rtt_ros2_topics +ament_export_dependencies( + geometry_msgs + rclcpp + tf2_msgs + tf2_ros + tf2 ) ament_export_include_directories(include/orocos) if(COMMAND ament_export_targets) 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 index 8670c5f..15c898f 100644 --- 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 @@ -19,20 +19,16 @@ #include #include -#include "rtt/Service.hpp" -#include "rtt/InputPort.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" -#include "tf2/buffer_core.h" +#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 "geometry_msgs/typekit/msg/transform_stamped_Types.hpp" -// #include "geometry_msgs/typekit/Types.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -// #include "tf2_msgs/typekit/msg/tf_message_Types.hpp" -// #include "tf2_msgs/typekit/Types.hpp" -#include "tf2_msgs/msg/tf_message.hpp" +#include "tf2/buffer_core.h" namespace rtt_ros2_tf2 { diff --git a/rtt_ros2_tf2/package.xml b/rtt_ros2_tf2/package.xml index 05f1c16..f3ce481 100644 --- a/rtt_ros2_tf2/package.xml +++ b/rtt_ros2_tf2/package.xml @@ -11,18 +11,13 @@ ament_cmake - ament_index_cpp - - rclcpp - geometry_msgs - tf2 - tf2_msgs - tf2_ros - rtt - rtt_ros2 - rtt_ros2_node - rtt_ros2_interfaces - + geometry_msgs + rclcpp + rtt_ros2_node + rtt_ros2 + tf2_msgs + tf2_ros + tf2 ament_lint_auto ament_lint_common From b94ffcda3f40feca00bf721f163a74f7c23417eb Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Tue, 28 Jul 2020 23:42:43 +0200 Subject: [PATCH 20/21] rtt_ros2_tf2_tests: fix loading of renamed service rosnode in test_ros_tf2.cpp --- tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp b/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp index ab10b35..3c0785e 100644 --- a/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp +++ b/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp @@ -213,7 +213,7 @@ TEST_F(TestRosTf2, TestComponentNodeTf2) RTT::Logger::In in(getName()); // Create a component-local ROS node - ASSERT_TRUE(loadService("ros2-node")); + ASSERT_TRUE(loadService("rosnode")); ASSERT_TRUE(rtt_ros2_node::getNodeService(this) != nullptr); ASSERT_TRUE(rtt_ros2_node::getNode(this) != nullptr); From 30adf612a53f40961c78a1bfe5a52280f7b00571 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Wed, 29 Jul 2020 00:57:44 +0200 Subject: [PATCH 21/21] rtt_ros2_tf2_tests: remove import of package rtt_ros2_geometry_msgs If it would be required, it must be declared as a plugin dependency of rtt_ros2_tf2 using the macro rtt_ros2_export_plugin_depend() provided by rtt_ros2. But it is not. --- tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp b/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp index 3c0785e..20362d7 100644 --- a/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp +++ b/tests/rtt_ros2_tf2_tests/test/test_ros_tf2.cpp @@ -43,7 +43,6 @@ class TestRosTf2Environment void SetUp() override { EXPECT_TRUE(rtt_ros2::import("rtt_ros2_tf2")); - EXPECT_TRUE(rtt_ros2::import("rtt_ros2_geometry_msgs")); } };