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 384 additions and 281 deletions
src/CMakeLists.txt
View file @ c2c3026d
SET(plugins
ros_publish
ros_subscribe
ros_queued_subscribe
ros_tf_listener
ros_time
)
set(plugins ros_publish ros_subscribe ros_queued_subscribe ros_tf_listener
ros_time)
FOREACH(plugin ${plugins})
GET_FILENAME_COMPONENT(LIBRARY_NAME ${plugin} NAME)
ADD_LIBRARY(${LIBRARY_NAME} SHARED ${plugin}.cpp ${plugin}.hh)
foreach(plugin ${plugins})
get_filename_component(LIBRARY_NAME ${plugin} NAME)
add_library(${LIBRARY_NAME} SHARED ${plugin}.cpp ${plugin}.hh)
IF(SUFFIX_SO_VERSION)
SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
ENDIF(SUFFIX_SO_VERSION)
if(SUFFIX_SO_VERSION)
set_target_properties(${LIBRARY_NAME} PROPERTIES SOVERSION
${PROJECT_VERSION})
endif(SUFFIX_SO_VERSION)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${${LIBRARY_NAME}_deps} ${catkin_LIBRARIES} ros_bridge
dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
target_link_libraries(${LIBRARY_NAME} ${${LIBRARY_NAME}_deps}
${catkin_LIBRARIES} ros_bridge)
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(TARGETS ${LIBRARY_NAME} EXPORT ${TARGETS_EXPORT_NAME}
if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
install(
TARGETS ${LIBRARY_NAME}
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
IF(BUILD_PYTHON_INTERFACE)
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
if(BUILD_PYTHON_INTERFACE)
string(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
DYNAMIC_GRAPH_PYTHON_MODULE("ros/${PYTHON_LIBRARY_NAME}"
${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
SOURCE_PYTHON_MODULE "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
dynamic_graph_python_module(
"ros/${PYTHON_LIBRARY_NAME}" ${LIBRARY_NAME}
${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap SOURCE_PYTHON_MODULE
"${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
DYNAMIC_GRAPH_PYTHON_MODULE("ros/${PYTHON_LIBRARY_NAME}"
${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
MODULE_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
dynamic_graph_python_module(
"ros/${PYTHON_LIBRARY_NAME}" ${LIBRARY_NAME}
${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap MODULE_HEADER
"${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
endif()
ENDIF(BUILD_PYTHON_INTERFACE)
ENDFOREACH(plugin)
endif(BUILD_PYTHON_INTERFACE)
endforeach(plugin)
target_link_libraries(ros_publish ros_bridge)
IF(BUILD_PYTHON_INTERFACE)
PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "__init__.py")
PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "ros.py")
PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "dgcompleter.py")
if(BUILD_PYTHON_INTERFACE)
python_install_on_site("dynamic_graph/ros" "__init__.py")
python_install_on_site("dynamic_graph/ros" "ros.py")
python_install_on_site("dynamic_graph/ros" "dgcompleter.py")
# ros_interperter library.
add_library(ros_interpreter ros_interpreter.cpp)
TARGET_LINK_LIBRARIES(ros_interpreter ros_bridge ${catkin_LIBRARIES}
dynamic-graph-python::dynamic-graph-python
dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
target_link_libraries(ros_interpreter ros_bridge ${catkin_LIBRARIES}
dynamic-graph-python::dynamic-graph-python)
install(TARGETS ros_interpreter
install(
TARGETS ros_interpreter
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION lib)
ENDIF(BUILD_PYTHON_INTERFACE)
endif(BUILD_PYTHON_INTERFACE)
# Stand alone embedded intepreter with a robot controller.
add_executable(geometric_simu geometric_simu.cpp sot_loader.cpp sot_loader_basic.cpp)
target_link_libraries(geometric_simu Boost::program_options ${CMAKE_DL_LIBS} ${catkin_LIBRARIES} ros_bridge
dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
install(TARGETS geometric_simu
DESTINATION lib/${PROJECT_NAME})
add_executable(geometric_simu geometric_simu.cpp sot_loader.cpp
sot_loader_basic.cpp)
target_link_libraries(geometric_simu Boost::program_options ${CMAKE_DL_LIBS}
${catkin_LIBRARIES} ros_bridge)
install(TARGETS geometric_simu DESTINATION lib/${PROJECT_NAME})
# Sot loader library
add_library(sot_loader sot_loader.cpp sot_loader_basic.cpp)
target_link_libraries(sot_loader Boost::program_options ${catkin_LIBRARIES} ros_bridge
dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs)
install(TARGETS sot_loader EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
target_link_libraries(sot_loader Boost::program_options ${catkin_LIBRARIES}
ros_bridge)
install(
TARGETS sot_loader
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION lib)
src/converter.hh
View file @ c2c3026d
#ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH
#define DYNAMIC_GRAPH_ROS_CONVERTER_HH
#include <stdexcept>
#include "sot_to_ros.hh"
#include <ros/time.h>
#include <std_msgs/Header.h>
#include <boost/static_assert.hpp>
#include <boost/date_time/date.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/static_assert.hpp>
#include <stdexcept>
#include <ros/time.h>
#include <std_msgs/Header.h>
#include "sot_to_ros.hh"
#define SOT_TO_ROS_IMPL(T) \
template <> \
inline void converter(SotToRos<T>::ros_t& dst, const SotToRos<T>::sot_t& src)
#define SOT_TO_ROS_IMPL(T) \
template <> \
inline void converter(SotToRos<T>::ros_t& dst, \
const SotToRos<T>::sot_t& src)
#define ROS_TO_SOT_IMPL(T) \
template <> \
inline void converter(SotToRos<T>::sot_t& dst, const SotToRos<T>::ros_t& src)
#define ROS_TO_SOT_IMPL(T) \
template <> \
inline void converter(SotToRos<T>::sot_t& dst, \
const SotToRos<T>::ros_t& src)
namespace dynamicgraph {
inline void makeHeader(std_msgs::Header& header) {
......@@ -99,7 +101,8 @@ SOT_TO_ROS_IMPL(Matrix) {
}
ROS_TO_SOT_IMPL(Matrix) {
dst.resize(src.width, (unsigned int)src.data.size() / (unsigned int)src.width);
dst.resize(src.width,
(unsigned int)src.data.size() / (unsigned int)src.width);
for (unsigned i = 0; i < src.data.size(); ++i) dst.data()[i] = src.data[i];
}
......@@ -117,7 +120,8 @@ SOT_TO_ROS_IMPL(sot::MatrixHomogeneous) {
}
ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y, src.rotation.z);
sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y,
src.rotation.z);
dst.linear() = q.matrix();
// Copy the translation component.
......@@ -128,7 +132,8 @@ ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
// Twist.
SOT_TO_ROS_IMPL(specific::Twist) {
if (src.size() != 6) throw std::runtime_error("failed to convert invalid twist");
if (src.size() != 6)
throw std::runtime_error("failed to convert invalid twist");
dst.linear.x = src(0);
dst.linear.y = src(1);
dst.linear.z = src(2);
......@@ -163,7 +168,8 @@ ROS_TO_SOT_IMPL(specific::Twist) {
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, dst.child_frame_id = "";);
DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform,
dst.child_frame_id = "";);
DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
/// \brief This macro generates a converter for a shared pointer on
......@@ -172,11 +178,13 @@ DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
/// A converter for the underlying type is required. I.e. to
/// convert a shared_ptr<T> to T', a converter from T to T'
/// is required.
#define DG_BRIDGE_MAKE_SHPTR_IMPL(T) \
template <> \
inline void converter(SotToRos<T>::sot_t& dst, const boost::shared_ptr<SotToRos<T>::ros_t const>& src) { \
converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, *src); \
} \
#define DG_BRIDGE_MAKE_SHPTR_IMPL(T) \
template <> \
inline void converter( \
SotToRos<T>::sot_t& dst, \
const boost::shared_ptr<SotToRos<T>::ros_t const>& src) { \
converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, *src); \
} \
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
......@@ -213,15 +221,17 @@ DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
/// a stamped type. I.e. A data associated with its timestamp.
///
/// FIXME: the timestamp is not yet forwarded to the dg signal.
#define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA) \
template <> \
inline void converter(SotToRos<std::pair<T, Vector> >::sot_t& dst, \
const boost::shared_ptr<SotToRos<std::pair<T, Vector> >::ros_t const>& src) { \
converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src->ATTRIBUTE); \
do { \
EXTRA \
} while (0); \
} \
#define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA) \
template <> \
inline void converter( \
SotToRos<std::pair<T, Vector> >::sot_t& dst, \
const boost::shared_ptr<SotToRos<std::pair<T, Vector> >::ros_t const>& \
src) { \
converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src->ATTRIBUTE); \
do { \
EXTRA \
} while (0); \
} \
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Vector3, vector, ;);
......@@ -251,7 +261,8 @@ typedef boost::posix_time::time_duration time_duration;
typedef boost::gregorian::date date;
boost::posix_time::ptime rosTimeToPtime(const ros::Time& rosTime) {
ptime time(date(1970, 1, 1), seconds(rosTime.sec) + microseconds(rosTime.nsec / 1000));
ptime time(date(1970, 1, 1),
seconds(rosTime.sec) + microseconds(rosTime.nsec / 1000));
return time;
}
......@@ -259,7 +270,8 @@ ros::Time pTimeToRostime(const boost::posix_time::ptime& time) {
static ptime timeStart(date(1970, 1, 1));
time_duration diff = time - timeStart;
uint32_t sec = (unsigned int)diff.ticks() / (unsigned int)time_duration::rep_type::res_adjust();
uint32_t sec = (unsigned int)diff.ticks() /
(unsigned int)time_duration::rep_type::res_adjust();
uint32_t nsec = (unsigned int)diff.fractional_seconds();
return ros::Time(sec, nsec);
......
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
......@@ -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
......@@ -5,17 +5,19 @@
* CNRS
*
*/
#include <iostream>
#include <ros/console.h>
#include <iostream>
#define ENABLE_RT_LOG
#include <dynamic-graph/real-time-logger.h>
#include <dynamic_graph_bridge/sot_loader.hh>
int main(int argc, char *argv[]) {
::dynamicgraph::RealTimeLogger::instance()
.addOutputStream(::dynamicgraph::LoggerStreamPtr_t(new dynamicgraph::LoggerIOStream(std::cout)));
::dynamicgraph::RealTimeLogger::instance().addOutputStream(
::dynamicgraph::LoggerStreamPtr_t(
new dynamicgraph::LoggerIOStream(std::cout)));
ros::init(argc, argv, "sot_ros_encapsulator");
SotLoader aSotLoader;
......@@ -27,6 +29,6 @@ int main(int argc, char *argv[]) {
// Load dynamic library and run python prologue.
aSotLoader.Initialization();
ros::waitForShutdown ();
ros::waitForShutdown();
return 0;
}
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
View file @ c2c3026d
#include <sot/core/robot-utils.hh>
#include <pinocchio/fwd.hpp>
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"
// include pinocchio first
//
#include <ros/ros.h>
#include <urdf_parser/urdf_parser.h>
#include <stdexcept>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <sot/core/robot-utils.hh>
#include <stdexcept>
#include <urdf_parser/urdf_parser.h>
#include <ros/ros.h>
#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()
{
bool parameter_server_read_robot_description() {
ros::NodeHandle nh;
if (!nh.hasParam("/robot_description"))
{
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);
nh.getParam(parameter_name, robot_description);
std::string model_name("robot");
......@@ -35,11 +35,10 @@ bool parameter_server_read_robot_description()
aRobotUtil = sot::createRobotUtil(model_name);
// If the creation is fine
if (aRobotUtil != sot::RefVoidRobotUtil())
{
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());
aRobotUtil->set_parameter(parameter_name, robot_description);
ROS_INFO("Set parameter_name : %s.", parameter_name.c_str());
// Everything went fine.
return true;
}
......@@ -48,7 +47,6 @@ bool parameter_server_read_robot_description()
// Otherwise something went wrong.
return false;
}
}
} // namespace dynamicgraph
src/ros_publish-python-module-py.cc
View file @ c2c3026d
#include <dynamic-graph/python/module.hh>
#include "ros_publish.hh"
namespace dg = dynamicgraph;
BOOST_PYTHON_MODULE(wrap)
{
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")
;
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>
......@@ -30,7 +29,10 @@ 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) {}
: Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() {
RosPublish& entity = static_cast<RosPublish&>(owner());
......@@ -98,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);
......@@ -120,13 +123,16 @@ RosPublish::RosPublish(const std::string& n)
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;
}
......@@ -139,8 +145,8 @@ void RosPublish::rm(const std::string& signal) {
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; });
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;
......@@ -47,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;
......@@ -65,8 +64,11 @@ class RosPublish : public dynamicgraph::Entity {
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
View file @ c2c3026d
#include <dynamic-graph/python/module.hh>
#include "ros_queued_subscribe.hh"
namespace dg = dynamicgraph;
BOOST_PYTHON_MODULE(wrap)
{
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"))
;
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,20 +4,20 @@
//
//
#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");
......@@ -25,7 +25,10 @@ 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) {}
: Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() {
RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
......@@ -62,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"
......@@ -78,7 +84,9 @@ RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n)
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";
......@@ -94,16 +102,15 @@ void RosQueuedSubscribe::rm(const std::string& signal) {
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; });
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> RosQueuedSubscribe::listTopics() {
std::vector<std::string> result(topics_.size());
std::transform(topics_.begin(), topics_.end(),
result.begin(), [](const auto& pair) { return pair.second; });
std::transform(topics_.begin(), topics_.end(), result.begin(),
[](const auto& pair) { return pair.second; });
return result;
}
......@@ -122,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"
......@@ -71,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();
......@@ -144,18 +146,24 @@ class RosQueuedSubscribe : public dynamicgraph::Entity {
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;
......
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,11 +48,13 @@ 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;
......@@ -86,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())
......@@ -102,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.
......
src/ros_subscribe-python-module-py.cc
View file @ c2c3026d
#include <dynamic-graph/python/module.hh>
#include "ros_subscribe.hh"
namespace dg = dynamicgraph;
BOOST_PYTHON_MODULE(wrap)
{
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")
;
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");
......@@ -19,7 +19,10 @@ 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) {}
: Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() {
RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
......@@ -63,7 +66,8 @@ const std::string RosSubscribe::docstring_(
"\n"
" Use command \"add\" to subscribe to a new signal.\n");
RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_() {
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"
......@@ -81,7 +85,9 @@ RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n), nh_(
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";
......@@ -97,8 +103,8 @@ void RosSubscribe::rm(const std::string& signal) {
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; });
std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
[](const auto& pair) { return pair.first; });
return result;
}
......
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/command.h>
#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 <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"
......@@ -48,7 +46,8 @@ class RosSubscribe : public dynamicgraph::Entity {
typedef boost::posix_time::ptime ptime;
public:
typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >, boost::shared_ptr<ros::Subscriber> >
typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
boost::shared_ptr<ros::Subscriber> >
bindedSignal_t;
RosSubscribe(const std::string& n);
......@@ -65,15 +64,20 @@ class RosSubscribe : public dynamicgraph::Entity {
template <typename T>
void add(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_;
}
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;
......
src/ros_subscribe.hxx
View file @ c2c3026d
#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
#define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
#include <vector>
#include <boost/bind.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <dynamic-graph/signal-caster.h>
#include <dynamic-graph/linear-algebra.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"
#include "ros_time.hh"
......@@ -15,7 +17,8 @@ namespace dg = dynamicgraph;
namespace dynamicgraph {
template <typename R, typename S>
void RosSubscribe::callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data) {
void RosSubscribe::callback(
boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data) {
typedef S sot_t;
sot_t value;
converter(value, data);
......@@ -23,7 +26,9 @@ void RosSubscribe::callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> >
}
template <typename R>
void RosSubscribe::callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data) {
void RosSubscribe::callbackTimestamp(
boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
const R& data) {
ptime time = rosTimeToPtime(data->header.stamp);
signal->setConstant(time);
}
......@@ -31,7 +36,8 @@ void RosSubscribe::callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<p
namespace internal {
template <typename T>
struct Add {
void operator()(RosSubscribe& RosSubscribe, const std::string& signal, const std::string& topic) {
void operator()(RosSubscribe& RosSubscribe, 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 typename SotToRos<T>::signalIn_t signal_t;
......@@ -50,9 +56,12 @@ struct Add {
// Initialize the subscriber.
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
callback_t callback = boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>, &RosSubscribe, signal_, _1);
callback_t callback =
boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>,
&RosSubscribe, signal_, _1);
bindedSignal.second = boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callback));
bindedSignal.second = boost::make_shared<ros::Subscriber>(
RosSubscribe.nh().subscribe(topic, 1, callback));
RosSubscribe.bindedSignal()[signal] = bindedSignal;
}
......@@ -60,7 +69,8 @@ struct Add {
template <typename T>
struct Add<std::pair<T, dg::Vector> > {
void operator()(RosSubscribe& RosSubscribe, const std::string& signal, const std::string& topic) {
void operator()(RosSubscribe& RosSubscribe, const std::string& signal,
const std::string& topic) {
typedef std::pair<T, dg::Vector> type_t;
typedef typename SotToRos<type_t>::sot_t sot_t;
......@@ -81,15 +91,19 @@ struct Add<std::pair<T, dg::Vector> > {
// Initialize the publisher.
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
callback_t callback = boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>, &RosSubscribe, signal_, _1);
callback_t callback =
boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>,
&RosSubscribe, signal_, _1);
bindedSignal.second = boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callback));
bindedSignal.second = boost::make_shared<ros::Subscriber>(
RosSubscribe.nh().subscribe(topic, 1, callback));
RosSubscribe.bindedSignal()[signal] = bindedSignal;
// Timestamp.
typedef dynamicgraph::SignalPtr<RosSubscribe::ptime, int> signalTimestamp_t;
std::string signalTimestamp = (boost::format("%1%%2%") % signal % "Timestamp").str();
std::string signalTimestamp =
(boost::format("%1%%2%") % signal % "Timestamp").str();
// Initialize the bindedSignal object.
RosSubscribe::bindedSignal_t bindedSignalTimestamp;
......@@ -98,7 +112,8 @@ struct Add<std::pair<T, dg::Vector> > {
boost::format signalNameTimestamp("RosSubscribe(%1%)::%2%");
signalNameTimestamp % RosSubscribe.name % signalTimestamp;
boost::shared_ptr<signalTimestamp_t> signalTimestamp_(new signalTimestamp_t(0, signalNameTimestamp.str()));
boost::shared_ptr<signalTimestamp_t> signalTimestamp_(
new signalTimestamp_t(0, signalNameTimestamp.str()));
RosSubscribe::ptime zero(rosTimeToPtime(ros::Time(0, 0)));
signalTimestamp_->setConstant(zero);
......@@ -108,10 +123,11 @@ struct Add<std::pair<T, dg::Vector> > {
// Initialize the publisher.
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
callback_t callbackTimestamp =
boost::bind(&RosSubscribe::callbackTimestamp<ros_const_ptr_t>, &RosSubscribe, signalTimestamp_, _1);
boost::bind(&RosSubscribe::callbackTimestamp<ros_const_ptr_t>,
&RosSubscribe, signalTimestamp_, _1);
bindedSignalTimestamp.second =
boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp));
bindedSignalTimestamp.second = boost::make_shared<ros::Subscriber>(
RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp));
RosSubscribe.bindedSignal()[signalTimestamp] = bindedSignalTimestamp;
}
......