diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs
new file mode 100644
index 0000000000000000000000000000000000000000..f557ab753deedb8e2a5dbb6500978d3945c11b57
--- /dev/null
+++ b/.git-blame-ignore-revs
@@ -0,0 +1,2 @@
+# format (Guilhem Saurel, 2022-09-06)
+426c21a7f4e5b445d17825235f983e2f8e1a86ec
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c3162e5088b070a18ffdf86b64d4c4cc48bbbd9f
--- /dev/null
+++ b/.pre-commit-config.yaml
@@ -0,0 +1,38 @@
+ci:
+ autoupdate_branch: 'devel'
+repos:
+- repo: https://github.com/pre-commit/mirrors-clang-format
+ rev: v14.0.6
+ hooks:
+ - id: clang-format
+ args: [--style=Google]
+- repo: https://github.com/pre-commit/pre-commit-hooks
+ rev: v4.3.0
+ hooks:
+ - id: check-added-large-files
+ - id: check-ast
+ - id: check-executables-have-shebangs
+ - id: check-json
+ - id: check-merge-conflict
+ - id: check-symlinks
+ - id: check-toml
+ - id: check-yaml
+ - id: debug-statements
+ - id: destroyed-symlinks
+ - id: detect-private-key
+ - id: end-of-file-fixer
+ - id: fix-byte-order-marker
+ - id: mixed-line-ending
+ - id: trailing-whitespace
+- repo: https://github.com/psf/black
+ rev: 22.8.0
+ hooks:
+ - id: black
+- repo: https://github.com/PyCQA/flake8
+ rev: 5.0.4
+ hooks:
+ - id: flake8
+- repo: https://github.com/cheshirekow/cmake-format-precommit
+ rev: v0.6.13
+ hooks:
+ - id: cmake-format
diff --git a/.travis.yml b/.travis.yml
deleted file mode 100644
index 811deb2a626a910c318a725f9ceda3356e8ee319..0000000000000000000000000000000000000000
--- a/.travis.yml
+++ /dev/null
@@ -1,14 +0,0 @@
-sudo: required
-
-services:
- - docker
-
-before_install:
-- docker build -t dyngb-trusty -f ./travis_custom/Dockerfile .
-- docker run -t -d --name dyngb-trusty-test dyngb-trusty
-- docker exec dyngb-trusty-test sudo mkdir -p /catkin_ws_dyngb/src
-- docker exec dyngb-trusty-test sudo sh -c "cd /catkin_ws_dyngb/src;. /opt/ros/indigo/setup.sh;catkin_init_workspace"
-- docker exec dyngb-trusty-test sudo git clone --recursive https://github.com/stack-of-tasks/dynamic_graph_bridge.git /catkin_ws_dyngb/src/dynamic_graph_bridge
-
-script:
-- docker exec dyngb-trusty-test sudo sh -c "cd /catkin_ws_dyngb;. /opt/ros/indigo/setup.sh;. /home/docker/setup-opt-robotpkg.sh;catkin_make"
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c51eaddc11e0ae1d3064cb8b82f8a259df6e866c..0e1515806486dfbf96c1b3e16fface7b395d8d26 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,100 +3,99 @@
# Author: Florent Lamiraux, Nirmal Giftsun, Guilhem Saurel
#
-CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
+cmake_minimum_required(VERSION 3.1)
# Project properties
-SET(PROJECT_ORG stack-of-tasks)
-SET(PROJECT_NAME dynamic_graph_bridge)
-SET(PROJECT_DESCRIPTION "Dynamic graph bridge library")
-SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
+set(PROJECT_ORG stack-of-tasks)
+set(PROJECT_NAME dynamic_graph_bridge)
+set(PROJECT_DESCRIPTION "Dynamic graph bridge library")
+set(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
# Project options
-OPTION(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
+option(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
# Project configuration
-SET(PROJECT_USE_CMAKE_EXPORT TRUE)
-SET(CUSTOM_HEADER_DIR ${PROJECT_NAME})
+set(PROJECT_USE_CMAKE_EXPORT TRUE)
+set(CUSTOM_HEADER_DIR ${PROJECT_NAME})
set(CXX_DISABLE_WERROR FALSE)
-SET(DOXYGEN_USE_MATHJAX YES)
-SET(CATKIN_ENABLE_TESTING OFF)
+set(DOXYGEN_USE_MATHJAX YES)
+set(CATKIN_ENABLE_TESTING OFF)
# JRL-cmakemodule setup
-INCLUDE(cmake/base.cmake)
-INCLUDE(cmake/boost.cmake)
-INCLUDE(cmake/python.cmake)
-INCLUDE(cmake/ros.cmake)
+include(cmake/base.cmake)
+include(cmake/boost.cmake)
+include(cmake/ros.cmake)
# Project definition
-COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
-PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
+compute_project_args(PROJECT_ARGS LANGUAGES CXX)
+project(${PROJECT_NAME} ${PROJECT_ARGS})
# Project dependencies
-SET(CATKIN_REQUIRED_COMPONENTS roscpp std_msgs message_generation std_srvs geometry_msgs sensor_msgs tf2_ros
+set(CATKIN_REQUIRED_COMPONENTS
+ roscpp
+ std_msgs
+ message_generation
+ std_srvs
+ geometry_msgs
+ sensor_msgs
+ tf2_ros
realtime_tools)
-ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS program_options)
-ADD_PROJECT_DEPENDENCY(dynamic_graph_bridge_msgs 0.3.0 REQUIRED)
-
-IF(BUILD_PYTHON_INTERFACE)
- FINDPYTHON()
- SEARCH_FOR_BOOST_PYTHON()
- STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
- ADD_PROJECT_DEPENDENCY(dynamic-graph-python 4.0.0 REQUIRED)
- SET(CATKIN_REQUIRED_COMPONENTS ${CATKIN_REQUIRED_COMPONENTS} rospy)
-
- IF(Boost_VERSION GREATER 107299 OR Boost_VERSION_MACRO GREATER 107299)
- # Silence a warning about a deprecated use of boost bind by boost >= 1.73
- # without dropping support for boost < 1.73
- ADD_DEFINITIONS(-DBOOST_BIND_GLOBAL_PLACEHOLDERS)
- ENDIF()
-ENDIF(BUILD_PYTHON_INTERFACE)
-find_package(catkin REQUIRED COMPONENTS ${CATKIN_REQUIRED_COMPONENTS})
+if(BUILD_PYTHON_INTERFACE)
+ add_project_dependency(dynamic-graph-python 4.0.0 REQUIRED)
+ string(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
+ set(CATKIN_REQUIRED_COMPONENTS ${CATKIN_REQUIRED_COMPONENTS} rospy)
+endif(BUILD_PYTHON_INTERFACE)
+
+add_project_dependency(Boost REQUIRED COMPONENTS program_options)
+add_project_dependency(dynamic_graph_bridge_msgs 0.3.0 REQUIRED)
+add_project_dependency(sot-core REQUIRED)
-ADD_PROJECT_DEPENDENCY(sot-core REQUIRED)
+if(Boost_VERSION GREATER 107299 OR Boost_VERSION_MACRO GREATER 107299)
+ # Silence a warning about a deprecated use of boost bind by boost >= 1.73
+ # without dropping support for boost < 1.73
+ add_definitions(-DBOOST_BIND_GLOBAL_PLACEHOLDERS)
+endif()
+
+find_package(catkin REQUIRED COMPONENTS ${CATKIN_REQUIRED_COMPONENTS})
# Main Library
set(${PROJECT_NAME}_HEADERS
- include/${PROJECT_NAME}/fwd.hh
- include/${PROJECT_NAME}/ros_init.hh
- include/${PROJECT_NAME}/sot_loader.hh
- include/${PROJECT_NAME}/sot_loader_basic.hh
- include/${PROJECT_NAME}/ros_interpreter.hh
- src/converter.hh
- src/sot_to_ros.hh
- )
-
-SET(${PROJECT_NAME}_SOURCES
- src/ros_init.cpp
- src/sot_to_ros.cpp
- src/ros_parameter.cpp
- )
-
-ADD_LIBRARY(ros_bridge SHARED
- ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
-TARGET_INCLUDE_DIRECTORIES(ros_bridge SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
-TARGET_INCLUDE_DIRECTORIES(ros_bridge PUBLIC $<INSTALL_INTERFACE:include>)
-TARGET_LINK_LIBRARIES(ros_bridge ${catkin_LIBRARIES}
- sot-core::sot-core pinocchio::pinocchio dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
-
-IF(SUFFIX_SO_VERSION)
- SET_TARGET_PROPERTIES(ros_bridge PROPERTIES SOVERSION ${PROJECT_VERSION})
-ENDIF(SUFFIX_SO_VERSION)
-
-IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
- INSTALL(TARGETS ros_bridge EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
-ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ include/${PROJECT_NAME}/fwd.hh
+ include/${PROJECT_NAME}/ros_init.hh
+ include/${PROJECT_NAME}/sot_loader.hh
+ include/${PROJECT_NAME}/sot_loader_basic.hh
+ include/${PROJECT_NAME}/ros_interpreter.hh
+ src/converter.hh
+ src/sot_to_ros.hh)
+
+set(${PROJECT_NAME}_SOURCES src/ros_init.cpp src/sot_to_ros.cpp
+ src/ros_parameter.cpp)
+
+add_library(ros_bridge SHARED ${${PROJECT_NAME}_SOURCES}
+ ${${PROJECT_NAME}_HEADERS})
+target_include_directories(ros_bridge SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
+target_include_directories(ros_bridge PUBLIC $<INSTALL_INTERFACE:include>)
+target_link_libraries(ros_bridge ${catkin_LIBRARIES} sot-core::sot-core
+ pinocchio::pinocchio dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
+
+if(SUFFIX_SO_VERSION)
+ set_target_properties(ros_bridge PROPERTIES SOVERSION ${PROJECT_VERSION})
+endif(SUFFIX_SO_VERSION)
+
+if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ install(
+ TARGETS ros_bridge
+ EXPORT ${TARGETS_EXPORT_NAME}
+ DESTINATION lib)
+endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
add_subdirectory(src)
add_subdirectory(tests)
-#install ros executables
-install(PROGRAMS
- scripts/robot_pose_publisher
- scripts/run_command
- scripts/tf_publisher
- DESTINATION share/${PROJECT_NAME}
- )
+# install ros executables
+install(PROGRAMS scripts/robot_pose_publisher scripts/run_command
+ scripts/tf_publisher DESTINATION share/${PROJECT_NAME})
# Install package information
install(FILES manifest.xml package.xml DESTINATION share/${PROJECT_NAME})
diff --git a/README.md b/README.md
index 4b7cc8054e67311a36324177b3febb060777bc17..21e8db6bf9ff34602256ddcf7658b7b7f34d822b 100644
--- a/README.md
+++ b/README.md
@@ -1,9 +1,9 @@
-dynamic-graph bindings
-======================
+# Dynamic Graph bindings
-[](https://travis-ci.org/stack-of-tasks/dynamic_graph_bridge)
[](https://gitlab.laas.fr/stack-of-tasks/dynamic_graph_bridge/commits/master)
-[](http://projects.laas.fr/gepetto/doc/stack-of-tasks/dynamic_graph_bridge/master/coverage/)
+[](https://gepettoweb.laas.fr/doc/stack-of-tasks/dynamic_graph_bridge/master/coverage/)
+[](https://github.com/psf/black)
+[](https://results.pre-commit.ci/latest/github/stack-of-tasks/dynamic_graph_bridge)
This ROS package binds together the ROS framework with the
dynamic-graph real-time control architecture.
diff --git a/cmake b/cmake
index 7a5475bcbac62643eb5c03744f1ee16f83607fe2..57c46b2d7b26bb0c25f8f98cfc2a9868e03be603 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 7a5475bcbac62643eb5c03744f1ee16f83607fe2
+Subproject commit 57c46b2d7b26bb0c25f8f98cfc2a9868e03be603
diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
index 707e217c78663d2961058b00721d3e5fadf35c7a..f90759040df5d7a53160a16d3312dd559cdeb1b4 100644
--- a/doc/Doxyfile.extra.in
+++ b/doc/Doxyfile.extra.in
@@ -1,3 +1,2 @@
INPUT = @PROJECT_SOURCE_DIR@/include \
@PROJECT_SOURCE_DIR@/doc
-
diff --git a/include/dynamic_graph_bridge/fwd.hh b/include/dynamic_graph_bridge/fwd.hh
index b3f172d58954d2e6c7de4d07f34e69b28bdb7818..450d646ec1fbe54996ac23c305fcb6ce6f5c7c88 100644
--- a/include/dynamic_graph_bridge/fwd.hh
+++ b/include/dynamic_graph_bridge/fwd.hh
@@ -12,9 +12,9 @@
#include <dynamic-graph/fwd.hh>
namespace dynamicgraph {
- // classes defined in sot-core
- class Interpreter;
- typedef std::shared_ptr<Interpreter> InterpreterPtr_t;
-}// namespace dynamicgraph
+// classes defined in sot-core
+class Interpreter;
+typedef std::shared_ptr<Interpreter> InterpreterPtr_t;
+} // namespace dynamicgraph
-#endif // DYNAMIC_GRAPH_PYTHON_FWD_HH
+#endif // DYNAMIC_GRAPH_PYTHON_FWD_HH
diff --git a/include/dynamic_graph_bridge/ros_init.hh b/include/dynamic_graph_bridge/ros_init.hh
index fd6f855f39560caef54bb1e14c9437aeb2a35765..a4d1dd46bff1abb8affd7dc34ee3b83d2efe58fd 100644
--- a/include/dynamic_graph_bridge/ros_init.hh
+++ b/include/dynamic_graph_bridge/ros_init.hh
@@ -3,7 +3,8 @@
#include <ros/ros.h>
namespace dynamicgraph {
-ros::NodeHandle& rosInit(bool createAsyncSpinner = false, bool createMultiThreadSpinner = true);
+ros::NodeHandle& rosInit(bool createAsyncSpinner = false,
+ bool createMultiThreadSpinner = true);
/// \brief Return spinner or throw an exception if spinner
/// creation has been disabled at startup.
diff --git a/include/dynamic_graph_bridge/ros_interpreter.hh b/include/dynamic_graph_bridge/ros_interpreter.hh
index 1eacc9e5b375f0aec2f6532079d024e4efd36f3d..6dfa245be8c75de28958575b8734e559babe3238 100644
--- a/include/dynamic_graph_bridge/ros_interpreter.hh
+++ b/include/dynamic_graph_bridge/ros_interpreter.hh
@@ -5,10 +5,11 @@
#include <ros/ros.h>
#pragma GCC diagnostic pop
-#include <dynamic_graph_bridge/fwd.hh>
#include <dynamic_graph_bridge_msgs/RunCommand.h>
#include <dynamic_graph_bridge_msgs/RunPythonFile.h>
+
#include <dynamic-graph/python/interpreter.hh>
+#include <dynamic_graph_bridge/fwd.hh>
namespace dynamicgraph {
/// \brief This class wraps the implementation of the runCommand
@@ -19,19 +20,22 @@ namespace dynamicgraph {
/// the outside.
class Interpreter {
public:
- typedef boost::function<bool(dynamic_graph_bridge_msgs::RunCommand::Request&,
- dynamic_graph_bridge_msgs::RunCommand::Response&)>
+ typedef boost::function<bool(
+ dynamic_graph_bridge_msgs::RunCommand::Request&,
+ dynamic_graph_bridge_msgs::RunCommand::Response&)>
runCommandCallback_t;
- typedef boost::function<bool(dynamic_graph_bridge_msgs::RunPythonFile::Request&,
- dynamic_graph_bridge_msgs::RunPythonFile::Response&)>
+ typedef boost::function<bool(
+ dynamic_graph_bridge_msgs::RunPythonFile::Request&,
+ dynamic_graph_bridge_msgs::RunPythonFile::Response&)>
runPythonFileCallback_t;
explicit Interpreter(ros::NodeHandle& nodeHandle);
/// \brief Method to start python interpreter and deal with messages.
/// \param Command string to execute, result, stdout, stderr strings.
- void runCommand(const std::string& command, std::string& result, std::string& out, std::string& err);
+ void runCommand(const std::string& command, std::string& result,
+ std::string& out, std::string& err);
/// \brief Method to parse python scripts.
/// \param Input file name to parse.
@@ -46,8 +50,9 @@ class Interpreter {
dynamic_graph_bridge_msgs::RunCommand::Response& res);
/// \brief Run a Python file.
- bool runPythonFileCallback(dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
- dynamic_graph_bridge_msgs::RunPythonFile::Response& res);
+ bool runPythonFileCallback(
+ dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
+ dynamic_graph_bridge_msgs::RunPythonFile::Response& res);
private:
python::Interpreter interpreter_;
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index 2e83aad5840d66bdf732dad597a31353809c34f6..53f88d4f8a66761a68e28cc2b5f4b7ec2781d712 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -22,20 +22,21 @@
#include <pinocchio/fwd.hpp>
// Boost includes
-#include <boost/program_options.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
+#include <boost/program_options.hpp>
#include <boost/thread/thread.hpp>
// ROS includes
-#include "ros/ros.h"
-#include "std_srvs/Empty.h"
#include <sensor_msgs/JointState.h>
#include <tf2_ros/transform_broadcaster.h>
+#include "ros/ros.h"
+#include "std_srvs/Empty.h"
+
// Sot Framework includes
-#include <sot/core/debug.hh>
#include <sot/core/abstract-sot-external-interface.hh>
+#include <sot/core/debug.hh>
// Dynamic-graph-bridge includes.
#include <dynamic_graph_bridge/sot_loader_basic.hh>
diff --git a/include/dynamic_graph_bridge/sot_loader_basic.hh b/include/dynamic_graph_bridge/sot_loader_basic.hh
index b635be5afba8afe294a6f4f954834bec0345c89a..0bd63df24ca330589f54fd0d3ca7d3bf5999a40c 100644
--- a/include/dynamic_graph_bridge/sot_loader_basic.hh
+++ b/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -22,18 +22,19 @@
#include <pinocchio/fwd.hpp>
// Boost includes
-#include <boost/program_options.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
+#include <boost/program_options.hpp>
// ROS includes
+#include <sensor_msgs/JointState.h>
+
#include "ros/ros.h"
#include "std_srvs/Empty.h"
-#include <sensor_msgs/JointState.h>
// Sot Framework includes
-#include <sot/core/debug.hh>
#include <sot/core/abstract-sot-external-interface.hh>
+#include <sot/core/debug.hh>
namespace po = boost::program_options;
namespace dgs = dynamicgraph::sot;
@@ -94,10 +95,12 @@ class SotLoaderBasic {
virtual void initializeRosNode(int argc, char* argv[]);
// \brief Callback function when starting dynamic graph.
- bool start_dg(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
+ bool start_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response);
// \brief Callback function when stopping dynamic graph.
- bool stop_dg(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
+ bool stop_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response);
// \brief Read the state vector description based upon the robot links.
int readSotVectorStateParam();
diff --git a/package.xml b/package.xml
index 71bb2de10c29564565edc8b87e436be186af2392..57c3fd293c385440783c9009f1416fd056b8d959 100644
--- a/package.xml
+++ b/package.xml
@@ -1,6 +1,6 @@
<package format="2">
<name>dynamic_graph_bridge</name>
- <version>3.4.2</version>
+ <version>3.4.3</version>
<description>
ROS bindings for dynamic graph.
diff --git a/pyproject.toml b/pyproject.toml
new file mode 100644
index 0000000000000000000000000000000000000000..7ad22b44c945457602878cb6d16e799856c6634f
--- /dev/null
+++ b/pyproject.toml
@@ -0,0 +1,2 @@
+[tool.black]
+exclude = "cmake"
diff --git a/scripts/robot_pose_publisher b/scripts/robot_pose_publisher
index f742b2ac40e3db646688ff6451b254adfdfb3de2..7fa4410a0548809fb3c42706e522de4ecc4ca89a 100755
--- a/scripts/robot_pose_publisher
+++ b/scripts/robot_pose_publisher
@@ -8,23 +8,26 @@ import rospy
import tf
import sensor_msgs.msg
-frame = ''
-childFrame = ''
+frame = ""
+childFrame = ""
+
-#DEPRECATED. Robot Pose is already being published
def pose_broadcaster(msg):
- translation = msg.position[0:3];
- rotation = tf.transformations.quaternion_from_euler(msg.position[3], msg.position[4], msg.position[5])
+ # DEPRECATED. Robot Pose is already being published
+ translation = msg.position[0:3]
+ rotation = tf.transformations.quaternion_from_euler(
+ msg.position[3], msg.position[4], msg.position[5]
+ )
tfbr = tf.TransformBroadcaster()
- tfbr.sendTransform(translation, rotation,
- rospy.Time.now(), childFrame, frame)
+ tfbr.sendTransform(translation, rotation, rospy.Time.now(), childFrame, frame)
+
+
+if __name__ == "__main__":
+ rospy.init_node("robot_pose_publisher", anonymous=True)
-if __name__ == '__main__':
- rospy.init_node('robot_pose_publisher', anonymous=True)
+ frame = rospy.get_param("~frame", "odom")
+ childFrame = rospy.get_param("~child_frame", "base_link")
+ topic = rospy.get_param("~topic", "joint_states")
- frame = rospy.get_param('~frame', 'odom')
- childFrame = rospy.get_param('~child_frame', 'base_link')
- topic = rospy.get_param('~topic', 'joint_states')
-
rospy.Subscriber(topic, sensor_msgs.msg.JointState, pose_broadcaster)
rospy.spin()
diff --git a/scripts/run_command b/scripts/run_command
index f69b5db1a18962683a39c61fe7667ff92d725498..4e7b7e878592c6838d80ee1762074b1333ca3af6 100755
--- a/scripts/run_command
+++ b/scripts/run_command
@@ -2,7 +2,7 @@
import rospy
-import dynamic_graph
+import dynamic_graph # noqa: F401
import dynamic_graph_bridge_msgs.srv
import sys
import code
@@ -18,7 +18,8 @@ except ImportError:
print("Module readline not available.")
# Enable a History
-HISTFILE="%s/.pyhistory" % os.environ["HOME"]
+HISTFILE = "%s/.pyhistory" % os.environ["HOME"]
+
def savehist():
readline.write_history_file(HISTFILE)
@@ -29,30 +30,31 @@ class RosShell(InteractiveConsole):
self.cache = ""
InteractiveConsole.__init__(self)
- rospy.loginfo('waiting for service...')
- rospy.wait_for_service('run_command')
+ rospy.loginfo("waiting for service...")
+ rospy.wait_for_service("run_command")
self.client = rospy.ServiceProxy(
- 'run_command', dynamic_graph_bridge_msgs.srv.RunCommand, True)
- rospy.wait_for_service('run_script')
+ "run_command", dynamic_graph_bridge_msgs.srv.RunCommand, True
+ )
+ rospy.wait_for_service("run_script")
self.scriptClient = rospy.ServiceProxy(
- 'run_script', dynamic_graph_bridge_msgs.srv.RunPythonFile, True)
-
+ "run_script", dynamic_graph_bridge_msgs.srv.RunPythonFile, True
+ )
+
readline.set_completer(DGCompleter(self.client).complete)
readline.parse_and_bind("tab: complete")
# Read the existing history if there is one
if os.path.exists(HISTFILE):
readline.read_history_file(HISTFILE)
-
+
# Set maximum number of items that will be written to the history file
readline.set_history_length(300)
-
-
+
import atexit
- atexit.register(savehist)
+ atexit.register(savehist)
- def runcode(self, code, retry = True):
+ def runcode(self, code, retry=True):
source = self.cache[:-1]
self.cache = ""
if source != "":
@@ -60,7 +62,8 @@ class RosShell(InteractiveConsole):
if not self.client:
print("Connection to remote server lost. Reconnecting...")
self.client = rospy.ServiceProxy(
- 'run_command', dynamic_graph_bridge_msgs.srv.RunCommand, True)
+ "run_command", dynamic_graph_bridge_msgs.srv.RunCommand, True
+ )
if retry:
print("Retrying previous command...")
self.cache = source
@@ -72,16 +75,17 @@ class RosShell(InteractiveConsole):
print(response.standarderror[:-1])
elif response.result != "None":
print(response.result)
- except rospy.ServiceException as e:
+ except rospy.ServiceException:
print("Connection to remote server lost. Reconnecting...")
self.client = rospy.ServiceProxy(
- 'run_command', dynamic_graph_bridge_msgs.srv.RunCommand, True)
+ "run_command", dynamic_graph_bridge_msgs.srv.RunCommand, True
+ )
if retry:
print("Retrying previous command...")
self.cache = source
self.runcode(code, False)
- def runsource(self, source, filename = '<input>', symbol = 'single'):
+ def runsource(self, source, filename="<input>", symbol="single"):
try:
c = code.compile_command(source, filename, symbol)
if c:
@@ -93,17 +97,18 @@ class RosShell(InteractiveConsole):
self.cache = ""
return False
- def push(self,line):
+ def push(self, line):
self.cache += line + "\n"
- return InteractiveConsole.push(self,line)
+ return InteractiveConsole.push(self, line)
-if __name__ == '__main__':
+
+if __name__ == "__main__":
import optparse
import os.path
- rospy.init_node('run_command', argv=sys.argv)
+
+ rospy.init_node("run_command", argv=sys.argv)
sys.argv = rospy.myargv(argv=None)
- parser = optparse.OptionParser(
- usage='\n\t%prog [options]')
+ parser = optparse.OptionParser(usage="\n\t%prog [options]")
(options, args) = parser.parse_args(sys.argv[1:])
sh = RosShell()
@@ -118,6 +123,6 @@ if __name__ == '__main__':
if not response:
print("Error while file parsing ")
else:
- print("Provided file does not exist: %s"%(infile))
+ print("Provided file does not exist: %s" % (infile))
else:
sh.interact("Interacting with remote server.")
diff --git a/setup.cfg b/setup.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..15656840ec973b675dfc5cea3d568beb55c9cdcc
--- /dev/null
+++ b/setup.cfg
@@ -0,0 +1,4 @@
+[flake8]
+exclude = cmake
+max-line-length = 88
+extend-ignore = E203
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f19f4142aa9b82a6ccdb0dff747a078f77d7463c..30e9d0eb1731c4a4923892d98d60c6a0dd17e8ef 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,64 +1,73 @@
-SET(plugins
- ros_publish
- ros_subscribe
- ros_queued_subscribe
- ros_tf_listener
- ros_time
- )
+set(plugins ros_publish ros_subscribe ros_queued_subscribe ros_tf_listener
+ ros_time)
-FOREACH(plugin ${plugins})
- GET_FILENAME_COMPONENT(LIBRARY_NAME ${plugin} NAME)
- ADD_LIBRARY(${LIBRARY_NAME} SHARED ${plugin}.cpp ${plugin}.hh)
+foreach(plugin ${plugins})
+ get_filename_component(LIBRARY_NAME ${plugin} NAME)
+ add_library(${LIBRARY_NAME} SHARED ${plugin}.cpp ${plugin}.hh)
- IF(SUFFIX_SO_VERSION)
- SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
- ENDIF(SUFFIX_SO_VERSION)
+ if(SUFFIX_SO_VERSION)
+ set_target_properties(${LIBRARY_NAME} PROPERTIES SOVERSION
+ ${PROJECT_VERSION})
+ endif(SUFFIX_SO_VERSION)
- TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${${LIBRARY_NAME}_deps} ${catkin_LIBRARIES} ros_bridge)
+ target_link_libraries(
+ ${LIBRARY_NAME} ${${LIBRARY_NAME}_deps} ${catkin_LIBRARIES} ros_bridge)
- IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
- INSTALL(TARGETS ${LIBRARY_NAME} EXPORT ${TARGETS_EXPORT_NAME}
+ if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ install(
+ TARGETS ${LIBRARY_NAME}
+ EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
- ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
- IF(BUILD_PYTHON_INTERFACE)
- STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
+ if(BUILD_PYTHON_INTERFACE)
+ string(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
- DYNAMIC_GRAPH_PYTHON_MODULE("ros/${PYTHON_LIBRARY_NAME}"
- ${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
- SOURCE_PYTHON_MODULE "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
+ dynamic_graph_python_module(
+ "ros/${PYTHON_LIBRARY_NAME}" ${LIBRARY_NAME}
+ ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap SOURCE_PYTHON_MODULE
+ "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
- DYNAMIC_GRAPH_PYTHON_MODULE("ros/${PYTHON_LIBRARY_NAME}"
- ${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
- MODULE_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
+ dynamic_graph_python_module(
+ "ros/${PYTHON_LIBRARY_NAME}" ${LIBRARY_NAME}
+ ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap MODULE_HEADER
+ "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
endif()
- ENDIF(BUILD_PYTHON_INTERFACE)
-ENDFOREACH(plugin)
+ endif(BUILD_PYTHON_INTERFACE)
+endforeach(plugin)
target_link_libraries(ros_publish ros_bridge)
-IF(BUILD_PYTHON_INTERFACE)
- PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "__init__.py")
- PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "ros.py")
- PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "dgcompleter.py")
+if(BUILD_PYTHON_INTERFACE)
+ python_install_on_site("dynamic_graph/ros" "__init__.py")
+ python_install_on_site("dynamic_graph/ros" "ros.py")
+ python_install_on_site("dynamic_graph/ros" "dgcompleter.py")
# ros_interperter library.
add_library(ros_interpreter ros_interpreter.cpp)
- TARGET_LINK_LIBRARIES(ros_interpreter ros_bridge ${catkin_LIBRARIES}
+ target_link_libraries(
+ ros_interpreter ros_bridge ${catkin_LIBRARIES}
dynamic-graph-python::dynamic-graph-python)
- install(TARGETS ros_interpreter
+ install(
+ TARGETS ros_interpreter
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION lib)
-ENDIF(BUILD_PYTHON_INTERFACE)
+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)
-install(TARGETS geometric_simu
- DESTINATION lib/${PROJECT_NAME})
+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)
+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)
-install(TARGETS sot_loader EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
+target_link_libraries(
+ sot_loader Boost::program_options ${catkin_LIBRARIES} ros_bridge)
+install(
+ TARGETS sot_loader
+ EXPORT ${TARGETS_EXPORT_NAME}
+ DESTINATION lib)
diff --git a/src/converter.hh b/src/converter.hh
index 82034e6732186673d6e93a6f0d95b6574d35b50e..772890340c58eaa2076114c72810d80275065ba1 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -1,22 +1,24 @@
#ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH
#define DYNAMIC_GRAPH_ROS_CONVERTER_HH
-#include <stdexcept>
-#include "sot_to_ros.hh"
+#include <ros/time.h>
+#include <std_msgs/Header.h>
-#include <boost/static_assert.hpp>
#include <boost/date_time/date.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/static_assert.hpp>
+#include <stdexcept>
-#include <ros/time.h>
-#include <std_msgs/Header.h>
+#include "sot_to_ros.hh"
-#define SOT_TO_ROS_IMPL(T) \
- template <> \
- inline void converter(SotToRos<T>::ros_t& dst, const SotToRos<T>::sot_t& src)
+#define SOT_TO_ROS_IMPL(T) \
+ template <> \
+ inline void converter(SotToRos<T>::ros_t& dst, \
+ const SotToRos<T>::sot_t& src)
-#define ROS_TO_SOT_IMPL(T) \
- template <> \
- inline void converter(SotToRos<T>::sot_t& dst, const SotToRos<T>::ros_t& src)
+#define ROS_TO_SOT_IMPL(T) \
+ template <> \
+ inline void converter(SotToRos<T>::sot_t& dst, \
+ const SotToRos<T>::ros_t& src)
namespace dynamicgraph {
inline void makeHeader(std_msgs::Header& header) {
@@ -99,7 +101,8 @@ SOT_TO_ROS_IMPL(Matrix) {
}
ROS_TO_SOT_IMPL(Matrix) {
- dst.resize(src.width, (unsigned int)src.data.size() / (unsigned int)src.width);
+ dst.resize(src.width,
+ (unsigned int)src.data.size() / (unsigned int)src.width);
for (unsigned i = 0; i < src.data.size(); ++i) dst.data()[i] = src.data[i];
}
@@ -117,7 +120,8 @@ SOT_TO_ROS_IMPL(sot::MatrixHomogeneous) {
}
ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
- sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y, src.rotation.z);
+ sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y,
+ src.rotation.z);
dst.linear() = q.matrix();
// Copy the translation component.
@@ -128,7 +132,8 @@ ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
// Twist.
SOT_TO_ROS_IMPL(specific::Twist) {
- if (src.size() != 6) throw std::runtime_error("failed to convert invalid twist");
+ if (src.size() != 6)
+ throw std::runtime_error("failed to convert invalid twist");
dst.linear.x = src(0);
dst.linear.y = src(1);
dst.linear.z = src(2);
@@ -163,7 +168,8 @@ ROS_TO_SOT_IMPL(specific::Twist) {
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
-DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, dst.child_frame_id = "";);
+DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform,
+ dst.child_frame_id = "";);
DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
/// \brief This macro generates a converter for a shared pointer on
@@ -172,11 +178,13 @@ DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
/// A converter for the underlying type is required. I.e. to
/// convert a shared_ptr<T> to T', a converter from T to T'
/// is required.
-#define DG_BRIDGE_MAKE_SHPTR_IMPL(T) \
- template <> \
- inline void converter(SotToRos<T>::sot_t& dst, const boost::shared_ptr<SotToRos<T>::ros_t const>& src) { \
- converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, *src); \
- } \
+#define DG_BRIDGE_MAKE_SHPTR_IMPL(T) \
+ template <> \
+ inline void converter( \
+ SotToRos<T>::sot_t& dst, \
+ const boost::shared_ptr<SotToRos<T>::ros_t const>& src) { \
+ converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, *src); \
+ } \
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
@@ -213,15 +221,17 @@ DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
/// a stamped type. I.e. A data associated with its timestamp.
///
/// FIXME: the timestamp is not yet forwarded to the dg signal.
-#define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA) \
- template <> \
- inline void converter(SotToRos<std::pair<T, Vector> >::sot_t& dst, \
- const boost::shared_ptr<SotToRos<std::pair<T, Vector> >::ros_t const>& src) { \
- converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src->ATTRIBUTE); \
- do { \
- EXTRA \
- } while (0); \
- } \
+#define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA) \
+ template <> \
+ inline void converter( \
+ SotToRos<std::pair<T, Vector> >::sot_t& dst, \
+ const boost::shared_ptr<SotToRos<std::pair<T, Vector> >::ros_t const>& \
+ src) { \
+ converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src->ATTRIBUTE); \
+ do { \
+ EXTRA \
+ } while (0); \
+ } \
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Vector3, vector, ;);
@@ -251,7 +261,8 @@ typedef boost::posix_time::time_duration time_duration;
typedef boost::gregorian::date date;
boost::posix_time::ptime rosTimeToPtime(const ros::Time& rosTime) {
- ptime time(date(1970, 1, 1), seconds(rosTime.sec) + microseconds(rosTime.nsec / 1000));
+ ptime time(date(1970, 1, 1),
+ seconds(rosTime.sec) + microseconds(rosTime.nsec / 1000));
return time;
}
@@ -259,7 +270,8 @@ ros::Time pTimeToRostime(const boost::posix_time::ptime& time) {
static ptime timeStart(date(1970, 1, 1));
time_duration diff = time - timeStart;
- uint32_t sec = (unsigned int)diff.ticks() / (unsigned int)time_duration::rep_type::res_adjust();
+ uint32_t sec = (unsigned int)diff.ticks() /
+ (unsigned int)time_duration::rep_type::res_adjust();
uint32_t nsec = (unsigned int)diff.fractional_seconds();
return ros::Time(sec, nsec);
diff --git a/src/dynamic_graph/ros/dgcompleter.py b/src/dynamic_graph/ros/dgcompleter.py
index ff98b6df8b90c8c557183cdcb99db36a9f7f3077..ad5313df3adb9404545714328a838bde3eb061f6 100644
--- a/src/dynamic_graph/ros/dgcompleter.py
+++ b/src/dynamic_graph/ros/dgcompleter.py
@@ -62,18 +62,18 @@ class DGCompleter:
self.client(astr)
astr = "readline.set_completer(aCompleter.complete)"
self.client(astr)
- astr = "readline.parse_and_bind(\"tab: complete\")"
+ astr = 'readline.parse_and_bind("tab: complete")'
self.client(astr)
def complete(self, text, state):
- """Return the next possible completion for 'text'. readline.parse_and_bind("tab: complete")
-
+ """Return the next possible completion for 'text'.
+ readline.parse_and_bind("tab: complete")
This is called successively with state == 0, 1, 2, ... until it
returns None. The completion should begin with 'text'.
"""
- astr = "aCompleter.complete(\"" + text + "\"," + str(state) + ")"
+ astr = 'aCompleter.complete("' + text + '",' + str(state) + ")"
response = self.client(astr)
res2 = ast.literal_eval(response.result)
return res2
diff --git a/src/dynamic_graph/ros/ros.py b/src/dynamic_graph/ros/ros.py
index 18d00d581771d34c01fa3b23b7c1ba7d621aacd1..9c2fdb13b15cd5ec5063974d242a4a41ff70120c 100644
--- a/src/dynamic_graph/ros/ros.py
+++ b/src/dynamic_graph/ros/ros.py
@@ -12,13 +12,13 @@ class Ros(object):
rosImport = None
rosExport = None
- def __init__(self, robot, suffix=''):
+ def __init__(self, robot, suffix=""):
self.robot = robot
- self.rosPublish = RosPublish('rosPublish{0}'.format(suffix))
- self.rosSubscribe = RosSubscribe('rosSubscribe{0}'.format(suffix))
- self.rosTime = RosTime('rosTime{0}'.format(suffix))
+ self.rosPublish = RosPublish("rosPublish{0}".format(suffix))
+ self.rosSubscribe = RosSubscribe("rosSubscribe{0}".format(suffix))
+ self.rosTime = RosTime("rosTime{0}".format(suffix))
- self.robot.device.after.addSignal('{0}.trigger'.format(self.rosPublish.name))
+ self.robot.device.after.addSignal("{0}.trigger".format(self.rosPublish.name))
# aliases, for retro compatibility
self.rosImport = self.rosPublish
diff --git a/src/geometric_simu.cpp b/src/geometric_simu.cpp
index f6f7225a2de2ae550f4d15f109cbb336af6c22eb..6433f048b2247a81906da5c22f857fc0b16577f3 100644
--- a/src/geometric_simu.cpp
+++ b/src/geometric_simu.cpp
@@ -5,17 +5,19 @@
* CNRS
*
*/
-#include <iostream>
#include <ros/console.h>
+#include <iostream>
+
#define ENABLE_RT_LOG
#include <dynamic-graph/real-time-logger.h>
#include <dynamic_graph_bridge/sot_loader.hh>
int main(int argc, char *argv[]) {
- ::dynamicgraph::RealTimeLogger::instance()
- .addOutputStream(::dynamicgraph::LoggerStreamPtr_t(new dynamicgraph::LoggerIOStream(std::cout)));
+ ::dynamicgraph::RealTimeLogger::instance().addOutputStream(
+ ::dynamicgraph::LoggerStreamPtr_t(
+ new dynamicgraph::LoggerIOStream(std::cout)));
ros::init(argc, argv, "sot_ros_encapsulator");
SotLoader aSotLoader;
@@ -27,6 +29,6 @@ int main(int argc, char *argv[]) {
// Load dynamic library and run python prologue.
aSotLoader.Initialization();
- ros::waitForShutdown ();
+ ros::waitForShutdown();
return 0;
}
diff --git a/src/ros_init.cpp b/src/ros_init.cpp
index 56b868ac69400790253d83907edf22a50b5cfcba..30457ef35ede3457030a238a069d3b695fb798d6 100644
--- a/src/ros_init.cpp
+++ b/src/ros_init.cpp
@@ -1,8 +1,8 @@
-#include <stdexcept>
+#include "dynamic_graph_bridge/ros_init.hh"
+
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
-
-#include "dynamic_graph_bridge/ros_init.hh"
+#include <stdexcept>
namespace dynamicgraph {
struct GlobalRos {
@@ -17,7 +17,8 @@ struct GlobalRos {
};
GlobalRos ros;
-ros::NodeHandle& rosInit(bool createAsyncSpinner, bool createMultiThreadedSpinner) {
+ros::NodeHandle& rosInit(bool createAsyncSpinner,
+ bool createMultiThreadedSpinner) {
if (!ros.nodeHandle) {
int argc = 1;
char* arg0 = strdup("dynamic_graph_bridge");
@@ -34,11 +35,14 @@ ros::NodeHandle& rosInit(bool createAsyncSpinner, bool createMultiThreadedSpinne
// priority
int oldThreadPolicy, newThreadPolicy;
struct sched_param oldThreadParam, newThreadParam;
- if (pthread_getschedparam(pthread_self(), &oldThreadPolicy, &oldThreadParam) == 0) {
+ if (pthread_getschedparam(pthread_self(), &oldThreadPolicy,
+ &oldThreadParam) == 0) {
newThreadPolicy = SCHED_OTHER;
newThreadParam = oldThreadParam;
- newThreadParam.sched_priority -= 5; // Arbitrary number, TODO: choose via param/file?
- if (newThreadParam.sched_priority < sched_get_priority_min(newThreadPolicy))
+ newThreadParam.sched_priority -=
+ 5; // Arbitrary number, TODO: choose via param/file?
+ if (newThreadParam.sched_priority <
+ sched_get_priority_min(newThreadPolicy))
newThreadParam.sched_priority = sched_get_priority_min(newThreadPolicy);
pthread_setschedparam(pthread_self(), newThreadPolicy, &newThreadParam);
diff --git a/src/ros_interpreter.cpp b/src/ros_interpreter.cpp
index 6fb690a6f1e6cca01b056b339eec3746afd739a1..c68186c26535495f95b628861ab5fd121f7eb107 100644
--- a/src/ros_interpreter.cpp
+++ b/src/ros_interpreter.cpp
@@ -4,37 +4,49 @@ namespace dynamicgraph {
static const int queueSize = 5;
Interpreter::Interpreter(ros::NodeHandle& nodeHandle)
- : interpreter_(), nodeHandle_(nodeHandle), runCommandSrv_(), runPythonFileSrv_() {}
+ : interpreter_(),
+ nodeHandle_(nodeHandle),
+ runCommandSrv_(),
+ runPythonFileSrv_() {}
void Interpreter::startRosService() {
- runCommandCallback_t runCommandCb = boost::bind(&Interpreter::runCommandCallback, this, _1, _2);
+ runCommandCallback_t runCommandCb =
+ boost::bind(&Interpreter::runCommandCallback, this, _1, _2);
runCommandSrv_ = nodeHandle_.advertiseService("run_command", runCommandCb);
- runPythonFileCallback_t runPythonFileCb = boost::bind(&Interpreter::runPythonFileCallback, this, _1, _2);
- runPythonFileSrv_ = nodeHandle_.advertiseService("run_script", runPythonFileCb);
+ runPythonFileCallback_t runPythonFileCb =
+ boost::bind(&Interpreter::runPythonFileCallback, this, _1, _2);
+ runPythonFileSrv_ =
+ nodeHandle_.advertiseService("run_script", runPythonFileCb);
}
-bool Interpreter::runCommandCallback(dynamic_graph_bridge_msgs::RunCommand::Request& req,
- dynamic_graph_bridge_msgs::RunCommand::Response& res) {
- interpreter_.python(req.input, res.result, res.standardoutput, res.standarderror);
+bool Interpreter::runCommandCallback(
+ dynamic_graph_bridge_msgs::RunCommand::Request& req,
+ dynamic_graph_bridge_msgs::RunCommand::Response& res) {
+ interpreter_.python(req.input, res.result, res.standardoutput,
+ res.standarderror);
return true;
}
-bool Interpreter::runPythonFileCallback(dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
- dynamic_graph_bridge_msgs::RunPythonFile::Response& res) {
+bool Interpreter::runPythonFileCallback(
+ dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
+ dynamic_graph_bridge_msgs::RunPythonFile::Response& res) {
interpreter_.runPythonFile(req.input);
res.result = "File parsed"; // FIX: It is just an echo, is there a way to
// have a feedback?
return true;
}
-void Interpreter::runCommand(const std::string& command, std::string& result, std::string& out, std::string& err) {
+void Interpreter::runCommand(const std::string& command, std::string& result,
+ std::string& out, std::string& err) {
interpreter_.python(command, result, out, err);
if (err.size() > 0) {
ROS_ERROR(err.c_str());
}
}
-void Interpreter::runPythonFile(std::string ifilename) { interpreter_.runPythonFile(ifilename); }
+void Interpreter::runPythonFile(std::string ifilename) {
+ interpreter_.runPythonFile(ifilename);
+}
} // end of namespace dynamicgraph.
diff --git a/src/ros_parameter.cpp b/src/ros_parameter.cpp
index c7ab7095cbff36936b16d3a322123c884338ff80..1c3f34a6eb1c7c8fe11941b39ab97bac6c4a248f 100644
--- a/src/ros_parameter.cpp
+++ b/src/ros_parameter.cpp
@@ -1,30 +1,30 @@
-#include <sot/core/robot-utils.hh>
+#include <pinocchio/fwd.hpp>
-#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/parsers/urdf.hpp"
+// include pinocchio first
+//
+#include <ros/ros.h>
+#include <urdf_parser/urdf_parser.h>
-#include <stdexcept>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
+#include <sot/core/robot-utils.hh>
+#include <stdexcept>
-#include <urdf_parser/urdf_parser.h>
-
-#include <ros/ros.h>
#include "dynamic_graph_bridge/ros_parameter.hh"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/urdf.hpp"
namespace dynamicgraph {
-bool parameter_server_read_robot_description()
-{
+bool parameter_server_read_robot_description() {
ros::NodeHandle nh;
- if (!nh.hasParam("/robot_description"))
- {
+ if (!nh.hasParam("/robot_description")) {
ROS_ERROR("No /robot_description parameter");
return false;
}
std::string robot_description;
std::string parameter_name("/robot_description");
- nh.getParam(parameter_name,robot_description);
+ nh.getParam(parameter_name, robot_description);
std::string model_name("robot");
@@ -35,11 +35,10 @@ bool parameter_server_read_robot_description()
aRobotUtil = sot::createRobotUtil(model_name);
// If the creation is fine
- if (aRobotUtil != sot::RefVoidRobotUtil())
- {
+ if (aRobotUtil != sot::RefVoidRobotUtil()) {
// Then set the robot model.
- aRobotUtil->set_parameter(parameter_name,robot_description);
- ROS_INFO("Set parameter_name : %s.",parameter_name.c_str());
+ aRobotUtil->set_parameter(parameter_name, robot_description);
+ ROS_INFO("Set parameter_name : %s.", parameter_name.c_str());
// Everything went fine.
return true;
}
@@ -48,7 +47,6 @@ bool parameter_server_read_robot_description()
// Otherwise something went wrong.
return false;
-
}
-}
+} // namespace dynamicgraph
diff --git a/src/ros_publish-python-module-py.cc b/src/ros_publish-python-module-py.cc
index a50e445c4ff6a4bc5a10250ad3f8ef9d67f675cd..cbe08b4fcb828e0aa82d93d41d5f333d5711aaca 100644
--- a/src/ros_publish-python-module-py.cc
+++ b/src/ros_publish-python-module-py.cc
@@ -1,17 +1,19 @@
#include <dynamic-graph/python/module.hh>
+
#include "ros_publish.hh"
namespace dg = dynamicgraph;
-
-BOOST_PYTHON_MODULE(wrap)
-{
+BOOST_PYTHON_MODULE(wrap) {
bp::import("dynamic_graph");
- dg::python::exposeEntity<dg::RosPublish, bp::bases<dg::Entity>, dg::python::AddCommands>()
- .def("clear", &dg::RosPublish::clear, "Remove all signals writing data to a ROS topic")
- .def("rm", &dg::RosPublish::rm, "Remove a signal writing data to a ROS topic",
- bp::args("signal_name"))
- .def("list", &dg::RosPublish::list, "List signals writing data to a ROS topic")
- ;
+ dg::python::exposeEntity<dg::RosPublish, bp::bases<dg::Entity>,
+ dg::python::AddCommands>()
+ .def("clear", &dg::RosPublish::clear,
+ "Remove all signals writing data to a ROS topic")
+ .def("rm", &dg::RosPublish::rm,
+ "Remove a signal writing data to a ROS topic",
+ bp::args("signal_name"))
+ .def("list", &dg::RosPublish::list,
+ "List signals writing data to a ROS topic");
}
diff --git a/src/ros_publish.cpp b/src/ros_publish.cpp
index 407d7145b366ad1427f6bf5ab74e181a84e7d65f..41c66addd018119b79517730785e562899b35103 100644
--- a/src/ros_publish.cpp
+++ b/src/ros_publish.cpp
@@ -1,4 +1,11 @@
-#include <stdexcept>
+#include "ros_publish.hh"
+
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/linear-algebra.h>
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/UInt32.h>
#include <boost/assign.hpp>
#include <boost/bind.hpp>
@@ -6,17 +13,9 @@
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
-
-#include <ros/ros.h>
-#include <std_msgs/Float64.h>
-#include <std_msgs/UInt32.h>
-
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/command.h>
-#include <dynamic-graph/linear-algebra.h>
+#include <stdexcept>
#include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_publish.hh"
#define ENABLE_RT_LOG
#include <dynamic-graph/real-time-logger.h>
@@ -30,7 +29,10 @@ namespace command {
namespace rosPublish {
Add::Add(RosPublish& entity, const std::string& docstring)
- : Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), docstring) {}
+ : Command(
+ entity,
+ boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
+ docstring) {}
Value Add::doExecute() {
RosPublish& entity = static_cast<RosPublish&>(owner());
@@ -98,7 +100,8 @@ RosPublish::RosPublish(const std::string& n)
clock_gettime(CLOCK_REALTIME, &nextPublicationRT_);
}
} catch (const std::exception& exc) {
- throw std::runtime_error("Failed to call ros::Time::now ():" + std::string(exc.what()));
+ throw std::runtime_error("Failed to call ros::Time::now ():" +
+ std::string(exc.what()));
}
signalRegistration(trigger_);
trigger_.setNeedUpdateFromAllChildren(true);
@@ -120,13 +123,16 @@ RosPublish::RosPublish(const std::string& n)
RosPublish::~RosPublish() { aofs_.close(); }
-void RosPublish::display(std::ostream& os) const { os << CLASS_NAME << std::endl; }
+void RosPublish::display(std::ostream& os) const {
+ os << CLASS_NAME << std::endl;
+}
void RosPublish::rm(const std::string& signal) {
if (bindedSignal_.find(signal) == bindedSignal_.end()) return;
if (signal == "trigger") {
- std::cerr << "The trigger signal should not be removed. Aborting." << std::endl;
+ std::cerr << "The trigger signal should not be removed. Aborting."
+ << std::endl;
return;
}
@@ -139,8 +145,8 @@ void RosPublish::rm(const std::string& signal) {
std::vector<std::string> RosPublish::list() const {
std::vector<std::string> result(bindedSignal_.size());
- std::transform(bindedSignal_.begin(), bindedSignal_.end(),
- result.begin(), [](const auto& pair) { return pair.first; });
+ std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
+ [](const auto& pair) { return pair.first; });
return result;
}
diff --git a/src/ros_publish.hh b/src/ros_publish.hh
index 31b8cceede21b5c53fa6bbef15ee3f55678db4f4..106ec9ec79135df1ba8757b2486cbaa58ad377fc 100644
--- a/src/ros_publish.hh
+++ b/src/ros_publish.hh
@@ -1,22 +1,19 @@
#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HH
#define DYNAMIC_GRAPH_ROS_PUBLISH_HH
-#include <map>
-
-#include <boost/shared_ptr.hpp>
-#include <boost/tuple/tuple.hpp>
-#include <boost/thread/mutex.hpp>
-
+#include <dynamic-graph/command.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/command.h>
-
+#include <realtime_tools/realtime_publisher.h>
#include <ros/ros.h>
-#include <realtime_tools/realtime_publisher.h>
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <fstream>
+#include <map>
#include "converter.hh"
#include "sot_to_ros.hh"
-#include <fstream>
namespace dynamicgraph {
class RosPublish;
@@ -47,7 +44,9 @@ class RosPublish : public dynamicgraph::Entity {
public:
typedef boost::function<void(int)> callback_t;
- typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >, callback_t> bindedSignal_t;
+ typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+ callback_t>
+ bindedSignal_t;
static const double ROS_JOINT_STATE_PUBLISHER_RATE;
@@ -65,8 +64,11 @@ class RosPublish : public dynamicgraph::Entity {
int& trigger(int&, int);
template <typename T>
- void sendData(boost::shared_ptr<realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> > publisher,
- boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time);
+ void sendData(
+ boost::shared_ptr<
+ realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> >
+ publisher,
+ boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time);
template <typename T>
void add(const std::string& signal, const std::string& topic);
diff --git a/src/ros_publish.hxx b/src/ros_publish.hxx
index 026b8595a2650347f37b38fca32287747044b5e6..6534003242134eafae3ba3e1ad6a834d62bdbe3f 100644
--- a/src/ros_publish.hxx
+++ b/src/ros_publish.hxx
@@ -1,19 +1,23 @@
#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HXX
#define DYNAMIC_GRAPH_ROS_PUBLISH_HXX
-#include <vector>
#include <std_msgs/Float64.h>
+#include <vector>
+
#include "dynamic_graph_bridge_msgs/Matrix.h"
#include "dynamic_graph_bridge_msgs/Vector.h"
-
#include "sot_to_ros.hh"
namespace dynamicgraph {
template <>
inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >(
- boost::shared_ptr<realtime_tools::RealtimePublisher<SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> >
+ boost::shared_ptr<realtime_tools::RealtimePublisher<
+ SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> >
publisher,
- boost::shared_ptr<SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t> signal, int time) {
+ boost::shared_ptr<
+ SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t>
+ signal,
+ int time) {
SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t result;
if (publisher->trylock()) {
publisher->msg_.child_frame_id = "/dynamic_graph/world";
@@ -23,8 +27,11 @@ inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >(
}
template <typename T>
-void RosPublish::sendData(boost::shared_ptr<realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> > publisher,
- boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time) {
+void RosPublish::sendData(
+ boost::shared_ptr<
+ realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> >
+ publisher,
+ boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time) {
typename SotToRos<T>::ros_t result;
if (publisher->trylock()) {
converter(publisher->msg_, signal->access(time));
@@ -42,17 +49,19 @@ void RosPublish::add(const std::string& signal, const std::string& topic) {
// Initialize the publisher.
boost::shared_ptr<realtime_tools::RealtimePublisher<ros_t> > pubPtr =
- boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >(nh_, topic, 1);
+ boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >(nh_, topic,
+ 1);
// Initialize the signal.
- boost::shared_ptr<signal_t> signalPtr(
- new signal_t(0, MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
+ boost::shared_ptr<signal_t> signalPtr(new signal_t(
+ 0, MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
boost::get<0>(bindedSignal) = signalPtr;
SotToRos<T>::setDefault(*signalPtr);
signalRegistration(*boost::get<0>(bindedSignal));
// Initialize the callback.
- callback_t callback = boost::bind(&RosPublish::sendData<T>, this, pubPtr, signalPtr, _1);
+ callback_t callback =
+ boost::bind(&RosPublish::sendData<T>, this, pubPtr, signalPtr, _1);
boost::get<1>(bindedSignal) = callback;
bindedSignal_[signal] = bindedSignal;
diff --git a/src/ros_queued_subscribe-python-module-py.cc b/src/ros_queued_subscribe-python-module-py.cc
index c14f8e638b81c54838b43bdc5f07468d3cc268e7..f5c9632bb31da513d2874d5ba52a47c3c3e43721 100644
--- a/src/ros_queued_subscribe-python-module-py.cc
+++ b/src/ros_queued_subscribe-python-module-py.cc
@@ -1,25 +1,28 @@
#include <dynamic-graph/python/module.hh>
+
#include "ros_queued_subscribe.hh"
namespace dg = dynamicgraph;
-
-BOOST_PYTHON_MODULE(wrap)
-{
+BOOST_PYTHON_MODULE(wrap) {
bp::import("dynamic_graph");
- dg::python::exposeEntity<dg::RosQueuedSubscribe, bp::bases<dg::Entity>, dg::python::AddCommands>()
- .def("clear", &dg::RosQueuedSubscribe::clear, "Remove all signals reading data from a ROS topic")
- .def("rm", &dg::RosQueuedSubscribe::rm, "Remove a signal reading data from a ROS topic",
- bp::args("signal_name"))
- .def("list", &dg::RosQueuedSubscribe::list, "List signals reading data from a ROS topic")
- .def("listTopics", &dg::RosQueuedSubscribe::listTopics, "List subscribed topics from ROS in the same order as list command")
- .def("clearQueue", &dg::RosQueuedSubscribe::clearQueue,
- "Empty the queue of a given signal", bp::args("signal_name"))
- .def("queueSize", &dg::RosQueuedSubscribe::queueSize,
- "Return the queue size of a given signal", bp::args("signal_name"))
- .def("readQueue", &dg::RosQueuedSubscribe::readQueue,
- "Whether signals should read values from the queues, and when.",
- bp::args("start_time"))
- ;
+ dg::python::exposeEntity<dg::RosQueuedSubscribe, bp::bases<dg::Entity>,
+ dg::python::AddCommands>()
+ .def("clear", &dg::RosQueuedSubscribe::clear,
+ "Remove all signals reading data from a ROS topic")
+ .def("rm", &dg::RosQueuedSubscribe::rm,
+ "Remove a signal reading data from a ROS topic",
+ bp::args("signal_name"))
+ .def("list", &dg::RosQueuedSubscribe::list,
+ "List signals reading data from a ROS topic")
+ .def("listTopics", &dg::RosQueuedSubscribe::listTopics,
+ "List subscribed topics from ROS in the same order as list command")
+ .def("clearQueue", &dg::RosQueuedSubscribe::clearQueue,
+ "Empty the queue of a given signal", bp::args("signal_name"))
+ .def("queueSize", &dg::RosQueuedSubscribe::queueSize,
+ "Return the queue size of a given signal", bp::args("signal_name"))
+ .def("readQueue", &dg::RosQueuedSubscribe::readQueue,
+ "Whether signals should read values from the queues, and when.",
+ bp::args("start_time"));
}
diff --git a/src/ros_queued_subscribe.cpp b/src/ros_queued_subscribe.cpp
index f5226afcd62678387ed7f222c7c9b633b3b5d4ef..d67f69d997da48c960ef5428fa4ee7d8a66ce019 100644
--- a/src/ros_queued_subscribe.cpp
+++ b/src/ros_queued_subscribe.cpp
@@ -4,20 +4,20 @@
//
//
-#include <boost/assign.hpp>
-#include <boost/bind.hpp>
-#include <boost/format.hpp>
-#include <boost/function.hpp>
-#include <boost/make_shared.hpp>
+#include "ros_queued_subscribe.hh"
+#include <dynamic-graph/factory.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/UInt32.h>
-#include <dynamic-graph/factory.h>
+#include <boost/assign.hpp>
+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+#include <boost/function.hpp>
+#include <boost/make_shared.hpp>
#include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_queued_subscribe.hh"
namespace dynamicgraph {
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosQueuedSubscribe, "RosQueuedSubscribe");
@@ -25,7 +25,10 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosQueuedSubscribe, "RosQueuedSubscribe");
namespace command {
namespace rosQueuedSubscribe {
Add::Add(RosQueuedSubscribe& entity, const std::string& docstring)
- : Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), docstring) {}
+ : Command(
+ entity,
+ boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
+ docstring) {}
Value Add::doExecute() {
RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
@@ -62,7 +65,10 @@ const std::string RosQueuedSubscribe::docstring_(
" Use command \"add\" to subscribe to a new signal.\n");
RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n)
- : dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_(), readQueue_(-1) {
+ : dynamicgraph::Entity(n),
+ nh_(rosInit(true)),
+ bindedSignal_(),
+ readQueue_(-1) {
std::string docstring =
"\n"
" Add a signal reading data from a ROS topic\n"
@@ -78,7 +84,9 @@ RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n)
RosQueuedSubscribe::~RosQueuedSubscribe() {}
-void RosQueuedSubscribe::display(std::ostream& os) const { os << CLASS_NAME << std::endl; }
+void RosQueuedSubscribe::display(std::ostream& os) const {
+ os << CLASS_NAME << std::endl;
+}
void RosQueuedSubscribe::rm(const std::string& signal) {
std::string signalTs = signal + "Timestamp";
@@ -94,16 +102,15 @@ void RosQueuedSubscribe::rm(const std::string& signal) {
std::vector<std::string> RosQueuedSubscribe::list() {
std::vector<std::string> result(bindedSignal_.size());
- std::transform(bindedSignal_.begin(), bindedSignal_.end(),
- result.begin(), [](const auto& pair) { return pair.first; });
+ std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
+ [](const auto& pair) { return pair.first; });
return result;
}
-std::vector<std::string> RosQueuedSubscribe::listTopics()
-{
+std::vector<std::string> RosQueuedSubscribe::listTopics() {
std::vector<std::string> result(topics_.size());
- std::transform(topics_.begin(), topics_.end(),
- result.begin(), [](const auto& pair) { return pair.second; });
+ std::transform(topics_.begin(), topics_.end(), result.begin(),
+ [](const auto& pair) { return pair.second; });
return result;
}
@@ -122,7 +129,8 @@ void RosQueuedSubscribe::clearQueue(const std::string& signal) {
}
std::size_t RosQueuedSubscribe::queueSize(const std::string& signal) const {
- std::map<std::string, bindedSignal_t>::const_iterator _bs = bindedSignal_.find(signal);
+ std::map<std::string, bindedSignal_t>::const_iterator _bs =
+ bindedSignal_.find(signal);
if (_bs != bindedSignal_.end()) {
return _bs->second->size();
}
diff --git a/src/ros_queued_subscribe.hh b/src/ros_queued_subscribe.hh
index a54b6831ad6c59a1d1e119c233ca6b62fd78dfee..a97e9a211086c4a6679565c7a699dc91559617d8 100644
--- a/src/ros_queued_subscribe.hh
+++ b/src/ros_queued_subscribe.hh
@@ -6,19 +6,17 @@
#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
-#include <map>
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <ros/ros.h>
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
-
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/signal-ptr.h>
-#include <dynamic-graph/command.h>
+#include <map>
#include <sot/core/matrix-geometry.hh>
-#include <ros/ros.h>
-
#include "converter.hh"
#include "sot_to_ros.hh"
@@ -71,7 +69,11 @@ struct BindedSignal : BindedSignalBase {
typedef typename buffer_t::size_type size_type;
BindedSignal(RosQueuedSubscribe* e)
- : BindedSignalBase(e), frontIdx(0), backIdx(0), buffer(BufferSize), init(false) {}
+ : BindedSignalBase(e),
+ frontIdx(0),
+ backIdx(0),
+ buffer(BufferSize),
+ init(false) {}
~BindedSignal() {
signal.reset();
clear();
@@ -144,18 +146,24 @@ class RosQueuedSubscribe : public dynamicgraph::Entity {
std::size_t queueSize(const std::string& signal) const;
template <typename T>
- void add(const std::string& type, const std::string& signal, const std::string& topic);
+ void add(const std::string& type, const std::string& signal,
+ const std::string& topic);
- std::map<std::string, bindedSignal_t>& bindedSignal() { return bindedSignal_; }
+ std::map<std::string, bindedSignal_t>& bindedSignal() {
+ return bindedSignal_;
+ }
std::map<std::string, std::string>& topics() { return topics_; }
ros::NodeHandle& nh() { return nh_; }
template <typename R, typename S>
- void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data);
+ void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
+ const R& data);
template <typename R>
- void callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data);
+ void callbackTimestamp(
+ boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
+ const R& data);
template <typename T>
friend class internal::Add;
diff --git a/src/ros_queued_subscribe.hxx b/src/ros_queued_subscribe.hxx
index f8994a248ab3ab39cbb854d692f10177b934c081..0a00b6484b1d76d9c34c8e809bbd04f2883045b2 100644
--- a/src/ros_queued_subscribe.hxx
+++ b/src/ros_queued_subscribe.hxx
@@ -8,14 +8,16 @@
#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
#define ENABLE_RT_LOG
-#include <vector>
-#include <boost/bind.hpp>
-#include <boost/date_time/posix_time/posix_time.hpp>
-#include <dynamic-graph/real-time-logger.h>
-#include <dynamic-graph/signal-caster.h>
#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/real-time-logger.h>
#include <dynamic-graph/signal-cast-helper.h>
+#include <dynamic-graph/signal-caster.h>
#include <std_msgs/Float64.h>
+
+#include <boost/bind.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <vector>
+
#include "dynamic_graph_bridge_msgs/Matrix.h"
#include "dynamic_graph_bridge_msgs/Vector.h"
@@ -25,8 +27,8 @@ static const int BUFFER_SIZE = 1 << 10;
template <typename T>
struct Add {
- void operator()(RosQueuedSubscribe& rosSubscribe, const std::string& type, const std::string& signal,
- const std::string& topic) {
+ void operator()(RosQueuedSubscribe& rosSubscribe, const std::string& type,
+ const std::string& signal, const std::string& topic) {
typedef typename SotToRos<T>::sot_t sot_t;
typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
typedef BindedSignal<sot_t, BUFFER_SIZE> BindedSignal_t;
@@ -46,11 +48,13 @@ struct Add {
// Initialize the subscriber.
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
- callback_t callback = boost::bind(&BindedSignal_t::template writer<ros_const_ptr_t>, bs, _1);
+ callback_t callback =
+ boost::bind(&BindedSignal_t::template writer<ros_const_ptr_t>, bs, _1);
// Keep 50 messages in queue, but only 20 are sent every 100ms
// -> No message should be lost because of a full buffer
- bs->subscriber = boost::make_shared<ros::Subscriber>(rosSubscribe.nh().subscribe(topic, BUFFER_SIZE, callback));
+ bs->subscriber = boost::make_shared<ros::Subscriber>(
+ rosSubscribe.nh().subscribe(topic, BUFFER_SIZE, callback));
RosQueuedSubscribe::bindedSignal_t bindedSignal(bs);
rosSubscribe.bindedSignal()[signal] = bindedSignal;
@@ -86,7 +90,8 @@ T& BindedSignal<T, N>::reader(T& data, int time) {
// synchronize with method clear:
// If reading from the list cannot be done, then return last value.
boost::mutex::scoped_lock lock(rmutex, boost::try_to_lock);
- if (!lock.owns_lock() || entity->readQueue_ == -1 || time < entity->readQueue_) {
+ if (!lock.owns_lock() || entity->readQueue_ == -1 ||
+ time < entity->readQueue_) {
data = last;
} else {
if (empty())
@@ -102,7 +107,8 @@ T& BindedSignal<T, N>::reader(T& data, int time) {
} // end of namespace internal.
template <typename T>
-void RosQueuedSubscribe::add(const std::string& type, const std::string& signal, const std::string& topic) {
+void RosQueuedSubscribe::add(const std::string& type, const std::string& signal,
+ const std::string& topic) {
internal::Add<T>()(*this, type, signal, topic);
}
} // end of namespace dynamicgraph.
diff --git a/src/ros_subscribe-python-module-py.cc b/src/ros_subscribe-python-module-py.cc
index cbe164e5b46937607d1787c6e60913f26cbadcdb..b5e637a68012e10fe31ab614eb05b76342348c99 100644
--- a/src/ros_subscribe-python-module-py.cc
+++ b/src/ros_subscribe-python-module-py.cc
@@ -1,17 +1,19 @@
#include <dynamic-graph/python/module.hh>
+
#include "ros_subscribe.hh"
namespace dg = dynamicgraph;
-
-BOOST_PYTHON_MODULE(wrap)
-{
+BOOST_PYTHON_MODULE(wrap) {
bp::import("dynamic_graph");
- dg::python::exposeEntity<dg::RosSubscribe, bp::bases<dg::Entity>, dg::python::AddCommands>()
- .def("clear", &dg::RosSubscribe::clear, "Remove all signals reading data from a ROS topic")
- .def("rm", &dg::RosSubscribe::rm, "Remove a signal reading data from a ROS topic",
- bp::args("signal_name"))
- .def("list", &dg::RosSubscribe::list, "List signals reading data from a ROS topic")
- ;
+ dg::python::exposeEntity<dg::RosSubscribe, bp::bases<dg::Entity>,
+ dg::python::AddCommands>()
+ .def("clear", &dg::RosSubscribe::clear,
+ "Remove all signals reading data from a ROS topic")
+ .def("rm", &dg::RosSubscribe::rm,
+ "Remove a signal reading data from a ROS topic",
+ bp::args("signal_name"))
+ .def("list", &dg::RosSubscribe::list,
+ "List signals reading data from a ROS topic");
}
diff --git a/src/ros_subscribe.cpp b/src/ros_subscribe.cpp
index cfa01a201002db721842351a21227276b8404b27..0c9374e62c60957734dd887d097ef9969943eb3c 100644
--- a/src/ros_subscribe.cpp
+++ b/src/ros_subscribe.cpp
@@ -1,17 +1,17 @@
-#include <boost/assign.hpp>
-#include <boost/bind.hpp>
-#include <boost/format.hpp>
-#include <boost/function.hpp>
-#include <boost/make_shared.hpp>
+#include "ros_subscribe.hh"
+#include <dynamic-graph/factory.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/UInt32.h>
-#include <dynamic-graph/factory.h>
+#include <boost/assign.hpp>
+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+#include <boost/function.hpp>
+#include <boost/make_shared.hpp>
#include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_subscribe.hh"
namespace dynamicgraph {
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
@@ -19,7 +19,10 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
namespace command {
namespace rosSubscribe {
Add::Add(RosSubscribe& entity, const std::string& docstring)
- : Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), docstring) {}
+ : Command(
+ entity,
+ boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
+ docstring) {}
Value Add::doExecute() {
RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
@@ -63,7 +66,8 @@ const std::string RosSubscribe::docstring_(
"\n"
" Use command \"add\" to subscribe to a new signal.\n");
-RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_() {
+RosSubscribe::RosSubscribe(const std::string& n)
+ : dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_() {
std::string docstring =
"\n"
" Add a signal reading data from a ROS topic\n"
@@ -81,7 +85,9 @@ RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n), nh_(
RosSubscribe::~RosSubscribe() {}
-void RosSubscribe::display(std::ostream& os) const { os << CLASS_NAME << std::endl; }
+void RosSubscribe::display(std::ostream& os) const {
+ os << CLASS_NAME << std::endl;
+}
void RosSubscribe::rm(const std::string& signal) {
std::string signalTs = signal + "Timestamp";
@@ -97,8 +103,8 @@ void RosSubscribe::rm(const std::string& signal) {
std::vector<std::string> RosSubscribe::list() {
std::vector<std::string> result(bindedSignal_.size());
- std::transform(bindedSignal_.begin(), bindedSignal_.end(),
- result.begin(), [](const auto& pair) { return pair.first; });
+ std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
+ [](const auto& pair) { return pair.first; });
return result;
}
diff --git a/src/ros_subscribe.hh b/src/ros_subscribe.hh
index 1928e486bb4ec60793811b5228208b6d5be912bb..3b3bb88e9b0a3d401e14f52461f7ca56ec15d756 100644
--- a/src/ros_subscribe.hh
+++ b/src/ros_subscribe.hh
@@ -1,17 +1,15 @@
#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
#define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
-#include <map>
-
-#include <boost/shared_ptr.hpp>
-
+#include <dynamic-graph/command.h>
#include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/signal-ptr.h>
-#include <dynamic-graph/command.h>
-#include <sot/core/matrix-geometry.hh>
-
+#include <dynamic-graph/signal-time-dependent.h>
#include <ros/ros.h>
+#include <boost/shared_ptr.hpp>
+#include <map>
+#include <sot/core/matrix-geometry.hh>
+
#include "converter.hh"
#include "sot_to_ros.hh"
@@ -48,7 +46,8 @@ class RosSubscribe : public dynamicgraph::Entity {
typedef boost::posix_time::ptime ptime;
public:
- typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >, boost::shared_ptr<ros::Subscriber> >
+ typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+ boost::shared_ptr<ros::Subscriber> >
bindedSignal_t;
RosSubscribe(const std::string& n);
@@ -65,15 +64,20 @@ class RosSubscribe : public dynamicgraph::Entity {
template <typename T>
void add(const std::string& signal, const std::string& topic);
- std::map<std::string, bindedSignal_t>& bindedSignal() { return bindedSignal_; }
+ std::map<std::string, bindedSignal_t>& bindedSignal() {
+ return bindedSignal_;
+ }
ros::NodeHandle& nh() { return nh_; }
template <typename R, typename S>
- void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data);
+ void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
+ const R& data);
template <typename R>
- void callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data);
+ void callbackTimestamp(
+ boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
+ const R& data);
template <typename T>
friend class internal::Add;
diff --git a/src/ros_subscribe.hxx b/src/ros_subscribe.hxx
index b3467d42f77b4dd6c7dc5332d0163fe61c5e8dec..6e322615909259a23e2d5094a22e6e2e4a3264a1 100644
--- a/src/ros_subscribe.hxx
+++ b/src/ros_subscribe.hxx
@@ -1,12 +1,14 @@
#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
#define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
-#include <vector>
-#include <boost/bind.hpp>
-#include <boost/date_time/posix_time/posix_time.hpp>
-#include <dynamic-graph/signal-caster.h>
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-cast-helper.h>
+#include <dynamic-graph/signal-caster.h>
#include <std_msgs/Float64.h>
+
+#include <boost/bind.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <vector>
+
#include "dynamic_graph_bridge_msgs/Matrix.h"
#include "dynamic_graph_bridge_msgs/Vector.h"
#include "ros_time.hh"
@@ -15,7 +17,8 @@ namespace dg = dynamicgraph;
namespace dynamicgraph {
template <typename R, typename S>
-void RosSubscribe::callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data) {
+void RosSubscribe::callback(
+ boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data) {
typedef S sot_t;
sot_t value;
converter(value, data);
@@ -23,7 +26,9 @@ void RosSubscribe::callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> >
}
template <typename R>
-void RosSubscribe::callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data) {
+void RosSubscribe::callbackTimestamp(
+ boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
+ const R& data) {
ptime time = rosTimeToPtime(data->header.stamp);
signal->setConstant(time);
}
@@ -31,7 +36,8 @@ void RosSubscribe::callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<p
namespace internal {
template <typename T>
struct Add {
- void operator()(RosSubscribe& RosSubscribe, const std::string& signal, const std::string& topic) {
+ void operator()(RosSubscribe& RosSubscribe, const std::string& signal,
+ const std::string& topic) {
typedef typename SotToRos<T>::sot_t sot_t;
typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
typedef typename SotToRos<T>::signalIn_t signal_t;
@@ -50,9 +56,12 @@ struct Add {
// Initialize the subscriber.
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
- callback_t callback = boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>, &RosSubscribe, signal_, _1);
+ callback_t callback =
+ boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>,
+ &RosSubscribe, signal_, _1);
- bindedSignal.second = boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callback));
+ bindedSignal.second = boost::make_shared<ros::Subscriber>(
+ RosSubscribe.nh().subscribe(topic, 1, callback));
RosSubscribe.bindedSignal()[signal] = bindedSignal;
}
@@ -60,7 +69,8 @@ struct Add {
template <typename T>
struct Add<std::pair<T, dg::Vector> > {
- void operator()(RosSubscribe& RosSubscribe, const std::string& signal, const std::string& topic) {
+ void operator()(RosSubscribe& RosSubscribe, const std::string& signal,
+ const std::string& topic) {
typedef std::pair<T, dg::Vector> type_t;
typedef typename SotToRos<type_t>::sot_t sot_t;
@@ -81,15 +91,19 @@ struct Add<std::pair<T, dg::Vector> > {
// Initialize the publisher.
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
- callback_t callback = boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>, &RosSubscribe, signal_, _1);
+ callback_t callback =
+ boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>,
+ &RosSubscribe, signal_, _1);
- bindedSignal.second = boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callback));
+ bindedSignal.second = boost::make_shared<ros::Subscriber>(
+ RosSubscribe.nh().subscribe(topic, 1, callback));
RosSubscribe.bindedSignal()[signal] = bindedSignal;
// Timestamp.
typedef dynamicgraph::SignalPtr<RosSubscribe::ptime, int> signalTimestamp_t;
- std::string signalTimestamp = (boost::format("%1%%2%") % signal % "Timestamp").str();
+ std::string signalTimestamp =
+ (boost::format("%1%%2%") % signal % "Timestamp").str();
// Initialize the bindedSignal object.
RosSubscribe::bindedSignal_t bindedSignalTimestamp;
@@ -98,7 +112,8 @@ struct Add<std::pair<T, dg::Vector> > {
boost::format signalNameTimestamp("RosSubscribe(%1%)::%2%");
signalNameTimestamp % RosSubscribe.name % signalTimestamp;
- boost::shared_ptr<signalTimestamp_t> signalTimestamp_(new signalTimestamp_t(0, signalNameTimestamp.str()));
+ boost::shared_ptr<signalTimestamp_t> signalTimestamp_(
+ new signalTimestamp_t(0, signalNameTimestamp.str()));
RosSubscribe::ptime zero(rosTimeToPtime(ros::Time(0, 0)));
signalTimestamp_->setConstant(zero);
@@ -108,10 +123,11 @@ struct Add<std::pair<T, dg::Vector> > {
// Initialize the publisher.
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
callback_t callbackTimestamp =
- boost::bind(&RosSubscribe::callbackTimestamp<ros_const_ptr_t>, &RosSubscribe, signalTimestamp_, _1);
+ boost::bind(&RosSubscribe::callbackTimestamp<ros_const_ptr_t>,
+ &RosSubscribe, signalTimestamp_, _1);
- bindedSignalTimestamp.second =
- boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp));
+ bindedSignalTimestamp.second = boost::make_shared<ros::Subscriber>(
+ RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp));
RosSubscribe.bindedSignal()[signalTimestamp] = bindedSignalTimestamp;
}
diff --git a/src/ros_tf_listener-python-module-py.cc b/src/ros_tf_listener-python-module-py.cc
index 3815f4530df6fe81bc1f273b3bdc777dca061f6a..0678d9844644a23ac7a8f9bc1585a90a3c5ae1eb 100644
--- a/src/ros_tf_listener-python-module-py.cc
+++ b/src/ros_tf_listener-python-module-py.cc
@@ -1,18 +1,18 @@
#include <dynamic-graph/python/module.hh>
+
#include "ros_tf_listener.hh"
namespace dg = dynamicgraph;
-BOOST_PYTHON_MODULE(wrap)
-{
+BOOST_PYTHON_MODULE(wrap) {
bp::import("dynamic_graph");
- dg::python::exposeEntity<dg::RosTfListener, bp::bases<dg::Entity>, dg::python::AddCommands>()
- .def("add", &dg::RosTfListener::add,
- "Add a signal containing the transform between two frames.",
- bp::args( "to_frame_name", "from_frame_name", "out_signal_name"))
- .def("setMaximumDelay", &dg::RosTfListener::setMaximumDelay,
- "Set the maximum time delay of the transform obtained from tf.",
- bp::args("signal_name", "delay_seconds"))
- ;
+ dg::python::exposeEntity<dg::RosTfListener, bp::bases<dg::Entity>,
+ dg::python::AddCommands>()
+ .def("add", &dg::RosTfListener::add,
+ "Add a signal containing the transform between two frames.",
+ bp::args("to_frame_name", "from_frame_name", "out_signal_name"))
+ .def("setMaximumDelay", &dg::RosTfListener::setMaximumDelay,
+ "Set the maximum time delay of the transform obtained from tf.",
+ bp::args("signal_name", "delay_seconds"));
}
diff --git a/src/ros_tf_listener.cpp b/src/ros_tf_listener.cpp
index 5c7438bb688cb36d9f88e059d1418b0cc3077bb7..68e94c924b20099ed6d137d61fc54e888b631f44 100644
--- a/src/ros_tf_listener.cpp
+++ b/src/ros_tf_listener.cpp
@@ -1,13 +1,15 @@
-#include <pinocchio/fwd.hpp>
-#include "dynamic_graph_bridge/ros_init.hh"
#include "ros_tf_listener.hh"
#include <dynamic-graph/factory.h>
+#include <pinocchio/fwd.hpp>
+
+#include "dynamic_graph_bridge/ros_init.hh"
+
namespace dynamicgraph {
namespace internal {
-sot::MatrixHomogeneous& TransformListenerData::getTransform(sot::MatrixHomogeneous& res, int time)
-{
+sot::MatrixHomogeneous& TransformListenerData::getTransform(
+ sot::MatrixHomogeneous& res, int time) {
availableSig.recompute(time);
bool available = availableSig.accessCopy();
@@ -18,17 +20,16 @@ sot::MatrixHomogeneous& TransformListenerData::getTransform(sot::MatrixHomogeneo
return res;
}
- const geometry_msgs::TransformStamped::_transform_type::_rotation_type&
- quat = transform.transform.rotation;
+ const geometry_msgs::TransformStamped::_transform_type::_rotation_type& quat =
+ transform.transform.rotation;
const geometry_msgs::TransformStamped::_transform_type::_translation_type&
- trans = transform.transform.translation;
+ trans = transform.transform.translation;
res.linear() = sot::Quaternion(quat.w, quat.x, quat.y, quat.z).matrix();
res.translation() << trans.x, trans.y, trans.z;
return res;
}
-bool& TransformListenerData::isAvailable(bool& available, int time)
-{
+bool& TransformListenerData::isAvailable(bool& available, int time) {
static const ros::Time origin(0);
available = false;
ros::Duration elapsed;
@@ -47,13 +48,13 @@ bool& TransformListenerData::isAvailable(bool& available, int time)
if (!available) {
std::ostringstream oss;
oss << "Use failback " << signal.getName() << " at time " << time
- << ". Time since last update of the transform: " << elapsed;
+ << ". Time since last update of the transform: " << elapsed;
entity->SEND_INFO_STREAM_MSG(oss.str());
}
} else {
std::ostringstream oss;
- oss << "Unable to get transform " << signal.getName() << " at time "
- << time << ": " << msg;
+ oss << "Unable to get transform " << signal.getName() << " at time " << time
+ << ": " << msg;
entity->SEND_WARNING_STREAM_MSG(oss.str());
available = false;
}
diff --git a/src/ros_tf_listener.hh b/src/ros_tf_listener.hh
index 778c812a1bfd42b67a6c95687000e568a162e8e2..d753aa4f8daec3ac8feaff9145446958408815be 100644
--- a/src/ros_tf_listener.hh
+++ b/src/ros_tf_listener.hh
@@ -1,19 +1,16 @@
#ifndef DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
#define DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
-#include <boost/bind.hpp>
-
-#include <tf2_ros/transform_listener.h>
-#include <geometry_msgs/TransformStamped.h>
-
+#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/signal-ptr.h>
-#include <dynamic-graph/command-bind.h>
-
-#include <sot/core/matrix-geometry.hh>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <tf2_ros/transform_listener.h>
+#include <boost/bind.hpp>
#include <dynamic_graph_bridge/ros_init.hh>
+#include <sot/core/matrix-geometry.hh>
namespace dynamicgraph {
class RosTfListener;
@@ -36,15 +33,14 @@ struct TransformListenerData {
TransformListenerData(RosTfListener* e, tf2_ros::Buffer& b,
const std::string& to, const std::string& from,
const std::string& signame)
- : entity(e)
- , buffer(b)
- , toFrame(to)
- , fromFrame(from)
- , max_elapsed(0.5)
- , availableSig(signame+"_available")
- , signal(signame)
- , failbackSig(NULL, signame+"_failback")
- {
+ : entity(e),
+ buffer(b),
+ toFrame(to),
+ fromFrame(from),
+ max_elapsed(0.5),
+ availableSig(signame + "_available"),
+ signal(signame),
+ failbackSig(NULL, signame + "_failback") {
signal.setFunction(
boost::bind(&TransformListenerData::getTransform, this, _1, _2));
@@ -52,7 +48,7 @@ struct TransformListenerData {
boost::bind(&TransformListenerData::isAvailable, this, _1, _2));
availableSig.setNeedUpdateFromAllChildren(true);
- failbackSig.setConstant (sot::MatrixHomogeneous::Identity());
+ failbackSig.setConstant(sot::MatrixHomogeneous::Identity());
signal.addDependencies(failbackSig << availableSig);
}
@@ -69,20 +65,18 @@ class RosTfListener : public Entity {
typedef internal::TransformListenerData TransformListenerData;
RosTfListener(const std::string& _name)
- : Entity(_name)
- , buffer()
- , listener(buffer, rosInit(), false)
- {}
+ : Entity(_name), buffer(), listener(buffer, rosInit(), false) {}
- ~RosTfListener()
- {
+ ~RosTfListener() {
for (const auto& pair : listenerDatas) delete pair.second;
}
- void add(const std::string& to, const std::string& from, const std::string& signame)
- {
+ void add(const std::string& to, const std::string& from,
+ const std::string& signame) {
if (listenerDatas.find(signame) != listenerDatas.end())
- throw std::invalid_argument("A signal " + signame + " already exists in RosTfListener " + getName());
+ throw std::invalid_argument("A signal " + signame +
+ " already exists in RosTfListener " +
+ getName());
boost::format signalName("RosTfListener(%1%)::output(MatrixHomo)::%2%");
signalName % getName() % signame;
@@ -93,10 +87,10 @@ class RosTfListener : public Entity {
listenerDatas[signame] = tld;
}
- void setMaximumDelay(const std::string& signame, const double& max_elapsed)
- {
+ void setMaximumDelay(const std::string& signame, const double& max_elapsed) {
if (listenerDatas.count(signame) == 0)
- throw std::invalid_argument("No signal " + signame + " in RosTfListener " + getName());
+ throw std::invalid_argument("No signal " + signame +
+ " in RosTfListener " + getName());
listenerDatas[signame]->max_elapsed = ros::Duration(max_elapsed);
}
diff --git a/src/ros_time-python.hh b/src/ros_time-python.hh
index 24b46c70e415a7011ea9cb17ccb0492ceb553157..185b85a454953b9c4f4eadb4e509d3990c03b5d2 100644
--- a/src/ros_time-python.hh
+++ b/src/ros_time-python.hh
@@ -1,3 +1,3 @@
#include "ros_time.hh"
-typedef boost::mpl::vector< dynamicgraph::RosTime > entities_t;
+typedef boost::mpl::vector<dynamicgraph::RosTime> entities_t;
diff --git a/src/ros_time.cpp b/src/ros_time.cpp
index 7233ad3b60de45c0473f4f6753ccccfaa5fb38ad..79519ca7fb35eda4a3559fc977bcd21de7cb9f73 100644
--- a/src/ros_time.cpp
+++ b/src/ros_time.cpp
@@ -3,11 +3,12 @@
///
/// Author: Florent Lamiraux
+#include "ros_time.hh"
+
#include <dynamic-graph/factory.h>
-#include <dynamic-graph/signal-caster.h>
#include <dynamic-graph/signal-cast-helper.h>
+#include <dynamic-graph/signal-caster.h>
-#include "ros_time.hh"
#include "converter.hh"
namespace dynamicgraph {
@@ -23,7 +24,8 @@ const std::string RosTime::docstring_(
" boost::posix_time::ptime type.\n");
RosTime::RosTime(const std::string& _name)
- : Entity(_name), now_("RosTime(" + _name + ")::output(boost::posix_time::ptime)::time") {
+ : Entity(_name),
+ now_("RosTime(" + _name + ")::output(boost::posix_time::ptime)::time") {
signalRegistration(now_);
now_.setConstant(rosTimeToPtime(ros::Time::now()));
now_.setFunction(boost::bind(&RosTime::update, this, _1, _2));
diff --git a/src/ros_time.hh b/src/ros_time.hh
index a9472f389ef23ec0b447b6d73ef01f151b08fcb4..1601d88d680c05b20e286c510a58fdb894050c16 100644
--- a/src/ros_time.hh
+++ b/src/ros_time.hh
@@ -6,10 +6,11 @@
#ifndef DYNAMIC_GRAPH_ROS_TIME_HH
#define DYNAMIC_GRAPH_ROS_TIME_HH
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal.h>
#include <ros/time.h>
+
#include <boost/date_time/posix_time/posix_time_types.hpp>
-#include <dynamic-graph/signal.h>
-#include <dynamic-graph/entity.h>
namespace dynamicgraph {
@@ -22,13 +23,16 @@ class RosTime : public dynamicgraph::Entity {
virtual std::string getDocString() const;
protected:
- boost::posix_time::ptime& update(boost::posix_time::ptime& time, const int& t);
+ boost::posix_time::ptime& update(boost::posix_time::ptime& time,
+ const int& t);
private:
static const std::string docstring_;
}; // class RosTime
-template <> struct signal_io<boost::posix_time::ptime> : signal_io_unimplemented<boost::posix_time::ptime> {};
+template <>
+struct signal_io<boost::posix_time::ptime>
+ : signal_io_unimplemented<boost::posix_time::ptime> {};
} // namespace dynamicgraph
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 80f4fd3c7c5b01ebbb12570fc43d38a0da7d9ecf..09a68c8b7b9c4f5bea8e6d1dd1da0ac04010dea8 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -10,7 +10,9 @@
/* -------------------------------------------------------------------------- */
#include <ros/rate.h>
+
#include <dynamic_graph_bridge/sot_loader.hh>
+
#include "dynamic_graph_bridge/ros_init.hh"
// POSIX.1-2001
@@ -29,17 +31,11 @@ struct DataToLog {
std::vector<double> ittimes;
DataToLog(std::size_t N_)
- : N(N_)
- , idx(0)
- , iter(0)
- , iters(N, 0)
- , times(N, 0)
- , ittimes(N, 0)
- {}
+ : 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;
+ iters[idx] = iter;
+ times[idx] = t;
ittimes[idx] = itt;
++idx;
++iter;
@@ -53,10 +49,8 @@ struct DataToLog {
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 << iters[(idx + k) % N] << ' ' << times[(idx + k) % N] << ' '
+ << ittimes[(idx + k) % N] << '\n';
}
}
aof.close();
@@ -64,11 +58,11 @@ struct DataToLog {
};
void workThreadLoader(SotLoader *aSotLoader) {
- ros::Rate rate(1000); // 1 kHz
+ ros::Rate rate(1000); // 1 kHz
if (ros::param::has("/sot_controller/dt")) {
double periodd;
ros::param::get("/sot_controller/dt", periodd);
- rate = ros::Rate(1/periodd);
+ rate = ros::Rate(1 / periodd);
}
DataToLog dataToLog(5000);
@@ -113,8 +107,7 @@ SotLoader::SotLoader()
freeFlyerPose_.header.frame_id = "odom";
freeFlyerPose_.child_frame_id = "base_link";
- if (ros::param::get("/sot/tf_base_link",
- freeFlyerPose_.child_frame_id)) {
+ if (ros::param::get("/sot/tf_base_link", freeFlyerPose_.child_frame_id)) {
ROS_INFO_STREAM("Publishing " << freeFlyerPose_.child_frame_id << " wrt "
<< freeFlyerPose_.header.frame_id);
}
@@ -125,7 +118,9 @@ SotLoader::~SotLoader() {
thread_.join();
}
-void SotLoader::startControlLoop() { thread_ = boost::thread(workThreadLoader, this); }
+void SotLoader::startControlLoop() {
+ thread_ = boost::thread(workThreadLoader, this);
+}
void SotLoader::initializeRosNode(int argc, char *argv[]) {
SotLoaderBasic::initializeRosNode(argc, argv);
@@ -143,7 +138,8 @@ void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
assert(angleControl_.size() == angleEncoder_.size());
sensorsIn["joints"].setName("angle");
- for (unsigned int i = 0; i < angleControl_.size(); i++) angleEncoder_[i] = angleControl_[i];
+ for (unsigned int i = 0; i < angleControl_.size(); i++)
+ angleEncoder_[i] = angleControl_[i];
sensorsIn["joints"].setValues(angleEncoder_);
}
@@ -152,7 +148,8 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
angleControl_ = controlValues["control"].getValues();
// Debug
- std::map<std::string, dgs::ControlValues>::iterator it = controlValues.begin();
+ 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 << ":";
@@ -165,8 +162,8 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
// Check if the size if coherent with the robot description.
if (angleControl_.size() != (unsigned int)nbOfJoints_) {
- std::cerr << " angleControl_" << angleControl_.size() << " and nbOfJoints" << (unsigned int)nbOfJoints_
- << " are different !" << std::endl;
+ std::cerr << " angleControl_" << angleControl_.size() << " and nbOfJoints"
+ << (unsigned int)nbOfJoints_ << " are different !" << std::endl;
exit(-1);
}
// Publish the data.
@@ -176,7 +173,8 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
}
for (unsigned int i = 0; i < parallel_joints_to_state_vector_.size(); i++) {
joint_state_.position[i + nbOfJoints_] =
- coefficient_parallel_joints_[i] * angleControl_[parallel_joints_to_state_vector_[i]];
+ coefficient_parallel_joints_[i] *
+ angleControl_[parallel_joints_to_state_vector_[i]];
}
joint_pub_.publish(joint_state_);
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index 1b3024fab714ed547b1453190512d2a5bbbfd360..8f9ea82fa464932ae37208cb162813d07864c416 100644
--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -9,12 +9,13 @@
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
+#include <dynamic-graph/pool.h>
+
#include <dynamic_graph_bridge/sot_loader.hh>
+
#include "dynamic_graph_bridge/ros_init.hh"
#include "dynamic_graph_bridge/ros_parameter.hh"
-#include <dynamic-graph/pool.h>
-
// POSIX.1-2001
#include <dlfcn.h>
@@ -22,7 +23,8 @@ using namespace std;
using namespace dynamicgraph::sot;
namespace po = boost::program_options;
-SotLoaderBasic::SotLoaderBasic() : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
+SotLoaderBasic::SotLoaderBasic()
+ : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
readSotVectorStateParam();
initPublication();
}
@@ -36,17 +38,21 @@ int SotLoaderBasic::initPublication() {
return 0;
}
-void SotLoaderBasic::initializeRosNode(int, char* []) {
+void SotLoaderBasic::initializeRosNode(int, char*[]) {
ROS_INFO("Ready to start dynamic graph.");
ros::NodeHandle n;
- service_start_ = n.advertiseService("start_dynamic_graph", &SotLoaderBasic::start_dg, this);
+ service_start_ = n.advertiseService("start_dynamic_graph",
+ &SotLoaderBasic::start_dg, this);
- service_stop_ = n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);
+ service_stop_ =
+ n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);
dynamicgraph::parameter_server_read_robot_description();
}
-void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) { dynamicLibraryName_ = afilename; }
+void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) {
+ dynamicLibraryName_ = afilename;
+}
int SotLoaderBasic::readSotVectorStateParam() {
std::map<std::string, int> from_state_name_to_state_vector;
@@ -66,12 +72,16 @@ int SotLoaderBasic::readSotVectorStateParam() {
if (ros::param::has("/sot/joint_state_parallel")) {
XmlRpc::XmlRpcValue joint_state_parallel;
n.getParam("/sot/joint_state_parallel", joint_state_parallel);
- ROS_ASSERT(joint_state_parallel.getType() == XmlRpc::XmlRpcValue::TypeStruct);
- std::cout << "Type of joint_state_parallel:" << joint_state_parallel.getType() << std::endl;
+ ROS_ASSERT(joint_state_parallel.getType() ==
+ XmlRpc::XmlRpcValue::TypeStruct);
+ std::cout << "Type of joint_state_parallel:"
+ << joint_state_parallel.getType() << std::endl;
- for (XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin(); it != joint_state_parallel.end(); it++) {
+ for (XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin();
+ it != joint_state_parallel.end(); it++) {
XmlRpc::XmlRpcValue local_value = it->second;
- std::string final_expression, map_expression = static_cast<string>(local_value);
+ std::string final_expression,
+ map_expression = static_cast<string>(local_value);
double final_coefficient = 1.0;
// deal with sign
if (map_expression[0] == '-') {
@@ -81,7 +91,8 @@ int SotLoaderBasic::readSotVectorStateParam() {
final_expression = map_expression;
std::cout << it->first.c_str() << ": " << final_coefficient << std::endl;
- from_parallel_name_to_state_vector_name[it->first.c_str()] = final_expression;
+ from_parallel_name_to_state_vector_name[it->first.c_str()] =
+ final_expression;
coefficient_parallel_joints_.push_back(final_coefficient);
}
nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
@@ -103,10 +114,12 @@ int SotLoaderBasic::readSotVectorStateParam() {
// and build map from parallel name to state vector
int i = 0;
parallel_joints_to_state_vector_.resize(nbOfParallelJoints_);
- for (std::map<std::string, std::string>::iterator it = from_parallel_name_to_state_vector_name.begin();
+ for (std::map<std::string, std::string>::iterator it =
+ from_parallel_name_to_state_vector_name.begin();
it != from_parallel_name_to_state_vector_name.end(); it++, i++) {
joint_state_.name[i + nbOfJoints_] = it->first.c_str();
- parallel_joints_to_state_vector_[i] = from_state_name_to_state_vector[it->second];
+ parallel_joints_to_state_vector_[i] =
+ from_state_name_to_state_vector[it->second];
}
return 0;
@@ -114,7 +127,8 @@ int SotLoaderBasic::readSotVectorStateParam() {
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");
+ desc.add_options()("help", "produce help message")(
+ "input-file", po::value<string>(), "library to load");
po::store(po::parse_command_line(argc, argv, desc), vm_);
po::notify(vm_);
@@ -134,7 +148,8 @@ int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
void SotLoaderBasic::Initialization() {
// Load the SotRobotBipedController library.
- sotRobotControllerLibrary_ = dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
+ sotRobotControllerLibrary_ =
+ dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
if (!sotRobotControllerLibrary_) {
std::cerr << "Cannot load library: " << dlerror() << '\n';
return;
@@ -144,8 +159,9 @@ void SotLoaderBasic::Initialization() {
dlerror();
// Load the symbols.
- createSotExternalInterface_t* createSot = reinterpret_cast<createSotExternalInterface_t*>(
- reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
+ createSotExternalInterface_t* createSot =
+ reinterpret_cast<createSotExternalInterface_t*>(reinterpret_cast<long>(
+ dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
const char* dlsym_error = dlerror();
if (dlsym_error) {
std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
@@ -165,8 +181,9 @@ void SotLoaderBasic::CleanUp() {
// SignalCaster singleton could probably be destroyed.
// Load the symbols.
- destroySotExternalInterface_t* destroySot = reinterpret_cast<destroySotExternalInterface_t*>(
- reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
+ 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';
@@ -180,12 +197,14 @@ void SotLoaderBasic::CleanUp() {
dlclose(sotRobotControllerLibrary_);
}
-bool SotLoaderBasic::start_dg(std_srvs::Empty::Request&, std_srvs::Empty::Response&) {
+bool SotLoaderBasic::start_dg(std_srvs::Empty::Request&,
+ std_srvs::Empty::Response&) {
dynamic_graph_stopped_ = false;
return true;
}
-bool SotLoaderBasic::stop_dg(std_srvs::Empty::Request&, std_srvs::Empty::Response&) {
+bool SotLoaderBasic::stop_dg(std_srvs::Empty::Request&,
+ std_srvs::Empty::Response&) {
dynamic_graph_stopped_ = true;
return true;
}
diff --git a/src/sot_to_ros.cpp b/src/sot_to_ros.cpp
index 9d6e9982650e533ab237becc5d333a0a8c41bf4d..8b03235588c3110377d78caa64903994c579b919 100644
--- a/src/sot_to_ros.cpp
+++ b/src/sot_to_ros.cpp
@@ -12,8 +12,12 @@ const char* SotToRos<Vector>::signalTypeName = "Vector";
const char* SotToRos<specific::Vector3>::signalTypeName = "Vector3";
const char* SotToRos<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
const char* SotToRos<specific::Twist>::signalTypeName = "Twist";
-const char* SotToRos<std::pair<specific::Vector3, Vector> >::signalTypeName = "Vector3Stamped";
-const char* SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName = "MatrixHomo";
-const char* SotToRos<std::pair<specific::Twist, Vector> >::signalTypeName = "Twist";
+const char* SotToRos<std::pair<specific::Vector3, Vector> >::signalTypeName =
+ "Vector3Stamped";
+const char*
+ SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName =
+ "MatrixHomo";
+const char* SotToRos<std::pair<specific::Twist, Vector> >::signalTypeName =
+ "Twist";
} // end of namespace dynamicgraph.
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index 054e6ffbf4304955a8b69551d16f41c97cf5de76..d16d7076579de2cc73ba3e3793f2b0dd542adab1 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -1,31 +1,30 @@
#ifndef DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
#define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
-#include <vector>
-#include <utility>
-
-#include <boost/format.hpp>
-
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
-#include <std_msgs/UInt32.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
+#include <std_msgs/UInt32.h>
+
+#include <boost/format.hpp>
+#include <sot/core/matrix-geometry.hh>
+#include <utility>
+#include <vector>
+
#include "dynamic_graph_bridge_msgs/Matrix.h"
#include "dynamic_graph_bridge_msgs/Vector.h"
-
#include "geometry_msgs/Transform.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/TwistStamped.h"
#include "geometry_msgs/Vector3Stamped.h"
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/linear-algebra.h>
-#include <dynamic-graph/signal-ptr.h>
-#include <sot/core/matrix-geometry.hh>
-
-#define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME) \
- makeSignalString(typeid(*this).name(), NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)
+#define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME) \
+ makeSignalString(typeid(*this).name(), NAME, IS_INPUT, OUTPUT_TYPE, \
+ SIGNAL_NAME)
namespace dynamicgraph {
@@ -300,10 +299,14 @@ struct SotToRos<std::pair<specific::Twist, Vector> > {
}
};
-inline std::string makeSignalString(const std::string& className, const std::string& instanceName, bool isInputSignal,
- const std::string& signalType, const std::string& signalName) {
+inline std::string makeSignalString(const std::string& className,
+ const std::string& instanceName,
+ bool isInputSignal,
+ const std::string& signalType,
+ const std::string& signalName) {
boost::format fmt("%s(%s)::%s(%s)::%s");
- fmt % className % instanceName % (isInputSignal ? "input" : "output") % signalType % signalName;
+ fmt % className % instanceName % (isInputSignal ? "input" : "output") %
+ signalType % signalName;
return fmt.str();
}
} // end of namespace dynamicgraph.
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 92e10ebeee8bbdf9e86e85165efce935541b6c0d..0773ed988866e0811ba50f66704730e8e3ebebfb 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -1,4 +1,4 @@
-IF(BUILD_PYTHON_INTERFACE)
- # TODO: this test requires a ros master
- #ADD_PYTHON_UNIT_TEST("py-import" "tests/test_import.py")
-ENDIF(BUILD_PYTHON_INTERFACE)
+if(BUILD_PYTHON_INTERFACE)
+ # TODO: this test requires a ros master ADD_PYTHON_UNIT_TEST("py-import"
+ # "tests/test_import.py")
+endif(BUILD_PYTHON_INTERFACE)
diff --git a/tests/test_import.py b/tests/test_import.py
index b9195659641eac1ef888bfb8659ee812379ed5c0..677a047d6b8411f287788be8937e453eba71d80f 100755
--- a/tests/test_import.py
+++ b/tests/test_import.py
@@ -2,25 +2,25 @@
from dynamic_graph.ros import RosImport
-ri = RosImport('rosimport')
+ri = RosImport("rosimport")
-ri.add('double', 'doubleS', 'doubleT')
-ri.add('vector', 'vectorS', 'vectorT')
-ri.add('matrix', 'matrixS', 'matrixT')
+ri.add("double", "doubleS", "doubleT")
+ri.add("vector", "vectorS", "vectorT")
+ri.add("matrix", "matrixS", "matrixT")
-ri.doubleS.value = 42.
+ri.doubleS.value = 42.0
ri.vectorS.value = (
- 42.,
- 42.,
+ 42.0,
+ 42.0,
)
ri.matrixS.value = (
(
- 42.,
- 42.,
+ 42.0,
+ 42.0,
),
(
- 42.,
- 42.,
+ 42.0,
+ 42.0,
),
)
diff --git a/travis_custom/Dockerfile b/travis_custom/Dockerfile
deleted file mode 100644
index 4be6b247cded0284b94ad6768a421f572595436b..0000000000000000000000000000000000000000
--- a/travis_custom/Dockerfile
+++ /dev/null
@@ -1,39 +0,0 @@
-# Pull ros docker image
-FROM ros:indigo-ros-base
-
-# Prepapre sudo environment
-RUN apt-get update && \
- apt-get -y install sudo && \
- apt-get -y install curl
-
-RUN useradd -m docker && echo "docker:docker" | chpasswd && adduser docker sudo
-
-USER root
-ADD /travis_custom/sudoers.txt /etc/sudoers
-ADD /travis_custom/setup-opt-robotpkg.sh /home/docker/setup-opt-robotpkg.sh
-RUN chmod 440 /etc/sudoers
-
-USER docker
-CMD /bin/bash
-# Add robotpkg binary repo
-
-RUN sudo sh -c "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub trusty robotpkg\" >> /etc/apt/sources.list "
-RUN sudo sh -c "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub trusty robotpkg\" >> /etc/apt/sources.list "
-
-RUN curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
-
-# Update the reference to packages
-RUN sudo apt-get update
-RUN more /etc/apt/sources.list
-#
-RUN sudo apt-get install -y g++ python2.7 python2.7-dev
-RUN sudo apt-get install -y cppcheck doxygen libboost-system-dev libboost-test-dev libboost-filesystem-dev libboost-program-options-dev libeigen3-dev libtinyxml-dev
-RUN sudo apt-get install -y ros-indigo-tf ros-indigo-tf2-bullet ros-indigo-realtime-tools
-RUN sudo apt-get install -y robotpkg-pinocchio
-RUN sudo apt-get install -y robotpkg-dynamic-graph-v3 robotpkg-py27-dynamic-graph-v3 robotpkg-dynamic-graph-bridge-msgs
-RUN sudo apt-get install -y robotpkg-sot-core-v3 robotpkg-py27-sot-tools-v3 robotpkg-sot-dynamic-pinocchio-v3
-RUN sudo apt-get install -y libboost-python-dev robotpkg-py27-eigenpy python2.7-dev python-numpy python-sphinx
-RUN env
-
-
-
diff --git a/travis_custom/setup-opt-robotpkg.sh b/travis_custom/setup-opt-robotpkg.sh
deleted file mode 100755
index 891921f3dbf185a3820141ab612d7ea6b3386587..0000000000000000000000000000000000000000
--- a/travis_custom/setup-opt-robotpkg.sh
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/bin/bash
-export ROBOTPKG_BASE=/opt/openrobots
-export PATH=$PATH:$ROBOTPKG_BASE/sbin:$ROBOTPKG_BASE/bin
-export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$ROBOTPKG_BASE/lib:$ROBOTPKG_BASE/lib/plugin:$ROBOTPKG_BASE/lib64
-export PYTHONPATH=$PYTHONPATH:$ROBOTPKG_BASE/lib/python2.7/site-packages:$ROBOTPKG_BASE/lib/python2.7/dist-packages
-export PKG_CONFIG_PATH=$PKG_CONFIG_PATH:$ROBOTPKG_BASE/lib/pkgconfig/
-export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$ROBOTPKG_BASE/share:$ROBOTPKG_BASE/stacks
-export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:$ROBOTPKG_BASE
diff --git a/travis_custom/sudoers.txt b/travis_custom/sudoers.txt
deleted file mode 100644
index e97045b890204545bc21b86375704902df3cb040..0000000000000000000000000000000000000000
--- a/travis_custom/sudoers.txt
+++ /dev/null
@@ -1,4 +0,0 @@
-root ALL=(ALL) ALL
-docker ALL=(ALL) NOPASSWD: ALL
-Defaults env_reset
-Defaults secure_path="/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin"