From adbb769f257dcd64314e764d03aab168ef8e9e10 Mon Sep 17 00:00:00 2001
From: MaximilienNaveau <maximilien.naveau@gmail.com>
Date: Wed, 8 Dec 2021 15:11:32 +0100
Subject: [PATCH] Huge commit: - split ros1 and ros2 packaging - unit-test on
ros1 noetic and melodic - unit-test on ros2 foxy
---
.github/workflows/ci_ubuntu18_04_ros1.yml | 36 +++
.github/workflows/ci_ubuntu20_04_ros1.yml | 36 +++
.github/workflows/ci_ubuntu20_04_ros2.yml | 36 +++
CMakeLists.txt | 2 +
ros1/CMakeLists.txt | 2 +
ros2/CMakeLists.txt | 19 +-
.../ros_python_interpreter_server.hpp | 10 +-
.../dynamic_graph_bridge/ros_sot_loader.hpp | 0
.../dynamic_graph_bridge/sot_loader.hh | 111 +++++++
.../dynamic_graph_bridge/sot_loader_basic.hh | 131 ++++++++
ros2/scripts/remote_python_client.py | 6 +-
ros2/scripts/run_command | 1 +
ros2/src/CMakeLists.txt | 182 ++++++++++--
ros2/src/dynamic_graph/ros/__init__.py | 2 +-
ros2/src/dynamic_graph/ros/dgcompleter.py | 4 +-
ros2/src/dynamic_graph/ros/ros.py | 6 +-
ros2/src/programs/geometric_simu.cpp | 11 +-
ros2/src/ros.cpp | 6 +-
ros2/src/ros_publish-python-module-py.cc | 12 +-
ros2/src/ros_publish.cpp | 3 +-
ros2/src/ros_publish.hpp | 2 +-
ros2/src/ros_publish.hxx | 2 +-
ros2/src/ros_python_interpreter_client.cpp | 4 +-
ros2/src/ros_sot_loader.cpp | 0
ros2/src/ros_subscribe-python-module-py.cc | 11 +-
ros2/src/ros_subscribe.cpp | 5 +-
ros2/src/ros_subscribe.hpp | 2 +-
ros2/src/ros_subscribe.hxx | 2 +-
ros2/src/sot_loader.cpp | 281 ++++++++++++++++++
ros2/src/sot_loader_basic.cpp | 249 ++++++++++++++++
ros2/tests/CMakeLists.txt | 77 +++--
.../impl_test_sot_external_interface.cpp | 77 +++++
.../tests/impl_test_sot_external_interface.hh | 39 +++
.../launching_test_sot_loader.cpython-38.pyc | Bin 0 -> 2062 bytes
...ching_test_sot_loader_basic.cpython-38.pyc | Bin 0 -> 2103 bytes
.../tests/launch/launching_test_sot_loader.py | 78 +++++
.../launch/launching_test_sot_loader_basic.py | 74 +++++
ros2/tests/main.cpp | 24 ++
ros2/tests/python_scripts/simple_add.py | 1 +
ros2/tests/python_scripts/simple_add_fail.py | 1 +
ros2/tests/test_common.hpp | 167 +++++++++++
ros2/tests/test_import.py | 27 ++
ros2/tests/test_ros_init.cpp | 191 ++++++++++++
ros2/tests/test_ros_interpreter.cpp | 272 +++++++++++++++++
ros2/tests/test_sot_loader.cpp | 93 ++++++
ros2/tests/test_sot_loader_basic.cpp | 184 ++++++++++++
ros2/tests/urdf/dgb_minimal_robot.urdf | 98 ++++++
47 files changed, 2466 insertions(+), 111 deletions(-)
create mode 100644 .github/workflows/ci_ubuntu18_04_ros1.yml
create mode 100644 .github/workflows/ci_ubuntu20_04_ros1.yml
create mode 100644 .github/workflows/ci_ubuntu20_04_ros2.yml
delete mode 100644 ros2/include/dynamic_graph_bridge/ros_sot_loader.hpp
create mode 100644 ros2/include/dynamic_graph_bridge/sot_loader.hh
create mode 100644 ros2/include/dynamic_graph_bridge/sot_loader_basic.hh
create mode 120000 ros2/scripts/run_command
delete mode 100644 ros2/src/ros_sot_loader.cpp
create mode 100644 ros2/src/sot_loader.cpp
create mode 100644 ros2/src/sot_loader_basic.cpp
create mode 100644 ros2/tests/impl_test_sot_external_interface.cpp
create mode 100644 ros2/tests/impl_test_sot_external_interface.hh
create mode 100644 ros2/tests/launch/__pycache__/launching_test_sot_loader.cpython-38.pyc
create mode 100644 ros2/tests/launch/__pycache__/launching_test_sot_loader_basic.cpython-38.pyc
create mode 100644 ros2/tests/launch/launching_test_sot_loader.py
create mode 100755 ros2/tests/launch/launching_test_sot_loader_basic.py
create mode 100644 ros2/tests/main.cpp
create mode 100644 ros2/tests/python_scripts/simple_add.py
create mode 100644 ros2/tests/python_scripts/simple_add_fail.py
create mode 100644 ros2/tests/test_common.hpp
create mode 100755 ros2/tests/test_import.py
create mode 100644 ros2/tests/test_ros_init.cpp
create mode 100644 ros2/tests/test_ros_interpreter.cpp
create mode 100644 ros2/tests/test_sot_loader.cpp
create mode 100644 ros2/tests/test_sot_loader_basic.cpp
create mode 100644 ros2/tests/urdf/dgb_minimal_robot.urdf
diff --git a/.github/workflows/ci_ubuntu18_04_ros1.yml b/.github/workflows/ci_ubuntu18_04_ros1.yml
new file mode 100644
index 0000000..d5740dd
--- /dev/null
+++ b/.github/workflows/ci_ubuntu18_04_ros1.yml
@@ -0,0 +1,36 @@
+name: Ubuntu18.04, ROS1 Continuous Integration
+
+on:
+ push:
+ branches:
+ - master
+ - devel
+ pull_request:
+ branches:
+ - master
+ - devel
+
+jobs:
+ build:
+ runs-on: ubuntu-18.04
+ steps:
+ #
+ # Setup the machines and build environment
+ #
+ - name: Install ROS.
+ uses: ros-tooling/setup-ros@v0.2
+ with:
+ required-ros-distributions: melodic
+
+ #
+ # Checkout the current branch
+ #
+ - uses: actions/checkout@v2
+
+ #
+ # Build and test the repo
+ #
+ - uses: ros-tooling/action-ros-ci@v0.2
+ with:
+ package-name: dynamic_graph_bridge_msgs
+ target-ros1-distro: melodic
diff --git a/.github/workflows/ci_ubuntu20_04_ros1.yml b/.github/workflows/ci_ubuntu20_04_ros1.yml
new file mode 100644
index 0000000..210afd4
--- /dev/null
+++ b/.github/workflows/ci_ubuntu20_04_ros1.yml
@@ -0,0 +1,36 @@
+name: Ubuntu20.04, ROS1 Continuous Integration
+
+on:
+ push:
+ branches:
+ - master
+ - devel
+ 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
+
+ #
+ # Build and test the repo
+ #
+ - uses: ros-tooling/action-ros-ci@v0.2
+ with:
+ package-name: dynamic_graph_bridge_msgs
+ target-ros1-distro: noetic
diff --git a/.github/workflows/ci_ubuntu20_04_ros2.yml b/.github/workflows/ci_ubuntu20_04_ros2.yml
new file mode 100644
index 0000000..237cf8c
--- /dev/null
+++ b/.github/workflows/ci_ubuntu20_04_ros2.yml
@@ -0,0 +1,36 @@
+name: Ubuntu20.04, ROS2 Continuous Integration
+
+on:
+ push:
+ branches:
+ - master
+ - devel
+ 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: foxy
+
+ #
+ # Checkout the current branch
+ #
+ - uses: actions/checkout@v2
+
+ #
+ # Build and test the repo
+ #
+ - uses: ros-tooling/action-ros-ci@v0.2
+ with:
+ package-name: dynamic_graph_bridge_msgs
+ target-ros2-distro: foxy
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ee63a29..5daa8b0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -34,6 +34,8 @@ 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()
diff --git a/ros1/CMakeLists.txt b/ros1/CMakeLists.txt
index 87041c6..a80c75f 100644
--- a/ros1/CMakeLists.txt
+++ b/ros1/CMakeLists.txt
@@ -74,3 +74,5 @@ install(PROGRAMS
DESTINATION share/${PROJECT_NAME}
)
+# we also need to install the header files
+install(DIRECTORY include/ DESTINATION include)
diff --git a/ros2/CMakeLists.txt b/ros2/CMakeLists.txt
index 6e0fc75..4e8f2ac 100644
--- a/ros2/CMakeLists.txt
+++ b/ros2/CMakeLists.txt
@@ -37,18 +37,11 @@ IF(BUILD_PYTHON_INTERFACE)
ENDIF(BUILD_PYTHON_INTERFACE)
add_subdirectory(src)
-# add_subdirectory(tests)
+add_subdirectory(tests)
#install ros executables
-install(PROGRAMS
- scripts/robot_pose_publisher
- scripts/run_command
- scripts/tf_publisher
- DESTINATION share/${PROJECT_NAME}
- )
-
-# Install package information
-install(FILES package.xml DESTINATION share/${PROJECT_NAME})
-
-#ROS 2 packaging
-ament_package()
+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/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp b/ros2/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
index 378665b..c9ba3dd 100644
--- a/ros2/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
+++ b/ros2/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
@@ -33,9 +33,9 @@ public:
*
* The first argument of "runCommandCallback"
* (const std::string & command) is bound to
- * (dynamic_graph_manager::srv::RunPythonCommand::Request).
+ * (dynamic_graph_bridge::srv::RunPythonCommand::Request).
* And the second argument (std::string &result) is bound to
- * (dynamic_graph_manager::srv::RunPythonCommand::Response)
+ * (dynamic_graph_bridge::srv::RunPythonCommand::Response)
*/
typedef std::function<void(RunPythonCommandRequestPtr,
RunPythonCommandResponsePtr)>
@@ -47,9 +47,9 @@ public:
*
* The first argument of "runPythonFileCallback"
* (std::string ifilename) is bound to
- * (dynamic_graph_manager::srv::RunPythonCommand::Request).
+ * (dynamic_graph_bridge::srv::RunPythonCommand::Request).
* And a fake second argument is simulated in
- * (dynamic_graph_manager::srv::RunPythonCommand::Response)
+ * (dynamic_graph_bridge::srv::RunPythonCommand::Response)
*/
typedef std::function<void(RunPythonFileRequestPtr,
RunPythonFileResponsePtr)>
@@ -86,7 +86,7 @@ public:
* @param ifilename is the path to the script to execute
*/
void run_python_file(const std::string ifilename,
- std::string& standarderror);
+ std::string& result);
/**
* @brief start_ros_service advertize the "run_python_command" and
diff --git a/ros2/include/dynamic_graph_bridge/ros_sot_loader.hpp b/ros2/include/dynamic_graph_bridge/ros_sot_loader.hpp
deleted file mode 100644
index e69de29..0000000
diff --git a/ros2/include/dynamic_graph_bridge/sot_loader.hh b/ros2/include/dynamic_graph_bridge/sot_loader.hh
new file mode 100644
index 0000000..5b4b320
--- /dev/null
+++ b/ros2/include/dynamic_graph_bridge/sot_loader.hh
@@ -0,0 +1,111 @@
+/*
+ * 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 <thread>
+
+// ROS includes
+//#include "ros/ros.h"
+#include "std_srvs/srv/empty.hpp"
+#include <sensor_msgs/msg/joint_state.hpp>
+#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> control_;
+ /// 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
+ std::shared_ptr<std::thread> thread_;
+
+ // \brief Start control loop
+ virtual void startControlLoop();
+
+ // Robot Pose Publisher
+ std::shared_ptr<tf2_ros::TransformBroadcaster> freeFlyerPublisher_;
+
+ // Free flyer pose
+ geometry_msgs::msg::TransformStamped freeFlyerPose_;
+
+ public:
+ SotLoader();
+ virtual ~SotLoader();
+
+ // \brief Create a thread for ROS and start the control loop.
+ void initializeServices();
+
+ // \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();
+
+ // \brief Method for the thread implementing the starting and stopping part of dynamic_graph
+ void workThreadLoader();
+
+ // \brief Join the thread.
+ void lthread_join();
+ typedef std::shared_ptr<SotLoader> SharedPtr;
+};
+
+#endif /* SOT_LOADER_HH_ */
diff --git a/ros2/include/dynamic_graph_bridge/sot_loader_basic.hh b/ros2/include/dynamic_graph_bridge/sot_loader_basic.hh
new file mode 100644
index 0000000..fd38405
--- /dev/null
+++ b/ros2/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -0,0 +1,131 @@
+/*
+ * 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 <rclcpp/rclcpp.hpp>
+#include "std_srvs/srv/empty.hpp"
+#include <sensor_msgs/msg/joint_state.hpp>
+
+// Sot Framework includes
+#include <sot/core/debug.hh>
+#include <sot/core/abstract-sot-external-interface.hh>
+#include "dynamic_graph_bridge/ros.hpp"
+
+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
+ rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service_start_;
+
+ /// Advertises stop_dynamic_graph services
+ rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service_stop_;
+
+ // Joint state publication.
+ rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
+
+ // Node reference
+ rclcpp::Node::SharedPtr nh_;
+
+ // Joint state to be published.
+ sensor_msgs::msg::JointState joint_state_;
+
+ // Number of DOFs according to KDL.
+ int nbOfJoints_;
+ parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
+
+ // Ordered list of joint names describing the robot state.
+ std::vector<std::string> stateVectorMap_;
+
+
+ public:
+ SotLoaderBasic();
+ virtual ~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 loadController();
+
+ // Initialize ROS Context
+ void initialize();
+
+ // Returns nodeHandle
+ rclcpp::Node::SharedPtr returnsNodeHandle();
+
+ /// \brief Unload the library which handles the robot device.
+ void CleanUp();
+
+ // \brief Create ROS services start_dg and stop_dg.
+ virtual void initializeServices();
+
+ // \brief Callback function when starting dynamic graph.
+ void start_dg(const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+ std::shared_ptr<std_srvs::srv::Empty::Response> response);
+
+ // \brief Callback function when stopping dynamic graph.
+ void stop_dg(const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+ std::shared_ptr<std_srvs::srv::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/ros2/scripts/remote_python_client.py b/ros2/scripts/remote_python_client.py
index d756d5f..9732f36 100644
--- a/ros2/scripts/remote_python_client.py
+++ b/ros2/scripts/remote_python_client.py
@@ -1,6 +1,6 @@
#!/usr/bin/env python
-"""@package dynamic_graph_manager
+"""@package dynamic_graph_bridge
@file
@license License BSD-3-Clause
@@ -23,8 +23,8 @@ import signal
import rclpy
# Used to connect to ROS services
-from dynamic_graph_manager.ros.dgcompleter import DGCompleter
-from dynamic_graph_manager_cpp_bindings import RosPythonInterpreterClient
+from dynamic_graph_bridge.ros.dgcompleter import DGCompleter
+from dynamic_graph_bridge_cpp_bindings import RosPythonInterpreterClient
def signal_handler(sig, frame):
diff --git a/ros2/scripts/run_command b/ros2/scripts/run_command
new file mode 120000
index 0000000..9feef56
--- /dev/null
+++ b/ros2/scripts/run_command
@@ -0,0 +1 @@
+remote_python_client.py
\ No newline at end of file
diff --git a/ros2/src/CMakeLists.txt b/ros2/src/CMakeLists.txt
index 93229f7..73d141e 100644
--- a/ros2/src/CMakeLists.txt
+++ b/ros2/src/CMakeLists.txt
@@ -3,6 +3,121 @@
# 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
@@ -51,38 +166,44 @@ endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
# Dynamic graph ros plugin.
set(plugins ros_publish ros_subscribe)
foreach(plugin ${plugins})
- get_filename_component(LIBRARY_NAME ${plugin} NAME)
- add_library(${LIBRARY_NAME} SHARED
+ 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(${LIBRARY_NAME} PROPERTIES SOVERSION
+ set_target_properties(${plugin_library_name} PROPERTIES SOVERSION
${PROJECT_VERSION})
endif(SUFFIX_SO_VERSION)
target_link_libraries(
- ${LIBRARY_NAME} ${${LIBRARY_NAME}_deps} ${catkin_LIBRARIES} ros_bridge
+ ${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 ${LIBRARY_NAME}
+ 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 ${LIBRARY_NAME})
+ 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}" ${LIBRARY_NAME}
+ 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")
- dynamic_graph_python_module(
- "ros/${PYTHON_LIBRARY_NAME}" ${LIBRARY_NAME}
+ 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)
@@ -92,23 +213,26 @@ if(BUILD_PYTHON_INTERFACE)
python_install_on_site("dynamic_graph/ros" "dgcompleter.py")
endif(BUILD_PYTHON_INTERFACE)
-# Stand alone embedded intepreter with a robot controller.
-# add_executable(geometric_simu
-# geometric_simu.cpp
-# sot_loader.cpp
-# sot_loader_basic.cpp)
-# target_link_libraries(
-# geometric_simu Boost::program_options ${CMAKE_DL_LIBS} ${catkin_LIBRARIES}
-# ros_bridge
-# dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp)
-# install(TARGETS geometric_simu DESTINATION lib/${PROJECT_NAME})
-
# Sot loader library
-# add_library(sot_loader sot_loader.cpp sot_loader_basic.cpp)
-# target_link_libraries(
-# sot_loader Boost::program_options ${catkin_LIBRARIES} ros_bridge
-# dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp)
-# install(
-# TARGETS sot_loader
-# EXPORT ${TARGETS_EXPORT_NAME}
-# DESTINATION lib)
+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/src/dynamic_graph/ros/__init__.py b/ros2/src/dynamic_graph/ros/__init__.py
index cc585ac..12ef9f7 100644
--- a/ros2/src/dynamic_graph/ros/__init__.py
+++ b/ros2/src/dynamic_graph/ros/__init__.py
@@ -1,4 +1,4 @@
-"""@package dynamic_graph_manager
+"""@package dynamic_graph_bridge
@file __init__.py
@author Maximilien Naveau (maximilien.naveau@gmail.com)
diff --git a/ros2/src/dynamic_graph/ros/dgcompleter.py b/ros2/src/dynamic_graph/ros/dgcompleter.py
index aa24433..7c40708 100644
--- a/ros2/src/dynamic_graph/ros/dgcompleter.py
+++ b/ros2/src/dynamic_graph/ros/dgcompleter.py
@@ -36,7 +36,7 @@ its input.
"""
import ast
-from dynamic_graph_manager_cpp_bindings import RosPythonInterpreterClient
+from dynamic_graph_bridge_cpp_bindings import RosPythonInterpreterClient
__all__ = ["DGCompleter"]
@@ -84,7 +84,7 @@ class DGCompleter:
# self.client.run_python_command("global local_completer")
cmd = 'local_completer.complete("' + text + '",' + str(state) + ")"
response = self.client.run_python_command(cmd)
- if not response or response is "\n" or response is None:
+ if not response or response == "\n" or response is None:
response = None
self.buffer = []
else:
diff --git a/ros2/src/dynamic_graph/ros/ros.py b/ros2/src/dynamic_graph/ros/ros.py
index 2bb925d..ca9c495 100644
--- a/ros2/src/dynamic_graph/ros/ros.py
+++ b/ros2/src/dynamic_graph/ros/ros.py
@@ -1,4 +1,4 @@
-"""@package dynamic_graph_manager
+"""@package dynamic_graph_bridge
@file ros.py
@author Maximilien Naveau (maximilien.naveau@gmail.com)
@@ -12,8 +12,8 @@ between ROS and the dynamic graph
"""
from dynamic_graph import plug
-from dynamic_graph_manager.dynamic_graph.ros_entities import RosPublish
-from dynamic_graph_manager.dynamic_graph.ros_entities import RosSubscribe
+from dynamic_graph_bridge.dynamic_graph.ros_entities import RosPublish
+from dynamic_graph_bridge.dynamic_graph.ros_entities import RosSubscribe
class Ros(object):
diff --git a/ros2/src/programs/geometric_simu.cpp b/ros2/src/programs/geometric_simu.cpp
index 8dbe445..0781323 100644
--- a/ros2/src/programs/geometric_simu.cpp
+++ b/ros2/src/programs/geometric_simu.cpp
@@ -12,7 +12,7 @@
#include <dynamic-graph/real-time-logger.h>
#include <dynamic_graph_bridge/sot_loader.hh>
-#include <dynamic_graph_bridge/ros2_init.hh>
+#include <dynamic_graph_bridge/ros.hpp>
int main(int argc, char *argv[]) {
::dynamicgraph::RealTimeLogger::instance()
@@ -20,23 +20,18 @@ int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
- dynamicgraph::RosContext::SharedPtr aRosContext =
- std::make_shared<dynamicgraph::RosContext>();
-
- aRosContext->rosInit();
-
SotLoader aSotLoader;
if (aSotLoader.parseOptions(argc, argv) < 0) return -1;
// Advertize service "(start|stop)_dynamic_graph" and
// load parameter "robot_description in SoT.
- aSotLoader.initializeFromRosContext(aRosContext);
+ aSotLoader.initialize();
// Load dynamic library and run python prologue.
aSotLoader.initPublication();
aSotLoader.initializeServices();
- aRosContext->mtExecutor->spin();
+ dynamic_graph_bridge::ros_spin();
return 0;
}
diff --git a/ros2/src/ros.cpp b/ros2/src/ros.cpp
index b765654..4917f4d 100644
--- a/ros2/src/ros.cpp
+++ b/ros2/src/ros.cpp
@@ -269,7 +269,9 @@ RosNodePtr get_ros_node(std::string node_name)
{
/** RosNode instanciation */
GLOBAL_LIST_OF_ROS_NODE[node_name] =
- std::make_shared<RosNode>(node_name, "dynamic_graph_manager");
+ std::make_shared<RosNode>(
+ node_name, "dynamic_graph_bridge");
+
}
/** Return a reference to the node handle so any function can use it */
return GLOBAL_LIST_OF_ROS_NODE[node_name];
@@ -311,7 +313,7 @@ void ros_clean()
bool ros_ok()
{
- return rclcpp::ok() && rclcpp::is_initialized();
+ return rclcpp::ok();
}
void ros_spin()
diff --git a/ros2/src/ros_publish-python-module-py.cc b/ros2/src/ros_publish-python-module-py.cc
index 5134a78..831462b 100644
--- a/ros2/src/ros_publish-python-module-py.cc
+++ b/ros2/src/ros_publish-python-module-py.cc
@@ -1,19 +1,21 @@
#include <dynamic-graph/python/module.hh>
-#include "ros_publish.hh"
+#include "ros_publish.hpp"
+namespace dgb = dynamic_graph_bridge;
namespace dg = dynamicgraph;
+
BOOST_PYTHON_MODULE(wrap)
{
bp::import("dynamic_graph");
- dg::python::exposeEntity<dg::RosPublish, bp::bases<dg::Entity>,
+ dg::python::exposeEntity<dgb::RosPublish, bp::bases<dg::Entity>,
dg::python::AddCommands>()
- .def("clear", &dg::RosPublish::clear,
+ .def("clear", &dgb::RosPublish::clear,
"Remove all signals writing data to a ROS topic")
- .def("rm", &dg::RosPublish::rm,
+ .def("rm", &dgb::RosPublish::rm,
"Remove a signal writing data to a ROS topic",
bp::args("signal_name"))
- .def("list", &dg::RosPublish::list,
+ .def("list", &dgb::RosPublish::list,
"List signals writing data to a ROS topic");
}
diff --git a/ros2/src/ros_publish.cpp b/ros2/src/ros_publish.cpp
index 8bbd459..9ed9b7f 100644
--- a/ros2/src/ros_publish.cpp
+++ b/ros2/src/ros_publish.cpp
@@ -7,9 +7,8 @@
* @brief Implements the RosPublish class.
*/
-#include "dynamic_graph_bridge/ros_entities/ros_publish.hpp"
-
#include "dynamic_graph_bridge/ros.hpp"
+#include "ros_publish.hpp"
namespace dynamic_graph_bridge
{
diff --git a/ros2/src/ros_publish.hpp b/ros2/src/ros_publish.hpp
index edb4f6c..68b89ef 100644
--- a/ros2/src/ros_publish.hpp
+++ b/ros2/src/ros_publish.hpp
@@ -18,7 +18,7 @@
#include <map>
#include <mutex>
-#include "dynamic_graph_bridge/ros_entities/dg_ros_mapping.hpp"
+#include "dg_ros_mapping.hpp"
namespace dynamic_graph_bridge
{
diff --git a/ros2/src/ros_publish.hxx b/ros2/src/ros_publish.hxx
index 8e1422c..9882cbe 100644
--- a/ros2/src/ros_publish.hxx
+++ b/ros2/src/ros_publish.hxx
@@ -9,7 +9,7 @@
#pragma once
-#include "dynamic_graph_bridge/ros_entities/dg_ros_mapping.hpp"
+#include "dg_ros_mapping.hpp"
namespace dynamic_graph_bridge
{
diff --git a/ros2/src/ros_python_interpreter_client.cpp b/ros2/src/ros_python_interpreter_client.cpp
index 4870016..dd73256 100644
--- a/ros2/src/ros_python_interpreter_client.cpp
+++ b/ros2/src/ros_python_interpreter_client.cpp
@@ -22,13 +22,13 @@ RosPythonInterpreterClient::RosPythonInterpreterClient()
// Create a client for the single python command service of the
// DynamicGraphManager.
- run_command_service_name_ = "/dynamic_graph_manager/run_python_command";
+ run_command_service_name_ = "/dynamic_graph_bridge/run_python_command";
run_command_request_ = std::make_shared<RunPythonCommandSrvType::Request>();
connect_to_rosservice_run_python_command();
// Create a client for the python file reading service of the
// DynamicGraphManager.
- run_script_service_name_ = "/dynamic_graph_manager/run_python_file";
+ run_script_service_name_ = "/dynamic_graph_bridge/run_python_file";
run_file_request_ = std::make_shared<RunPythonFileSrvType::Request>();
connect_to_rosservice_run_python_script();
timeout_connection_s_ = DurationSec(1);
diff --git a/ros2/src/ros_sot_loader.cpp b/ros2/src/ros_sot_loader.cpp
deleted file mode 100644
index e69de29..0000000
diff --git a/ros2/src/ros_subscribe-python-module-py.cc b/ros2/src/ros_subscribe-python-module-py.cc
index dd02bf5..338cf5a 100644
--- a/ros2/src/ros_subscribe-python-module-py.cc
+++ b/ros2/src/ros_subscribe-python-module-py.cc
@@ -1,19 +1,20 @@
#include <dynamic-graph/python/module.hh>
-#include "ros_subscribe.hh"
+#include "ros_subscribe.hpp"
+namespace dgb = dynamic_graph_bridge;
namespace dg = dynamicgraph;
BOOST_PYTHON_MODULE(wrap)
{
bp::import("dynamic_graph");
- dg::python::exposeEntity<dg::RosSubscribe, bp::bases<dg::Entity>,
+ dg::python::exposeEntity<dgb::RosSubscribe, bp::bases<dg::Entity>,
dg::python::AddCommands>()
- .def("clear", &dg::RosSubscribe::clear,
+ .def("clear", &dgb::RosSubscribe::clear,
"Remove all signals reading data from a ROS topic")
- .def("rm", &dg::RosSubscribe::rm,
+ .def("rm", &dgb::RosSubscribe::rm,
"Remove a signal reading data from a ROS topic",
bp::args("signal_name"))
- .def("list", &dg::RosSubscribe::list,
+ .def("list", &dgb::RosSubscribe::list,
"List signals reading data from a ROS topic");
}
diff --git a/ros2/src/ros_subscribe.cpp b/ros2/src/ros_subscribe.cpp
index bf02200..9d65b3d 100644
--- a/ros2/src/ros_subscribe.cpp
+++ b/ros2/src/ros_subscribe.cpp
@@ -7,11 +7,8 @@
* @date 2019-05-22
*/
-#include "dynamic_graph_bridge/ros_entities/ros_subscribe.hpp"
-
#include <dynamic-graph/factory.h>
-
-#include "dynamic_graph_bridge/dynamic_graph_manager.hpp"
+#include "ros_subscribe.hpp"
namespace dynamic_graph_bridge
{
diff --git a/ros2/src/ros_subscribe.hpp b/ros2/src/ros_subscribe.hpp
index a4e40c0..5f361be 100644
--- a/ros2/src/ros_subscribe.hpp
+++ b/ros2/src/ros_subscribe.hpp
@@ -18,7 +18,7 @@
#include <map>
#include <mutex>
-#include "dynamic_graph_bridge/ros_entities/dg_ros_mapping.hpp"
+#include "dg_ros_mapping.hpp"
namespace dynamic_graph_bridge
{
diff --git a/ros2/src/ros_subscribe.hxx b/ros2/src/ros_subscribe.hxx
index ccde69d..c3a4b41 100644
--- a/ros2/src/ros_subscribe.hxx
+++ b/ros2/src/ros_subscribe.hxx
@@ -11,7 +11,7 @@
#include <message_filters/message_traits.h>
-#include "dynamic_graph_bridge/ros_entities/dg_ros_mapping.hpp"
+#include "dg_ros_mapping.hpp"
namespace dynamic_graph_bridge
{
diff --git a/ros2/src/sot_loader.cpp b/ros2/src/sot_loader.cpp
new file mode 100644
index 0000000..66511e4
--- /dev/null
+++ b/ros2/src/sot_loader.cpp
@@ -0,0 +1,281 @@
+/*
+ * Copyright 2011,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#include <dynamic_graph_bridge/sot_loader.hh>
+#include "dynamic_graph_bridge/ros.hpp"
+#include <std_msgs/msg/string.hpp>
+
+// 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();
+ }
+};
+
+
+SotLoader::SotLoader()
+ : SotLoaderBasic(),
+ sensorsIn_(),
+ controlValues_(),
+ angleEncoder_(),
+ control_(),
+ forces_(),
+ torques_(),
+ baseAtt_(),
+ accelerometer_(3),
+ gyrometer_(3) {
+}
+
+
+SotLoader::~SotLoader() {
+ dynamic_graph_stopped_ = true;
+ lthread_join();
+}
+
+void SotLoader::startControlLoop() {
+ thread_ = std::make_shared<std::thread>(&SotLoader::workThreadLoader,this);
+}
+
+void SotLoader::initializeServices() {
+ SotLoaderBasic::initializeServices();
+
+ freeFlyerPose_.header.frame_id = "odom";
+ freeFlyerPose_.child_frame_id = "base_link";
+
+ if (nh_==0) {
+ logic_error aLogicError("SotLoaderBasic::initializeFromRosContext aRosCtxt is empty !");
+ throw aLogicError;
+ }
+
+ if (not nh_->has_parameter("tf_base_link"))
+ nh_->declare_parameter("tf_base_link",std::string("base_link"));
+
+ if (nh_->get_parameter("tf_base_link",
+ freeFlyerPose_.child_frame_id)) {
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("dynamic_graph_bridge"),
+ "Publishing " << freeFlyerPose_.child_frame_id <<
+ " wrt " << freeFlyerPose_.header.frame_id);
+ }
+
+ // Temporary fix. TODO: where should nbOfJoints_ be initialized from?
+
+ angleEncoder_.resize(static_cast<std::size_t>(nbOfJoints_));
+ control_.resize(static_cast<std::size_t>(nbOfJoints_));
+
+ // Creates a publisher for the free flyer transform.
+ freeFlyerPublisher_ = std::make_shared<tf2_ros::TransformBroadcaster>
+ (nh_);
+}
+
+void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
+ // Update joint values.w
+ assert(control_.size() == angleEncoder_.size());
+
+ sensorsIn["joints"].setName("angle");
+ for (unsigned int i = 0; i < control_.size(); i++) angleEncoder_[i] = control_[i];
+ sensorsIn["joints"].setValues(angleEncoder_);
+}
+
+void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
+ // Update joint values.
+ std::map<std::string, dgs::ControlValues>::iterator it_ctrl_map;
+ it_ctrl_map = controlValues.find("control");
+ if (it_ctrl_map == controlValues.end())
+ {
+ invalid_argument anInvalidArgument("control is not present in controlValues !");
+ throw anInvalidArgument;
+ }
+ control_ = controlValues["control"].getValues();
+
+#ifdef NDEBUG
+ // 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;
+#endif
+ // Check if the size if coherent with the robot description.
+ if (control_.size() != (unsigned int)nbOfJoints_) {
+ nbOfJoints_ = control_.size();
+ angleEncoder_.resize(nbOfJoints_);
+ }
+
+ // Publish the data.
+ std::vector<string> joint_state_names;
+ joint_state_.name = joint_state_names;
+ joint_state_.header.stamp = rclcpp::Clock().now();
+
+ if (joint_state_.position.size()!=nbOfJoints_+parallel_joints_to_state_vector_.size())
+ {
+ joint_state_.position.resize(nbOfJoints_+parallel_joints_to_state_vector_.size());
+ joint_state_.velocity.resize(nbOfJoints_+parallel_joints_to_state_vector_.size());
+ joint_state_.effort.resize(nbOfJoints_+parallel_joints_to_state_vector_.size());
+ }
+
+ for (int i = 0; i < nbOfJoints_; i++) {
+ joint_state_.position[i] = angleEncoder_[i];
+ }
+
+ std::cerr << "parallel_joints_to_state_vector_.size(): "
+ << parallel_joints_to_state_vector_.size()
+ << std::endl;
+
+ for (unsigned int i = 0; i < parallel_joints_to_state_vector_.size(); i++) {
+ joint_state_.position[i + nbOfJoints_] =
+ coefficient_parallel_joints_[i] * angleEncoder_[parallel_joints_to_state_vector_[i]];
+ }
+
+ joint_pub_->publish(joint_state_);
+
+ // Get the estimation of the robot base.
+ it_ctrl_map = controlValues.find("baseff");
+ if (it_ctrl_map == controlValues.end())
+ {
+ invalid_argument anInvalidArgument("baseff is not present in controlValues !");
+ throw anInvalidArgument;
+ }
+
+ std::cerr << "Reached poseValue_" << std::endl;
+ 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_);
+ std::cerr << "end of readControl" << std::endl;
+}
+
+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_);
+}
+
+void SotLoader::lthread_join() {
+ if (thread_!=0)
+ if (thread_->joinable())
+ thread_->join();
+}
+
+void SotLoader::workThreadLoader() {
+ double periodd;
+
+ /// Declare parameters
+ if (not nh_->has_parameter("dt"))
+ nh_->declare_parameter<double>("dt",0.01);
+ if (not nh_->has_parameter("paused"))
+ nh_->declare_parameter<bool>("paused",false);
+
+ //
+ nh_->get_parameter_or("dt",periodd,0.001);
+ rclcpp::Rate rate(1/periodd); // 1 kHz
+
+ DataToLog dataToLog(5000);
+
+ rate.reset();
+ while (rclcpp::ok() && isDynamicGraphStopped()) {
+ rate.sleep();
+ }
+
+ bool paused;
+ rclcpp::Clock aClock;
+ rclcpp::Time timeOrigin = aClock.now();
+ rclcpp::Time time;
+ while (rclcpp::ok() && !isDynamicGraphStopped()) {
+
+ // Check if the user did not paused geometric_simu
+ nh_->get_parameter_or("paused", paused, false);
+
+ if (!paused) {
+ time = aClock.now();
+ oneIteration();
+
+ rclcpp::Duration d = aClock.now() - time;
+ dataToLog.record((time - timeOrigin).nanoseconds()*1.0e9, d.nanoseconds()*1.0e9);
+ }
+ rate.sleep();
+ }
+ dataToLog.save("/tmp/geometric_simu");
+ std::cerr << "End of this thread: "
+ << std::this_thread::get_id()
+ << std::endl;
+
+}
diff --git a/ros2/src/sot_loader_basic.cpp b/ros2/src/sot_loader_basic.cpp
new file mode 100644
index 0000000..6941472
--- /dev/null
+++ b/ros2/src/sot_loader_basic.cpp
@@ -0,0 +1,249 @@
+/*
+ * Copyright 2016,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#include "dynamic_graph_bridge/sot_loader_basic.hh"
+#include "dynamic_graph_bridge/ros_parameter.hpp"
+
+#include <dynamic-graph/pool.h>
+
+#include <exception>
+
+// 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) {
+ nh_ = dynamic_graph_bridge::get_ros_node("SotLoaderBasic");
+}
+
+SotLoaderBasic::~SotLoaderBasic()
+{
+ dynamic_graph_bridge::ros_shutdown("SotLoaderBasic");
+}
+
+void SotLoaderBasic::initialize()
+{
+}
+
+rclcpp::Node::SharedPtr SotLoaderBasic::returnsNodeHandle() {
+ return nh_;
+}
+int SotLoaderBasic::initPublication() {
+
+ // Prepare message to be published
+ joint_pub_ = nh_->create_publisher<sensor_msgs::msg::JointState>("joint_states", 1);
+
+ return 0;
+}
+
+void SotLoaderBasic::initializeServices() {
+ RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+ "Ready to start dynamic graph.");
+
+ using namespace std::placeholders;
+ service_start_ = nh_->create_service<std_srvs::srv::Empty>("start_dynamic_graph",
+ std::bind(&SotLoaderBasic::start_dg,
+ this,std::placeholders::_1,
+ std::placeholders::_2));
+
+ service_stop_ = nh_->create_service<std_srvs::srv::Empty>("stop_dynamic_graph",
+ std::bind(&SotLoaderBasic::stop_dg,
+ this,std::placeholders::_1,
+ std::placeholders::_2));
+
+ dynamicgraph::parameter_server_read_robot_description(nh_);
+}
+
+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;
+
+ if (!nh_) {
+ throw std::logic_error("SotLoaderBasic::readSotVectorStateParam() nh_ not initialized");
+ }
+
+ // It is necessary to declare parameters first
+ // before trying to access them.
+ if (not nh_->has_parameter("state_vector_map"))
+ nh_->declare_parameter("state_vector_map",std::vector<std::string>{});
+ if (not nh_->has_parameter("joint_state_parallel"))
+ nh_->declare_parameter("joint_state_parallel",std::vector<std::string>{});
+
+ // Read the state vector of the robot
+ // Defines the order in which the actuators are ordered
+ try {
+ std::string aParameterName("state_vector_map");
+ if (!nh_->get_parameter(aParameterName,stateVectorMap_)) {
+ logic_error aLogicError(
+ "SotLoaderBasic::readSotVectorStateParam : State_vector_map is empty");
+ throw aLogicError;
+ }
+ RCLCPP_INFO(nh_->get_logger(), "state_vector_map parameter size %d",
+ stateVectorMap_.size());
+ }
+ catch (exception &e)
+ {
+ std::throw_with_nested(std::logic_error(
+ "Unable to call nh_->get_parameter"));
+ }
+
+ nbOfJoints_ = static_cast<int>(stateVectorMap_.size());
+ nbOfParallelJoints_ = 0;
+
+ // Read the parallel joints.
+ // Specify the constraint between the joints
+ // Currently acts as a mimic or a an inverse mimic joint.
+
+ std::string prefix("joint_state_parallel");
+ std::map<std::string,rclcpp::Parameter> joint_state_parallel;
+ nh_->get_parameters(prefix,joint_state_parallel);
+
+ // Iterates over the map joint_state_parallel
+ for ( std::map<std::string,rclcpp::Parameter>::iterator
+ it_map_expression = joint_state_parallel.begin();
+ it_map_expression != joint_state_parallel.end();
+ ++it_map_expression)
+ {
+ std::string key = it_map_expression->first;
+ std::string map_expression = it_map_expression->second.as_string();
+ std::string final_expression;
+ 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 << key << ": " << final_coefficient << std::endl;
+ from_parallel_name_to_state_vector_name[key] = 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(static_cast<unsigned long>(nbOfJoints_) + nbOfParallelJoints_);
+ joint_state_.position.resize(static_cast<unsigned long>(nbOfJoints_) + nbOfParallelJoints_);
+
+ // Fill in the name of the joints from the state vector.
+ // and build local map from state name to state vector
+ for (std::size_t i = 0; i < stateVectorMap_.size(); ++i) {
+ joint_state_.name[i] = stateVectorMap_[i];
+
+ from_state_name_to_state_vector[joint_state_.name[i]] = static_cast<int>(i);
+ }
+
+ // Fill in the name of the joints from the parallel joint vector.
+ // and build map from parallel name to state vector
+ std::size_t 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 + static_cast<std::size_t>(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");
+
+ vm_.clear();
+
+ 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::loadController() {
+ // 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
+ << " from " << dynamicLibraryName_
+ << '\n';
+ return;
+ }
+
+ // Create robot-controller
+ sotController_ = createSot();
+ cout << "Went out from loadController." << 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.
+ try {
+ if (sotRobotControllerLibrary_ != 0)
+ dlclose(sotRobotControllerLibrary_);
+ }
+ catch(exception &e) {
+ std::throw_with_nested(std::logic_error("SotLoaderBasic::CleanUp() Unable to call dlclose"));
+ }
+}
+
+void SotLoaderBasic::start_dg(const std::shared_ptr<std_srvs::srv::Empty::Request>,
+ std::shared_ptr<std_srvs::srv::Empty::Response>) {
+ dynamic_graph_stopped_ = false;
+}
+
+void SotLoaderBasic::stop_dg(const std::shared_ptr<std_srvs::srv::Empty::Request>,
+ std::shared_ptr<std_srvs::srv::Empty::Response>) {
+ dynamic_graph_stopped_ = true;
+}
diff --git a/ros2/tests/CMakeLists.txt b/ros2/tests/CMakeLists.txt
index 82f89d5..ca26786 100644
--- a/ros2/tests/CMakeLists.txt
+++ b/ros2/tests/CMakeLists.txt
@@ -12,41 +12,72 @@ if(BUILD_TESTING)
# Library for sot_external_interface
add_library(impl_test_sot_external_interface SHARED
- tests/impl_test_sot_external_interface)
+ impl_test_sot_external_interface)
- target_link_libraries(impl_test_sot_external_interface PUBLIC sot-core::sot-core)
+ target_link_libraries(impl_test_sot_external_interface
+ PUBLIC sot-core::sot-core)
# Executable for SotLoaderBasic test
- add_executable(test_sot_loader_basic
- tests/test_sot_loader_basic.cpp
- )
- target_include_directories(test_sot_loader_basic PUBLIC include "${GMOCK_INCLUDE_DIRS}")
- target_link_libraries(test_sot_loader_basic sot_loader "${GMOCK_LIBRARIES}" )
+ add_executable(test_sot_loader_basic test_sot_loader_basic.cpp)
+ target_include_directories(
+ test_sot_loader_basic
+ PUBLIC include "${GMOCK_INCLUDE_DIRS}"
+ PRIVATE ${PROJECT_SOURCE_DIR}/ros2/include)
+ target_link_libraries(test_sot_loader_basic sot_loader "${GMOCK_LIBRARIES}")
- add_launch_test(tests/launching_test_sot_loader_basic.py)
+ add_launch_test(launch/launching_test_sot_loader_basic.py)
# Test for class SotLoader
- add_executable(test_sot_loader
- tests/test_sot_loader.cpp
- )
- target_include_directories(test_sot_loader PUBLIC include "${GMOCK_INCLUDE_DIRS}")
+ add_executable(test_sot_loader test_sot_loader.cpp)
+ target_include_directories(
+ test_sot_loader
+ PUBLIC include "${GMOCK_INCLUDE_DIRS}"
+ PRIVATE ${PROJECT_SOURCE_DIR}/ros2/include)
target_link_libraries(test_sot_loader sot_loader "${GMOCK_LIBRARIES}")
- add_launch_test(tests/launching_test_sot_loader.py)
+ add_launch_test(launch/launching_test_sot_loader.py)
# Install tests
- install(TARGETS test_sot_loader_basic
- test_sot_loader
- DESTINATION lib/${PROJECT_NAME}
- )
+ install(TARGETS test_sot_loader_basic test_sot_loader
+ DESTINATION lib/${PROJECT_NAME})
# Install library for tests
- install(TARGETS impl_test_sot_external_interface
- DESTINATION lib
- )
+ install(TARGETS impl_test_sot_external_interface DESTINATION lib)
+
+ install(DIRECTORY launch urdf DESTINATION share/${PROJECT_NAME})
+
+ #
+ # this macro define a unit tests using gtest
+ #
+ macro(create_ros_bridge_unittest test_name dependencies)
+ # Set a general config folder path for all tests
+ set(TEST_CONFIG_PATH ${PROJECT_SOURCE_DIR}/tests/config/)
+
+ # Create the cmake target using ament and gtest.
+ ament_add_gtest(test_${test_name} main.cpp test_${test_name}.cpp)
+
+ # Include dependencies.
+ target_include_directories(
+ test_${test_name}
+ PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+ $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/demos>
+ $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/tests>
+ $<INSTALL_INTERFACE:include>
+ SYSTEM
+ PUBLIC ${PYTHON_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}
+ PRIVATE ${PROJECT_SOURCE_DIR}/ros2/include)
+
+ # Link the dependecies to it.
+ target_link_libraries(test_${test_name} ros_bridge)
- install(DIRECTORY launch urdf
- DESTINATION share/${PROJECT_NAME}
- )
+ # add some preprocessor variable
+ target_compile_definitions(
+ test_${test_name}
+ PUBLIC TEST_CONFIG_PATH="${CMAKE_CURRENT_LIST_DIR}/python_scripts/")
+ endmacro(create_ros_bridge_unittest test_name)
+ # C++ unit-tests.
+ find_package(ament_cmake_gtest)
+ create_ros_bridge_unittest(ros_init dynamic_graph_bridge)
+ create_ros_bridge_unittest(ros_interpreter dynamic_graph_bridge)
endif()
diff --git a/ros2/tests/impl_test_sot_external_interface.cpp b/ros2/tests/impl_test_sot_external_interface.cpp
new file mode 100644
index 0000000..0fae4d8
--- /dev/null
+++ b/ros2/tests/impl_test_sot_external_interface.cpp
@@ -0,0 +1,77 @@
+#include "impl_test_sot_external_interface.hh"
+
+ImplTestSotExternalInterface::ImplTestSotExternalInterface()
+{
+ std::vector<double> ctrl_vector;
+ ctrl_vector.resize(2);
+
+ // Dummy values for the control vector
+ ctrl_vector[0] = 0.0;
+ ctrl_vector[1] = 0.0;
+
+ // Creates the named control vector
+ named_ctrl_vec_.setName("control");
+ named_ctrl_vec_.setValues(ctrl_vector);
+
+ // Creates the base eff vector
+ named_base_ff_vec_.setName("baseff");
+
+ ctrl_vector.resize(7);
+ for( auto i=0;i<6;i++)
+ ctrl_vector[i] = 0.0;
+ ctrl_vector[6] = 0.0;
+ named_base_ff_vec_.setValues(ctrl_vector);
+}
+
+ImplTestSotExternalInterface::~ImplTestSotExternalInterface()
+{}
+
+void ImplTestSotExternalInterface::setupSetSensors
+(std::map<std::string,
+ dynamicgraph::sot::SensorValues> &)
+{
+ std::cout << "ImplTestSotExternalInterface::setupSetSensors" << std::endl;
+}
+
+void ImplTestSotExternalInterface::nominalSetSensors
+(std::map<std::string,
+ dynamicgraph::sot::SensorValues> &)
+{
+ std::cout << "ImplTestSotExternalInterface::nominalSetSensors" << std::endl;
+}
+
+void ImplTestSotExternalInterface::cleanupSetSensors
+(std::map<std::string,
+ dynamicgraph::sot::SensorValues> &)
+{
+ std::cout << "ImplTestSotExternalInterface::cleanupSetSensors" << std::endl;
+}
+
+void ImplTestSotExternalInterface::getControl
+(std::map<std::string,
+ dynamicgraph::sot::ControlValues> & controlValues)
+{
+ // Put the default named_ctrl_vec inside the map controlValues.
+ controlValues["control"] = named_ctrl_vec_;
+ controlValues["baseff"] = named_base_ff_vec_;
+}
+void ImplTestSotExternalInterface::setSecondOrderIntegration
+(void)
+{
+ std::cout << "ImplTestSotExternalInterface::setSecondOrderIntegration" << std::endl;
+}
+
+void ImplTestSotExternalInterface::setNoIntegration
+(void)
+{
+ std::cout << "setNoIntegration" << std::endl;
+}
+
+extern "C" {
+ dynamicgraph::sot::AbstractSotExternalInterface *createSotExternalInterface()
+ { return new ImplTestSotExternalInterface; }
+}
+
+extern "C" {
+ void destroySotExternalInterface(dynamicgraph::sot::AbstractSotExternalInterface *p) { delete p; }
+}
diff --git a/ros2/tests/impl_test_sot_external_interface.hh b/ros2/tests/impl_test_sot_external_interface.hh
new file mode 100644
index 0000000..bfca53b
--- /dev/null
+++ b/ros2/tests/impl_test_sot_external_interface.hh
@@ -0,0 +1,39 @@
+#ifndef _DGB_IMPL_TEST_SOT_EXTERNAL_INTEFACE_HH_
+#define _DGB_IMPL_TEST_SOT_EXTERNAL_INTEFACE_HH_
+
+#include <iostream>
+#include <sot/core/abstract-sot-external-interface.hh>
+
+
+class ImplTestSotExternalInterface : public
+dynamicgraph::sot::AbstractSotExternalInterface {
+public:
+ ImplTestSotExternalInterface();
+ virtual ~ImplTestSotExternalInterface();
+
+ virtual void setupSetSensors(std::map<std::string,
+ dynamicgraph::sot::SensorValues> &) final;
+
+ virtual void nominalSetSensors(std::map<std::string,
+ dynamicgraph::sot::SensorValues> &) final;
+
+ virtual void cleanupSetSensors(std::map<std::string,
+ dynamicgraph::sot::SensorValues> &) final;
+ virtual void getControl(std::map<std::string,
+ dynamicgraph::sot::ControlValues> &) final;
+
+ virtual void setSecondOrderIntegration(void);
+
+ virtual void setNoIntegration(void);
+
+protected:
+ // Named ctrl vector
+ dynamicgraph::sot::ControlValues named_ctrl_vec_;
+
+ // Named base free flyer vector
+ dynamicgraph::sot::ControlValues named_base_ff_vec_;
+
+};
+
+
+#endif
diff --git a/ros2/tests/launch/__pycache__/launching_test_sot_loader.cpython-38.pyc b/ros2/tests/launch/__pycache__/launching_test_sot_loader.cpython-38.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..38a7e4a70f12f5d39995c8bd4a099fedc28c2e57
GIT binary patch
literal 2062
zcmaJ?OOG5i5VqYtJ<okjHVGjTganZ0vLkXvD6j+&iI#{BLh_~MZF^^W(vQJ*Z!*#B
zX>-j#;4pLK#xcLeCnQe!3q(<<a?d84Y``s-%dc|PSC9T>ZLLF~{q^im!P|uVjgzyj
z0h3$M%`+g9NSYB|kCanCXPo&l;DH}Q9{RDd8Y3S0dDD+9Yvpa;rYIxG!o0&fo)5Eb
z-s3&bH?p;Sov(YIW*dA%ZA>;NTQZW(C**|k{`W+-Wcvw`ZN(-RP6EEIqQ%7}f%T5;
z!g{w}hqIR?-6w<IbI_jb4XE3`C+zsgGA;J217lNL*|aQRee15Mie&t)GKo&7tHp4y
zlnOSlj+Bk3B6%Q2DmG)GRV-7j5?kt7f|5@gqSb`H3*8hzEIA>^#8S&7J)x4xU=*Ct
zV`_srT>wr?SY0%h<e14&Hb!jDWb`{b4s0Z;Y@(I(U_@o-Y2!Gw%>|i<wl!%lI!gj$
zchOstQLv<T?SvdMu(P|zx8<x5d78u{EvDo6K&Nt~cvxw9`1q0>9mIKBq`AmqT^^Kn
z2iJKt0YK~*tMTT5@l9ieRq>IxG0w%5Z~61Fd;<{Qu*%rjls3*vA(eI!fTvV(f6#Ev
z6<n^P9;u|V;viElDl0p!tm~@0%22+wGuN34Epla*HiLB?0BD*aK`dQZPE`TIls+tV
z?n14E)FJLfnRl35ug4g5NsEyS!K0a5yQ9?f>nuG|_jH-Krby7G#&v%N0kiw%eK-vw
z%0y&l(BLeS{G)RQ#z|RNRiNwbm|ywNeHVEXjJp6v0K5VfiKp<eGUgAWKLCCIQNwYW
zt6^S<BPFV#R7WZsJ}mVE14yZ%(aG?Ao*U}Y+#I5DW>{Og8t{pC@ZXuv+?A0kl=j%7
zbI!Qy254b!0wFA7j75}D_L98{n|<V8MFGrLU-%+whrqXJ>tz^*FZ73S>#acdVu0L&
zZnl9ag5>~80R;II0;M2;a131^f!;6eT^}Id2$LoWy7;sqkM7)sZu&sxY(nM~r95R=
z06<Ex_Z(}~fI%G8n7XD-a|OW!xp`3-qqO~1r1rZ~?~f}h%ZJ4vaG_Dzp}q_^T^sMk
zX#p{WzNpC++}ZRHzIIz#+=f+y@ki+68|RhZdH4Jt7}<(rNI4b9t`o<3DXR?mUL5~c
ziLBny*FoZ$$GsY;fxZgFTaS;nn4c%WzSVH=7WDsvdJDD1XosNt$>&*u*G?Pqh_-)g
z6BtV-biw9yNmn@2y+NRH2hzo3YTQQ6+<uhWT_}ZS!1OgZpg%$K4wUO28U;roZ=|{d
zM`|F0$JfrOR^#~kFrcV!pv+H^e1>Fz<joUr0Q(#^et_<Uw*rmx!DYuv;{r1?u2mJO
zMO}CwoC4XK7E(Qmr!zY)3%voS`Ko5@tg3=HnqPXmkeqJv?RtI3ocUpxx<nzKyuZ2!
ziF$g+S15k<{PR1js%Yx!7J*Wrs0(wUAGq!r1$@T27UH%mAign5Tz^_(y<LU4&wg+H
n`(SddFY8GB3N1F*fj~CX7K0Bd@IBU}@Ud19(ay*HMnwMw_>DID
literal 0
HcmV?d00001
diff --git a/ros2/tests/launch/__pycache__/launching_test_sot_loader_basic.cpython-38.pyc b/ros2/tests/launch/__pycache__/launching_test_sot_loader_basic.cpython-38.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..14e3f9104e814af03429f3496e89f183a7955e00
GIT binary patch
literal 2103
zcmbVN&2Jnv6t_LIv$LN`wrQmjsE`VUl~7jn!~vm}P%1$cDpFKwE+g;Q*`3XNEVegI
zs?CMu+W%qqh{S;#|BJ7jdgg*aw7ln?wCM*IJo5ANv;F-1dmr;dr_&_R{`l=ze$XJ~
zcbr^oHJIFju73d{h@ctCX+$aVEMpOKqZfH@^sBKJ)!aOAqnFk5FbdrnKlAcN)Np()
zYv!$}<@g|L=bfli>Fq{c*`2IS)<s=}XXKPd8+VClh~^m)P01#`Q!lzCYm3b#0eMTb
zLEc`;Vee(Z_DH|;6f7ovYW@4%jCX<AMCO|^E%s(ZZBjEcX<2}L;~t+C$@nX&6O~RY
zp}$)S35#1JY2qnQ4){pMdd!uKMXF?CN_Cvz4d=}VCWO8NU7rCl<dmEcLk$!36h6VW
zi14<^37yl0w<P+8@qePGW&%^6yB&*AkR@=7#*&;c(-c(rq9y`FK{Q4zrJ^|pI%1;r
zq;}$&)`HAE)1Gt|-6er>ZLz*2BX3E~#wpoH6xzGdrZ_Hmo+j}~@##1os#J_*<j<7Y
ze|$xZhH;)2Y0k4)mBZ3(qddB7q&BfGO`MfnNEHvcP7@mdLQ)nFqIGS!k@2C!A<p?U
z3MK%o`Lr5uMjK9BEG~drv-Ju{+muH#nHfIJqz%fMna+%D$$XZfR%4EBbIKLZrIAYa
zdkV2t3`qdQ2J$G?M%(Uft))VT-BGGk)-R{BP#$jav{7)KRQsjMt*;~(3S-#@nRB-7
zuErQ2nieDLLo{Tz^R1NA+gW-j@2fJgb)KMhZCl@ig5w9}1K15J%7kaS??)^X(X}@o
z8Yg98WPxF0#?jUPT(_6tc>q2~1L7%MD7F5Xs4cMbuY<w3%;g|2_@U&pfsltX8$2r2
zfd&laK&xc%Hop#3sc#POQF^d?VKv|tZ*}VS^w?e<$wDfJJ^KBE)7}MJ^k+Z_3%n*}
z&lzLSfY#cS(e>w{$6$0HYtrBu`#Wg+jM0#W|G=)g|EGEn&c2c;P8pC}(Dfz|Ng^r;
zloCjhGXf>VgTVD|55e9m%{>=^Ut$0z=-cy#JO;)Nbln3oXA?4~sO1U6LIPU6-KSXJ
z`V3-Wg|DrfG?x%^klUBJi=T%)HD8x%Z#*-id{p#3>uZ_qt9Rg}ZQ!{$Eg+82FDrtm
z*mVcz3m2BfZIEh=QbHeJf7AHw*Z1$jBf~g`<dku2n{k|%VwNG_j^iI^JX@`(4?*LF
z%bgi$fw~6d7Ib%MNZtH@LA`<2V#GrLe&@0=aqj@#*pNrG9n$N-7&4&?Hm6JXBLJM~
zZr@YrOGp`qskUn?=FU-ScA%8%K2z6WgZco;YfyF_G?3K>a!1I!uw@0L_xSxcOk3gj
zVl$wqZlKPOkbI2f6C^LMcnMe^7Vbb_0mc;0`&Y0mnQ@j5U5T;btVj(Sq%Og7R27Qt
zsuZ|aMps^~9_NeE=1RV;FZ^M&t4OYfMZM}AWa;@CSC*nJ2m+`WX(8lMJUuq!ve>SQ
zrLKw|NWm!U=Ug4w)&&DxirEl>+Tl<TF)VCvT4DvSV%nv?d+ytCvEk>d(EDNy!NK!w
TOyTNl)72Mx0c~FI)dKo2Kng{+
literal 0
HcmV?d00001
diff --git a/ros2/tests/launch/launching_test_sot_loader.py b/ros2/tests/launch/launching_test_sot_loader.py
new file mode 100644
index 0000000..788c222
--- /dev/null
+++ b/ros2/tests/launch/launching_test_sot_loader.py
@@ -0,0 +1,78 @@
+# Copyright 2021 LAAS-CNRS
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import sys
+import unittest
+
+import ament_index_python
+
+import launch
+import launch.actions
+
+import launch_testing
+import launch_testing.actions
+from launch.substitutions import PathJoinSubstitution
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+import pytest
+
+
+@pytest.mark.launch_test
+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};
+
+ terminating_process = Node(
+ 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()
+ )
+
+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
+ launch_testing.asserts.assertExitCodes(proc_info)
diff --git a/ros2/tests/launch/launching_test_sot_loader_basic.py b/ros2/tests/launch/launching_test_sot_loader_basic.py
new file mode 100755
index 0000000..0d0b3b8
--- /dev/null
+++ b/ros2/tests/launch/launching_test_sot_loader_basic.py
@@ -0,0 +1,74 @@
+# Copyright 2021 LAAS-CNRS
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from pathlib import Path
+import unittest
+import launch
+import launch.actions
+import launch_testing
+import launch_testing.actions
+from launch.substitutions import PathJoinSubstitution
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import pytest
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+ ld = LaunchDescription()
+
+ robot_description_content_path = (
+ Path(get_package_share_directory("dynamic_graph_bridge")) /
+ "urdf" /
+ "dgb_minimal_robot.urdf"
+ )
+ assert(robot_description_content_path.exists())
+ robot_description_content = open(
+ PathJoinSubstitution(str(robot_description_content_path)).perform(None)
+ ).read()
+ terminating_process = Node(
+ package="dynamic_graph_bridge",
+ executable="test_sot_loader_basic",
+ output="screen",
+ emulate_tty=True,
+ parameters=[
+ {"state_vector_map": ["joint1", "joint2"]},
+ {"robot_description": robot_description_content},
+ ],
+ )
+
+ return (
+ launch.LaunchDescription(
+ [
+ terminating_process,
+ launch_testing.util.KeepAliveProc(),
+ launch_testing.actions.ReadyToTest(),
+ ]
+ ),
+ locals(),
+ )
+
+
+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
+ launch_testing.asserts.assertExitCodes(proc_info)
diff --git a/ros2/tests/main.cpp b/ros2/tests/main.cpp
new file mode 100644
index 0000000..f4adc6a
--- /dev/null
+++ b/ros2/tests/main.cpp
@@ -0,0 +1,24 @@
+/**
+ * @file main.cpp
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ * @date 2019-05-22
+ *
+ * @brief Main file that runs all unittest using gtest
+ * @see
+ * https://git-amd.tuebingen.mpg.de/amd-clmc/ci_example/wikis/catkin:-how-to-implement-unit-tests
+ */
+
+#include <gtest/gtest.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+int main(int argc, char **argv)
+{
+ rclcpp::init(argc, argv);
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+ rclcpp::shutdown();
+}
diff --git a/ros2/tests/python_scripts/simple_add.py b/ros2/tests/python_scripts/simple_add.py
new file mode 100644
index 0000000..5d8b44e
--- /dev/null
+++ b/ros2/tests/python_scripts/simple_add.py
@@ -0,0 +1 @@
+a = 1 + 1 +1
diff --git a/ros2/tests/python_scripts/simple_add_fail.py b/ros2/tests/python_scripts/simple_add_fail.py
new file mode 100644
index 0000000..ee2ab51
--- /dev/null
+++ b/ros2/tests/python_scripts/simple_add_fail.py
@@ -0,0 +1 @@
+a = 1 + 1 + b
diff --git a/ros2/tests/test_common.hpp b/ros2/tests/test_common.hpp
new file mode 100644
index 0000000..008a183
--- /dev/null
+++ b/ros2/tests/test_common.hpp
@@ -0,0 +1,167 @@
+/**
+ * @file
+ * @license BSD 3-clause
+ * @copyright Copyright (c) 2020, New York University and Max Planck
+ * Gesellschaft
+ *
+ * @brief Common include and declaration for the tests.
+ */
+
+#pragma once
+
+#include <gtest/gtest.h>
+#include <signal.h>
+#include <sys/types.h>
+
+#include <chrono>
+#include <thread>
+
+// We use another class of this package which has an independant unittest.
+#include "dynamic_graph_bridge/ros.hpp"
+
+using namespace std::chrono_literals;
+using namespace dynamic_graph_bridge;
+
+/**
+ * @brief Global ros node name used unitquely for the unit-tests.
+ */
+const std::string ROS_NODE_NAME = "test_dgm";
+
+void start_dg_ros_service()
+{
+ // Service name.
+ std::string service_name = "/dynamic_graph_bridge/start_dynamic_graph";
+ // Create a client from a temporary node.
+ RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
+ auto client = ros_node->create_client<std_srvs::srv::Empty>(service_name);
+ ASSERT_TRUE(client->wait_for_service(1s));
+
+ // Fill the command message.
+ std_srvs::srv::Empty::Request::SharedPtr request =
+ std::make_shared<std_srvs::srv::Empty::Request>();
+ std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> response;
+ // Call the service.
+ response = client->async_send_request(request);
+ // Wait for the answer.
+ ASSERT_TRUE(response.valid());
+ // wait_for_response(response);
+ ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
+ rclcpp::executor::FutureReturnCode::SUCCESS);
+}
+
+void stop_dg_ros_service()
+{
+ // Service name.
+ std::string service_name = "/dynamic_graph_bridge/stop_dynamic_graph";
+ // Create a client from a temporary node.
+ RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
+ auto client = ros_node->create_client<std_srvs::srv::Empty>(service_name);
+ ASSERT_TRUE(client->wait_for_service(1s));
+
+ // Fill the command message.
+ std_srvs::srv::Empty::Request::SharedPtr request =
+ std::make_shared<std_srvs::srv::Empty::Request>();
+ std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> response;
+ // Call the service.
+ response = client->async_send_request(request);
+ // Wait for the answer.
+ ASSERT_TRUE(response.valid());
+ // wait_for_response(response);
+ ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
+ rclcpp::executor::FutureReturnCode::SUCCESS);
+}
+
+void start_run_python_command_ros_service(const std::string& cmd,
+ std::string& result,
+ std::string& standarderror,
+ std::string& standardoutput)
+{
+ // Service name.
+ std::string service_name = "/dynamic_graph_bridge/run_python_command";
+ // Create a client from a temporary node.
+ RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
+ auto client =
+ ros_node->create_client<dynamic_graph_bridge_msgs::srv::RunPythonCommand>(service_name);
+ ASSERT_TRUE(client->wait_for_service(1s));
+
+ // Fill the command message.
+ dynamic_graph_bridge_msgs::srv::RunPythonCommand::Request::SharedPtr request =
+ std::make_shared<dynamic_graph_bridge_msgs::srv::RunPythonCommand::Request>();
+ std::shared_future<dynamic_graph_bridge_msgs::srv::RunPythonCommand::Response::SharedPtr>
+ response;
+ request->input = cmd;
+ // Call the service.
+ response = client->async_send_request(request);
+ // Wait for the answer.
+ ASSERT_TRUE(response.valid());
+ // wait_for_response(response);
+ ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
+ rclcpp::executor::FutureReturnCode::SUCCESS);
+ // Get the results.
+ result = response.get()->result;
+ standarderror = response.get()->standarderror;
+ standardoutput = response.get()->standardoutput;
+}
+
+void start_run_python_script_ros_service(const std::string& file_name,
+ std::string& result)
+{
+ // Service name.
+ std::string service_name = "/dynamic_graph_bridge/run_python_file";
+ // Create a client from a temporary node.
+ RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
+ auto client =
+ ros_node->create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(service_name);
+ ASSERT_TRUE(client->wait_for_service(1s));
+
+ // Fill the command message.
+ dynamic_graph_bridge_msgs::srv::RunPythonFile::Request::SharedPtr request =
+ std::make_shared<dynamic_graph_bridge_msgs::srv::RunPythonFile::Request>();
+ std::shared_future<dynamic_graph_bridge_msgs::srv::RunPythonFile::Response::SharedPtr>
+ response;
+ request->input = file_name;
+ // Call the service.
+ response = client->async_send_request(request);
+ // Wait for the answer.
+ ASSERT_TRUE(response.valid());
+ // wait_for_response(response);
+ ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
+ rclcpp::executor::FutureReturnCode::SUCCESS);
+ // Get the results.
+ result = response.get()->result;
+}
+
+void display_services(
+ std::map<std::string, std::vector<std::string> >& list_service_and_types)
+{
+ for (std::map<std::string, std::vector<std::string> >::const_iterator
+ const_it = list_service_and_types.begin();
+ const_it != list_service_and_types.end();
+ ++const_it)
+ {
+ std::cerr << const_it->first << "\t\t\t";
+ for (unsigned i = 0; i < const_it->second.size(); ++i)
+ {
+ std::cerr << std::right;
+ std::cerr << const_it->second[i] << std::endl;
+ }
+ }
+}
+
+void display_services()
+{
+ std::map<std::string, std::vector<std::string> > list_service_and_types =
+ get_ros_node("display_services")->get_service_names_and_types();
+ for (std::map<std::string, std::vector<std::string> >::const_iterator
+ const_it = list_service_and_types.begin();
+ const_it != list_service_and_types.end();
+ ++const_it)
+ {
+ std::cerr << const_it->first << "\t\t\t";
+ for (unsigned i = 0; i < const_it->second.size(); ++i)
+ {
+ std::cerr << std::right;
+ std::cerr << const_it->second[i] << std::endl;
+ }
+ }
+}
\ No newline at end of file
diff --git a/ros2/tests/test_import.py b/ros2/tests/test_import.py
new file mode 100755
index 0000000..b919565
--- /dev/null
+++ b/ros2/tests/test_import.py
@@ -0,0 +1,27 @@
+#!/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/tests/test_ros_init.cpp b/ros2/tests/test_ros_init.cpp
new file mode 100644
index 0000000..b671466
--- /dev/null
+++ b/ros2/tests/test_ros_init.cpp
@@ -0,0 +1,191 @@
+/**
+ * @file test_get_ros_node.cpp
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ * @date 2019-05-22
+ */
+
+#include "test_common.hpp"
+
+/***************************
+ * SETUP THE TESTING CLASS *
+ ***************************/
+
+using namespace std::chrono_literals;
+using namespace dynamic_graph_bridge;
+
+/**
+ * @brief This is the test environment
+ */
+class TestRosInit : public ::testing::Test
+{
+ /**
+ * @brief SetUp, is executed before the unit tests
+ */
+ void SetUp()
+ {
+ ros_spin_non_blocking();
+ }
+
+ /**
+ * @brief TearDown, is executed after the unit tests
+ */
+ void TearDown()
+ {
+ ros_clean();
+ }
+};
+
+/**
+ * @brief This is the test environment
+ */
+class TestRosInitGlobalVar : public ::testing::Test
+{
+};
+
+/*****************
+ * Start Testing *
+ *****************/
+
+/**
+ * Here we check that we can create a ros node
+ */
+TEST_F(TestRosInitGlobalVar, test_create_node)
+{
+ /* Setup */
+ std::string node_name = "my_global_node";
+ get_ros_node(node_name);
+
+ /* Test */
+ ASSERT_TRUE(ros_node_exists(node_name));
+}
+
+/**
+ * Here we check that we still have the ros node created (global variable)
+ */
+TEST_F(TestRosInitGlobalVar, test_global_variable_deleted_after_tear_down)
+{
+ /* Setup */
+ std::string node_name = "my_global_node";
+
+ /* Test */
+ ASSERT_TRUE(ros_node_exists(node_name));
+}
+
+/**
+ * Here we check that the destruction of the variable
+ */
+TEST_F(TestRosInitGlobalVar, test_delete_global_variable)
+{
+ /* Setup */
+ std::string node_name = "my_global_node";
+ ros_shutdown(node_name);
+
+ /* Test */
+ ASSERT_FALSE(ros_node_exists(node_name));
+}
+
+/**
+ * Here we check if we can destruct the object several time
+ */
+TEST_F(TestRosInit, test_multiple_shutdown_call)
+{
+ /* Setup */
+ std::string node_name = "my_global_node";
+
+ /* Test */
+ ASSERT_NO_THROW(for (unsigned i = 0; i < 20; ++i) {
+ ros_shutdown(node_name);
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ });
+}
+
+/**
+ * @brief Dummy function to be called as ros services
+ *
+ * @return true
+ */
+bool simple_service(std_srvs::srv::Empty::Request::SharedPtr,
+ std_srvs::srv::Empty::Response::SharedPtr)
+{
+ return true;
+}
+
+/**
+ * Check if services are available
+ */
+TEST_F(TestRosInit, test_services_available)
+{
+ /* Setup */
+ // create a node and a service
+ RosNodePtr n0 = get_ros_node("node_0");
+ ros_add_node_to_executor("node_0");
+ std::string service_name = "/simple_service";
+ rclcpp::Service<std_srvs::srv::Empty>::SharedPtr server =
+ n0->create_service<std_srvs::srv::Empty>(service_name, &simple_service);
+
+ /* Test */
+ rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
+ n0->create_client<std_srvs::srv::Empty>(service_name);
+ ASSERT_TRUE(client->wait_for_service(1s));
+}
+
+/**
+ * Check if services are not longer available after shutdown
+ */
+TEST_F(TestRosInit, test_services_shut_down)
+{
+ /* Setup */
+ std::string service_name = "/simple_service";
+
+ // create a node and a service and shutdown the node
+ {
+ RosNodePtr n0 = get_ros_node("node_0");
+ ros_add_node_to_executor("node_0");
+ rclcpp::Service<std_srvs::srv::Empty>::SharedPtr server =
+ n0->create_service<std_srvs::srv::Empty>(service_name,
+ &simple_service);
+ ros_shutdown("node_0");
+ }
+
+ // create a client to this service
+ RosNodePtr n1 = get_ros_node("node_1");
+ rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
+ n1->create_client<std_srvs::srv::Empty>(service_name);
+
+ /* Test */
+ ASSERT_FALSE(client->wait_for_service(100ms));
+}
+
+/**
+ * Check if shutdown without name kills all created nodes.
+ */
+TEST_F(TestRosInit, test_killall_nodes)
+{
+ /* Setup */
+ // creates tones of nodes
+ unsigned int nb_nodes = 20;
+ for (unsigned int i = 0; i < nb_nodes; ++i)
+ {
+ {
+ std::ostringstream os;
+ os << "node_" << i;
+ get_ros_node(os.str());
+ }
+ }
+
+ // kill them all
+ ros_clean();
+
+ /* Test if they are all killed */
+ for (unsigned int i = 0; i < nb_nodes; ++i)
+ {
+ {
+ std::ostringstream os;
+ os << "node_" << i;
+ ASSERT_FALSE(ros_node_exists(os.str()));
+ }
+ }
+}
diff --git a/ros2/tests/test_ros_interpreter.cpp b/ros2/tests/test_ros_interpreter.cpp
new file mode 100644
index 0000000..7966e70
--- /dev/null
+++ b/ros2/tests/test_ros_interpreter.cpp
@@ -0,0 +1,272 @@
+/**
+ * @file test_ros_interpreter.cpp
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-3-Clause
+ * @copyright Copyright (c) 2019, New York University and Max Planck
+ * Gesellschaft.
+ * @date 2019-05-22
+ */
+
+#include <iomanip>
+
+#include "test_common.hpp"
+#include "dynamic_graph_bridge/ros_python_interpreter_server.hpp"
+
+/***************************
+ * SETUP THE TESTING CLASS *
+ ***************************/
+
+using namespace std::chrono_literals;
+using namespace dynamic_graph_bridge;
+
+/**
+ * @brief This is the test environment
+ */
+class TestRosInterpreter : public ::testing::Test
+{
+protected:
+ /**
+ * @brief SetUp, is executed before the unit tests
+ */
+ void SetUp()
+ {
+ // Create the ros node
+ node_name_ = "unittest_ros_python_interpreter";
+ run_python_command_service_name_ =
+ "/dynamic_graph_bridge/run_python_command";
+ run_python_file_service_name_ =
+ "/dynamic_graph_bridge/run_python_file";
+ ros_spin_non_blocking();
+ }
+
+ /**
+ * @brief TearDown, is executed after the unit tests
+ */
+ void TearDown()
+ {
+ // delete the ros node
+ ros_clean();
+ }
+ /**
+ * @brief Node name
+ */
+ std::string node_name_;
+
+ /**
+ * @brief Name of the ros service.
+ */
+ std::string run_python_command_service_name_;
+
+ /**
+ * @brief Name of the ros service.
+ */
+ std::string run_python_file_service_name_;
+};
+
+/*****************
+ * Start Testing *
+ *****************/
+
+TEST_F(TestRosInterpreter, test_constructor_no_throw)
+{
+ /* setup */
+
+ /* test */
+ ASSERT_NO_THROW(RosPythonInterpreterServer rpi;);
+}
+
+TEST_F(TestRosInterpreter, test_destructor_no_throw)
+{
+ ASSERT_NO_THROW({ RosPythonInterpreterServer rpi; });
+}
+
+TEST_F(TestRosInterpreter, test_run_cmd_not_available_upon_construction)
+{
+ /* Setup. */
+
+ // Init the class to test.
+ RosPythonInterpreterServer rpi;
+
+ // Fetch the information using ROS.
+ std::map<std::string, std::vector<std::string>> list_service_and_types;
+ RosNodePtr ros_node = get_ros_node(node_name_);
+ bool simple_service_exists = false;
+
+ // Check if the service exists.
+ list_service_and_types = ros_node->get_service_names_and_types();
+ display_services(list_service_and_types);
+ simple_service_exists =
+ list_service_and_types.find(run_python_command_service_name_) !=
+ list_service_and_types.end();
+
+ /* Test. */
+ ASSERT_FALSE(simple_service_exists);
+}
+
+TEST_F(TestRosInterpreter, test_run_file_not_available_upon_construction)
+{
+ /* Setup. */
+
+ // Init the class to test.
+ RosPythonInterpreterServer rpi;
+
+ // Fetch the information using ROS.
+ std::map<std::string, std::vector<std::string>> list_service_and_types;
+ RosNodePtr ros_node = get_ros_node(node_name_);
+ bool simple_service_exists = false;
+
+ // Check if the service exists.
+ list_service_and_types = ros_node->get_service_names_and_types();
+ display_services(list_service_and_types);
+ simple_service_exists =
+ list_service_and_types.find(run_python_file_service_name_) !=
+ list_service_and_types.end();
+
+ /* Test. */
+ ASSERT_FALSE(simple_service_exists);
+}
+
+TEST_F(TestRosInterpreter, test_run_cmd_available_after_init)
+{
+ /* Setup. */
+
+ // Init the class to test.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
+
+ // print the service available
+ RosNodePtr ros_node = get_ros_node(node_name_);
+ std::map<std::string, std::vector<std::string>> list_service_and_types =
+ ros_node->get_service_names_and_types();
+ display_services(list_service_and_types);
+
+ // Fetch the information using ROS.
+ auto client = get_ros_node(node_name_)
+ ->create_client<dynamic_graph_bridge_msgs::srv::RunPythonCommand>(
+ "/dynamic_graph_bridge/run_python_command");
+
+ /* Test. */
+ ASSERT_TRUE(client->wait_for_service(1s));
+}
+
+TEST_F(TestRosInterpreter, test_run_file_available_after_init)
+{
+ /* Setup. */
+
+ // Init the class to test.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
+
+ // Fetch the information using ROS.
+ auto client = get_ros_node(node_name_)
+ ->create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(
+ "/dynamic_graph_bridge/run_python_file");
+
+ /* Test. */
+ ASSERT_TRUE(client->wait_for_service(1s));
+}
+
+TEST_F(TestRosInterpreter, test_call_run_command_result)
+{
+ /* Setup. */
+
+ // Create the ros python interpreter.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
+
+ // Create and call the clients.
+ std::string cmd = "1 + 1";
+ std::string result = "";
+ std::string standarderror = "";
+ std::string standardoutput = "";
+ start_run_python_command_ros_service(
+ cmd, result, standarderror, standardoutput);
+
+ /* Tests. */
+ ASSERT_EQ(result, "2");
+}
+
+TEST_F(TestRosInterpreter, test_call_run_command_standardoutput)
+{
+ /* Setup. */
+
+ // Create the ros python interpreter.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
+
+ // Create and call the clients.
+ std::string cmd = "print(\"Banana\")";
+ std::string result = "";
+ std::string standarderror = "";
+ std::string standardoutput = "";
+ start_run_python_command_ros_service(
+ cmd, result, standarderror, standardoutput);
+
+ /* Tests. */
+ ASSERT_EQ(standardoutput, "Banana\nprint(\"Banana\")\nOutput:Banana\n\n");
+}
+
+TEST_F(TestRosInterpreter, test_call_run_command_standarderror)
+{
+ /* Setup. */
+
+ // Create the ros python interpreter.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
+
+ // Create and call the clients.
+ std::string cmd = "a";
+ std::string result = "";
+ std::string standarderror = "";
+ std::string standardoutput = "";
+ start_run_python_command_ros_service(
+ cmd, result, standarderror, standardoutput);
+
+ /* Tests. */
+ ASSERT_EQ(
+ standarderror,
+ "Traceback (most recent call last):\n File \"<string>\", line 1, "
+ "in <module>\nNameError: name 'a' is not defined\n");
+}
+
+TEST_F(TestRosInterpreter, test_call_run_script_result)
+{
+ /* Setup. */
+
+ // Create the ros python interpreter.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
+
+ // Create and call the clients.
+ std::string file_name = TEST_CONFIG_PATH + std::string("simple_add.py");
+ std::string result = "";
+ start_run_python_script_ros_service(file_name, result);
+
+ /* Tests. */
+ ASSERT_EQ(result, "");
+}
+
+TEST_F(TestRosInterpreter, test_call_run_script_standarderror)
+{
+ /* Setup. */
+
+ // Create the ros python interpreter.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
+
+ // Create and call the clients.
+ std::string file_name =
+ TEST_CONFIG_PATH + std::string("simple_add_fail.py");
+ std::string result = "";
+ start_run_python_script_ros_service(file_name, result);
+
+ // Prepare the test.
+ std::string error_first_part = "a = 1 + 1 + b";
+ std::string error_second_part = "NameError: name \'b\' is not defined";
+ std::size_t found_first_part = result.find(error_first_part);
+ std::size_t found_second_part = result.find(error_second_part);
+
+ /* test */
+ ASSERT_TRUE(found_first_part != std::string::npos);
+ ASSERT_TRUE(found_second_part != std::string::npos);
+}
diff --git a/ros2/tests/test_sot_loader.cpp b/ros2/tests/test_sot_loader.cpp
new file mode 100644
index 0000000..b914ee2
--- /dev/null
+++ b/ros2/tests/test_sot_loader.cpp
@@ -0,0 +1,93 @@
+
+#include <gmock/gmock.h>
+#include "dynamic_graph_bridge/ros.hpp"
+#include "dynamic_graph_bridge/sot_loader.hh"
+
+class MockSotLoaderTest: public ::testing::Test {
+
+public:
+
+ class MockSotLoader : public SotLoader {
+ public:
+ ~MockSotLoader(){}
+
+ void generateEvents() {
+ usleep(20000);
+ std::cerr << "Start Dynamic Graph " << std::endl;
+ dynamic_graph_stopped_ = false;
+
+ usleep(20000);
+ std::cerr << "Stop Dynamic Graph " << std::endl;
+ dynamic_graph_stopped_ = true;
+ }
+
+ void testLoadController() {
+ // Set input call
+ int argc=2;
+ char *argv[2];
+ char argv1[30]="mocktest";
+ argv[0]=argv1;
+ char argv2[60]="--input-file=libimpl_test_sot_external_interface.so";
+ argv[1]=argv2;
+ parseOptions(argc,argv);
+
+ std::string finalname("libimpl_test_sot_external_interface.so");
+ EXPECT_TRUE(finalname == dynamicLibraryName_);
+
+ // Performs initialization of libimpl_test_sot_external_interface.so
+ loadController();
+ EXPECT_TRUE(sotRobotControllerLibrary_ != 0);
+ EXPECT_TRUE(sotController_ != nullptr);
+ // initialize start/stop and runCommand services
+ initializeServices();
+
+ // Constructor should default freeFlyerPose
+ EXPECT_TRUE( freeFlyerPose_.header.frame_id == std::string("odom"));
+ EXPECT_TRUE( freeFlyerPose_.child_frame_id == std::string("base_link"));
+
+ // Initialie publication of sot joint states
+ // and base eff
+ initPublication();
+
+ // Start the control loop thread.
+ startControlLoop();
+
+ // Start the thread generating events.
+ std::thread local_events(&MockSotLoader::generateEvents,this);
+
+ // Wait for each threads.
+ SotLoader::lthread_join(); // Wait 100 ms
+ local_events.join();
+ }
+ };
+
+public:
+ MockSotLoader* mockSotLoader_ptr_;
+
+ void SetUp() {
+ mockSotLoader_ptr_ = new MockSotLoader();
+ mockSotLoader_ptr_->initialize();
+ }
+
+ void TearDown() {
+ delete mockSotLoader_ptr_;
+ mockSotLoader_ptr_ = nullptr;
+ }
+
+};
+
+TEST_F(MockSotLoaderTest,TestLoadController)
+{
+ mockSotLoader_ptr_->testLoadController();
+}
+
+
+int main(int argc, char **argv) {
+ ::testing::InitGoogleTest(&argc, argv);
+ rclcpp::init(argc, argv);
+
+ int r=RUN_ALL_TESTS();
+
+ rclcpp::shutdown();
+ return r;
+}
diff --git a/ros2/tests/test_sot_loader_basic.cpp b/ros2/tests/test_sot_loader_basic.cpp
new file mode 100644
index 0000000..20b0b1b
--- /dev/null
+++ b/ros2/tests/test_sot_loader_basic.cpp
@@ -0,0 +1,184 @@
+
+#include <gmock/gmock.h>
+#include "dynamic_graph_bridge/ros.hpp"
+#include "dynamic_graph_bridge/sot_loader_basic.hh"
+
+class MockSotLoaderBasicTest: public ::testing::Test {
+
+public:
+
+ class MockSotLoaderBasic: public SotLoaderBasic {
+ public:
+ virtual ~MockSotLoaderBasic(){}
+
+ void checkStateVectorMap() {
+ readSotVectorStateParam();
+ ASSERT_EQ(static_cast<int>(stateVectorMap_.size()),2);
+ ASSERT_EQ(nbOfJoints_, 2);
+ std::string aJoint1("joint1");
+ EXPECT_TRUE(aJoint1 == stateVectorMap_[0]);
+ std::string aJoint2("joint2");
+ EXPECT_TRUE(aJoint2 == stateVectorMap_[1]);
+ }
+
+ void testParseOptions() {
+
+ // Void call
+ int argc=1;
+ char argv1[30]="mocktest";
+ char *argv[3];
+ argv[0]=argv1;
+ EXPECT_EQ(parseOptions(argc,argv),-1);
+
+ // Test help call
+ argc=2;
+ char argv2[30]="--help";
+ argv[1]=argv2;
+ EXPECT_EQ(parseOptions(argc,argv),-1);
+
+ // Test input file
+ char argv3[60]="--input-file=libimpl_test_sot_external_interface.so";
+ argv[1]=argv3;
+ EXPECT_EQ(parseOptions(argc,argv),0);
+
+ // Check that the file is what we specified
+ std::string finalname("libimpl_test_sot_external_interface.so");
+ EXPECT_TRUE(finalname == dynamicLibraryName_);
+ }
+
+ void testInitializeRosNode() {
+ int argc=1;
+ char *argv[1];
+ char argv1[30]="mocktest";
+ argv[0] = argv1;
+ parseOptions(argc,argv);
+ initializeServices();
+ EXPECT_TRUE(service_start_ != 0);
+ EXPECT_TRUE(service_stop_ != 0);
+ initPublication();
+ EXPECT_TRUE(joint_pub_ != 0);
+ }
+
+ void testJointStatesPublication() {
+ int argc=1;
+ char *argv[1];
+ char argv1[30]="mocktest";
+ argv[0] = argv1;
+ parseOptions(argc,argv);
+
+ }
+
+ void testInitialization() {
+
+ // Set input arguments
+ int argc=2;
+ char *argv[2];
+
+ char argv1[30]="mocktest";
+ argv[0]=argv1;
+ char argv2[60]="--input-file=libimpl_test_sot_external_interface.so";
+ argv[1]=argv2;
+ parseOptions(argc,argv);
+
+ std::string finalname("libimpl_test_sot_external_interface.so");
+ EXPECT_TRUE(finalname == dynamicLibraryName_);
+
+ // Performs initializatio of libimpl_test_sot_external_interface.so
+ loadController();
+ EXPECT_TRUE(sotRobotControllerLibrary_ != 0);
+ EXPECT_TRUE(sotController_ != nullptr);
+ }
+
+ void test_start_stop_dg() {
+ std::shared_ptr<std_srvs::srv::Empty::Request> empty_rqst;
+ std::shared_ptr<std_srvs::srv::Empty::Response> empty_resp;
+ start_dg(empty_rqst,empty_resp);
+ EXPECT_FALSE(dynamic_graph_stopped_);
+
+ stop_dg(empty_rqst,empty_resp);
+ EXPECT_TRUE(dynamic_graph_stopped_);
+ }
+
+ void test_cleanup() {
+ CleanUp();
+ EXPECT_TRUE(sotController_ == NULL);
+ std::cout << "sotController_: " << sotController_
+ << std::endl;
+
+ // Set input arguments
+ int argc=2;
+ char *argv[2];
+
+ char argv1[30]="mocktest";
+ argv[0]=argv1;
+ char argv2[60]="--input-file=libimpl_test_sot_external_interface.so";
+ argv[1]=argv2;
+ parseOptions(argc,argv);
+
+ std::string finalname("libimpl_test_sot_external_interface.so");
+ EXPECT_TRUE(finalname == dynamicLibraryName_);
+
+ // Performs initializatio of libimpl_test_sot_external_interface.so
+ loadController();
+ // Remove
+ CleanUp();
+ }
+ };
+
+public:
+
+ MockSotLoaderBasic* mockSotLoaderBasic_ptr_;
+
+ void SetUp() {
+ mockSotLoaderBasic_ptr_ = new MockSotLoaderBasic();
+ mockSotLoaderBasic_ptr_->initialize();
+ }
+
+ void TearDown() {
+ delete mockSotLoaderBasic_ptr_;
+ mockSotLoaderBasic_ptr_ = nullptr;
+ }
+
+};
+
+TEST_F(MockSotLoaderBasicTest, TestReadingParameters)
+{
+ mockSotLoaderBasic_ptr_->checkStateVectorMap();
+}
+
+TEST_F(MockSotLoaderBasicTest,ParseOptions)
+{
+ mockSotLoaderBasic_ptr_->testParseOptions();
+}
+
+TEST_F(MockSotLoaderBasicTest,TestInitialization)
+{
+ mockSotLoaderBasic_ptr_->testInitialization();
+}
+
+TEST_F(MockSotLoaderBasicTest,TestStartStopDG)
+{
+ mockSotLoaderBasic_ptr_->test_start_stop_dg();
+}
+
+TEST_F(MockSotLoaderBasicTest,TestInitializeRosNode)
+{
+ mockSotLoaderBasic_ptr_->testInitializeRosNode();
+}
+
+TEST_F(MockSotLoaderBasicTest,CleanUp)
+{
+ mockSotLoaderBasic_ptr_->test_cleanup();
+}
+
+int main(int argc, char **argv) {
+ ::testing::InitGoogleTest(&argc, argv);
+ rclcpp::init(argc, argv);
+
+ int r=RUN_ALL_TESTS();
+
+ std::cout << "ros shutdown" << std::endl;
+ rclcpp::shutdown();
+ std::cout << "ros shutdown done" << std::endl;
+ return r;
+}
diff --git a/ros2/tests/urdf/dgb_minimal_robot.urdf b/ros2/tests/urdf/dgb_minimal_robot.urdf
new file mode 100644
index 0000000..e15cb93
--- /dev/null
+++ b/ros2/tests/urdf/dgb_minimal_robot.urdf
@@ -0,0 +1,98 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from minimal_robot.urdf.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<robot name="MinimalRobot">
+ <!-- Used for fixing robot -->
+ <link name="world"/>
+ <joint name="base_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <parent link="world"/>
+ <child link="base_link"/>
+ </joint>
+ <link name="base_link">
+ <inertial>
+ <mass value="0.01"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.2" radius="0.1"/>
+ </geometry>
+ <material name="DarkGrey">
+ <color rgba="0.4 0.4 0.4 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="1" radius="0.1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="joint1" type="revolute">
+ <origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
+ <parent link="base_link"/>
+ <child link="link1"/>
+ <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
+ </joint>
+ <link name="link1">
+ <inertial>
+ <mass value="0.01"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="1" radius="0.1"/>
+ </geometry>
+ <material name="DarkGrey">
+ <color rgba="0.4 0.4 0.4 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="1" radius="0.1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="joint2" type="revolute">
+ <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
+ <parent link="link1"/>
+ <child link="link2"/>
+ <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
+ </joint>
+ <link name="link2">
+ <inertial>
+ <mass value="0.01"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="1" radius="0.1"/>
+ </geometry>
+ <material name="DarkGrey">
+ <color rgba="0.4 0.4 0.4 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="1" radius="0.1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="tool_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 1"/>
+ <parent link="link2"/>
+ <child link="tool_link"/>
+ </joint>
+ <link name="tool_link">
+ </link>
--
GitLab