From fe886ac1bba9972c18bda4febe6f9c6dc9ed07fe Mon Sep 17 00:00:00 2001
From: MaximilienNaveau <maximilien.naveau@gmail.com>
Date: Fri, 10 Dec 2021 16:25:29 +0100
Subject: [PATCH] remove ros1 and the ros2 subfolder. Set it to ros2
---
.github/workflows/ci_ubuntu18_04_ros1.yml | 48 ---
..._ros2.yml => ci_ubuntu20_04_foxy_ros2.yml} | 21 +-
.github/workflows/ci_ubuntu20_04_ros1.yml | 47 ---
CMakeLists.txt | 96 ++++--
.../dynamic_graph_bridge/ros.hpp | 0
.../dynamic_graph_bridge/ros_parameter.hpp | 0
.../ros_python_interpreter_client.hpp | 0
.../ros_python_interpreter_server.hpp | 0
.../dynamic_graph_bridge/sot_loader.hh | 0
.../dynamic_graph_bridge/sot_loader_basic.hh | 0
manifest.xml | 39 ---
package.xml | 63 +---
ros1/CMakeLists.txt | 78 -----
ros1/include/dynamic_graph_bridge/fwd.hh | 20 --
ros1/include/dynamic_graph_bridge/ros_init.hh | 18 -
.../dynamic_graph_bridge/ros_interpreter.hh | 60 ----
.../dynamic_graph_bridge/ros_parameter.hh | 9 -
.../dynamic_graph_bridge/sot_loader.hh | 102 ------
.../dynamic_graph_bridge/sot_loader_basic.hh | 115 -------
ros1/scripts/robot_pose_publisher | 30 --
ros1/scripts/run_command | 123 -------
ros1/scripts/tf_publisher | 63 ----
ros1/src/CMakeLists.txt | 198 -----------
ros1/src/converter.hh | 269 ---------------
ros1/src/dynamic_graph/ros/__init__.py | 6 -
ros1/src/dynamic_graph/ros/dgcompleter.py | 79 -----
ros1/src/dynamic_graph/ros/ros.py | 25 --
ros1/src/geometric_simu.cpp | 32 --
ros1/src/ros_init.cpp | 73 ----
ros1/src/ros_interpreter.cpp | 40 ---
ros1/src/ros_parameter.cpp | 54 ---
ros1/src/ros_publish-python-module-py.cc | 17 -
ros1/src/ros_publish.cpp | 188 -----------
ros1/src/ros_publish.hh | 88 -----
ros1/src/ros_publish.hxx | 63 ----
.../ros_queued_subscribe-python-module-py.cc | 25 --
ros1/src/ros_queued_subscribe.cpp | 142 --------
ros1/src/ros_queued_subscribe.hh | 178 ----------
ros1/src/ros_queued_subscribe.hxx | 110 -------
ros1/src/ros_subscribe-python-module-py.cc | 17 -
ros1/src/ros_subscribe.cpp | 114 -------
ros1/src/ros_subscribe.hh | 89 -----
ros1/src/ros_subscribe.hxx | 127 -------
ros1/src/ros_tf_listener-python-module-py.cc | 18 -
ros1/src/ros_tf_listener.cpp | 66 ----
ros1/src/ros_tf_listener.hh | 111 -------
ros1/src/ros_time-python.hh | 3 -
ros1/src/ros_time.cpp | 38 ---
ros1/src/ros_time.hh | 35 --
ros1/src/sot_loader.cpp | 219 ------------
ros1/src/sot_loader_basic.cpp | 191 -----------
ros1/src/sot_to_ros.cpp | 19 --
ros1/src/sot_to_ros.hh | 311 ------------------
ros1/tests/CMakeLists.txt | 4 -
ros2/CMakeLists.txt | 47 ---
ros2/src/CMakeLists.txt | 238 --------------
ros2/tests/test_import.py | 27 --
.../remote_python_client.py | 0
{ros2/scripts => scripts}/run_command | 0
src/CMakeLists.txt | 123 +++++++
{ros2/src => src}/dg_ros_mapping.cpp | 0
{ros2/src => src}/dg_ros_mapping.hpp | 0
.../src => src}/dynamic_graph/ros/__init__.py | 0
.../dynamic_graph/ros/dgcompleter.py | 0
{ros2/src => src}/dynamic_graph/ros/ros.py | 0
{ros2/src => src}/matrix_geometry.hpp | 0
{ros2/src => src}/programs/geometric_simu.cpp | 0
{ros2/src => src}/ros.cpp | 0
{ros2/src => src}/ros_parameter.cpp | 0
.../ros_publish-python-module-py.cc | 0
{ros2/src => src}/ros_publish.cpp | 0
{ros2/src => src}/ros_publish.hpp | 0
{ros2/src => src}/ros_publish.hxx | 0
.../ros_python_interpreter_client.cpp | 0
.../ros_python_interpreter_server.cpp | 0
.../ros_subscribe-python-module-py.cc | 0
{ros2/src => src}/ros_subscribe.cpp | 0
{ros2/src => src}/ros_subscribe.hpp | 0
{ros2/src => src}/ros_subscribe.hxx | 0
{ros2/src => src}/sot_loader.cpp | 0
{ros2/src => src}/sot_loader_basic.cpp | 0
{ros2/src => src}/time_point_io.hpp | 0
{ros2/tests => tests}/CMakeLists.txt | 6 +-
.../impl_test_sot_external_interface.cpp | 0
.../impl_test_sot_external_interface.hh | 0
.../launching_test_sot_loader.cpython-38.pyc | Bin 2062 -> 2063 bytes
...ching_test_sot_loader_basic.cpython-38.pyc | Bin 2103 -> 2098 bytes
.../launch/launching_test_sot_loader.py | 53 +--
.../launch/launching_test_sot_loader_basic.py | 0
{ros2/tests => tests}/main.cpp | 0
.../python_scripts/simple_add.py | 0
.../python_scripts/simple_add_fail | 0
{ros2/tests => tests}/test_common.hpp | 0
{ros1/tests => tests}/test_import.py | 0
{ros2/tests => tests}/test_ros_init.cpp | 0
.../tests => tests}/test_ros_interpreter.cpp | 2 +-
{ros2/tests => tests}/test_sot_loader.cpp | 0
.../tests => tests}/test_sot_loader_basic.cpp | 0
.../urdf/dgb_minimal_robot.urdf | 0
99 files changed, 253 insertions(+), 4124 deletions(-)
delete mode 100644 .github/workflows/ci_ubuntu18_04_ros1.yml
rename .github/workflows/{ci_ubuntu20_04_ros2.yml => ci_ubuntu20_04_foxy_ros2.yml} (53%)
delete mode 100644 .github/workflows/ci_ubuntu20_04_ros1.yml
rename {ros2/include => include}/dynamic_graph_bridge/ros.hpp (100%)
rename {ros2/include => include}/dynamic_graph_bridge/ros_parameter.hpp (100%)
rename {ros2/include => include}/dynamic_graph_bridge/ros_python_interpreter_client.hpp (100%)
rename {ros2/include => include}/dynamic_graph_bridge/ros_python_interpreter_server.hpp (100%)
rename {ros2/include => include}/dynamic_graph_bridge/sot_loader.hh (100%)
rename {ros2/include => include}/dynamic_graph_bridge/sot_loader_basic.hh (100%)
delete mode 100644 manifest.xml
delete mode 100644 ros1/CMakeLists.txt
delete mode 100644 ros1/include/dynamic_graph_bridge/fwd.hh
delete mode 100644 ros1/include/dynamic_graph_bridge/ros_init.hh
delete mode 100644 ros1/include/dynamic_graph_bridge/ros_interpreter.hh
delete mode 100644 ros1/include/dynamic_graph_bridge/ros_parameter.hh
delete mode 100644 ros1/include/dynamic_graph_bridge/sot_loader.hh
delete mode 100644 ros1/include/dynamic_graph_bridge/sot_loader_basic.hh
delete mode 100755 ros1/scripts/robot_pose_publisher
delete mode 100755 ros1/scripts/run_command
delete mode 100755 ros1/scripts/tf_publisher
delete mode 100644 ros1/src/CMakeLists.txt
delete mode 100644 ros1/src/converter.hh
delete mode 100644 ros1/src/dynamic_graph/ros/__init__.py
delete mode 100644 ros1/src/dynamic_graph/ros/dgcompleter.py
delete mode 100644 ros1/src/dynamic_graph/ros/ros.py
delete mode 100644 ros1/src/geometric_simu.cpp
delete mode 100644 ros1/src/ros_init.cpp
delete mode 100644 ros1/src/ros_interpreter.cpp
delete mode 100644 ros1/src/ros_parameter.cpp
delete mode 100644 ros1/src/ros_publish-python-module-py.cc
delete mode 100644 ros1/src/ros_publish.cpp
delete mode 100644 ros1/src/ros_publish.hh
delete mode 100644 ros1/src/ros_publish.hxx
delete mode 100644 ros1/src/ros_queued_subscribe-python-module-py.cc
delete mode 100644 ros1/src/ros_queued_subscribe.cpp
delete mode 100644 ros1/src/ros_queued_subscribe.hh
delete mode 100644 ros1/src/ros_queued_subscribe.hxx
delete mode 100644 ros1/src/ros_subscribe-python-module-py.cc
delete mode 100644 ros1/src/ros_subscribe.cpp
delete mode 100644 ros1/src/ros_subscribe.hh
delete mode 100644 ros1/src/ros_subscribe.hxx
delete mode 100644 ros1/src/ros_tf_listener-python-module-py.cc
delete mode 100644 ros1/src/ros_tf_listener.cpp
delete mode 100644 ros1/src/ros_tf_listener.hh
delete mode 100644 ros1/src/ros_time-python.hh
delete mode 100644 ros1/src/ros_time.cpp
delete mode 100644 ros1/src/ros_time.hh
delete mode 100644 ros1/src/sot_loader.cpp
delete mode 100644 ros1/src/sot_loader_basic.cpp
delete mode 100644 ros1/src/sot_to_ros.cpp
delete mode 100644 ros1/src/sot_to_ros.hh
delete mode 100644 ros1/tests/CMakeLists.txt
delete mode 100644 ros2/CMakeLists.txt
delete mode 100644 ros2/src/CMakeLists.txt
delete mode 100755 ros2/tests/test_import.py
rename {ros2/scripts => scripts}/remote_python_client.py (100%)
rename {ros2/scripts => scripts}/run_command (100%)
create mode 100644 src/CMakeLists.txt
rename {ros2/src => src}/dg_ros_mapping.cpp (100%)
rename {ros2/src => src}/dg_ros_mapping.hpp (100%)
rename {ros2/src => src}/dynamic_graph/ros/__init__.py (100%)
rename {ros2/src => src}/dynamic_graph/ros/dgcompleter.py (100%)
rename {ros2/src => src}/dynamic_graph/ros/ros.py (100%)
rename {ros2/src => src}/matrix_geometry.hpp (100%)
rename {ros2/src => src}/programs/geometric_simu.cpp (100%)
rename {ros2/src => src}/ros.cpp (100%)
rename {ros2/src => src}/ros_parameter.cpp (100%)
rename {ros2/src => src}/ros_publish-python-module-py.cc (100%)
rename {ros2/src => src}/ros_publish.cpp (100%)
rename {ros2/src => src}/ros_publish.hpp (100%)
rename {ros2/src => src}/ros_publish.hxx (100%)
rename {ros2/src => src}/ros_python_interpreter_client.cpp (100%)
rename {ros2/src => src}/ros_python_interpreter_server.cpp (100%)
rename {ros2/src => src}/ros_subscribe-python-module-py.cc (100%)
rename {ros2/src => src}/ros_subscribe.cpp (100%)
rename {ros2/src => src}/ros_subscribe.hpp (100%)
rename {ros2/src => src}/ros_subscribe.hxx (100%)
rename {ros2/src => src}/sot_loader.cpp (100%)
rename {ros2/src => src}/sot_loader_basic.cpp (100%)
rename {ros2/src => src}/time_point_io.hpp (100%)
rename {ros2/tests => tests}/CMakeLists.txt (94%)
rename {ros2/tests => tests}/impl_test_sot_external_interface.cpp (100%)
rename {ros2/tests => tests}/impl_test_sot_external_interface.hh (100%)
rename {ros2/tests => tests}/launch/__pycache__/launching_test_sot_loader.cpython-38.pyc (65%)
rename {ros2/tests => tests}/launch/__pycache__/launching_test_sot_loader_basic.cpython-38.pyc (89%)
rename {ros2/tests => tests}/launch/launching_test_sot_loader.py (65%)
rename {ros2/tests => tests}/launch/launching_test_sot_loader_basic.py (100%)
rename {ros2/tests => tests}/main.cpp (100%)
rename {ros2/tests => tests}/python_scripts/simple_add.py (100%)
rename ros2/tests/python_scripts/simple_add_fail.py => tests/python_scripts/simple_add_fail (100%)
rename {ros2/tests => tests}/test_common.hpp (100%)
rename {ros1/tests => tests}/test_import.py (100%)
rename {ros2/tests => tests}/test_ros_init.cpp (100%)
rename {ros2/tests => tests}/test_ros_interpreter.cpp (99%)
rename {ros2/tests => tests}/test_sot_loader.cpp (100%)
rename {ros2/tests => tests}/test_sot_loader_basic.cpp (100%)
rename {ros2/tests => tests}/urdf/dgb_minimal_robot.urdf (100%)
diff --git a/.github/workflows/ci_ubuntu18_04_ros1.yml b/.github/workflows/ci_ubuntu18_04_ros1.yml
deleted file mode 100644
index 1cb416d..0000000
--- a/.github/workflows/ci_ubuntu18_04_ros1.yml
+++ /dev/null
@@ -1,48 +0,0 @@
-name: Ubuntu18.04, ROS1 Continuous Integration
-
-on:
- push:
- branches:
- - master
- - devel
- - mnaveau/ubuntu20.04_ros1_ros2
- pull_request:
- branches:
- - master
- - devel
-
-jobs:
- build:
- runs-on: ubuntu-18.04
- steps:
- #
- # Setup the machines and build environment
- #
- - name: Install ROS.
- id: ros_install
- uses: ros-tooling/setup-ros@v0.2
- with:
- required-ros-distributions: melodic
-
- #
- # Checkout the current branch
- #
- - uses: actions/checkout@v2
-
- #
- # Clone the dependencies in the ROS workspace
- #
- - name: Clone dependencies in the current workspace.
- shell: bash
- run: |
- mkdir -p /home/runner/work/dynamic_graph_bridge/dynamic_graph_bridge/ros_ws/src
- cd /home/runner/work/dynamic_graph_bridge/dynamic_graph_bridge/ros_ws/src
- git clone --recursive https://github.com/stack-of-tasks/dynamic_graph_bridge_msgs.git
-
- #
- # Build and test the repo
- #
- - uses: ros-tooling/action-ros-ci@v0.2
- with:
- package-name: dynamic_graph_bridge
- target-ros1-distro: melodic
diff --git a/.github/workflows/ci_ubuntu20_04_ros2.yml b/.github/workflows/ci_ubuntu20_04_foxy_ros2.yml
similarity index 53%
rename from .github/workflows/ci_ubuntu20_04_ros2.yml
rename to .github/workflows/ci_ubuntu20_04_foxy_ros2.yml
index 20c3302..d61b608 100644
--- a/.github/workflows/ci_ubuntu20_04_ros2.yml
+++ b/.github/workflows/ci_ubuntu20_04_foxy_ros2.yml
@@ -34,14 +34,19 @@ jobs:
- name: Clone dependencies in the current workspace.
shell: bash
run: |
- mkdir -p /home/runner/work/dynamic_graph_bridge/dynamic_graph_bridge/ros_ws/src
- cd /home/runner/work/dynamic_graph_bridge/dynamic_graph_bridge/ros_ws/src
- git clone --recursive https://github.com/stack-of-tasks/dynamic_graph_bridge_msgs.git
+ ws_path = /home/runner/work/dynamic_graph_bridge/colcon_workspace
+ mkdir -p $ws_path/src
+ cd $ws_path/src
+ git clone -b devel --single-branch --recursive https://github.com/stack-of-tasks/dynamic_graph_bridge_msgs.git
+
+ #
+ # Build dep and checkout from source.
+ #
+ - name: Build packages.
+ ln -s /home/runner/work/dynamic_graph_bridge/dynamic_graph_bridge .
+ colcon build
#
- # Build and test the repo
+ # Test the checkout package
#
- - uses: ros-tooling/action-ros-ci@v0.2
- with:
- package-name: dynamic_graph_bridge
- target-ros2-distro: foxy
+ colcon test --packages-select dynamic_graph_bridge --event-handlers console_direct+ --return-code-on-test-failure
diff --git a/.github/workflows/ci_ubuntu20_04_ros1.yml b/.github/workflows/ci_ubuntu20_04_ros1.yml
deleted file mode 100644
index 55edc39..0000000
--- a/.github/workflows/ci_ubuntu20_04_ros1.yml
+++ /dev/null
@@ -1,47 +0,0 @@
-name: Ubuntu20.04, ROS1 Continuous Integration
-
-on:
- push:
- branches:
- - master
- - devel
- - mnaveau/ubuntu20.04_ros1_ros2
- pull_request:
- branches:
- - master
- - devel
-
-jobs:
- build:
- runs-on: ubuntu-20.04
- steps:
- #
- # Setup the machines and build environment
- #
- - name: Install ROS.
- uses: ros-tooling/setup-ros@v0.2
- with:
- required-ros-distributions: noetic
-
- #
- # Checkout the current branch
- #
- - uses: actions/checkout@v2
-
- #
- # Clone the dependencies in the ROS workspace
- #
- - name: Clone dependencies in the current workspace.
- shell: bash
- run: |
- mkdir -p /home/runner/work/dynamic_graph_bridge/dynamic_graph_bridge/ros_ws/src
- cd /home/runner/work/dynamic_graph_bridge/dynamic_graph_bridge/ros_ws/src
- git clone --recursive https://github.com/stack-of-tasks/dynamic_graph_bridge_msgs.git
-
- #
- # Build and test the repo
- #
- - uses: ros-tooling/action-ros-ci@v0.2
- with:
- package-name: dynamic_graph_bridge
- target-ros1-distro: noetic
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5daa8b0..fd447b8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,45 +3,85 @@
# Author: Florent Lamiraux, Nirmal Giftsun, Guilhem Saurel
#
-CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
+cmake_minimum_required(VERSION 3.1)
# Project properties
-SET(PROJECT_ORG stack-of-tasks)
-SET(PROJECT_NAME dynamic_graph_bridge)
-SET(PROJECT_DESCRIPTION "Dynamic graph bridge library")
-SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
+set(PROJECT_ORG stack-of-tasks)
+set(PROJECT_NAME dynamic_graph_bridge)
+set(PROJECT_DESCRIPTION "Dynamic graph bridge library")
+set(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
# Project options
-OPTION(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
+option(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
# Project configuration
-SET(PROJECT_USE_CMAKE_EXPORT TRUE)
-SET(CUSTOM_HEADER_DIR ${PROJECT_NAME})
+set(PROJECT_USE_CMAKE_EXPORT TRUE)
+set(CUSTOM_HEADER_DIR ${PROJECT_NAME})
set(CXX_DISABLE_WERROR FALSE)
-SET(DOXYGEN_USE_MATHJAX YES)
-SET(CATKIN_ENABLE_TESTING OFF)
+set(DOXYGEN_USE_MATHJAX YES)
+set(CATKIN_ENABLE_TESTING OFF)
# JRL-cmakemodule setup
-INCLUDE(cmake/base.cmake)
-INCLUDE(cmake/boost.cmake)
-INCLUDE(cmake/python.cmake)
-INCLUDE(cmake/ros.cmake)
+include(cmake/base.cmake)
+include(cmake/boost.cmake)
+include(cmake/python.cmake)
+include(cmake/ros.cmake)
# Project definition
-COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
-PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
-
-if(DEFINED ENV{ROS_VERSION} )
- if ($ENV{ROS_VERSION} EQUAL 2) # if ROS2
- add_subdirectory(ros2)
- #ROS 2 packaging
- ament_package()
- else()
- add_subdirectory(ros1)
- endif()
-else()
- message(FATAL_ERROR "No ROS version found.")
+compute_project_args(PROJECT_ARGS LANGUAGES CXX)
+project(${PROJECT_NAME} ${PROJECT_ARGS})
+
+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/remote_python_client.py
+ RENAME remote_python_client
+ DESTINATION lib/${PROJECT_NAME})
+install(
+ PROGRAMS scripts/remote_python_client.py
+ RENAME run_command
+ DESTINATION lib/${PROJECT_NAME})
+
+# we also need to install the header files
+install(DIRECTORY include/ DESTINATION include)
+
# Install package information
-install(FILES manifest.xml package.xml DESTINATION share/${PROJECT_NAME})
\ No newline at end of file
+install(FILES package.xml DESTINATION share/${PROJECT_NAME})
+
+#ROS 2 packaging
+ament_package()
\ No newline at end of file
diff --git a/ros2/include/dynamic_graph_bridge/ros.hpp b/include/dynamic_graph_bridge/ros.hpp
similarity index 100%
rename from ros2/include/dynamic_graph_bridge/ros.hpp
rename to include/dynamic_graph_bridge/ros.hpp
diff --git a/ros2/include/dynamic_graph_bridge/ros_parameter.hpp b/include/dynamic_graph_bridge/ros_parameter.hpp
similarity index 100%
rename from ros2/include/dynamic_graph_bridge/ros_parameter.hpp
rename to include/dynamic_graph_bridge/ros_parameter.hpp
diff --git a/ros2/include/dynamic_graph_bridge/ros_python_interpreter_client.hpp b/include/dynamic_graph_bridge/ros_python_interpreter_client.hpp
similarity index 100%
rename from ros2/include/dynamic_graph_bridge/ros_python_interpreter_client.hpp
rename to include/dynamic_graph_bridge/ros_python_interpreter_client.hpp
diff --git a/ros2/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp b/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
similarity index 100%
rename from ros2/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
rename to include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
diff --git a/ros2/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
similarity index 100%
rename from ros2/include/dynamic_graph_bridge/sot_loader.hh
rename to include/dynamic_graph_bridge/sot_loader.hh
diff --git a/ros2/include/dynamic_graph_bridge/sot_loader_basic.hh b/include/dynamic_graph_bridge/sot_loader_basic.hh
similarity index 100%
rename from ros2/include/dynamic_graph_bridge/sot_loader_basic.hh
rename to include/dynamic_graph_bridge/sot_loader_basic.hh
diff --git a/manifest.xml b/manifest.xml
deleted file mode 100644
index 14c5c87..0000000
--- a/manifest.xml
+++ /dev/null
@@ -1,39 +0,0 @@
-<package>
- <description brief="Dynamic graph ROS bridge">
-
- ROS bindings for dynamic graph.
-
- </description>
- <author>Thomas Moulard</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://ros.org/wiki/dynamic_graph_bridge</url>
-
- <export>
- <cpp
- cflags="-I${prefix}/include"
- lflags="-L${prefix}/lib -lros_bridge -lros_interpreter -Wl,-rpath,${prefix}/lib"
- />
- <rosdoc config="rosdoc.yaml" />
- </export>
-
-
- <rosdep name="boost"/>
-
- <!-- Disable non-standard dependencies for now
- <rosdep name="jrl-mal"/>
- <rosdep name="dynamic-graph"/>
- <rosdep name="sot-core"/>
- <rosdep name="sot-dynamic"/>
- -->
-
- <depend package="std_msgs"/>
- <depend package="std_srvs"/>
- <depend package="roscpp"/>
- <depend package="geometry_msgs"/>
- <depend package="sensor_msgs"/>
- <depend package="tf"/>
-
- <depend package="realtime_tools"/>
- <depend package="dynamic_graph_bridge_msgs"/>
-</package>
diff --git a/package.xml b/package.xml
index e0d5432..82696fe 100644
--- a/package.xml
+++ b/package.xml
@@ -10,58 +10,25 @@
<license>LGPL</license>
<url>http://ros.org/wiki/dynamic_graph_bridge</url>
- <!-- ROS1 dependencies -->
- <buildtool_depend condition="$ROS_VERSION == 1">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>
-
- <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>
-
<!-- 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>
+ <buildtool_depend>ament_cmake</buildtool_depend>
+ <buildtool_depend>ament_index_cpp</buildtool_depend>
+ <depend > rclcpp </depend>
+ <exec_depend > rclpy </exec_depend>
+ <depend > rcutils </depend>
+ <depend > std_msgs </depend>
+ <depend > std_srvs </depend>
+ <depend > geometry_msgs </depend>
+ <depend > sensor_msgs </depend>
+ <depend > tf2_ros </depend>
+ <depend > boost </depend>
+ <depend > dynamic_graph_bridge_msgs </depend>
+ <depend > sot-core </depend>
<export>
<rosdoc config="rosdoc.yaml" />
- <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
+ <build_type>ament_cmake</build_type>
</export>
- <test_depend condition="$ROS_VERSION == 2">rclpy</test_depend>
+ <test_depend>rclpy</test_depend>
</package>
diff --git a/ros1/CMakeLists.txt b/ros1/CMakeLists.txt
deleted file mode 100644
index a80c75f..0000000
--- a/ros1/CMakeLists.txt
+++ /dev/null
@@ -1,78 +0,0 @@
-# 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}
- )
-
-# we also need to install the header files
-install(DIRECTORY include/ DESTINATION include)
diff --git a/ros1/include/dynamic_graph_bridge/fwd.hh b/ros1/include/dynamic_graph_bridge/fwd.hh
deleted file mode 100644
index 0a98b01..0000000
--- a/ros1/include/dynamic_graph_bridge/fwd.hh
+++ /dev/null
@@ -1,20 +0,0 @@
-/*
- * Copyright CNRS 2021
- *
- * Author: Florent Lamiraux
- *
- * This file is part of sot-core.
- */
-
-#ifndef DYNAMIC_GRAPH_BRIDGE_FWD_HH
-#define DYNAMIC_GRAPH_BRIDGE_FWD_HH
-
-#include <dynamic-graph/python/fwd.hh>
-
-namespace dynamicgraph {
- // classes defined in sot-core
- class Interpreter;
- typedef shared_ptr<Interpreter> InterpreterPtr_t;
-}// namespace dynamicgraph
-
-#endif // DYNAMIC_GRAPH_PYTHON_FWD_HH
diff --git a/ros1/include/dynamic_graph_bridge/ros_init.hh b/ros1/include/dynamic_graph_bridge/ros_init.hh
deleted file mode 100644
index fd6f855..0000000
--- a/ros1/include/dynamic_graph_bridge/ros_init.hh
+++ /dev/null
@@ -1,18 +0,0 @@
-#ifndef ROS_INIT_HH
-#define ROS_INIT_HH
-#include <ros/ros.h>
-
-namespace dynamicgraph {
-ros::NodeHandle& rosInit(bool createAsyncSpinner = false, bool createMultiThreadSpinner = true);
-
-/// \brief Return spinner or throw an exception if spinner
-/// creation has been disabled at startup.
-ros::AsyncSpinner& spinner();
-
-/// \brief Return multi threaded spinner or throw an exception if spinner
-/// creation has been disabled at startup.
-ros::MultiThreadedSpinner& mtSpinner();
-
-} // end of namespace dynamicgraph.
-
-#endif //! ROS_INIT_HH
diff --git a/ros1/include/dynamic_graph_bridge/ros_interpreter.hh b/ros1/include/dynamic_graph_bridge/ros_interpreter.hh
deleted file mode 100644
index 1eacc9e..0000000
--- a/ros1/include/dynamic_graph_bridge/ros_interpreter.hh
+++ /dev/null
@@ -1,60 +0,0 @@
-#ifndef DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
-#define DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
-#pragma GCC diagnostic push
-#pragma GCC system_header
-#include <ros/ros.h>
-#pragma GCC diagnostic pop
-
-#include <dynamic_graph_bridge/fwd.hh>
-#include <dynamic_graph_bridge_msgs/RunCommand.h>
-#include <dynamic_graph_bridge_msgs/RunPythonFile.h>
-#include <dynamic-graph/python/interpreter.hh>
-
-namespace dynamicgraph {
-/// \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 Interpreter {
- public:
- typedef boost::function<bool(dynamic_graph_bridge_msgs::RunCommand::Request&,
- dynamic_graph_bridge_msgs::RunCommand::Response&)>
- runCommandCallback_t;
-
- typedef boost::function<bool(dynamic_graph_bridge_msgs::RunPythonFile::Request&,
- dynamic_graph_bridge_msgs::RunPythonFile::Response&)>
- runPythonFileCallback_t;
-
- explicit Interpreter(ros::NodeHandle& nodeHandle);
-
- /// \brief Method to start python interpreter and deal with messages.
- /// \param Command string to execute, result, stdout, stderr strings.
- void runCommand(const std::string& command, std::string& result, std::string& out, std::string& err);
-
- /// \brief Method to parse python scripts.
- /// \param Input file name to parse.
- void runPythonFile(std::string ifilename);
-
- /// Initialize service run_command
- void startRosService();
-
- protected:
- /// \brief Run a Python command and return result, stderr and stdout.
- bool runCommandCallback(dynamic_graph_bridge_msgs::RunCommand::Request& req,
- dynamic_graph_bridge_msgs::RunCommand::Response& res);
-
- /// \brief Run a Python file.
- bool runPythonFileCallback(dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
- dynamic_graph_bridge_msgs::RunPythonFile::Response& res);
-
- private:
- python::Interpreter interpreter_;
- ros::NodeHandle& nodeHandle_;
- ros::ServiceServer runCommandSrv_;
- ros::ServiceServer runPythonFileSrv_;
-};
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
diff --git a/ros1/include/dynamic_graph_bridge/ros_parameter.hh b/ros1/include/dynamic_graph_bridge/ros_parameter.hh
deleted file mode 100644
index c2e804f..0000000
--- a/ros1/include/dynamic_graph_bridge/ros_parameter.hh
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef _ROS_DYNAMIC_GRAPH_PARAMETER_
-#define _ROS_DYNAMIC_GRAPH_PARAMETER_
-
-namespace dynamicgraph {
-
-bool parameter_server_read_robot_description();
-
-}
-#endif /* _ROS_DYNAMIC_GRAPH_PARAMETER_ */
diff --git a/ros1/include/dynamic_graph_bridge/sot_loader.hh b/ros1/include/dynamic_graph_bridge/sot_loader.hh
deleted file mode 100644
index 2e83aad..0000000
--- a/ros1/include/dynamic_graph_bridge/sot_loader.hh
+++ /dev/null
@@ -1,102 +0,0 @@
-/*
- * Copyright 2016,
- * Olivier Stasse,
- *
- * CNRS
- *
- */
-/* -------------------------------------------------------------------------- */
-/* --- INCLUDES ------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-#ifndef _SOT_LOADER_HH_
-#define _SOT_LOADER_HH_
-
-// System includes
-#include <cassert>
-
-// STL includes
-#include <map>
-
-// Pinocchio includes
-#include <pinocchio/fwd.hpp>
-
-// Boost includes
-#include <boost/program_options.hpp>
-#include <boost/foreach.hpp>
-#include <boost/format.hpp>
-#include <boost/thread/thread.hpp>
-
-// ROS includes
-#include "ros/ros.h"
-#include "std_srvs/Empty.h"
-#include <sensor_msgs/JointState.h>
-#include <tf2_ros/transform_broadcaster.h>
-
-// Sot Framework includes
-#include <sot/core/debug.hh>
-#include <sot/core/abstract-sot-external-interface.hh>
-
-// Dynamic-graph-bridge includes.
-#include <dynamic_graph_bridge/sot_loader_basic.hh>
-
-namespace po = boost::program_options;
-namespace dgs = dynamicgraph::sot;
-
-class SotLoader : public SotLoaderBasic {
- protected:
- /// Map of sensor readings
- std::map<std::string, dgs::SensorValues> sensorsIn_;
- /// Map of control values
- std::map<std::string, dgs::ControlValues> controlValues_;
-
- /// Angular values read by encoders
- std::vector<double> angleEncoder_;
- /// Angular values sent to motors
- std::vector<double> angleControl_;
- /// Forces read by force sensors
- std::vector<double> forces_;
- /// Torques
- std::vector<double> torques_;
- /// Attitude of the robot computed by extended Kalman filter.
- std::vector<double> baseAtt_;
- /// Accelerations read by Accelerometers
- std::vector<double> accelerometer_;
- /// Angular velocity read by gyrometers
- std::vector<double> gyrometer_;
-
- /// URDF string description of the robot.
- std::string robot_desc_string_;
-
- /// The thread running dynamic graph
- boost::thread thread_;
-
- // \brief Start control loop
- virtual void startControlLoop();
-
- // Robot Pose Publisher
- tf2_ros::TransformBroadcaster freeFlyerPublisher_;
- geometry_msgs::TransformStamped freeFlyerPose_;
-
- public:
- SotLoader();
- ~SotLoader();
-
- // \brief Create a thread for ROS and start the control loop.
- void initializeRosNode(int argc, char *argv[]);
-
- // \brief Compute one iteration of control.
- // Basically calls fillSensors, the SoT and the readControl.
- void oneIteration();
-
- // \brief Fill the sensors value for the SoT.
- void fillSensors(std::map<std::string, dgs::SensorValues> &sensorsIn);
-
- // \brief Read the control computed by the SoT framework.
- void readControl(std::map<std::string, dgs::ControlValues> &controlValues);
-
- // \brief Prepare the SoT framework.
- void setup();
-};
-
-#endif /* SOT_LOADER_HH_ */
diff --git a/ros1/include/dynamic_graph_bridge/sot_loader_basic.hh b/ros1/include/dynamic_graph_bridge/sot_loader_basic.hh
deleted file mode 100644
index b635be5..0000000
--- a/ros1/include/dynamic_graph_bridge/sot_loader_basic.hh
+++ /dev/null
@@ -1,115 +0,0 @@
-/*
- * Copyright 2016,
- * Olivier Stasse,
- *
- * CNRS
- *
- */
-/* -------------------------------------------------------------------------- */
-/* --- INCLUDES ------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-#ifndef _SOT_LOADER_BASIC_HH_
-#define _SOT_LOADER_BASIC_HH_
-
-// System includes
-#include <cassert>
-
-// STL includes
-#include <map>
-
-// Pinocchio includes
-#include <pinocchio/fwd.hpp>
-
-// Boost includes
-#include <boost/program_options.hpp>
-#include <boost/foreach.hpp>
-#include <boost/format.hpp>
-
-// ROS includes
-#include "ros/ros.h"
-#include "std_srvs/Empty.h"
-#include <sensor_msgs/JointState.h>
-
-// Sot Framework includes
-#include <sot/core/debug.hh>
-#include <sot/core/abstract-sot-external-interface.hh>
-
-namespace po = boost::program_options;
-namespace dgs = dynamicgraph::sot;
-
-class SotLoaderBasic {
- protected:
- // Dynamic graph is stopped.
- bool dynamic_graph_stopped_;
-
- /// \brief the sot-hrp2 controller
- dgs::AbstractSotExternalInterface* sotController_;
-
- po::variables_map vm_;
- std::string dynamicLibraryName_;
-
- /// \brief Handle on the SoT library.
- void* sotRobotControllerLibrary_;
-
- /// \brief Map between SoT state vector and some joint_state_links
- XmlRpc::XmlRpcValue stateVectorMap_;
-
- /// \brief List of parallel joints from the state vector.
- typedef std::vector<int> parallel_joints_to_state_vector_t;
- parallel_joints_to_state_vector_t parallel_joints_to_state_vector_;
-
- /// \brief Coefficient between parallel joints and the state vector.
- std::vector<double> coefficient_parallel_joints_;
- /// Advertises start_dynamic_graph services
- ros::ServiceServer service_start_;
-
- /// Advertises stop_dynamic_graph services
- ros::ServiceServer service_stop_;
-
- // Joint state publication.
- ros::Publisher joint_pub_;
-
- // Joint state to be published.
- sensor_msgs::JointState joint_state_;
-
- // Number of DOFs according to KDL.
- int nbOfJoints_;
- parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
-
- public:
- SotLoaderBasic();
- ~SotLoaderBasic(){};
-
- // \brief Read user input to extract the path of the SoT dynamic library.
- int parseOptions(int argc, char* argv[]);
-
- /// \brief Load the SoT device corresponding to the robot.
- void Initialization();
-
- /// \brief Unload the library which handles the robot device.
- void CleanUp();
-
- // \brief Create a thread for ROS.
- virtual void initializeRosNode(int argc, char* argv[]);
-
- // \brief Callback function when starting dynamic graph.
- bool start_dg(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
-
- // \brief Callback function when stopping dynamic graph.
- bool stop_dg(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
-
- // \brief Read the state vector description based upon the robot links.
- int readSotVectorStateParam();
-
- // \brief Init publication of joint states.
- int initPublication();
-
- // \brief Get Status of dg.
- bool isDynamicGraphStopped() { return dynamic_graph_stopped_; }
-
- // \brief Specify the name of the dynamic library.
- void setDynamicLibraryName(std::string& afilename);
-};
-
-#endif /* _SOT_LOADER_BASIC_HH_ */
diff --git a/ros1/scripts/robot_pose_publisher b/ros1/scripts/robot_pose_publisher
deleted file mode 100755
index f742b2a..0000000
--- a/ros1/scripts/robot_pose_publisher
+++ /dev/null
@@ -1,30 +0,0 @@
-#!/usr/bin/env python
-#
-# Listens to TransformStamped messages and publish them to tf
-#
-
-import rospy
-
-import tf
-import sensor_msgs.msg
-
-frame = ''
-childFrame = ''
-
-#DEPRECATED. Robot Pose is already being published
-def pose_broadcaster(msg):
- translation = msg.position[0:3];
- rotation = tf.transformations.quaternion_from_euler(msg.position[3], msg.position[4], msg.position[5])
- tfbr = tf.TransformBroadcaster()
- tfbr.sendTransform(translation, rotation,
- rospy.Time.now(), childFrame, frame)
-
-if __name__ == '__main__':
- rospy.init_node('robot_pose_publisher', anonymous=True)
-
- frame = rospy.get_param('~frame', 'odom')
- childFrame = rospy.get_param('~child_frame', 'base_link')
- topic = rospy.get_param('~topic', 'joint_states')
-
- rospy.Subscriber(topic, sensor_msgs.msg.JointState, pose_broadcaster)
- rospy.spin()
diff --git a/ros1/scripts/run_command b/ros1/scripts/run_command
deleted file mode 100755
index f69b5db..0000000
--- a/ros1/scripts/run_command
+++ /dev/null
@@ -1,123 +0,0 @@
-#!/usr/bin/env python
-
-import rospy
-
-import dynamic_graph
-import dynamic_graph_bridge_msgs.srv
-import sys
-import code
-from code import InteractiveConsole
-
-import os
-
-from dynamic_graph.ros.dgcompleter import DGCompleter
-
-try:
- import readline
-except ImportError:
- print("Module readline not available.")
-
-# Enable a History
-HISTFILE="%s/.pyhistory" % os.environ["HOME"]
-
-def savehist():
- readline.write_history_file(HISTFILE)
-
-
-class RosShell(InteractiveConsole):
- def __init__(self):
- self.cache = ""
- InteractiveConsole.__init__(self)
-
- rospy.loginfo('waiting for service...')
- rospy.wait_for_service('run_command')
- self.client = rospy.ServiceProxy(
- 'run_command', dynamic_graph_bridge_msgs.srv.RunCommand, True)
- rospy.wait_for_service('run_script')
- self.scriptClient = rospy.ServiceProxy(
- 'run_script', dynamic_graph_bridge_msgs.srv.RunPythonFile, True)
-
- readline.set_completer(DGCompleter(self.client).complete)
- readline.parse_and_bind("tab: complete")
-
- # Read the existing history if there is one
- if os.path.exists(HISTFILE):
- readline.read_history_file(HISTFILE)
-
- # Set maximum number of items that will be written to the history file
- readline.set_history_length(300)
-
-
- import atexit
- atexit.register(savehist)
-
-
- def runcode(self, code, retry = True):
- source = self.cache[:-1]
- self.cache = ""
- if source != "":
- try:
- if not self.client:
- print("Connection to remote server lost. Reconnecting...")
- self.client = rospy.ServiceProxy(
- 'run_command', dynamic_graph_bridge_msgs.srv.RunCommand, True)
- if retry:
- print("Retrying previous command...")
- self.cache = source
- return self.runcode(code, False)
- response = self.client(str(source))
- if response.standardoutput != "":
- print(response.standardoutput[:-1])
- if response.standarderror != "":
- print(response.standarderror[:-1])
- elif response.result != "None":
- print(response.result)
- except rospy.ServiceException as e:
- print("Connection to remote server lost. Reconnecting...")
- self.client = rospy.ServiceProxy(
- 'run_command', dynamic_graph_bridge_msgs.srv.RunCommand, True)
- if retry:
- print("Retrying previous command...")
- self.cache = source
- self.runcode(code, False)
-
- def runsource(self, source, filename = '<input>', symbol = 'single'):
- try:
- c = code.compile_command(source, filename, symbol)
- if c:
- return self.runcode(c)
- else:
- return True
- except (SyntaxError, OverflowError):
- self.showsyntaxerror()
- self.cache = ""
- return False
-
- def push(self,line):
- self.cache += line + "\n"
- return InteractiveConsole.push(self,line)
-
-if __name__ == '__main__':
- import optparse
- import os.path
- rospy.init_node('run_command', argv=sys.argv)
- sys.argv = rospy.myargv(argv=None)
- parser = optparse.OptionParser(
- usage='\n\t%prog [options]')
- (options, args) = parser.parse_args(sys.argv[1:])
-
- sh = RosShell()
-
- if args[:]:
- infile = args[0]
- if os.path.isfile(infile):
- if not sh.client:
- print("Connection to remote server has been lost.")
- sys.exit(1)
- response = sh.scriptClient(os.path.abspath(infile))
- if not response:
- print("Error while file parsing ")
- else:
- print("Provided file does not exist: %s"%(infile))
- else:
- sh.interact("Interacting with remote server.")
diff --git a/ros1/scripts/tf_publisher b/ros1/scripts/tf_publisher
deleted file mode 100755
index 2dd388b..0000000
--- a/ros1/scripts/tf_publisher
+++ /dev/null
@@ -1,63 +0,0 @@
-#!/usr/bin/env python
-#
-# This script looks for a particular tf transformation
-# and publish it as a TransformStamped topic.
-# This may be useful to insert tf frames into dynamic-graph
-# through dynamic_graph_bridge.
-#
-
-import rospy
-
-import tf
-import geometry_msgs.msg
-
-
-def main():
- rospy.init_node('tf_publisher', anonymous=True)
-
- frame = rospy.get_param('~frame', '')
- childFrame = rospy.get_param('~child_frame', '')
- topic = rospy.get_param('~topic', '')
- rateSeconds = rospy.get_param('~rate', 5)
-
- if not frame or not childFrame or not topic:
- logpy.error("frame, childFrame and topic are required parameters")
- return
-
- rate = rospy.Rate(rateSeconds)
- tl = tf.TransformListener()
- pub = rospy.Publisher(topic, geometry_msgs.msg.TransformStamped)
-
- transform = geometry_msgs.msg.TransformStamped()
- transform.header.frame_id = frame
- transform.child_frame_id = childFrame
-
- ok = False
- while not rospy.is_shutdown() and not ok:
- try:
- tl.waitForTransform(childFrame, frame,
- rospy.Time(), rospy.Duration(0.1))
- ok = True
- except tf.Exception, e:
- rospy.logwarn("waiting for tf transform")
- ok = False
-
- while not rospy.is_shutdown():
- time = tl.getLatestCommonTime(frame, childFrame)
- (p, q) = tl.lookupTransform(childFrame, frame, time)
- transform.header.seq += 1
- transform.header.stamp = time
-
- transform.transform.translation.x = p[0]
- transform.transform.translation.y = p[1]
- transform.transform.translation.z = p[2]
-
- transform.transform.rotation.x = q[0]
- transform.transform.rotation.y = q[1]
- transform.transform.rotation.z = q[2]
- transform.transform.rotation.w = q[3]
-
- pub.publish(transform)
- rate.sleep()
-
-main()
diff --git a/ros1/src/CMakeLists.txt b/ros1/src/CMakeLists.txt
deleted file mode 100644
index e58c99d..0000000
--- a/ros1/src/CMakeLists.txt
+++ /dev/null
@@ -1,198 +0,0 @@
-#.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/ros1/src/converter.hh b/ros1/src/converter.hh
deleted file mode 100644
index 82034e6..0000000
--- a/ros1/src/converter.hh
+++ /dev/null
@@ -1,269 +0,0 @@
-#ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH
-#define DYNAMIC_GRAPH_ROS_CONVERTER_HH
-#include <stdexcept>
-#include "sot_to_ros.hh"
-
-#include <boost/static_assert.hpp>
-#include <boost/date_time/date.hpp>
-#include <boost/date_time/posix_time/posix_time.hpp>
-
-#include <ros/time.h>
-#include <std_msgs/Header.h>
-
-#define SOT_TO_ROS_IMPL(T) \
- template <> \
- inline void converter(SotToRos<T>::ros_t& dst, const SotToRos<T>::sot_t& src)
-
-#define ROS_TO_SOT_IMPL(T) \
- template <> \
- inline void converter(SotToRos<T>::sot_t& dst, const SotToRos<T>::ros_t& src)
-
-namespace dynamicgraph {
-inline void makeHeader(std_msgs::Header& header) {
- header.seq = 0;
- header.stamp = ros::Time::now();
- header.frame_id = "/dynamic_graph/world";
-}
-
-/// \brief Handle ROS <-> dynamic-graph conversion.
-///
-/// Implements all ROS/dynamic-graph conversions required by the
-/// bridge.
-///
-/// Relies on the SotToRos type trait to make sure valid pair of
-/// types are used.
-template <typename D, typename S>
-void converter(D& dst, const S& src);
-
-// Boolean
-SOT_TO_ROS_IMPL(bool) { dst.data = src; }
-
-ROS_TO_SOT_IMPL(bool) { dst = src.data; }
-
-// Double
-SOT_TO_ROS_IMPL(double) { dst.data = src; }
-
-ROS_TO_SOT_IMPL(double) { dst = src.data; }
-
-// Int
-SOT_TO_ROS_IMPL(int) { dst.data = src; }
-
-ROS_TO_SOT_IMPL(int) { dst = src.data; }
-
-// Unsigned
-SOT_TO_ROS_IMPL(unsigned int) { dst.data = src; }
-
-ROS_TO_SOT_IMPL(unsigned int) { dst = src.data; }
-
-// String
-SOT_TO_ROS_IMPL(std::string) { dst.data = src; }
-
-ROS_TO_SOT_IMPL(std::string) { dst = src.data; }
-
-// Vector
-SOT_TO_ROS_IMPL(Vector) {
- dst.data.resize(src.size());
- for (int i = 0; i < src.size(); ++i) dst.data[i] = src(i);
-}
-
-ROS_TO_SOT_IMPL(Vector) {
- dst.resize(src.data.size());
- for (unsigned int i = 0; i < src.data.size(); ++i) dst(i) = src.data[i];
-}
-
-// Vector3
-SOT_TO_ROS_IMPL(specific::Vector3) {
- if (src.size() > 0) {
- dst.x = src(0);
- if (src.size() > 1) {
- dst.y = src(1);
- if (src.size() > 2) dst.z = src(2);
- }
- }
-}
-
-ROS_TO_SOT_IMPL(specific::Vector3) {
- dst.resize(3);
- dst(0) = src.x;
- dst(1) = src.y;
- dst(2) = src.z;
-}
-
-// Matrix
-SOT_TO_ROS_IMPL(Matrix) {
- // TODO: Confirm Ros Matrix Storage order. It changes the RosMatrix to
- // ColMajor.
- dst.width = (unsigned int)src.rows();
- dst.data.resize(src.cols() * src.rows());
- for (int i = 0; i < src.cols() * src.rows(); ++i) dst.data[i] = src.data()[i];
-}
-
-ROS_TO_SOT_IMPL(Matrix) {
- dst.resize(src.width, (unsigned int)src.data.size() / (unsigned int)src.width);
- for (unsigned i = 0; i < src.data.size(); ++i) dst.data()[i] = src.data[i];
-}
-
-// Homogeneous matrix.
-SOT_TO_ROS_IMPL(sot::MatrixHomogeneous) {
- sot::VectorQuaternion q(src.linear());
- dst.translation.x = src.translation()(0);
- dst.translation.y = src.translation()(1);
- dst.translation.z = src.translation()(2);
-
- dst.rotation.x = q.x();
- dst.rotation.y = q.y();
- dst.rotation.z = q.z();
- dst.rotation.w = q.w();
-}
-
-ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
- sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y, src.rotation.z);
- dst.linear() = q.matrix();
-
- // Copy the translation component.
- dst.translation()(0) = src.translation.x;
- dst.translation()(1) = src.translation.y;
- dst.translation()(2) = src.translation.z;
-}
-
-// Twist.
-SOT_TO_ROS_IMPL(specific::Twist) {
- if (src.size() != 6) throw std::runtime_error("failed to convert invalid twist");
- dst.linear.x = src(0);
- dst.linear.y = src(1);
- dst.linear.z = src(2);
- dst.angular.x = src(3);
- dst.angular.y = src(4);
- dst.angular.z = src(5);
-}
-
-ROS_TO_SOT_IMPL(specific::Twist) {
- dst.resize(6);
- dst(0) = src.linear.x;
- dst(1) = src.linear.y;
- dst(2) = src.linear.z;
- dst(3) = src.angular.x;
- dst(4) = src.angular.y;
- dst(5) = src.angular.z;
-}
-
-/// \brief This macro generates a converter for a stamped type from
-/// dynamic-graph to ROS. I.e. A data associated with its
-/// timestamp.
-#define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
- template <> \
- inline void converter(SotToRos<std::pair<T, Vector> >::ros_t& dst, \
- const SotToRos<std::pair<T, Vector> >::sot_t& src) { \
- makeHeader(dst.header); \
- converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t>(dst.ATTRIBUTE, src); \
- do { \
- EXTRA \
- } while (0); \
- } \
- struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
-
-DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
-DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, dst.child_frame_id = "";);
-DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
-
-/// \brief This macro generates a converter for a shared pointer on
-/// a ROS type to a dynamic-graph type.
-///
-/// A converter for the underlying type is required. I.e. to
-/// convert a shared_ptr<T> to T', a converter from T to T'
-/// is required.
-#define DG_BRIDGE_MAKE_SHPTR_IMPL(T) \
- template <> \
- inline void converter(SotToRos<T>::sot_t& dst, const boost::shared_ptr<SotToRos<T>::ros_t const>& src) { \
- converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, *src); \
- } \
- struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
-
-DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
-DG_BRIDGE_MAKE_SHPTR_IMPL(double);
-DG_BRIDGE_MAKE_SHPTR_IMPL(int);
-DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
-DG_BRIDGE_MAKE_SHPTR_IMPL(std::string);
-DG_BRIDGE_MAKE_SHPTR_IMPL(Vector);
-DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
-DG_BRIDGE_MAKE_SHPTR_IMPL(Matrix);
-DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
-DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
-
-/// \brief This macro generates a converter for a stamped type.
-/// I.e. A data associated with its timestamp.
-///
-/// FIXME: the timestamp is not yet forwarded to the dg signal.
-#define DG_BRIDGE_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
- template <> \
- inline void converter(SotToRos<std::pair<T, Vector> >::sot_t& dst, \
- const SotToRos<std::pair<T, Vector> >::ros_t& src) { \
- converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src.ATTRIBUTE); \
- do { \
- EXTRA \
- } while (0); \
- } \
- struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
-
-DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
-DG_BRIDGE_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, ;);
-DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
-
-/// \brief This macro generates a converter for a shared pointer on
-/// a stamped type. I.e. A data associated with its timestamp.
-///
-/// FIXME: the timestamp is not yet forwarded to the dg signal.
-#define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA) \
- template <> \
- inline void converter(SotToRos<std::pair<T, Vector> >::sot_t& dst, \
- const boost::shared_ptr<SotToRos<std::pair<T, Vector> >::ros_t const>& src) { \
- converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src->ATTRIBUTE); \
- do { \
- EXTRA \
- } while (0); \
- } \
- struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
-
-DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Vector3, vector, ;);
-DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(sot::MatrixHomogeneous, transform, ;);
-DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Twist, twist, ;);
-
-/// \brief If an impossible/unimplemented conversion is required, fail.
-///
-/// IMPORTANT, READ ME:
-///
-/// If the compiler generates an error in the following function,
-/// this is /normal/.
-///
-/// This error means that either you try to use an undefined
-/// conversion. You can either fix your code or provide the wanted
-/// conversion by updating this header.
-template <typename U, typename V>
-inline void converter(U& dst, V& src) {
- // This will always fail if instantiated.
- BOOST_STATIC_ASSERT(sizeof(U) == 0);
-}
-
-typedef boost::posix_time::ptime ptime;
-typedef boost::posix_time::seconds seconds;
-typedef boost::posix_time::microseconds microseconds;
-typedef boost::posix_time::time_duration time_duration;
-typedef boost::gregorian::date date;
-
-boost::posix_time::ptime rosTimeToPtime(const ros::Time& rosTime) {
- ptime time(date(1970, 1, 1), seconds(rosTime.sec) + microseconds(rosTime.nsec / 1000));
- return time;
-}
-
-ros::Time pTimeToRostime(const boost::posix_time::ptime& time) {
- static ptime timeStart(date(1970, 1, 1));
- time_duration diff = time - timeStart;
-
- uint32_t sec = (unsigned int)diff.ticks() / (unsigned int)time_duration::rep_type::res_adjust();
- uint32_t nsec = (unsigned int)diff.fractional_seconds();
-
- return ros::Time(sec, nsec);
-}
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH
diff --git a/ros1/src/dynamic_graph/ros/__init__.py b/ros1/src/dynamic_graph/ros/__init__.py
deleted file mode 100644
index 7f58645..0000000
--- a/ros1/src/dynamic_graph/ros/__init__.py
+++ /dev/null
@@ -1,6 +0,0 @@
-# flake8: noqa
-from .ros import RosPublish as RosImport
-from .ros import RosSubscribe as RosExport
-from .ros_publish import RosPublish
-from .ros_subscribe import RosSubscribe
-from .ros_queued_subscribe import RosQueuedSubscribe
diff --git a/ros1/src/dynamic_graph/ros/dgcompleter.py b/ros1/src/dynamic_graph/ros/dgcompleter.py
deleted file mode 100644
index ff98b6d..0000000
--- a/ros1/src/dynamic_graph/ros/dgcompleter.py
+++ /dev/null
@@ -1,79 +0,0 @@
-"""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
-
-__all__ = ["DGCompleter"]
-
-
-class DGCompleter:
- def __init__(self, client):
- """Create a new completer for the command line.
-
- Completer([client]) -> completer instance.
-
- Client is a ROS proxy to dynamic_graph run_command service.
-
- Completer instances should be used as the completion mechanism of
- readline via the set_completer() call:
-
- readline.set_completer(Completer(client).complete)
- """
- self.client = client
- astr = "import readline"
- self.client(astr)
- astr = "from rlcompleter import Completer"
- self.client(astr)
- astr = "aCompleter=Completer()"
- self.client(astr)
- astr = "readline.set_completer(aCompleter.complete)"
- self.client(astr)
- astr = "readline.parse_and_bind(\"tab: complete\")"
- self.client(astr)
-
- 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'.
-
- """
- astr = "aCompleter.complete(\"" + text + "\"," + str(state) + ")"
- response = self.client(astr)
- res2 = ast.literal_eval(response.result)
- return res2
diff --git a/ros1/src/dynamic_graph/ros/ros.py b/ros1/src/dynamic_graph/ros/ros.py
deleted file mode 100644
index 18d00d5..0000000
--- a/ros1/src/dynamic_graph/ros/ros.py
+++ /dev/null
@@ -1,25 +0,0 @@
-from .ros_publish import RosPublish
-from .ros_subscribe import RosSubscribe
-from .ros_time import RosTime
-
-
-class Ros(object):
- device = None
- rosPublish = None
- rosSubscribe = None
-
- # aliases, for retro compatibility
- rosImport = None
- rosExport = None
-
- def __init__(self, robot, suffix=''):
- self.robot = robot
- self.rosPublish = RosPublish('rosPublish{0}'.format(suffix))
- self.rosSubscribe = RosSubscribe('rosSubscribe{0}'.format(suffix))
- self.rosTime = RosTime('rosTime{0}'.format(suffix))
-
- self.robot.device.after.addSignal('{0}.trigger'.format(self.rosPublish.name))
-
- # aliases, for retro compatibility
- self.rosImport = self.rosPublish
- self.rosExport = self.rosSubscribe
diff --git a/ros1/src/geometric_simu.cpp b/ros1/src/geometric_simu.cpp
deleted file mode 100644
index f6f7225..0000000
--- a/ros1/src/geometric_simu.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * Copyright 2011,
- * Olivier Stasse,
- *
- * CNRS
- *
- */
-#include <iostream>
-#include <ros/console.h>
-
-#define ENABLE_RT_LOG
-#include <dynamic-graph/real-time-logger.h>
-
-#include <dynamic_graph_bridge/sot_loader.hh>
-
-int main(int argc, char *argv[]) {
- ::dynamicgraph::RealTimeLogger::instance()
- .addOutputStream(::dynamicgraph::LoggerStreamPtr_t(new dynamicgraph::LoggerIOStream(std::cout)));
-
- ros::init(argc, argv, "sot_ros_encapsulator");
- SotLoader aSotLoader;
- if (aSotLoader.parseOptions(argc, argv) < 0) return -1;
-
- // Advertize service "(start|stop)_dynamic_graph" and
- // load parameter "robot_description in SoT.
- aSotLoader.initializeRosNode(argc, argv);
- // Load dynamic library and run python prologue.
- aSotLoader.Initialization();
-
- ros::waitForShutdown ();
- return 0;
-}
diff --git a/ros1/src/ros_init.cpp b/ros1/src/ros_init.cpp
deleted file mode 100644
index 56b868a..0000000
--- a/ros1/src/ros_init.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-#include <stdexcept>
-#include <boost/make_shared.hpp>
-#include <boost/shared_ptr.hpp>
-
-#include "dynamic_graph_bridge/ros_init.hh"
-
-namespace dynamicgraph {
-struct GlobalRos {
- ~GlobalRos() {
- if (spinner) spinner->stop();
- if (nodeHandle) nodeHandle->shutdown();
- }
-
- boost::shared_ptr<ros::NodeHandle> nodeHandle;
- boost::shared_ptr<ros::AsyncSpinner> spinner;
- boost::shared_ptr<ros::MultiThreadedSpinner> mtSpinner;
-};
-GlobalRos ros;
-
-ros::NodeHandle& rosInit(bool createAsyncSpinner, bool createMultiThreadedSpinner) {
- if (!ros.nodeHandle) {
- int argc = 1;
- char* arg0 = strdup("dynamic_graph_bridge");
- char* argv[] = {arg0, 0};
- ros::init(argc, argv, "dynamic_graph_bridge");
- free(arg0);
-
- ros.nodeHandle = boost::make_shared<ros::NodeHandle>("");
- }
- if (!ros.spinner && createAsyncSpinner) {
- ros.spinner = boost::make_shared<ros::AsyncSpinner>(4);
-
- // Change the thread's scheduler from real-time to normal and reduce its
- // priority
- int oldThreadPolicy, newThreadPolicy;
- struct sched_param oldThreadParam, newThreadParam;
- if (pthread_getschedparam(pthread_self(), &oldThreadPolicy, &oldThreadParam) == 0) {
- newThreadPolicy = SCHED_OTHER;
- newThreadParam = oldThreadParam;
- newThreadParam.sched_priority -= 5; // Arbitrary number, TODO: choose via param/file?
- if (newThreadParam.sched_priority < sched_get_priority_min(newThreadPolicy))
- newThreadParam.sched_priority = sched_get_priority_min(newThreadPolicy);
-
- pthread_setschedparam(pthread_self(), newThreadPolicy, &newThreadParam);
- }
-
- // AsyncSpinners are created with the reduced priority
- ros.spinner->start();
-
- // Switch the priority of the parent thread (this thread) back to real-time.
- pthread_setschedparam(pthread_self(), oldThreadPolicy, &oldThreadParam);
- } else {
- if (!ros.mtSpinner && createMultiThreadedSpinner) {
- // Seems not to be used.
- // If we need to reduce its threads priority, it needs to be done before
- // calling the MultiThreadedSpinner::spin() method
- ros.mtSpinner = boost::make_shared<ros::MultiThreadedSpinner>(4);
- }
- }
- return *ros.nodeHandle;
-}
-
-ros::AsyncSpinner& spinner() {
- if (!ros.spinner) throw std::runtime_error("spinner has not been created");
- return *ros.spinner;
-}
-
-ros::MultiThreadedSpinner& mtSpinner() {
- if (!ros.mtSpinner) throw std::runtime_error("spinner has not been created");
- return *ros.mtSpinner;
-}
-
-} // end of namespace dynamicgraph.
diff --git a/ros1/src/ros_interpreter.cpp b/ros1/src/ros_interpreter.cpp
deleted file mode 100644
index 6fb690a..0000000
--- a/ros1/src/ros_interpreter.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-#include "dynamic_graph_bridge/ros_interpreter.hh"
-
-namespace dynamicgraph {
-static const int queueSize = 5;
-
-Interpreter::Interpreter(ros::NodeHandle& nodeHandle)
- : interpreter_(), nodeHandle_(nodeHandle), runCommandSrv_(), runPythonFileSrv_() {}
-
-void Interpreter::startRosService() {
- runCommandCallback_t runCommandCb = boost::bind(&Interpreter::runCommandCallback, this, _1, _2);
- runCommandSrv_ = nodeHandle_.advertiseService("run_command", runCommandCb);
-
- runPythonFileCallback_t runPythonFileCb = boost::bind(&Interpreter::runPythonFileCallback, this, _1, _2);
- runPythonFileSrv_ = nodeHandle_.advertiseService("run_script", runPythonFileCb);
-}
-
-bool Interpreter::runCommandCallback(dynamic_graph_bridge_msgs::RunCommand::Request& req,
- dynamic_graph_bridge_msgs::RunCommand::Response& res) {
- interpreter_.python(req.input, res.result, res.standardoutput, res.standarderror);
- return true;
-}
-
-bool Interpreter::runPythonFileCallback(dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
- dynamic_graph_bridge_msgs::RunPythonFile::Response& res) {
- interpreter_.runPythonFile(req.input);
- res.result = "File parsed"; // FIX: It is just an echo, is there a way to
- // have a feedback?
- return true;
-}
-
-void Interpreter::runCommand(const std::string& command, std::string& result, std::string& out, std::string& err) {
- interpreter_.python(command, result, out, err);
- if (err.size() > 0) {
- ROS_ERROR(err.c_str());
- }
-}
-
-void Interpreter::runPythonFile(std::string ifilename) { interpreter_.runPythonFile(ifilename); }
-
-} // end of namespace dynamicgraph.
diff --git a/ros1/src/ros_parameter.cpp b/ros1/src/ros_parameter.cpp
deleted file mode 100644
index c7ab709..0000000
--- a/ros1/src/ros_parameter.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#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 <ros/ros.h>
-#include "dynamic_graph_bridge/ros_parameter.hh"
-
-namespace dynamicgraph {
-bool parameter_server_read_robot_description()
-{
- ros::NodeHandle nh;
- if (!nh.hasParam("/robot_description"))
- {
- ROS_ERROR("No /robot_description parameter");
- return false;
- }
-
- std::string robot_description;
- std::string parameter_name("/robot_description");
- nh.getParam(parameter_name,robot_description);
-
- 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);
- ROS_INFO("Set parameter_name : %s.",parameter_name.c_str());
- // Everything went fine.
- return true;
- }
- ROS_ERROR("Wrong initialization of parameter_name %s",
- parameter_name.c_str());
-
- // Otherwise something went wrong.
- return false;
-
-}
-
-}
diff --git a/ros1/src/ros_publish-python-module-py.cc b/ros1/src/ros_publish-python-module-py.cc
deleted file mode 100644
index a50e445..0000000
--- a/ros1/src/ros_publish-python-module-py.cc
+++ /dev/null
@@ -1,17 +0,0 @@
-#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/ros1/src/ros_publish.cpp b/ros1/src/ros_publish.cpp
deleted file mode 100644
index 407d714..0000000
--- a/ros1/src/ros_publish.cpp
+++ /dev/null
@@ -1,188 +0,0 @@
-#include <stdexcept>
-
-#include <boost/assign.hpp>
-#include <boost/bind.hpp>
-#include <boost/foreach.hpp>
-#include <boost/format.hpp>
-#include <boost/function.hpp>
-#include <boost/make_shared.hpp>
-
-#include <ros/ros.h>
-#include <std_msgs/Float64.h>
-#include <std_msgs/UInt32.h>
-
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/command.h>
-#include <dynamic-graph/linear-algebra.h>
-
-#include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_publish.hh"
-
-#define ENABLE_RT_LOG
-#include <dynamic-graph/real-time-logger.h>
-
-namespace dynamicgraph {
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish");
-const double RosPublish::ROS_JOINT_STATE_PUBLISHER_RATE = 0.01;
-
-namespace command {
-namespace rosPublish {
-
-Add::Add(RosPublish& entity, const std::string& docstring)
- : Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), docstring) {}
-
-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 == "boolean")
- entity.add<bool>(signal, topic);
- else if (type == "double")
- entity.add<double>(signal, topic);
- else if (type == "unsigned")
- entity.add<unsigned int>(signal, topic);
- else if (type == "int")
- entity.add<int>(signal, topic);
- else if (type == "matrix")
- entity.add<Matrix>(signal, topic);
- else if (type == "vector")
- entity.add<Vector>(signal, topic);
- else if (type == "vector3")
- entity.add<specific::Vector3>(signal, topic);
- else if (type == "vector3Stamped")
- entity.add<std::pair<specific::Vector3, Vector> >(signal, topic);
- else if (type == "matrixHomo")
- entity.add<sot::MatrixHomogeneous>(signal, topic);
- else if (type == "matrixHomoStamped")
- entity.add<std::pair<sot::MatrixHomogeneous, Vector> >(signal, topic);
- else if (type == "twist")
- entity.add<specific::Twist>(signal, topic);
- else if (type == "twistStamped")
- entity.add<std::pair<specific::Twist, Vector> >(signal, topic);
- else if (type == "string")
- entity.add<std::string>(signal, topic);
- else
- throw std::runtime_error("bad type");
- return Value();
-}
-
-} // namespace rosPublish
-} // end of namespace command.
-
-const std::string RosPublish::docstring_(
- "Publish dynamic-graph signals as ROS topics.\n"
- "\n"
- " Use command \"add\" to publish a new ROS topic.\n");
-
-RosPublish::RosPublish(const std::string& n)
- : dynamicgraph::Entity(n),
- // RosPublish do not use callback so do not create a useless spinner.
- nh_(rosInit(false)),
- bindedSignal_(),
- trigger_(boost::bind(&RosPublish::trigger, this, _1, _2), sotNOSIGNAL,
- MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
- rate_(0, 10000000),
- nextPublication_() {
- aofs_.open("/tmp/ros_publish.txt");
- dgADD_OSTREAM_TO_RTLOG(aofs_);
-
- try {
- if (ros::Time::isSimTime())
- nextPublication_ = ros::Time::now();
- else {
- clock_gettime(CLOCK_REALTIME, &nextPublicationRT_);
- }
- } catch (const std::exception& exc) {
- throw std::runtime_error("Failed to call ros::Time::now ():" + std::string(exc.what()));
- }
- signalRegistration(trigger_);
- trigger_.setNeedUpdateFromAllChildren(true);
-
- std::string docstring =
- "\n"
- " Add a signal writing data to a ROS topic\n"
- "\n"
- " Input:\n"
- " - type: string among ['double', 'matrix', 'vector', 'vector3',\n"
- " 'vector3Stamped', 'matrixHomo', "
- "'matrixHomoStamped',\n"
- " 'twist', 'twistStamped'],\n"
- " - signal: the signal name in dynamic-graph,\n"
- " - topic: the topic name in ROS.\n"
- "\n";
- addCommand("add", new command::rosPublish::Add(*this, docstring));
-}
-
-RosPublish::~RosPublish() { aofs_.close(); }
-
-void RosPublish::display(std::ostream& os) const { os << CLASS_NAME << std::endl; }
-
-void RosPublish::rm(const std::string& signal) {
- if (bindedSignal_.find(signal) == bindedSignal_.end()) return;
-
- if (signal == "trigger") {
- 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
- boost::mutex::scoped_lock lock(mutex_);
-
- signalDeregistration(signal);
- bindedSignal_.erase(signal);
-}
-
-std::vector<std::string> RosPublish::list() const {
- std::vector<std::string> result(bindedSignal_.size());
- std::transform(bindedSignal_.begin(), bindedSignal_.end(),
- result.begin(), [](const auto& pair) { return pair.first; });
- return result;
-}
-
-void RosPublish::clear() {
- std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
- for (; it != bindedSignal_.end();) {
- if (it->first != "trigger") {
- rm(it->first);
- it = bindedSignal_.begin();
- } else {
- ++it;
- }
- }
-}
-
-int& RosPublish::trigger(int& dummy, int t) {
- typedef std::map<std::string, bindedSignal_t>::iterator iterator_t;
- ros::Time aTime;
- if (ros::Time::isSimTime()) {
- aTime = ros::Time::now();
- if (aTime <= nextPublication_) return dummy;
-
- nextPublication_ = aTime + rate_;
- } else {
- struct timespec aTimeRT;
- clock_gettime(CLOCK_REALTIME, &aTimeRT);
- nextPublicationRT_.tv_sec = aTimeRT.tv_sec + rate_.sec;
- nextPublicationRT_.tv_nsec = aTimeRT.tv_nsec + rate_.nsec;
- if (nextPublicationRT_.tv_nsec > 1000000000) {
- nextPublicationRT_.tv_nsec -= 1000000000;
- nextPublicationRT_.tv_sec += 1;
- }
- }
-
- boost::mutex::scoped_lock lock(mutex_);
-
- for (iterator_t it = bindedSignal_.begin(); it != bindedSignal_.end(); ++it) {
- boost::get<1>(it->second)(t);
- }
- return dummy;
-}
-
-std::string RosPublish::getDocString() const { return docstring_; }
-
-} // end of namespace dynamicgraph.
diff --git a/ros1/src/ros_publish.hh b/ros1/src/ros_publish.hh
deleted file mode 100644
index 31b8cce..0000000
--- a/ros1/src/ros_publish.hh
+++ /dev/null
@@ -1,88 +0,0 @@
-#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HH
-#define DYNAMIC_GRAPH_ROS_PUBLISH_HH
-#include <map>
-
-#include <boost/shared_ptr.hpp>
-#include <boost/tuple/tuple.hpp>
-#include <boost/thread/mutex.hpp>
-
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/command.h>
-
-#include <ros/ros.h>
-
-#include <realtime_tools/realtime_publisher.h>
-
-#include "converter.hh"
-#include "sot_to_ros.hh"
-#include <fstream>
-
-namespace dynamicgraph {
-class RosPublish;
-
-namespace command {
-namespace rosPublish {
-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);
-
-#undef ROS_PUBLISH_MAKE_COMMAND
-
-} // namespace rosPublish
-} // end of namespace command.
-
-/// \brief Publish dynamic-graph information into ROS.
-class RosPublish : public dynamicgraph::Entity {
- DYNAMIC_GRAPH_ENTITY_DECL();
-
- public:
- typedef boost::function<void(int)> callback_t;
-
- typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >, callback_t> bindedSignal_t;
-
- static const double ROS_JOINT_STATE_PUBLISHER_RATE;
-
- RosPublish(const std::string& n);
- virtual ~RosPublish();
-
- virtual std::string getDocString() const;
- void display(std::ostream& os) const;
-
- void add(const std::string& signal, const std::string& topic);
- void rm(const std::string& signal);
- std::vector<std::string> list() const;
- void clear();
-
- int& trigger(int&, int);
-
- template <typename T>
- void sendData(boost::shared_ptr<realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> > publisher,
- boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time);
-
- template <typename T>
- void add(const std::string& signal, const std::string& topic);
-
- private:
- static const std::string docstring_;
- ros::NodeHandle& nh_;
- std::map<std::string, bindedSignal_t> bindedSignal_;
- dynamicgraph::SignalTimeDependent<int, int> trigger_;
- ros::Duration rate_;
- ros::Time nextPublication_;
- boost::mutex mutex_;
- std::ofstream aofs_;
- struct timespec nextPublicationRT_;
-};
-} // end of namespace dynamicgraph.
-
-#include "ros_publish.hxx"
-#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HH
diff --git a/ros1/src/ros_publish.hxx b/ros1/src/ros_publish.hxx
deleted file mode 100644
index 026b859..0000000
--- a/ros1/src/ros_publish.hxx
+++ /dev/null
@@ -1,63 +0,0 @@
-#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HXX
-#define DYNAMIC_GRAPH_ROS_PUBLISH_HXX
-#include <vector>
-#include <std_msgs/Float64.h>
-
-#include "dynamic_graph_bridge_msgs/Matrix.h"
-#include "dynamic_graph_bridge_msgs/Vector.h"
-
-#include "sot_to_ros.hh"
-
-namespace dynamicgraph {
-template <>
-inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >(
- boost::shared_ptr<realtime_tools::RealtimePublisher<SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> >
- publisher,
- boost::shared_ptr<SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t> signal, int time) {
- SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t result;
- if (publisher->trylock()) {
- publisher->msg_.child_frame_id = "/dynamic_graph/world";
- converter(publisher->msg_, signal->access(time));
- publisher->unlockAndPublish();
- }
-}
-
-template <typename T>
-void RosPublish::sendData(boost::shared_ptr<realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> > publisher,
- boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time) {
- typename SotToRos<T>::ros_t result;
- if (publisher->trylock()) {
- converter(publisher->msg_, signal->access(time));
- publisher->unlockAndPublish();
- }
-}
-
-template <typename T>
-void RosPublish::add(const std::string& signal, const std::string& topic) {
- typedef typename SotToRos<T>::ros_t ros_t;
- typedef typename SotToRos<T>::signalIn_t signal_t;
-
- // Initialize the bindedSignal object.
- bindedSignal_t bindedSignal;
-
- // Initialize the publisher.
- boost::shared_ptr<realtime_tools::RealtimePublisher<ros_t> > pubPtr =
- boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >(nh_, topic, 1);
-
- // Initialize the signal.
- boost::shared_ptr<signal_t> signalPtr(
- new signal_t(0, MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
- boost::get<0>(bindedSignal) = signalPtr;
- SotToRos<T>::setDefault(*signalPtr);
- signalRegistration(*boost::get<0>(bindedSignal));
-
- // Initialize the callback.
- callback_t callback = boost::bind(&RosPublish::sendData<T>, this, pubPtr, signalPtr, _1);
- boost::get<1>(bindedSignal) = callback;
-
- bindedSignal_[signal] = bindedSignal;
-}
-
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HXX
diff --git a/ros1/src/ros_queued_subscribe-python-module-py.cc b/ros1/src/ros_queued_subscribe-python-module-py.cc
deleted file mode 100644
index c14f8e6..0000000
--- a/ros1/src/ros_queued_subscribe-python-module-py.cc
+++ /dev/null
@@ -1,25 +0,0 @@
-#include <dynamic-graph/python/module.hh>
-#include "ros_queued_subscribe.hh"
-
-namespace dg = dynamicgraph;
-
-
-BOOST_PYTHON_MODULE(wrap)
-{
- bp::import("dynamic_graph");
-
- dg::python::exposeEntity<dg::RosQueuedSubscribe, bp::bases<dg::Entity>, dg::python::AddCommands>()
- .def("clear", &dg::RosQueuedSubscribe::clear, "Remove all signals reading data from a ROS topic")
- .def("rm", &dg::RosQueuedSubscribe::rm, "Remove a signal reading data from a ROS topic",
- bp::args("signal_name"))
- .def("list", &dg::RosQueuedSubscribe::list, "List signals reading data from a ROS topic")
- .def("listTopics", &dg::RosQueuedSubscribe::listTopics, "List subscribed topics from ROS in the same order as list command")
- .def("clearQueue", &dg::RosQueuedSubscribe::clearQueue,
- "Empty the queue of a given signal", bp::args("signal_name"))
- .def("queueSize", &dg::RosQueuedSubscribe::queueSize,
- "Return the queue size of a given signal", bp::args("signal_name"))
- .def("readQueue", &dg::RosQueuedSubscribe::readQueue,
- "Whether signals should read values from the queues, and when.",
- bp::args("start_time"))
- ;
-}
diff --git a/ros1/src/ros_queued_subscribe.cpp b/ros1/src/ros_queued_subscribe.cpp
deleted file mode 100644
index f5226af..0000000
--- a/ros1/src/ros_queued_subscribe.cpp
+++ /dev/null
@@ -1,142 +0,0 @@
-//
-// Copyright (c) 2017-2018 CNRS
-// Authors: Joseph Mirabel
-//
-//
-
-#include <boost/assign.hpp>
-#include <boost/bind.hpp>
-#include <boost/format.hpp>
-#include <boost/function.hpp>
-#include <boost/make_shared.hpp>
-
-#include <ros/ros.h>
-#include <std_msgs/Float64.h>
-#include <std_msgs/UInt32.h>
-
-#include <dynamic-graph/factory.h>
-
-#include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_queued_subscribe.hh"
-
-namespace dynamicgraph {
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosQueuedSubscribe, "RosQueuedSubscribe");
-
-namespace command {
-namespace rosQueuedSubscribe {
-Add::Add(RosQueuedSubscribe& entity, const std::string& docstring)
- : Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), docstring) {}
-
-Value Add::doExecute() {
- RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(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 == "double")
- entity.add<double>(type, signal, topic);
- else if (type == "unsigned")
- entity.add<unsigned int>(type, signal, topic);
- else if (type == "matrix")
- entity.add<Matrix>(type, signal, topic);
- else if (type == "vector")
- entity.add<Vector>(type, signal, topic);
- else if (type == "vector3")
- entity.add<specific::Vector3>(type, signal, topic);
- else if (type == "matrixHomo")
- entity.add<sot::MatrixHomogeneous>(type, signal, topic);
- else if (type == "twist")
- entity.add<specific::Twist>(type, signal, topic);
- else
- throw std::runtime_error("bad type");
- return Value();
-}
-} // namespace rosQueuedSubscribe
-} // end of namespace command.
-
-const std::string RosQueuedSubscribe::docstring_(
- "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");
-
-RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n)
- : dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_(), readQueue_(-1) {
- std::string docstring =
- "\n"
- " Add a signal reading data from a ROS topic\n"
- "\n"
- " Input:\n"
- " - type: string among ['double', 'matrix', 'vector', 'vector3',\n"
- " 'matrixHomo', 'twist'],\n"
- " - signal: the signal name in dynamic-graph,\n"
- " - topic: the topic name in ROS.\n"
- "\n";
- addCommand("add", new command::rosQueuedSubscribe::Add(*this, docstring));
-}
-
-RosQueuedSubscribe::~RosQueuedSubscribe() {}
-
-void RosQueuedSubscribe::display(std::ostream& os) const { os << CLASS_NAME << std::endl; }
-
-void RosQueuedSubscribe::rm(const std::string& signal) {
- std::string signalTs = signal + "Timestamp";
-
- signalDeregistration(signal);
- bindedSignal_.erase(signal);
-
- if (bindedSignal_.find(signalTs) != bindedSignal_.end()) {
- signalDeregistration(signalTs);
- bindedSignal_.erase(signalTs);
- }
-}
-
-std::vector<std::string> RosQueuedSubscribe::list() {
- std::vector<std::string> result(bindedSignal_.size());
- std::transform(bindedSignal_.begin(), bindedSignal_.end(),
- result.begin(), [](const auto& pair) { return pair.first; });
- return result;
-}
-
-std::vector<std::string> RosQueuedSubscribe::listTopics()
-{
- std::vector<std::string> result(topics_.size());
- std::transform(topics_.begin(), topics_.end(),
- result.begin(), [](const auto& pair) { return pair.second; });
- return result;
-}
-
-void RosQueuedSubscribe::clear() {
- std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
- for (; it != bindedSignal_.end();) {
- rm(it->first);
- it = bindedSignal_.begin();
- }
-}
-
-void RosQueuedSubscribe::clearQueue(const std::string& signal) {
- if (bindedSignal_.find(signal) != bindedSignal_.end()) {
- bindedSignal_[signal]->clear();
- }
-}
-
-std::size_t RosQueuedSubscribe::queueSize(const std::string& signal) const {
- std::map<std::string, bindedSignal_t>::const_iterator _bs = bindedSignal_.find(signal);
- if (_bs != bindedSignal_.end()) {
- return _bs->second->size();
- }
- return -1;
-}
-
-void RosQueuedSubscribe::readQueue(int beginReadingAt) {
- // Prints signal queues sizes
- /*for (std::map<std::string, bindedSignal_t>::const_iterator it =
- bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
- std::cout << it->first << " : " << it->second->size() << '\n';
- }*/
- readQueue_ = beginReadingAt;
-}
-
-std::string RosQueuedSubscribe::getDocString() const { return docstring_; }
-} // end of namespace dynamicgraph.
diff --git a/ros1/src/ros_queued_subscribe.hh b/ros1/src/ros_queued_subscribe.hh
deleted file mode 100644
index a54b683..0000000
--- a/ros1/src/ros_queued_subscribe.hh
+++ /dev/null
@@ -1,178 +0,0 @@
-//
-// Copyright (c) 2017-2018 CNRS
-// Authors: Joseph Mirabel
-//
-//
-
-#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
-#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
-#include <map>
-
-#include <boost/shared_ptr.hpp>
-#include <boost/thread/mutex.hpp>
-
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/signal-ptr.h>
-#include <dynamic-graph/command.h>
-#include <sot/core/matrix-geometry.hh>
-
-#include <ros/ros.h>
-
-#include "converter.hh"
-#include "sot_to_ros.hh"
-
-namespace dynamicgraph {
-class RosQueuedSubscribe;
-
-namespace command {
-namespace rosQueuedSubscribe {
-using ::dynamicgraph::command::Command;
-using ::dynamicgraph::command::Value;
-
-#define ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(CMD) \
- class CMD : public Command { \
- public: \
- CMD(RosQueuedSubscribe& entity, const std::string& docstring); \
- virtual Value doExecute(); \
- }
-
-ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Add);
-
-#undef ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND
-
-} // end of namespace rosQueuedSubscribe.
-} // end of namespace command.
-
-class RosQueuedSubscribe;
-
-namespace internal {
-template <typename T>
-struct Add;
-
-struct BindedSignalBase {
- typedef boost::shared_ptr<ros::Subscriber> Subscriber_t;
-
- BindedSignalBase(RosQueuedSubscribe* e) : entity(e) {}
- virtual ~BindedSignalBase() {}
-
- virtual void clear() = 0;
- virtual std::size_t size() const = 0;
-
- Subscriber_t subscriber;
- RosQueuedSubscribe* entity;
-};
-
-template <typename T, int BufferSize>
-struct BindedSignal : BindedSignalBase {
- typedef dynamicgraph::Signal<T, int> Signal_t;
- typedef boost::shared_ptr<Signal_t> SignalPtr_t;
- typedef std::vector<T> buffer_t;
- typedef typename buffer_t::size_type size_type;
-
- BindedSignal(RosQueuedSubscribe* e)
- : BindedSignalBase(e), frontIdx(0), backIdx(0), buffer(BufferSize), init(false) {}
- ~BindedSignal() {
- signal.reset();
- clear();
- }
-
- /// See comments in reader and writer for details about synchronisation.
- void clear() {
- // synchronize with method writer
- wmutex.lock();
- if (!empty()) {
- if (backIdx == 0)
- last = buffer[BufferSize - 1];
- else
- last = buffer[backIdx - 1];
- }
- // synchronize with method reader
- rmutex.lock();
- frontIdx = backIdx = 0;
- rmutex.unlock();
- wmutex.unlock();
- }
-
- bool empty() const { return frontIdx == backIdx; }
-
- bool full() const { return ((backIdx + 1) % BufferSize) == frontIdx; }
-
- size_type size() const {
- if (frontIdx <= backIdx)
- return backIdx - frontIdx;
- else
- return backIdx + BufferSize - frontIdx;
- }
-
- SignalPtr_t signal;
- /// Index of the next value to be read.
- size_type frontIdx;
- /// Index of the slot where to write next value (does not contain valid data).
- size_type backIdx;
- buffer_t buffer;
- boost::mutex wmutex, rmutex;
- T last;
- bool init;
-
- template <typename R>
- void writer(const R& data);
- T& reader(T& val, int time);
-};
-} // namespace internal
-
-/// \brief Publish ROS information in the dynamic-graph.
-class RosQueuedSubscribe : public dynamicgraph::Entity {
- DYNAMIC_GRAPH_ENTITY_DECL();
- typedef boost::posix_time::ptime ptime;
-
- public:
- typedef boost::shared_ptr<internal::BindedSignalBase> bindedSignal_t;
-
- RosQueuedSubscribe(const std::string& n);
- virtual ~RosQueuedSubscribe();
-
- virtual std::string getDocString() const;
- void display(std::ostream& os) const;
-
- void rm(const std::string& signal);
- std::vector<std::string> list();
- std::vector<std::string> listTopics();
- void clear();
- void clearQueue(const std::string& signal);
- void readQueue(int beginReadingAt);
- std::size_t queueSize(const std::string& signal) const;
-
- template <typename T>
- void add(const std::string& type, const std::string& signal, const std::string& topic);
-
- std::map<std::string, bindedSignal_t>& bindedSignal() { return bindedSignal_; }
- std::map<std::string, std::string>& topics() { return topics_; }
-
- ros::NodeHandle& nh() { return nh_; }
-
- template <typename R, typename S>
- void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data);
-
- template <typename R>
- void callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data);
-
- template <typename T>
- friend class internal::Add;
-
- private:
- static const std::string docstring_;
- ros::NodeHandle& nh_;
- std::map<std::string, bindedSignal_t> bindedSignal_;
- std::map<std::string, std::string> topics_;
-
- int readQueue_;
- // Signal<bool, int> readQueue_;
-
- template <typename T>
- friend class internal::BindedSignal;
-};
-} // end of namespace dynamicgraph.
-
-#include "ros_queued_subscribe.hxx"
-#endif //! DYNAMIC_GRAPH_QUEUED_ROS_SUBSCRIBE_HH
diff --git a/ros1/src/ros_queued_subscribe.hxx b/ros1/src/ros_queued_subscribe.hxx
deleted file mode 100644
index f8994a2..0000000
--- a/ros1/src/ros_queued_subscribe.hxx
+++ /dev/null
@@ -1,110 +0,0 @@
-//
-// Copyright (c) 2017-2018 CNRS
-// Authors: Joseph Mirabel
-//
-//
-
-#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
-#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
-#define ENABLE_RT_LOG
-
-#include <vector>
-#include <boost/bind.hpp>
-#include <boost/date_time/posix_time/posix_time.hpp>
-#include <dynamic-graph/real-time-logger.h>
-#include <dynamic-graph/signal-caster.h>
-#include <dynamic-graph/linear-algebra.h>
-#include <dynamic-graph/signal-cast-helper.h>
-#include <std_msgs/Float64.h>
-#include "dynamic_graph_bridge_msgs/Matrix.h"
-#include "dynamic_graph_bridge_msgs/Vector.h"
-
-namespace dynamicgraph {
-namespace internal {
-static const int BUFFER_SIZE = 1 << 10;
-
-template <typename T>
-struct Add {
- void operator()(RosQueuedSubscribe& rosSubscribe, const std::string& type, const std::string& signal,
- const std::string& topic) {
- typedef typename SotToRos<T>::sot_t sot_t;
- typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
- typedef BindedSignal<sot_t, BUFFER_SIZE> BindedSignal_t;
- typedef typename BindedSignal_t::Signal_t Signal_t;
-
- // Initialize the bindedSignal object.
- BindedSignal_t* bs = new BindedSignal_t(&rosSubscribe);
- SotToRos<T>::setDefault(bs->last);
-
- // Initialize the signal.
- boost::format signalName("RosQueuedSubscribe(%1%)::output(%2%)::%3%");
- signalName % rosSubscribe.getName() % type % signal;
-
- bs->signal.reset(new Signal_t(signalName.str()));
- bs->signal->setFunction(boost::bind(&BindedSignal_t::reader, bs, _1, _2));
- rosSubscribe.signalRegistration(*bs->signal);
-
- // Initialize the subscriber.
- typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
- callback_t callback = boost::bind(&BindedSignal_t::template writer<ros_const_ptr_t>, bs, _1);
-
- // Keep 50 messages in queue, but only 20 are sent every 100ms
- // -> No message should be lost because of a full buffer
- bs->subscriber = boost::make_shared<ros::Subscriber>(rosSubscribe.nh().subscribe(topic, BUFFER_SIZE, callback));
-
- RosQueuedSubscribe::bindedSignal_t bindedSignal(bs);
- rosSubscribe.bindedSignal()[signal] = bindedSignal;
- rosSubscribe.topics()[signal] = topic;
- }
-};
-
-// template <typename T, typename R>
-template <typename T, int N>
-template <typename R>
-void BindedSignal<T, N>::writer(const R& data) {
- // synchronize with method clear
- boost::mutex::scoped_lock lock(wmutex);
- if (full()) {
- rmutex.lock();
- frontIdx = (frontIdx + 1) % N;
- rmutex.unlock();
- }
- converter(buffer[backIdx], data);
- // No need to synchronize with reader here because:
- // - if the buffer was not empty, then it stays not empty,
- // - if it was empty, then the current value will be used at next time. It
- // means the transmission bandwidth is too low.
- if (!init) {
- last = buffer[backIdx];
- init = true;
- }
- backIdx = (backIdx + 1) % N;
-}
-
-template <typename T, int N>
-T& BindedSignal<T, N>::reader(T& data, int time) {
- // synchronize with method clear:
- // If reading from the list cannot be done, then return last value.
- boost::mutex::scoped_lock lock(rmutex, boost::try_to_lock);
- if (!lock.owns_lock() || entity->readQueue_ == -1 || time < entity->readQueue_) {
- data = last;
- } else {
- if (empty())
- data = last;
- else {
- data = buffer[frontIdx];
- frontIdx = (frontIdx + 1) % N;
- last = data;
- }
- }
- return data;
-}
-} // end of namespace internal.
-
-template <typename T>
-void RosQueuedSubscribe::add(const std::string& type, const std::string& signal, const std::string& topic) {
- internal::Add<T>()(*this, type, signal, topic);
-}
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
diff --git a/ros1/src/ros_subscribe-python-module-py.cc b/ros1/src/ros_subscribe-python-module-py.cc
deleted file mode 100644
index cbe164e..0000000
--- a/ros1/src/ros_subscribe-python-module-py.cc
+++ /dev/null
@@ -1,17 +0,0 @@
-#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/ros1/src/ros_subscribe.cpp b/ros1/src/ros_subscribe.cpp
deleted file mode 100644
index cfa01a2..0000000
--- a/ros1/src/ros_subscribe.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-#include <boost/assign.hpp>
-#include <boost/bind.hpp>
-#include <boost/format.hpp>
-#include <boost/function.hpp>
-#include <boost/make_shared.hpp>
-
-#include <ros/ros.h>
-#include <std_msgs/Float64.h>
-#include <std_msgs/UInt32.h>
-
-#include <dynamic-graph/factory.h>
-
-#include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_subscribe.hh"
-
-namespace dynamicgraph {
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
-
-namespace command {
-namespace rosSubscribe {
-Add::Add(RosSubscribe& entity, const std::string& docstring)
- : Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), docstring) {}
-
-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 = values[1].value();
- const std::string& topic = values[2].value();
-
- if (type == "double")
- entity.add<double>(signal, topic);
- else if (type == "unsigned")
- entity.add<unsigned int>(signal, topic);
- else if (type == "matrix")
- entity.add<dg::Matrix>(signal, topic);
- else if (type == "vector")
- entity.add<dg::Vector>(signal, topic);
- else if (type == "vector3")
- entity.add<specific::Vector3>(signal, topic);
- else if (type == "vector3Stamped")
- entity.add<std::pair<specific::Vector3, dg::Vector> >(signal, topic);
- else if (type == "matrixHomo")
- entity.add<sot::MatrixHomogeneous>(signal, topic);
- else if (type == "matrixHomoStamped")
- entity.add<std::pair<sot::MatrixHomogeneous, dg::Vector> >(signal, topic);
- else if (type == "twist")
- entity.add<specific::Twist>(signal, topic);
- else if (type == "twistStamped")
- entity.add<std::pair<specific::Twist, dg::Vector> >(signal, topic);
- else if (type == "string")
- entity.add<std::string>(signal, topic);
- else
- throw std::runtime_error("bad type");
- return Value();
-}
-} // namespace rosSubscribe
-} // end of namespace command.
-
-const std::string RosSubscribe::docstring_(
- "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), nh_(rosInit(true)), bindedSignal_() {
- std::string docstring =
- "\n"
- " Add a signal reading data from a ROS topic\n"
- "\n"
- " Input:\n"
- " - type: string among ['double', 'matrix', 'vector', 'vector3',\n"
- " 'vector3Stamped', 'matrixHomo', "
- "'matrixHomoStamped',\n"
- " 'twist', 'twistStamped'],\n"
- " - signal: the signal name in dynamic-graph,\n"
- " - topic: the topic name in ROS.\n"
- "\n";
- addCommand("add", new command::rosSubscribe::Add(*this, docstring));
-}
-
-RosSubscribe::~RosSubscribe() {}
-
-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);
- bindedSignal_.erase(signal);
-
- if (bindedSignal_.find(signalTs) != bindedSignal_.end()) {
- signalDeregistration(signalTs);
- bindedSignal_.erase(signalTs);
- }
-}
-
-std::vector<std::string> RosSubscribe::list() {
- std::vector<std::string> result(bindedSignal_.size());
- std::transform(bindedSignal_.begin(), bindedSignal_.end(),
- result.begin(), [](const auto& pair) { return pair.first; });
- return result;
-}
-
-void RosSubscribe::clear() {
- std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
- for (; it != bindedSignal_.end();) {
- rm(it->first);
- it = bindedSignal_.begin();
- }
-}
-
-std::string RosSubscribe::getDocString() const { return docstring_; }
-} // end of namespace dynamicgraph.
diff --git a/ros1/src/ros_subscribe.hh b/ros1/src/ros_subscribe.hh
deleted file mode 100644
index 1928e48..0000000
--- a/ros1/src/ros_subscribe.hh
+++ /dev/null
@@ -1,89 +0,0 @@
-#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
-#define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
-#include <map>
-
-#include <boost/shared_ptr.hpp>
-
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/signal-ptr.h>
-#include <dynamic-graph/command.h>
-#include <sot/core/matrix-geometry.hh>
-
-#include <ros/ros.h>
-
-#include "converter.hh"
-#include "sot_to_ros.hh"
-
-namespace dynamicgraph {
-class RosSubscribe;
-
-namespace command {
-namespace rosSubscribe {
-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& docstring); \
- virtual Value doExecute(); \
- }
-
-ROS_SUBSCRIBE_MAKE_COMMAND(Add);
-
-#undef ROS_SUBSCRIBE_MAKE_COMMAND
-
-} // namespace rosSubscribe
-} // end of namespace command.
-
-namespace internal {
-template <typename T>
-struct Add;
-} // namespace internal
-
-/// \brief Publish ROS information in the dynamic-graph.
-class RosSubscribe : public dynamicgraph::Entity {
- DYNAMIC_GRAPH_ENTITY_DECL();
- typedef boost::posix_time::ptime ptime;
-
- public:
- typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >, boost::shared_ptr<ros::Subscriber> >
- bindedSignal_t;
-
- RosSubscribe(const std::string& n);
- virtual ~RosSubscribe();
-
- virtual std::string getDocString() const;
- void display(std::ostream& os) const;
-
- void add(const std::string& signal, const std::string& topic);
- void rm(const std::string& signal);
- std::vector<std::string> list();
- void clear();
-
- template <typename T>
- void add(const std::string& signal, const std::string& topic);
-
- std::map<std::string, bindedSignal_t>& bindedSignal() { return bindedSignal_; }
-
- ros::NodeHandle& nh() { return nh_; }
-
- template <typename R, typename S>
- void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data);
-
- template <typename R>
- void callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data);
-
- template <typename T>
- friend class internal::Add;
-
- private:
- static const std::string docstring_;
- ros::NodeHandle& nh_;
- std::map<std::string, bindedSignal_t> bindedSignal_;
-};
-} // end of namespace dynamicgraph.
-
-#include "ros_subscribe.hxx"
-#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
diff --git a/ros1/src/ros_subscribe.hxx b/ros1/src/ros_subscribe.hxx
deleted file mode 100644
index b3467d4..0000000
--- a/ros1/src/ros_subscribe.hxx
+++ /dev/null
@@ -1,127 +0,0 @@
-#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
-#define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
-#include <vector>
-#include <boost/bind.hpp>
-#include <boost/date_time/posix_time/posix_time.hpp>
-#include <dynamic-graph/signal-caster.h>
-#include <dynamic-graph/linear-algebra.h>
-#include <dynamic-graph/signal-cast-helper.h>
-#include <std_msgs/Float64.h>
-#include "dynamic_graph_bridge_msgs/Matrix.h"
-#include "dynamic_graph_bridge_msgs/Vector.h"
-#include "ros_time.hh"
-
-namespace dg = dynamicgraph;
-
-namespace dynamicgraph {
-template <typename R, typename S>
-void RosSubscribe::callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data) {
- typedef S sot_t;
- sot_t value;
- converter(value, data);
- signal->setConstant(value);
-}
-
-template <typename R>
-void RosSubscribe::callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data) {
- ptime time = rosTimeToPtime(data->header.stamp);
- signal->setConstant(time);
-}
-
-namespace internal {
-template <typename T>
-struct Add {
- void operator()(RosSubscribe& RosSubscribe, const std::string& signal, const std::string& topic) {
- typedef typename SotToRos<T>::sot_t sot_t;
- typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
- typedef typename SotToRos<T>::signalIn_t signal_t;
-
- // Initialize the bindedSignal object.
- RosSubscribe::bindedSignal_t bindedSignal;
-
- // Initialize the signal.
- boost::format signalName("RosSubscribe(%1%)::%2%");
- signalName % RosSubscribe.getName() % signal;
-
- boost::shared_ptr<signal_t> signal_(new signal_t(0, signalName.str()));
- SotToRos<T>::setDefault(*signal_);
- bindedSignal.first = signal_;
- RosSubscribe.signalRegistration(*bindedSignal.first);
-
- // Initialize the subscriber.
- typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
- callback_t callback = boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>, &RosSubscribe, signal_, _1);
-
- bindedSignal.second = boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callback));
-
- RosSubscribe.bindedSignal()[signal] = bindedSignal;
- }
-};
-
-template <typename T>
-struct Add<std::pair<T, dg::Vector> > {
- void operator()(RosSubscribe& RosSubscribe, const std::string& signal, const std::string& topic) {
- typedef std::pair<T, dg::Vector> type_t;
-
- typedef typename SotToRos<type_t>::sot_t sot_t;
- typedef typename SotToRos<type_t>::ros_const_ptr_t ros_const_ptr_t;
- typedef typename SotToRos<type_t>::signalIn_t signal_t;
-
- // Initialize the bindedSignal object.
- RosSubscribe::bindedSignal_t bindedSignal;
-
- // Initialize the signal.
- boost::format signalName("RosSubscribe(%1%)::%2%");
- signalName % RosSubscribe.getName() % signal;
-
- boost::shared_ptr<signal_t> signal_(new signal_t(0, signalName.str()));
- SotToRos<T>::setDefault(*signal_);
- bindedSignal.first = signal_;
- RosSubscribe.signalRegistration(*bindedSignal.first);
-
- // Initialize the publisher.
- typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
- callback_t callback = boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>, &RosSubscribe, signal_, _1);
-
- bindedSignal.second = boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callback));
-
- RosSubscribe.bindedSignal()[signal] = bindedSignal;
-
- // Timestamp.
- typedef dynamicgraph::SignalPtr<RosSubscribe::ptime, int> signalTimestamp_t;
- std::string signalTimestamp = (boost::format("%1%%2%") % signal % "Timestamp").str();
-
- // Initialize the bindedSignal object.
- RosSubscribe::bindedSignal_t bindedSignalTimestamp;
-
- // Initialize the signal.
- boost::format signalNameTimestamp("RosSubscribe(%1%)::%2%");
- signalNameTimestamp % RosSubscribe.name % signalTimestamp;
-
- boost::shared_ptr<signalTimestamp_t> signalTimestamp_(new signalTimestamp_t(0, signalNameTimestamp.str()));
-
- RosSubscribe::ptime zero(rosTimeToPtime(ros::Time(0, 0)));
- signalTimestamp_->setConstant(zero);
- bindedSignalTimestamp.first = signalTimestamp_;
- RosSubscribe.signalRegistration(*bindedSignalTimestamp.first);
-
- // Initialize the publisher.
- typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
- callback_t callbackTimestamp =
- boost::bind(&RosSubscribe::callbackTimestamp<ros_const_ptr_t>, &RosSubscribe, signalTimestamp_, _1);
-
- bindedSignalTimestamp.second =
- boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp));
-
- RosSubscribe.bindedSignal()[signalTimestamp] = bindedSignalTimestamp;
- }
-};
-} // end of namespace internal.
-
-template <typename T>
-void RosSubscribe::add(const std::string& signal, const std::string& topic) {
- internal::Add<T>()(*this, signal, topic);
-}
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
diff --git a/ros1/src/ros_tf_listener-python-module-py.cc b/ros1/src/ros_tf_listener-python-module-py.cc
deleted file mode 100644
index 3815f45..0000000
--- a/ros1/src/ros_tf_listener-python-module-py.cc
+++ /dev/null
@@ -1,18 +0,0 @@
-#include <dynamic-graph/python/module.hh>
-#include "ros_tf_listener.hh"
-
-namespace dg = dynamicgraph;
-
-BOOST_PYTHON_MODULE(wrap)
-{
- bp::import("dynamic_graph");
-
- dg::python::exposeEntity<dg::RosTfListener, bp::bases<dg::Entity>, dg::python::AddCommands>()
- .def("add", &dg::RosTfListener::add,
- "Add a signal containing the transform between two frames.",
- bp::args( "to_frame_name", "from_frame_name", "out_signal_name"))
- .def("setMaximumDelay", &dg::RosTfListener::setMaximumDelay,
- "Set the maximum time delay of the transform obtained from tf.",
- bp::args("signal_name", "delay_seconds"))
- ;
-}
diff --git a/ros1/src/ros_tf_listener.cpp b/ros1/src/ros_tf_listener.cpp
deleted file mode 100644
index 5c7438b..0000000
--- a/ros1/src/ros_tf_listener.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-#include <pinocchio/fwd.hpp>
-#include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_tf_listener.hh"
-
-#include <dynamic-graph/factory.h>
-
-namespace dynamicgraph {
-namespace internal {
-sot::MatrixHomogeneous& TransformListenerData::getTransform(sot::MatrixHomogeneous& res, int time)
-{
- availableSig.recompute(time);
-
- bool available = availableSig.accessCopy();
-
- if (!available) {
- failbackSig.recompute(time);
- res = failbackSig.accessCopy();
- return res;
- }
-
- const geometry_msgs::TransformStamped::_transform_type::_rotation_type&
- quat = transform.transform.rotation;
- const geometry_msgs::TransformStamped::_transform_type::_translation_type&
- trans = transform.transform.translation;
- res.linear() = sot::Quaternion(quat.w, quat.x, quat.y, quat.z).matrix();
- res.translation() << trans.x, trans.y, trans.z;
- return res;
-}
-
-bool& TransformListenerData::isAvailable(bool& available, int time)
-{
- static const ros::Time origin(0);
- available = false;
- ros::Duration elapsed;
- std::string msg;
-
- if (buffer.canTransform(toFrame, fromFrame, origin, &msg)) {
- transform = buffer.lookupTransform(toFrame, fromFrame, origin);
- if (transform.header.stamp == origin) {
- // This is likely a TF2 static transform.
- available = true;
- } else {
- elapsed = ros::Time::now() - transform.header.stamp;
- available = (elapsed <= max_elapsed);
- }
-
- if (!available) {
- std::ostringstream oss;
- oss << "Use failback " << signal.getName() << " at time " << time
- << ". Time since last update of the transform: " << elapsed;
- entity->SEND_INFO_STREAM_MSG(oss.str());
- }
- } else {
- std::ostringstream oss;
- oss << "Unable to get transform " << signal.getName() << " at time "
- << time << ": " << msg;
- entity->SEND_WARNING_STREAM_MSG(oss.str());
- available = false;
- }
- return available;
-}
-
-} // namespace internal
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTfListener, "RosTfListener");
-} // namespace dynamicgraph
diff --git a/ros1/src/ros_tf_listener.hh b/ros1/src/ros_tf_listener.hh
deleted file mode 100644
index 778c812..0000000
--- a/ros1/src/ros_tf_listener.hh
+++ /dev/null
@@ -1,111 +0,0 @@
-#ifndef DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
-#define DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
-
-#include <boost/bind.hpp>
-
-#include <tf2_ros/transform_listener.h>
-#include <geometry_msgs/TransformStamped.h>
-
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/signal-ptr.h>
-#include <dynamic-graph/command-bind.h>
-
-#include <sot/core/matrix-geometry.hh>
-
-#include <dynamic_graph_bridge/ros_init.hh>
-
-namespace dynamicgraph {
-class RosTfListener;
-
-namespace internal {
-struct TransformListenerData {
- typedef SignalTimeDependent<bool, int> AvailableSignal_t;
- typedef SignalTimeDependent<sot::MatrixHomogeneous, int> MatrixSignal_t;
- typedef SignalPtr<sot::MatrixHomogeneous, int> DefaultSignal_t;
-
- RosTfListener* entity;
- tf2_ros::Buffer& buffer;
- const std::string toFrame, fromFrame;
- geometry_msgs::TransformStamped transform;
- ros::Duration max_elapsed;
- AvailableSignal_t availableSig;
- MatrixSignal_t signal;
- DefaultSignal_t failbackSig;
-
- TransformListenerData(RosTfListener* e, tf2_ros::Buffer& b,
- const std::string& to, const std::string& from,
- const std::string& signame)
- : entity(e)
- , buffer(b)
- , toFrame(to)
- , fromFrame(from)
- , max_elapsed(0.5)
- , availableSig(signame+"_available")
- , signal(signame)
- , failbackSig(NULL, signame+"_failback")
- {
- signal.setFunction(
- boost::bind(&TransformListenerData::getTransform, this, _1, _2));
-
- availableSig.setFunction(
- boost::bind(&TransformListenerData::isAvailable, this, _1, _2));
- availableSig.setNeedUpdateFromAllChildren(true);
-
- failbackSig.setConstant (sot::MatrixHomogeneous::Identity());
- signal.addDependencies(failbackSig << availableSig);
- }
-
- sot::MatrixHomogeneous& getTransform(sot::MatrixHomogeneous& res, int time);
-
- bool& isAvailable(bool& isAvailable, int time);
-};
-} // namespace internal
-
-class RosTfListener : public Entity {
- DYNAMIC_GRAPH_ENTITY_DECL();
-
- public:
- typedef internal::TransformListenerData TransformListenerData;
-
- RosTfListener(const std::string& _name)
- : Entity(_name)
- , buffer()
- , listener(buffer, rosInit(), false)
- {}
-
- ~RosTfListener()
- {
- for (const auto& pair : listenerDatas) delete pair.second;
- }
-
- void add(const std::string& to, const std::string& from, const std::string& signame)
- {
- if (listenerDatas.find(signame) != listenerDatas.end())
- throw std::invalid_argument("A signal " + signame + " already exists in RosTfListener " + getName());
-
- boost::format signalName("RosTfListener(%1%)::output(MatrixHomo)::%2%");
- signalName % getName() % signame;
-
- TransformListenerData* tld =
- new TransformListenerData(this, buffer, to, from, signalName.str());
- signalRegistration(tld->signal << tld->availableSig << tld->failbackSig);
- listenerDatas[signame] = tld;
- }
-
- void setMaximumDelay(const std::string& signame, const double& max_elapsed)
- {
- if (listenerDatas.count(signame) == 0)
- throw std::invalid_argument("No signal " + signame + " in RosTfListener " + getName());
- listenerDatas[signame]->max_elapsed = ros::Duration(max_elapsed);
- }
-
- private:
- typedef std::map<std::string, TransformListenerData*> Map_t;
- Map_t listenerDatas;
- tf2_ros::Buffer buffer;
- tf2_ros::TransformListener listener;
-};
-} // end of namespace dynamicgraph.
-
-#endif // DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
diff --git a/ros1/src/ros_time-python.hh b/ros1/src/ros_time-python.hh
deleted file mode 100644
index 24b46c7..0000000
--- a/ros1/src/ros_time-python.hh
+++ /dev/null
@@ -1,3 +0,0 @@
-#include "ros_time.hh"
-
-typedef boost::mpl::vector< dynamicgraph::RosTime > entities_t;
diff --git a/ros1/src/ros_time.cpp b/ros1/src/ros_time.cpp
deleted file mode 100644
index 7233ad3..0000000
--- a/ros1/src/ros_time.cpp
+++ /dev/null
@@ -1,38 +0,0 @@
-///
-/// Copyright 2012 CNRS
-///
-/// Author: Florent Lamiraux
-
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/signal-caster.h>
-#include <dynamic-graph/signal-cast-helper.h>
-
-#include "ros_time.hh"
-#include "converter.hh"
-
-namespace dynamicgraph {
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTime, "RosTime");
-
-using namespace boost::posix_time;
-
-const std::string RosTime::docstring_(
- "Export ROS time into dynamic-graph.\n"
- "\n"
- " Signal \"time\" provides time as given by ros::time as\n"
- " boost::posix_time::ptime type.\n");
-
-RosTime::RosTime(const std::string& _name)
- : Entity(_name), now_("RosTime(" + _name + ")::output(boost::posix_time::ptime)::time") {
- signalRegistration(now_);
- now_.setConstant(rosTimeToPtime(ros::Time::now()));
- now_.setFunction(boost::bind(&RosTime::update, this, _1, _2));
-}
-
-ptime& RosTime::update(ptime& time, const int&) {
- time = rosTimeToPtime(ros::Time::now());
- return time;
-}
-
-std::string RosTime::getDocString() const { return docstring_; }
-} // namespace dynamicgraph
diff --git a/ros1/src/ros_time.hh b/ros1/src/ros_time.hh
deleted file mode 100644
index a9472f3..0000000
--- a/ros1/src/ros_time.hh
+++ /dev/null
@@ -1,35 +0,0 @@
-///
-/// Copyright 2012 CNRS
-///
-/// Author: Florent Lamiraux
-
-#ifndef DYNAMIC_GRAPH_ROS_TIME_HH
-#define DYNAMIC_GRAPH_ROS_TIME_HH
-
-#include <ros/time.h>
-#include <boost/date_time/posix_time/posix_time_types.hpp>
-#include <dynamic-graph/signal.h>
-#include <dynamic-graph/entity.h>
-
-namespace dynamicgraph {
-
-class RosTime : public dynamicgraph::Entity {
- DYNAMIC_GRAPH_ENTITY_DECL();
-
- public:
- Signal<boost::posix_time::ptime, int> now_;
- RosTime(const std::string& name);
- virtual std::string getDocString() const;
-
- protected:
- boost::posix_time::ptime& update(boost::posix_time::ptime& time, const int& t);
-
- private:
- static const std::string docstring_;
-}; // class RosTime
-
-template <> struct signal_io<boost::posix_time::ptime> : signal_io_unimplemented<boost::posix_time::ptime> {};
-
-} // namespace dynamicgraph
-
-#endif // DYNAMIC_GRAPH_ROS_TIME_HH
diff --git a/ros1/src/sot_loader.cpp b/ros1/src/sot_loader.cpp
deleted file mode 100644
index 80f4fd3..0000000
--- a/ros1/src/sot_loader.cpp
+++ /dev/null
@@ -1,219 +0,0 @@
-/*
- * Copyright 2011,
- * Olivier Stasse,
- *
- * CNRS
- *
- */
-/* -------------------------------------------------------------------------- */
-/* --- INCLUDES ------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-#include <ros/rate.h>
-#include <dynamic_graph_bridge/sot_loader.hh>
-#include "dynamic_graph_bridge/ros_init.hh"
-
-// POSIX.1-2001
-#include <dlfcn.h>
-
-using namespace std;
-using namespace dynamicgraph::sot;
-namespace po = boost::program_options;
-
-struct DataToLog {
- const std::size_t N;
- std::size_t idx, iter;
-
- std::vector<std::size_t> iters;
- std::vector<double> times;
- std::vector<double> ittimes;
-
- DataToLog(std::size_t N_)
- : N(N_)
- , idx(0)
- , iter(0)
- , iters(N, 0)
- , times(N, 0)
- , ittimes(N, 0)
- {}
-
- void record(const double t, const double itt) {
- iters [idx] = iter;
- times [idx] = t;
- ittimes[idx] = itt;
- ++idx;
- ++iter;
- if (idx == N) idx = 0;
- }
-
- void save(const char *prefix) {
- std::ostringstream oss;
- oss << prefix << "-times.log";
-
- std::ofstream aof(oss.str().c_str());
- if (aof.is_open()) {
- for (std::size_t k = 0; k < N; ++k) {
- aof
- << iters [(idx + k) % N] << ' '
- << times [(idx + k) % N] << ' '
- << ittimes[(idx + k) % N] << '\n';
- }
- }
- aof.close();
- }
-};
-
-void workThreadLoader(SotLoader *aSotLoader) {
- ros::Rate rate(1000); // 1 kHz
- if (ros::param::has("/sot_controller/dt")) {
- double periodd;
- ros::param::get("/sot_controller/dt", periodd);
- rate = ros::Rate(1/periodd);
- }
- DataToLog dataToLog(5000);
-
- rate.reset();
- while (ros::ok() && aSotLoader->isDynamicGraphStopped()) {
- rate.sleep();
- }
-
- ros::NodeHandle nh("/geometric_simu");
- bool paused;
- ros::Time timeOrigin = ros::Time::now();
- ros::Time time;
- while (ros::ok() && !aSotLoader->isDynamicGraphStopped()) {
- nh.param<bool>("paused", paused, false);
-
- if (!paused) {
- time = ros::Time::now();
- aSotLoader->oneIteration();
-
- ros::Duration d = ros::Time::now() - time;
- dataToLog.record((time - timeOrigin).toSec(), d.toSec());
- }
- rate.sleep();
- }
- dataToLog.save("/tmp/geometric_simu");
- ros::waitForShutdown();
-}
-
-SotLoader::SotLoader()
- : sensorsIn_(),
- controlValues_(),
- angleEncoder_(),
- angleControl_(),
- forces_(),
- torques_(),
- baseAtt_(),
- accelerometer_(3),
- gyrometer_(3),
- thread_() {
- readSotVectorStateParam();
- initPublication();
-
- freeFlyerPose_.header.frame_id = "odom";
- freeFlyerPose_.child_frame_id = "base_link";
- if (ros::param::get("/sot/tf_base_link",
- freeFlyerPose_.child_frame_id)) {
- ROS_INFO_STREAM("Publishing " << freeFlyerPose_.child_frame_id << " wrt "
- << freeFlyerPose_.header.frame_id);
- }
-}
-
-SotLoader::~SotLoader() {
- dynamic_graph_stopped_ = true;
- thread_.join();
-}
-
-void SotLoader::startControlLoop() { thread_ = boost::thread(workThreadLoader, this); }
-
-void SotLoader::initializeRosNode(int argc, char *argv[]) {
- SotLoaderBasic::initializeRosNode(argc, argv);
- // Temporary fix. TODO: where should nbOfJoints_ be initialized from?
- if (ros::param::has("/sot/state_vector_map")) {
- angleEncoder_.resize(nbOfJoints_);
- angleControl_.resize(nbOfJoints_);
- }
-
- startControlLoop();
-}
-
-void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
- // Update joint values.w
- assert(angleControl_.size() == angleEncoder_.size());
-
- sensorsIn["joints"].setName("angle");
- for (unsigned int i = 0; i < angleControl_.size(); i++) angleEncoder_[i] = angleControl_[i];
- sensorsIn["joints"].setValues(angleEncoder_);
-}
-
-void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
- // Update joint values.
- angleControl_ = controlValues["control"].getValues();
-
- // Debug
- std::map<std::string, dgs::ControlValues>::iterator it = controlValues.begin();
- sotDEBUG(30) << "ControlValues to be broadcasted:" << std::endl;
- for (; it != controlValues.end(); it++) {
- sotDEBUG(30) << it->first << ":";
- std::vector<double> ctrlValues_ = it->second.getValues();
- std::vector<double>::iterator it_d = ctrlValues_.begin();
- for (; it_d != ctrlValues_.end(); it_d++) sotDEBUG(30) << *it_d << " ";
- sotDEBUG(30) << std::endl;
- }
- sotDEBUG(30) << "End ControlValues" << std::endl;
-
- // Check if the size if coherent with the robot description.
- if (angleControl_.size() != (unsigned int)nbOfJoints_) {
- std::cerr << " angleControl_" << angleControl_.size() << " and nbOfJoints" << (unsigned int)nbOfJoints_
- << " are different !" << std::endl;
- exit(-1);
- }
- // Publish the data.
- joint_state_.header.stamp = ros::Time::now();
- for (int i = 0; i < nbOfJoints_; i++) {
- joint_state_.position[i] = angleControl_[i];
- }
- for (unsigned int i = 0; i < parallel_joints_to_state_vector_.size(); i++) {
- joint_state_.position[i + nbOfJoints_] =
- coefficient_parallel_joints_[i] * angleControl_[parallel_joints_to_state_vector_[i]];
- }
-
- joint_pub_.publish(joint_state_);
-
- // Publish robot pose
- // get the robot pose values
- std::vector<double> poseValue_ = controlValues["baseff"].getValues();
-
- freeFlyerPose_.transform.translation.x = poseValue_[0];
- freeFlyerPose_.transform.translation.y = poseValue_[1];
- freeFlyerPose_.transform.translation.z = poseValue_[2];
-
- freeFlyerPose_.transform.rotation.w = poseValue_[3];
- freeFlyerPose_.transform.rotation.x = poseValue_[4];
- freeFlyerPose_.transform.rotation.y = poseValue_[5];
- freeFlyerPose_.transform.rotation.z = poseValue_[6];
-
- freeFlyerPose_.header.stamp = joint_state_.header.stamp;
- // Publish
- freeFlyerPublisher_.sendTransform(freeFlyerPose_);
-}
-
-void SotLoader::setup() {
- fillSensors(sensorsIn_);
- sotController_->setupSetSensors(sensorsIn_);
- sotController_->getControl(controlValues_);
- readControl(controlValues_);
-}
-
-void SotLoader::oneIteration() {
- fillSensors(sensorsIn_);
- try {
- sotController_->nominalSetSensors(sensorsIn_);
- sotController_->getControl(controlValues_);
- } catch (std::exception &) {
- throw;
- }
-
- readControl(controlValues_);
-}
diff --git a/ros1/src/sot_loader_basic.cpp b/ros1/src/sot_loader_basic.cpp
deleted file mode 100644
index 1b3024f..0000000
--- a/ros1/src/sot_loader_basic.cpp
+++ /dev/null
@@ -1,191 +0,0 @@
-/*
- * Copyright 2016,
- * Olivier Stasse,
- *
- * CNRS
- *
- */
-/* -------------------------------------------------------------------------- */
-/* --- INCLUDES ------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-
-#include <dynamic_graph_bridge/sot_loader.hh>
-#include "dynamic_graph_bridge/ros_init.hh"
-#include "dynamic_graph_bridge/ros_parameter.hh"
-
-#include <dynamic-graph/pool.h>
-
-// POSIX.1-2001
-#include <dlfcn.h>
-
-using namespace std;
-using namespace dynamicgraph::sot;
-namespace po = boost::program_options;
-
-SotLoaderBasic::SotLoaderBasic() : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
- readSotVectorStateParam();
- initPublication();
-}
-
-int SotLoaderBasic::initPublication() {
- ros::NodeHandle& n = dynamicgraph::rosInit(false);
-
- // Prepare message to be published
- joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
-
- return 0;
-}
-
-void SotLoaderBasic::initializeRosNode(int, char* []) {
- ROS_INFO("Ready to start dynamic graph.");
- ros::NodeHandle n;
- service_start_ = n.advertiseService("start_dynamic_graph", &SotLoaderBasic::start_dg, this);
-
- service_stop_ = n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);
-
- dynamicgraph::parameter_server_read_robot_description();
-}
-
-void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) { dynamicLibraryName_ = afilename; }
-
-int SotLoaderBasic::readSotVectorStateParam() {
- std::map<std::string, int> from_state_name_to_state_vector;
- std::map<std::string, std::string> from_parallel_name_to_state_vector_name;
- ros::NodeHandle n;
-
- if (!ros::param::has("/sot/state_vector_map")) {
- std::cerr << " Read Sot Vector State Param " << std::endl;
- return 1;
- }
-
- n.getParam("/sot/state_vector_map", stateVectorMap_);
- ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
- nbOfJoints_ = stateVectorMap_.size();
- nbOfParallelJoints_ = 0;
-
- if (ros::param::has("/sot/joint_state_parallel")) {
- XmlRpc::XmlRpcValue joint_state_parallel;
- n.getParam("/sot/joint_state_parallel", joint_state_parallel);
- ROS_ASSERT(joint_state_parallel.getType() == XmlRpc::XmlRpcValue::TypeStruct);
- std::cout << "Type of joint_state_parallel:" << joint_state_parallel.getType() << std::endl;
-
- for (XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin(); it != joint_state_parallel.end(); it++) {
- XmlRpc::XmlRpcValue local_value = it->second;
- std::string final_expression, map_expression = static_cast<string>(local_value);
- double final_coefficient = 1.0;
- // deal with sign
- if (map_expression[0] == '-') {
- final_expression = map_expression.substr(1, map_expression.size() - 1);
- final_coefficient = -1.0;
- } else
- final_expression = map_expression;
-
- std::cout << it->first.c_str() << ": " << final_coefficient << std::endl;
- from_parallel_name_to_state_vector_name[it->first.c_str()] = final_expression;
- coefficient_parallel_joints_.push_back(final_coefficient);
- }
- nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
- }
-
- // Prepare joint_state according to robot description.
- joint_state_.name.resize(nbOfJoints_ + nbOfParallelJoints_);
- joint_state_.position.resize(nbOfJoints_ + nbOfParallelJoints_);
-
- // Fill in the name of the joints from the state vector.
- // and build local map from state name to state vector
- for (int32_t i = 0; i < stateVectorMap_.size(); ++i) {
- joint_state_.name[i] = static_cast<string>(stateVectorMap_[i]);
-
- from_state_name_to_state_vector[joint_state_.name[i]] = i;
- }
-
- // Fill in the name of the joints from the parallel joint vector.
- // and build map from parallel name to state vector
- int i = 0;
- parallel_joints_to_state_vector_.resize(nbOfParallelJoints_);
- for (std::map<std::string, std::string>::iterator it = from_parallel_name_to_state_vector_name.begin();
- it != from_parallel_name_to_state_vector_name.end(); it++, i++) {
- joint_state_.name[i + nbOfJoints_] = it->first.c_str();
- parallel_joints_to_state_vector_[i] = from_state_name_to_state_vector[it->second];
- }
-
- return 0;
-}
-
-int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
- po::options_description desc("Allowed options");
- desc.add_options()("help", "produce help message")("input-file", po::value<string>(), "library to load");
-
- po::store(po::parse_command_line(argc, argv, desc), vm_);
- po::notify(vm_);
-
- if (vm_.count("help")) {
- cout << desc << "\n";
- return -1;
- }
- if (!vm_.count("input-file")) {
- cout << "No filename specified\n";
- return -1;
- } else
- dynamicLibraryName_ = vm_["input-file"].as<string>();
-
- return 0;
-}
-
-void SotLoaderBasic::Initialization() {
- // Load the SotRobotBipedController library.
- sotRobotControllerLibrary_ = dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
- if (!sotRobotControllerLibrary_) {
- std::cerr << "Cannot load library: " << dlerror() << '\n';
- return;
- }
-
- // reset errors
- dlerror();
-
- // Load the symbols.
- createSotExternalInterface_t* createSot = reinterpret_cast<createSotExternalInterface_t*>(
- reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
- const char* dlsym_error = dlerror();
- if (dlsym_error) {
- std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
- return;
- }
-
- // Create robot-controller
- sotController_ = createSot();
- cout << "Went out from Initialization." << endl;
-}
-
-void SotLoaderBasic::CleanUp() {
- dynamicgraph::PoolStorage::destroy();
- // We do not destroy the FactoryStorage singleton because the module will not
- // be reloaded at next initialization (because Python C API cannot safely
- // unload a module...).
- // SignalCaster singleton could probably be destroyed.
-
- // Load the symbols.
- destroySotExternalInterface_t* destroySot = reinterpret_cast<destroySotExternalInterface_t*>(
- reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
- const char* dlsym_error = dlerror();
- if (dlsym_error) {
- std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
- return;
- }
-
- destroySot(sotController_);
- sotController_ = NULL;
-
- /// Uncount the number of access to this library.
- dlclose(sotRobotControllerLibrary_);
-}
-
-bool SotLoaderBasic::start_dg(std_srvs::Empty::Request&, std_srvs::Empty::Response&) {
- dynamic_graph_stopped_ = false;
- return true;
-}
-
-bool SotLoaderBasic::stop_dg(std_srvs::Empty::Request&, std_srvs::Empty::Response&) {
- dynamic_graph_stopped_ = true;
- return true;
-}
diff --git a/ros1/src/sot_to_ros.cpp b/ros1/src/sot_to_ros.cpp
deleted file mode 100644
index 9d6e998..0000000
--- a/ros1/src/sot_to_ros.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-#include "sot_to_ros.hh"
-
-namespace dynamicgraph {
-
-const char* SotToRos<bool>::signalTypeName = "bool";
-const char* SotToRos<double>::signalTypeName = "Double";
-const char* SotToRos<int>::signalTypeName = "int";
-const char* SotToRos<std::string>::signalTypeName = "string";
-const char* SotToRos<unsigned int>::signalTypeName = "Unsigned";
-const char* SotToRos<Matrix>::signalTypeName = "Matrix";
-const char* SotToRos<Vector>::signalTypeName = "Vector";
-const char* SotToRos<specific::Vector3>::signalTypeName = "Vector3";
-const char* SotToRos<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
-const char* SotToRos<specific::Twist>::signalTypeName = "Twist";
-const char* SotToRos<std::pair<specific::Vector3, Vector> >::signalTypeName = "Vector3Stamped";
-const char* SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName = "MatrixHomo";
-const char* SotToRos<std::pair<specific::Twist, Vector> >::signalTypeName = "Twist";
-
-} // end of namespace dynamicgraph.
diff --git a/ros1/src/sot_to_ros.hh b/ros1/src/sot_to_ros.hh
deleted file mode 100644
index 054e6ff..0000000
--- a/ros1/src/sot_to_ros.hh
+++ /dev/null
@@ -1,311 +0,0 @@
-#ifndef DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
-#define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
-#include <vector>
-#include <utility>
-
-#include <boost/format.hpp>
-
-#include <std_msgs/Bool.h>
-#include <std_msgs/Float64.h>
-#include <std_msgs/UInt32.h>
-#include <std_msgs/Int32.h>
-#include <std_msgs/String.h>
-#include "dynamic_graph_bridge_msgs/Matrix.h"
-#include "dynamic_graph_bridge_msgs/Vector.h"
-
-#include "geometry_msgs/Transform.h"
-#include "geometry_msgs/TransformStamped.h"
-#include "geometry_msgs/Twist.h"
-#include "geometry_msgs/TwistStamped.h"
-#include "geometry_msgs/Vector3Stamped.h"
-
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/linear-algebra.h>
-#include <dynamic-graph/signal-ptr.h>
-#include <sot/core/matrix-geometry.hh>
-
-#define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME) \
- makeSignalString(typeid(*this).name(), NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)
-
-namespace dynamicgraph {
-
-/// \brief Types dedicated to identify pairs of (dg,ros) data.
-///
-/// They do not contain any data but allow to make the difference
-/// between different types with the same structure.
-/// For instance a vector of six elements vs a twist coordinate.
-namespace specific {
-class Vector3 {};
-class Twist {};
-} // end of namespace specific.
-
-/// \brief SotToRos trait type.
-///
-/// This trait provides types associated to a dynamic-graph type:
-/// - ROS corresponding type,
-/// - signal type / input signal type,
-/// - ROS callback type.
-template <typename SotType>
-class SotToRos;
-
-template <>
-struct SotToRos<bool> {
- typedef bool sot_t;
- typedef std_msgs::Bool ros_t;
- typedef std_msgs::BoolConstPtr ros_const_ptr_t;
- typedef dynamicgraph::Signal<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- s.setConstant(false);
- }
-
- static void setDefault(sot_t& s) { s = false; }
-};
-
-template <>
-struct SotToRos<double> {
- typedef double sot_t;
- typedef std_msgs::Float64 ros_t;
- typedef std_msgs::Float64ConstPtr ros_const_ptr_t;
- typedef dynamicgraph::Signal<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- s.setConstant(0.);
- }
-
- static void setDefault(sot_t& s) { s = 0.; }
-};
-
-template <>
-struct SotToRos<unsigned int> {
- typedef unsigned int sot_t;
- typedef std_msgs::UInt32 ros_t;
- typedef std_msgs::UInt32ConstPtr ros_const_ptr_t;
- typedef dynamicgraph::Signal<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- s.setConstant(0);
- }
-
- static void setDefault(sot_t& s) { s = 0; }
-};
-
-template <>
-struct SotToRos<int> {
- typedef int sot_t;
- typedef std_msgs::Int32 ros_t;
- typedef std_msgs::Int32ConstPtr ros_const_ptr_t;
- typedef dynamicgraph::Signal<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- s.setConstant(0);
- }
-
- static void setDefault(sot_t& s) { s = 0; }
-};
-
-template <>
-struct SotToRos<std::string> {
- typedef std::string sot_t;
- typedef std_msgs::String ros_t;
- typedef std_msgs::StringConstPtr ros_const_ptr_t;
- typedef dynamicgraph::Signal<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- s.setConstant("");
- }
-
- static void setDefault(sot_t& s) { s = std::string(); }
-};
-
-template <>
-struct SotToRos<Matrix> {
- typedef Matrix sot_t;
- typedef dynamic_graph_bridge_msgs::Matrix ros_t;
- typedef dynamic_graph_bridge_msgs::MatrixConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- Matrix m;
- m.resize(0, 0);
- s.setConstant(m);
- }
-
- static void setDefault(sot_t& s) { s.resize(0, 0); }
-};
-
-template <>
-struct SotToRos<Vector> {
- typedef Vector sot_t;
- typedef dynamic_graph_bridge_msgs::Vector ros_t;
- typedef dynamic_graph_bridge_msgs::VectorConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- Vector v;
- v.resize(0);
- s.setConstant(v);
- }
-
- static void setDefault(sot_t& s) { s.resize(0); }
-};
-
-template <>
-struct SotToRos<specific::Vector3> {
- typedef Vector sot_t;
- typedef geometry_msgs::Vector3 ros_t;
- typedef geometry_msgs::Vector3ConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- Vector v(Vector::Zero(3));
- s.setConstant(v);
- }
-
- static void setDefault(sot_t& s) { s = Vector::Zero(3); }
-};
-
-template <>
-struct SotToRos<sot::MatrixHomogeneous> {
- typedef sot::MatrixHomogeneous sot_t;
- typedef geometry_msgs::Transform ros_t;
- typedef geometry_msgs::TransformConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- sot::MatrixHomogeneous m;
- s.setConstant(m);
- }
-
- static void setDefault(sot_t& s) { s.setIdentity(); }
-};
-
-template <>
-struct SotToRos<specific::Twist> {
- typedef Vector sot_t;
- typedef geometry_msgs::Twist ros_t;
- typedef geometry_msgs::TwistConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- Vector v(6);
- v.setZero();
- s.setConstant(v);
- }
-
- static void setDefault(sot_t& s) { s = Vector::Zero(6); }
-};
-
-// Stamped vector3
-template <>
-struct SotToRos<std::pair<specific::Vector3, Vector> > {
- typedef Vector sot_t;
- typedef geometry_msgs::Vector3Stamped ros_t;
- typedef geometry_msgs::Vector3StampedConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- SotToRos<sot_t>::setDefault(s);
- }
-};
-
-// Stamped transformation
-template <>
-struct SotToRos<std::pair<sot::MatrixHomogeneous, Vector> > {
- typedef sot::MatrixHomogeneous sot_t;
- typedef geometry_msgs::TransformStamped ros_t;
- typedef geometry_msgs::TransformStampedConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- SotToRos<sot_t>::setDefault(s);
- }
-};
-
-// Stamped twist
-template <>
-struct SotToRos<std::pair<specific::Twist, Vector> > {
- typedef Vector sot_t;
- typedef geometry_msgs::TwistStamped ros_t;
- typedef geometry_msgs::TwistStampedConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t&(sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s) {
- SotToRos<sot_t>::setDefault(s);
- }
-};
-
-inline std::string makeSignalString(const std::string& className, const std::string& instanceName, bool isInputSignal,
- const std::string& signalType, const std::string& signalName) {
- boost::format fmt("%s(%s)::%s(%s)::%s");
- fmt % className % instanceName % (isInputSignal ? "input" : "output") % signalType % signalName;
- return fmt.str();
-}
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
diff --git a/ros1/tests/CMakeLists.txt b/ros1/tests/CMakeLists.txt
deleted file mode 100644
index 92e10eb..0000000
--- a/ros1/tests/CMakeLists.txt
+++ /dev/null
@@ -1,4 +0,0 @@
-IF(BUILD_PYTHON_INTERFACE)
- # TODO: this test requires a ros master
- #ADD_PYTHON_UNIT_TEST("py-import" "tests/test_import.py")
-ENDIF(BUILD_PYTHON_INTERFACE)
diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt
deleted file mode 100644
index 4e8f2ac..0000000
--- a/ros2/CMakeLists.txt
+++ /dev/null
@@ -1,47 +0,0 @@
-# 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/remote_python_client.py RENAME remote_python_client DESTINATION lib/${PROJECT_NAME})
-install(PROGRAMS scripts/remote_python_client.py RENAME run_command DESTINATION lib/${PROJECT_NAME})
-
-# we also need to install the header files
-install(DIRECTORY include/ DESTINATION include)
diff --git a/ros2/src/CMakeLists.txt b/ros2/src/CMakeLists.txt
deleted file mode 100644
index 73d141e..0000000
--- a/ros2/src/CMakeLists.txt
+++ /dev/null
@@ -1,238 +0,0 @@
-# Copyright (C) 2021 LAAS-CNRS.
-#
-# Author: Maxmilien Naveau
-#
-
-
-#.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}/ros2/src/dynamic_graph/${SUBMODULENAME}/python-module-py.cc
- @ONLY
- )
- SET(ARG_SOURCE_PYTHON_MODULE "${PROJECT_BINARY_DIR}/ros2/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}/ros2/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}/ros2/src/dynamic_graph/${SUBMODULENAME}/__init__.py
- )
-
- INSTALL(
- FILES ${PROJECT_BINARY_DIR}/ros2/src/dynamic_graph/${SUBMODULENAME}/__init__.py
- DESTINATION ${PYTHON_INSTALL_DIR}
- )
-
- endif()
-
-ENDMACRO(CUSTOM_DYNAMIC_GRAPH_PYTHON_MODULE SUBMODULENAME)
-
-
-
-
-# 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(plugin_library_name ${plugin} NAME)
- add_library(${plugin_library_name} SHARED
- ${PROJECT_SOURCE_DIR}/ros2/src/${plugin}.cpp #
- ${PROJECT_SOURCE_DIR}/ros2/src/${plugin}.hpp #
- )
- if(SUFFIX_SO_VERSION)
- set_target_properties(${plugin_library_name} PROPERTIES SOVERSION
- ${PROJECT_VERSION})
- endif(SUFFIX_SO_VERSION)
- target_link_libraries(
- ${plugin_library_name} ${${plugin_library_name}_deps} ${catkin_LIBRARIES} ros_bridge
- dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp
- )
- target_include_directories(${plugin_library_name}
- PUBLIC $<INSTALL_INTERFACE:include>
- PRIVATE ${PROJECT_SOURCE_DIR}/ros2/include)
- if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
- install(
- TARGETS ${plugin_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 ${plugin_library_name})
- if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
- custom_dynamic_graph_python_module(
- "ros/${PYTHON_LIBRARY_NAME}" ${plugin_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}" ${plugin_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
- PUBLIC $<INSTALL_INTERFACE:include>
- PRIVATE ${PROJECT_SOURCE_DIR}/ros2/include)
- 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)
-
-# 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)
-target_include_directories(sot_loader
- PUBLIC $<INSTALL_INTERFACE:include>
- PRIVATE ${PROJECT_SOURCE_DIR}/ros2/include)
-install(
- TARGETS sot_loader
- EXPORT ${TARGETS_EXPORT_NAME}
- DESTINATION lib)
-
-# Stand alone embedded intepreter with a robot controller.
-add_executable(geometric_simu programs/geometric_simu.cpp)
-target_link_libraries(
- geometric_simu Boost::program_options ${CMAKE_DL_LIBS} ${catkin_LIBRARIES}
- ros_bridge sot_loader
- dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp)
-target_include_directories(geometric_simu
- PUBLIC $<INSTALL_INTERFACE:include>
- PRIVATE ${PROJECT_SOURCE_DIR}/ros2/include)
-install(TARGETS geometric_simu DESTINATION lib/${PROJECT_NAME})
diff --git a/ros2/tests/test_import.py b/ros2/tests/test_import.py
deleted file mode 100755
index b919565..0000000
--- a/ros2/tests/test_import.py
+++ /dev/null
@@ -1,27 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_graph.ros import RosImport
-
-ri = RosImport('rosimport')
-
-ri.add('double', 'doubleS', 'doubleT')
-ri.add('vector', 'vectorS', 'vectorT')
-ri.add('matrix', 'matrixS', 'matrixT')
-
-ri.doubleS.value = 42.
-ri.vectorS.value = (
- 42.,
- 42.,
-)
-ri.matrixS.value = (
- (
- 42.,
- 42.,
- ),
- (
- 42.,
- 42.,
- ),
-)
-
-ri.trigger.recompute(ri.trigger.time + 1)
diff --git a/ros2/scripts/remote_python_client.py b/scripts/remote_python_client.py
similarity index 100%
rename from ros2/scripts/remote_python_client.py
rename to scripts/remote_python_client.py
diff --git a/ros2/scripts/run_command b/scripts/run_command
similarity index 100%
rename from ros2/scripts/run_command
rename to scripts/run_command
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000..222aa75
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,123 @@
+# Copyright (C) 2021 LAAS-CNRS.
+#
+# Author: Maxmilien Naveau
+#
+
+# Main Library
+set(${PROJECT_NAME}_HEADERS
+ ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/ros_parameter.hpp
+ ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/ros_python_interpreter_client.hpp
+ ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/ros_python_interpreter_server.hpp
+ ${PROJECT_SOURCE_DIR}/src/dg_ros_mapping.hpp
+ ${PROJECT_SOURCE_DIR}/src/matrix_geometry.hpp
+ ${PROJECT_SOURCE_DIR}/src/time_point_io.hpp)
+set(${PROJECT_NAME}_SOURCES
+ ${PROJECT_SOURCE_DIR}/src/dg_ros_mapping.cpp #
+ ${PROJECT_SOURCE_DIR}/src/ros_python_interpreter_client.cpp #
+ ${PROJECT_SOURCE_DIR}/src/ros_python_interpreter_server.cpp #
+ ${PROJECT_SOURCE_DIR}/src/ros.cpp #
+ ${PROJECT_SOURCE_DIR}/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}/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(plugin_library_name ${plugin} NAME)
+ add_library(${plugin_library_name} SHARED
+ ${PROJECT_SOURCE_DIR}/src/${plugin}.cpp #
+ ${PROJECT_SOURCE_DIR}/src/${plugin}.hpp #
+ )
+ if(SUFFIX_SO_VERSION)
+ set_target_properties(${plugin_library_name} PROPERTIES SOVERSION
+ ${PROJECT_VERSION})
+ endif(SUFFIX_SO_VERSION)
+ target_link_libraries(
+ ${plugin_library_name} ${${plugin_library_name}_deps} ${catkin_LIBRARIES} ros_bridge
+ dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp
+ )
+ target_include_directories(${plugin_library_name}
+ PUBLIC $<INSTALL_INTERFACE:include>
+ PRIVATE ${PROJECT_SOURCE_DIR}/include)
+ if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ install(
+ TARGETS ${plugin_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 ${plugin_library_name})
+ if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
+ dynamic_graph_python_module(
+ "ros/${PYTHON_LIBRARY_NAME}" ${plugin_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}" ${plugin_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
+ PUBLIC $<INSTALL_INTERFACE:include>
+ PRIVATE ${PROJECT_SOURCE_DIR}/include)
+ 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)
+
+# 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)
+target_include_directories(sot_loader
+ PUBLIC $<INSTALL_INTERFACE:include>
+ PRIVATE ${PROJECT_SOURCE_DIR}/include)
+install(
+ TARGETS sot_loader
+ EXPORT ${TARGETS_EXPORT_NAME}
+ DESTINATION lib)
+
+# Stand alone embedded intepreter with a robot controller.
+add_executable(geometric_simu programs/geometric_simu.cpp)
+target_link_libraries(
+ geometric_simu Boost::program_options ${CMAKE_DL_LIBS} ${catkin_LIBRARIES}
+ ros_bridge sot_loader
+ dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp)
+target_include_directories(geometric_simu
+ PUBLIC $<INSTALL_INTERFACE:include>
+ PRIVATE ${PROJECT_SOURCE_DIR}/include)
+install(TARGETS geometric_simu DESTINATION lib/${PROJECT_NAME})
diff --git a/ros2/src/dg_ros_mapping.cpp b/src/dg_ros_mapping.cpp
similarity index 100%
rename from ros2/src/dg_ros_mapping.cpp
rename to src/dg_ros_mapping.cpp
diff --git a/ros2/src/dg_ros_mapping.hpp b/src/dg_ros_mapping.hpp
similarity index 100%
rename from ros2/src/dg_ros_mapping.hpp
rename to src/dg_ros_mapping.hpp
diff --git a/ros2/src/dynamic_graph/ros/__init__.py b/src/dynamic_graph/ros/__init__.py
similarity index 100%
rename from ros2/src/dynamic_graph/ros/__init__.py
rename to src/dynamic_graph/ros/__init__.py
diff --git a/ros2/src/dynamic_graph/ros/dgcompleter.py b/src/dynamic_graph/ros/dgcompleter.py
similarity index 100%
rename from ros2/src/dynamic_graph/ros/dgcompleter.py
rename to src/dynamic_graph/ros/dgcompleter.py
diff --git a/ros2/src/dynamic_graph/ros/ros.py b/src/dynamic_graph/ros/ros.py
similarity index 100%
rename from ros2/src/dynamic_graph/ros/ros.py
rename to src/dynamic_graph/ros/ros.py
diff --git a/ros2/src/matrix_geometry.hpp b/src/matrix_geometry.hpp
similarity index 100%
rename from ros2/src/matrix_geometry.hpp
rename to src/matrix_geometry.hpp
diff --git a/ros2/src/programs/geometric_simu.cpp b/src/programs/geometric_simu.cpp
similarity index 100%
rename from ros2/src/programs/geometric_simu.cpp
rename to src/programs/geometric_simu.cpp
diff --git a/ros2/src/ros.cpp b/src/ros.cpp
similarity index 100%
rename from ros2/src/ros.cpp
rename to src/ros.cpp
diff --git a/ros2/src/ros_parameter.cpp b/src/ros_parameter.cpp
similarity index 100%
rename from ros2/src/ros_parameter.cpp
rename to src/ros_parameter.cpp
diff --git a/ros2/src/ros_publish-python-module-py.cc b/src/ros_publish-python-module-py.cc
similarity index 100%
rename from ros2/src/ros_publish-python-module-py.cc
rename to src/ros_publish-python-module-py.cc
diff --git a/ros2/src/ros_publish.cpp b/src/ros_publish.cpp
similarity index 100%
rename from ros2/src/ros_publish.cpp
rename to src/ros_publish.cpp
diff --git a/ros2/src/ros_publish.hpp b/src/ros_publish.hpp
similarity index 100%
rename from ros2/src/ros_publish.hpp
rename to src/ros_publish.hpp
diff --git a/ros2/src/ros_publish.hxx b/src/ros_publish.hxx
similarity index 100%
rename from ros2/src/ros_publish.hxx
rename to src/ros_publish.hxx
diff --git a/ros2/src/ros_python_interpreter_client.cpp b/src/ros_python_interpreter_client.cpp
similarity index 100%
rename from ros2/src/ros_python_interpreter_client.cpp
rename to src/ros_python_interpreter_client.cpp
diff --git a/ros2/src/ros_python_interpreter_server.cpp b/src/ros_python_interpreter_server.cpp
similarity index 100%
rename from ros2/src/ros_python_interpreter_server.cpp
rename to src/ros_python_interpreter_server.cpp
diff --git a/ros2/src/ros_subscribe-python-module-py.cc b/src/ros_subscribe-python-module-py.cc
similarity index 100%
rename from ros2/src/ros_subscribe-python-module-py.cc
rename to src/ros_subscribe-python-module-py.cc
diff --git a/ros2/src/ros_subscribe.cpp b/src/ros_subscribe.cpp
similarity index 100%
rename from ros2/src/ros_subscribe.cpp
rename to src/ros_subscribe.cpp
diff --git a/ros2/src/ros_subscribe.hpp b/src/ros_subscribe.hpp
similarity index 100%
rename from ros2/src/ros_subscribe.hpp
rename to src/ros_subscribe.hpp
diff --git a/ros2/src/ros_subscribe.hxx b/src/ros_subscribe.hxx
similarity index 100%
rename from ros2/src/ros_subscribe.hxx
rename to src/ros_subscribe.hxx
diff --git a/ros2/src/sot_loader.cpp b/src/sot_loader.cpp
similarity index 100%
rename from ros2/src/sot_loader.cpp
rename to src/sot_loader.cpp
diff --git a/ros2/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
similarity index 100%
rename from ros2/src/sot_loader_basic.cpp
rename to src/sot_loader_basic.cpp
diff --git a/ros2/src/time_point_io.hpp b/src/time_point_io.hpp
similarity index 100%
rename from ros2/src/time_point_io.hpp
rename to src/time_point_io.hpp
diff --git a/ros2/tests/CMakeLists.txt b/tests/CMakeLists.txt
similarity index 94%
rename from ros2/tests/CMakeLists.txt
rename to tests/CMakeLists.txt
index ca26786..d1bce31 100644
--- a/ros2/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -22,7 +22,7 @@ if(BUILD_TESTING)
target_include_directories(
test_sot_loader_basic
PUBLIC include "${GMOCK_INCLUDE_DIRS}"
- PRIVATE ${PROJECT_SOURCE_DIR}/ros2/include)
+ PRIVATE ${PROJECT_SOURCE_DIR}/include)
target_link_libraries(test_sot_loader_basic sot_loader "${GMOCK_LIBRARIES}")
add_launch_test(launch/launching_test_sot_loader_basic.py)
@@ -32,7 +32,7 @@ if(BUILD_TESTING)
target_include_directories(
test_sot_loader
PUBLIC include "${GMOCK_INCLUDE_DIRS}"
- PRIVATE ${PROJECT_SOURCE_DIR}/ros2/include)
+ PRIVATE ${PROJECT_SOURCE_DIR}/include)
target_link_libraries(test_sot_loader sot_loader "${GMOCK_LIBRARIES}")
add_launch_test(launch/launching_test_sot_loader.py)
@@ -65,7 +65,7 @@ if(BUILD_TESTING)
$<INSTALL_INTERFACE:include>
SYSTEM
PUBLIC ${PYTHON_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}
- PRIVATE ${PROJECT_SOURCE_DIR}/ros2/include)
+ PRIVATE ${PROJECT_SOURCE_DIR}/include)
# Link the dependecies to it.
target_link_libraries(test_${test_name} ros_bridge)
diff --git a/ros2/tests/impl_test_sot_external_interface.cpp b/tests/impl_test_sot_external_interface.cpp
similarity index 100%
rename from ros2/tests/impl_test_sot_external_interface.cpp
rename to tests/impl_test_sot_external_interface.cpp
diff --git a/ros2/tests/impl_test_sot_external_interface.hh b/tests/impl_test_sot_external_interface.hh
similarity index 100%
rename from ros2/tests/impl_test_sot_external_interface.hh
rename to tests/impl_test_sot_external_interface.hh
diff --git a/ros2/tests/launch/__pycache__/launching_test_sot_loader.cpython-38.pyc b/tests/launch/__pycache__/launching_test_sot_loader.cpython-38.pyc
similarity index 65%
rename from ros2/tests/launch/__pycache__/launching_test_sot_loader.cpython-38.pyc
rename to tests/launch/__pycache__/launching_test_sot_loader.cpython-38.pyc
index 38a7e4a70f12f5d39995c8bd4a099fedc28c2e57..a04ec3480b0bdac3ec66823938d686adb30a1d98 100644
GIT binary patch
delta 134
zcmeAZ=ojD(<>lpK00O_v&53Cnd1ILw6DDUfn@?WO?8#^``6IKi1`{L4e-2hQMy7vk
ztW1nB$nu+wlZ6Q=$H?)Q=|2-Y%a6%wEUy`TCJV8aaC-v{EM@`{9E_7EvYuoNnjFm*
U!yNz@W!ij*&7F}|T?l9_0G4wa^8f$<
delta 153
zcmeAd=o8=#<>lpK0D=cQHY7%E<c(!!Or4y~Y|c`YUu-maA+wj65lEJajfshgjgg6w
z=`YiN7Iq;J`#&2qkPn3{zu7ohfNTy%j=wA{EWanKv%F?>oh-~+!s`swU(5s~IGBp0
jCu_1;Og_Wf&*(ilhAoEM11`tB`7oP1Bdfd+D;pyKU;`aP
diff --git a/ros2/tests/launch/__pycache__/launching_test_sot_loader_basic.cpython-38.pyc b/tests/launch/__pycache__/launching_test_sot_loader_basic.cpython-38.pyc
similarity index 89%
rename from ros2/tests/launch/__pycache__/launching_test_sot_loader_basic.cpython-38.pyc
rename to tests/launch/__pycache__/launching_test_sot_loader_basic.cpython-38.pyc
index 14e3f9104e814af03429f3496e89f183a7955e00..42ec10ae70c1459d9ab397281188d01c5900f11d 100644
GIT binary patch
delta 31
lcmdlkut|VBl$V!_0SJOKH*e(bXJ$;BJe%2k^L6F~765#(2#Wv!
delta 36
qcmdlauw8&Vl$V!_0SG>C->{LppP4ax@@!^vmZJP(qs<qY6IlShr3(N6
diff --git a/ros2/tests/launch/launching_test_sot_loader.py b/tests/launch/launching_test_sot_loader.py
similarity index 65%
rename from ros2/tests/launch/launching_test_sot_loader.py
rename to tests/launch/launching_test_sot_loader.py
index 788c222..10a97f5 100644
--- a/ros2/tests/launch/launching_test_sot_loader.py
+++ b/tests/launch/launching_test_sot_loader.py
@@ -36,42 +36,49 @@ import pytest
def generate_test_description():
ld = LaunchDescription()
- robot_description_content_path = PathJoinSubstitution(
- [
- get_package_share_directory("dynamic_graph_bridge"),
- "urdf",
- "dgb_minimal_robot.urdf",
- ]
- )
- robot_description_content = open(robot_description_content_path.perform(None)).read()
-
- params = { "state_vector_map": [ "joint1", "joint2"],
- "robot_description": robot_description_content};
+ robot_description_content_path = PathJoinSubstitution(
+ [
+ get_package_share_directory("dynamic_graph_bridge"),
+ "urdf",
+ "dgb_minimal_robot.urdf",
+ ]
+ )
+ robot_description_content = open(
+ robot_description_content_path.perform(None)
+ ).read()
+
+ params = {
+ "state_vector_map": ["joint1", "joint2"],
+ "robot_description": robot_description_content,
+ }
terminating_process = Node(
- package="dynamic_graph_bridge",
- executable="test_sot_loader",
- output="screen",
- emulate_tty=True,
- parameters=[params],
+ package="dynamic_graph_bridge",
+ executable="test_sot_loader",
+ output="screen",
+ emulate_tty=True,
+ parameters=[params],
)
return (
- launch.LaunchDescription([
- terminating_process,
- launch_testing.util.KeepAliveProc(),
- launch_testing.actions.ReadyToTest(),
- ]), locals()
+ launch.LaunchDescription(
+ [
+ terminating_process,
+ launch_testing.util.KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ locals(),
)
-class TestSotLoaderBasic(unittest.TestCase):
+class TestSotLoaderBasic(unittest.TestCase):
def test_termination(self, terminating_process, proc_info):
proc_info.assertWaitForShutdown(process=terminating_process, timeout=(10))
+
@launch_testing.post_shutdown_test()
class TestProcessOutput(unittest.TestCase):
-
def test_exit_code(self, proc_info):
# Check that all processes in the launch (in this case, there's just one) exit
# with code 0
diff --git a/ros2/tests/launch/launching_test_sot_loader_basic.py b/tests/launch/launching_test_sot_loader_basic.py
similarity index 100%
rename from ros2/tests/launch/launching_test_sot_loader_basic.py
rename to tests/launch/launching_test_sot_loader_basic.py
diff --git a/ros2/tests/main.cpp b/tests/main.cpp
similarity index 100%
rename from ros2/tests/main.cpp
rename to tests/main.cpp
diff --git a/ros2/tests/python_scripts/simple_add.py b/tests/python_scripts/simple_add.py
similarity index 100%
rename from ros2/tests/python_scripts/simple_add.py
rename to tests/python_scripts/simple_add.py
diff --git a/ros2/tests/python_scripts/simple_add_fail.py b/tests/python_scripts/simple_add_fail
similarity index 100%
rename from ros2/tests/python_scripts/simple_add_fail.py
rename to tests/python_scripts/simple_add_fail
diff --git a/ros2/tests/test_common.hpp b/tests/test_common.hpp
similarity index 100%
rename from ros2/tests/test_common.hpp
rename to tests/test_common.hpp
diff --git a/ros1/tests/test_import.py b/tests/test_import.py
similarity index 100%
rename from ros1/tests/test_import.py
rename to tests/test_import.py
diff --git a/ros2/tests/test_ros_init.cpp b/tests/test_ros_init.cpp
similarity index 100%
rename from ros2/tests/test_ros_init.cpp
rename to tests/test_ros_init.cpp
diff --git a/ros2/tests/test_ros_interpreter.cpp b/tests/test_ros_interpreter.cpp
similarity index 99%
rename from ros2/tests/test_ros_interpreter.cpp
rename to tests/test_ros_interpreter.cpp
index 7966e70..ebc73db 100644
--- a/ros2/tests/test_ros_interpreter.cpp
+++ b/tests/test_ros_interpreter.cpp
@@ -256,7 +256,7 @@ TEST_F(TestRosInterpreter, test_call_run_script_standarderror)
// Create and call the clients.
std::string file_name =
- TEST_CONFIG_PATH + std::string("simple_add_fail.py");
+ TEST_CONFIG_PATH + std::string("simple_add_fail");
std::string result = "";
start_run_python_script_ros_service(file_name, result);
diff --git a/ros2/tests/test_sot_loader.cpp b/tests/test_sot_loader.cpp
similarity index 100%
rename from ros2/tests/test_sot_loader.cpp
rename to tests/test_sot_loader.cpp
diff --git a/ros2/tests/test_sot_loader_basic.cpp b/tests/test_sot_loader_basic.cpp
similarity index 100%
rename from ros2/tests/test_sot_loader_basic.cpp
rename to tests/test_sot_loader_basic.cpp
diff --git a/ros2/tests/urdf/dgb_minimal_robot.urdf b/tests/urdf/dgb_minimal_robot.urdf
similarity index 100%
rename from ros2/tests/urdf/dgb_minimal_robot.urdf
rename to tests/urdf/dgb_minimal_robot.urdf
--
GitLab