diff --git a/CMakeLists.txt b/CMakeLists.txt
index 353e88399270ea098b65a857ecece7f34dca8d0e..ee63a29ada1ffdf8f0319cd32f88157578942eea 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -31,72 +31,15 @@ INCLUDE(cmake/ros.cmake)
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
-# Project dependencies
-SET(CATKIN_REQUIRED_COMPONENTS roscpp std_msgs message_generation std_srvs geometry_msgs sensor_msgs tf2_ros
- realtime_tools)
-ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS program_options)
-ADD_PROJECT_DEPENDENCY(dynamic_graph_bridge_msgs 0.3.0 REQUIRED)
-
-IF(BUILD_PYTHON_INTERFACE)
- FINDPYTHON()
- SEARCH_FOR_BOOST_PYTHON()
- STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
- ADD_PROJECT_DEPENDENCY(dynamic-graph-python 4.0.0 REQUIRED)
- SET(CATKIN_REQUIRED_COMPONENTS ${CATKIN_REQUIRED_COMPONENTS} rospy)
-
- IF(Boost_VERSION GREATER 107299 OR Boost_VERSION_MACRO GREATER 107299)
- # Silence a warning about a deprecated use of boost bind by boost >= 1.73
- # without dropping support for boost < 1.73
- ADD_DEFINITIONS(-DBOOST_BIND_GLOBAL_PLACEHOLDERS)
- ENDIF()
-ENDIF(BUILD_PYTHON_INTERFACE)
-
-find_package(catkin REQUIRED COMPONENTS ${CATKIN_REQUIRED_COMPONENTS})
-
-ADD_PROJECT_DEPENDENCY(sot-core REQUIRED)
-
-# Main Library
-set(${PROJECT_NAME}_HEADERS
- include/${PROJECT_NAME}/fwd.hh
- include/${PROJECT_NAME}/ros_init.hh
- include/${PROJECT_NAME}/sot_loader.hh
- include/${PROJECT_NAME}/sot_loader_basic.hh
- include/${PROJECT_NAME}/ros_interpreter.hh
- src/converter.hh
- src/sot_to_ros.hh
- )
-
-SET(${PROJECT_NAME}_SOURCES
- src/ros_init.cpp
- src/sot_to_ros.cpp
- src/ros_parameter.cpp
- )
-
-ADD_LIBRARY(ros_bridge SHARED
- ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
-TARGET_INCLUDE_DIRECTORIES(ros_bridge SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
-TARGET_INCLUDE_DIRECTORIES(ros_bridge PUBLIC $<INSTALL_INTERFACE:include>)
-TARGET_LINK_LIBRARIES(ros_bridge ${catkin_LIBRARIES}
- sot-core::sot-core pinocchio::pinocchio)
-
-IF(SUFFIX_SO_VERSION)
- SET_TARGET_PROPERTIES(ros_bridge PROPERTIES SOVERSION ${PROJECT_VERSION})
-ENDIF(SUFFIX_SO_VERSION)
-
-IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
- INSTALL(TARGETS ros_bridge EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
-ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
-
-add_subdirectory(src)
-add_subdirectory(tests)
-
-#install ros executables
-install(PROGRAMS
- scripts/robot_pose_publisher
- scripts/run_command
- scripts/tf_publisher
- DESTINATION share/${PROJECT_NAME}
- )
+if(DEFINED ENV{ROS_VERSION} )
+ if ($ENV{ROS_VERSION} EQUAL 2) # if ROS2
+ add_subdirectory(ros2)
+ else()
+ add_subdirectory(ros1)
+ endif()
+else()
+ message(FATAL_ERROR "No ROS version found.")
+endif()
# Install package information
-install(FILES manifest.xml package.xml DESTINATION share/${PROJECT_NAME})
+install(FILES manifest.xml package.xml DESTINATION share/${PROJECT_NAME})
\ No newline at end of file
diff --git a/package.xml b/package.xml
index 7ad0d4d1ee47ea66f94d94bf2ce99810928a1b40..e0d5432ab52990daa70ca3ed78135a8bbc4b6cbb 100644
--- a/package.xml
+++ b/package.xml
@@ -1,10 +1,8 @@
-<package format="2">
+<package format="3">
<name>dynamic_graph_bridge</name>
<version>3.4.1</version>
<description>
-
ROS bindings for dynamic graph.
-
</description>
<maintainer email="guilhem.saurel@laas.fr">guilhem.saurel@laas.fr</maintainer>
<author email="hpp@laas.fr">hpp@laas.fr</author>
@@ -12,40 +10,58 @@
<license>LGPL</license>
<url>http://ros.org/wiki/dynamic_graph_bridge</url>
- <export>
- <rosdoc config="rosdoc.yaml" />
- </export>
+ <!-- ROS1 dependencies -->
+ <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
- <buildtool_depend>catkin</buildtool_depend>
+ <build_depend condition="$ROS_VERSION == 1">std_msgs</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">std_srvs</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">roscpp</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">geometry_msgs</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">sensor_msgs</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">tf2_ros</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">realtime_tools</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">message_generation</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">message_runtime</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">dynamic-graph</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">dynamic-graph-python</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">sot-core</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">sot-dynamic-pinocchio</build_depend>
+ <build_depend condition="$ROS_VERSION == 1">dynamic_graph_bridge_msgs</build_depend>
- <build_depend>std_msgs</build_depend>
- <build_depend>std_srvs</build_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>geometry_msgs</build_depend>
- <build_depend>sensor_msgs</build_depend>
- <build_depend>tf2_ros</build_depend>
- <build_depend>realtime_tools</build_depend>
- <build_depend>message_generation</build_depend>
- <build_depend>message_runtime</build_depend>
- <build_depend>dynamic-graph</build_depend>
- <build_depend>dynamic-graph-python</build_depend>
- <build_depend>sot-core</build_depend>
- <build_depend>sot-dynamic-pinocchio</build_depend>
- <build_depend>dynamic_graph_bridge_msgs</build_depend>
+ <exec_depend condition="$ROS_VERSION == 1">std_msgs</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">std_srvs</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">roscpp</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">geometry_msgs</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">sensor_msgs</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">tf2_ros</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">realtime_tools</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">message_generation</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">message_runtime</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">dynamic-graph</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">dynamic-graph-python</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">sot-core</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">sot-dynamic-pinocchio</exec_depend>
+ <exec_depend condition="$ROS_VERSION == 1">dynamic_graph_bridge_msgs</exec_depend>
- <exec_depend>std_msgs</exec_depend>
- <exec_depend>std_srvs</exec_depend>
- <exec_depend>roscpp</exec_depend>
- <exec_depend>geometry_msgs</exec_depend>
- <exec_depend>sensor_msgs</exec_depend>
- <exec_depend>tf2_ros</exec_depend>
- <exec_depend>realtime_tools</exec_depend>
- <exec_depend>message_generation</exec_depend>
- <exec_depend>message_runtime</exec_depend>
- <exec_depend>dynamic-graph</exec_depend>
- <exec_depend>dynamic-graph-python</exec_depend>
- <exec_depend>sot-core</exec_depend>
- <exec_depend>sot-dynamic-pinocchio</exec_depend>
- <exec_depend>dynamic_graph_bridge_msgs</exec_depend>
+ <!-- ROS2 dependencies -->
+ <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
+ <buildtool_depend condition="$ROS_VERSION == 2">ament_index_cpp</buildtool_depend>
+ <depend condition="$ROS_VERSION == 2"> rclcpp </depend>
+ <exec_depend condition="$ROS_VERSION == 2"> rclpy </exec_depend>
+ <depend condition="$ROS_VERSION == 2"> rcutils </depend>
+ <depend condition="$ROS_VERSION == 2"> std_msgs </depend>
+ <depend condition="$ROS_VERSION == 2"> std_srvs </depend>
+ <depend condition="$ROS_VERSION == 2"> geometry_msgs </depend>
+ <depend condition="$ROS_VERSION == 2"> sensor_msgs </depend>
+ <depend condition="$ROS_VERSION == 2"> tf2_ros </depend>
+ <depend condition="$ROS_VERSION == 2"> boost </depend>
+ <depend condition="$ROS_VERSION == 2"> dynamic_graph_bridge_msgs </depend>
+ <depend condition="$ROS_VERSION == 2"> sot-core </depend>
+
+ <export>
+ <rosdoc config="rosdoc.yaml" />
+ <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
+ </export>
+ <test_depend condition="$ROS_VERSION == 2">rclpy</test_depend>
</package>
diff --git a/ros1/CMakeLists.txt b/ros1/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..87041c6d1e2fe22aad88441fdac53c7c60d768c3
--- /dev/null
+++ b/ros1/CMakeLists.txt
@@ -0,0 +1,76 @@
+# Copyright (C) 2008-2020 LAAS-CNRS, JRL AIST-CNRS.
+#
+# Author: Florent Lamiraux, Nirmal Giftsun, Guilhem Saurel, Maxmilien Naveau
+#
+
+# Project dependencies
+SET(CATKIN_REQUIRED_COMPONENTS roscpp std_msgs message_generation std_srvs geometry_msgs sensor_msgs tf2_ros
+ realtime_tools)
+ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS program_options)
+ADD_PROJECT_DEPENDENCY(dynamic_graph_bridge_msgs 0.3.0 REQUIRED)
+
+IF(BUILD_PYTHON_INTERFACE)
+ FINDPYTHON()
+ SEARCH_FOR_BOOST_PYTHON()
+ STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
+ ADD_PROJECT_DEPENDENCY(dynamic-graph-python 4.0.0 REQUIRED)
+ SET(CATKIN_REQUIRED_COMPONENTS ${CATKIN_REQUIRED_COMPONENTS} rospy)
+
+ IF(Boost_VERSION GREATER 107299 OR Boost_VERSION_MACRO GREATER 107299)
+ # Silence a warning about a deprecated use of boost bind by boost >= 1.73
+ # without dropping support for boost < 1.73
+ ADD_DEFINITIONS(-DBOOST_BIND_GLOBAL_PLACEHOLDERS)
+ ENDIF()
+ENDIF(BUILD_PYTHON_INTERFACE)
+
+find_package(catkin REQUIRED COMPONENTS ${CATKIN_REQUIRED_COMPONENTS})
+
+ADD_PROJECT_DEPENDENCY(sot-core REQUIRED)
+
+# Main Library
+set(${PROJECT_NAME}_HEADERS
+ include/${PROJECT_NAME}/fwd.hh
+ include/${PROJECT_NAME}/ros_init.hh
+ include/${PROJECT_NAME}/sot_loader.hh
+ include/${PROJECT_NAME}/sot_loader_basic.hh
+ include/${PROJECT_NAME}/ros_interpreter.hh
+ src/converter.hh
+ src/sot_to_ros.hh
+ )
+
+SET(${PROJECT_NAME}_SOURCES
+ src/ros_init.cpp
+ src/sot_to_ros.cpp
+ src/ros_parameter.cpp
+ )
+
+ADD_LIBRARY(ros_bridge SHARED
+ ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
+TARGET_INCLUDE_DIRECTORIES(ros_bridge SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
+TARGET_INCLUDE_DIRECTORIES(ros_bridge PUBLIC $<INSTALL_INTERFACE:include>
+ PRIVATE ${PROJECT_SOURCE_DIR}/ros1/include)
+TARGET_LINK_LIBRARIES(ros_bridge
+ ${catkin_LIBRARIES}
+ sot-core::sot-core
+ pinocchio::pinocchio
+ dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs
+)
+IF(SUFFIX_SO_VERSION)
+ SET_TARGET_PROPERTIES(ros_bridge PROPERTIES SOVERSION ${PROJECT_VERSION})
+ENDIF(SUFFIX_SO_VERSION)
+
+IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ INSTALL(TARGETS ros_bridge EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
+ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+
+add_subdirectory(src)
+add_subdirectory(tests)
+
+#install ros executables
+install(PROGRAMS
+ scripts/robot_pose_publisher
+ scripts/run_command
+ scripts/tf_publisher
+ DESTINATION share/${PROJECT_NAME}
+ )
+
diff --git a/include/dynamic_graph_bridge/fwd.hh b/ros1/include/dynamic_graph_bridge/fwd.hh
similarity index 100%
rename from include/dynamic_graph_bridge/fwd.hh
rename to ros1/include/dynamic_graph_bridge/fwd.hh
diff --git a/include/dynamic_graph_bridge/ros_init.hh b/ros1/include/dynamic_graph_bridge/ros_init.hh
similarity index 100%
rename from include/dynamic_graph_bridge/ros_init.hh
rename to ros1/include/dynamic_graph_bridge/ros_init.hh
diff --git a/include/dynamic_graph_bridge/ros_interpreter.hh b/ros1/include/dynamic_graph_bridge/ros_interpreter.hh
similarity index 100%
rename from include/dynamic_graph_bridge/ros_interpreter.hh
rename to ros1/include/dynamic_graph_bridge/ros_interpreter.hh
diff --git a/include/dynamic_graph_bridge/ros_parameter.hh b/ros1/include/dynamic_graph_bridge/ros_parameter.hh
similarity index 100%
rename from include/dynamic_graph_bridge/ros_parameter.hh
rename to ros1/include/dynamic_graph_bridge/ros_parameter.hh
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/ros1/include/dynamic_graph_bridge/sot_loader.hh
similarity index 100%
rename from include/dynamic_graph_bridge/sot_loader.hh
rename to ros1/include/dynamic_graph_bridge/sot_loader.hh
diff --git a/include/dynamic_graph_bridge/sot_loader_basic.hh b/ros1/include/dynamic_graph_bridge/sot_loader_basic.hh
similarity index 100%
rename from include/dynamic_graph_bridge/sot_loader_basic.hh
rename to ros1/include/dynamic_graph_bridge/sot_loader_basic.hh
diff --git a/scripts/robot_pose_publisher b/ros1/scripts/robot_pose_publisher
similarity index 100%
rename from scripts/robot_pose_publisher
rename to ros1/scripts/robot_pose_publisher
diff --git a/scripts/run_command b/ros1/scripts/run_command
similarity index 100%
rename from scripts/run_command
rename to ros1/scripts/run_command
diff --git a/scripts/tf_publisher b/ros1/scripts/tf_publisher
similarity index 100%
rename from scripts/tf_publisher
rename to ros1/scripts/tf_publisher
diff --git a/ros1/src/CMakeLists.txt b/ros1/src/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e58c99d6d8936f107d5fc6c6cd70ed0a8c890dd1
--- /dev/null
+++ b/ros1/src/CMakeLists.txt
@@ -0,0 +1,198 @@
+#.rst:
+# .. command:: CUSTOM_DYNAMIC_GRAPH_PYTHON_MODULE ( SUBMODULENAME LIBRARYNAME TARGETNAME INSTALL_INIT_PY=1 SOURCE_PYTHON_MODULE=cmake/dynamic_graph/python-module-py.cc)
+#
+# Add a python submodule to dynamic_graph
+#
+# :param SUBMODULENAME: the name of the submodule (can be foo/bar),
+#
+# :param LIBRARYNAME: library to link the submodule with.
+#
+# :param TARGETNAME: name of the target: should be different for several
+# calls to the macro.
+#
+# :param INSTALL_INIT_PY: if set to 1 install and generated a __init__.py file.
+# Set to 1 by default.
+#
+# :param SOURCE_PYTHON_MODULE: Location of the cpp file for the python module in the package.
+# Set to cmake/dynamic_graph/python-module-py.cc by default.
+#
+# .. note::
+# Before calling this macro, set variable NEW_ENTITY_CLASS as
+# the list of new Entity types that you want to be bound.
+# Entity class name should match the name referencing the type
+# in the factory.
+#
+MACRO(CUSTOM_DYNAMIC_GRAPH_PYTHON_MODULE SUBMODULENAME LIBRARYNAME TARGETNAME)
+
+ set(options DONT_INSTALL_INIT_PY)
+ set(oneValueArgs SOURCE_PYTHON_MODULE MODULE_HEADER)
+ cmake_parse_arguments(ARG "${options}" "${oneValueArgs}"
+ "${multiValueArgs}" ${ARGN} )
+
+ # By default the __init__.py file is installed.
+ if(NOT DEFINED ARG_SOURCE_PYTHON_MODULE)
+ set(DYNAMICGRAPH_MODULE_HEADER ${ARG_MODULE_HEADER})
+ configure_file(
+ ${PROJECT_JRL_CMAKE_MODULE_DIR}/dynamic_graph/python-module-py.cc.in
+ ${PROJECT_BINARY_DIR}/ros1/src/dynamic_graph/${SUBMODULENAME}/python-module-py.cc
+ @ONLY
+ )
+ SET(ARG_SOURCE_PYTHON_MODULE "${PROJECT_BINARY_DIR}/ros1/src/dynamic_graph/${SUBMODULENAME}/python-module-py.cc")
+ endif()
+
+ IF(NOT DEFINED PYTHONLIBS_FOUND)
+ FINDPYTHON()
+ ELSEIF(NOT ${PYTHONLIBS_FOUND} STREQUAL "TRUE")
+ MESSAGE(FATAL_ERROR "Python has not been found.")
+ ENDIF()
+ if(NOT DEFINED Boost_PYTHON_LIBRARIES)
+ MESSAGE(FATAL_ERROR "Boost Python library must have been found to call this macro.")
+ endif()
+
+ SET(PYTHON_MODULE ${TARGETNAME})
+
+ ADD_LIBRARY(${PYTHON_MODULE}
+ MODULE
+ ${ARG_SOURCE_PYTHON_MODULE})
+
+ FILE(MAKE_DIRECTORY ${PROJECT_BINARY_DIR}/ros1/src/dynamic_graph/${SUBMODULENAME})
+
+ SET(PYTHON_INSTALL_DIR "${PYTHON_SITELIB}/dynamic_graph/${SUBMODULENAME}")
+ STRING(REGEX REPLACE "[^/]+" ".." PYTHON_INSTALL_DIR_REVERSE ${PYTHON_INSTALL_DIR})
+
+ SET_TARGET_PROPERTIES(${PYTHON_MODULE}
+ PROPERTIES PREFIX ""
+ OUTPUT_NAME dynamic_graph/${SUBMODULENAME}/wrap
+ BUILD_RPATH "${DYNAMIC_GRAPH_PLUGINDIR}:\$ORIGIN/${PYTHON_INSTALL_DIR_REVERSE}/${DYNAMIC_GRAPH_PLUGINDIR}"
+ )
+
+ IF (UNIX AND NOT APPLE)
+ TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD} "-Wl,--no-as-needed")
+ ENDIF(UNIX AND NOT APPLE)
+ TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD} ${LIBRARYNAME} ${PYTHON_LIBRARY} dynamic-graph::dynamic-graph)
+ TARGET_LINK_BOOST_PYTHON(${PYTHON_MODULE} ${PUBLIC_KEYWORD})
+ if(PROJECT_NAME STREQUAL "dynamic-graph-python")
+ TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD} dynamic-graph-python)
+ else()
+ TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD} dynamic-graph-python::dynamic-graph-python)
+ endif()
+
+ TARGET_INCLUDE_DIRECTORIES(${PYTHON_MODULE} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS})
+
+ #
+ # Installation
+ #
+
+ INSTALL(TARGETS ${PYTHON_MODULE}
+ DESTINATION
+ ${PYTHON_INSTALL_DIR})
+
+ SET(ENTITY_CLASS_LIST "")
+ FOREACH (ENTITY ${NEW_ENTITY_CLASS})
+ SET(ENTITY_CLASS_LIST "${ENTITY_CLASS_LIST}${ENTITY}('')\n")
+ ENDFOREACH(ENTITY ${NEW_ENTITY_CLASS})
+
+ # Install if not DONT_INSTALL_INIT_PY
+ if(NOT DONT_INSTALL_INIT_PY)
+
+ CONFIGURE_FILE(
+ ${PROJECT_JRL_CMAKE_MODULE_DIR}/dynamic_graph/submodule/__init__.py.cmake
+ ${PROJECT_BINARY_DIR}/ros1/src/dynamic_graph/${SUBMODULENAME}/__init__.py
+ )
+
+ INSTALL(
+ FILES ${PROJECT_BINARY_DIR}/ros1/src/dynamic_graph/${SUBMODULENAME}/__init__.py
+ DESTINATION ${PYTHON_INSTALL_DIR}
+ )
+
+ endif()
+
+ENDMACRO(CUSTOM_DYNAMIC_GRAPH_PYTHON_MODULE SUBMODULENAME)
+
+
+
+
+
+
+
+
+SET(plugins
+ ros_publish
+ ros_subscribe
+ ros_queued_subscribe
+ ros_tf_listener
+ ros_time
+ )
+
+FOREACH(plugin ${plugins})
+ GET_FILENAME_COMPONENT(LIBRARY_NAME ${plugin} NAME)
+ ADD_LIBRARY(${LIBRARY_NAME} SHARED ${plugin}.cpp ${plugin}.hh)
+
+ IF(SUFFIX_SO_VERSION)
+ SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
+ ENDIF(SUFFIX_SO_VERSION)
+
+ TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${${LIBRARY_NAME}_deps} ${catkin_LIBRARIES} ros_bridge
+ dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
+
+ TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME} PUBLIC $<INSTALL_INTERFACE:include>
+ PRIVATE ${PROJECT_SOURCE_DIR}/ros1/include)
+
+ IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ INSTALL(TARGETS ${LIBRARY_NAME} EXPORT ${TARGETS_EXPORT_NAME}
+ DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
+ ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+
+ IF(BUILD_PYTHON_INTERFACE)
+ STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
+ if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
+ CUSTOM_DYNAMIC_GRAPH_PYTHON_MODULE("ros/${PYTHON_LIBRARY_NAME}"
+ ${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
+ SOURCE_PYTHON_MODULE "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
+ elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
+ CUSTOM_DYNAMIC_GRAPH_PYTHON_MODULE("ros/${PYTHON_LIBRARY_NAME}"
+ ${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
+ MODULE_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
+ endif()
+ TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
+ PRIVATE ${PROJECT_SOURCE_DIR}/ros1/include)
+ ENDIF(BUILD_PYTHON_INTERFACE)
+ENDFOREACH(plugin)
+
+target_link_libraries(ros_publish ros_bridge)
+
+IF(BUILD_PYTHON_INTERFACE)
+ PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "__init__.py")
+ PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "ros.py")
+ PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "dgcompleter.py")
+
+ # ros_interperter library.
+ add_library(ros_interpreter ros_interpreter.cpp)
+ TARGET_LINK_LIBRARIES(ros_interpreter ros_bridge ${catkin_LIBRARIES}
+ dynamic-graph-python::dynamic-graph-python
+ dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
+ TARGET_INCLUDE_DIRECTORIES(ros_interpreter PUBLIC $<INSTALL_INTERFACE:include>
+ PRIVATE ${PROJECT_SOURCE_DIR}/ros1/include)
+
+
+ install(TARGETS ros_interpreter
+ EXPORT ${TARGETS_EXPORT_NAME}
+ DESTINATION lib)
+ENDIF(BUILD_PYTHON_INTERFACE)
+
+# Stand alone embedded intepreter with a robot controller.
+add_executable(geometric_simu geometric_simu.cpp sot_loader.cpp sot_loader_basic.cpp)
+TARGET_INCLUDE_DIRECTORIES(geometric_simu PUBLIC $<INSTALL_INTERFACE:include>
+ PRIVATE ${PROJECT_SOURCE_DIR}/ros1/include)
+target_link_libraries(geometric_simu Boost::program_options ${CMAKE_DL_LIBS} ${catkin_LIBRARIES} ros_bridge
+ dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
+install(TARGETS geometric_simu
+ DESTINATION lib/${PROJECT_NAME})
+
+# Sot loader library
+add_library(sot_loader sot_loader.cpp sot_loader_basic.cpp)
+TARGET_INCLUDE_DIRECTORIES(sot_loader PUBLIC $<INSTALL_INTERFACE:include>
+ PRIVATE ${PROJECT_SOURCE_DIR}/ros1/include)
+target_link_libraries(sot_loader Boost::program_options ${catkin_LIBRARIES} ros_bridge
+ dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
+install(TARGETS sot_loader EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
diff --git a/src/converter.hh b/ros1/src/converter.hh
similarity index 100%
rename from src/converter.hh
rename to ros1/src/converter.hh
diff --git a/src/dynamic_graph/ros/__init__.py b/ros1/src/dynamic_graph/ros/__init__.py
similarity index 100%
rename from src/dynamic_graph/ros/__init__.py
rename to ros1/src/dynamic_graph/ros/__init__.py
diff --git a/src/dynamic_graph/ros/dgcompleter.py b/ros1/src/dynamic_graph/ros/dgcompleter.py
similarity index 100%
rename from src/dynamic_graph/ros/dgcompleter.py
rename to ros1/src/dynamic_graph/ros/dgcompleter.py
diff --git a/src/dynamic_graph/ros/ros.py b/ros1/src/dynamic_graph/ros/ros.py
similarity index 100%
rename from src/dynamic_graph/ros/ros.py
rename to ros1/src/dynamic_graph/ros/ros.py
diff --git a/src/geometric_simu.cpp b/ros1/src/geometric_simu.cpp
similarity index 100%
rename from src/geometric_simu.cpp
rename to ros1/src/geometric_simu.cpp
diff --git a/src/ros_init.cpp b/ros1/src/ros_init.cpp
similarity index 100%
rename from src/ros_init.cpp
rename to ros1/src/ros_init.cpp
diff --git a/src/ros_interpreter.cpp b/ros1/src/ros_interpreter.cpp
similarity index 100%
rename from src/ros_interpreter.cpp
rename to ros1/src/ros_interpreter.cpp
diff --git a/src/ros_parameter.cpp b/ros1/src/ros_parameter.cpp
similarity index 100%
rename from src/ros_parameter.cpp
rename to ros1/src/ros_parameter.cpp
diff --git a/src/ros_publish-python-module-py.cc b/ros1/src/ros_publish-python-module-py.cc
similarity index 100%
rename from src/ros_publish-python-module-py.cc
rename to ros1/src/ros_publish-python-module-py.cc
diff --git a/src/ros_publish.cpp b/ros1/src/ros_publish.cpp
similarity index 100%
rename from src/ros_publish.cpp
rename to ros1/src/ros_publish.cpp
diff --git a/src/ros_publish.hh b/ros1/src/ros_publish.hh
similarity index 100%
rename from src/ros_publish.hh
rename to ros1/src/ros_publish.hh
diff --git a/src/ros_publish.hxx b/ros1/src/ros_publish.hxx
similarity index 100%
rename from src/ros_publish.hxx
rename to ros1/src/ros_publish.hxx
diff --git a/src/ros_queued_subscribe-python-module-py.cc b/ros1/src/ros_queued_subscribe-python-module-py.cc
similarity index 100%
rename from src/ros_queued_subscribe-python-module-py.cc
rename to ros1/src/ros_queued_subscribe-python-module-py.cc
diff --git a/src/ros_queued_subscribe.cpp b/ros1/src/ros_queued_subscribe.cpp
similarity index 100%
rename from src/ros_queued_subscribe.cpp
rename to ros1/src/ros_queued_subscribe.cpp
diff --git a/src/ros_queued_subscribe.hh b/ros1/src/ros_queued_subscribe.hh
similarity index 100%
rename from src/ros_queued_subscribe.hh
rename to ros1/src/ros_queued_subscribe.hh
diff --git a/src/ros_queued_subscribe.hxx b/ros1/src/ros_queued_subscribe.hxx
similarity index 100%
rename from src/ros_queued_subscribe.hxx
rename to ros1/src/ros_queued_subscribe.hxx
diff --git a/src/ros_subscribe-python-module-py.cc b/ros1/src/ros_subscribe-python-module-py.cc
similarity index 100%
rename from src/ros_subscribe-python-module-py.cc
rename to ros1/src/ros_subscribe-python-module-py.cc
diff --git a/src/ros_subscribe.cpp b/ros1/src/ros_subscribe.cpp
similarity index 100%
rename from src/ros_subscribe.cpp
rename to ros1/src/ros_subscribe.cpp
diff --git a/src/ros_subscribe.hh b/ros1/src/ros_subscribe.hh
similarity index 100%
rename from src/ros_subscribe.hh
rename to ros1/src/ros_subscribe.hh
diff --git a/src/ros_subscribe.hxx b/ros1/src/ros_subscribe.hxx
similarity index 100%
rename from src/ros_subscribe.hxx
rename to ros1/src/ros_subscribe.hxx
diff --git a/src/ros_tf_listener-python-module-py.cc b/ros1/src/ros_tf_listener-python-module-py.cc
similarity index 100%
rename from src/ros_tf_listener-python-module-py.cc
rename to ros1/src/ros_tf_listener-python-module-py.cc
diff --git a/src/ros_tf_listener.cpp b/ros1/src/ros_tf_listener.cpp
similarity index 100%
rename from src/ros_tf_listener.cpp
rename to ros1/src/ros_tf_listener.cpp
diff --git a/src/ros_tf_listener.hh b/ros1/src/ros_tf_listener.hh
similarity index 100%
rename from src/ros_tf_listener.hh
rename to ros1/src/ros_tf_listener.hh
diff --git a/src/ros_time-python.hh b/ros1/src/ros_time-python.hh
similarity index 100%
rename from src/ros_time-python.hh
rename to ros1/src/ros_time-python.hh
diff --git a/src/ros_time.cpp b/ros1/src/ros_time.cpp
similarity index 100%
rename from src/ros_time.cpp
rename to ros1/src/ros_time.cpp
diff --git a/src/ros_time.hh b/ros1/src/ros_time.hh
similarity index 100%
rename from src/ros_time.hh
rename to ros1/src/ros_time.hh
diff --git a/src/sot_loader.cpp b/ros1/src/sot_loader.cpp
similarity index 100%
rename from src/sot_loader.cpp
rename to ros1/src/sot_loader.cpp
diff --git a/src/sot_loader_basic.cpp b/ros1/src/sot_loader_basic.cpp
similarity index 100%
rename from src/sot_loader_basic.cpp
rename to ros1/src/sot_loader_basic.cpp
diff --git a/src/sot_to_ros.cpp b/ros1/src/sot_to_ros.cpp
similarity index 100%
rename from src/sot_to_ros.cpp
rename to ros1/src/sot_to_ros.cpp
diff --git a/src/sot_to_ros.hh b/ros1/src/sot_to_ros.hh
similarity index 100%
rename from src/sot_to_ros.hh
rename to ros1/src/sot_to_ros.hh
diff --git a/tests/CMakeLists.txt b/ros1/tests/CMakeLists.txt
similarity index 100%
rename from tests/CMakeLists.txt
rename to ros1/tests/CMakeLists.txt
diff --git a/tests/test_import.py b/ros1/tests/test_import.py
similarity index 100%
rename from tests/test_import.py
rename to ros1/tests/test_import.py
diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6e0fc7520e783ab30b8dbfd4da379fdc6f20d14d
--- /dev/null
+++ b/ros2/CMakeLists.txt
@@ -0,0 +1,54 @@
+# Copyright (C) 2021 LAAS-CNRS.
+#
+# Author: Maxmilien Naveau
+#
+
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 14)
+endif()
+
+cmake_policy(SET CMP0057 NEW)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(ament_cmake_core REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+add_project_dependency(Boost REQUIRED COMPONENTS program_options)
+add_project_dependency(dynamic_graph_bridge_msgs 0.3.0 REQUIRED)
+add_project_dependency(sot-core REQUIRED)
+
+IF(BUILD_PYTHON_INTERFACE)
+ findpython()
+ search_for_boost_python()
+ string(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
+ add_project_dependency(dynamic-graph-python 4.0.0 REQUIRED)
+
+ IF(Boost_VERSION GREATER 107299 OR Boost_VERSION_MACRO GREATER 107299)
+ # Silence a warning about a deprecated use of boost bind by boost >= 1.73
+ # without dropping support for boost < 1.73
+ add_definitions(-DBOOST_BIND_GLOBAL_PLACEHOLDERS)
+ ENDIF()
+ENDIF(BUILD_PYTHON_INTERFACE)
+
+add_subdirectory(src)
+# add_subdirectory(tests)
+
+#install ros executables
+install(PROGRAMS
+ scripts/robot_pose_publisher
+ scripts/run_command
+ scripts/tf_publisher
+ DESTINATION share/${PROJECT_NAME}
+ )
+
+# Install package information
+install(FILES package.xml DESTINATION share/${PROJECT_NAME})
+
+#ROS 2 packaging
+ament_package()
diff --git a/ros2/include/dynamic_graph_bridge/ros.hpp b/ros2/include/dynamic_graph_bridge/ros.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d173ffc7aefbf4686d2de5355443c01477f7a3bc
--- /dev/null
+++ b/ros2/include/dynamic_graph_bridge/ros.hpp
@@ -0,0 +1,126 @@
+/**
+ * @file
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ */
+
+#pragma once
+
+#include <memory>
+
+// ROS includes
+#include "dynamic_graph_bridge_msgs/srv/run_python_command.hpp"
+#include "dynamic_graph_bridge_msgs/srv/run_python_file.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_srvs/srv/empty.hpp"
+
+namespace dynamic_graph_bridge
+{
+const std::string DG_ROS_NODE_NAME = "dynamic_graph";
+const std::string HWC_ROS_NODE_NAME = "hardware_communication";
+
+/*
+ * A Bunch of convenient shortcuts.
+ */
+typedef rclcpp::Node RosNode;
+typedef std::shared_ptr<RosNode> RosNodePtr;
+typedef rclcpp::executors::MultiThreadedExecutor RosExecutor;
+typedef std::shared_ptr<RosExecutor> RosExecutorPtr;
+
+// Python commands shortcuts
+typedef dynamic_graph_bridge_msgs::srv::RunPythonCommand RunPythonCommandSrvType;
+typedef rclcpp::Service<RunPythonCommandSrvType>::SharedPtr
+ RunPythonCommandServerPtr;
+typedef rclcpp::Client<RunPythonCommandSrvType>::SharedPtr
+ RunPythonCommandClientPtr;
+typedef RunPythonCommandSrvType::Request::SharedPtr RunPythonCommandRequestPtr;
+typedef std::shared_ptr<RunPythonCommandSrvType::Response>
+ RunPythonCommandResponsePtr;
+typedef std::shared_future<std::shared_ptr<RunPythonCommandSrvType::Response> >
+ RunPythonCommandResponseFuturePtr;
+
+// Parse python file shortcuts.
+typedef dynamic_graph_bridge_msgs::srv::RunPythonFile RunPythonFileSrvType;
+typedef rclcpp::Service<RunPythonFileSrvType>::SharedPtr RunPythonFileServerPtr;
+typedef rclcpp::Client<RunPythonFileSrvType>::SharedPtr RunPythonFileClientPtr;
+typedef std::shared_ptr<RunPythonFileSrvType::Request> RunPythonFileRequestPtr;
+typedef std::shared_ptr<RunPythonFileSrvType::Response>
+ RunPythonFileResponsePtr;
+typedef std::shared_future<std::shared_ptr<RunPythonFileSrvType::Response> >
+ RunPythonFileResponseFuturePtr;
+
+// Shortcut for chrono constant handling.
+typedef std::chrono::duration<int64_t, std::ratio<1, 1> > DurationSec;
+
+// Used to save the hardware service pointers.
+typedef std::shared_ptr<rclcpp::ServiceBase> ServiceBasePtr;
+typedef rclcpp::Service<std_srvs::srv::Empty>::SharedPtr EmptyServicePtr;
+typedef std_srvs::srv::Empty EmptyServiceType;
+
+/**
+ * @brief rosInit is a global method that uses the structure GlobalRos.
+ * Its role is to create the ros::nodeHandle and the ros::spinner once at the
+ * first call. It always returns a reference to the node hanlde.
+ * @return the reference of GLOBAL_ROS_VAR.node_.
+ */
+RosNodePtr get_ros_node(std::string node_name);
+
+/**
+ * @brief Add a ros node to the global executor.
+ *
+ * @param node_name
+ */
+void ros_add_node_to_executor(const std::string& node_name);
+
+/**
+ * @brief Checks if the Node has been created.
+ *
+ * @param node_name
+ * @return true
+ * @return false
+ */
+bool ros_node_exists(std::string node_name);
+
+/**
+ * @brief Allow all the created ROS node to perform their callbacks.
+ * Warning: blocking function.
+ */
+void ros_spin();
+
+/**
+ * @brief Allow all the created ROS node to perform their callbacks.
+ */
+void ros_spin_non_blocking();
+
+/**
+ * @brief Stop the ros spinning.
+ */
+void ros_stop_spinning();
+
+/**
+ * @brief ros_shutdown shuts down ros and stop the ros spinner with the
+ * associate name
+ */
+void ros_shutdown(std::string node_name);
+
+/**
+ * @brief ros_shutdown shuts down ros and stop the ros spinners
+ */
+void ros_shutdown();
+
+/**
+ * @brief Remove all node from the executor and delete it.
+ */
+void ros_clean();
+
+/**
+ * @brief Check if ROS is fine.
+ *
+ * @param node_name
+ * @return true
+ * @return false
+ */
+bool ros_ok();
+
+} // end of namespace dynamic_graph_bridge.
diff --git a/ros2/include/dynamic_graph_bridge/ros_parameter.hpp b/ros2/include/dynamic_graph_bridge/ros_parameter.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..688850ce0353159a4c60f1f75cf215e4b07362d8
--- /dev/null
+++ b/ros2/include/dynamic_graph_bridge/ros_parameter.hpp
@@ -0,0 +1,10 @@
+#ifndef _ROS_DYNAMIC_GRAPH_PARAMETER_
+#define _ROS_DYNAMIC_GRAPH_PARAMETER_
+
+#include "rclcpp/node.hpp"
+namespace dynamicgraph {
+
+ bool parameter_server_read_robot_description(rclcpp::Node::SharedPtr nh);
+
+}
+#endif /* _ROS_DYNAMIC_GRAPH_PARAMETER_ */
diff --git a/ros2/include/dynamic_graph_bridge/ros_python_interpreter_client.hpp b/ros2/include/dynamic_graph_bridge/ros_python_interpreter_client.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa0598605efd60885be10ba2a9abcac33e1ec3a2
--- /dev/null
+++ b/ros2/include/dynamic_graph_bridge/ros_python_interpreter_client.hpp
@@ -0,0 +1,152 @@
+/**
+ * @file
+ * @license BSD 3-clause
+ * @copyright Copyright (c) 2020, New York University and Max Planck
+ * Gesellschaft
+ *
+ * @brief Defines a C++ interface for the ros-service of the dynamic-graph
+ * python interpreter. (Available in python through bindings.)
+ */
+
+#pragma once
+
+#include "dynamic_graph_bridge/ros.hpp"
+
+namespace dynamic_graph_bridge
+{
+/**
+ * @brief Client of the RosPythonInterpreterServer through rosservices.
+ */
+class RosPythonInterpreterClient
+{
+public:
+ /**
+ * @brief Construct a new RosPythonInterpreterClient object.
+ */
+ RosPythonInterpreterClient();
+
+ /**
+ * @brief Destroy the RosPythonInterpreterClient object.
+ */
+ ~RosPythonInterpreterClient()
+ {
+ }
+
+ /**
+ * @brief Call the rosservice of the current running DynamicGraphManager.
+ *
+ * @param code_string To be executed in the embeded python interpreter.
+ * @return std::string
+ */
+ std::string run_python_command(const std::string& code_string);
+
+ /**
+ * @brief Call the rosservice of the current running DynamicGraphManager.
+ *
+ * @param filename
+ */
+ std::string run_python_script(const std::string& filename);
+
+private:
+ /**
+ * @brief Connect to a designated service.
+ *
+ * @tparam RosServiceType
+ * @param service_name
+ * @param client
+ * @param timeout
+ */
+ template <typename RosServiceType>
+ typename rclcpp::Client<RosServiceType>::SharedPtr connect_to_rosservice(
+ const std::string& service_name, const DurationSec& timeout)
+ {
+ typename rclcpp::Client<RosServiceType>::SharedPtr client;
+ try
+ {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Waiting for service %s ...",
+ service_name.c_str());
+ // let use wait for the existance of the services
+ client =
+ ros_node_->create_client<RosServiceType>(service_name.c_str());
+ if (client->wait_for_service(timeout))
+ {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Successfully connected to %s",
+ service_name.c_str());
+ }
+ else
+ {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Failed to connect to %s",
+ service_name.c_str());
+ }
+ }
+ catch (...)
+ {
+ throw std::runtime_error(service_name + " not available.");
+ }
+ return client;
+ }
+
+ /**
+ * @brief Connects to the RunCommand rosservice.
+ *
+ * @param timeout [in]
+ */
+ void connect_to_rosservice_run_python_command(
+ const DurationSec& timeout = DurationSec(-1))
+ {
+ command_client_ = connect_to_rosservice<RunPythonCommandSrvType>(
+ run_command_service_name_, timeout);
+ }
+
+ /**
+ * @brief Connects to the RunPythonFile rosservice.
+ *
+ * @param timeout [in]
+ */
+ void connect_to_rosservice_run_python_script(
+ const DurationSec& timeout = DurationSec(-1))
+ {
+ script_client_ = connect_to_rosservice<RunPythonFileSrvType>(
+ run_script_service_name_, timeout);
+ }
+
+private:
+ /**
+ * @brief Name of the local ros_node;
+ */
+ std::string ros_node_name_;
+
+ /** @brief Handle for manipulating ROS objects. */
+ RosNodePtr ros_node_;
+
+ /** @brief Name of the DynamicGraphManager RosPythonInterpreterServer
+ * rosservice for running a python command. */
+ std::string run_command_service_name_;
+
+ /** @brief Rosservice to run a python command.
+ * @see run_command_service_name_ */
+ RunPythonCommandClientPtr command_client_;
+
+ /** @brief Input of the rosservice. */
+ RunPythonCommandRequestPtr run_command_request_;
+
+ /** @brief Name of the DynamicGraphManager RosPythonInterpreterServer
+ * rosservice for running a python script. */
+ std::string run_script_service_name_;
+
+ /** @brief Rosservice to run a python script.
+ * @see run_command_service_name_ */
+ RunPythonFileClientPtr script_client_;
+
+ /** @brief Input of the rosservice. */
+ RunPythonFileRequestPtr run_file_request_;
+
+ /** @brief timeout used during the connection to the rosservices in
+ * seconds. */
+ DurationSec timeout_connection_s_;
+};
+
+} // namespace dynamic_graph_bridge
\ No newline at end of file
diff --git a/ros2/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp b/ros2/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..378665b04fbd27671815c1ad97ed67a2f67afe00
--- /dev/null
+++ b/ros2/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
@@ -0,0 +1,154 @@
+/**
+ * @file ros_interpreter.hpp
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ * @date 2019-05-22
+ *
+ * This file declares a ros bridge on top of Python interpretor. It is used
+ * an asynchronuous communcation between the user and the controller.
+ */
+
+#ifndef DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
+#define DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
+
+#include "dynamic-graph/python/interpreter.hh"
+#include "dynamic_graph_bridge/ros.hpp"
+
+namespace dynamic_graph_bridge
+{
+/// \brief This class wraps the implementation of the runCommand
+/// service.
+///
+/// This takes as input a ROS node handle and do not handle the
+/// callback so that the service behavior can be controlled from
+/// the outside.
+class RosPythonInterpreterServer
+{
+public:
+ /**
+ * @brief run_python_command_callback_t define a std::function to be used
+ * as callback to the rclcpp::service.
+ *
+ * The first argument of "runCommandCallback"
+ * (const std::string & command) is bound to
+ * (dynamic_graph_manager::srv::RunPythonCommand::Request).
+ * And the second argument (std::string &result) is bound to
+ * (dynamic_graph_manager::srv::RunPythonCommand::Response)
+ */
+ typedef std::function<void(RunPythonCommandRequestPtr,
+ RunPythonCommandResponsePtr)>
+ run_python_command_callback_t;
+
+ /**
+ * @brief run_python_file_callback_t define a std::function to be used as
+ * callback to the rclcpp::service.
+ *
+ * The first argument of "runPythonFileCallback"
+ * (std::string ifilename) is bound to
+ * (dynamic_graph_manager::srv::RunPythonCommand::Request).
+ * And a fake second argument is simulated in
+ * (dynamic_graph_manager::srv::RunPythonCommand::Response)
+ */
+ typedef std::function<void(RunPythonFileRequestPtr,
+ RunPythonFileResponsePtr)>
+ run_python_file_callback_t;
+
+ /**
+ * @brief RosPythonInterpreterServer is the unique constructor of the class
+ * @param node_handle is the RosNode used to advertize the
+ * rclcpp::services
+ */
+ explicit RosPythonInterpreterServer();
+
+ /**
+ * @brief ~RosPythonInterpreterServer is the default destructor of the class
+ */
+ ~RosPythonInterpreterServer();
+
+ /**
+ * @brief run_python_command used the python interpreter to execute the
+ * input command
+ * @param[in] command is the user input string command.
+ * @param[out] result is the numerical result of the operation done.
+ * @param[out] out is the string representation of the results.
+ * @param[out] err is the output error string from python.
+ */
+ void run_python_command(const std::string& command,
+ std::string& result,
+ std::string& out,
+ std::string& err);
+
+ /**
+ * @brief run_python_file executes the input scripts in the python
+ * interpreter
+ * @param ifilename is the path to the script to execute
+ */
+ void run_python_file(const std::string ifilename,
+ std::string& standarderror);
+
+ /**
+ * @brief start_ros_service advertize the "run_python_command" and
+ * "run_python_scripts" ros services
+ */
+ void start_ros_service();
+
+protected:
+ /**
+ * @brief runCommandCallback is the "run_python_command" ros service
+ * callback function.
+ * @param req is the request. it is defined as a string in the
+ * RunCommand.msg
+ * @param res is the request. it is defined as a string in the
+ * RunCommand.msg
+ * @return true
+ */
+ void runCommandCallback(RunPythonCommandRequestPtr req,
+ RunPythonCommandResponsePtr res);
+
+ /**
+ * @brief runCommandCallback is the "run_python_script" ros service callback
+ * function.
+ * @param req is the request. it is defined as a string in the
+ * RunCommand.msg
+ * @param res is the request. it is defined as a string in the
+ * RunCommand.msg
+ * @return true
+ */
+ void runPythonFileCallback(RunPythonFileRequestPtr req,
+ RunPythonFileResponsePtr res);
+
+private:
+ /**
+ * @brief interpreter_ is the python interpreter itself
+ */
+ dynamicgraph::python::Interpreter interpreter_;
+ /**
+ * @brief ros_node_ is reference to the RosNode used to
+ * advertize the rclcpp::services
+ */
+ RosNodePtr ros_node_;
+ /**
+ * @brief run_python_command_srv_ is the "run_python_command"
+ * rclcpp::service c++ object
+ *
+ * This kind of ros object require *NOT* to be destroyed. otherwize the
+ * rclcpp::service is cancelled. This is the reason why this object is an
+ * attribute of the class
+ */
+ RunPythonCommandServerPtr run_python_command_srv_;
+ /**
+ * @brief run_python_file_srv_ is the "run_python_script" rclcpp::service
+ * c++ object
+ *
+ * This kind of ros object require *NOT* to be destroyed. otherwize the
+ * rclcpp::service is cancelled. This is the reason why this object is an
+ * attribute of the class
+ */
+ RunPythonFileServerPtr run_python_file_srv_;
+};
+
+} // namespace dynamic_graph_bridge
+
+#endif //! DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
diff --git a/ros2/include/dynamic_graph_bridge/ros_sot_loader.hpp b/ros2/include/dynamic_graph_bridge/ros_sot_loader.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2/scripts/remote_python_client.py b/ros2/scripts/remote_python_client.py
new file mode 100644
index 0000000000000000000000000000000000000000..d756d5f00cf8abc8c0007b421872392c31c859f7
--- /dev/null
+++ b/ros2/scripts/remote_python_client.py
@@ -0,0 +1,156 @@
+#!/usr/bin/env python
+
+"""@package dynamic_graph_manager
+
+@file
+@license License BSD-3-Clause
+@copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
+
+@brief Instanciate a python terminal communication with the dynamic-graph
+embeded terminal.
+
+"""
+
+# Standard import.
+import optparse
+import os.path
+import code
+import os
+import sys
+import readline
+import atexit
+import signal
+import rclpy
+
+# Used to connect to ROS services
+from dynamic_graph_manager.ros.dgcompleter import DGCompleter
+from dynamic_graph_manager_cpp_bindings import RosPythonInterpreterClient
+
+
+def signal_handler(sig, frame):
+ """
+ Catch Ctrl+C and quit.
+ """
+ print("")
+ print("You pressed Ctrl+C! Closing ros client and shell.")
+ rclpy.try_shutdown()
+ sys.exit(0)
+
+
+# Command history, auto-completetion and keyboard management
+python_history = os.path.join(os.environ["HOME"], ".dg_python_history")
+readline.parse_and_bind("tab: complete")
+readline.set_history_length(100000)
+
+
+def save_history(histfile):
+ """ Write the history of the user command in a file """
+ readline.write_history_file(histfile)
+
+
+"""
+Read the current history if it exists and program its save upon the program end.
+"""
+if hasattr(readline, "read_history_file"):
+ try:
+ readline.read_history_file(python_history)
+ except IOError:
+ pass
+ atexit.register(save_history, python_history)
+
+
+class DynamicGraphInteractiveConsole(code.InteractiveConsole):
+ """
+ For the subtilities please read https://docs.python.org/3/library/code.html
+ """
+
+ def __init__(self):
+
+ # Create the python terminal
+ code.InteractiveConsole.__init__(self)
+
+ # Command lines from the terminal.
+ self.lines_pushed = ""
+
+ self.ros_python_interpreter = RosPythonInterpreterClient()
+ if sys.version[:2].startswith("2."):
+ self.dg_completer = DGCompleter(self.ros_python_interpreter)
+ readline.set_completer(self.dg_completer.complete)
+
+ def runcode(self, code):
+ """
+ Inherited from code.InteractiveConsole
+
+ We execute the code pushed in the cache `self.lines_pushed`. The code is
+ pushed whenever the user press enter during the interactive session.
+ see https://docs.python.org/3/library/code.html
+ """
+ try:
+ # we copy the line in a tmp var
+ code_string = self.lines_pushed[:-1]
+ result = self.ros_python_interpreter.run_python_command(
+ code_string
+ )
+ self.write(result)
+ if not result.endswith("\n"):
+ self.write("\n")
+ # we reset the cache here
+ self.lines_pushed = ""
+ except Exception as e:
+ self.write(str(e))
+ return False
+
+ def runsource(self, source, filename="<input>", symbol="single"):
+ """
+ Inherited from code.InteractiveConsole
+
+ see https://docs.python.org/3/library/code.html
+ """
+ try:
+ c = code.compile_command(source, filename, symbol)
+ if c:
+ return self.runcode(c)
+ else:
+ return True
+ except SyntaxError:
+ self.showsyntaxerror()
+ self.lines_pushed = ""
+ return False
+ except OverflowError:
+ self.showsyntaxerror()
+ self.write("OverflowError")
+ self.lines_pushed = ""
+ return False
+
+ def push(self, line):
+ """
+ Upon pressing enter in the interactive shell the user "push" a string.
+ This method is then called with the string pushed.
+ We catch the string to send it via the rosservice.
+ """
+ self.lines_pushed += line + "\n"
+ return code.InteractiveConsole.push(self, line)
+
+
+if __name__ == "__main__":
+ rclpy.init()
+
+ parser = optparse.OptionParser(usage="\n\t%prog [options]")
+ (options, args) = parser.parse_args(sys.argv[1:])
+
+ dg_console = DynamicGraphInteractiveConsole()
+
+ if args[:]:
+ infile = args[0]
+ response = dg_console.ros_python_interpreter.run_python_script(
+ os.path.abspath(infile)
+ )
+ print(
+ dg_console.ros_python_interpreter.run_python_command(
+ "print('File parsed')"
+ )
+ )
+
+ signal.signal(signal.SIGINT, signal_handler)
+ dg_console.interact("Interacting with remote python server.")
+ rclpy.shutdown()
diff --git a/ros2/src/CMakeLists.txt b/ros2/src/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..93229f7cae70b2832577c32d8327e218106b1ac7
--- /dev/null
+++ b/ros2/src/CMakeLists.txt
@@ -0,0 +1,114 @@
+# Copyright (C) 2021 LAAS-CNRS.
+#
+# Author: Maxmilien Naveau
+#
+
+# Main Library
+set(${PROJECT_NAME}_HEADERS
+ ${PROJECT_SOURCE_DIR}/ros2/include/${PROJECT_NAME}/ros_parameter.hpp
+ ${PROJECT_SOURCE_DIR}/ros2/include/${PROJECT_NAME}/ros_python_interpreter_client.hpp
+ ${PROJECT_SOURCE_DIR}/ros2/include/${PROJECT_NAME}/ros_python_interpreter_server.hpp
+ ${PROJECT_SOURCE_DIR}/ros2/src/dg_ros_mapping.hpp
+ ${PROJECT_SOURCE_DIR}/ros2/src/matrix_geometry.hpp
+ ${PROJECT_SOURCE_DIR}/ros2/src/time_point_io.hpp)
+set(${PROJECT_NAME}_SOURCES
+ ${PROJECT_SOURCE_DIR}/ros2/src/dg_ros_mapping.cpp #
+ ${PROJECT_SOURCE_DIR}/ros2/src/ros_python_interpreter_client.cpp #
+ ${PROJECT_SOURCE_DIR}/ros2/src/ros_python_interpreter_server.cpp #
+ ${PROJECT_SOURCE_DIR}/ros2/src/ros.cpp #
+ ${PROJECT_SOURCE_DIR}/ros2/src/ros_parameter.cpp #
+)
+add_library(ros_bridge SHARED ${${PROJECT_NAME}_SOURCES}
+ ${${PROJECT_NAME}_HEADERS})
+target_include_directories(ros_bridge
+ PUBLIC $<INSTALL_INTERFACE:include>
+ PRIVATE ${PROJECT_SOURCE_DIR}/ros2/include)
+target_link_libraries(
+ ros_bridge
+ sot-core::sot-core #
+ pinocchio::pinocchio #
+ dynamic-graph-python::dynamic-graph-python #
+ dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp)
+ament_target_dependencies(
+ ros_bridge
+ ament_index_cpp
+ std_msgs
+ std_srvs
+ geometry_msgs
+ sensor_msgs
+ tf2_ros
+ rcutils)
+if(SUFFIX_SO_VERSION)
+ set_target_properties(ros_bridge PROPERTIES SOVERSION ${PROJECT_VERSION})
+endif(SUFFIX_SO_VERSION)
+if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ install(
+ TARGETS ros_bridge
+ EXPORT ${TARGETS_EXPORT_NAME}
+ DESTINATION lib)
+endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+
+# Dynamic graph ros plugin.
+set(plugins ros_publish ros_subscribe)
+foreach(plugin ${plugins})
+ get_filename_component(LIBRARY_NAME ${plugin} NAME)
+ add_library(${LIBRARY_NAME} SHARED
+ ${PROJECT_SOURCE_DIR}/ros2/src/${plugin}.cpp #
+ ${PROJECT_SOURCE_DIR}/ros2/src/${plugin}.hpp #
+ )
+ if(SUFFIX_SO_VERSION)
+ set_target_properties(${LIBRARY_NAME} PROPERTIES SOVERSION
+ ${PROJECT_VERSION})
+ endif(SUFFIX_SO_VERSION)
+ target_link_libraries(
+ ${LIBRARY_NAME} ${${LIBRARY_NAME}_deps} ${catkin_LIBRARIES} ros_bridge
+ dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp
+ )
+ if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ install(
+ TARGETS ${LIBRARY_NAME}
+ EXPORT ${TARGETS_EXPORT_NAME}
+ DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
+ endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ if(BUILD_PYTHON_INTERFACE)
+ string(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
+ if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
+ dynamic_graph_python_module(
+ "ros/${PYTHON_LIBRARY_NAME}" ${LIBRARY_NAME}
+ ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap SOURCE_PYTHON_MODULE
+ "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
+ elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
+ dynamic_graph_python_module(
+ "ros/${PYTHON_LIBRARY_NAME}" ${LIBRARY_NAME}
+ ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap MODULE_HEADER
+ "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
+ endif()
+ endif(BUILD_PYTHON_INTERFACE)
+endforeach(plugin)
+
+if(BUILD_PYTHON_INTERFACE)
+ python_install_on_site("dynamic_graph/ros" "__init__.py")
+ python_install_on_site("dynamic_graph/ros" "ros.py")
+ python_install_on_site("dynamic_graph/ros" "dgcompleter.py")
+endif(BUILD_PYTHON_INTERFACE)
+
+# Stand alone embedded intepreter with a robot controller.
+# add_executable(geometric_simu
+# geometric_simu.cpp
+# sot_loader.cpp
+# sot_loader_basic.cpp)
+# target_link_libraries(
+# geometric_simu Boost::program_options ${CMAKE_DL_LIBS} ${catkin_LIBRARIES}
+# ros_bridge
+# dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp)
+# install(TARGETS geometric_simu DESTINATION lib/${PROJECT_NAME})
+
+# Sot loader library
+# add_library(sot_loader sot_loader.cpp sot_loader_basic.cpp)
+# target_link_libraries(
+# sot_loader Boost::program_options ${catkin_LIBRARIES} ros_bridge
+# dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp)
+# install(
+# TARGETS sot_loader
+# EXPORT ${TARGETS_EXPORT_NAME}
+# DESTINATION lib)
diff --git a/ros2/src/dg_ros_mapping.cpp b/ros2/src/dg_ros_mapping.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0f85f6c9362ed1094a6ad30a4e74b23ac1f3b290
--- /dev/null
+++ b/ros2/src/dg_ros_mapping.cpp
@@ -0,0 +1,280 @@
+/**
+ * @file dg_to_ros.cpp
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ * @date 2019-05-22
+ */
+
+#include "dg_ros_mapping.hpp"
+
+namespace dynamic_graph_bridge
+{
+#define DG_TO_ROS_IMPL(DgRosMappingType) \
+ template <> \
+ void DgRosMappingType::dg_to_ros(const DgRosMappingType::dg_t& dg_src, \
+ DgRosMappingType::ros_t& ros_dst)
+
+#define ROS_TO_DG_IMPL(DgRosMappingType) \
+ template <> \
+ void DgRosMappingType::ros_to_dg(const DgRosMappingType::ros_t& ros_src, \
+ DgRosMappingType::dg_t& dg_dst)
+
+#define DG_DEFAULT_VALUE(DgRosMappingType) \
+ template <> \
+ const DgRosMappingType::dg_t DgRosMappingType::default_value
+
+#define DG_SIGNAL_TYPE_NAME(DgRosMappingType) \
+ template <> \
+ const std::string DgRosMappingType::signal_type_name
+
+/*
+ * Create the list of available types for error and documentation.
+ * purposes.
+ */
+const std::vector<std::string> DgRosTypes::type_list =
+ DgRosTypes::create_type_list();
+
+/*
+ * Float
+ */
+typedef DgRosMapping<std_msgs::msg::Float64, double> DgRosMappingFloat;
+DG_SIGNAL_TYPE_NAME(DgRosMappingFloat) = DgRosTypes::get_double();
+DG_DEFAULT_VALUE(DgRosMappingFloat) = 0.0;
+DG_TO_ROS_IMPL(DgRosMappingFloat)
+{
+ ros_dst.data = dg_src;
+}
+ROS_TO_DG_IMPL(DgRosMappingFloat)
+{
+ dg_dst = ros_src.data;
+}
+
+/*
+ * Unsigned int
+ */
+
+typedef DgRosMapping<std_msgs::msg::UInt32, unsigned int>
+ DgRosMappingUnsignedInt;
+DG_SIGNAL_TYPE_NAME(DgRosMappingUnsignedInt) = DgRosTypes::get_unsigned();
+DG_DEFAULT_VALUE(DgRosMappingUnsignedInt) = 0.0;
+DG_TO_ROS_IMPL(DgRosMappingUnsignedInt)
+{
+ ros_dst.data = dg_src;
+}
+ROS_TO_DG_IMPL(DgRosMappingUnsignedInt)
+{
+ dg_dst = ros_src.data;
+}
+
+/*
+ * Matrix, DG and Ros matrix storage orders are row major.
+ */
+
+typedef DgRosMapping<RosMatrix, DgMatrix> DgRosMappingMatrix;
+DG_SIGNAL_TYPE_NAME(DgRosMappingMatrix) = DgRosTypes::get_matrix();
+DG_DEFAULT_VALUE(DgRosMappingMatrix) = DgMatrix::Zero(0, 0);
+DG_TO_ROS_IMPL(DgRosMappingMatrix)
+{
+ // For simplicity and easiness of maintenance we implement a slower version
+ // of the copy here.
+ ros_dst.width = static_cast<unsigned int>(dg_src.rows());
+ ros_dst.data.resize(dg_src.size());
+ for (int row = 0; row < dg_src.rows(); ++row)
+ {
+ for (int col = 0; col < dg_src.cols(); ++col)
+ {
+ ros_dst.data[row * ros_dst.width + col] = dg_src(row, col);
+ }
+ }
+}
+ROS_TO_DG_IMPL(DgRosMappingMatrix)
+{
+ // For simplicity and easiness of maintenance we implement a slower version
+ // of the copy here.
+ int rows = ros_src.width;
+ int cols = static_cast<int>(ros_src.data.size()) / static_cast<int>(rows);
+ dg_dst.resize(rows, cols);
+ for (int row = 0; row < dg_dst.rows(); ++row)
+ {
+ for (int col = 0; col < dg_dst.cols(); ++col)
+ {
+ dg_dst(row, col) = ros_src.data[row * rows + col];
+ }
+ }
+}
+
+/*
+ * Vector
+ */
+typedef DgRosMapping<RosVector, DgVector> DgRosMappingVector;
+DG_SIGNAL_TYPE_NAME(DgRosMappingVector) = DgRosTypes::get_vector();
+DG_DEFAULT_VALUE(DgRosMappingVector) = DgVector::Zero(0);
+DG_TO_ROS_IMPL(DgRosMappingVector)
+{
+ ros_dst.data.resize(static_cast<unsigned int>(dg_src.size()));
+ for (unsigned int i = 0; i < dg_src.size(); ++i)
+ {
+ ros_dst.data[i] = dg_src[i];
+ }
+}
+ROS_TO_DG_IMPL(DgRosMappingVector)
+{
+ dg_dst.resize(static_cast<int>(ros_src.data.size()));
+ for (unsigned int i = 0; i < dg_dst.size(); ++i)
+ {
+ dg_dst[i] = ros_src.data[i];
+ }
+}
+
+/*
+ * Vector3, ROS is {x, y, z}, DG is a DgVector
+ */
+typedef DgRosMapping<geometry_msgs::msg::Vector3, DgVector> DgRosMappingVector3;
+DG_SIGNAL_TYPE_NAME(DgRosMappingVector3) = DgRosTypes::get_vector3();
+DG_DEFAULT_VALUE(DgRosMappingVector3) = DgVector::Zero(3);
+DG_TO_ROS_IMPL(DgRosMappingVector3)
+{
+ if (dg_src.size() > 0)
+ {
+ ros_dst.x = dg_src(0);
+ if (dg_src.size() > 1)
+ {
+ ros_dst.y = dg_src(1);
+ if (dg_src.size() > 2)
+ {
+ ros_dst.z = dg_src(2);
+ }
+ }
+ }
+}
+ROS_TO_DG_IMPL(DgRosMappingVector3)
+{
+ dg_dst.resize(3);
+ dg_dst[0] = ros_src.x;
+ dg_dst[1] = ros_src.y;
+ dg_dst[2] = ros_src.z;
+}
+
+/*
+ * MatrixHomo, ROS is a Transform (x, y, z, qx, qy, qz, qw),
+ * DG is a homogeneous matrix.
+ */
+typedef DgRosMapping<geometry_msgs::msg::Transform, MatrixHomogeneous>
+ DgRosMappingMatrixHomo;
+DG_SIGNAL_TYPE_NAME(DgRosMappingMatrixHomo) =
+ DgRosTypes::get_matrix_homogeneous();
+DG_DEFAULT_VALUE(DgRosMappingMatrixHomo) = MatrixHomogeneous::Identity();
+DG_TO_ROS_IMPL(DgRosMappingMatrixHomo)
+{
+ ros_dst.translation.x = dg_src.translation()(0);
+ ros_dst.translation.y = dg_src.translation()(1);
+ ros_dst.translation.z = dg_src.translation()(2);
+
+ VectorQuaternion q(dg_src.linear());
+ ros_dst.rotation.x = q.x();
+ ros_dst.rotation.y = q.y();
+ ros_dst.rotation.z = q.z();
+ ros_dst.rotation.w = q.w();
+}
+ROS_TO_DG_IMPL(DgRosMappingMatrixHomo)
+{
+ dg_dst.translation()(0) = ros_src.translation.x;
+ dg_dst.translation()(1) = ros_src.translation.y;
+ dg_dst.translation()(2) = ros_src.translation.z;
+
+ VectorQuaternion q(ros_src.rotation.w,
+ ros_src.rotation.x,
+ ros_src.rotation.y,
+ ros_src.rotation.z);
+ dg_dst.linear() = q.matrix();
+}
+
+/*
+ * MatrixHomo, ROS is a twist (x, y, z, wx, wy, wz),
+ * DG is a homogeneous matrix.
+ */
+typedef DgRosMapping<geometry_msgs::msg::Twist, DgVector> DgRosMappingTwist;
+DG_SIGNAL_TYPE_NAME(DgRosMappingTwist) = DgRosTypes::get_twist();
+DG_DEFAULT_VALUE(DgRosMappingTwist) = DgVector::Zero(6);
+DG_TO_ROS_IMPL(DgRosMappingTwist)
+{
+ if (dg_src.size() != 6)
+ {
+ throw std::runtime_error("failed to convert invalid twist");
+ }
+ ros_dst.linear.x = dg_src(0);
+ ros_dst.linear.y = dg_src(1);
+ ros_dst.linear.z = dg_src(2);
+ ros_dst.angular.x = dg_src(3);
+ ros_dst.angular.y = dg_src(4);
+ ros_dst.angular.z = dg_src(5);
+}
+ROS_TO_DG_IMPL(DgRosMappingTwist)
+{
+ dg_dst.resize(6);
+ dg_dst(0) = ros_src.linear.x;
+ dg_dst(1) = ros_src.linear.y;
+ dg_dst(2) = ros_src.linear.z;
+ dg_dst(3) = ros_src.angular.x;
+ dg_dst(4) = ros_src.angular.y;
+ dg_dst(5) = ros_src.angular.z;
+}
+
+/*
+ * Vector3Stamped, ROS is {x, y, z} stamped, DG is a DgVector
+ */
+typedef DgRosMapping<geometry_msgs::msg::Vector3Stamped, DgVector>
+ DgRosMappingVector3Stamped;
+DG_SIGNAL_TYPE_NAME(DgRosMappingVector3Stamped) =
+ DgRosTypes::get_vector3_stamped();
+DG_DEFAULT_VALUE(DgRosMappingVector3Stamped) = DgVector::Zero(3);
+DG_TO_ROS_IMPL(DgRosMappingVector3Stamped)
+{
+ make_header(ros_dst.header);
+ DgRosMappingVector3::dg_to_ros(dg_src, ros_dst.vector);
+}
+ROS_TO_DG_IMPL(DgRosMappingVector3Stamped)
+{
+ DgRosMappingVector3::ros_to_dg(ros_src.vector, dg_dst);
+}
+
+/*
+ * MatrixHomo, ROS is a stamped Transform (x, y, z, qx, qy, qz, qw),
+ * DG is a homogeneous matrix.
+ */
+typedef DgRosMapping<geometry_msgs::msg::TransformStamped, MatrixHomogeneous>
+ DgRosMappingMatrixHomoStamped;
+DG_SIGNAL_TYPE_NAME(DgRosMappingMatrixHomoStamped) =
+ DgRosTypes::get_matrix_homogeneous_stamped();
+DG_DEFAULT_VALUE(DgRosMappingMatrixHomoStamped) = MatrixHomogeneous::Identity();
+DG_TO_ROS_IMPL(DgRosMappingMatrixHomoStamped)
+{
+ make_header(ros_dst.header);
+ DgRosMappingMatrixHomo::dg_to_ros(dg_src, ros_dst.transform);
+}
+ROS_TO_DG_IMPL(DgRosMappingMatrixHomoStamped)
+{
+ DgRosMappingMatrixHomo::ros_to_dg(ros_src.transform, dg_dst);
+}
+
+/*
+ * MatrixHomo, ROS is a stamped twist (x, y, z, wx, wy, wz),
+ * DG is a homogeneous matrix.
+ */
+typedef DgRosMapping<geometry_msgs::msg::TwistStamped, DgVector>
+ DgRosMappingTwistStamped;
+DG_SIGNAL_TYPE_NAME(DgRosMappingTwistStamped) = DgRosTypes::get_twist_stamped();
+DG_DEFAULT_VALUE(DgRosMappingTwistStamped) = DgVector::Zero(6);
+DG_TO_ROS_IMPL(DgRosMappingTwistStamped)
+{
+ make_header(ros_dst.header);
+ DgRosMappingTwist::dg_to_ros(dg_src, ros_dst.twist);
+}
+ROS_TO_DG_IMPL(DgRosMappingTwistStamped)
+{
+ DgRosMappingTwist::ros_to_dg(ros_src.twist, dg_dst);
+}
+
+} // namespace dynamic_graph_bridge
diff --git a/ros2/src/dg_ros_mapping.hpp b/ros2/src/dg_ros_mapping.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5d5d443be6353fa234ddc858961410e82251df1a
--- /dev/null
+++ b/ros2/src/dg_ros_mapping.hpp
@@ -0,0 +1,203 @@
+/**
+ * @file dg_to_ros.hpp
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ * @date 2019-05-22
+ */
+#pragma once
+
+// Standard includes
+#include <sstream>
+#include <utility>
+#include <vector>
+
+// Dynamic Graph types.
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+
+// ROS types
+#include <geometry_msgs/msg/transform.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/u_int32.hpp>
+
+// internal DG types
+#include "matrix_geometry.hpp"
+#include "time_point_io.hpp"
+
+// internal ros types
+#include "dynamic_graph_bridge/ros.hpp"
+#include "dynamic_graph_bridge_msgs/msg/matrix.hpp"
+#include "dynamic_graph_bridge_msgs/msg/vector.hpp"
+
+namespace dynamic_graph_bridge
+{
+/** @brief Conventient renaming for ease of implementation. */
+typedef dynamicgraph::Vector DgVector;
+/** @brief Conventient renaming for ease of implementation. */
+typedef dynamicgraph::Matrix DgMatrix;
+/** @brief Conventient renaming for ease of implementation. */
+typedef dynamic_graph_bridge_msgs::msg::Vector RosVector;
+/** @brief Conventient renaming for ease of implementation. */
+typedef dynamic_graph_bridge_msgs::msg::Matrix RosMatrix;
+
+struct DgRosTypes
+{
+ static std::vector<std::string> create_type_list()
+ {
+ std::vector<std::string> tmp_type_list;
+ tmp_type_list.push_back("double");
+ tmp_type_list.push_back("unsigned");
+ tmp_type_list.push_back("matrix");
+ tmp_type_list.push_back("vector");
+ tmp_type_list.push_back("vector3");
+ tmp_type_list.push_back("vector3_stamped");
+ tmp_type_list.push_back("matrix_homogeneous");
+ tmp_type_list.push_back("matrix_homogeneous_stamped");
+ tmp_type_list.push_back("twist");
+ tmp_type_list.push_back("twist_stamped");
+ return tmp_type_list;
+ }
+ // clang-format off
+ static const std::string& get_double(){return type_list[0];};
+ static const std::string& get_unsigned(){return type_list[1];};
+ static const std::string& get_matrix(){return type_list[2];};
+ static const std::string& get_vector(){return type_list[3];};
+ static const std::string& get_vector3(){return type_list[4];};
+ static const std::string& get_vector3_stamped(){return type_list[5];};
+ static const std::string& get_matrix_homogeneous(){return type_list[6];};
+ static const std::string& get_matrix_homogeneous_stamped()
+ {return type_list[7];};
+ static const std::string& get_twist(){return type_list[8];};
+ static const std::string& get_twist_stamped(){return type_list[9];};
+ // clang-format on
+ static const std::vector<std::string> type_list;
+};
+
+/**
+ * @brief DgRosMapping trait type.
+ *
+ * DG stands for Dynamic Graph and ROS for Robot Operating System.
+ * This trait provides types associated to a dynamic-graph type:
+ * - DG / ROS corresponding types,
+ * - Output/Input signal type,
+ * - ROS / DG callback type.
+ *
+ * @tparam RosType is the type of the ROS data.
+ * @tparam DgType is the type of the Dynamic Graph data.
+ */
+template <class RosType, class DgType>
+class DgRosMapping
+{
+public:
+ /*
+ * Some renaming first
+ */
+
+ /** @brief Dynamic Graph type. */
+ typedef DgType dg_t;
+ /** @brief Ros type. */
+ typedef RosType ros_t;
+ /** @brief Ros type as a shared pointer. */
+ typedef std::shared_ptr<RosType> ros_shared_ptr_t;
+ /** @brief Output signal type. */
+ typedef dynamicgraph::SignalTimeDependent<dg_t, int> signal_out_t;
+ /** @brief Output signal type. */
+ typedef dynamicgraph::SignalTimeDependent<timestamp_t, int>
+ signal_timestamp_out_t;
+ /** @brief Input signal type. */
+ typedef dynamicgraph::SignalPtr<dg_t, int> signal_in_t;
+ /** @brief Signal callback function types. */
+ typedef std::function<dg_t&(dg_t&, int)> signal_callback_t;
+
+ /** @brief Name of the signal type. Used during the creation of signals. */
+ static const std::string signal_type_name;
+ static const dg_t default_value;
+
+ /**
+ * @brief Set the default value to the inuput DG signal.
+ *
+ * @tparam SignalType (in/out)
+ * @param s
+ */
+ template <typename SignalTypePtr>
+ static void set_default(SignalTypePtr s)
+ {
+ s->setConstant(DgRosMapping<RosType, DgType>::default_value);
+ }
+
+ /**
+ * @brief Set the default value to the input DG object.
+ *
+ * @param s
+ */
+ static void set_default(dg_t& d)
+ {
+ d = DgRosMapping<RosType, DgType>::default_value;
+ }
+
+ /**
+ * @brief Convert ROS time to std::chrono.
+ *
+ * @param ros_time
+ * @return timestamp_t
+ */
+ static timestamp_t from_ros_time(rclcpp::Time ros_time)
+ {
+ return epoch_time() + std::chrono::nanoseconds(ros_time.nanoseconds());
+ }
+
+ /**
+ * @brief Get epoch time as ROS time start from there.
+ *
+ * @return timestamp_t
+ */
+ static timestamp_t epoch_time()
+ {
+ return std::chrono::time_point<std::chrono::high_resolution_clock>{};
+ }
+
+ /**
+ * @brief Convert a ROS object into DG one.
+ *
+ * @param src
+ * @param dst
+ */
+ static void ros_to_dg(const ros_t& ros_src, dg_t& dg_dst);
+
+ /**
+ * @brief Convert a DG object into a ROS one.
+ *
+ * @param src
+ * @param dst
+ */
+ static void dg_to_ros(const dg_t& dg_src, ros_t& ros_dst);
+};
+
+template <class RosEntity>
+inline std::string make_signal_string(const RosEntity& entity,
+ bool isInputSignal,
+ const std::string& signal_type,
+ const std::string& signal_name)
+{
+ std::ostringstream oss;
+ oss << entity.getClassName() << "(" << entity.getName() << ")"
+ << "::" << (isInputSignal ? "input" : "output") << "(" << signal_type
+ << ")"
+ << "::" << signal_name;
+ return oss.str();
+}
+
+inline void make_header(std_msgs::msg::Header& header)
+{
+ header.stamp = get_ros_node(DG_ROS_NODE_NAME)->get_clock()->now();
+ header.frame_id = "/dynamic_graph/world";
+}
+
+} // namespace dynamic_graph_bridge
diff --git a/ros2/src/dynamic_graph/ros/__init__.py b/ros2/src/dynamic_graph/ros/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..cc585acab0f67b9d27a123f53b08b9249d91b81b
--- /dev/null
+++ b/ros2/src/dynamic_graph/ros/__init__.py
@@ -0,0 +1,11 @@
+"""@package dynamic_graph_manager
+
+@file __init__.py
+@author Maximilien Naveau (maximilien.naveau@gmail.com)
+@license License BSD-3-Clause
+@copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
+@date 2019-05-22
+
+@brief This file Defines the python ROS import.
+
+"""
diff --git a/ros2/src/dynamic_graph/ros/dgcompleter.py b/ros2/src/dynamic_graph/ros/dgcompleter.py
new file mode 100644
index 0000000000000000000000000000000000000000..aa24433fdd03bec44b0860f87b6b7f6f932c8c65
--- /dev/null
+++ b/ros2/src/dynamic_graph/ros/dgcompleter.py
@@ -0,0 +1,92 @@
+"""Word completion for GNU readline.
+
+The completer completes keywords, built-ins and globals in a selectable
+namespace (which defaults to __main__); when completing NAME.NAME..., it
+evaluates (!) the expression up to the last dot and completes its attributes.
+
+It's very cool to do "import sys" type "sys.", hit the completion key (twice),
+and see the list of names defined by the sys module!
+
+Tip: to use the tab key as the completion key, call
+
+ readline.parse_and_bind("tab: complete")
+
+Notes:
+
+- Exceptions raised by the completer function are *ignored* (and generally cause
+ the completion to fail). This is a feature -- since readline sets the tty
+ device in raw (or cbreak) mode, printing a traceback wouldn't work well
+ without some complicated hoopla to save, reset and restore the tty state.
+
+- The evaluation of the NAME.NAME... form may cause arbitrary application
+ defined code to be executed if an object with a __getattr__ hook is found.
+ Since it is the responsibility of the application (or the user) to enable this
+ feature, I consider this an acceptable risk. More complicated expressions
+ (e.g. function calls or indexing operations) are *not* evaluated.
+
+- GNU readline is also used by the built-in functions input() and
+raw_input(), and thus these also benefit/suffer from the completer
+features. Clearly an interactive application can benefit by
+specifying its own completer function and using raw_input() for all
+its input.
+
+- When the original stdin is not a tty device, GNU readline is never
+ used, and this module (and the readline module) are silently inactive.
+
+"""
+
+import ast
+from dynamic_graph_manager_cpp_bindings import RosPythonInterpreterClient
+
+__all__ = ["DGCompleter"]
+
+
+class DGCompleter:
+ def __init__(self, ros_python_interpreter_client=None):
+ """Create a new completer for the command line.
+
+ Completer([client]) -> completer instance.
+
+ Client is a ROS proxy to dynamic_graph run_python_command service.
+
+ Completer instances should be used as the completion mechanism of
+ readline via the set_completer() call:
+
+ readline.set_completer(Completer(client).complete)
+ """
+ if ros_python_interpreter_client is None:
+ self.client = RosPythonInterpreterClient()
+ else:
+ self.client = ros_python_interpreter_client
+
+ cmd = [
+ 'if "local_completer" not in globals():\n'
+ ' print("Load the dg_completer")\n'
+ " from rlcompleter import Completer\n"
+ " local_completer=Completer()\n"
+ " import readline\n"
+ " readline.set_completer(local_completer.complete)\n"
+ ' readline.parse_and_bind("tab: complete")'
+ ]
+
+ for python_command in cmd:
+ print(python_command)
+ self.client.run_python_command(python_command)
+
+ self.buffer = []
+
+ def complete(self, text, state):
+ """Return the next possible completion for 'text'.readline.parse_and_bind("tab: complete")
+
+ This is called successively with state == 0, 1, 2, ... until it
+ returns None. The completion should begin with 'text'.
+ """
+ # self.client.run_python_command("global local_completer")
+ cmd = 'local_completer.complete("' + text + '",' + str(state) + ")"
+ response = self.client.run_python_command(cmd)
+ if not response or response is "\n" or response is None:
+ response = None
+ self.buffer = []
+ else:
+ self.buffer += [response]
+ return response
diff --git a/ros2/src/dynamic_graph/ros/ros.py b/ros2/src/dynamic_graph/ros/ros.py
new file mode 100644
index 0000000000000000000000000000000000000000..2bb925d12edc4091f73ad743c4055c64558a6748
--- /dev/null
+++ b/ros2/src/dynamic_graph/ros/ros.py
@@ -0,0 +1,31 @@
+"""@package dynamic_graph_manager
+
+@file ros.py
+@author Maximilien Naveau (maximilien.naveau@gmail.com)
+@license License BSD-3-Clause
+@copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
+@date 2019-05-22
+
+@brief This file defines a class that create ros entities for the interaction
+between ROS and the dynamic graph
+
+"""
+
+from dynamic_graph import plug
+from dynamic_graph_manager.dynamic_graph.ros_entities import RosPublish
+from dynamic_graph_manager.dynamic_graph.ros_entities import RosSubscribe
+
+
+class Ros(object):
+ def __init__(self, device, suffix=""):
+ self.device = device
+ self.ros_publish = RosPublish("ros_publish" + suffix)
+ self.ros_subscribe = RosSubscribe("ros_subscribe" + suffix)
+
+ # make sure that the publishing is done by plugging the refresh
+ # (trigger) to the device periodic call system
+ self.device.after.addDownsampledSignal(
+ self.ros_publish.name + ".trigger", 1
+ )
+ # not needed for self.ros_subscribe and self.ros_subscribe as they get
+ # the information from ROS, they do not have output signals
diff --git a/ros2/src/matrix_geometry.hpp b/ros2/src/matrix_geometry.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..98ba8029c39da5c42cabb5dd1dd5c2584aefa0ea
--- /dev/null
+++ b/ros2/src/matrix_geometry.hpp
@@ -0,0 +1,47 @@
+/**
+ * @file matrix_geometry.hpp
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ * @date 2019-05-22
+ */
+
+#ifndef MATRIX_GEOMETRY_HH
+#define MATRIX_GEOMETRY_HH
+
+#include <dynamic-graph/eigen-io.h>
+#include <dynamic-graph/linear-algebra.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#define MRAWDATA(x) x.data()
+
+namespace dynamic_graph_bridge
+{
+typedef Eigen::Transform<double, 3, Eigen::Affine> MatrixHomogeneous;
+typedef Eigen::Matrix<double, 3, 3> MatrixRotation;
+typedef Eigen::AngleAxis<double> VectorUTheta;
+typedef Eigen::Quaternion<double> VectorQuaternion;
+typedef Eigen::Vector3d VectorRotation;
+typedef Eigen::Vector3d VectorRollPitchYaw;
+typedef Eigen::Matrix<double, 6, 6> MatrixForce;
+typedef Eigen::Matrix<double, 6, 6> MatrixTwist;
+
+inline void buildFrom(const MatrixHomogeneous& MH, MatrixTwist& MT)
+{
+ Eigen::Vector3d _t = MH.translation();
+ MatrixRotation R(MH.linear());
+ Eigen::Matrix3d Tx;
+ Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
+ Eigen::Matrix3d sk;
+ sk = Tx * R;
+
+ MT.block<3, 3>(0, 0) = R;
+ MT.block<3, 3>(0, 3) = sk;
+ MT.block<3, 3>(3, 0).setZero();
+ MT.block<3, 3>(3, 3) = R;
+}
+} // namespace dynamic_graph_bridge
+
+#endif // MATRIX_GEOMETRY_HH
diff --git a/ros2/src/programs/geometric_simu.cpp b/ros2/src/programs/geometric_simu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8dbe4459c9744f5dd00d8772883af3ebcf077734
--- /dev/null
+++ b/ros2/src/programs/geometric_simu.cpp
@@ -0,0 +1,42 @@
+/*
+ * Copyright 2011,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ */
+#include <iostream>
+
+
+#define ENABLE_RT_LOG
+#include <dynamic-graph/real-time-logger.h>
+
+#include <dynamic_graph_bridge/sot_loader.hh>
+#include <dynamic_graph_bridge/ros2_init.hh>
+
+int main(int argc, char *argv[]) {
+ ::dynamicgraph::RealTimeLogger::instance()
+ .addOutputStream(::dynamicgraph::LoggerStreamPtr_t(new dynamicgraph::LoggerIOStream(std::cout)));
+
+ rclcpp::init(argc, argv);
+
+ dynamicgraph::RosContext::SharedPtr aRosContext =
+ std::make_shared<dynamicgraph::RosContext>();
+
+ aRosContext->rosInit();
+
+ SotLoader aSotLoader;
+ if (aSotLoader.parseOptions(argc, argv) < 0) return -1;
+
+ // Advertize service "(start|stop)_dynamic_graph" and
+ // load parameter "robot_description in SoT.
+ aSotLoader.initializeFromRosContext(aRosContext);
+
+ // Load dynamic library and run python prologue.
+ aSotLoader.initPublication();
+
+ aSotLoader.initializeServices();
+
+ aRosContext->mtExecutor->spin();
+ return 0;
+}
diff --git a/ros2/src/ros.cpp b/ros2/src/ros.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b765654d703bf54e9059667f65748bb4fc44bda2
--- /dev/null
+++ b/ros2/src/ros.cpp
@@ -0,0 +1,332 @@
+/**
+ * @file ros_init.cpp
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ * @date 2019-05-22
+ */
+
+#include <deque>
+#include <atomic>
+#include <fstream>
+#include <chrono>
+#include <thread>
+
+#include "dynamic_graph_bridge/ros.hpp"
+
+namespace dynamic_graph_bridge
+{
+/*
+ * Private methods
+ */
+
+/**
+ * @brief Shortcut.
+ */
+typedef std::map<std::string, RosNodePtr> GlobalListOfRosNodeType;
+
+/**
+ * @brief GLOBAL_LIST_OF_ROS_NODE is global variable that acts as a singleton on
+ * the ROS node handle and the spinner.
+ *
+ * The use of the std::unique_ptr allows to delete the object and re-create
+ * at will. It is usefull to reset the ros environment specifically for
+ * unittesting.
+ *
+ * If the node handle does not exist we call the global method rclcpp::init.
+ * This method has for purpose to initialize the ROS environment. The
+ * creation of ROS object is permitted only after the call of this function.
+ * After rclcpp::init being called we create the node handle which allows in
+ * turn to advertize the ROS services, or create topics (data pipe).
+ *
+ */
+static GlobalListOfRosNodeType GLOBAL_LIST_OF_ROS_NODE;
+
+/**
+ * @brief Small class allowing to start a ROS spin in a thread.
+ */
+class Executor
+{
+public:
+ Executor() : ros_executor_(rclcpp::executor::ExecutorArgs(), 4)
+ {
+ is_thread_running_ = false;
+ is_spinning_ = false;
+ list_node_added_.clear();
+ }
+
+ /**
+ * @brief Upon destruction close the thread and stop spinning.
+ *
+ * @return
+ */
+ ~Executor()
+ {
+ stop_spinning();
+ }
+
+ /**
+ * @brief Start ros_spin in a different thread to not block the current one.
+ */
+ void spin_non_blocking()
+ {
+ if (!is_thread_running_ && !is_spinning_)
+ {
+ std::cout << "Start ros spin in thread." << std::endl;
+ // Marking thread as started to avoid a second thread from getting
+ // started.
+ is_thread_running_ = true;
+ thread_ = std::thread(&Executor::thread_callback, this);
+ }
+ }
+
+ /**
+ * @brief Block the current thread and make ROS spinning.
+ */
+ void spin()
+ {
+ if (is_thread_running_)
+ {
+ while (ros_ok() && is_thread_running_)
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+ }
+ else
+ {
+ is_spinning_ = true;
+ ros_executor_.spin();
+ is_spinning_ = false;
+ }
+ }
+
+ void add_node(const std::string& ros_node_name)
+ {
+ if (std::find(list_node_added_.begin(),
+ list_node_added_.end(),
+ ros_node_name) == list_node_added_.end())
+ {
+ list_node_added_.push_back(ros_node_name);
+ ros_executor_.add_node(get_ros_node(ros_node_name));
+ }
+ }
+
+ void remove_node(const std::string& ros_node_name)
+ {
+ std::deque<std::string>::iterator el = std::find(
+ list_node_added_.begin(), list_node_added_.end(), ros_node_name);
+ if (el != list_node_added_.end())
+ {
+ list_node_added_.erase(el);
+ assert(std::find(list_node_added_.begin(),
+ list_node_added_.end(),
+ ros_node_name) == list_node_added_.end() &&
+ "The node has not been removed properly.");
+ ros_executor_.remove_node(get_ros_node(ros_node_name));
+ }
+ }
+
+ /**
+ * @brief Stop the spinning al together. Callable in a different thread.
+ */
+ void stop_spinning()
+ {
+ while (is_thread_running_ || is_spinning_)
+ {
+ ros_executor_.cancel();
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+ if (thread_.joinable())
+ {
+ thread_.join();
+ }
+ }
+
+private:
+ /**
+ * @brief Thread callback function
+ */
+ void thread_callback()
+ {
+ ros_executor_.spin();
+ is_thread_running_ = false;
+ }
+
+ /**
+ * @brief Check if the thread is running.
+ */
+ std::atomic<bool> is_thread_running_;
+
+ /**
+ * @brief Check if the the executor is spinning.
+ */
+ std::atomic<bool> is_spinning_;
+
+ /**
+ * @brief Thread in which the EXECUTOR spins.
+ */
+ std::thread thread_;
+
+ /**
+ * @brief Object that execute the ROS callbacks in a different thread.
+ */
+ RosExecutor ros_executor_;
+
+ std::deque<std::string> list_node_added_;
+
+}; // class Executor
+
+typedef std::shared_ptr<Executor> ExecutorPtr;
+
+/**
+ * @brief EXECUTOR is a ros object that handles in a global way
+ * all the ros callbacks and interruption. Call EXECUTOR.spin()
+ * in order to start handling the callback in a separate thread.
+ */
+ExecutorPtr EXECUTOR = nullptr;
+
+/**
+ * @brief Private function that allow us to get the current executable name.
+ *
+ * @return std::string the current executable name.
+ */
+std::string executable_name()
+{
+#if defined(PLATFORM_POSIX) || \
+ defined(__linux__) // check defines for your setup
+
+ std::string sp;
+ std::ifstream("/proc/self/comm") >> sp;
+ return sp;
+
+#elif defined(_WIN32)
+
+ char buf[MAX_PATH];
+ GetModuleFileNameA(nullptr, buf, MAX_PATH);
+ return buf;
+
+#else
+
+ static_assert(false, "unrecognized platform");
+
+#endif
+}
+
+/**
+ * @brief Private function that allow us to initialize ROS only once.
+ */
+void ros_init()
+{
+ if (!rclcpp::ok())
+ {
+ /** call rclcpp::init */
+ int argc = 1;
+ char* arg0 = strdup(executable_name().c_str());
+ char* argv[] = {arg0, nullptr};
+ rclcpp::init(argc, argv);
+ free(arg0);
+ }
+}
+
+/*
+ * Public Functions => user API, see ros.hpp for the docstrings.
+ */
+
+bool ros_node_exists(std::string node_name)
+{
+ if (GLOBAL_LIST_OF_ROS_NODE.find(node_name) ==
+ GLOBAL_LIST_OF_ROS_NODE.end())
+ {
+ return false;
+ }
+ if (GLOBAL_LIST_OF_ROS_NODE.at(node_name) == nullptr)
+ {
+ return false;
+ }
+ return true;
+}
+
+ExecutorPtr get_ros_executor()
+{
+ ros_init();
+ if (!EXECUTOR)
+ {
+ EXECUTOR = std::make_shared<Executor>();
+ }
+ return EXECUTOR;
+}
+
+RosNodePtr get_ros_node(std::string node_name)
+{
+ ros_init();
+ if (!ros_node_exists(node_name))
+ {
+ GLOBAL_LIST_OF_ROS_NODE[node_name] = RosNodePtr(nullptr);
+ }
+ if (!GLOBAL_LIST_OF_ROS_NODE[node_name] ||
+ GLOBAL_LIST_OF_ROS_NODE[node_name].get() == nullptr)
+ {
+ /** RosNode instanciation */
+ GLOBAL_LIST_OF_ROS_NODE[node_name] =
+ std::make_shared<RosNode>(node_name, "dynamic_graph_manager");
+ }
+ /** Return a reference to the node handle so any function can use it */
+ return GLOBAL_LIST_OF_ROS_NODE[node_name];
+}
+
+void ros_add_node_to_executor(const std::string& node_name)
+{
+ get_ros_executor()->add_node(node_name);
+}
+
+void ros_shutdown(std::string node_name)
+{
+ if (GLOBAL_LIST_OF_ROS_NODE.find(node_name) ==
+ GLOBAL_LIST_OF_ROS_NODE.end())
+ {
+ return;
+ }
+ get_ros_executor()->remove_node(node_name);
+ GLOBAL_LIST_OF_ROS_NODE.erase(node_name);
+}
+
+void ros_shutdown()
+{
+ // rclcpp::shutdown();
+}
+
+void ros_clean()
+{
+ ros_stop_spinning();
+ GlobalListOfRosNodeType::iterator ros_node_it =
+ GLOBAL_LIST_OF_ROS_NODE.begin();
+ while (!GLOBAL_LIST_OF_ROS_NODE.empty())
+ {
+ ros_shutdown(ros_node_it->first);
+ ros_node_it = GLOBAL_LIST_OF_ROS_NODE.begin();
+ }
+ GLOBAL_LIST_OF_ROS_NODE.clear();
+}
+
+bool ros_ok()
+{
+ return rclcpp::ok() && rclcpp::is_initialized();
+}
+
+void ros_spin()
+{
+ get_ros_executor()->spin();
+}
+
+void ros_spin_non_blocking()
+{
+ get_ros_executor()->spin_non_blocking();
+}
+
+void ros_stop_spinning()
+{
+ get_ros_executor()->stop_spinning();
+}
+
+} // end of namespace dynamic_graph_bridge.
diff --git a/ros2/src/ros_parameter.cpp b/ros2/src/ros_parameter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57dcfd180a1741bcb866552d99aabecae19ff54a
--- /dev/null
+++ b/ros2/src/ros_parameter.cpp
@@ -0,0 +1,60 @@
+#include <sot/core/robot-utils.hh>
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+
+#include <stdexcept>
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
+
+#include <urdf_parser/urdf_parser.h>
+
+#include "dynamic_graph_bridge/ros_parameter.hpp"
+
+namespace dynamicgraph {
+ bool parameter_server_read_robot_description(rclcpp::Node::SharedPtr nh)
+ {
+
+ if (!nh->has_parameter("robot_description"))
+ {
+ nh->declare_parameter("robot_description",std::string(""));
+ }
+
+ std::string robot_description;
+ std::string parameter_name("robot_description");
+ nh->get_parameter(parameter_name,robot_description);
+ if (robot_description.empty())
+ {
+ RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
+ "Parameter robot_description is empty");
+ return false;
+ }
+
+ std::string model_name("robot");
+
+ // Search for the robot util related to robot_name.
+ sot::RobotUtilShrPtr aRobotUtil = sot::getRobotUtil(model_name);
+ // If does not exist then it is created.
+ if (aRobotUtil == sot::RefVoidRobotUtil())
+ aRobotUtil = sot::createRobotUtil(model_name);
+
+ // If the creation is fine
+ if (aRobotUtil != sot::RefVoidRobotUtil())
+ {
+ // Then set the robot model.
+ aRobotUtil->set_parameter(parameter_name,robot_description);
+ RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+ "Set parameter_name : %s.",parameter_name.c_str());
+ // Everything went fine.
+ return true;
+ }
+ RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
+ "Wrong initialization of parameter_name %s",
+ parameter_name.c_str());
+
+ // Otherwise something went wrong.
+ return false;
+
+ }
+
+}
diff --git a/ros2/src/ros_publish-python-module-py.cc b/ros2/src/ros_publish-python-module-py.cc
new file mode 100644
index 0000000000000000000000000000000000000000..5134a7873eea588da3f1d675f6e8716ee6ef0dfe
--- /dev/null
+++ b/ros2/src/ros_publish-python-module-py.cc
@@ -0,0 +1,19 @@
+#include <dynamic-graph/python/module.hh>
+#include "ros_publish.hh"
+
+namespace dg = dynamicgraph;
+
+BOOST_PYTHON_MODULE(wrap)
+{
+ bp::import("dynamic_graph");
+
+ dg::python::exposeEntity<dg::RosPublish, bp::bases<dg::Entity>,
+ dg::python::AddCommands>()
+ .def("clear", &dg::RosPublish::clear,
+ "Remove all signals writing data to a ROS topic")
+ .def("rm", &dg::RosPublish::rm,
+ "Remove a signal writing data to a ROS topic",
+ bp::args("signal_name"))
+ .def("list", &dg::RosPublish::list,
+ "List signals writing data to a ROS topic");
+}
diff --git a/ros2/src/ros_publish.cpp b/ros2/src/ros_publish.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8bbd45972c14ffa3a43fb78a755beb4c227a393f
--- /dev/null
+++ b/ros2/src/ros_publish.cpp
@@ -0,0 +1,310 @@
+/**
+ * @file
+ * @license BSD 3-clause
+ * @copyright Copyright (c) 2020, New York University and Max Planck
+ * Gesellschaft
+ *
+ * @brief Implements the RosPublish class.
+ */
+
+#include "dynamic_graph_bridge/ros_entities/ros_publish.hpp"
+
+#include "dynamic_graph_bridge/ros.hpp"
+
+namespace dynamic_graph_bridge
+{
+/*
+ * RosPublish class
+ */
+
+const std::string RosPublish::doc_string_ =
+ "Publish dynamic-graph signals as ROS topics.\n"
+ "\n"
+ " Use command \"add\" to publish a new ROS topic.\n";
+
+const std::string RosPublish::trigger_signal_name_ = "trigger";
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish");
+
+RosPublish::RosPublish(const std::string& n)
+ : dynamicgraph::Entity(n),
+ trigger_(boost::bind(&RosPublish::trigger, this, _1, _2),
+ dynamicgraph::sotNOSIGNAL,
+ make_signal_string(*this, true, "int", trigger_signal_name_)),
+ rate_nanosec_(std::chrono::nanoseconds(50000000)) // 50ms
+{
+ signalRegistration(trigger_);
+
+ ros_node_ = get_ros_node(DG_ROS_NODE_NAME);
+ ros_add_node_to_executor(DG_ROS_NODE_NAME);
+ ros_spin_non_blocking();
+ binded_signals_.clear();
+ /** @todo Check this needUpdateFromAllChildren */
+ trigger_.setNeedUpdateFromAllChildren(true);
+ try
+ {
+ last_publicated_ = ros_node_->get_clock()->now();
+ }
+ catch (const std::exception& exc)
+ {
+ throw std::runtime_error(
+ "Failed to call ros_node_->get_clock()->now():" +
+ std::string(exc.what()));
+ }
+ mutex_.unlock();
+
+ std::string doc_string;
+ doc_string =
+ "\n"
+ " Add a signal writing data to a ROS topic.\n"
+ "\n"
+ " Input:\n"
+ " - type: string among:\n";
+ for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i)
+ {
+ doc_string += " - " + DgRosTypes::type_list[i] + "\n";
+ }
+ doc_string +=
+ " - signal: the signal name in dynamic-graph,\n"
+ " - topic: the topic name in ROS.\n"
+ "\n";
+ addCommand("add", new command::ros_publish::Add(*this, doc_string));
+ doc_string =
+ "\n"
+ " Remove a signal writing data to a ROS topic.\n"
+ "\n"
+ " Input:\n"
+ " - name of the signal to remove (see method list for the list of "
+ "signals).\n"
+ "\n";
+ addCommand("rm", new command::ros_publish::Rm(*this, doc_string));
+ doc_string =
+ "\n"
+ " Remove all signals writing data to a ROS topic.\n"
+ "\n"
+ " No input.\n"
+ "\n";
+ addCommand("clear", new command::ros_publish::Clear(*this, doc_string));
+ doc_string =
+ "\n"
+ " List signals writing data to a ROS topic.\n"
+ "\n"
+ " No input.\n"
+ "\n";
+ addCommand("list", new command::ros_publish::List(*this, doc_string));
+} // namespace dynamic_graph_bridge
+
+RosPublish::~RosPublish()
+{
+}
+
+void RosPublish::display(std::ostream& os) const
+{
+ os << this->CLASS_NAME << "(" << this->name << ")." << std::endl;
+}
+
+void RosPublish::rm(const std::string& signal)
+{
+ if (binded_signals_.find(signal) == binded_signals_.end())
+ {
+ return;
+ }
+
+ if (signal == trigger_signal_name_)
+ {
+ std::cerr << "The trigger signal should not be removed. Aborting."
+ << std::endl;
+ return;
+ }
+
+ // lock the mutex to avoid deleting the signal during a call to trigger
+ while (!mutex_.try_lock())
+ {
+ }
+ signalDeregistration(signal);
+ binded_signals_.erase(signal);
+ mutex_.unlock();
+}
+
+std::string RosPublish::list() const
+{
+ std::string result("[");
+ for (std::map<std::string, BindedSignal>::const_iterator it =
+ binded_signals_.begin();
+ it != binded_signals_.end();
+ it++)
+ {
+ result += "'" + it->first + "',";
+ }
+ result += "]";
+ return result;
+}
+
+void RosPublish::clear()
+{
+ std::map<std::string, BindedSignal>::iterator it = binded_signals_.begin();
+ for (; it != binded_signals_.end();)
+ {
+ if (it->first != trigger_signal_name_)
+ {
+ rm(it->first);
+ it = binded_signals_.begin();
+ }
+ else
+ {
+ ++it;
+ }
+ }
+}
+
+int& RosPublish::trigger(int& dummy, int)
+{
+ typedef std::map<std::string, BindedSignal>::iterator iterator_t;
+
+ // If we did not wait long enough (> rate_nanosec_), we do nothing.
+ rclcpp::Duration dt = ros_node_->get_clock()->now() - last_publicated_;
+ if (dt < rate_nanosec_)
+ {
+ return dummy;
+ }
+
+ // We reset the previous publishing time as "now".
+ last_publicated_ = ros_node_->get_clock()->now();
+
+ // We lock the mutex for the full duration of the publication.
+ mutex_.lock();
+ for (iterator_t it = binded_signals_.begin(); it != binded_signals_.end();
+ ++it)
+ {
+ // calling the callback corresponding to the input signal in the tuple.
+ std::get<1>(it->second)();
+ }
+ mutex_.unlock();
+ return dummy;
+}
+
+std::string RosPublish::getDocString() const
+{
+ return doc_string_;
+}
+
+/*
+ * RosPublish command
+ */
+namespace command
+{
+namespace ros_publish
+{
+Clear::Clear(RosPublish& entity, const std::string& doc_string)
+ : Command(entity, std::vector<Value::Type>(), doc_string)
+{
+}
+
+Value Clear::doExecute()
+{
+ RosPublish& entity = static_cast<RosPublish&>(owner());
+
+ entity.clear();
+ return Value();
+}
+
+List::List(RosPublish& entity, const std::string& doc_string)
+ : Command(entity, std::vector<Value::Type>(), doc_string)
+{
+}
+
+Value List::doExecute()
+{
+ RosPublish& entity = static_cast<RosPublish&>(owner());
+ return Value(entity.list());
+}
+
+Add::Add(RosPublish& entity, const std::string& doc_string)
+ : Command(
+ entity,
+ boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
+ doc_string)
+{
+}
+
+Value Add::doExecute()
+{
+ RosPublish& entity = static_cast<RosPublish&>(owner());
+ std::vector<Value> values = getParameterValues();
+
+ const std::string& type = values[0].value();
+ const std::string& signal = values[1].value();
+ const std::string& topic = values[2].value();
+
+ if (type == DgRosTypes::get_double())
+ {
+ entity.add<std_msgs::msg::Float64, double>(signal, topic);
+ }
+ else if (type == DgRosTypes::get_unsigned())
+ {
+ entity.add<std_msgs::msg::UInt32, unsigned int>(signal, topic);
+ }
+ else if (type == DgRosTypes::get_matrix())
+ {
+ entity.add<RosMatrix, DgMatrix>(signal, topic);
+ }
+ else if (type == DgRosTypes::get_vector())
+ {
+ entity.add<RosVector, DgVector>(signal, topic);
+ }
+ else if (type == DgRosTypes::get_vector3())
+ {
+ entity.add<geometry_msgs::msg::Vector3, DgVector>(signal, topic);
+ }
+ else if (type == DgRosTypes::get_vector3_stamped())
+ {
+ entity.add<geometry_msgs::msg::Vector3Stamped, DgVector>(signal, topic);
+ }
+ else if (type == DgRosTypes::get_matrix_homogeneous())
+ {
+ entity.add<geometry_msgs::msg::Transform, MatrixHomogeneous>(signal,
+ topic);
+ }
+ else if (type == DgRosTypes::get_matrix_homogeneous_stamped())
+ {
+ entity.add<geometry_msgs::msg::TransformStamped, MatrixHomogeneous>(
+ signal, topic);
+ }
+ else if (type == DgRosTypes::get_twist())
+ {
+ entity.add<geometry_msgs::msg::Twist, DgVector>(signal, topic);
+ }
+ else if (type == DgRosTypes::get_twist_stamped())
+ {
+ entity.add<geometry_msgs::msg::TwistStamped, DgVector>(signal, topic);
+ }
+ else
+ {
+ std::cerr << "RosPublish(" << entity.getName()
+ << ")::add(): bad type given (" << type << ")."
+ << " Possible choice is among:";
+ for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i)
+ {
+ std::cerr << " - " << DgRosTypes::type_list[i] << std::endl;
+ }
+ }
+ return Value();
+}
+
+Rm::Rm(RosPublish& entity, const std::string& doc_string)
+ : Command(entity, boost::assign::list_of(Value::STRING), doc_string)
+{
+}
+
+Value Rm::doExecute()
+{
+ RosPublish& entity = static_cast<RosPublish&>(owner());
+ std::vector<Value> values = getParameterValues();
+ const std::string& signal = values[0].value();
+ entity.rm(signal);
+ return Value();
+}
+} // namespace ros_publish
+} // end of namespace command.
+
+} // namespace dynamic_graph_bridge
diff --git a/ros2/src/ros_publish.hpp b/ros2/src/ros_publish.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..edb4f6ccc014540f9a0dcd3f587be85e0978e712
--- /dev/null
+++ b/ros2/src/ros_publish.hpp
@@ -0,0 +1,182 @@
+/**
+ * @file
+ * @license BSD 3-clause
+ * @copyright Copyright (c) 2020, New York University and Max Planck
+ * Gesellschaft
+ *
+ * @brief Declare the RosPublish class.
+ */
+
+#pragma once
+
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/signal-time-dependent.h>
+
+#include <iostream>
+#include <map>
+#include <mutex>
+
+#include "dynamic_graph_bridge/ros_entities/dg_ros_mapping.hpp"
+
+namespace dynamic_graph_bridge
+{
+/** @brief Publish dynamic-graph information into ROS. */
+class RosPublish : public dynamicgraph::Entity
+{
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+public:
+ /**
+ * @brief Callback function publishing in the ROS topic.
+ * This callback is called during the trigger signal callback function.
+ */
+ typedef std::function<void(void)> PublisherCallback;
+
+ /**
+ * @brief Tuple composed by the generated input signal and its callback
+ * function. The callback function publishes the input signal content.
+ */
+ typedef std::tuple<std::shared_ptr<dynamicgraph::SignalBase<int> >,
+ PublisherCallback>
+ BindedSignal;
+
+ /**
+ * @brief Construct a new RosPublish object.
+ *
+ * @param n
+ */
+ RosPublish(const std::string& n);
+
+ /**
+ * @brief Destroy the RosPublish object.
+ *
+ */
+ virtual ~RosPublish();
+
+ /**
+ * @brief Get the documentation string.
+ *
+ * @return std::string
+ */
+ virtual std::string getDocString() const;
+
+ /**
+ * @brief Display the class information.
+ *
+ * @param os
+ */
+ void display(std::ostream& os) const;
+
+ /**
+ * @brief Add a signal to publish to ROS.
+ *
+ * @tparam RosType type of the ROS message.
+ * @tparam DgType type of the Dynamic Graph signal data.
+ * @param signal name.
+ * @param topic name.
+ */
+ template <typename RosType, typename DgType>
+ void add(const std::string& signal, const std::string& topic);
+
+ /**
+ * @brief Remove a signal to publish to ROS.
+ *
+ * @param signal name.
+ */
+ void rm(const std::string& signal);
+
+ /**
+ * @brief List of all signal and topics currently published.
+ *
+ * @return std::string
+ */
+ std::string list() const;
+
+ /**
+ * @brief Remove all signal published to ROS.
+ */
+ void clear();
+
+private:
+ /**
+ * @brief Trigger signal callback method.
+ *
+ * @return int&
+ */
+ int& trigger(int&, int);
+
+ /**
+ * @brief Send the data from the input signal to the ROS topic.
+ *
+ * @tparam RosType ROS message type.
+ * @tparam DgType Dynamic graph signal data type.
+ * @param publisher pointer to the ros publisher.
+ * @param signal signal name.
+ */
+ template <typename RosType, typename DgType>
+ void send_data(
+ std::shared_ptr<rclcpp::Publisher<
+ typename DgRosMapping<RosType, DgType>::ros_t> > publisher,
+ std::shared_ptr<typename DgRosMapping<RosType, DgType>::signal_in_t>
+ signal);
+
+private:
+ /** @brief Doc string associated to the entity. */
+ static const std::string doc_string_;
+
+ /** @brief Name of the trigger signal. */
+ static const std::string trigger_signal_name_;
+
+ /** @brief Trigger signal publishing the signal values every other
+ * iterations. */
+ dynamicgraph::SignalTimeDependent<int, int> trigger_;
+
+ /** @brief Publishing rate. Default is 50ms. */
+ rclcpp::Duration rate_nanosec_;
+
+ /** @brief ROS node pointer. */
+ RosNodePtr ros_node_;
+
+ /** @brief Named list of signals associated with it's callback functions. */
+ std::map<std::string, BindedSignal> binded_signals_;
+
+ /** @brief Last published time. */
+ rclcpp::Time last_publicated_;
+
+ /** @brief Protects the command and trigger callbacks. */
+ std::mutex mutex_;
+};
+
+/*
+ * Commands
+ */
+namespace command
+{
+namespace ros_publish
+{
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+
+#define ROS_PUBLISH_MAKE_COMMAND(CMD) \
+ class CMD : public Command \
+ { \
+ public: \
+ CMD(RosPublish& entity, const std::string& docstring); \
+ virtual Value doExecute(); \
+ }
+
+ROS_PUBLISH_MAKE_COMMAND(Add);
+ROS_PUBLISH_MAKE_COMMAND(Clear);
+ROS_PUBLISH_MAKE_COMMAND(List);
+ROS_PUBLISH_MAKE_COMMAND(Rm);
+
+#undef ROS_PUBLISH_MAKE_COMMAND
+
+} // namespace ros_publish
+} // end of namespace command.
+
+} // namespace dynamic_graph_bridge
+
+#include "ros_publish.hxx"
diff --git a/ros2/src/ros_publish.hxx b/ros2/src/ros_publish.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..8e1422cd84d5921442bc595282c99ed68839b2df
--- /dev/null
+++ b/ros2/src/ros_publish.hxx
@@ -0,0 +1,59 @@
+/**
+ * @file
+ * @license BSD 3-clause
+ * @copyright Copyright (c) 2020, New York University and Max Planck
+ * Gesellschaft
+ *
+ * @brief Implements the template method of the RosPublish class.
+ */
+
+#pragma once
+
+#include "dynamic_graph_bridge/ros_entities/dg_ros_mapping.hpp"
+
+namespace dynamic_graph_bridge
+{
+template <typename RosType, typename DgType>
+void RosPublish::send_data(
+ std::shared_ptr<rclcpp::Publisher<
+ typename DgRosMapping<RosType, DgType>::ros_t> > publisher,
+ std::shared_ptr<typename DgRosMapping<RosType, DgType>::signal_in_t> signal)
+{
+ typename DgRosMapping<RosType, DgType>::ros_t msg;
+ DgRosMapping<RosType, DgType>::dg_to_ros(signal->accessCopy(), msg);
+ publisher->publish(msg);
+}
+
+template <typename RosType, typename DgType>
+void RosPublish::add(const std::string& signal, const std::string& topic)
+{
+ using ros_t = typename DgRosMapping<RosType, DgType>::ros_t;
+ using signal_in_t = typename DgRosMapping<RosType, DgType>::signal_in_t;
+
+ // Initialize the bindedSignal object.
+ BindedSignal binded_signal;
+
+ // Initialize the publisher.
+ std::shared_ptr<rclcpp::Publisher<ros_t> > pub_ptr =
+ ros_node_->create_publisher<ros_t>(topic, 10);
+
+ // Initialize the signal.
+ std::shared_ptr<signal_in_t> signal_ptr = std::make_shared<signal_in_t>(
+ nullptr, // no explicit dependence
+ make_signal_string(*this,
+ true,
+ DgRosMapping<RosType, DgType>::signal_type_name,
+ signal));
+ std::get<0>(binded_signal) = signal_ptr;
+ DgRosMapping<RosType, DgType>::set_default(signal_ptr);
+ signalRegistration(*std::get<0>(binded_signal));
+
+ // Initialize the callback.
+ PublisherCallback callback = std::bind(
+ &RosPublish::send_data<RosType, DgType>, this, pub_ptr, signal_ptr);
+ std::get<1>(binded_signal) = callback;
+
+ binded_signals_[signal] = binded_signal;
+}
+
+} // namespace dynamic_graph_bridge
diff --git a/ros2/src/ros_python_interpreter_client.cpp b/ros2/src/ros_python_interpreter_client.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..487001698661ca0aac29bb6b9037a8c3f776e513
--- /dev/null
+++ b/ros2/src/ros_python_interpreter_client.cpp
@@ -0,0 +1,157 @@
+/**
+ * @file
+ * @license BSD 3-clause
+ * @copyright Copyright (c) 2020, New York University and Max Planck
+ * Gesellschaft
+ *
+ * @brief Defines a C++ interface for the ros-service of the dynamic-graph
+ * python interpreter. (Available in python through bindings.)
+ */
+
+#include "dynamic_graph_bridge/ros_python_interpreter_client.hpp"
+
+#include <boost/algorithm/string.hpp>
+#include <fstream>
+
+namespace dynamic_graph_bridge
+{
+RosPythonInterpreterClient::RosPythonInterpreterClient()
+{
+ ros_node_name_ = "ros_python_interpreter_client";
+ ros_node_ = get_ros_node(ros_node_name_);
+
+ // Create a client for the single python command service of the
+ // DynamicGraphManager.
+ run_command_service_name_ = "/dynamic_graph_manager/run_python_command";
+ run_command_request_ = std::make_shared<RunPythonCommandSrvType::Request>();
+ connect_to_rosservice_run_python_command();
+
+ // Create a client for the python file reading service of the
+ // DynamicGraphManager.
+ run_script_service_name_ = "/dynamic_graph_manager/run_python_file";
+ run_file_request_ = std::make_shared<RunPythonFileSrvType::Request>();
+ connect_to_rosservice_run_python_script();
+ timeout_connection_s_ = DurationSec(1);
+}
+
+std::string RosPythonInterpreterClient::run_python_command(
+ const std::string& code_string)
+{
+ std::string return_string = "";
+
+ // If the code is empty or just a line break, return an empty string.
+ if (code_string == "" || code_string == "\n")
+ {
+ return return_string;
+ }
+
+ try
+ {
+ if (!command_client_->wait_for_service(timeout_connection_s_))
+ {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Connection to remote server lost. Reconnecting...");
+ connect_to_rosservice_run_python_command(timeout_connection_s_);
+ return return_string;
+ }
+
+ run_command_request_->input = code_string;
+
+ auto response =
+ command_client_->async_send_request(run_command_request_);
+ // Wait for the result.
+ while (rclcpp::ok() &&
+ rclcpp::spin_until_future_complete(
+ ros_node_, response, std::chrono::seconds(1)) !=
+ rclcpp::executor::FutureReturnCode::SUCCESS)
+ {
+ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),
+ "Error while parsing command, retrying...");
+ }
+
+ // Get the standard output (print).
+ return_string += response.get()->standardoutput;
+
+ // Get the error.
+ return_string += response.get()->standarderror;
+
+ // Get the Result and print it is any.
+ if (response.get()->result != "None")
+ {
+ return_string += response.get()->result;
+ }
+ }
+ catch (const std::exception& ex)
+ {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ ex.what());
+ connect_to_rosservice_run_python_command(timeout_connection_s_);
+ }
+ catch (...)
+ {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Error caught, maybe the connection to remote server is "
+ "lost. Reconnecting...");
+ connect_to_rosservice_run_python_command(timeout_connection_s_);
+ }
+ return return_string;
+} // namespace dynamic_graph_bridge
+
+std::string RosPythonInterpreterClient::run_python_script(
+ const std::string& filename)
+{
+ std::string return_string = "";
+
+ std::ifstream file_if(filename.c_str());
+ if (!file_if.good())
+ {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Provided file does not exist: %s",
+ filename.c_str());
+ return return_string;
+ }
+
+ try
+ {
+ if (!script_client_->wait_for_service(timeout_connection_s_))
+ {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Connection to remote server lost. Reconnecting...");
+ connect_to_rosservice_run_python_script(timeout_connection_s_);
+ return return_string;
+ }
+
+ run_file_request_->input = filename;
+
+ auto response = script_client_->async_send_request(run_file_request_);
+
+ if (rclcpp::spin_until_future_complete(ros_node_, response) ==
+ rclcpp::executor::FutureReturnCode::SUCCESS)
+ {
+ // Get the error.
+ return_string += response.get()->result;
+
+ // Get the Result and print it is any.
+ return_string += response.get()->result;
+ if (!boost::algorithm::ends_with(return_string, "\n"))
+ {
+ return_string += "\n";
+ }
+ }
+ else
+ {
+ // We had an issue calling the service.
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Error while parsing scripts.");
+ }
+ }
+ catch (...)
+ {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Connection to remote server lost. Reconnecting...");
+ connect_to_rosservice_run_python_script(timeout_connection_s_);
+ }
+ return return_string;
+}
+
+} // namespace dynamic_graph_bridge
\ No newline at end of file
diff --git a/ros2/src/ros_python_interpreter_server.cpp b/ros2/src/ros_python_interpreter_server.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa7ddb4b9963f30486eceeec8e95276c85363af3
--- /dev/null
+++ b/ros2/src/ros_python_interpreter_server.cpp
@@ -0,0 +1,96 @@
+/**
+ * @file ros_interpreter.cpp
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ * @date 2019-05-22
+ */
+
+#include "dynamic_graph_bridge/ros_python_interpreter_server.hpp"
+
+#include <boost/algorithm/string/predicate.hpp>
+#include <functional>
+#include <memory>
+
+namespace dynamic_graph_bridge
+{
+static const int queueSize = 5;
+
+RosPythonInterpreterServer::RosPythonInterpreterServer()
+ : interpreter_(),
+ ros_node_(get_ros_node("python_interpreter")),
+ run_python_command_srv_(nullptr),
+ run_python_file_srv_(nullptr)
+{
+ ros_add_node_to_executor("python_interpreter");
+}
+
+RosPythonInterpreterServer::~RosPythonInterpreterServer()
+{
+}
+
+void RosPythonInterpreterServer::start_ros_service()
+{
+ run_python_command_callback_t runCommandCb =
+ std::bind(&RosPythonInterpreterServer::runCommandCallback,
+ this,
+ std::placeholders::_1,
+ std::placeholders::_2);
+ run_python_command_srv_ =
+ ros_node_->create_service<RunPythonCommandSrvType>("run_python_command",
+ runCommandCb);
+
+ run_python_file_callback_t runPythonFileCb =
+ std::bind(&RosPythonInterpreterServer::runPythonFileCallback,
+ this,
+ std::placeholders::_1,
+ std::placeholders::_2);
+ run_python_file_srv_ = ros_node_->create_service<RunPythonFileSrvType>(
+ "run_python_file", runPythonFileCb);
+}
+
+void RosPythonInterpreterServer::runCommandCallback(
+ RunPythonCommandRequestPtr req, RunPythonCommandResponsePtr res)
+{
+ std::stringstream buffer;
+ std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
+
+ run_python_command(
+ req->input, res->result, res->standardoutput, res->standarderror);
+
+ if (!buffer.str().empty())
+ {
+ if (!boost::algorithm::ends_with(res->standardoutput, "\n") &&
+ !res->standardoutput.empty())
+ {
+ res->standardoutput += "\n";
+ }
+ res->standardoutput += buffer.str();
+ }
+ std::cout.rdbuf(old);
+
+ std::cout << res->standardoutput << std::endl;
+}
+
+void RosPythonInterpreterServer::runPythonFileCallback(
+ RunPythonFileRequestPtr req, RunPythonFileResponsePtr res)
+{
+ run_python_file(req->input, res->result);
+}
+
+void RosPythonInterpreterServer::run_python_command(const std::string& command,
+ std::string& result,
+ std::string& out,
+ std::string& err)
+{
+ interpreter_.python(command, result, out, err);
+}
+
+void RosPythonInterpreterServer::run_python_file(const std::string ifilename,
+ std::string& result)
+{
+ interpreter_.runPythonFile(ifilename, result);
+}
+
+} // namespace dynamic_graph_bridge
diff --git a/ros2/src/ros_sot_loader.cpp b/ros2/src/ros_sot_loader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2/src/ros_subscribe-python-module-py.cc b/ros2/src/ros_subscribe-python-module-py.cc
new file mode 100644
index 0000000000000000000000000000000000000000..dd02bf58baa17f86841cadfdaeecf56bd6abe0c4
--- /dev/null
+++ b/ros2/src/ros_subscribe-python-module-py.cc
@@ -0,0 +1,19 @@
+#include <dynamic-graph/python/module.hh>
+#include "ros_subscribe.hh"
+
+namespace dg = dynamicgraph;
+
+BOOST_PYTHON_MODULE(wrap)
+{
+ bp::import("dynamic_graph");
+
+ dg::python::exposeEntity<dg::RosSubscribe, bp::bases<dg::Entity>,
+ dg::python::AddCommands>()
+ .def("clear", &dg::RosSubscribe::clear,
+ "Remove all signals reading data from a ROS topic")
+ .def("rm", &dg::RosSubscribe::rm,
+ "Remove a signal reading data from a ROS topic",
+ bp::args("signal_name"))
+ .def("list", &dg::RosSubscribe::list,
+ "List signals reading data from a ROS topic");
+}
diff --git a/ros2/src/ros_subscribe.cpp b/ros2/src/ros_subscribe.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf0220003ec2dca66bc0a6e9238b2562634f225d
--- /dev/null
+++ b/ros2/src/ros_subscribe.cpp
@@ -0,0 +1,252 @@
+/**
+ * @file ros_subscribe.cpp
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ * @date 2019-05-22
+ */
+
+#include "dynamic_graph_bridge/ros_entities/ros_subscribe.hpp"
+
+#include <dynamic-graph/factory.h>
+
+#include "dynamic_graph_bridge/dynamic_graph_manager.hpp"
+
+namespace dynamic_graph_bridge
+{
+/*
+ * RosSubscribe
+ */
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
+
+const std::string RosSubscribe::doc_string_(
+ "Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n"
+ "\n"
+ " Use command \"add\" to subscribe to a new signal.\n");
+
+RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n)
+{
+ ros_node_ = get_ros_node(DG_ROS_NODE_NAME);
+ ros_add_node_to_executor(DG_ROS_NODE_NAME);
+ ros_spin_non_blocking();
+ binded_signals_.clear();
+ std::string doc_string =
+ "\n"
+ " Add a signal reading data from a ROS topic.\n"
+ "\n"
+ " Input:\n"
+ " - type: string among:\n";
+ for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i)
+ {
+ doc_string += " - " + DgRosTypes::type_list[i] + "\n";
+ }
+ doc_string +=
+ " - signal: the signal name in dynamic-graph,\n"
+ " - topic: the topic name in ROS.\n"
+ "\n";
+ addCommand("add", new command::ros_subscribe::Add(*this, doc_string));
+ doc_string =
+ "\n"
+ " Remove a signal reading data from a ROS topic\n"
+ "\n"
+ " Input:\n"
+ " - name of the signal to remove (see method list for the list of "
+ "signals).\n"
+ "\n";
+ addCommand("rm", new command::ros_subscribe::Rm(*this, doc_string));
+ doc_string =
+ "\n"
+ " Remove all signals reading data from a ROS topic\n"
+ "\n"
+ " No input:\n"
+ "\n";
+ addCommand("clear", new command::ros_subscribe::Clear(*this, doc_string));
+ doc_string =
+ "\n"
+ " List signals reading data from a ROS topic\n"
+ "\n"
+ " No input:\n"
+ "\n";
+ addCommand("list", new command::ros_subscribe::List(*this, doc_string));
+}
+
+RosSubscribe::~RosSubscribe()
+{
+ clear();
+}
+
+void RosSubscribe::display(std::ostream& os) const
+{
+ os << CLASS_NAME << std::endl;
+}
+
+void RosSubscribe::rm(const std::string& signal)
+{
+ std::string signalTs = signal + "Timestamp";
+
+ signalDeregistration(signal);
+ binded_signals_.erase(signal);
+
+ if (binded_signals_.find(signalTs) != binded_signals_.end())
+ {
+ signalDeregistration(signalTs);
+ binded_signals_.erase(signalTs);
+ }
+}
+
+std::string RosSubscribe::list()
+{
+ std::string result("[");
+ for (std::map<std::string, BindedSignal>::const_iterator it =
+ binded_signals_.begin();
+ it != binded_signals_.end();
+ it++)
+ {
+ result += "'" + it->first + "',";
+ }
+ result += "]";
+ return result;
+}
+
+void RosSubscribe::clear()
+{
+ std::map<std::string, BindedSignal>::iterator it = binded_signals_.begin();
+ for (; it != binded_signals_.end();)
+ {
+ rm(it->first);
+ it = binded_signals_.begin();
+ }
+}
+
+std::string RosSubscribe::getDocString() const
+{
+ return doc_string_;
+}
+
+/*
+ * Commands
+ */
+
+namespace command
+{
+namespace ros_subscribe
+{
+Clear::Clear(RosSubscribe& entity, const std::string& doc_string)
+ : Command(entity, std::vector<Value::Type>(), doc_string)
+{
+}
+
+Value Clear::doExecute()
+{
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+
+ entity.clear();
+ return Value();
+}
+
+List::List(RosSubscribe& entity, const std::string& doc_string)
+ : Command(entity, std::vector<Value::Type>(), doc_string)
+{
+}
+
+Value List::doExecute()
+{
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+ return Value(entity.list());
+}
+
+Add::Add(RosSubscribe& entity, const std::string& doc_string)
+ : Command(
+ entity,
+ boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
+ doc_string)
+{
+}
+
+Value Add::doExecute()
+{
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+ std::vector<Value> values = getParameterValues();
+
+ const std::string& type = values[0].value();
+ const std::string& signal_name = values[1].value();
+ const std::string& topic_name = values[2].value();
+
+ if (type == DgRosTypes::get_double())
+ {
+ entity.add<std_msgs::msg::Float64, double>(signal_name, topic_name);
+ }
+ else if (type == DgRosTypes::get_unsigned())
+ {
+ entity.add<std_msgs::msg::UInt32, unsigned int>(signal_name,
+ topic_name);
+ }
+ else if (type == DgRosTypes::get_matrix())
+ {
+ entity.add<RosMatrix, DgMatrix>(signal_name, topic_name);
+ }
+ else if (type == DgRosTypes::get_vector())
+ {
+ entity.add<RosVector, DgVector>(signal_name, topic_name);
+ }
+ else if (type == DgRosTypes::get_vector3())
+ {
+ entity.add<geometry_msgs::msg::Vector3, DgVector>(signal_name,
+ topic_name);
+ }
+ else if (type == DgRosTypes::get_vector3_stamped())
+ {
+ entity.add<geometry_msgs::msg::Vector3Stamped, DgVector>(signal_name,
+ topic_name);
+ }
+ else if (type == DgRosTypes::get_matrix_homogeneous())
+ {
+ entity.add<geometry_msgs::msg::Transform, MatrixHomogeneous>(
+ signal_name, topic_name);
+ }
+ else if (type == DgRosTypes::get_matrix_homogeneous_stamped())
+ {
+ entity.add<geometry_msgs::msg::TransformStamped, MatrixHomogeneous>(
+ signal_name, topic_name);
+ }
+ else if (type == DgRosTypes::get_twist())
+ {
+ entity.add<geometry_msgs::msg::Twist, DgVector>(signal_name,
+ topic_name);
+ }
+ else if (type == DgRosTypes::get_twist_stamped())
+ {
+ entity.add<geometry_msgs::msg::TwistStamped, DgVector>(signal_name,
+ topic_name);
+ }
+ else
+ {
+ std::cerr << "RosSubscribe(" << entity.getName()
+ << ")::add(): bad type given (" << type << ")."
+ << " Possible choice is among:";
+ for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i)
+ {
+ std::cerr << " - " << DgRosTypes::type_list[i] << std::endl;
+ }
+ }
+ return Value();
+}
+
+Rm::Rm(RosSubscribe& entity, const std::string& doc_string)
+ : Command(entity, boost::assign::list_of(Value::STRING), doc_string)
+{
+}
+
+Value Rm::doExecute()
+{
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+ std::vector<Value> values = getParameterValues();
+ const std::string& signal = values[0].value();
+ entity.rm(signal);
+ return Value();
+}
+} // namespace ros_subscribe
+} // end of namespace command.
+
+} // namespace dynamic_graph_bridge
diff --git a/ros2/src/ros_subscribe.hpp b/ros2/src/ros_subscribe.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a4e40c0b43aa7f149be7097ebf4fff9c037bdc5e
--- /dev/null
+++ b/ros2/src/ros_subscribe.hpp
@@ -0,0 +1,157 @@
+/**
+ * @file
+ * @license BSD 3-clause
+ * @copyright Copyright (c) 2020, New York University and Max Planck
+ * Gesellschaft
+ *
+ * @brief Entity that subscribe to rostopic and forward the data to the dynamic
+ * graph trough a signal.
+ */
+
+#pragma once
+
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+
+#include <map>
+#include <mutex>
+
+#include "dynamic_graph_bridge/ros_entities/dg_ros_mapping.hpp"
+
+namespace dynamic_graph_bridge
+{
+/**
+ * @brief Publish ROS information in the dynamic-graph.
+ */
+class RosSubscribe : public dynamicgraph::Entity
+{
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+public:
+ /**
+ * @brief Tuple composed by the generated input signal and its callback
+ * function. The callback function publishes the input signal content.
+ */
+ typedef std::tuple<std::shared_ptr<dynamicgraph::SignalBase<int> >,
+ std::shared_ptr<dynamicgraph::SignalBase<int> >,
+ rclcpp::SubscriptionBase::SharedPtr>
+ BindedSignal;
+
+ /**
+ * @brief Construct a new Ros Subscribe object
+ *
+ * @param name entity name.
+ */
+ RosSubscribe(const std::string& name);
+
+ /**
+ * @brief Destroy the Ros Subscribe object.
+ */
+ virtual ~RosSubscribe();
+
+ /**
+ * @brief Get Doc String.
+ *
+ * @return std::string
+ */
+ virtual std::string getDocString() const;
+
+ /**
+ * @brief Display information about the entity.
+ *
+ * @param os
+ */
+ void display(std::ostream& os) const;
+
+ /**
+ * @brief Subscribe to a topic and add a signal containing the topic data.
+ *
+ * @tparam RosType
+ * @tparam DgType
+ * @param signal_name
+ * @param topic_name
+ */
+ template <typename RosType, typename DgType>
+ void add(const std::string& signal_name, const std::string& topic_name);
+
+ /**
+ * @brief Unsubscribe to a topic and remove the signal.
+ *
+ * @param signal
+ */
+ void rm(const std::string& signal);
+
+ /**
+ * @brief List of all subscribed topic and singals.
+ *
+ * @return std::string
+ */
+ std::string list();
+
+ /**
+ * @brief Unsubscribe to all topics and remove all signals.
+ *
+ */
+ void clear();
+
+private:
+ /**
+ * @brief This callback feeds the data signal upon reception of a ROS
+ * message.
+ *
+ * @tparam RosType
+ * @tparam DgType
+ * @param signal pointer to the dynamic graph signal.
+ * @param data ROS data to copy from.
+ */
+ template <typename RosType, typename DgType>
+ void callback(
+ std::shared_ptr<typename DgRosMapping<RosType, DgType>::signal_out_t>
+ signal_out,
+ std::shared_ptr<
+ typename DgRosMapping<RosType, DgType>::signal_timestamp_out_t>
+ signal_timestamp_out,
+ const std::shared_ptr<typename DgRosMapping<RosType, DgType>::ros_t>
+ ros_data);
+
+private:
+ /** @brief Doc string associated to the entity. */
+ static const std::string doc_string_;
+
+ /** @brief ROS node pointer. */
+ RosNodePtr ros_node_;
+
+ /** @brief Named list of signals associated with it's callback functions. */
+ std::map<std::string, BindedSignal> binded_signals_;
+};
+
+namespace command
+{
+namespace ros_subscribe
+{
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+
+#define ROS_SUBSCRIBE_MAKE_COMMAND(CMD) \
+ class CMD : public Command \
+ { \
+ public: \
+ CMD(RosSubscribe& entity, const std::string& doc_string); \
+ virtual Value doExecute(); \
+ }
+
+ROS_SUBSCRIBE_MAKE_COMMAND(Add);
+ROS_SUBSCRIBE_MAKE_COMMAND(Clear);
+ROS_SUBSCRIBE_MAKE_COMMAND(List);
+ROS_SUBSCRIBE_MAKE_COMMAND(Rm);
+
+#undef ROS_SUBSCRIBE_MAKE_COMMAND
+
+} // namespace ros_subscribe
+} // end of namespace command.
+
+} // namespace dynamic_graph_bridge
+
+#include "ros_subscribe.hxx"
diff --git a/ros2/src/ros_subscribe.hxx b/ros2/src/ros_subscribe.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..ccde69d910fdd09cb27d7a89527fd5bdeeb6b17e
--- /dev/null
+++ b/ros2/src/ros_subscribe.hxx
@@ -0,0 +1,112 @@
+/**
+ * @file ros_subscribe.hxx
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ * @date 2019-05-22
+ */
+
+#pragma once
+
+#include <message_filters/message_traits.h>
+
+#include "dynamic_graph_bridge/ros_entities/dg_ros_mapping.hpp"
+
+namespace dynamic_graph_bridge
+{
+template <typename RosType, typename DgType>
+void RosSubscribe::callback(
+ std::shared_ptr<typename DgRosMapping<RosType, DgType>::signal_out_t>
+ signal_out,
+ std::shared_ptr<
+ typename DgRosMapping<RosType, DgType>::signal_timestamp_out_t>
+ signal_timestamp_out,
+ const std::shared_ptr<typename DgRosMapping<RosType, DgType>::ros_t>
+ ros_data)
+{
+ // Some convenient shortcut.
+ using ros_t = typename DgRosMapping<RosType, DgType>::ros_t;
+ using dg_t = typename DgRosMapping<RosType, DgType>::dg_t;
+
+ // Create a dynamic graph object to copy the information in. Not real time
+ // safe?
+ dg_t value;
+ // Convert the ROS topic into a dynamic graph type.
+ DgRosMapping<RosType, DgType>::ros_to_dg(*ros_data, value);
+ // Update the signal.
+ signal_out->setConstant(value);
+ // If he ROS message has a header then copy the header into the timestamp
+ // signal
+ if (signal_timestamp_out)
+ {
+ signal_timestamp_out->setConstant(
+ DgRosMapping<RosType, DgType>::from_ros_time(
+ message_filters::message_traits::TimeStamp<ros_t>::value(
+ *ros_data)));
+ }
+}
+
+template <typename RosType, typename DgType>
+void RosSubscribe::add(const std::string& signal_name,
+ const std::string& topic_name)
+{
+ using dg_t = typename DgRosMapping<RosType, DgType>::dg_t;
+ using ros_t = typename DgRosMapping<RosType, DgType>::ros_t;
+ using signal_out_t = typename DgRosMapping<RosType, DgType>::signal_out_t;
+ using signal_timestamp_out_t =
+ typename DgRosMapping<RosType, DgType>::signal_timestamp_out_t;
+
+ if (binded_signals_.find(signal_name) != binded_signals_.end())
+ {
+ std::cout << "Signal already created, nothing to be done." << std::endl;
+ return;
+ }
+
+ // Initialize the binded_signal object.
+ RosSubscribe::BindedSignal binded_signal;
+
+ // Initialize the signal.
+ std::string full_signal_name =
+ this->getClassName() + "(" + this->getName() + ")::" + signal_name;
+ std::shared_ptr<signal_out_t> signal_ptr =
+ std::make_shared<signal_out_t>(full_signal_name);
+ DgRosMapping<RosType, DgType>::set_default(signal_ptr);
+ signal_ptr->setDependencyType(
+ dynamicgraph::TimeDependency<int>::ALWAYS_READY);
+ std::get<0>(binded_signal) = signal_ptr;
+ this->signalRegistration(*std::get<0>(binded_signal));
+
+ // Initialize the time stamp signal if needed.
+ std::shared_ptr<signal_timestamp_out_t> signal_timestamp_ptr = nullptr;
+ if (message_filters::message_traits::HasHeader<ros_t>())
+ {
+ std::string full_time_stamp_signal_name =
+ this->getClassName() + "(" + this->getName() + ")::" + signal_name +
+ "_timestamp";
+ signal_timestamp_ptr = std::make_shared<signal_timestamp_out_t>(
+ full_time_stamp_signal_name);
+ signal_timestamp_ptr->setConstant(
+ DgRosMapping<RosType, DgType>::epoch_time());
+ signal_timestamp_ptr->setDependencyType(
+ dynamicgraph::TimeDependency<int>::ALWAYS_READY);
+ this->signalRegistration(*signal_timestamp_ptr);
+ }
+ std::get<1>(binded_signal) = signal_timestamp_ptr;
+
+ // Initialize the subscriber.
+ std::function<void(const std::shared_ptr<ros_t>)> callback =
+ std::bind(&RosSubscribe::callback<ros_t, dg_t>,
+ this,
+ signal_ptr,
+ signal_timestamp_ptr,
+ std::placeholders::_1);
+ typename rclcpp::Subscription<ros_t>::SharedPtr sub =
+ ros_node_->create_subscription<ros_t>(topic_name, 10, callback);
+ std::get<2>(binded_signal) = sub;
+
+ // Store the different pointers.
+ binded_signals_[signal_name] = binded_signal;
+}
+
+} // namespace dynamic_graph_bridge
diff --git a/ros2/src/time_point_io.hpp b/ros2/src/time_point_io.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0827b07929a852e7d634cbe7dd4709c74af61332
--- /dev/null
+++ b/ros2/src/time_point_io.hpp
@@ -0,0 +1,58 @@
+/**
+ * @file
+ * @license BSD 3-clause
+ * @copyright Copyright (c) 2020, New York University and Max Planck
+ * Gesellschaft
+ *
+ * @brief Define the time stamp signal.
+ */
+
+#pragma once
+
+#include <dynamic-graph/signal-caster.h>
+
+#include <chrono>
+
+namespace dynamic_graph_bridge
+{
+/** @brief Time stamp type. */
+typedef std::chrono::time_point<std::chrono::high_resolution_clock> timestamp_t;
+
+} // namespace dynamic_graph_bridge
+
+namespace dynamicgraph
+{
+/**
+ * @brief out stream the time stamp data.
+ *
+ * @param os
+ * @param time_stamp
+ * @return std::ostream&
+ */
+inline std::ostream &operator<<(
+ std::ostream &os, const dynamic_graph_bridge::timestamp_t &time_stamp)
+{
+ std::chrono::time_point<std::chrono::high_resolution_clock,
+ std::chrono::milliseconds>
+ time_stamp_nanosec =
+ std::chrono::time_point_cast<std::chrono::milliseconds>(time_stamp);
+ os << time_stamp_nanosec.time_since_epoch().count();
+ return os;
+}
+
+/**
+ * @brief Structure used to serialize/deserialize the time stamp.
+ *
+ * @tparam
+ */
+template <>
+struct signal_io<dynamic_graph_bridge::timestamp_t>
+ : signal_io_base<dynamic_graph_bridge::timestamp_t>
+{
+ inline static dynamic_graph_bridge::timestamp_t cast(std::istringstream &)
+ {
+ throw std::logic_error("this cast is not implemented.");
+ }
+};
+
+} // namespace dynamicgraph
diff --git a/ros2/tests/CMakeLists.txt b/ros2/tests/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..82f89d52f9633e6af8b7aff20882c76931f520ef
--- /dev/null
+++ b/ros2/tests/CMakeLists.txt
@@ -0,0 +1,52 @@
+# Copyright (C) 2021 LAAS-CNRS.
+#
+# Author: Maxmilien Naveau
+#
+
+if(BUILD_TESTING)
+ find_package(ament_cmake_gmock REQUIRED)
+ find_package(ament_lint_auto REQUIRED)
+ _ament_cmake_gmock_find_gmock()
+ find_package(launch_testing_ament_cmake)
+ ament_lint_auto_find_test_dependencies()
+
+ # Library for sot_external_interface
+ add_library(impl_test_sot_external_interface SHARED
+ tests/impl_test_sot_external_interface)
+
+ target_link_libraries(impl_test_sot_external_interface PUBLIC sot-core::sot-core)
+
+ # Executable for SotLoaderBasic test
+ add_executable(test_sot_loader_basic
+ tests/test_sot_loader_basic.cpp
+ )
+ target_include_directories(test_sot_loader_basic PUBLIC include "${GMOCK_INCLUDE_DIRS}")
+ target_link_libraries(test_sot_loader_basic sot_loader "${GMOCK_LIBRARIES}" )
+
+ add_launch_test(tests/launching_test_sot_loader_basic.py)
+
+ # Test for class SotLoader
+ add_executable(test_sot_loader
+ tests/test_sot_loader.cpp
+ )
+ target_include_directories(test_sot_loader PUBLIC include "${GMOCK_INCLUDE_DIRS}")
+ target_link_libraries(test_sot_loader sot_loader "${GMOCK_LIBRARIES}")
+
+ add_launch_test(tests/launching_test_sot_loader.py)
+
+ # Install tests
+ install(TARGETS test_sot_loader_basic
+ test_sot_loader
+ DESTINATION lib/${PROJECT_NAME}
+ )
+
+ # Install library for tests
+ install(TARGETS impl_test_sot_external_interface
+ DESTINATION lib
+ )
+
+ install(DIRECTORY launch urdf
+ DESTINATION share/${PROJECT_NAME}
+ )
+
+endif()
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index 9396cbf0720bcbf595b8bf0c55fac59d40fa9462..0000000000000000000000000000000000000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,68 +0,0 @@
-SET(plugins
- ros_publish
- ros_subscribe
- ros_queued_subscribe
- ros_tf_listener
- ros_time
- )
-
-FOREACH(plugin ${plugins})
- GET_FILENAME_COMPONENT(LIBRARY_NAME ${plugin} NAME)
- ADD_LIBRARY(${LIBRARY_NAME} SHARED ${plugin}.cpp ${plugin}.hh)
-
- IF(SUFFIX_SO_VERSION)
- SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
- ENDIF(SUFFIX_SO_VERSION)
-
- TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${${LIBRARY_NAME}_deps} ${catkin_LIBRARIES} ros_bridge
- dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
-
- IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
- INSTALL(TARGETS ${LIBRARY_NAME} EXPORT ${TARGETS_EXPORT_NAME}
- DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
- ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
-
- IF(BUILD_PYTHON_INTERFACE)
- STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
- if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
- DYNAMIC_GRAPH_PYTHON_MODULE("ros/${PYTHON_LIBRARY_NAME}"
- ${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
- SOURCE_PYTHON_MODULE "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
- elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
- DYNAMIC_GRAPH_PYTHON_MODULE("ros/${PYTHON_LIBRARY_NAME}"
- ${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
- MODULE_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
- endif()
- ENDIF(BUILD_PYTHON_INTERFACE)
-ENDFOREACH(plugin)
-
-target_link_libraries(ros_publish ros_bridge)
-
-IF(BUILD_PYTHON_INTERFACE)
- PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "__init__.py")
- PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "ros.py")
- PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "dgcompleter.py")
-
- # ros_interperter library.
- add_library(ros_interpreter ros_interpreter.cpp)
- TARGET_LINK_LIBRARIES(ros_interpreter ros_bridge ${catkin_LIBRARIES}
- dynamic-graph-python::dynamic-graph-python
- dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
-
- install(TARGETS ros_interpreter
- EXPORT ${TARGETS_EXPORT_NAME}
- DESTINATION lib)
-ENDIF(BUILD_PYTHON_INTERFACE)
-
-# Stand alone embedded intepreter with a robot controller.
-add_executable(geometric_simu geometric_simu.cpp sot_loader.cpp sot_loader_basic.cpp)
-target_link_libraries(geometric_simu Boost::program_options ${CMAKE_DL_LIBS} ${catkin_LIBRARIES} ros_bridge
- dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
-install(TARGETS geometric_simu
- DESTINATION lib/${PROJECT_NAME})
-
-# Sot loader library
-add_library(sot_loader sot_loader.cpp sot_loader_basic.cpp)
-target_link_libraries(sot_loader Boost::program_options ${catkin_LIBRARIES} ros_bridge
- dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
-install(TARGETS sot_loader EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)