.git-blame-ignore-revs

+# format (Guilhem Saurel, 2022-09-06)
+426c21a7f4e5b445d17825235f983e2f8e1a86ec

.gitignore

@@ -5,3 +5,5 @@ msg_gen/
 src/dynamic_graph_bridge/
 bin/
 build/
+srv_gen/
+*~

.gitlab-ci.yml

+include: http://rainboard.laas.fr/project/dynamic_graph_bridge/.gitlab-ci.yml

.gitmodules

 [submodule "cmake"]
 	path = cmake
-	url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
+	url = https://github.com/jrl-umi3218/jrl-cmakemodules.git

.pre-commit-config.yaml

+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

CMakeLists.txt

-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-include(FindPkgConfig)
-
-set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rosbuild_init()
-
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-rosbuild_genmsg()
-
-rosbuild_add_boost_directories()
-
-pkg_check_modules(JRL_MAL REQUIRED jrl-mal)
-pkg_check_modules(DYNAMIC_GRAPH REQUIRED dynamic-graph)
-pkg_check_modules(SOT_CORE REQUIRED sot-core)
-
-rosbuild_add_library(ros_bridge
-  src/converter.hh
-  src/ros_init.hh src/ros_init.cpp
-  src/sot_to_ros.hh src/sot_to_ros.cpp
-  )
-
-rosbuild_add_library(ros_import src/ros_import.cpp src/ros_import.hh)
-rosbuild_add_compile_flags(ros_import ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
-  ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(ros_import ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS}
-  ${SOT_CORE_LDFLAGS})
-target_link_libraries(ros_import ros_bridge)
-target_link_libraries(ros_import
-  ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
-
-rosbuild_add_library(ros_export src/ros_export.cpp src/ros_export.hh)
-rosbuild_add_compile_flags(ros_export ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
-  ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(ros_export ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS}
-  ${SOT_CORE_LDFLAGS})
-target_link_libraries(ros_export ros_bridge)
-target_link_libraries(ros_export
-  ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
-
-rosbuild_add_library(ros_joint_state
-  src/ros_joint_state.cpp src/ros_joint_state)
-rosbuild_add_compile_flags(ros_joint_state
-  ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
-  ${SOT_CORE_CFLAGS})
-rosbuild_add_link_flags(ros_joint_state ${JRL_MAL_LDFLAGS}
-  ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS})
-target_link_libraries(ros_joint_state ros_bridge)
-target_link_libraries(ros_joint_state
-  ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
-
-
-
-INSTALL(TARGETS ros_bridge DESTINATION lib)
-INSTALL(TARGETS ros_import DESTINATION lib)
-INSTALL(TARGETS ros_export DESTINATION lib)
-INSTALL(TARGETS ros_joint_state DESTINATION lib)
-
-INCLUDE(cmake/python.cmake)
-
-INCLUDE_DIRECTORIES(${DYNAMIC_GRAPH_INCLUDE_DIRS})
-LINK_DIRECTORIES(${DYNAMIC_GRAPH_LIBRARY_DIRS})
-
-DYNAMIC_GRAPH_PYTHON_MODULE("ros/ros_import"
-  ros_import
-  ros/ros_import/wrap
-  )
-SET_TARGET_PROPERTIES(dynamic_graph/ros/ros_import/wrap
-  PROPERTIES
-  COMPILE_FLAGS
-  ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
-  LINK_FLAGS
-  ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
-  )
-
-DYNAMIC_GRAPH_PYTHON_MODULE("ros/ros_export"
-  ros_export
-  ros/ros_export/wrap
-  )
-SET_TARGET_PROPERTIES(dynamic_graph/ros/ros_export/wrap
-  PROPERTIES
-  COMPILE_FLAGS
-  ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
-  LINK_FLAGS
-  ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
-  )
-
-DYNAMIC_GRAPH_PYTHON_MODULE("ros/ros_joint_state"
-  ros_joint_state
-  ros/ros_joint_state/wrap
-  )
-SET_TARGET_PROPERTIES(dynamic_graph/ros/ros_joint_state/wrap
-  PROPERTIES
-  COMPILE_FLAGS
-  ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
-  LINK_FLAGS
-  ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
-  )
-
-ADD_SUBDIRECTORY(src)
+# Copyright (C) 2008-2020 LAAS-CNRS, JRL AIST-CNRS.
+#
+# Author: Florent Lamiraux, Nirmal Giftsun, Guilhem Saurel
+#
+
+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}")
+
+# Project options
+option(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
+
+# Project configuration
+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)
+
+# JRL-cmakemodule setup
+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})
+
+# Project dependencies
+set(CATKIN_REQUIRED_COMPONENTS
+    roscpp
+    std_msgs
+    message_generation
+    std_srvs
+    geometry_msgs
+    sensor_msgs
+    tf2_ros
+    realtime_tools)
+
+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)
+
+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)
+
+add_subdirectory(src)
+add_subdirectory(tests)
+
+# install ros executables
+install(PROGRAMS scripts/robot_pose_publisher scripts/run_command
+                 scripts/tf_publisher DESTINATION share/${PROJECT_NAME})
+
+# Install package information
+install(FILES manifest.xml package.xml DESTINATION share/${PROJECT_NAME})

