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 411 additions and 534 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 <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);
......
src/dynamic_graph/ros/__init__.py
View file @ c2c3026d
# 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 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
......@@ -62,18 +62,18 @@ class DGCompleter:
self.client(astr)
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")
"""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) + ")"
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
class Ros(object):
......@@ -12,13 +12,13 @@ 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
......
src/geometric_simu.cpp
View file @ c2c3026d
......@@ -7,26 +7,28 @@
*/
#include <ros/console.h>
#include <iostream>
#define ENABLE_RT_LOG
#include <dynamic-graph/real-time-logger.h>
#include <dynamic_graph_bridge/sot_loader.hh>
class LoggerROSStream : public ::dynamicgraph::LoggerStream {
public:
void write(const char *c) { ROS_ERROR(c); }
};
int main(int argc, char *argv[]) {
::dynamicgraph::RealTimeLogger::instance()
.addOutputStream(::dynamicgraph::LoggerStreamPtr_t(new LoggerROSStream()));
::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;
// 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::waitForShutdown ();
ros::waitForShutdown();
return 0;
}
src/robot_model.cpp deleted 100644 → 0
View file @ 387b381d
#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 @ 387b381d
#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 "dynamic_graph_bridge/ros_init.hh"
#include <stdexcept>
namespace dynamicgraph {
struct GlobalRos {
......@@ -17,7 +17,8 @@ struct GlobalRos {
};
GlobalRos ros;
ros::NodeHandle& rosInit(bool createAsyncSpinner, bool createMultiThreadedSpinner) {
ros::NodeHandle& rosInit(bool createAsyncSpinner,
bool createMultiThreadedSpinner) {
if (!ros.nodeHandle) {
int argc = 1;
char* arg0 = strdup("dynamic_graph_bridge");
......@@ -34,11 +35,14 @@ ros::NodeHandle& rosInit(bool createAsyncSpinner, bool createMultiThreadedSpinne
// priority
int oldThreadPolicy, newThreadPolicy;
struct sched_param oldThreadParam, newThreadParam;
if (pthread_getschedparam(pthread_self(), &oldThreadPolicy, &oldThreadParam) == 0) {
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 -=
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);
......
src/ros_interpreter.cpp
View file @ c2c3026d
......@@ -4,37 +4,49 @@ namespace dynamicgraph {
static const int queueSize = 5;
Interpreter::Interpreter(ros::NodeHandle& nodeHandle)
: interpreter_(), nodeHandle_(nodeHandle), runCommandSrv_(), runPythonFileSrv_() {}
: interpreter_(),
nodeHandle_(nodeHandle),
runCommandSrv_(),
runPythonFileSrv_() {}
void Interpreter::startRosService() {
runCommandCallback_t runCommandCb = boost::bind(&Interpreter::runCommandCallback, this, _1, _2);
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);
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);
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) {
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) {
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::runPythonFile(std::string ifilename) { interpreter_.runPythonFile(ifilename); }
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,17 +13,9 @@
#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"
#define ENABLE_RT_LOG
#include <dynamic-graph/real-time-logger.h>
......@@ -28,26 +27,12 @@ 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) {}
: Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() {
RosPublish& entity = static_cast<RosPublish&>(owner());
......@@ -88,16 +73,6 @@ Value Add::doExecute() {
return Value();
}
Rm::Rm(RosPublish& entity, const std::string& docstring)
: Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
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();
}
} // namespace rosPublish
} // end of namespace command.
......@@ -125,7 +100,8 @@ RosPublish::RosPublish(const std::string& n)
clock_gettime(CLOCK_REALTIME, &nextPublicationRT_);
}
} catch (const std::exception& exc) {
throw std::runtime_error("Failed to call ros::Time::now ():" + std::string(exc.what()));
throw std::runtime_error("Failed to call ros::Time::now ():" +
std::string(exc.what()));
}
signalRegistration(trigger_);
trigger_.setNeedUpdateFromAllChildren(true);
......@@ -143,40 +119,20 @@ RosPublish::RosPublish(const std::string& 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));
}
RosPublish::~RosPublish() { aofs_.close(); }
void RosPublish::display(std::ostream& os) const { os << CLASS_NAME << std::endl; }
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;
if (signal == "trigger") {
std::cerr << "The trigger signal should not be removed. Aborting." << std::endl;
std::cerr << "The trigger signal should not be removed. Aborting."
<< std::endl;
return;
}
......@@ -187,13 +143,10 @@ void RosPublish::rm(const std::string& signal) {
bindedSignal_.erase(signal);
}
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 + "',";
}
result += "]";
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;
}
......
src/ros_publish.hh
View file @ c2c3026d
#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HH
#define DYNAMIC_GRAPH_ROS_PUBLISH_HH
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/thread/mutex.hpp>
#include <dynamic-graph/command.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/command.h>
#include <realtime_tools/realtime_publisher.h>
#include <ros/ros.h>
#include <realtime_tools/realtime_publisher.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"
#include <fstream>
namespace dynamicgraph {
class RosPublish;
......@@ -34,9 +31,6 @@ using ::dynamicgraph::command::Value;
}
ROS_PUBLISH_MAKE_COMMAND(Add);
ROS_PUBLISH_MAKE_COMMAND(Clear);
ROS_PUBLISH_MAKE_COMMAND(List);
ROS_PUBLISH_MAKE_COMMAND(Rm);
#undef ROS_PUBLISH_MAKE_COMMAND
......@@ -50,7 +44,9 @@ class RosPublish : public dynamicgraph::Entity {
public:
typedef boost::function<void(int)> callback_t;
typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >, callback_t> bindedSignal_t;
typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
callback_t>
bindedSignal_t;
static const double ROS_JOINT_STATE_PUBLISHER_RATE;
......@@ -62,14 +58,17 @@ class RosPublish : public dynamicgraph::Entity {
void add(const std::string& signal, const std::string& topic);
void rm(const std::string& signal);
std::string list() const;
std::vector<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);
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);
......
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>
#include <vector>
#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> >
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) {
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";
......@@ -23,8 +27,11 @@ inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >(
}
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) {
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));
......@@ -42,17 +49,19 @@ void RosPublish::add(const std::string& signal, const std::string& topic) {
// Initialize the publisher.
boost::shared_ptr<realtime_tools::RealtimePublisher<ros_t> > pubPtr =
boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >(nh_, topic, 1);
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::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);
callback_t callback =
boost::bind(&RosPublish::sendData<T>, this, pubPtr, signalPtr, _1);
boost::get<1>(bindedSignal) = callback;
bindedSignal_[signal] = bindedSignal;
......
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
......@@ -4,59 +4,31 @@
//
//
#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) {}
: Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() {
RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
......@@ -84,42 +56,6 @@ Value Add::doExecute() {
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();
}
} // namespace rosQueuedSubscribe
} // end of namespace command.
......@@ -129,7 +65,10 @@ const std::string RosQueuedSubscribe::docstring_(
" 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) {
: dynamicgraph::Entity(n),
nh_(rosInit(true)),
bindedSignal_(),
readQueue_(-1) {
std::string docstring =
"\n"
" Add a signal reading data from a ROS topic\n"
......@@ -141,58 +80,13 @@ RosQueuedSubscribe::RosQueuedSubscribe(const std::string& 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));
}
RosQueuedSubscribe::~RosQueuedSubscribe() {}
void RosQueuedSubscribe::display(std::ostream& os) const { os << CLASS_NAME << std::endl; }
void RosQueuedSubscribe::display(std::ostream& os) const {
os << CLASS_NAME << std::endl;
}
void RosQueuedSubscribe::rm(const std::string& signal) {
std::string signalTs = signal + "Timestamp";
......@@ -206,13 +100,17 @@ void RosQueuedSubscribe::rm(const std::string& 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 += "]";
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;
}
......@@ -231,7 +129,8 @@ void RosQueuedSubscribe::clearQueue(const std::string& signal) {
}
std::size_t RosQueuedSubscribe::queueSize(const std::string& signal) const {
std::map<std::string, bindedSignal_t>::const_iterator _bs = bindedSignal_.find(signal);
std::map<std::string, bindedSignal_t>::const_iterator _bs =
bindedSignal_.find(signal);
if (_bs != bindedSignal_.end()) {
return _bs->second->size();
}
......
src/ros_queued_subscribe.hh
View file @ c2c3026d
......@@ -6,19 +6,17 @@
#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
#include <map>
#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 <dynamic-graph/entity.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/command.h>
#include <map>
#include <sot/core/matrix-geometry.hh>
#include <ros/ros.h>
#include "converter.hh"
#include "sot_to_ros.hh"
......@@ -38,12 +36,6 @@ using ::dynamicgraph::command::Value;
}
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);
#undef ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND
......@@ -77,7 +69,11 @@ struct BindedSignal : BindedSignalBase {
typedef typename buffer_t::size_type size_type;
BindedSignal(RosQueuedSubscribe* e)
: BindedSignalBase(e), frontIdx(0), backIdx(0), buffer(BufferSize), init(false) {}
: BindedSignalBase(e),
frontIdx(0),
backIdx(0),
buffer(BufferSize),
init(false) {}
~BindedSignal() {
signal.reset();
clear();
......@@ -142,24 +138,32 @@ class RosQueuedSubscribe : public dynamicgraph::Entity {
void display(std::ostream& os) const;
void rm(const std::string& signal);
std::string list();
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);
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, 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);
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);
void callbackTimestamp(
boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
const R& data);
template <typename T>
friend class internal::Add;
......@@ -168,6 +172,7 @@ class RosQueuedSubscribe : public dynamicgraph::Entity {
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_;
......
src/ros_queued_subscribe.hxx
View file @ c2c3026d
......@@ -8,14 +8,16 @@
#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/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"
......@@ -25,8 +27,8 @@ 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) {
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;
......@@ -46,14 +48,17 @@ struct Add {
// 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);
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));
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;
}
};
......@@ -85,7 +90,8 @@ 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_) {
if (!lock.owns_lock() || entity->readQueue_ == -1 ||
time < entity->readQueue_) {
data = last;
} else {
if (empty())
......@@ -101,7 +107,8 @@ T& BindedSignal<T, N>::reader(T& data, int time) {
} // end of namespace internal.
template <typename T>
void RosQueuedSubscribe::add(const std::string& type, const std::string& signal, const std::string& topic) {
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.
......