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 393 additions and 456 deletions
setup.cfg 0 → 100644
View file @ c2c3026d
[flake8]
exclude = cmake
max-line-length = 88
extend-ignore = E203
src/CMakeLists.txt
View file @ c2c3026d
SET(plugins
ros_publish
ros_subscribe
ros_queued_subscribe
ros_tf_listener
ros_time
)
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)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic_graph_bridge_msgs)
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(TARGETS ${LIBRARY_NAME} EXPORT ${TARGETS_EXPORT_NAME}
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)
endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
IF(BUILD_PYTHON_INTERFACE)
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
DYNAMIC_GRAPH_PYTHON_MODULE("ros/${PYTHON_LIBRARY_NAME}"
${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap)
ENDIF(BUILD_PYTHON_INTERFACE)
ENDFOREACH(plugin)
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")
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)
PKG_CONFIG_USE_DEPENDENCY(ros_interpreter 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_LIBRARIES} ${CMAKE_DL_LIBS} ${catkin_LIBRARIES} ros_bridge)
pkg_config_use_dependency(geometric_simu 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_LIBRARIES} ${catkin_LIBRARIES} ros_bridge)
pkg_config_use_dependency(sot_loader 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/__init__.py
View file @ c2c3026d
......@@ -3,3 +3,4 @@ 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
......@@ -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,24 +5,30 @@
* 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;
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/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 "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include <stdexcept>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#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 <ros/ros.h>
#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()
{
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");
// 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())
if (aRobotUtil == sot::RefVoidRobotUtil())
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;
}
......@@ -47,7 +47,6 @@ bool parameter_server_read_robot_description()
// 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.
......
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) {}
: Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() {
RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
......@@ -73,17 +58,6 @@ Value Add::doExecute() {
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();
}
} // namespace rosSubscribe
} // end of namespace command.
......@@ -92,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"
......@@ -106,34 +81,13 @@ RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n), nh_(
" - 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));
}
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";
......@@ -147,13 +101,10 @@ void RosSubscribe::rm(const std::string& signal) {
}
}
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 += "]";
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;
}
......