LICENSE

+BSD 2-Clause License
+
+Copyright (c) 2019, CNRS
+Author: Stack of Tasks Development Team
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

-dynamic-graph bindings
-======================
+# Dynamic Graph bindings
+
+[![Pipeline status](https://gitlab.laas.fr/stack-of-tasks/dynamic_graph_bridge/badges/master/pipeline.svg)](https://gitlab.laas.fr/stack-of-tasks/dynamic_graph_bridge/commits/master)
+[![Coverage report](https://gitlab.laas.fr/stack-of-tasks/dynamic_graph_bridge/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/stack-of-tasks/dynamic_graph_bridge/master/coverage/)
+[![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black)
+[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/stack-of-tasks/dynamic_graph_bridge/master.svg)](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.

cmake @ 57c46b2d

-Subproject commit b70f9e7e91f246938403b30e96ceaa879abd12ae
+Subproject commit 57c46b2d7b26bb0c25f8f98cfc2a9868e03be603

doc/Doxyfile.extra.in

+INPUT                  = @PROJECT_SOURCE_DIR@/include \
+			 @PROJECT_SOURCE_DIR@/doc

doc/mainpage.hh

+/// \mainpage
+///
+/// \section Introduction
+///
+/// This package implements a bridge between ROS and the stack of tasks.
+/// The main features are
+///   \li A service <c>Run_command</c> that enables users to communicate with
+///       a remote python interpreter in order to build and control a graph,
+///   \li A class <c>dynamicgraph::RosRobotModel</c> deriving from
+///       <c>dynamicgraph::sot::Dynamic</c> that implements an entity computing
+///       forward kinematics and dynamics based on a urdf file.  for
+///       more information, type in a python terminal
+/// \code
+/// from dynamic_graph.ros.robot_model import RosRobotModel
+/// model = RosRobotModel ("model")
+/// model.help ()
+/// \endcode

include/dynamic_graph_bridge/fwd.hh

+/*
+ * Copyright CNRS 2021
+ *
+ * Author: Florent Lamiraux
+ *
+ * This file is part of sot-core.
+ */
+
+#ifndef DYNAMIC_GRAPH_BRIDGE_FWD_HH
+#define DYNAMIC_GRAPH_BRIDGE_FWD_HH
+
+#include <dynamic-graph/fwd.hh>
+
+namespace dynamicgraph {
+// classes defined in sot-core
+class Interpreter;
+typedef std::shared_ptr<Interpreter> InterpreterPtr_t;
+}  // namespace dynamicgraph
+
+#endif  // DYNAMIC_GRAPH_PYTHON_FWD_HH

include/dynamic_graph_bridge/ros_init.hh

+#ifndef ROS_INIT_HH
+#define ROS_INIT_HH
+#include <ros/ros.h>
+
+namespace dynamicgraph {
+ros::NodeHandle& rosInit(bool createAsyncSpinner = false,
+                         bool createMultiThreadSpinner = true);
+
+/// \brief Return spinner or throw an exception if spinner
+/// creation has been disabled at startup.
+ros::AsyncSpinner& spinner();
+
+/// \brief Return multi threaded spinner or throw an exception if spinner
+/// creation has been disabled at startup.
+ros::MultiThreadedSpinner& mtSpinner();
+
+}  // end of namespace dynamicgraph.
+
+#endif  //! ROS_INIT_HH

include/dynamic_graph_bridge/ros_interpreter.hh

+#ifndef DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
+#define DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
+#pragma GCC diagnostic push
+#pragma GCC system_header
+#include <ros/ros.h>
+#pragma GCC diagnostic pop
+
+#include <dynamic_graph_bridge_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
+/// service.
+///
+/// This takes as input a ROS node handle and do not handle the
+/// callback so that the service behavior can be controlled from
+/// the outside.
+class Interpreter {
+ public:
+  typedef boost::function<bool(
+      dynamic_graph_bridge_msgs::RunCommand::Request&,
+      dynamic_graph_bridge_msgs::RunCommand::Response&)>
+      runCommandCallback_t;
+
+  typedef boost::function<bool(
+      dynamic_graph_bridge_msgs::RunPythonFile::Request&,
+      dynamic_graph_bridge_msgs::RunPythonFile::Response&)>
+      runPythonFileCallback_t;
+
+  explicit Interpreter(ros::NodeHandle& nodeHandle);
+
+  /// \brief Method to start python interpreter and deal with messages.
+  /// \param Command string to execute, result, stdout, stderr strings.
+  void runCommand(const std::string& command, std::string& result,
+                  std::string& out, std::string& err);
+
+  /// \brief Method to parse python scripts.
+  /// \param Input file name to parse.
+  void runPythonFile(std::string ifilename);
+
+  /// Initialize service run_command
+  void startRosService();
+
+ protected:
+  /// \brief Run a Python command and return result, stderr and stdout.
+  bool runCommandCallback(dynamic_graph_bridge_msgs::RunCommand::Request& req,
+                          dynamic_graph_bridge_msgs::RunCommand::Response& res);
+
+  /// \brief Run a Python file.
+  bool runPythonFileCallback(
+      dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
+      dynamic_graph_bridge_msgs::RunPythonFile::Response& res);
+
+ private:
+  python::Interpreter interpreter_;
+  ros::NodeHandle& nodeHandle_;
+  ros::ServiceServer runCommandSrv_;
+  ros::ServiceServer runPythonFileSrv_;
+};
+}  // end of namespace dynamicgraph.
+
+#endif  //! DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH

include/dynamic_graph_bridge/ros_parameter.hh

+#ifndef _ROS_DYNAMIC_GRAPH_PARAMETER_
+#define _ROS_DYNAMIC_GRAPH_PARAMETER_
+
+namespace dynamicgraph {
+
+bool parameter_server_read_robot_description();
+
+}
+#endif /* _ROS_DYNAMIC_GRAPH_PARAMETER_ */

include/dynamic_graph_bridge/sot_loader.hh

+/*
+ * Copyright 2016,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#ifndef _SOT_LOADER_HH_
+#define _SOT_LOADER_HH_
+
+// System includes
+#include <cassert>
+
+// STL includes
+#include <map>
+
+// Pinocchio includes
+#include <pinocchio/fwd.hpp>
+
+// Boost includes
+#include <boost/foreach.hpp>
+#include <boost/format.hpp>
+#include <boost/program_options.hpp>
+#include <boost/thread/thread.hpp>
+
+// ROS includes
+#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/abstract-sot-external-interface.hh>
+#include <sot/core/debug.hh>
+
+// Dynamic-graph-bridge includes.
+#include <dynamic_graph_bridge/sot_loader_basic.hh>
+
+namespace po = boost::program_options;
+namespace dgs = dynamicgraph::sot;
+
+class SotLoader : public SotLoaderBasic {
+ protected:
+  /// Map of sensor readings
+  std::map<std::string, dgs::SensorValues> sensorsIn_;
+  /// Map of control values
+  std::map<std::string, dgs::ControlValues> controlValues_;
+
+  /// Angular values read by encoders
+  std::vector<double> angleEncoder_;
+  /// Angular values sent to motors
+  std::vector<double> angleControl_;
+  /// Forces read by force sensors
+  std::vector<double> forces_;
+  /// Torques
+  std::vector<double> torques_;
+  /// Attitude of the robot computed by extended Kalman filter.
+  std::vector<double> baseAtt_;
+  /// Accelerations read by Accelerometers
+  std::vector<double> accelerometer_;
+  /// Angular velocity read by gyrometers
+  std::vector<double> gyrometer_;
+
+  /// URDF string description of the robot.
+  std::string robot_desc_string_;
+
+  /// The thread running dynamic graph
+  boost::thread thread_;
+
+  // \brief Start control loop
+  virtual void startControlLoop();
+
+  // Robot Pose Publisher
+  tf2_ros::TransformBroadcaster freeFlyerPublisher_;
+  geometry_msgs::TransformStamped freeFlyerPose_;
+
+ public:
+  SotLoader();
+  ~SotLoader();
+
+  // \brief Create a thread for ROS and start the control loop.
+  void initializeRosNode(int argc, char *argv[]);
+
+  // \brief Compute one iteration of control.
+  // Basically calls fillSensors, the SoT and the readControl.
+  void oneIteration();
+
+  // \brief Fill the sensors value for the SoT.
+  void fillSensors(std::map<std::string, dgs::SensorValues> &sensorsIn);
+
+  // \brief Read the control computed by the SoT framework.
+  void readControl(std::map<std::string, dgs::ControlValues> &controlValues);
+
+  // \brief Prepare the SoT framework.
+  void setup();
+};
+
+#endif /* SOT_LOADER_HH_ */

include/dynamic_graph_bridge/sot_loader_basic.hh

+/*
+ * Copyright 2016,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#ifndef _SOT_LOADER_BASIC_HH_
+#define _SOT_LOADER_BASIC_HH_
+
+// System includes
+#include <cassert>
+
+// STL includes
+#include <map>
+
+// Pinocchio includes
+#include <pinocchio/fwd.hpp>
+
+// Boost includes
+#include <boost/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"
+
+// Sot Framework includes
+#include <sot/core/abstract-sot-external-interface.hh>
+#include <sot/core/debug.hh>
+
+namespace po = boost::program_options;
+namespace dgs = dynamicgraph::sot;
+
+class SotLoaderBasic {
+ protected:
+  // Dynamic graph is stopped.
+  bool dynamic_graph_stopped_;
+
+  /// \brief the sot-hrp2 controller
+  dgs::AbstractSotExternalInterface* sotController_;
+
+  po::variables_map vm_;
+  std::string dynamicLibraryName_;
+
+  /// \brief Handle on the SoT library.
+  void* sotRobotControllerLibrary_;
+
+  /// \brief Map between SoT state vector and some joint_state_links
+  XmlRpc::XmlRpcValue stateVectorMap_;
+
+  /// \brief List of parallel joints from the state vector.
+  typedef std::vector<int> parallel_joints_to_state_vector_t;
+  parallel_joints_to_state_vector_t parallel_joints_to_state_vector_;
+
+  /// \brief Coefficient between parallel joints and the state vector.
+  std::vector<double> coefficient_parallel_joints_;
+  /// Advertises start_dynamic_graph services
+  ros::ServiceServer service_start_;
+
+  /// Advertises stop_dynamic_graph services
+  ros::ServiceServer service_stop_;
+
+  // Joint state publication.
+  ros::Publisher joint_pub_;
+
+  // Joint state to be published.
+  sensor_msgs::JointState joint_state_;
+
+  // Number of DOFs according to KDL.
+  int nbOfJoints_;
+  parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
+
+ public:
+  SotLoaderBasic();
+  ~SotLoaderBasic(){};
+
+  // \brief Read user input to extract the path of the SoT dynamic library.
+  int parseOptions(int argc, char* argv[]);
+
+  /// \brief Load the SoT device corresponding to the robot.
+  void Initialization();
+
+  /// \brief Unload the library which handles the robot device.
+  void CleanUp();
+
+  // \brief Create a thread for ROS.
+  virtual void initializeRosNode(int argc, char* argv[]);
+
+  // \brief Callback function when starting dynamic graph.
+  bool start_dg(std_srvs::Empty::Request& request,
+                std_srvs::Empty::Response& response);
+
+  // \brief Callback function when stopping dynamic graph.
+  bool stop_dg(std_srvs::Empty::Request& request,
+               std_srvs::Empty::Response& response);
+
+  // \brief Read the state vector description based upon the robot links.
+  int readSotVectorStateParam();
+
+  // \brief Init publication of joint states.
+  int initPublication();
+
+  // \brief Get Status of dg.
+  bool isDynamicGraphStopped() { return dynamic_graph_stopped_; }
+
+  // \brief Specify the name of the dynamic library.
+  void setDynamicLibraryName(std::string& afilename);
+};
+
+#endif /* _SOT_LOADER_BASIC_HH_ */

mainpage.dox

@@ -2,12 +2,8 @@
 \mainpage
 \htmlinclude manifest.html
 
-\b dynamic_graph is ... 
-
-<!-- 
-Provide an overview of your package.
--->
-
+\b dynamic_graph_bridge provides communication tools for
+ROS/dynamic-graph interoperation.
 
 \section codeapi Code API
 

manifest.xml

@@ -7,16 +7,33 @@
   <author>Thomas Moulard</author>
   <license>BSD</license>
   <review status="unreviewed" notes=""/>
-  <url>http://ros.org/wiki/dynamic-graph-bridge</url>
+  <url>http://ros.org/wiki/dynamic_graph_bridge</url>
+
+  <export>
+    <cpp
+       cflags="-I${prefix}/include"
+       lflags="-L${prefix}/lib -lros_bridge -lros_interpreter -Wl,-rpath,${prefix}/lib"
+       />
+    <rosdoc config="rosdoc.yaml" />
+  </export>
+
 
   <rosdep name="boost"/>
 
+  <!-- Disable non-standard dependencies for now
+  <rosdep name="jrl-mal"/>
+  <rosdep name="dynamic-graph"/>
+  <rosdep name="sot-core"/>
+  <rosdep name="sot-dynamic"/>
+  -->
+
   <depend package="std_msgs"/>
+  <depend package="std_srvs"/>
   <depend package="roscpp"/>
   <depend package="geometry_msgs"/>
   <depend package="sensor_msgs"/>
-  <depend package="bullet"/>
   <depend package="tf"/>
 
   <depend package="realtime_tools"/>
+  <depend package="dynamic_graph_bridge_msgs"/>
 </package>

msg/Matrix.msg

-float64[] data
-uint32 width