Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • gsaurel/dynamic_graph_bridge
  • stack-of-tasks/dynamic_graph_bridge
2 results
Show changes
Commits on Source (400)
300 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 535 additions and 197 deletions
.git-blame-ignore-revs 0 → 100644
View file @ c2c3026d
# format (Guilhem Saurel, 2022-09-06)
426c21a7f4e5b445d17825235f983e2f8e1a86ec
.gitignore
View file @ c2c3026d
...@@ -6,3 +6,4 @@ src/dynamic_graph_bridge/ ...@@ -6,3 +6,4 @@ src/dynamic_graph_bridge/
bin/ bin/
build/ build/
srv_gen/ srv_gen/
*~
.gitlab-ci.yml 0 → 100644
View file @ c2c3026d
include: http://rainboard.laas.fr/project/dynamic_graph_bridge/.gitlab-ci.yml
.gitmodules
View file @ c2c3026d
[submodule "cmake"] [submodule "cmake"]
path = 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 0 → 100644
View file @ c2c3026d
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
View file @ c2c3026d
cmake_minimum_required(VERSION 2.4.6) # Copyright (C) 2008-2020 LAAS-CNRS, JRL AIST-CNRS.
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) #
# Author: Florent Lamiraux, Nirmal Giftsun, Guilhem Saurel
include(FindPkgConfig) #
set(ROS_BUILD_TYPE RelWithDebInfo) cmake_minimum_required(VERSION 3.1)
rosbuild_init() # Project properties
set(PROJECT_ORG stack-of-tasks)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) set(PROJECT_NAME dynamic_graph_bridge)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) set(PROJECT_DESCRIPTION "Dynamic graph bridge library")
set(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
rosbuild_genmsg()
rosbuild_gensrv() # Project options
option(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
rosbuild_add_boost_directories()
# Project configuration
pkg_check_modules(JRL_MAL REQUIRED jrl-mal) set(PROJECT_USE_CMAKE_EXPORT TRUE)
pkg_check_modules(DYNAMIC_GRAPH REQUIRED dynamic-graph) set(CUSTOM_HEADER_DIR ${PROJECT_NAME})
pkg_check_modules(DYNAMIC_GRAPH_PYTHON REQUIRED dynamic-graph-python) set(CXX_DISABLE_WERROR FALSE)
pkg_check_modules(SOT_CORE REQUIRED sot-core) set(DOXYGEN_USE_MATHJAX YES)
set(CATKIN_ENABLE_TESTING OFF)
include_directories(include)
# JRL-cmakemodule setup
file(MAKE_DIRECTORY "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/ros_export") include(cmake/base.cmake)
file(MAKE_DIRECTORY "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/ros_import") include(cmake/boost.cmake)
file(MAKE_DIRECTORY include(cmake/ros.cmake)
"${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/ros_joint_state")
# Project definition
rosbuild_add_library(ros_bridge compute_project_args(PROJECT_ARGS LANGUAGES CXX)
src/converter.hh project(${PROJECT_NAME} ${PROJECT_ARGS})
include/dynamic_graph_bridge/ros_init.hh src/ros_init.cpp
src/sot_to_ros.hh src/sot_to_ros.cpp # Project dependencies
) set(CATKIN_REQUIRED_COMPONENTS
roscpp
rosbuild_add_library(ros_import src/ros_import.cpp src/ros_import.hh) std_msgs
rosbuild_add_compile_flags(ros_import ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} message_generation
${SOT_CORE_CFLAGS}) std_srvs
rosbuild_add_link_flags(ros_import ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} geometry_msgs
${SOT_CORE_LDFLAGS}) sensor_msgs
target_link_libraries(ros_import ros_bridge) tf2_ros
target_link_libraries(ros_import realtime_tools)
${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
if(BUILD_PYTHON_INTERFACE)
rosbuild_add_library(ros_export src/ros_export.cpp src/ros_export.hh) add_project_dependency(dynamic-graph-python 4.0.0 REQUIRED)
rosbuild_add_compile_flags(ros_export ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} string(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
${SOT_CORE_CFLAGS}) set(CATKIN_REQUIRED_COMPONENTS ${CATKIN_REQUIRED_COMPONENTS} rospy)
rosbuild_add_link_flags(ros_export ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} endif(BUILD_PYTHON_INTERFACE)
${SOT_CORE_LDFLAGS})
target_link_libraries(ros_export ros_bridge) add_project_dependency(Boost REQUIRED COMPONENTS program_options)
target_link_libraries(ros_export add_project_dependency(dynamic_graph_bridge_msgs 0.3.0 REQUIRED)
${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES}) add_project_dependency(sot-core REQUIRED)
rosbuild_add_library(ros_joint_state if(Boost_VERSION GREATER 107299 OR Boost_VERSION_MACRO GREATER 107299)
src/ros_joint_state.cpp src/ros_joint_state) # Silence a warning about a deprecated use of boost bind by boost >= 1.73
rosbuild_add_compile_flags(ros_joint_state # without dropping support for boost < 1.73
${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} add_definitions(-DBOOST_BIND_GLOBAL_PLACEHOLDERS)
${SOT_CORE_CFLAGS}) endif()
rosbuild_add_link_flags(ros_joint_state ${JRL_MAL_LDFLAGS}
${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}) find_package(catkin REQUIRED COMPONENTS ${CATKIN_REQUIRED_COMPONENTS})
target_link_libraries(ros_joint_state ros_bridge)
target_link_libraries(ros_joint_state # Main Library
${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES}) set(${PROJECT_NAME}_HEADERS
include/${PROJECT_NAME}/fwd.hh
rosbuild_add_library(ros_interpreter include/${PROJECT_NAME}/ros_init.hh
src/ros_interpreter.cpp) include/${PROJECT_NAME}/sot_loader.hh
rosbuild_add_compile_flags(ros_interpreter include/${PROJECT_NAME}/sot_loader_basic.hh
${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${DYNAMIC_GRAPH_PYTHON_CFLAGS} include/${PROJECT_NAME}/ros_interpreter.hh
${SOT_CORE_CFLAGS}) src/converter.hh
rosbuild_add_link_flags(ros_interpreter ${JRL_MAL_LDFLAGS} src/sot_to_ros.hh)
${DYNAMIC_GRAPH_LDFLAGS} ${DYNAMIC_GRAPH_PYTHON_LDFLAGS} ${SOT_CORE_LDFLAGS})
target_link_libraries(ros_interpreter ros_bridge) set(${PROJECT_NAME}_SOURCES src/ros_init.cpp src/sot_to_ros.cpp
target_link_libraries(ros_interpreter src/ros_parameter.cpp)
${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES}
${DYNAMIC_GRAPH_PYTHON_LIBRARIES} ${SOT_CORE_LIBRARIES}) add_library(ros_bridge SHARED ${${PROJECT_NAME}_SOURCES}
${${PROJECT_NAME}_HEADERS})
# Stand alone remote dynamic-graph Python interpreter. target_include_directories(ros_bridge SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
rosbuild_add_executable(interpreter src/interpreter.cpp) target_include_directories(ros_bridge PUBLIC $<INSTALL_INTERFACE:include>)
target_link_libraries(interpreter ros_interpreter) target_link_libraries(
rosbuild_add_compile_flags(interpreter ${SOT_CORE_CFLAGS}) ros_bridge ${catkin_LIBRARIES} sot-core::sot-core pinocchio::pinocchio
rosbuild_add_link_flags(interpreter ${JRL_MAL_LDFLAGS} dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
${DYNAMIC_GRAPH_LDFLAGS} ${DYNAMIC_GRAPH_PYTHON_LDFLAGS} ${SOT_CORE_LDFLAGS})
if(SUFFIX_SO_VERSION)
INSTALL(TARGETS ros_bridge DESTINATION lib) set_target_properties(ros_bridge PROPERTIES SOVERSION ${PROJECT_VERSION})
INSTALL(TARGETS ros_import DESTINATION lib) endif(SUFFIX_SO_VERSION)
INSTALL(TARGETS ros_export DESTINATION lib)
INSTALL(TARGETS ros_joint_state DESTINATION lib) if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(TARGETS interpreter DESTINATION lib) install(
INSTALL(TARGETS ros_interpreter DESTINATION bin) TARGETS ros_bridge
EXPORT ${TARGETS_EXPORT_NAME}
INCLUDE(cmake/python.cmake) DESTINATION lib)
endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INCLUDE_DIRECTORIES(${DYNAMIC_GRAPH_INCLUDE_DIRS})
LINK_DIRECTORIES(${DYNAMIC_GRAPH_LIBRARY_DIRS}) add_subdirectory(src)
add_subdirectory(tests)
DYNAMIC_GRAPH_PYTHON_MODULE("ros/ros_import"
ros_import # install ros executables
ros/ros_import/wrap install(PROGRAMS scripts/robot_pose_publisher scripts/run_command
) scripts/tf_publisher DESTINATION share/${PROJECT_NAME})
SET_TARGET_PROPERTIES(ros/ros_import/wrap
PROPERTIES # Install package information
COMPILE_FLAGS install(FILES manifest.xml package.xml DESTINATION share/${PROJECT_NAME})
${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(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(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)
LICENSE 0 → 100644
View file @ c2c3026d
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
View file @ c2c3026d
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 This ROS package binds together the ROS framework with the
dynamic-graph real-time control architecture. dynamic-graph real-time control architecture.
cmake @ 57c46b2d
Subproject commit 283f294850e15b9f86708be61d53441ecc0419da Subproject commit 57c46b2d7b26bb0c25f8f98cfc2a9868e03be603
doc/Doxyfile.extra.in 0 → 100644
View file @ c2c3026d
INPUT = @PROJECT_SOURCE_DIR@/include \
@PROJECT_SOURCE_DIR@/doc
doc/mainpage.hh 0 → 100644
View file @ c2c3026d
/// \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 0 → 100644
View file @ c2c3026d
/*
* 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
View file @ c2c3026d
#ifndef ROS_INIT_HH #ifndef ROS_INIT_HH
# define ROS_INIT_HH #define ROS_INIT_HH
# include <ros/ros.h> #include <ros/ros.h>
namespace dynamicgraph namespace dynamicgraph {
{ ros::NodeHandle& rosInit(bool createAsyncSpinner = false,
ros::NodeHandle& rosInit (bool createAsyncSpinner); bool createMultiThreadSpinner = true);
/// \brief Return spinner or throw an exception if spinner /// \brief Return spinner or throw an exception if spinner
/// creation has been disabled at startup. /// creation has been disabled at startup.
ros::AsyncSpinner& spinner (); ros::AsyncSpinner& spinner();
} // end of namespace dynamicgraph. /// \brief Return multi threaded spinner or throw an exception if spinner
/// creation has been disabled at startup.
ros::MultiThreadedSpinner& mtSpinner();
#endif //! ROS_INIT_HH } // end of namespace dynamicgraph.
#endif //! ROS_INIT_HH
include/dynamic_graph_bridge/ros_interpreter.hh
View file @ c2c3026d
#ifndef DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH #ifndef DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
# define DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH #define DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
# include <ros/ros.h> #pragma GCC diagnostic push
# include <dynamic_graph_bridge/RunCommand.h> #pragma GCC system_header
#include <ros/ros.h>
# include <dynamic-graph/python/interpreter.hh> #pragma GCC diagnostic pop
namespace dynamicgraph #include <dynamic_graph_bridge_msgs/RunCommand.h>
{ #include <dynamic_graph_bridge_msgs/RunPythonFile.h>
/// \brief This class wraps the implementation of the runCommand
/// service. #include <dynamic-graph/python/interpreter.hh>
/// #include <dynamic_graph_bridge/fwd.hh>
/// This takes as input a ROS node handle and do not handle the
/// callback so that the service behavior can be controlled from namespace dynamicgraph {
/// the outside. /// \brief This class wraps the implementation of the runCommand
class Interpreter /// service.
{ ///
public: /// This takes as input a ROS node handle and do not handle the
typedef boost::function< /// callback so that the service behavior can be controlled from
bool (dynamic_graph_bridge::RunCommand::Request&, /// the outside.
dynamic_graph_bridge::RunCommand::Response&)> class Interpreter {
runCommandCallback_t; public:
typedef boost::function<bool(
explicit Interpreter (ros::NodeHandle& nodeHandle); dynamic_graph_bridge_msgs::RunCommand::Request&,
dynamic_graph_bridge_msgs::RunCommand::Response&)>
/// \brief Run a command and return result. runCommandCallback_t;
std::string runCommand (const std::string& command);
typedef boost::function<bool(
protected: dynamic_graph_bridge_msgs::RunPythonFile::Request&,
/// \brief Run a Python command and return result, stderr and stdout. dynamic_graph_bridge_msgs::RunPythonFile::Response&)>
bool runCommandCallback (dynamic_graph_bridge::RunCommand::Request& req, runPythonFileCallback_t;
dynamic_graph_bridge::RunCommand::Response& res);
explicit Interpreter(ros::NodeHandle& nodeHandle);
private:
python::Interpreter interpreter_; /// \brief Method to start python interpreter and deal with messages.
ros::NodeHandle& nodeHandle_; /// \param Command string to execute, result, stdout, stderr strings.
ros::ServiceServer runCommandSrv_; void runCommand(const std::string& command, std::string& result,
}; std::string& out, std::string& err);
} // end of namespace dynamicgraph.
/// \brief Method to parse python scripts.
#endif //! DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH /// \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 0 → 100644
View file @ c2c3026d
#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 0 → 100644
View file @ c2c3026d
/*
* 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 0 → 100644
View file @ c2c3026d
/*
* 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
View file @ c2c3026d
...@@ -2,12 +2,8 @@ ...@@ -2,12 +2,8 @@
\mainpage \mainpage
\htmlinclude manifest.html \htmlinclude manifest.html
\b dynamic_graph is ... \b dynamic_graph_bridge provides communication tools for
ROS/dynamic-graph interoperation.
<!--
Provide an overview of your package.
-->
\section codeapi Code API \section codeapi Code API
......
manifest.xml
View file @ c2c3026d
...@@ -7,28 +7,33 @@ ...@@ -7,28 +7,33 @@
<author>Thomas Moulard</author> <author>Thomas Moulard</author>
<license>BSD</license> <license>BSD</license>
<review status="unreviewed" notes=""/> <review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/dynamic-graph-bridge</url> <url>http://ros.org/wiki/dynamic_graph_bridge</url>
<export> <export>
<cpp <cpp
cflags="-I${prefix}/include" cflags="-I${prefix}/include"
lflags="-L${prefix}/lib -lros_bridge -Wl,-rpath,${prefix}/lib" lflags="-L${prefix}/lib -lros_bridge -lros_interpreter -Wl,-rpath,${prefix}/lib"
/> />
<rosdoc config="rosdoc.yaml" />
</export> </export>
<rosdep name="boost"/> <rosdep name="boost"/>
<!-- Disable non-standard dependencies for now
<rosdep name="jrl-mal"/> <rosdep name="jrl-mal"/>
<rosdep name="dynamic-graph"/> <rosdep name="dynamic-graph"/>
<rosdep name="sot-core"/> <rosdep name="sot-core"/>
<rosdep name="sot-dynamic"/>
-->
<depend package="std_msgs"/> <depend package="std_msgs"/>
<depend package="std_srvs"/>
<depend package="roscpp"/> <depend package="roscpp"/>
<depend package="geometry_msgs"/> <depend package="geometry_msgs"/>
<depend package="sensor_msgs"/> <depend package="sensor_msgs"/>
<depend package="bullet"/>
<depend package="tf"/> <depend package="tf"/>
<depend package="realtime_tools"/> <depend package="realtime_tools"/>
<depend package="dynamic_graph_bridge_msgs"/>
</package> </package>
msg/Matrix.msg deleted 100644 → 0
View file @ 85f265ca
float64[] data
uint32 width