Add new package rtt_ros2_tf2 support for ROS2 TF2

rtt_ros2_tf2/CMakeLists.txt

@@ -0,0 +1,122 @@
+cmake_minimum_required(VERSION 3.5)
+project(rtt_ros2_tf2)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rtt_ros2 REQUIRED)
+find_package(rtt_ros2_node REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+# setup targets
+include_directories(include/orocos)
+
+# Target: library
+# message(AUTHOR_WARNING "The tf2 includes are in: ${tf2_INCLUDE_DIRS}")
+# message(AUTHOR_WARNING "The rclcpp includes are in: ${rclcpp_INCLUDE_DIRS}")
+# message(AUTHOR_WARNING "The tf2_msgs includes are in: ${tf2_msgs_INCLUDE_DIRS}")
+orocos_library(rtt_ros2_tf2
+  src/rtt_ros2_tf2.cpp
+  EXPORT ${PROJECT_NAME}
+  INCLUDES DESTINATION include/orocos/${PROJECT_NAME}
+)
+# target_link_libraries(rtt_ros2_tf2 tf2)
+# target_include_directories(rtt_ros2_tf2 tf2)
+ament_target_dependencies(rtt_ros2_tf2
+  geometry_msgs
+  rclcpp
+  rtt_ros2
+  rtt_ros2_node
+  tf2
+  tf2_msgs
+  tf2_ros
+)
+target_link_libraries(rtt_ros2_tf2
+  rtt_ros2_node::rtt_ros2_node
+)
+
+# Target: orocos plugin
+orocos_plugin(rtt_ros2_tf2_service
+  src/rtt_ros2_tf2_service.cpp
+)
+target_link_libraries(rtt_ros2_tf2_service rtt_ros2_tf2)
+
+# Target: orocos component
+orocos_component(rtt_ros2_tf2_component
+  src/rtt_ros2_tf2_component.cpp
+)
+target_link_libraries(rtt_ros2_tf2_component rtt_ros2_tf2)
+
+# install
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+# linters
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# export information to downstream packages
+# ament_export_dependencies(rtt_ros2)
+ament_export_dependencies(
+  geometry_msgs
+  rclcpp
+  tf2_msgs
+  tf2_ros
+  tf2
+)
+ament_export_include_directories(include/orocos)
+if(COMMAND ament_export_targets)
+  ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
+else()
+  ament_export_interfaces(${PROJECT_NAME} HAS_LIBRARY_TARGET)
+endif()
+rtt_ros2_export_plugin_depend(rtt_ros2_node)
+
+# orocos_generate_package(
+#   INCLUDE_DIRS include
+#   DEPENDS tf2
+#   DEPENDS_TARGETS rtt_geometry_msgs rtt_tf2_msgs
+# )
+
+# Legacy from rtt_tf
+# Tests
+# if(CATKIN_ENABLE_TESTING)
+#   orocos_use_package(ocl-taskbrowser REQUIRED)
+#   orocos_use_package(ocl-deployment REQUIRED)
+
+#   add_definitions(-DRTT_COMPONENT)
+#   orocos_executable(broadcaster_test tests/broadcaster_test.cpp)
+#   target_link_libraries(broadcaster_test ${catkin_LIBRARIES} ${USE_OROCOS_LIBRARIES})
+#   orocos_executable(lookup_test tests/lookup_test.cpp)
+#   target_link_libraries(lookup_test ${catkin_LIBRARIES} ${USE_OROCOS_LIBRARIES})
+# endif()
+
+# must be called *after* the targets to check exported libraries etc.
+ament_package()
+
+# orocos_generate_package() is deprecated for ROS 2.
+# Prefer cmake target export and import instead, in combination with
+# ament_export_interfaces() or ament_export_targets() when building with
+# ament_cmake.
+orocos_generate_package()

rtt_ros2_tf2/README.md

@@ -0,0 +1,49 @@
+RTT ROS2 TF2
+============
+
+This package provides an Orocos RTT service for utilizing the [TF2 rigid body
+transform library](https://index.ros.org/doc/ros2/Tutorials/tf2) from within
+Orocos.
+This service has operations for requesting and broadcasting sing and
+batch transforms.
+
+## Service description
+
+The service provided by the library exposes some operations to interact with TF.
+The operations available are:
+* `sendTransform()`:
+* `sendTransforms()`:
+* `sendStaticTransform()`:
+* `sendStaticTransforms()`:
+* `lookupTransform()`:
+* `clear()`:
+
+
+### Usage
+
+#### Scripting API (*.ops)
+
+The package can be loaded independently form other related `ros` packages, but it requires the `rosnode` service
+to work.
+
+The service can be loaded globally by importing the package:
+
+```
+import("rtt_ros2")
+ros.import("rtt_ros2_tf2")
+```
+This will create a global service under the `ros` namespace. The operations run by this
+service will be executed in the `ClientThread` and are NOT real-time safe.
+
+To load the service into a component, after importing the package, it can be loaded with:
+
+```
+loadService("<component-name>", "tf2")
+```
+
+The component version will run the calls in the `OwnThread`. The component should not be
+real time.
+
+### C++ API
+
+*ToDo*

rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2.hpp

@@ -0,0 +1,124 @@
+// Copyright 2020 Intermodalics BVBA
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_HPP_
+#define OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_HPP_
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+#include "rtt/InputPort.hpp"
+#include "rtt/Service.hpp"
+#include "tf2_msgs/msg/tf_message.hpp"
+#include "tf2_ros/static_transform_broadcaster.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2/buffer_core.h"
+
+namespace rtt_ros2_tf2
+{
+
+class Tf2Service : public RTT::Service
+{
+public:
+  typedef boost::shared_ptr<Tf2Service> shared_ptr;
+
+  explicit Tf2Service(RTT::TaskContext * owner);
+  virtual ~Tf2Service();
+
+protected:
+  ///! Operations
+  // rclcpp::Time getLatestCommonTime(
+  //     const std::string & target,
+  //     const std::string & source) const;
+
+  bool canTransform(
+    const std::string & target,
+    const std::string & source) const;
+
+  geometry_msgs::msg::TransformStamped lookupTransform(
+    const std::string & target,
+    const std::string & source) const;
+
+  // geometry_msgs::msg::TransformStamped lookupTransformAtTime(
+  //     const std::string & target,
+  //     const std::string & source,
+  //     const rclcpp::Time& common_time) const;
+
+  void broadcastTransform(
+    const geometry_msgs::msg::TransformStamped & transform);
+
+  void broadcastTransforms(
+    const std::vector<geometry_msgs::msg::TransformStamped> & transforms);
+
+  void broadcastStaticTransform(
+    const geometry_msgs::msg::TransformStamped & transform);
+
+  void broadcastStaticTransforms(
+    const std::vector<geometry_msgs::msg::TransformStamped> & transforms);
+
+  void clear();
+
+  void addTf2Interface(RTT::Service::shared_ptr service);
+
+protected:
+  ///! Communication ports
+  // Input ports
+  RTT::InputPort<geometry_msgs::msg::TransformStamped> ip_stamped_transform_;
+  RTT::InputPort<geometry_msgs::msg::TransformStamped> ip_stamped_transform_static_;
+  // RTT::InputPort<tf2_msgs::msg::TFMessage> ip_tf_port_;
+
+private:
+  // Constant
+  static constexpr int kDefaultBufferSize = 100;
+
+  // Clock
+  rclcpp::Node::SharedPtr rosnode;
+  boost::shared_ptr<rclcpp::Clock> clock_;
+  boost::shared_ptr<tf2::BufferCore> buffer_core_;
+  // boost::shared_ptr<tf2_ros::BufferClient> buffer_client_;
+  boost::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster_;
+  boost::shared_ptr<tf2_ros::TransformListener> transform_listener_;
+  boost::shared_ptr<tf2_ros::StaticTransformBroadcaster>
+  static_transform_broadcaster_;
+
+  // Example members
+
+  int private_int_ = 42;
+  // tf2::BufferCore buffer_core_;
+
+  // Deprecated?
+  std::string prop_tf_prefix_;
+  double prop_cache_time_;
+  double prop_buffer_size_;
+
+private:
+  // void internalUpdate(
+  //   tf2_msgs::TFMessage & msg,
+  //   RTT::InputPort<tf2_msgs::TFMessage> & port,
+  //   bool is_static);
+  tf2_msgs::msg::TFMessage tf_msgs_;
+  void stamped_message_callback(RTT::base::PortInterface * port);
+  void stamped_message_static_callback(RTT::base::PortInterface * port);
+
+  bool rosReady();
+};  // class Tf2Service
+
+}  // namespace rtt_ros2_tf2
+
+#endif  // OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_HPP_

rtt_ros2_tf2/include/orocos/rtt_ros2_tf2/rtt_ros2_tf2_component.hpp

@@ -0,0 +1,41 @@
+// Copyright 2020 Intermodalics BVBA
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_COMPONENT_HPP_
+#define OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_COMPONENT_HPP_
+
+#include <string>
+
+#include "rtt/RTT.hpp"
+// #include "tf2_msgs/TFMessage.h"
+// #include "tf2/buffer_core.h"
+
+namespace rtt_ros2_tf2
+{
+// Inherit from TaskContext
+class TF2_Component : public RTT::TaskContext
+{
+public:
+  explicit TF2_Component(std::string const & name);
+
+  bool configureHook();
+
+  void updateHook();
+
+  void cleanupHook();
+};
+
+}  // namespace rtt_ros2_tf2
+
+#endif  // OROCOS__RTT_ROS2_TF2__RTT_ROS2_TF2_COMPONENT_HPP_

rtt_ros2_tf2/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rtt_ros2_tf2</name>
+  <version>0.0.0</version>
+  <description>
+    This package provides an interface to use TF2 in Orocos context.
+  </description>
+  <maintainer email="[email protected]">Orocos Developers</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rtt_ros2_node</depend>
+  <depend>rtt_ros2</depend>
+  <depend>tf2_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
+