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 (332)
232 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 524 additions and 249 deletions
.git-blame-ignore-revs 0 → 100644
View file @ c2c3026d
# format (Guilhem Saurel, 2022-09-06)
426c21a7f4e5b445d17825235f983e2f8e1a86ec
.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
# Copyright (C) 2008-2013 LAAS-CNRS, JRL AIST-CNRS. # Copyright (C) 2008-2020 LAAS-CNRS, JRL AIST-CNRS.
# #
# Author: Florent Lamiraux, Nirmal Giftsun # Author: Florent Lamiraux, Nirmal Giftsun, Guilhem Saurel
# #
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
# Catkin part
cmake_minimum_required(VERSION 2.4.6)
project(dynamic_graph_bridge)
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation std_srvs geometry_msgs sensor_msgs tf realtime_tools)
find_package(Boost REQUIRED COMPONENTS program_options)
add_message_files(FILES Matrix.msg Vector.msg)
add_service_files(FILES RunCommand.srv) cmake_minimum_required(VERSION 3.1)
generate_messages( # Project properties
DEPENDENCIES set(PROJECT_ORG stack-of-tasks)
dynamic_graph_bridge set(PROJECT_NAME dynamic_graph_bridge)
std_msgs set(PROJECT_DESCRIPTION "Dynamic graph bridge library")
) set(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
catkin_package(CATKIN_DEPENDS message_runtime)
# Project options
option(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
## LAAS cmake submodule part # Project configuration
set(PROJECT_DESCRIPTION "Dynamic graph bridge library") set(PROJECT_USE_CMAKE_EXPORT TRUE)
set(PROJECT_NAME dynamic_graph_bridge) set(CUSTOM_HEADER_DIR ${PROJECT_NAME})
set(PROJECT_URL "") set(CXX_DISABLE_WERROR FALSE)
set(DOXYGEN_USE_MATHJAX YES)
set(CATKIN_ENABLE_TESTING OFF)
set(CXX_DISABLE_WERROR False) # JRL-cmakemodule setup
set(CUSTOM_HEADER_DIR dynamic_graph_bridge)
set(${PROJECT_NAME}_HEADERS
include/dynamic_graph_bridge/ros_init.hh
include/dynamic_graph_bridge/ros_interpreter.hh
)
include(cmake/base.cmake) include(cmake/base.cmake)
include(cmake/GNUInstallDirs.cmake) include(cmake/boost.cmake)
include(cmake/python.cmake) include(cmake/ros.cmake)
SETUP_PROJECT() # Project definition
compute_project_args(PROJECT_ARGS LANGUAGES CXX)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin) project(${PROJECT_NAME} ${PROJECT_ARGS})
set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib)
set(CMAKE_INSTALL_RPATH "${LIBRARY_OUTPUT_PATH}") # Project dependencies
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${LIBRARY_OUTPUT_PATH}") set(CATKIN_REQUIRED_COMPONENTS
set(PKG_CONFIG_ADDITIONAL_VARIABLES roscpp
${PKG_CONFIG_ADDITIONAL_VARIABLES} std_msgs
plugindirname message_generation
plugindir std_srvs
) geometry_msgs
sensor_msgs
add_required_dependency(roscpp) tf2_ros
add_required_dependency("realtime_tools >= 1.8") realtime_tools)
add_required_dependency(bullet)
add_required_dependency(jrl-mal) if(BUILD_PYTHON_INTERFACE)
add_required_dependency(dynamic-graph) add_project_dependency(dynamic-graph-python 4.0.0 REQUIRED)
add_required_dependency(dynamic-graph-python) string(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
add_required_dependency(sot-core) set(CATKIN_REQUIRED_COMPONENTS ${CATKIN_REQUIRED_COMPONENTS} rospy)
add_required_dependency(sot-dynamic) endif(BUILD_PYTHON_INTERFACE)
add_required_dependency(jrl-dynamics-urdf)
add_project_dependency(Boost REQUIRED COMPONENTS program_options)
add_library(ros_bridge add_project_dependency(dynamic_graph_bridge_msgs 0.3.0 REQUIRED)
src/converter.hh add_project_dependency(sot-core REQUIRED)
include/dynamic_graph_bridge/ros_init.hh src/ros_init.cpp
src/sot_to_ros.hh src/sot_to_ros.cpp 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
pkg_config_use_dependency(ros_bridge jrl-mal) # without dropping support for boost < 1.73
pkg_config_use_dependency(ros_bridge bullet) add_definitions(-DBOOST_BIND_GLOBAL_PLACEHOLDERS)
install(TARGETS ros_bridge DESTINATION lib) endif()
# Make sure rpath are preserved during the install as ROS dependencies find_package(catkin REQUIRED COMPONENTS ${CATKIN_REQUIRED_COMPONENTS})
# are not installed.
set_target_properties(ros_bridge PROPERTIES BUILD_WITH_INSTALL_RPATH True) # Main Library
set(${PROJECT_NAME}_HEADERS
macro(compile_plugin NAME) include/${PROJECT_NAME}/fwd.hh
message(lib path ${LIBRARY_OUTPUT_PATH}) include/${PROJECT_NAME}/ros_init.hh
file(MAKE_DIRECTORY "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/${NAME}") include/${PROJECT_NAME}/sot_loader.hh
add_library(${NAME} src/${NAME}.cpp src/${NAME}.hh) include/${PROJECT_NAME}/sot_loader_basic.hh
pkg_config_use_dependency(${NAME} jrl-mal) include/${PROJECT_NAME}/ros_interpreter.hh
pkg_config_use_dependency(${NAME} dynamic-graph) src/converter.hh
pkg_config_use_dependency(${NAME} sot-core) src/sot_to_ros.hh)
pkg_config_use_dependency(${NAME} jrl-dynamics-urdf)
add_dependencies(${NAME} ros_bridge) set(${PROJECT_NAME}_SOURCES src/ros_init.cpp src/sot_to_ros.cpp
target_link_libraries(${NAME} ros_bridge) src/ros_parameter.cpp)
set_target_properties(${NAME} PROPERTIES BUILD_WITH_INSTALL_RPATH True)
install(TARGETS ${NAME} DESTINATION lib) add_library(ros_bridge SHARED ${${PROJECT_NAME}_SOURCES}
${${PROJECT_NAME}_HEADERS})
target_include_directories(ros_bridge SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
dynamic_graph_python_module("ros/${NAME}" target_include_directories(ros_bridge PUBLIC $<INSTALL_INTERFACE:include>)
${NAME} target_link_libraries(
ros/${NAME}/wrap ros_bridge ${catkin_LIBRARIES} sot-core::sot-core pinocchio::pinocchio
) dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap realtime_tools) if(SUFFIX_SO_VERSION)
PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap jrl-mal) set_target_properties(ros_bridge PROPERTIES SOVERSION ${PROJECT_VERSION})
PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap dynamic_graph) endif(SUFFIX_SO_VERSION)
PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap sot-core)
endmacro() if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
install(
include(cmake/python.cmake) TARGETS ros_bridge
EXPORT ${TARGETS_EXPORT_NAME}
compile_plugin(ros_publish) DESTINATION lib)
compile_plugin(ros_subscribe) endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
compile_plugin(ros_time)
compile_plugin(ros_joint_state)
target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dynamic.so")
compile_plugin(robot_model)
# ros_interperter library.
add_library(ros_interpreter src/ros_interpreter.cpp)
pkg_config_use_dependency(ros_interpreter jrl-mal)
pkg_config_use_dependency(ros_interpreter dynamic-graph)
pkg_config_use_dependency(ros_interpreter sot-core)
pkg_config_use_dependency(ros_interpreter roscpp)
add_dependencies(ros_interpreter ros_bridge)
target_link_libraries(ros_interpreter ros_bridge)
set_target_properties(ros_interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
message(cmakeinstalllibdir ${CMAKE_INSTALL_LIBDIR} )
install(TARGETS ros_interpreter DESTINATION ${CMAKE_INSTALL_LIBDIR})
# Stand alone remote dynamic-graph Python interpreter.
add_executable(interpreter src/interpreter.cpp)
add_dependencies(interpreter ros_interpreter)
target_link_libraries(interpreter ros_interpreter)
pkg_config_use_dependency(interpreter jrl-mal)
pkg_config_use_dependency(interpreter dynamic-graph)
pkg_config_use_dependency(interpreter sot-core)
pkg_config_use_dependency(interpreter sot-dynamic)
# set_target_properties(interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
#install(TARGETS interpreter DESTINATION bin)
# Stand alone embedded intepreter with a robot controller.
add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp)
pkg_config_use_dependency(geometric_simu roscpp)
target_link_libraries(geometric_simu ${Boost_LIBRARIES})
add_subdirectory(src) add_subdirectory(src)
add_subdirectory(tests)
#install ros executables # install ros executables
install(PROGRAMS install(PROGRAMS scripts/robot_pose_publisher scripts/run_command
${CMAKE_SOURCE_DIR}/scripts/robot_pose_publisher scripts/tf_publisher DESTINATION share/${PROJECT_NAME})
${CMAKE_SOURCE_DIR}/scripts/run_command
${CMAKE_SOURCE_DIR}/scripts/tf_publisher
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
SETUP_PROJECT_FINALIZE()
# Install package information
install(FILES manifest.xml package.xml DESTINATION share/${PROJECT_NAME})
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 74528c9829ca4e2bbeb3ed3605c2d686cceac7e0 Subproject commit 57c46b2d7b26bb0c25f8f98cfc2a9868e03be603
doc/Doxyfile.extra.in
View file @ c2c3026d
INPUT = @PROJECT_SOURCE_DIR@/include \ INPUT = @PROJECT_SOURCE_DIR@/include \
@PROJECT_SOURCE_DIR@/doc @PROJECT_SOURCE_DIR@/doc
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=false, bool createMultiThreadSpinner=true); 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();
/// \brief Return multi threaded spinner or throw an exception if spinner /// \brief Return multi threaded spinner or throw an exception if spinner
/// creation has been disabled at startup. /// creation has been disabled at startup.
ros::MultiThreadedSpinner& mtSpinner (); ros::MultiThreadedSpinner& mtSpinner();
} // end of namespace dynamicgraph.
} // end of namespace dynamicgraph. #endif //! ROS_INIT_HH
#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 <dynamic-graph/python/interpreter.hh> #include <ros/ros.h>
#pragma GCC diagnostic pop
namespace dynamicgraph
{ #include <dynamic_graph_bridge_msgs/RunCommand.h>
/// \brief This class wraps the implementation of the runCommand #include <dynamic_graph_bridge_msgs/RunPythonFile.h>
/// service.
/// #include <dynamic-graph/python/interpreter.hh>
/// This takes as input a ROS node handle and do not handle the #include <dynamic_graph_bridge/fwd.hh>
/// callback so that the service behavior can be controlled from
/// the outside. namespace dynamicgraph {
class Interpreter /// \brief This class wraps the implementation of the runCommand
{ /// service.
public: ///
typedef boost::function< /// This takes as input a ROS node handle and do not handle the
bool (dynamic_graph_bridge::RunCommand::Request&, /// callback so that the service behavior can be controlled from
dynamic_graph_bridge::RunCommand::Response&)> /// the outside.
runCommandCallback_t; class Interpreter {
public:
explicit Interpreter (ros::NodeHandle& nodeHandle); typedef boost::function<bool(
dynamic_graph_bridge_msgs::RunCommand::Request&,
/// \brief Run a command and return result. dynamic_graph_bridge_msgs::RunCommand::Response&)>
std::string runCommand (const std::string& command) __attribute__ ((deprecated)); runCommandCallback_t;
/// \brief Method to start python interpreter and deal with messages.
/// \param Command string to execute, result, stdout, stderr strings. typedef boost::function<bool(
void runCommand(const std::string & command, std::string &result, dynamic_graph_bridge_msgs::RunPythonFile::Request&,
std::string &out, std::string &err); dynamic_graph_bridge_msgs::RunPythonFile::Response&)>
runPythonFileCallback_t;
/// Initialize service run_command
void startRosService (); explicit Interpreter(ros::NodeHandle& nodeHandle);
protected: /// \brief Method to start python interpreter and deal with messages.
/// \brief Run a Python command and return result, stderr and stdout. /// \param Command string to execute, result, stdout, stderr strings.
bool runCommandCallback (dynamic_graph_bridge::RunCommand::Request& req, void runCommand(const std::string& command, std::string& result,
dynamic_graph_bridge::RunCommand::Response& res); std::string& out, std::string& err);
private: /// \brief Method to parse python scripts.
python::Interpreter interpreter_; /// \param Input file name to parse.
ros::NodeHandle& nodeHandle_; void runPythonFile(std::string ifilename);
ros::ServiceServer runCommandSrv_;
}; /// Initialize service run_command
} // end of namespace dynamicgraph. void startRosService();
#endif //! DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH 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_ */
src/sot_loader.hh → include/dynamic_graph_bridge/sot_loader_basic.hh
View file @ c2c3026d
/* /*
* Copyright 2011, * Copyright 2016,
* Olivier Stasse, * Olivier Stasse,
* *
* CNRS * CNRS
* *
* This file is part of sot-core.
* sot-core is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-core is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-core. If not, see <http://www.gnu.org/licenses/>.
*/ */
/* -------------------------------------------------------------------------- */ /* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */ /* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */ /* -------------------------------------------------------------------------- */
#ifndef _SOT_LOADER_BASIC_HH_
#define _SOT_LOADER_BASIC_HH_
// System includes // System includes
#include <iostream>
#include <cassert> #include <cassert>
// STL includes // STL includes
#include <map> #include <map>
// Pinocchio includes
#include <pinocchio/fwd.hpp>
// Boost includes // Boost includes
#include <boost/program_options.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/program_options.hpp>
// ROS includes // ROS includes
#include <sensor_msgs/JointState.h>
#include "ros/ros.h" #include "ros/ros.h"
#include "std_srvs/Empty.h" #include "std_srvs/Empty.h"
#include <sensor_msgs/JointState.h>
// Sot Framework includes // Sot Framework includes
#include <sot/core/debug.hh>
#include <sot/core/abstract-sot-external-interface.hh> #include <sot/core/abstract-sot-external-interface.hh>
#include <sot/core/debug.hh>
namespace po = boost::program_options; namespace po = boost::program_options;
namespace dgs = dynamicgraph::sot; namespace dgs = dynamicgraph::sot;
class SotLoaderBasic {
class SotLoader { protected:
protected:
// Dynamic graph is stopped. // Dynamic graph is stopped.
bool dynamic_graph_stopped_; bool dynamic_graph_stopped_;
/// \brief the sot-hrp2 controller /// \brief the sot-hrp2 controller
dgs::AbstractSotExternalInterface * sotController_; dgs::AbstractSotExternalInterface* sotController_;
po::variables_map vm_; po::variables_map vm_;
std::string dynamicLibraryName_; std::string dynamicLibraryName_;
/// Map of sensor readings /// \brief Handle on the SoT library.
std::map <std::string,dgs::SensorValues> sensorsIn_; void* sotRobotControllerLibrary_;
/// 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_;
/// \brief Map between SoT state vector and some joint_state_links /// \brief Map between SoT state vector and some joint_state_links
XmlRpc::XmlRpcValue stateVectorMap_; XmlRpc::XmlRpcValue stateVectorMap_;
/// \brief List of parallel joints from the state vector. /// \brief List of parallel joints from the state vector.
std::vector<int> parallel_joints_to_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. /// \brief Coefficient between parallel joints and the state vector.
std::vector<double> coefficient_parallel_joints_; 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. // Joint state publication.
ros::Publisher joint_pub_; ros::Publisher joint_pub_;
// Joint state to be published. // Joint state to be published.
sensor_msgs::JointState joint_state_; sensor_msgs::JointState joint_state_;
// Number of DOFs according to KDL. // Number of DOFs according to KDL.
int nbOfJoints_; int nbOfJoints_;
int nbOfParallelJoints_; parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
public:
public: SotLoaderBasic();
SotLoader(); ~SotLoaderBasic(){};
~SotLoader() {};
// \brief Read user input to extract the path of the SoT dynamic library. // \brief Read user input to extract the path of the SoT dynamic library.
int parseOptions(int argc, char *argv[]); int parseOptions(int argc, char* argv[]);
// \brief Load the SoT /// \brief Load the SoT device corresponding to the robot.
void Initialization(); void Initialization();
// \brief Compute one iteration of control. /// \brief Unload the library which handles the robot device.
// Basically calls fillSensors, the SoT and the readControl. void CleanUp();
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. // \brief Create a thread for ROS.
void setup(); virtual void initializeRosNode(int argc, char* argv[]);
// \brief Callback function when starting dynamic graph. // \brief Callback function when starting dynamic graph.
bool start_dg(std_srvs::Empty::Request& request, bool start_dg(std_srvs::Empty::Request& request,
std_srvs::Empty::Response& response); std_srvs::Empty::Response& response);
// \brief Callback function when stopping dynamic graph. // \brief Callback function when stopping dynamic graph.
bool stop_dg(std_srvs::Empty::Request& request, bool stop_dg(std_srvs::Empty::Request& request,
std_srvs::Empty::Response& response); std_srvs::Empty::Response& response);
// \brief Read the state vector description based upon the robot links. // \brief Read the state vector description based upon the robot links.
int readSotVectorStateParam(); int readSotVectorStateParam();
...@@ -141,7 +109,10 @@ public: ...@@ -141,7 +109,10 @@ public:
int initPublication(); int initPublication();
// \brief Get Status of dg. // \brief Get Status of dg.
bool isDynamicGraphStopped() bool isDynamicGraphStopped() { return dynamic_graph_stopped_; }
{ return dynamic_graph_stopped_; }
// \brief Specify the name of the dynamic library.
void setDynamicLibraryName(std::string& afilename);
}; };
#endif /* _SOT_LOADER_BASIC_HH_ */
manifest.xml
View file @ c2c3026d
...@@ -35,6 +35,5 @@ ...@@ -35,6 +35,5 @@
<depend package="tf"/> <depend package="tf"/>
<depend package="realtime_tools"/> <depend package="realtime_tools"/>
<depend package="dynamic_graph_bridge_msgs"/>
<depend package="jrl_dynamics_urdf"/>
</package> </package>
msg/Matrix.msg deleted 100644 → 0
View file @ bf393bd2
float64[] data
uint32 width
msg/Vector.msg deleted 100644 → 0
View file @ bf393bd2
float64[] data
package.xml
View file @ c2c3026d
<package> <package format="2">
<name>dynamic_graph_bridge</name> <name>dynamic_graph_bridge</name>
<version>2.0.0</version> <version>3.4.5</version>
<description> <description>
URDF parser for jrl-dynamics ROS bindings for dynamic graph.
</description> </description>
<maintainer email="hpp@laas.fr">hpp@laas.fr</maintainer> <maintainer email="guilhem.saurel@laas.fr">guilhem.saurel@laas.fr</maintainer>
<author email="hpp@laas.fr">hpp@laas.fr</author> <author email="hpp@laas.fr">hpp@laas.fr</author>
<license>LGPL</license> <license>LGPL</license>
<url>http://ros.org/wiki/jrl_dynamics_urdf</url> <url>http://ros.org/wiki/dynamic_graph_bridge</url>
<export> <export>
<cpp
cflags="-I${prefix}/include"
lflags="-L${prefix}/lib -lros_bridge -lros_interpreter -Wl,-rpath,${prefix}/lib"
/>
<rosdoc config="rosdoc.yaml" /> <rosdoc config="rosdoc.yaml" />
</export> </export>
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>catkin</buildtool_depend>
<build_depend>std_msgs</build_depend> <build_depend>std_msgs</build_depend>
<build_depend>std_srvs</build_depend> <build_depend>std_srvs</build_depend>
<build_depend>roscpp</build_depend> <build_depend>roscpp</build_depend>
<build_depend>geometry_msgs</build_depend> <build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend> <build_depend>sensor_msgs</build_depend>
<build_depend>tf</build_depend> <build_depend>tf2_ros</build_depend>
<build_depend>realtime_tools</build_depend> <build_depend>realtime_tools</build_depend>
<build_depend>jrl_dynamics_urdf</build_depend> <build_depend>message_generation</build_depend>
<build_depend>message_generation</build_depend> <build_depend>message_runtime</build_depend>
<build_depend>dynamic-graph</build_depend>
<build_depend>dynamic-graph-python</build_depend>
<run_depend>std_msgs</run_depend> <build_depend>sot-core</build_depend>
<run_depend>std_srvs</run_depend> <build_depend>sot-dynamic-pinocchio</build_depend>
<run_depend>roscpp</run_depend> <build_depend>dynamic_graph_bridge_msgs</build_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>sensor_msgs</run_depend> <exec_depend>std_msgs</exec_depend>
<run_depend>tf</run_depend> <exec_depend>std_srvs</exec_depend>
<run_depend>realtime_tools</run_depend> <exec_depend>roscpp</exec_depend>
<run_depend>jrl_dynamics_urdf</run_depend> <exec_depend>geometry_msgs</exec_depend>
<run_depend>message_runtime</run_depend> <exec_depend>sensor_msgs</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>realtime_tools</exec_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>dynamic-graph</exec_depend>
<exec_depend>dynamic-graph-python</exec_depend>
<exec_depend>sot-core</exec_depend>
<exec_depend>sot-dynamic-pinocchio</exec_depend>
<exec_depend>dynamic_graph_bridge_msgs</exec_depend>
</package> </package>
pyproject.toml 0 → 100644
View file @ c2c3026d
[tool.black]
exclude = "cmake"