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
Hide whitespace changes
Inline Side-by-side
Showing
with 1281 additions and 1782 deletions
setup.cfg 0 → 100644
View file @ c2c3026d
[flake8]
exclude = cmake
max-line-length = 88
extend-ignore = E203
src/CMakeLists.txt
View file @ c2c3026d
IF(BUILD_PYTHON_INTERFACE)
INSTALL(FILES
"dynamic_graph/ros/__init__.py"
"dynamic_graph/ros/ros.py"
"dynamic_graph/ros/dgcompleter.py"
DESTINATION "${PYTHON_SITELIB}/dynamic_graph/ros"
)
ENDIF(BUILD_PYTHON_INTERFACE)
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)
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)
if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
install(
TARGETS ${LIBRARY_NAME}
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
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")
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")
endif()
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")
# 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)
install(
TARGETS ros_interpreter
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION lib)
endif(BUILD_PYTHON_INTERFACE)
# Stand alone embedded intepreter with a robot controller.
add_executable(geometric_simu geometric_simu.cpp sot_loader.cpp
sot_loader_basic.cpp)
target_link_libraries(geometric_simu Boost::program_options ${CMAKE_DL_LIBS}
${catkin_LIBRARIES} ros_bridge)
install(TARGETS geometric_simu DESTINATION lib/${PROJECT_NAME})
# Sot loader library
add_library(sot_loader sot_loader.cpp sot_loader_basic.cpp)
target_link_libraries(sot_loader Boost::program_options ${catkin_LIBRARIES}
ros_bridge)
install(
TARGETS sot_loader
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION lib)
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 <boost/static_assert.hpp>
# include <boost/date_time/date.hpp>
# include <boost/date_time/posix_time/posix_time.hpp>
# include <ros/time.h>
# include <std_msgs/Header.h>
# include <LinearMath/btMatrix3x3.h>
# include <LinearMath/btQuaternion.h>
# 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)
namespace dynamicgraph
{
inline
void
makeHeader(std_msgs::Header& header)
{
header.seq = 0;
header.stamp = ros::Time::now ();
header.frame_id = "/dynamic_graph/world";
#define DYNAMIC_GRAPH_ROS_CONVERTER_HH
#include <ros/time.h>
#include <std_msgs/Header.h>
#include <boost/date_time/date.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/static_assert.hpp>
#include <stdexcept>
#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 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) {
header.seq = 0;
header.stamp = ros::Time::now();
header.frame_id = "/dynamic_graph/world";
}
/// \brief Handle ROS <-> dynamic-graph conversion.
///
/// Implements all ROS/dynamic-graph conversions required by the
/// bridge.
///
/// Relies on the SotToRos type trait to make sure valid pair of
/// types are used.
template <typename D, typename S>
void converter(D& dst, const S& src);
// Boolean
SOT_TO_ROS_IMPL(bool) { dst.data = src; }
ROS_TO_SOT_IMPL(bool) { dst = src.data; }
// Double
SOT_TO_ROS_IMPL(double) { dst.data = src; }
ROS_TO_SOT_IMPL(double) { dst = src.data; }
// Int
SOT_TO_ROS_IMPL(int) { dst.data = src; }
ROS_TO_SOT_IMPL(int) { dst = src.data; }
// Unsigned
SOT_TO_ROS_IMPL(unsigned int) { dst.data = src; }
ROS_TO_SOT_IMPL(unsigned int) { dst = src.data; }
// String
SOT_TO_ROS_IMPL(std::string) { dst.data = src; }
ROS_TO_SOT_IMPL(std::string) { dst = src.data; }
// Vector
SOT_TO_ROS_IMPL(Vector) {
dst.data.resize(src.size());
for (int i = 0; i < src.size(); ++i) dst.data[i] = src(i);
}
ROS_TO_SOT_IMPL(Vector) {
dst.resize(src.data.size());
for (unsigned int i = 0; i < src.data.size(); ++i) dst(i) = src.data[i];
}
// Vector3
SOT_TO_ROS_IMPL(specific::Vector3) {
if (src.size() > 0) {
dst.x = src(0);
if (src.size() > 1) {
dst.y = src(1);
if (src.size() > 2) dst.z = src(2);
}
}
/// \brief Handle ROS <-> dynamic-graph conversion.
///
/// Implements all ROS/dynamic-graph conversions required by the
/// bridge.
///
///Relies on the SotToRos type trait to make sure valid pair of
/// types are used.
template <typename D, typename S>
void converter (D& dst, const S& src);
// Boolean
SOT_TO_ROS_IMPL(bool)
{
dst.data = src;
}
ROS_TO_SOT_IMPL(bool)
{
dst = src.data;
}
// Double
SOT_TO_ROS_IMPL(double)
{
dst.data = src;
}
ROS_TO_SOT_IMPL(double)
{
dst = src.data;
}
// Int
SOT_TO_ROS_IMPL(int)
{
dst.data = src;
}
ROS_TO_SOT_IMPL(int)
{
dst = src.data;
}
// Unsigned
SOT_TO_ROS_IMPL(unsigned int)
{
dst.data = src;
}
ROS_TO_SOT_IMPL(unsigned int)
{
dst = src.data;
}
// Vector
SOT_TO_ROS_IMPL(Vector)
{
dst.data.resize (src.size ());
for (int i = 0; i < src.size (); ++i)
dst.data[i] = src (i);
}
ROS_TO_SOT_IMPL(Vector)
{
dst.resize (src.data.size ());
for (unsigned int i = 0; i < src.data.size (); ++i)
dst (i) = src.data[i];
}
// Vector3
SOT_TO_ROS_IMPL(specific::Vector3)
{
if (src.size () > 0)
{
dst.x = src (0);
if (src.size () > 1)
{
dst.y = src (1);
if (src.size () > 2)
dst.z = src (2);
}
}
}
ROS_TO_SOT_IMPL(specific::Vector3)
{
dst.resize (3);
dst (0) = src.x;
dst (1) = src.y;
dst (2) = src.z;
}
// Matrix
SOT_TO_ROS_IMPL(Matrix)
{
//TODO: Confirm Ros Matrix Storage order. It changes the RosMatrix to ColMajor.
dst.width = (unsigned int)src.rows ();
dst.data.resize (src.cols () * src.rows ());
for (int i = 0; i < src.cols () * src.rows (); ++i)
dst.data[i] = src.data()[i];
}
ROS_TO_SOT_IMPL(Matrix)
{
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];
}
// Homogeneous matrix.
SOT_TO_ROS_IMPL(sot::MatrixHomogeneous)
{
sot::VectorQuaternion q(src.linear());
dst.translation.x = src.translation()(0);
dst.translation.y = src.translation()(1);
dst.translation.z = src.translation()(2);
dst.rotation.x = q.x();
dst.rotation.y = q.y();
dst.rotation.z = q.z();
dst.rotation.w = q.w();
}
ROS_TO_SOT_IMPL(sot::MatrixHomogeneous)
{
sot::VectorQuaternion q(src.rotation.w,
src.rotation.x,
src.rotation.y,
src.rotation.z);
dst.linear() = q.matrix();
// Copy the translation component.
dst.translation()(0) = src.translation.x;
dst.translation()(1) = src.translation.y;
dst.translation()(2) = src.translation.z;
}
// Twist.
SOT_TO_ROS_IMPL(specific::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);
dst.angular.x = src (3);
dst.angular.y = src (4);
dst.angular.z = src (5);
}
ROS_TO_SOT_IMPL(specific::Twist)
{
dst.resize (6);
dst (0) = src.linear.x;
dst (1) = src.linear.y;
dst (2) = src.linear.z;
dst (3) = src.angular.x;
dst (4) = src.angular.y;
dst (5) = src.angular.z;
}
/// \brief This macro generates a converter for a stamped type from
/// dynamic-graph to ROS. I.e. A data associated with its
/// timestamp.
# define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
template <> \
inline void converter \
(SotToRos<std::pair<T, Vector> >::ros_t& dst, \
const SotToRos<std::pair<T, Vector> >::sot_t& src) \
{ \
makeHeader(dst.header); \
converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t> (dst.ATTRIBUTE, src); \
do { EXTRA } while (0); \
} \
}
ROS_TO_SOT_IMPL(specific::Vector3) {
dst.resize(3);
dst(0) = src.x;
dst(1) = src.y;
dst(2) = src.z;
}
// Matrix
SOT_TO_ROS_IMPL(Matrix) {
// TODO: Confirm Ros Matrix Storage order. It changes the RosMatrix to
// ColMajor.
dst.width = (unsigned int)src.rows();
dst.data.resize(src.cols() * src.rows());
for (int i = 0; i < src.cols() * src.rows(); ++i) dst.data[i] = src.data()[i];
}
ROS_TO_SOT_IMPL(Matrix) {
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];
}
// Homogeneous matrix.
SOT_TO_ROS_IMPL(sot::MatrixHomogeneous) {
sot::VectorQuaternion q(src.linear());
dst.translation.x = src.translation()(0);
dst.translation.y = src.translation()(1);
dst.translation.z = src.translation()(2);
dst.rotation.x = q.x();
dst.rotation.y = q.y();
dst.rotation.z = q.z();
dst.rotation.w = q.w();
}
ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y,
src.rotation.z);
dst.linear() = q.matrix();
// Copy the translation component.
dst.translation()(0) = src.translation.x;
dst.translation()(1) = src.translation.y;
dst.translation()(2) = src.translation.z;
}
// Twist.
SOT_TO_ROS_IMPL(specific::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);
dst.angular.x = src(3);
dst.angular.y = src(4);
dst.angular.z = src(5);
}
ROS_TO_SOT_IMPL(specific::Twist) {
dst.resize(6);
dst(0) = src.linear.x;
dst(1) = src.linear.y;
dst(2) = src.linear.z;
dst(3) = src.angular.x;
dst(4) = src.angular.y;
dst(5) = src.angular.z;
}
/// \brief This macro generates a converter for a stamped type from
/// dynamic-graph to ROS. I.e. A data associated with its
/// timestamp.
#define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
template <> \
inline void converter(SotToRos<std::pair<T, Vector> >::ros_t& dst, \
const SotToRos<std::pair<T, Vector> >::sot_t& src) { \
makeHeader(dst.header); \
converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t>(dst.ATTRIBUTE, src); \
do { \
EXTRA \
} while (0); \
} \
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(specific::Twist, twist, ;);
/// \brief This macro generates a converter for a shared pointer on
/// a ROS type to a dynamic-graph type.
///
/// 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); \
} \
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(specific::Twist, twist, ;);
/// \brief This macro generates a converter for a shared pointer on
/// a ROS type to a dynamic-graph type.
///
/// 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); \
} \
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
DG_BRIDGE_MAKE_SHPTR_IMPL(double);
DG_BRIDGE_MAKE_SHPTR_IMPL(int);
DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
DG_BRIDGE_MAKE_SHPTR_IMPL(Vector);
DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
DG_BRIDGE_MAKE_SHPTR_IMPL(Matrix);
DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
/// \brief This macro generates a converter for 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_IMPL(T, ATTRIBUTE, EXTRA) \
template <> \
inline void converter \
(SotToRos<std::pair<T, Vector> >::sot_t& dst, \
const SotToRos<std::pair<T, Vector> >::ros_t& src) \
{ \
converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t> (dst, src.ATTRIBUTE); \
do { EXTRA } while (0); \
} \
DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
DG_BRIDGE_MAKE_SHPTR_IMPL(double);
DG_BRIDGE_MAKE_SHPTR_IMPL(int);
DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
DG_BRIDGE_MAKE_SHPTR_IMPL(std::string);
DG_BRIDGE_MAKE_SHPTR_IMPL(Vector);
DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
DG_BRIDGE_MAKE_SHPTR_IMPL(Matrix);
DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
/// \brief This macro generates a converter for 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_IMPL(T, ATTRIBUTE, EXTRA) \
template <> \
inline void converter(SotToRos<std::pair<T, Vector> >::sot_t& dst, \
const SotToRos<std::pair<T, Vector> >::ros_t& 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_IMPL(specific::Vector3, vector, ;);
DG_BRIDGE_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, ;);
DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
/// \brief This macro generates a converter for a shared pointer on
/// 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); \
} \
DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
DG_BRIDGE_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, ;);
DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
/// \brief This macro generates a converter for a shared pointer on
/// 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); \
} \
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, ;);
DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(sot::MatrixHomogeneous, transform, ;);
DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Twist, twist, ;);
/// \brief If an impossible/unimplemented conversion is required, fail.
///
/// IMPORTANT, READ ME:
///
/// If the compiler generates an error in the following function,
/// this is /normal/.
///
/// This error means that either you try to use an undefined
/// conversion. You can either fix your code or provide the wanted
/// conversion by updating this header.
template <typename U, typename V>
inline void converter (U& dst, V& src)
{
// This will always fail if instantiated.
BOOST_STATIC_ASSERT (sizeof (U) == 0);
}
typedef boost::posix_time::ptime ptime;
typedef boost::posix_time::seconds seconds;
typedef boost::posix_time::microseconds microseconds;
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));
return time;
}
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 nsec = (unsigned int)diff.fractional_seconds();
return ros::Time (sec, nsec);
}
} // end of namespace dynamicgraph.
#endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH
DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Vector3, vector, ;);
DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(sot::MatrixHomogeneous, transform, ;);
DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Twist, twist, ;);
/// \brief If an impossible/unimplemented conversion is required, fail.
///
/// IMPORTANT, READ ME:
///
/// If the compiler generates an error in the following function,
/// this is /normal/.
///
/// This error means that either you try to use an undefined
/// conversion. You can either fix your code or provide the wanted
/// conversion by updating this header.
template <typename U, typename V>
inline void converter(U& dst, V& src) {
// This will always fail if instantiated.
BOOST_STATIC_ASSERT(sizeof(U) == 0);
}
typedef boost::posix_time::ptime ptime;
typedef boost::posix_time::seconds seconds;
typedef boost::posix_time::microseconds microseconds;
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));
return time;
}
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 nsec = (unsigned int)diff.fractional_seconds();
return ros::Time(sec, nsec);
}
} // end of namespace dynamicgraph.
#endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH
src/dynamic_graph/ros/__init__.py
View file @ c2c3026d
from ros_publish import RosPublish
from ros_subscribe import RosSubscribe
# aliases, for retro compatibility
from ros import RosPublish as RosImport
from ros import RosSubscribe as RosExport
# flake8: noqa
from .ros import RosPublish as RosImport
from .ros import RosSubscribe as RosExport
from .ros_publish import RosPublish
from .ros_subscribe import RosSubscribe
from .ros_queued_subscribe import RosQueuedSubscribe
src/dynamic_graph/ros/dgcompleter.py
View file @ c2c3026d
......@@ -35,18 +35,17 @@ its input.
"""
import __builtin__
import __main__
import ast
__all__ = ["DGCompleter"]
class DGCompleter:
def __init__(self, client):
"""Create a new completer for the command line.
Completer([client]) -> completer instance.
Client is a ROS proxy to dynamic_graph run_command service.
Completer instances should be used as the completion mechanism of
......@@ -54,34 +53,27 @@ class DGCompleter:
readline.set_completer(Completer(client).complete)
"""
self.client=client
astr="import readline"
self.client = client
astr = "import readline"
self.client(astr)
astr="from rlcompleter import Completer"
astr = "from rlcompleter import Completer"
self.client(astr)
astr="aCompleter=Completer()"
astr = "aCompleter=Completer()"
self.client(astr)
astr="readline.set_completer(aCompleter.complete)"
astr = "readline.set_completer(aCompleter.complete)"
self.client(astr)
astr="readline.parse_and_bind(\"tab: complete\")"
astr = 'readline.parse_and_bind("tab: complete")'
self.client(astr)
def complete(self, text, state):
"""Return the next possible completion for 'text'. readline.parse_and_bind("tab: complete")
def complete(self, text, state):
"""Return the next possible completion for 'text'.
readline.parse_and_bind("tab: complete")
This is called successively with state == 0, 1, 2, ... until it
returns None. The completion should begin with 'text'.
"""
astr="aCompleter.complete(\""+text+"\","+str(state)+")"
response=self.client(astr);
res2=ast.literal_eval(response.result)
astr = 'aCompleter.complete("' + text + '",' + str(state) + ")"
response = self.client(astr)
res2 = ast.literal_eval(response.result)
return res2
src/dynamic_graph/ros/ros.py
View file @ c2c3026d
from ros_publish import RosPublish
from ros_subscribe import RosSubscribe
from ros_time import RosTime
from .ros_publish import RosPublish
from .ros_subscribe import RosSubscribe
from .ros_time import RosTime
from dynamic_graph import plug
class Ros(object):
device = None
......@@ -13,15 +12,14 @@ class Ros(object):
rosImport = None
rosExport = None
def __init__(self, robot, suffix = ''):
def __init__(self, robot, suffix=""):
self.robot = robot
self.rosPublish = RosPublish('rosPublish{0}'.format(suffix))
self.rosSubscribe = RosSubscribe('rosSubscribe{0}'.format(suffix))
self.rosTime = RosTime ('rosTime{0}'.format(suffix))
self.rosPublish = RosPublish("rosPublish{0}".format(suffix))
self.rosSubscribe = RosSubscribe("rosSubscribe{0}".format(suffix))
self.rosTime = RosTime("rosTime{0}".format(suffix))
self.robot.device.after.addSignal(
'{0}.trigger'.format(self.rosPublish.name))
self.robot.device.after.addSignal("{0}.trigger".format(self.rosPublish.name))
# aliases, for retro compatibility
self.rosImport=self.rosPublish
self.rosExport=self.rosSubscribe
self.rosImport = self.rosPublish
self.rosExport = self.rosSubscribe
src/geometric_simu.cpp
View file @ c2c3026d
......@@ -4,20 +4,8 @@
*
* CNRS
*
* This file is part of dynamic_graph_bridge.
* dynamic_graph_bridge 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.
* dynamic_graph_bridge 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 dynamic_graph_bridge. If not, see <http://www.gnu.org/licenses/>.
*/
#include <boost/thread/thread.hpp>
#include <boost/thread/condition.hpp>
#include <ros/console.h>
#include <iostream>
......@@ -26,18 +14,21 @@
#include <dynamic_graph_bridge/sot_loader.hh>
int main(int argc, char *argv[])
{
dgADD_OSTREAM_TO_RTLOG (std::cerr);
int main(int argc, char *argv[]) {
::dynamicgraph::RealTimeLogger::instance().addOutputStream(
::dynamicgraph::LoggerStreamPtr_t(
new dynamicgraph::LoggerIOStream(std::cout)));
ros::init(argc, argv, "sot_ros_encapsulator");
SotLoader aSotLoader;
if (aSotLoader.parseOptions(argc,argv)<0)
return -1;
aSotLoader.initializeRosNode(argc,argv);
if (aSotLoader.parseOptions(argc, argv) < 0) return -1;
// Advertize service "(start|stop)_dynamic_graph" and
// load parameter "robot_description in SoT.
aSotLoader.initializeRosNode(argc, argv);
// Load dynamic library and run python prologue.
aSotLoader.Initialization();
ros::spin();
ros::waitForShutdown();
return 0;
}
src/robot_model.cpp deleted 100644 → 0
View file @ 86cfdc71
#include "robot_model.hh"
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
#include <pinocchio/multibody/parser/urdf.hpp>
#include <pinocchio/multibody/model.hpp>
#include "dynamic_graph_bridge/ros_init.hh"
#include <ros/package.h>
namespace dynamicgraph
{
RosRobotModel::RosRobotModel(const std::string& name)
: Dynamic(name),
jointsParameterName_("jrl_map"),
ns_("sot_controller")
{
std::string docstring;
docstring =
"\n"
" Load the robot model from the parameter server.\n"
"\n"
" This is the recommended method.\n"
"\n";
addCommand("loadFromParameterServer", command::makeCommandVoid0(*this,&RosRobotModel::loadFromParameterServer,docstring));
docstring =
"\n"
" Load the robot model from an URDF file.\n"
"\n";
addCommand("loadUrdf", command::makeCommandVoid1(*this,&RosRobotModel::loadUrdf,docstring));
docstring =
"\n"
" Set the controller namespace."
"\n";
addCommand("setNamespace", command::makeCommandVoid1(*this,&RosRobotModel::setNamespace,docstring));
docstring =
"\n"
" Get current configuration of the robot.\n"
"\n";
addCommand ("curConf", new command::Getter<RosRobotModel,Vector> (*this,&RosRobotModel::curConf,docstring));
docstring =
"\n"
" Maps a link name in the URDF parser to actual robot link name.\n"
"\n";
addCommand ("addJointMapping", command::makeCommandVoid2(*this,&RosRobotModel::addJointMapping,docstring));
}
RosRobotModel::~RosRobotModel()
{}
void RosRobotModel::loadUrdf (const std::string& filename)
{
rosInit (false);
m_model = se3::urdf::buildModel(filename);
this->m_urdfPath = filename;
if (m_data) delete m_data;
m_data = new se3::Data(m_model);
init=true;
// m_HDR = parser.parse(filename);
ros::NodeHandle nh(ns_);
XmlRpc::XmlRpcValue JointsNamesByRank_;
JointsNamesByRank_.setSize(m_model.names.size());
std::vector<std::string>::const_iterator it = m_model.names.begin()+2; //first joint is universe, second is freeflyer
for (int i=0;it!=m_model.names.end();++it, ++i) JointsNamesByRank_[i]= (*it);
nh.setParam(jointsParameterName_, JointsNamesByRank_);
}
void RosRobotModel::setNamespace (const std::string& ns)
{
ns_ = ns;
}
void RosRobotModel::loadFromParameterServer()
{
rosInit (false);
std::string robotDescription;
ros::param::param<std::string> ("/robot_description", robotDescription, "");
if (robotDescription.empty ())
throw std::runtime_error("No model available as ROS parameter. Fail.");
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDF (robotDescription);
if (urdfTree)
se3::urdf::parseTree(urdfTree->getRoot(),
this->m_model, se3::SE3::Identity(), false);
else {
const std::string exception_message
("robot_description not parsed correctly.");
throw std::invalid_argument(exception_message);
}
this->m_urdfPath = "";
if (m_data) delete m_data;
m_data = new se3::Data(m_model);
init=true;
ros::NodeHandle nh(ns_);
XmlRpc::XmlRpcValue JointsNamesByRank_;
JointsNamesByRank_.setSize(m_model.names.size());
//first joint is universe, second is freeflyer
std::vector<std::string>::const_iterator it = m_model.names.begin()+2;
for (int i=0;it!=m_model.names.end();++it, ++i) JointsNamesByRank_[i]= (*it);
nh.setParam(jointsParameterName_, JointsNamesByRank_);
}
Vector RosRobotModel::curConf() const
{
// The first 6 dofs are associated to the Freeflyer frame
// Freeflyer reference frame should be the same as global
// frame so that operational point positions correspond to
// position in freeflyer frame.
XmlRpc::XmlRpcValue ffpose;
ros::NodeHandle nh(ns_);
std::string param_name = "ffpose";
if (nh.hasParam(param_name)){
nh.getParam(param_name, ffpose);
ROS_ASSERT(ffpose.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(ffpose.size() == 6);
for (int32_t i = 0; i < ffpose.size(); ++i) {
ROS_ASSERT(ffpose[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
}
}
else {
ffpose.setSize(6);
for (int32_t i = 0; i < ffpose.size(); ++i) ffpose[i] = 0.0;
}
if (!m_data )
throw std::runtime_error ("no robot loaded");
else {
//TODO: confirm accesscopy is for asynchronous commands
Vector currConf = jointPositionSIN.accessCopy();
for (int32_t i = 0; i < ffpose.size(); ++i)
currConf(i) = static_cast<double>(ffpose[i]);
return currConf;
}
}
void
RosRobotModel::addJointMapping(const std::string &link, const std::string &repName)
{
specialJoints_[link] = repName;
}
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosRobotModel, "RosRobotModel");
} // end of namespace dynamicgraph.
src/robot_model.hh deleted 100644 → 0
View file @ 86cfdc71
#ifndef DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
# define DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
# include <string>
#include <sot-dynamic/dynamic.h>
#include <dynamic-graph/linear-algebra.h>
#include "XmlRpcValue.h"
namespace dynamicgraph
{
class RosRobotModel;
/// \brief This entity load either the current model available in
/// the robot_description parameter or a specified file and build
/// a Dynamic entity
///
/// This relies on pinocchio urdf parser to load the model and pinocchio
/// to realize the computation.
class RosRobotModel : public sot::Dynamic
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
RosRobotModel(const std::string& n);
virtual ~RosRobotModel();
void loadUrdf(const std::string& filename);
void setNamespace (const std::string& ns);
void loadFromParameterServer();
Vector curConf() const;
void addJointMapping(const std::string& link, const std::string& repName);
protected:
unsigned getDimension () const
{
if (!m_data)
throw std::runtime_error ("no robot loaded");
//TODO: Configuration vector dimension or the dof?
return m_model.nv;
//return m_model.nq;
}
private:
/// \brief Name of the parameter where the joints list will be published
std::string jointsParameterName_;
/// \brief Name of the controller namespace
std::string ns_;
/// \brief Special joints map for the parser
std::map<std::string, std::string> specialJoints_;
};
} // end of namespace dynamicgraph.
#endif //! DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
src/ros_init.cpp
View file @ c2c3026d
#include <stdexcept>
#include "dynamic_graph_bridge/ros_init.hh"
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <stdexcept>
#include "dynamic_graph_bridge/ros_init.hh"
namespace dynamicgraph
{
struct GlobalRos
{
~GlobalRos ()
{
if (spinner)
spinner->stop ();
if (nodeHandle)
nodeHandle->shutdown ();
}
namespace dynamicgraph {
struct GlobalRos {
~GlobalRos() {
if (spinner) spinner->stop();
if (nodeHandle) nodeHandle->shutdown();
}
boost::shared_ptr<ros::NodeHandle> nodeHandle;
boost::shared_ptr<ros::AsyncSpinner> spinner;
boost::shared_ptr<ros::MultiThreadedSpinner> mtSpinner;
};
GlobalRos ros;
boost::shared_ptr<ros::NodeHandle> nodeHandle;
boost::shared_ptr<ros::AsyncSpinner> spinner;
boost::shared_ptr<ros::MultiThreadedSpinner> mtSpinner;
};
GlobalRos ros;
ros::NodeHandle& rosInit (bool createAsyncSpinner, bool createMultiThreadedSpinner)
{
if (!ros.nodeHandle)
{
int argc = 1;
char* arg0 = strdup("dynamic_graph_bridge");
char* argv[] = {arg0, 0};
ros::init(argc, argv, "dynamic_graph_bridge");
free (arg0);
ros::NodeHandle& rosInit(bool createAsyncSpinner,
bool createMultiThreadedSpinner) {
if (!ros.nodeHandle) {
int argc = 1;
char* arg0 = strdup("dynamic_graph_bridge");
char* argv[] = {arg0, 0};
ros::init(argc, argv, "dynamic_graph_bridge");
free(arg0);
ros.nodeHandle = boost::make_shared<ros::NodeHandle> ("");
}
if (!ros.spinner && createAsyncSpinner)
{
ros.spinner = boost::make_shared<ros::AsyncSpinner> (4);
ros.spinner->start ();
}
else
{
if (!ros.mtSpinner && createMultiThreadedSpinner)
{
ros.mtSpinner = boost::make_shared<ros::MultiThreadedSpinner>(4);
}
}
return *ros.nodeHandle;
ros.nodeHandle = boost::make_shared<ros::NodeHandle>("");
}
if (!ros.spinner && createAsyncSpinner) {
ros.spinner = boost::make_shared<ros::AsyncSpinner>(4);
ros::AsyncSpinner& spinner ()
{
if (!ros.spinner)
throw std::runtime_error ("spinner has not been created");
return *ros.spinner;
}
// Change the thread's scheduler from real-time to normal and reduce its
// priority
int oldThreadPolicy, newThreadPolicy;
struct sched_param oldThreadParam, newThreadParam;
if (pthread_getschedparam(pthread_self(), &oldThreadPolicy,
&oldThreadParam) == 0) {
newThreadPolicy = SCHED_OTHER;
newThreadParam = oldThreadParam;
newThreadParam.sched_priority -=
5; // Arbitrary number, TODO: choose via param/file?
if (newThreadParam.sched_priority <
sched_get_priority_min(newThreadPolicy))
newThreadParam.sched_priority = sched_get_priority_min(newThreadPolicy);
pthread_setschedparam(pthread_self(), newThreadPolicy, &newThreadParam);
}
// AsyncSpinners are created with the reduced priority
ros.spinner->start();
ros::MultiThreadedSpinner& mtSpinner ()
{
if (!ros.mtSpinner)
throw std::runtime_error ("spinner has not been created");
return *ros.mtSpinner;
// Switch the priority of the parent thread (this thread) back to real-time.
pthread_setschedparam(pthread_self(), oldThreadPolicy, &oldThreadParam);
} else {
if (!ros.mtSpinner && createMultiThreadedSpinner) {
// Seems not to be used.
// If we need to reduce its threads priority, it needs to be done before
// calling the MultiThreadedSpinner::spin() method
ros.mtSpinner = boost::make_shared<ros::MultiThreadedSpinner>(4);
}
}
return *ros.nodeHandle;
}
ros::AsyncSpinner& spinner() {
if (!ros.spinner) throw std::runtime_error("spinner has not been created");
return *ros.spinner;
}
ros::MultiThreadedSpinner& mtSpinner() {
if (!ros.mtSpinner) throw std::runtime_error("spinner has not been created");
return *ros.mtSpinner;
}
} // end of namespace dynamicgraph.
} // end of namespace dynamicgraph.
src/ros_interpreter.cpp
View file @ c2c3026d
#include "dynamic_graph_bridge/ros_interpreter.hh"
namespace dynamicgraph
{
static const int queueSize = 5;
Interpreter::Interpreter (ros::NodeHandle& nodeHandle)
: interpreter_ (),
nodeHandle_ (nodeHandle),
runCommandSrv_ (),
runPythonFileSrv_ ()
{
namespace dynamicgraph {
static const int queueSize = 5;
Interpreter::Interpreter(ros::NodeHandle& nodeHandle)
: interpreter_(),
nodeHandle_(nodeHandle),
runCommandSrv_(),
runPythonFileSrv_() {}
void Interpreter::startRosService() {
runCommandCallback_t runCommandCb =
boost::bind(&Interpreter::runCommandCallback, this, _1, _2);
runCommandSrv_ = nodeHandle_.advertiseService("run_command", runCommandCb);
runPythonFileCallback_t runPythonFileCb =
boost::bind(&Interpreter::runPythonFileCallback, this, _1, _2);
runPythonFileSrv_ =
nodeHandle_.advertiseService("run_script", runPythonFileCb);
}
bool Interpreter::runCommandCallback(
dynamic_graph_bridge_msgs::RunCommand::Request& req,
dynamic_graph_bridge_msgs::RunCommand::Response& res) {
interpreter_.python(req.input, res.result, res.standardoutput,
res.standarderror);
return true;
}
bool Interpreter::runPythonFileCallback(
dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
dynamic_graph_bridge_msgs::RunPythonFile::Response& res) {
interpreter_.runPythonFile(req.input);
res.result = "File parsed"; // FIX: It is just an echo, is there a way to
// have a feedback?
return true;
}
void Interpreter::runCommand(const std::string& command, std::string& result,
std::string& out, std::string& err) {
interpreter_.python(command, result, out, err);
if (err.size() > 0) {
ROS_ERROR(err.c_str());
}
}
void Interpreter::startRosService ()
{
runCommandCallback_t runCommandCb =
boost::bind (&Interpreter::runCommandCallback, this, _1, _2);
runCommandSrv_ =
nodeHandle_.advertiseService ("run_command", runCommandCb);
runPythonFileCallback_t runPythonFileCb =
boost::bind (&Interpreter::runPythonFileCallback, this, _1, _2);
runPythonFileSrv_ =
nodeHandle_.advertiseService ("run_script", runPythonFileCb);
}
bool
Interpreter::runCommandCallback
(dynamic_graph_bridge_msgs::RunCommand::Request& req,
dynamic_graph_bridge_msgs::RunCommand::Response& res)
{
interpreter_.python(req.input, res.result, res.standardoutput, res.standarderror);
return true;
}
bool
Interpreter::runPythonFileCallback (dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
dynamic_graph_bridge_msgs::RunPythonFile::Response& res)
{
interpreter_.runPythonFile(req.input);
res.result = "File parsed"; // FIX: It is just an echo, is there a way to have a feedback?
return true;
}
void Interpreter::runCommand
(const std::string & command,
std::string &result,
std::string &out,
std::string &err)
{
interpreter_.python(command, result, out, err);
}
void Interpreter::runPythonFile( std::string ifilename ){
interpreter_.runPythonFile(ifilename);
}
} // end of namespace dynamicgraph.
void Interpreter::runPythonFile(std::string ifilename) {
interpreter_.runPythonFile(ifilename);
}
} // end of namespace dynamicgraph.
src/ros_parameter.cpp 0 → 100644
View file @ c2c3026d
#include <pinocchio/fwd.hpp>
// include pinocchio first
//
#include <ros/ros.h>
#include <urdf_parser/urdf_parser.h>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <sot/core/robot-utils.hh>
#include <stdexcept>
#include "dynamic_graph_bridge/ros_parameter.hh"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"
namespace dynamicgraph {
bool parameter_server_read_robot_description() {
ros::NodeHandle nh;
if (!nh.hasParam("/robot_description")) {
ROS_ERROR("No /robot_description parameter");
return false;
}
std::string robot_description;
std::string parameter_name("/robot_description");
nh.getParam(parameter_name, robot_description);
std::string model_name("robot");
// Search for the robot util related to robot_name.
sot::RobotUtilShrPtr aRobotUtil = sot::getRobotUtil(model_name);
// If does not exist then it is created.
if (aRobotUtil == sot::RefVoidRobotUtil())
aRobotUtil = sot::createRobotUtil(model_name);
// If the creation is fine
if (aRobotUtil != sot::RefVoidRobotUtil()) {
// Then set the robot model.
aRobotUtil->set_parameter(parameter_name, robot_description);
ROS_INFO("Set parameter_name : %s.", parameter_name.c_str());
// Everything went fine.
return true;
}
ROS_ERROR("Wrong initialization of parameter_name %s",
parameter_name.c_str());
// Otherwise something went wrong.
return false;
}
} // namespace dynamicgraph
src/ros_publish-python-module-py.cc 0 → 100644
View file @ c2c3026d
#include <dynamic-graph/python/module.hh>
#include "ros_publish.hh"
namespace dg = dynamicgraph;
BOOST_PYTHON_MODULE(wrap) {
bp::import("dynamic_graph");
dg::python::exposeEntity<dg::RosPublish, bp::bases<dg::Entity>,
dg::python::AddCommands>()
.def("clear", &dg::RosPublish::clear,
"Remove all signals writing data to a ROS topic")
.def("rm", &dg::RosPublish::rm,
"Remove a signal writing data to a ROS topic",
bp::args("signal_name"))
.def("list", &dg::RosPublish::list,
"List signals writing data to a ROS topic");
}
src/ros_publish.cpp
View file @ c2c3026d
#include <stdexcept>
#include "ros_publish.hh"
#include <dynamic-graph/command.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/linear-algebra.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/UInt32.h>
#include <boost/assign.hpp>
#include <boost/bind.hpp>
......@@ -6,277 +13,182 @@
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/UInt32.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/command.h>
#include <dynamic-graph/linear-algebra.h>
#include <stdexcept>
#include "dynamic_graph_bridge/ros_init.hh"
#include "ros_publish.hh"
namespace dynamicgraph
{
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish");
const double RosPublish::ROS_JOINT_STATE_PUBLISHER_RATE = 0.01;
namespace command
{
namespace rosPublish
{
Clear::Clear
(RosPublish& entity, const std::string& docstring)
: Command
(entity,
std::vector<Value::Type> (),
docstring)
{}
Value Clear::doExecute ()
{
RosPublish& entity =
static_cast<RosPublish&> (owner ());
entity.clear ();
return Value ();
}
List::List
(RosPublish& entity, const std::string& docstring)
: Command
(entity,
std::vector<Value::Type> (),
docstring)
{}
Value List::doExecute ()
{
RosPublish& entity =
static_cast<RosPublish&> (owner ());
return Value (entity.list ());
}
Add::Add
(RosPublish& entity, const std::string& docstring)
: Command
(entity,
boost::assign::list_of
(Value::STRING) (Value::STRING) (Value::STRING),
docstring)
{}
Value Add::doExecute ()
{
RosPublish& entity =
static_cast<RosPublish&> (owner ());
std::vector<Value> values = getParameterValues ();
const std::string& type = values[0].value ();
const std::string& signal = values[1].value ();
const std::string& topic = values[2].value ();
if (type == "boolean")
entity.add<bool> (signal, topic);
else if (type == "double")
entity.add<double> (signal, topic);
else if (type == "unsigned")
entity.add<unsigned int> (signal, topic);
else if (type == "int")
entity.add<int> (signal, topic);
else if (type == "matrix")
entity.add<Matrix> (signal, topic);
else if (type == "vector")
entity.add<Vector> (signal, topic);
else if (type == "vector3")
entity.add<specific::Vector3> (signal, topic);
else if (type == "vector3Stamped")
entity.add<std::pair<specific::Vector3, Vector> > (signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous> (signal, topic);
else if (type == "matrixHomoStamped")
entity.add<std::pair<sot::MatrixHomogeneous, Vector> >
(signal, topic);
else if (type == "twist")
entity.add<specific::Twist> (signal, topic);
else if (type == "twistStamped")
entity.add<std::pair<specific::Twist, Vector> > (signal, topic);
else
throw std::runtime_error("bad type");
return Value ();
}
Rm::Rm
(RosPublish& entity, const std::string& docstring)
: Command
(entity,
boost::assign::list_of (Value::STRING),
docstring)
{
#define ENABLE_RT_LOG
#include <dynamic-graph/real-time-logger.h>
namespace dynamicgraph {
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish");
const double RosPublish::ROS_JOINT_STATE_PUBLISHER_RATE = 0.01;
namespace command {
namespace rosPublish {
Add::Add(RosPublish& entity, const std::string& docstring)
: Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() {
RosPublish& entity = static_cast<RosPublish&>(owner());
std::vector<Value> values = getParameterValues();
const std::string& type = values[0].value();
const std::string& signal = values[1].value();
const std::string& topic = values[2].value();
if (type == "boolean")
entity.add<bool>(signal, topic);
else if (type == "double")
entity.add<double>(signal, topic);
else if (type == "unsigned")
entity.add<unsigned int>(signal, topic);
else if (type == "int")
entity.add<int>(signal, topic);
else if (type == "matrix")
entity.add<Matrix>(signal, topic);
else if (type == "vector")
entity.add<Vector>(signal, topic);
else if (type == "vector3")
entity.add<specific::Vector3>(signal, topic);
else if (type == "vector3Stamped")
entity.add<std::pair<specific::Vector3, Vector> >(signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous>(signal, topic);
else if (type == "matrixHomoStamped")
entity.add<std::pair<sot::MatrixHomogeneous, Vector> >(signal, topic);
else if (type == "twist")
entity.add<specific::Twist>(signal, topic);
else if (type == "twistStamped")
entity.add<std::pair<specific::Twist, Vector> >(signal, topic);
else if (type == "string")
entity.add<std::string>(signal, topic);
else
throw std::runtime_error("bad type");
return Value();
}
Value Rm::doExecute ()
{
RosPublish& entity =
static_cast<RosPublish&> (owner ());
std::vector<Value> values = getParameterValues ();
const std::string& signal = values[0].value ();
entity.rm (signal);
return Value ();
}
} // end of errorEstimator.
} // end of namespace command.
} // namespace rosPublish
} // end of namespace command.
const std::string RosPublish::docstring_
("Publish dynamic-graph signals as ROS topics.\n"
"\n"
" Use command \"add\" to publish a new ROS topic.\n");
const std::string RosPublish::docstring_(
"Publish dynamic-graph signals as ROS topics.\n"
"\n"
" Use command \"add\" to publish a new ROS topic.\n");
RosPublish::RosPublish (const std::string& n)
RosPublish::RosPublish(const std::string& n)
: dynamicgraph::Entity(n),
// RosPublish do not use callback so do not create a useless spinner.
nh_ (rosInit (false)),
bindedSignal_ (),
trigger_ (boost::bind (&RosPublish::trigger, this, _1, _2),
sotNOSIGNAL,
MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
lastPublicated_ ()
{
try {
lastPublicated_ = ros::Time::now ();
} catch (const std::exception& exc) {
throw std::runtime_error ("Failed to call ros::Time::now ():" +
std::string (exc.what ()));
nh_(rosInit(false)),
bindedSignal_(),
trigger_(boost::bind(&RosPublish::trigger, this, _1, _2), sotNOSIGNAL,
MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
rate_(0, 10000000),
nextPublication_() {
aofs_.open("/tmp/ros_publish.txt");
dgADD_OSTREAM_TO_RTLOG(aofs_);
try {
if (ros::Time::isSimTime())
nextPublication_ = ros::Time::now();
else {
clock_gettime(CLOCK_REALTIME, &nextPublicationRT_);
}
signalRegistration (trigger_);
trigger_.setNeedUpdateFromAllChildren (true);
} catch (const std::exception& exc) {
throw std::runtime_error("Failed to call ros::Time::now ():" +
std::string(exc.what()));
}
signalRegistration(trigger_);
trigger_.setNeedUpdateFromAllChildren(true);
std::string docstring =
std::string docstring =
"\n"
" Add a signal writing data to a ROS topic\n"
"\n"
" Input:\n"
" - type: string among ['double', 'matrix', 'vector', 'vector3',\n"
" 'vector3Stamped', 'matrixHomo', 'matrixHomoStamped',\n"
" 'vector3Stamped', 'matrixHomo', "
"'matrixHomoStamped',\n"
" 'twist', 'twistStamped'],\n"
" - signal: the signal name in dynamic-graph,\n"
" - topic: the topic name in ROS.\n"
"\n";
addCommand ("add",
new command::rosPublish::Add
(*this, docstring));
docstring =
"\n"
" Remove a signal writing data to a ROS topic\n"
"\n"
" Input:\n"
" - name of the signal to remove (see method list for the list of signals).\n"
"\n";
addCommand ("rm",
new command::rosPublish::Rm
(*this, docstring));
docstring =
"\n"
" Remove all signals writing data to a ROS topic\n"
"\n"
" No input:\n"
"\n";
addCommand ("clear",
new command::rosPublish::Clear
(*this, docstring));
docstring =
"\n"
" List signals writing data to a ROS topic\n"
"\n"
" No input:\n"
"\n";
addCommand ("list",
new command::rosPublish::List
(*this, docstring));
}
addCommand("add", new command::rosPublish::Add(*this, docstring));
}
RosPublish::~RosPublish ()
{
}
RosPublish::~RosPublish() { aofs_.close(); }
void RosPublish::display(std::ostream& os) const {
os << CLASS_NAME << std::endl;
}
void RosPublish::rm(const std::string& signal) {
if (bindedSignal_.find(signal) == bindedSignal_.end()) return;
void RosPublish::display (std::ostream& os) const
{
os << CLASS_NAME << std::endl;
if (signal == "trigger") {
std::cerr << "The trigger signal should not be removed. Aborting."
<< std::endl;
return;
}
void RosPublish::rm (const std::string& signal)
{
if(bindedSignal_.find(signal) == bindedSignal_.end())
return;
// lock the mutex to avoid deleting the signal during a call to trigger
boost::mutex::scoped_lock lock(mutex_);
if(signal == "trigger")
{
std::cerr << "The trigger signal should not be removed. Aborting." << std::endl;
return;
}
signalDeregistration(signal);
bindedSignal_.erase(signal);
}
//lock the mutex to avoid deleting the signal during a call to trigger
while(! mutex_.try_lock() ){}
signalDeregistration(signal);
bindedSignal_.erase (signal);
mutex_.unlock();
}
std::vector<std::string> RosPublish::list() const {
std::vector<std::string> result(bindedSignal_.size());
std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
[](const auto& pair) { return pair.first; });
return result;
}
std::string RosPublish::list () const
{
std::string result("[");
for (std::map<std::string, bindedSignal_t>::const_iterator it =
bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
result += "'" + it->first + "',";
void RosPublish::clear() {
std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
for (; it != bindedSignal_.end();) {
if (it->first != "trigger") {
rm(it->first);
it = bindedSignal_.begin();
} else {
++it;
}
result += "]";
return result;
}
}
}
void RosPublish::clear ()
{
std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
for(; it!= bindedSignal_.end(); )
{
if (it->first != "trigger")
{
rm(it->first);
it = bindedSignal_.begin();
}
else
{
++it;
}
int& RosPublish::trigger(int& dummy, int t) {
typedef std::map<std::string, bindedSignal_t>::iterator iterator_t;
ros::Time aTime;
if (ros::Time::isSimTime()) {
aTime = ros::Time::now();
if (aTime <= nextPublication_) return dummy;
nextPublication_ = aTime + rate_;
} else {
struct timespec aTimeRT;
clock_gettime(CLOCK_REALTIME, &aTimeRT);
nextPublicationRT_.tv_sec = aTimeRT.tv_sec + rate_.sec;
nextPublicationRT_.tv_nsec = aTimeRT.tv_nsec + rate_.nsec;
if (nextPublicationRT_.tv_nsec > 1000000000) {
nextPublicationRT_.tv_nsec -= 1000000000;
nextPublicationRT_.tv_sec += 1;
}
}
int& RosPublish::trigger (int& dummy, int t)
{
typedef std::map<std::string, bindedSignal_t>::iterator iterator_t;
ros::Duration dt = ros::Time::now () - lastPublicated_;
if (dt < rate_)
return dummy;
boost::mutex::scoped_lock lock(mutex_);
lastPublicated_ = ros::Time::now();
while(! mutex_.try_lock() ){}
for (iterator_t it = bindedSignal_.begin ();
it != bindedSignal_.end (); ++it)
{
boost::get<1>(it->second) (t);
}
mutex_.unlock();
return dummy;
for (iterator_t it = bindedSignal_.begin(); it != bindedSignal_.end(); ++it) {
boost::get<1>(it->second)(t);
}
return dummy;
}
std::string RosPublish::getDocString () const
{
return docstring_;
}
std::string RosPublish::getDocString() const { return docstring_; }
} // end of namespace dynamicgraph.
} // end of namespace dynamicgraph.
src/ros_publish.hh
View file @ c2c3026d
#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HH
# define DYNAMIC_GRAPH_ROS_PUBLISH_HH
# include <map>
#define DYNAMIC_GRAPH_ROS_PUBLISH_HH
#include <dynamic-graph/command.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <realtime_tools/realtime_publisher.h>
#include <ros/ros.h>
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/tuple/tuple.hpp>
#include <fstream>
#include <map>
#include "converter.hh"
#include "sot_to_ros.hh"
namespace dynamicgraph {
class RosPublish;
namespace command {
namespace rosPublish {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
#define ROS_PUBLISH_MAKE_COMMAND(CMD) \
class CMD : public Command { \
public: \
CMD(RosPublish& entity, const std::string& docstring); \
virtual Value doExecute(); \
}
ROS_PUBLISH_MAKE_COMMAND(Add);
# include <boost/shared_ptr.hpp>
# include <boost/tuple/tuple.hpp>
# include <boost/interprocess/sync/interprocess_mutex.hpp>
#undef ROS_PUBLISH_MAKE_COMMAND
# include <dynamic-graph/entity.h>
# include <dynamic-graph/signal-time-dependent.h>
# include <dynamic-graph/command.h>
} // namespace rosPublish
} // end of namespace command.
# include <ros/ros.h>
/// \brief Publish dynamic-graph information into ROS.
class RosPublish : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
# include <realtime_tools/realtime_publisher.h>
public:
typedef boost::function<void(int)> callback_t;
# include "converter.hh"
# include "sot_to_ros.hh"
typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
callback_t>
bindedSignal_t;
namespace dynamicgraph
{
class RosPublish;
static const double ROS_JOINT_STATE_PUBLISHER_RATE;
namespace command
{
namespace rosPublish
{
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
RosPublish(const std::string& n);
virtual ~RosPublish();
# define ROS_PUBLISH_MAKE_COMMAND(CMD) \
class CMD : public Command \
{ \
public: \
CMD (RosPublish& entity, \
const std::string& docstring); \
virtual Value doExecute (); \
}
virtual std::string getDocString() const;
void display(std::ostream& os) const;
ROS_PUBLISH_MAKE_COMMAND(Add);
ROS_PUBLISH_MAKE_COMMAND(Clear);
ROS_PUBLISH_MAKE_COMMAND(List);
ROS_PUBLISH_MAKE_COMMAND(Rm);
void add(const std::string& signal, const std::string& topic);
void rm(const std::string& signal);
std::vector<std::string> list() const;
void clear();
#undef ROS_PUBLISH_MAKE_COMMAND
int& trigger(int&, int);
template <typename T>
void sendData(
boost::shared_ptr<
realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> >
publisher,
boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time);
template <typename T>
void add(const std::string& signal, const std::string& topic);
private:
static const std::string docstring_;
ros::NodeHandle& nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
dynamicgraph::SignalTimeDependent<int, int> trigger_;
ros::Duration rate_;
ros::Time nextPublication_;
boost::mutex mutex_;
std::ofstream aofs_;
struct timespec nextPublicationRT_;
};
} // end of namespace dynamicgraph.
} // end of namespace errorEstimator.
} // end of namespace command.
/// \brief Publish dynamic-graph information into ROS.
class RosPublish : public dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
typedef boost::function<void (int)> callback_t;
typedef boost::tuple<
boost::shared_ptr<dynamicgraph::SignalBase<int> >,
callback_t>
bindedSignal_t;
static const double ROS_JOINT_STATE_PUBLISHER_RATE;
RosPublish (const std::string& n);
virtual ~RosPublish ();
virtual std::string getDocString () const;
void display (std::ostream& os) const;
void add (const std::string& signal, const std::string& topic);
void rm (const std::string& signal);
std::string list () const;
void clear ();
int& trigger (int&, int);
template <typename T>
void
sendData
(boost::shared_ptr
<realtime_tools::RealtimePublisher
<typename SotToRos<T>::ros_t> > publisher,
boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal,
int time);
template <typename T>
void add (const std::string& signal, const std::string& topic);
private:
static const std::string docstring_;
ros::NodeHandle& nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
dynamicgraph::SignalTimeDependent<int,int> trigger_;
ros::Duration rate_;
ros::Time lastPublicated_;
boost::interprocess::interprocess_mutex mutex_;
};
} // end of namespace dynamicgraph.
# include "ros_publish.hxx"
#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HH
#include "ros_publish.hxx"
#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HH
src/ros_publish.hxx
View file @ c2c3026d
#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HXX
# define DYNAMIC_GRAPH_ROS_PUBLISH_HXX
# include <vector>
# include <std_msgs/Float64.h>
#define DYNAMIC_GRAPH_ROS_PUBLISH_HXX
#include <std_msgs/Float64.h>
# include "dynamic_graph_bridge_msgs/Matrix.h"
# include "dynamic_graph_bridge_msgs/Vector.h"
#include <vector>
# include "sot_to_ros.hh"
#include "dynamic_graph_bridge_msgs/Matrix.h"
#include "dynamic_graph_bridge_msgs/Vector.h"
#include "sot_to_ros.hh"
namespace dynamicgraph
{
template <>
inline void
RosPublish::sendData
<std::pair<sot::MatrixHomogeneous, Vector> >
(boost::shared_ptr
<realtime_tools::RealtimePublisher
<SotToRos
<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> > publisher,
boost::shared_ptr
<SotToRos
<std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t> signal,
int time)
{
SotToRos
<std::pair
<sot::MatrixHomogeneous, Vector> >::ros_t result;
if (publisher->trylock ())
{
publisher->msg_.child_frame_id = "/dynamic_graph/world";
converter (publisher->msg_, signal->access (time));
publisher->unlockAndPublish ();
}
namespace dynamicgraph {
template <>
inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >(
boost::shared_ptr<realtime_tools::RealtimePublisher<
SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> >
publisher,
boost::shared_ptr<
SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t>
signal,
int time) {
SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t result;
if (publisher->trylock()) {
publisher->msg_.child_frame_id = "/dynamic_graph/world";
converter(publisher->msg_, signal->access(time));
publisher->unlockAndPublish();
}
}
template <typename T>
void
RosPublish::sendData
(boost::shared_ptr
<realtime_tools::RealtimePublisher
<typename SotToRos<T>::ros_t> > publisher,
boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal,
int time)
{
typename SotToRos<T>::ros_t result;
if (publisher->trylock ())
{
converter (publisher->msg_, signal->access (time));
publisher->unlockAndPublish ();
}
template <typename T>
void RosPublish::sendData(
boost::shared_ptr<
realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> >
publisher,
boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time) {
typename SotToRos<T>::ros_t result;
if (publisher->trylock()) {
converter(publisher->msg_, signal->access(time));
publisher->unlockAndPublish();
}
}
template <typename T>
void RosPublish::add (const std::string& signal, const std::string& topic)
{
typedef typename SotToRos<T>::ros_t ros_t;
typedef typename SotToRos<T>::signalIn_t signal_t;
template <typename T>
void RosPublish::add(const std::string& signal, const std::string& topic) {
typedef typename SotToRos<T>::ros_t ros_t;
typedef typename SotToRos<T>::signalIn_t signal_t;
// Initialize the bindedSignal object.
bindedSignal_t bindedSignal;
// Initialize the bindedSignal object.
bindedSignal_t bindedSignal;
// Initialize the publisher.
boost::shared_ptr
<realtime_tools::RealtimePublisher<ros_t> >
pubPtr =
boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >
(nh_, topic, 1);
// Initialize the publisher.
boost::shared_ptr<realtime_tools::RealtimePublisher<ros_t> > pubPtr =
boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >(nh_, topic,
1);
// Initialize the signal.
boost::shared_ptr<signal_t> signalPtr
(new signal_t
(0,
MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
boost::get<0> (bindedSignal) = signalPtr;
SotToRos<T>::setDefault(*signalPtr);
signalRegistration (*boost::get<0> (bindedSignal));
// Initialize the signal.
boost::shared_ptr<signal_t> signalPtr(new signal_t(
0, MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
boost::get<0>(bindedSignal) = signalPtr;
SotToRos<T>::setDefault(*signalPtr);
signalRegistration(*boost::get<0>(bindedSignal));
// Initialize the callback.
callback_t callback = boost::bind
(&RosPublish::sendData<T>,
this,
pubPtr,
signalPtr,
_1);
boost::get<1> (bindedSignal) = callback;
// Initialize the callback.
callback_t callback =
boost::bind(&RosPublish::sendData<T>, this, pubPtr, signalPtr, _1);
boost::get<1>(bindedSignal) = callback;
bindedSignal_[signal] = bindedSignal;
}
bindedSignal_[signal] = bindedSignal;
}
} // end of namespace dynamicgraph.
} // end of namespace dynamicgraph.
#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HXX
#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HXX
src/ros_queued_subscribe-python-module-py.cc 0 → 100644
View file @ c2c3026d
#include <dynamic-graph/python/module.hh>
#include "ros_queued_subscribe.hh"
namespace dg = dynamicgraph;
BOOST_PYTHON_MODULE(wrap) {
bp::import("dynamic_graph");
dg::python::exposeEntity<dg::RosQueuedSubscribe, bp::bases<dg::Entity>,
dg::python::AddCommands>()
.def("clear", &dg::RosQueuedSubscribe::clear,
"Remove all signals reading data from a ROS topic")
.def("rm", &dg::RosQueuedSubscribe::rm,
"Remove a signal reading data from a ROS topic",
bp::args("signal_name"))
.def("list", &dg::RosQueuedSubscribe::list,
"List signals reading data from a ROS topic")
.def("listTopics", &dg::RosQueuedSubscribe::listTopics,
"List subscribed topics from ROS in the same order as list command")
.def("clearQueue", &dg::RosQueuedSubscribe::clearQueue,
"Empty the queue of a given signal", bp::args("signal_name"))
.def("queueSize", &dg::RosQueuedSubscribe::queueSize,
"Return the queue size of a given signal", bp::args("signal_name"))
.def("readQueue", &dg::RosQueuedSubscribe::readQueue,
"Whether signals should read values from the queues, and when.",
bp::args("start_time"));
}
src/ros_queued_subscribe.cpp
View file @ c2c3026d
......@@ -3,204 +3,73 @@
// Authors: Joseph Mirabel
//
//
// This file is part of dynamic_graph_bridge
// dynamic_graph_bridge 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.
//
// dynamic_graph_bridge 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 Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// dynamic_graph_bridge If not, see
// <http://www.gnu.org/licenses/>.
#include <boost/assign.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
#include "ros_queued_subscribe.hh"
#include <dynamic-graph/factory.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/UInt32.h>
#include <dynamic-graph/factory.h>
#include <boost/assign.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
#include "dynamic_graph_bridge/ros_init.hh"
#include "ros_queued_subscribe.hh"
namespace dynamicgraph
{
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosQueuedSubscribe, "RosQueuedSubscribe");
namespace command
{
namespace rosQueuedSubscribe
{
Clear::Clear
(RosQueuedSubscribe& entity, const std::string& docstring)
: Command
(entity,
std::vector<Value::Type> (),
docstring)
{}
Value Clear::doExecute ()
{
RosQueuedSubscribe& entity =
static_cast<RosQueuedSubscribe&> (owner ());
entity.clear ();
return Value ();
}
ClearQueue::ClearQueue
(RosQueuedSubscribe& entity, const std::string& docstring)
: Command
(entity,
boost::assign::list_of (Value::STRING),
docstring)
{}
Value ClearQueue::doExecute ()
{
RosQueuedSubscribe& entity =
static_cast<RosQueuedSubscribe&> (owner ());
std::vector<Value> values = getParameterValues ();
const std::string& signal = values[0].value ();
entity.clearQueue (signal);
return Value ();
}
List::List
(RosQueuedSubscribe& entity, const std::string& docstring)
: Command
(entity,
std::vector<Value::Type> (),
docstring)
{}
Value List::doExecute ()
{
RosQueuedSubscribe& entity =
static_cast<RosQueuedSubscribe&> (owner ());
return Value (entity.list ());
}
Add::Add
(RosQueuedSubscribe& entity, const std::string& docstring)
: Command
(entity,
boost::assign::list_of
(Value::STRING) (Value::STRING) (Value::STRING),
docstring)
{}
Value Add::doExecute ()
{
RosQueuedSubscribe& entity =
static_cast<RosQueuedSubscribe&> (owner ());
std::vector<Value> values = getParameterValues ();
const std::string& type = values[0].value ();
const std::string& signal = values[1].value ();
const std::string& topic = values[2].value ();
if (type == "double")
entity.add<double> (type, signal, topic);
else if (type == "unsigned")
entity.add<unsigned int> (type, signal, topic);
else if (type == "matrix")
entity.add<dg::Matrix> (type, signal, topic);
else if (type == "vector")
entity.add<dg::Vector> (type, signal, topic);
else if (type == "vector3")
entity.add<specific::Vector3> (type, signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous> (type, signal, topic);
else if (type == "twist")
entity.add<specific::Twist> (type, signal, topic);
else
throw std::runtime_error("bad type");
return Value ();
}
Rm::Rm
(RosQueuedSubscribe& entity, const std::string& docstring)
: Command
(entity,
boost::assign::list_of (Value::STRING),
docstring)
{}
Value Rm::doExecute ()
{
RosQueuedSubscribe& entity =
static_cast<RosQueuedSubscribe&> (owner ());
std::vector<Value> values = getParameterValues ();
const std::string& signal = values[0].value ();
entity.rm (signal);
return Value ();
}
QueueSize::QueueSize
(RosQueuedSubscribe& entity, const std::string& docstring)
: Command
(entity,
boost::assign::list_of (Value::STRING),
docstring)
{}
Value QueueSize::doExecute ()
{
RosQueuedSubscribe& entity =
static_cast<RosQueuedSubscribe&> (owner ());
std::vector<Value> values = getParameterValues ();
const std::string& signal = values[0].value ();
return Value ((unsigned)entity.queueSize (signal));
}
ReadQueue::ReadQueue
(RosQueuedSubscribe& entity, const std::string& docstring)
: Command
(entity,
boost::assign::list_of (Value::INT),
docstring)
{}
Value ReadQueue::doExecute ()
{
RosQueuedSubscribe& entity =
static_cast<RosQueuedSubscribe&> (owner ());
std::vector<Value> values = getParameterValues ();
int read = values[0].value ();
entity.readQueue (read);
return Value ();
}
} // end of errorEstimator.
} // end of namespace command.
const std::string RosQueuedSubscribe::docstring_
("Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n"
"\n"
" Use command \"add\" to subscribe to a new signal.\n");
RosQueuedSubscribe::RosQueuedSubscribe (const std::string& n)
namespace dynamicgraph {
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosQueuedSubscribe, "RosQueuedSubscribe");
namespace command {
namespace rosQueuedSubscribe {
Add::Add(RosQueuedSubscribe& entity, const std::string& docstring)
: Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() {
RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
std::vector<Value> values = getParameterValues();
const std::string& type = values[0].value();
const std::string& signal = values[1].value();
const std::string& topic = values[2].value();
if (type == "double")
entity.add<double>(type, signal, topic);
else if (type == "unsigned")
entity.add<unsigned int>(type, signal, topic);
else if (type == "matrix")
entity.add<Matrix>(type, signal, topic);
else if (type == "vector")
entity.add<Vector>(type, signal, topic);
else if (type == "vector3")
entity.add<specific::Vector3>(type, signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous>(type, signal, topic);
else if (type == "twist")
entity.add<specific::Twist>(type, signal, topic);
else
throw std::runtime_error("bad type");
return Value();
}
} // namespace rosQueuedSubscribe
} // end of namespace command.
const std::string RosQueuedSubscribe::docstring_(
"Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n"
"\n"
" Use command \"add\" to subscribe to a new signal.\n");
RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n)
: dynamicgraph::Entity(n),
nh_ (rosInit (true)),
bindedSignal_ (),
readQueue_ (-1)
{
std::string docstring =
nh_(rosInit(true)),
bindedSignal_(),
readQueue_(-1) {
std::string docstring =
"\n"
" Add a signal reading data from a ROS topic\n"
"\n"
......@@ -210,143 +79,72 @@ namespace dynamicgraph
" - signal: the signal name in dynamic-graph,\n"
" - topic: the topic name in ROS.\n"
"\n";
addCommand ("add",
new command::rosQueuedSubscribe::Add
(*this, docstring));
docstring =
"\n"
" Remove a signal reading data from a ROS topic\n"
"\n"
" Input:\n"
" - name of the signal to remove (see method list for the list of signals).\n"
"\n";
addCommand ("rm",
new command::rosQueuedSubscribe::Rm
(*this, docstring));
docstring =
"\n"
" Remove all signals reading data from a ROS topic\n"
"\n"
" No input:\n"
"\n";
addCommand ("clear",
new command::rosQueuedSubscribe::Clear
(*this, docstring));
docstring =
"\n"
" List signals reading data from a ROS topic\n"
"\n"
" No input:\n"
"\n";
addCommand ("list",
new command::rosQueuedSubscribe::List
(*this, docstring));
docstring =
"\n"
" Empty the queue of a given signal\n"
"\n"
" Input is:\n"
" - name of the signal (see method list for the list of signals).\n"
"\n";
addCommand ("clearQueue",
new command::rosQueuedSubscribe::ClearQueue
(*this, docstring));
docstring =
"\n"
" Return the queue size of a given signal\n"
"\n"
" Input is:\n"
" - name of the signal (see method list for the list of signals).\n"
"\n";
addCommand ("queueSize",
new command::rosQueuedSubscribe::QueueSize
(*this, docstring));
docstring =
"\n"
" Whether signals should read values from the queues, and when.\n"
"\n"
" Input is:\n"
" - int (dynamic graph time at which the reading begin).\n"
"\n";
addCommand ("readQueue",
new command::rosQueuedSubscribe::ReadQueue
(*this, docstring));
}
addCommand("add", new command::rosQueuedSubscribe::Add(*this, docstring));
}
RosQueuedSubscribe::~RosQueuedSubscribe ()
{}
RosQueuedSubscribe::~RosQueuedSubscribe() {}
void RosQueuedSubscribe::display (std::ostream& os) const
{
os << CLASS_NAME << std::endl;
}
void RosQueuedSubscribe::rm (const std::string& signal)
{
std::string signalTs = signal+"Timestamp";
void RosQueuedSubscribe::display(std::ostream& os) const {
os << CLASS_NAME << std::endl;
}
signalDeregistration(signal);
bindedSignal_.erase (signal);
void RosQueuedSubscribe::rm(const std::string& signal) {
std::string signalTs = signal + "Timestamp";
if(bindedSignal_.find(signalTs) != bindedSignal_.end())
{
signalDeregistration(signalTs);
bindedSignal_.erase(signalTs);
}
}
signalDeregistration(signal);
bindedSignal_.erase(signal);
std::string RosQueuedSubscribe::list ()
{
std::string result("[");
for (std::map<std::string, bindedSignal_t>::const_iterator it =
bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
result += "'" + it->first + "',";
}
result += "]";
return result;
if (bindedSignal_.find(signalTs) != bindedSignal_.end()) {
signalDeregistration(signalTs);
bindedSignal_.erase(signalTs);
}
void RosQueuedSubscribe::clear ()
{
std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
for(; it!= bindedSignal_.end(); )
{
rm(it->first);
it = bindedSignal_.begin();
}
}
std::vector<std::string> RosQueuedSubscribe::list() {
std::vector<std::string> result(bindedSignal_.size());
std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
[](const auto& pair) { return pair.first; });
return result;
}
std::vector<std::string> RosQueuedSubscribe::listTopics() {
std::vector<std::string> result(topics_.size());
std::transform(topics_.begin(), topics_.end(), result.begin(),
[](const auto& pair) { return pair.second; });
return result;
}
void RosQueuedSubscribe::clear() {
std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
for (; it != bindedSignal_.end();) {
rm(it->first);
it = bindedSignal_.begin();
}
}
void RosQueuedSubscribe::clearQueue (const std::string& signal)
{
if(bindedSignal_.find(signal) != bindedSignal_.end())
{
bindedSignal_[signal]->clear();
}
void RosQueuedSubscribe::clearQueue(const std::string& signal) {
if (bindedSignal_.find(signal) != bindedSignal_.end()) {
bindedSignal_[signal]->clear();
}
}
std::size_t RosQueuedSubscribe::queueSize (const std::string& signal) const
{
std::map<std::string, bindedSignal_t>::const_iterator _bs =
std::size_t RosQueuedSubscribe::queueSize(const std::string& signal) const {
std::map<std::string, bindedSignal_t>::const_iterator _bs =
bindedSignal_.find(signal);
if(_bs != bindedSignal_.end())
{
return _bs->second->size();
}
return -1;
}
void RosQueuedSubscribe::readQueue (int beginReadingAt)
{
// Prints signal queues sizes
/*for (std::map<std::string, bindedSignal_t>::const_iterator it =
bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
std::cout << it->first << " : " << it->second->size() << '\n';
}*/
readQueue_ = beginReadingAt;
}
std::string RosQueuedSubscribe::getDocString () const
{
return docstring_;
if (_bs != bindedSignal_.end()) {
return _bs->second->size();
}
} // end of namespace dynamicgraph.
return -1;
}
void RosQueuedSubscribe::readQueue(int beginReadingAt) {
// Prints signal queues sizes
/*for (std::map<std::string, bindedSignal_t>::const_iterator it =
bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
std::cout << it->first << " : " << it->second->size() << '\n';
}*/
readQueue_ = beginReadingAt;
}
std::string RosQueuedSubscribe::getDocString() const { return docstring_; }
} // end of namespace dynamicgraph.
src/ros_queued_subscribe.hh
View file @ c2c3026d
......@@ -3,222 +3,184 @@
// Authors: Joseph Mirabel
//
//
// This file is part of dynamic_graph_bridge
// dynamic_graph_bridge 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.
//
// dynamic_graph_bridge 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 Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// dynamic_graph_bridge If not, see
// <http://www.gnu.org/licenses/>.
#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
# define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
# include <map>
# include <boost/shared_ptr.hpp>
# include <boost/thread/mutex.hpp>
# include <dynamic-graph/entity.h>
# include <dynamic-graph/signal-time-dependent.h>
# include <dynamic-graph/signal-ptr.h>
# include <dynamic-graph/command.h>
# include <sot/core/matrix-geometry.hh>
# include <ros/ros.h>
# include "converter.hh"
# include "sot_to_ros.hh"
namespace dynamicgraph
{
class RosQueuedSubscribe;
namespace command
{
namespace rosQueuedSubscribe
{
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
# define ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(CMD) \
class CMD : public Command \
{ \
public: \
CMD (RosQueuedSubscribe& entity, \
const std::string& docstring); \
virtual Value doExecute (); \
}
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Add);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Clear);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(List);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Rm);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(ClearQueue);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(QueueSize);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(ReadQueue);
#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
#include <dynamic-graph/command.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <ros/ros.h>
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <map>
#include <sot/core/matrix-geometry.hh>
#include "converter.hh"
#include "sot_to_ros.hh"
namespace dynamicgraph {
class RosQueuedSubscribe;
namespace command {
namespace rosQueuedSubscribe {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
#define ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(CMD) \
class CMD : public Command { \
public: \
CMD(RosQueuedSubscribe& entity, const std::string& docstring); \
virtual Value doExecute(); \
}
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Add);
#undef ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND
} // end of namespace rosQueuedSubscribe.
} // end of namespace command.
class RosQueuedSubscribe;
namespace internal
{
template <typename T>
struct Add;
struct BindedSignalBase {
typedef boost::shared_ptr<ros::Subscriber> Subscriber_t;
BindedSignalBase(RosQueuedSubscribe* e) : entity(e) {}
virtual ~BindedSignalBase() {}
virtual void clear () = 0;
virtual std::size_t size () const = 0;
Subscriber_t subscriber;
RosQueuedSubscribe* entity;
};
template <typename T, int BufferSize>
struct BindedSignal : BindedSignalBase {
typedef dynamicgraph::Signal<T, int> Signal_t;
typedef boost::shared_ptr<Signal_t> SignalPtr_t;
typedef std::vector<T> buffer_t;
typedef typename buffer_t::size_type size_type;
BindedSignal(RosQueuedSubscribe* e)
: BindedSignalBase (e)
, frontIdx(0)
, backIdx(0)
, buffer (BufferSize)
, init(false)
{}
~BindedSignal()
{
signal.reset();
clear();
}
/// See comments in reader and writer for details about synchronisation.
void clear ()
{
// synchronize with method writer
wmutex.lock();
if (!empty()) {
if (backIdx == 0)
last = buffer[BufferSize-1];
else
last = buffer[backIdx-1];
}
// synchronize with method reader
rmutex.lock();
frontIdx = backIdx = 0;
rmutex.unlock();
wmutex.unlock();
}
bool empty () const
{
return frontIdx == backIdx;
}
bool full () const
{
return ((backIdx + 1) % BufferSize) == frontIdx;
}
size_type size () const
{
if (frontIdx <= backIdx)
return backIdx - frontIdx;
else
return backIdx + BufferSize - frontIdx;
}
SignalPtr_t signal;
/// Index of the next value to be read.
size_type frontIdx;
/// Index of the slot where to write next value (does not contain valid data).
size_type backIdx;
buffer_t buffer;
boost::mutex wmutex, rmutex;
T last;
bool init;
template <typename R> void writer (const R& data);
T& reader (T& val, int time);
};
} // end of internal namespace.
/// \brief Publish ROS information in the dynamic-graph.
class RosQueuedSubscribe : public dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
typedef boost::posix_time::ptime ptime;
public:
typedef boost::shared_ptr<internal::BindedSignalBase> bindedSignal_t;
RosQueuedSubscribe (const std::string& n);
virtual ~RosQueuedSubscribe ();
virtual std::string getDocString () const;
void display (std::ostream& os) const;
void rm (const std::string& signal);
std::string list ();
void clear ();
void clearQueue (const std::string& signal);
void readQueue (int beginReadingAt);
std::size_t queueSize (const std::string& signal) const;
template <typename T>
void add (const std::string& type, const std::string& signal, const std::string& topic);
std::map<std::string, bindedSignal_t>&
bindedSignal ()
{
return bindedSignal_;
}
ros::NodeHandle& nh ()
{
return nh_;
} // end of namespace rosQueuedSubscribe.
} // end of namespace command.
class RosQueuedSubscribe;
namespace internal {
template <typename T>
struct Add;
struct BindedSignalBase {
typedef boost::shared_ptr<ros::Subscriber> Subscriber_t;
BindedSignalBase(RosQueuedSubscribe* e) : entity(e) {}
virtual ~BindedSignalBase() {}
virtual void clear() = 0;
virtual std::size_t size() const = 0;
Subscriber_t subscriber;
RosQueuedSubscribe* entity;
};
template <typename T, int BufferSize>
struct BindedSignal : BindedSignalBase {
typedef dynamicgraph::Signal<T, int> Signal_t;
typedef boost::shared_ptr<Signal_t> SignalPtr_t;
typedef std::vector<T> buffer_t;
typedef typename buffer_t::size_type size_type;
BindedSignal(RosQueuedSubscribe* e)
: BindedSignalBase(e),
frontIdx(0),
backIdx(0),
buffer(BufferSize),
init(false) {}
~BindedSignal() {
signal.reset();
clear();
}
/// See comments in reader and writer for details about synchronisation.
void clear() {
// synchronize with method writer
wmutex.lock();
if (!empty()) {
if (backIdx == 0)
last = buffer[BufferSize - 1];
else
last = buffer[backIdx - 1];
}
template <typename R, typename S>
void callback
(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
const R& data);
template <typename R>
void callbackTimestamp
(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
const R& data);
template <typename T>
friend class internal::Add;
private:
static const std::string docstring_;
ros::NodeHandle& nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
int readQueue_;
// Signal<bool, int> readQueue_;
template <typename T>
friend class internal::BindedSignal;
};
} // end of namespace dynamicgraph.
# include "ros_queued_subscribe.hxx"
#endif //! DYNAMIC_GRAPH_QUEUED_ROS_SUBSCRIBE_HH
// synchronize with method reader
rmutex.lock();
frontIdx = backIdx = 0;
rmutex.unlock();
wmutex.unlock();
}
bool empty() const { return frontIdx == backIdx; }
bool full() const { return ((backIdx + 1) % BufferSize) == frontIdx; }
size_type size() const {
if (frontIdx <= backIdx)
return backIdx - frontIdx;
else
return backIdx + BufferSize - frontIdx;
}
SignalPtr_t signal;
/// Index of the next value to be read.
size_type frontIdx;
/// Index of the slot where to write next value (does not contain valid data).
size_type backIdx;
buffer_t buffer;
boost::mutex wmutex, rmutex;
T last;
bool init;
template <typename R>
void writer(const R& data);
T& reader(T& val, int time);
};
} // namespace internal
/// \brief Publish ROS information in the dynamic-graph.
class RosQueuedSubscribe : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
typedef boost::posix_time::ptime ptime;
public:
typedef boost::shared_ptr<internal::BindedSignalBase> bindedSignal_t;
RosQueuedSubscribe(const std::string& n);
virtual ~RosQueuedSubscribe();
virtual std::string getDocString() const;
void display(std::ostream& os) const;
void rm(const std::string& signal);
std::vector<std::string> list();
std::vector<std::string> listTopics();
void clear();
void clearQueue(const std::string& signal);
void readQueue(int beginReadingAt);
std::size_t queueSize(const std::string& signal) const;
template <typename T>
void add(const std::string& type, const std::string& signal,
const std::string& topic);
std::map<std::string, bindedSignal_t>& bindedSignal() {
return bindedSignal_;
}
std::map<std::string, std::string>& topics() { return topics_; }
ros::NodeHandle& nh() { return nh_; }
template <typename R, typename S>
void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
const R& data);
template <typename R>
void callbackTimestamp(
boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
const R& data);
template <typename T>
friend class internal::Add;
private:
static const std::string docstring_;
ros::NodeHandle& nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
std::map<std::string, std::string> topics_;
int readQueue_;
// Signal<bool, int> readQueue_;
template <typename T>
friend class internal::BindedSignal;
};
} // end of namespace dynamicgraph.
#include "ros_queued_subscribe.hxx"
#endif //! DYNAMIC_GRAPH_QUEUED_ROS_SUBSCRIBE_HH
src/ros_queued_subscribe.hxx
View file @ c2c3026d
......@@ -3,135 +3,114 @@
// Authors: Joseph Mirabel
//
//
// This file is part of dynamic_graph_bridge
// dynamic_graph_bridge 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.
//
// dynamic_graph_bridge 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 Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// dynamic_graph_bridge If not, see
// <http://www.gnu.org/licenses/>.
#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
# define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
# define ENABLE_RT_LOG
# include <vector>
# include <boost/bind.hpp>
# include <boost/date_time/posix_time/posix_time.hpp>
# include <dynamic-graph/real-time-logger.h>
# include <dynamic-graph/signal-caster.h>
# include <dynamic-graph/linear-algebra.h>
# include <dynamic-graph/signal-cast-helper.h>
# include <std_msgs/Float64.h>
# include "dynamic_graph_bridge_msgs/Matrix.h"
# include "dynamic_graph_bridge_msgs/Vector.h"
namespace dg = dynamicgraph;
typedef boost::mutex::scoped_lock scoped_lock;
namespace dynamicgraph
{
namespace internal
{
static const int BUFFER_SIZE = 1 << 10;
template <typename T>
struct Add
{
void operator () (RosQueuedSubscribe& rosSubscribe,
const std::string& type,
const std::string& signal,
const std::string& topic)
{
typedef typename SotToRos<T>::sot_t sot_t;
typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
typedef BindedSignal<sot_t, BUFFER_SIZE> BindedSignal_t;
typedef typename BindedSignal_t::Signal_t Signal_t;
// Initialize the bindedSignal object.
BindedSignal_t* bs = new BindedSignal_t(&rosSubscribe);
SotToRos<T>::setDefault (bs->last);
// Initialize the signal.
boost::format signalName ("RosQueuedSubscribe(%1%)::output(%2%)::%3%");
signalName % rosSubscribe.getName () % type % signal;
bs->signal.reset (new Signal_t (signalName.str ()));
bs->signal->setFunction (boost::bind(&BindedSignal_t::reader, bs, _1, _2));
rosSubscribe.signalRegistration (*bs->signal);
// Initialize the subscriber.
typedef boost::function<void (const ros_const_ptr_t& data)> callback_t;
callback_t callback = boost::bind
(&BindedSignal_t::template writer<ros_const_ptr_t>, bs, _1);
// Keep 50 messages in queue, but only 20 are sent every 100ms
// -> No message should be lost because of a full buffer
bs->subscriber =
boost::make_shared<ros::Subscriber>
(rosSubscribe.nh ().subscribe (topic, BUFFER_SIZE, callback));
RosQueuedSubscribe::bindedSignal_t bindedSignal (bs);
rosSubscribe.bindedSignal ()[signal] = bindedSignal;
}
};
// template <typename T, typename R>
template <typename T, int N>
template <typename R>
void BindedSignal<T, N>::writer (const R& data)
{
// synchronize with method clear
boost::mutex::scoped_lock lock(wmutex);
if (full()) {
rmutex.lock();
frontIdx = (frontIdx + 1) % N;
rmutex.unlock();
}
converter (buffer[backIdx], data);
// No need to synchronize with reader here because:
// - if the buffer was not empty, then it stays not empty,
// - if it was empty, then the current value will be used at next time. It
// means the transmission bandwidth is too low.
if (!init) {
last = buffer[backIdx];
init = true;
}
backIdx = (backIdx+1) % N;
}
#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
#define ENABLE_RT_LOG
template <typename T, int N>
T& BindedSignal<T, N>::reader (T& data, int time)
{
// synchronize with method clear:
// If reading from the list cannot be done, then return last value.
scoped_lock lock(rmutex, boost::try_to_lock);
if (!lock.owns_lock() || entity->readQueue_ == -1 || time < entity->readQueue_) {
data = last;
} else {
if (empty())
data = last;
else {
data = buffer[frontIdx];
frontIdx = (frontIdx + 1) % N;
last = data;
}
}
return data;
}
} // end of namespace internal.
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/real-time-logger.h>
#include <dynamic-graph/signal-cast-helper.h>
#include <dynamic-graph/signal-caster.h>
#include <std_msgs/Float64.h>
#include <boost/bind.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <vector>
#include "dynamic_graph_bridge_msgs/Matrix.h"
#include "dynamic_graph_bridge_msgs/Vector.h"
namespace dynamicgraph {
namespace internal {
static const int BUFFER_SIZE = 1 << 10;
template <typename T>
struct Add {
void operator()(RosQueuedSubscribe& rosSubscribe, const std::string& type,
const std::string& signal, const std::string& topic) {
typedef typename SotToRos<T>::sot_t sot_t;
typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
typedef BindedSignal<sot_t, BUFFER_SIZE> BindedSignal_t;
typedef typename BindedSignal_t::Signal_t Signal_t;
// Initialize the bindedSignal object.
BindedSignal_t* bs = new BindedSignal_t(&rosSubscribe);
SotToRos<T>::setDefault(bs->last);
// Initialize the signal.
boost::format signalName("RosQueuedSubscribe(%1%)::output(%2%)::%3%");
signalName % rosSubscribe.getName() % type % signal;
bs->signal.reset(new Signal_t(signalName.str()));
bs->signal->setFunction(boost::bind(&BindedSignal_t::reader, bs, _1, _2));
rosSubscribe.signalRegistration(*bs->signal);
template <typename T>
void RosQueuedSubscribe::add (const std::string& type, const std::string& signal, const std::string& topic)
{
internal::Add<T> () (*this, type, signal, topic);
// Initialize the subscriber.
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
callback_t callback =
boost::bind(&BindedSignal_t::template writer<ros_const_ptr_t>, bs, _1);
// Keep 50 messages in queue, but only 20 are sent every 100ms
// -> No message should be lost because of a full buffer
bs->subscriber = boost::make_shared<ros::Subscriber>(
rosSubscribe.nh().subscribe(topic, BUFFER_SIZE, callback));
RosQueuedSubscribe::bindedSignal_t bindedSignal(bs);
rosSubscribe.bindedSignal()[signal] = bindedSignal;
rosSubscribe.topics()[signal] = topic;
}
};
// template <typename T, typename R>
template <typename T, int N>
template <typename R>
void BindedSignal<T, N>::writer(const R& data) {
// synchronize with method clear
boost::mutex::scoped_lock lock(wmutex);
if (full()) {
rmutex.lock();
frontIdx = (frontIdx + 1) % N;
rmutex.unlock();
}
converter(buffer[backIdx], data);
// No need to synchronize with reader here because:
// - if the buffer was not empty, then it stays not empty,
// - if it was empty, then the current value will be used at next time. It
// means the transmission bandwidth is too low.
if (!init) {
last = buffer[backIdx];
init = true;
}
} // end of namespace dynamicgraph.
backIdx = (backIdx + 1) % N;
}
template <typename T, int N>
T& BindedSignal<T, N>::reader(T& data, int time) {
// synchronize with method clear:
// If reading from the list cannot be done, then return last value.
boost::mutex::scoped_lock lock(rmutex, boost::try_to_lock);
if (!lock.owns_lock() || entity->readQueue_ == -1 ||
time < entity->readQueue_) {
data = last;
} else {
if (empty())
data = last;
else {
data = buffer[frontIdx];
frontIdx = (frontIdx + 1) % N;
last = data;
}
}
return data;
}
} // end of namespace internal.
template <typename T>
void RosQueuedSubscribe::add(const std::string& type, const std::string& signal,
const std::string& topic) {
internal::Add<T>()(*this, type, signal, topic);
}
} // end of namespace dynamicgraph.
#endif //! DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
#endif //! DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX