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 (22)
Hide whitespace changes
Inline Side-by-side
Showing
with 309 additions and 243 deletions
.git-blame-ignore-revs 0 → 100644
View file @ c2c3026d
# format (Guilhem Saurel, 2022-09-06)
426c21a7f4e5b445d17825235f983e2f8e1a86ec
.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
.travis.yml deleted 100644 → 0
View file @ 4db43cf4
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"
CMakeLists.txt
View file @ c2c3026d
......@@ -3,100 +3,100 @@
# 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)
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})
README.md
View file @ c2c3026d
dynamic-graph bindings
======================
# Dynamic Graph bindings
[![Building Status](https://travis-ci.org/stack-of-tasks/dynamic_graph_bridge.svg?branch=master)](https://travis-ci.org/stack-of-tasks/dynamic_graph_bridge)
[![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)](http://projects.laas.fr/gepetto/doc/stack-of-tasks/dynamic_graph_bridge/master/coverage/)
[![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
Compare 7a5475bc...57c46b2d
Subproject commit 7a5475bcbac62643eb5c03744f1ee16f83607fe2
Subproject commit 57c46b2d7b26bb0c25f8f98cfc2a9868e03be603
doc/Doxyfile.extra.in
View file @ c2c3026d
INPUT = @PROJECT_SOURCE_DIR@/include \
@PROJECT_SOURCE_DIR@/doc
include/dynamic_graph_bridge/fwd.hh
View file @ c2c3026d
......@@ -9,12 +9,12 @@
#ifndef DYNAMIC_GRAPH_BRIDGE_FWD_HH
#define DYNAMIC_GRAPH_BRIDGE_FWD_HH
#include <dynamic-graph/python/fwd.hh>
#include <dynamic-graph/fwd.hh>
namespace dynamicgraph {
// classes defined in sot-core
class Interpreter;
typedef 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
include/dynamic_graph_bridge/ros_init.hh
View file @ c2c3026d
......@@ -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.
......
include/dynamic_graph_bridge/ros_interpreter.hh
View file @ c2c3026d
......@@ -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_;
......
include/dynamic_graph_bridge/sot_loader.hh
View file @ c2c3026d
......@@ -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>
......
include/dynamic_graph_bridge/sot_loader_basic.hh
View file @ c2c3026d
......@@ -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();
......
package.xml
View file @ c2c3026d
<package format="2">
<name>dynamic_graph_bridge</name>
<version>3.4.2</version>
<version>3.4.5</version>
<description>
ROS bindings for dynamic graph.
......
pyproject.toml 0 → 100644
View file @ c2c3026d
[tool.black]
exclude = "cmake"
scripts/robot_pose_publisher
View file @ c2c3026d
......@@ -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()
scripts/run_command
View file @ c2c3026d
......@@ -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,50 +62,53 @@ 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
return self.runcode(code, False)
response = self.client(str(source))
if response.standardoutput != "":
print response.standardoutput[:-1]
print(response.standardoutput[:-1])
if response.standarderror != "":
print response.standarderror[:-1]
print(response.standarderror[:-1])
elif response.result != "None":
print response.result
except rospy.ServiceException, e:
print(response.result)
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:
return self.runcode(c)
else:
return True
except SyntaxError, OverflowError:
except (SyntaxError, OverflowError):
self.showsyntaxerror()
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.")
scripts/tf_publisher
View file @ c2c3026d
......@@ -6,6 +6,8 @@
# through dynamic_graph_bridge.
#
import logging
import rospy
import tf
......@@ -13,15 +15,15 @@ import geometry_msgs.msg
def main():
rospy.init_node('tf_publisher', anonymous=True)
rospy.init_node("tf_publisher", anonymous=True)
frame = rospy.get_param('~frame', '')
childFrame = rospy.get_param('~child_frame', '')
topic = rospy.get_param('~topic', '')
rateSeconds = rospy.get_param('~rate', 5)
frame = rospy.get_param("~frame", "")
childFrame = rospy.get_param("~child_frame", "")
topic = rospy.get_param("~topic", "")
rateSeconds = rospy.get_param("~rate", 5)
if not frame or not childFrame or not topic:
logpy.error("frame, childFrame and topic are required parameters")
logging.error("frame, childFrame and topic are required parameters")
return
rate = rospy.Rate(rateSeconds)
......@@ -35,11 +37,10 @@ def main():
ok = False
while not rospy.is_shutdown() and not ok:
try:
tl.waitForTransform(childFrame, frame,
rospy.Time(), rospy.Duration(0.1))
tl.waitForTransform(childFrame, frame, rospy.Time(), rospy.Duration(0.1))
ok = True
except tf.Exception, e:
rospy.logwarn("waiting for tf transform")
except tf.Exception:
logging.warning("waiting for tf transform")
ok = False
while not rospy.is_shutdown():
......@@ -60,4 +61,5 @@ def main():
pub.publish(transform)
rate.sleep()
main()
setup.cfg 0 → 100644
View file @ c2c3026d
[flake8]
exclude = cmake
max-line-length = 88
extend-ignore = E203
src/CMakeLists.txt
View file @ c2c3026d
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
dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
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}
dynamic-graph-python::dynamic-graph-python
dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
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
dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
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
dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
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)
src/converter.hh
View file @ c2c3026d
#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);
......