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
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 1139 additions and 1346 deletions
src/ros_subscribe-python-module-py.cc 0 → 100644
View file @ c2c3026d
#include <dynamic-graph/python/module.hh>
#include "ros_subscribe.hh"
namespace dg = dynamicgraph;
BOOST_PYTHON_MODULE(wrap) {
bp::import("dynamic_graph");
dg::python::exposeEntity<dg::RosSubscribe, bp::bases<dg::Entity>,
dg::python::AddCommands>()
.def("clear", &dg::RosSubscribe::clear,
"Remove all signals reading data from a ROS topic")
.def("rm", &dg::RosSubscribe::rm,
"Remove a signal reading data from a ROS topic",
bp::args("signal_name"))
.def("list", &dg::RosSubscribe::list,
"List signals reading data from a ROS topic");
}
src/ros_subscribe.cpp
View file @ c2c3026d
#include <boost/assign.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
#include "ros_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_subscribe.hh"
namespace dynamicgraph
{
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
namespace command
{
namespace rosSubscribe
{
Clear::Clear
(RosSubscribe& entity, const std::string& docstring)
: Command
(entity,
std::vector<Value::Type> (),
docstring)
{}
Value Clear::doExecute ()
{
RosSubscribe& entity =
static_cast<RosSubscribe&> (owner ());
entity.clear ();
return Value ();
}
List::List
(RosSubscribe& entity, const std::string& docstring)
: Command
(entity,
std::vector<Value::Type> (),
docstring)
{}
Value List::doExecute ()
{
RosSubscribe& entity =
static_cast<RosSubscribe&> (owner ());
return Value (entity.list ());
}
Add::Add
(RosSubscribe& entity, const std::string& docstring)
: Command
(entity,
boost::assign::list_of
(Value::STRING) (Value::STRING) (Value::STRING),
docstring)
{}
Value Add::doExecute ()
{
RosSubscribe& entity =
static_cast<RosSubscribe&> (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> (signal, topic);
else if (type == "unsigned")
entity.add<unsigned int> (signal, topic);
else if (type == "matrix")
entity.add<dg::Matrix> (signal, topic);
else if (type == "vector")
entity.add<dg::Vector> (signal, topic);
else if (type == "vector3")
entity.add<specific::Vector3> (signal, topic);
else if (type == "vector3Stamped")
entity.add<std::pair<specific::Vector3, dg::Vector> > (signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous> (signal, topic);
else if (type == "matrixHomoStamped")
entity.add<std::pair<sot::MatrixHomogeneous, dg::Vector> >
(signal, topic);
else if (type == "twist")
entity.add<specific::Twist> (signal, topic);
else if (type == "twistStamped")
entity.add<std::pair<specific::Twist, dg::Vector> >
(signal, topic);
else
throw std::runtime_error("bad type");
return Value ();
}
Rm::Rm
(RosSubscribe& entity, const std::string& docstring)
: Command
(entity,
boost::assign::list_of (Value::STRING),
docstring)
{}
Value Rm::doExecute ()
{
RosSubscribe& entity =
static_cast<RosSubscribe&> (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.
const std::string RosSubscribe::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");
RosSubscribe::RosSubscribe (const std::string& n)
: dynamicgraph::Entity(n),
nh_ (rosInit (true)),
bindedSignal_ ()
{
std::string docstring =
namespace dynamicgraph {
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
namespace command {
namespace rosSubscribe {
Add::Add(RosSubscribe& entity, const std::string& docstring)
: Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() {
RosSubscribe& entity = static_cast<RosSubscribe&>(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>(signal, topic);
else if (type == "unsigned")
entity.add<unsigned int>(signal, topic);
else if (type == "matrix")
entity.add<dg::Matrix>(signal, topic);
else if (type == "vector")
entity.add<dg::Vector>(signal, topic);
else if (type == "vector3")
entity.add<specific::Vector3>(signal, topic);
else if (type == "vector3Stamped")
entity.add<std::pair<specific::Vector3, dg::Vector> >(signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous>(signal, topic);
else if (type == "matrixHomoStamped")
entity.add<std::pair<sot::MatrixHomogeneous, dg::Vector> >(signal, topic);
else if (type == "twist")
entity.add<specific::Twist>(signal, topic);
else if (type == "twistStamped")
entity.add<std::pair<specific::Twist, dg::Vector> >(signal, topic);
else if (type == "string")
entity.add<std::string>(signal, topic);
else
throw std::runtime_error("bad type");
return Value();
}
} // namespace rosSubscribe
} // end of namespace command.
const std::string RosSubscribe::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");
RosSubscribe::RosSubscribe(const std::string& n)
: dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_() {
std::string docstring =
"\n"
" Add a signal reading data from 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::rosSubscribe::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::rosSubscribe::Rm
(*this, docstring));
docstring =
"\n"
" Remove all signals reading data from a ROS topic\n"
"\n"
" No input:\n"
"\n";
addCommand ("clear",
new command::rosSubscribe::Clear
(*this, docstring));
docstring =
"\n"
" List signals reading data from a ROS topic\n"
"\n"
" No input:\n"
"\n";
addCommand ("list",
new command::rosSubscribe::List
(*this, docstring));
}
addCommand("add", new command::rosSubscribe::Add(*this, docstring));
}
RosSubscribe::~RosSubscribe ()
{}
RosSubscribe::~RosSubscribe() {}
void RosSubscribe::display (std::ostream& os) const
{
os << CLASS_NAME << std::endl;
}
void RosSubscribe::display(std::ostream& os) const {
os << CLASS_NAME << std::endl;
}
void RosSubscribe::rm (const std::string& signal)
{
std::string signalTs = signal+"Timestamp";
void RosSubscribe::rm(const std::string& signal) {
std::string signalTs = signal + "Timestamp";
signalDeregistration(signal);
bindedSignal_.erase (signal);
signalDeregistration(signal);
bindedSignal_.erase(signal);
if(bindedSignal_.find(signalTs) != bindedSignal_.end())
{
signalDeregistration(signalTs);
bindedSignal_.erase(signalTs);
}
if (bindedSignal_.find(signalTs) != bindedSignal_.end()) {
signalDeregistration(signalTs);
bindedSignal_.erase(signalTs);
}
std::string RosSubscribe::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;
}
std::vector<std::string> RosSubscribe::list() {
std::vector<std::string> result(bindedSignal_.size());
std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
[](const auto& pair) { return pair.first; });
return result;
}
void RosSubscribe::clear() {
std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
for (; it != bindedSignal_.end();) {
rm(it->first);
it = bindedSignal_.begin();
}
}
void RosSubscribe::clear ()
{
std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
for(; it!= bindedSignal_.end(); )
{
rm(it->first);
it = bindedSignal_.begin();
}
}
std::string RosSubscribe::getDocString () const
{
return docstring_;
}
} // end of namespace dynamicgraph.
std::string RosSubscribe::getDocString() const { return docstring_; }
} // end of namespace dynamicgraph.
src/ros_subscribe.hh
View file @ c2c3026d
#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
# define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
# include <map>
# include <boost/shared_ptr.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 RosSubscribe;
namespace command
{
namespace rosSubscribe
{
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
# define ROS_SUBSCRIBE_MAKE_COMMAND(CMD) \
class CMD : public Command \
{ \
public: \
CMD (RosSubscribe& entity, \
const std::string& docstring); \
virtual Value doExecute (); \
}
ROS_SUBSCRIBE_MAKE_COMMAND(Add);
ROS_SUBSCRIBE_MAKE_COMMAND(Clear);
ROS_SUBSCRIBE_MAKE_COMMAND(List);
ROS_SUBSCRIBE_MAKE_COMMAND(Rm);
#define DYNAMIC_GRAPH_ROS_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 <map>
#include <sot/core/matrix-geometry.hh>
#include "converter.hh"
#include "sot_to_ros.hh"
namespace dynamicgraph {
class RosSubscribe;
namespace command {
namespace rosSubscribe {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
#define ROS_SUBSCRIBE_MAKE_COMMAND(CMD) \
class CMD : public Command { \
public: \
CMD(RosSubscribe& entity, const std::string& docstring); \
virtual Value doExecute(); \
}
ROS_SUBSCRIBE_MAKE_COMMAND(Add);
#undef ROS_SUBSCRIBE_MAKE_COMMAND
} // end of namespace errorEstimator.
} // end of namespace command.
} // namespace rosSubscribe
} // end of namespace command.
namespace internal {
template <typename T>
struct Add;
} // namespace internal
namespace internal
{
template <typename T>
struct Add;
} // end of internal namespace.
/// \brief Publish ROS information in the dynamic-graph.
class RosSubscribe : public dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
typedef boost::posix_time::ptime ptime;
/// \brief Publish ROS information in the dynamic-graph.
class RosSubscribe : public dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
typedef boost::posix_time::ptime ptime;
public:
typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
boost::shared_ptr<ros::Subscriber> >
public:
typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
boost::shared_ptr<ros::Subscriber> >
bindedSignal_t;
RosSubscribe (const std::string& n);
virtual ~RosSubscribe ();
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 ();
void clear ();
template <typename T>
void add (const std::string& signal, const std::string& topic);
std::map<std::string, bindedSignal_t>&
bindedSignal ()
{
return bindedSignal_;
}
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_;
};
} // end of namespace dynamicgraph.
# include "ros_subscribe.hxx"
#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
RosSubscribe(const std::string& n);
virtual ~RosSubscribe();
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::vector<std::string> list();
void clear();
template <typename T>
void add(const std::string& signal, const std::string& topic);
std::map<std::string, bindedSignal_t>& bindedSignal() {
return bindedSignal_;
}
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_;
};
} // end of namespace dynamicgraph.
#include "ros_subscribe.hxx"
#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
src/ros_subscribe.hxx
View file @ c2c3026d
This diff is collapsed.
src/ros_tf_listener-python-module-py.cc 0 → 100644
View file @ c2c3026d
#include <dynamic-graph/python/module.hh>
#include "ros_tf_listener.hh"
namespace dg = dynamicgraph;
BOOST_PYTHON_MODULE(wrap) {
bp::import("dynamic_graph");
dg::python::exposeEntity<dg::RosTfListener, bp::bases<dg::Entity>,
dg::python::AddCommands>()
.def("add", &dg::RosTfListener::add,
"Add a signal containing the transform between two frames.",
bp::args("to_frame_name", "from_frame_name", "out_signal_name"))
.def("setMaximumDelay", &dg::RosTfListener::setMaximumDelay,
"Set the maximum time delay of the transform obtained from tf.",
bp::args("signal_name", "delay_seconds"));
}
src/ros_tf_listener.cpp
View file @ c2c3026d
This diff is collapsed.
src/ros_tf_listener.hh
View file @ c2c3026d
This diff is collapsed.
src/ros_time-python.hh 0 → 100644
View file @ c2c3026d
#include "ros_time.hh"
typedef boost::mpl::vector<dynamicgraph::RosTime> entities_t;
src/ros_time.cpp
View file @ c2c3026d
This diff is collapsed.
src/ros_time.hh
View file @ c2c3026d
This diff is collapsed.
src/sot_loader.cpp
View file @ c2c3026d
This diff is collapsed.
src/sot_loader_basic.cpp
View file @ c2c3026d
This diff is collapsed.
src/sot_to_ros.cpp
View file @ c2c3026d
This diff is collapsed.
src/sot_to_ros.hh
View file @ c2c3026d
This diff is collapsed.
srv/RunPythonFile.srv deleted 100644 → 0
View file @ 86cfdc71
string input
---
string result
tests/CMakeLists.txt
View file @ c2c3026d
IF(BUILD_PYTHON_INTERFACE)
# TODO: this test requires a ros master
#ADD_PYTHON_UNIT_TEST("py-import" "tests/test_import.py")
ENDIF(BUILD_PYTHON_INTERFACE)
if(BUILD_PYTHON_INTERFACE)
# TODO: this test requires a ros master ADD_PYTHON_UNIT_TEST("py-import"
# "tests/test_import.py")
endif(BUILD_PYTHON_INTERFACE)
tests/test_import.py
View file @ c2c3026d
This diff is collapsed.
travis_custom/Dockerfile deleted 100644 → 0
View file @ 86cfdc71
This diff is collapsed.
travis_custom/setup-opt-robotpkg.sh deleted 100755 → 0
View file @ 86cfdc71
#!/bin/bash
export ROBOTPKG_BASE=/opt/openrobots
export PATH=$PATH:$ROBOTPKG_BASE/sbin:$ROBOTPKG_BASE/bin
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$ROBOTPKG_BASE/lib:$ROBOTPKG_BASE/lib/plugin:$ROBOTPKG_BASE/lib64
export PYTHONPATH=$PYTHONPATH:$ROBOTPKG_BASE/lib/python2.7/site-packages:$ROBOTPKG_BASE/lib/python2.7/dist-packages
export PKG_CONFIG_PATH=$PKG_CONFIG_PATH:$ROBOTPKG_BASE/lib/pkgconfig/
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$ROBOTPKG_BASE/share:$ROBOTPKG_BASE/stacks
export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:$ROBOTPKG_BASE
travis_custom/sudoers.txt deleted 100644 → 0
View file @ 86cfdc71
root ALL=(ALL) ALL
docker ALL=(ALL) NOPASSWD: ALL
Defaults env_reset
Defaults secure_path="/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin"