From fdafc54bd7cfb6c9af9403920c603532923c18fd Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Tue, 29 Oct 2019 21:02:09 +0100
Subject: [PATCH] Format
---
include/dynamic_graph_bridge/ros_init.hh | 3 +-
.../dynamic_graph_bridge/ros_interpreter.hh | 18 +-
.../dynamic_graph_bridge/sot_loader_basic.hh | 6 +-
src/converter.hh | 64 ++---
src/robot_model.cpp | 41 +--
src/ros_init.cpp | 12 +-
src/ros_interpreter.cpp | 34 +--
src/ros_publish.cpp | 20 +-
src/ros_publish.hh | 11 +-
src/ros_publish.hxx | 134 ++++-----
src/ros_queued_subscribe.cpp | 31 +-
src/ros_queued_subscribe.hh | 20 +-
src/ros_queued_subscribe.hxx | 188 ++++++------
src/ros_subscribe.cpp | 17 +-
src/ros_subscribe.hh | 14 +-
src/ros_subscribe.hxx | 272 ++++++++----------
src/ros_tf_listener.cpp | 6 +-
src/ros_tf_listener.hh | 23 +-
src/ros_time.cpp | 3 +-
src/ros_time.hh | 3 +-
src/sot_loader.cpp | 29 +-
src/sot_loader_basic.cpp | 56 ++--
src/sot_to_ros.cpp | 10 +-
src/sot_to_ros.hh | 15 +-
24 files changed, 402 insertions(+), 628 deletions(-)
diff --git a/include/dynamic_graph_bridge/ros_init.hh b/include/dynamic_graph_bridge/ros_init.hh
index a4d1dd4..fd6f855 100644
--- a/include/dynamic_graph_bridge/ros_init.hh
+++ b/include/dynamic_graph_bridge/ros_init.hh
@@ -3,8 +3,7 @@
#include <ros/ros.h>
namespace dynamicgraph {
-ros::NodeHandle& rosInit(bool createAsyncSpinner = false,
- bool createMultiThreadSpinner = true);
+ros::NodeHandle& rosInit(bool createAsyncSpinner = false, bool createMultiThreadSpinner = true);
/// \brief Return spinner or throw an exception if spinner
/// creation has been disabled at startup.
diff --git a/include/dynamic_graph_bridge/ros_interpreter.hh b/include/dynamic_graph_bridge/ros_interpreter.hh
index f4fda6c..2321d23 100644
--- a/include/dynamic_graph_bridge/ros_interpreter.hh
+++ b/include/dynamic_graph_bridge/ros_interpreter.hh
@@ -14,22 +14,19 @@ namespace dynamicgraph {
/// the outside.
class Interpreter {
public:
- typedef boost::function<bool(
- dynamic_graph_bridge_msgs::RunCommand::Request&,
- dynamic_graph_bridge_msgs::RunCommand::Response&)>
+ typedef boost::function<bool(dynamic_graph_bridge_msgs::RunCommand::Request&,
+ dynamic_graph_bridge_msgs::RunCommand::Response&)>
runCommandCallback_t;
- typedef boost::function<bool(
- dynamic_graph_bridge_msgs::RunPythonFile::Request&,
- dynamic_graph_bridge_msgs::RunPythonFile::Response&)>
+ typedef boost::function<bool(dynamic_graph_bridge_msgs::RunPythonFile::Request&,
+ dynamic_graph_bridge_msgs::RunPythonFile::Response&)>
runPythonFileCallback_t;
explicit Interpreter(ros::NodeHandle& nodeHandle);
/// \brief Method to start python interpreter and deal with messages.
/// \param Command string to execute, result, stdout, stderr strings.
- void runCommand(const std::string& command, std::string& result,
- std::string& out, std::string& err);
+ void runCommand(const std::string& command, std::string& result, std::string& out, std::string& err);
/// \brief Method to parse python scripts.
/// \param Input file name to parse.
@@ -44,9 +41,8 @@ class Interpreter {
dynamic_graph_bridge_msgs::RunCommand::Response& res);
/// \brief Run a Python file.
- bool runPythonFileCallback(
- dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
- dynamic_graph_bridge_msgs::RunPythonFile::Response& res);
+ bool runPythonFileCallback(dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
+ dynamic_graph_bridge_msgs::RunPythonFile::Response& res);
private:
python::Interpreter interpreter_;
diff --git a/include/dynamic_graph_bridge/sot_loader_basic.hh b/include/dynamic_graph_bridge/sot_loader_basic.hh
index 0fdb1da..583cb73 100644
--- a/include/dynamic_graph_bridge/sot_loader_basic.hh
+++ b/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -91,12 +91,10 @@ class SotLoaderBasic {
virtual void initializeRosNode(int argc, char* argv[]);
// \brief Callback function when starting dynamic graph.
- bool start_dg(std_srvs::Empty::Request& request,
- std_srvs::Empty::Response& response);
+ bool start_dg(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
// \brief Callback function when stopping dynamic graph.
- bool stop_dg(std_srvs::Empty::Request& request,
- std_srvs::Empty::Response& response);
+ bool stop_dg(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
// \brief Read the state vector description based upon the robot links.
int readSotVectorStateParam();
diff --git a/src/converter.hh b/src/converter.hh
index 58440a8..48add3f 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -13,15 +13,13 @@
#include <LinearMath/btMatrix3x3.h>
#include <LinearMath/btQuaternion.h>
-#define SOT_TO_ROS_IMPL(T) \
- template <> \
- inline void converter(SotToRos<T>::ros_t& dst, \
- const SotToRos<T>::sot_t& src)
+#define 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) {
@@ -104,8 +102,7 @@ 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];
}
@@ -123,8 +120,7 @@ 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.
@@ -135,8 +131,7 @@ 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);
@@ -171,8 +166,7 @@ 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
@@ -181,13 +175,11 @@ 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);
@@ -224,17 +216,15 @@ 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, ;);
@@ -264,8 +254,7 @@ 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;
}
@@ -273,8 +262,7 @@ 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);
diff --git a/src/robot_model.cpp b/src/robot_model.cpp
index 23ec297..986fb39 100644
--- a/src/robot_model.cpp
+++ b/src/robot_model.cpp
@@ -22,38 +22,31 @@ RosRobotModel::RosRobotModel(const std::string& name)
" This is the recommended method.\n"
"\n";
addCommand("loadFromParameterServer",
- command::makeCommandVoid0(
- *this, &RosRobotModel::loadFromParameterServer, docstring));
+ command::makeCommandVoid0(*this, &RosRobotModel::loadFromParameterServer, docstring));
docstring =
"\n"
" Load the robot model from an URDF file.\n"
"\n";
- addCommand("loadUrdf", command::makeCommandVoid1(
- *this, &RosRobotModel::loadUrdf, docstring));
+ addCommand("loadUrdf", command::makeCommandVoid1(*this, &RosRobotModel::loadUrdf, docstring));
docstring =
"\n"
" Set the controller namespace."
"\n";
- addCommand("setNamespace",
- command::makeCommandVoid1(*this, &RosRobotModel::setNamespace,
- docstring));
+ addCommand("setNamespace", command::makeCommandVoid1(*this, &RosRobotModel::setNamespace, docstring));
docstring =
"\n"
" Get current configuration of the robot.\n"
"\n";
- addCommand("curConf", new command::Getter<RosRobotModel, Vector>(
- *this, &RosRobotModel::curConf, docstring));
+ addCommand("curConf", new command::Getter<RosRobotModel, Vector>(*this, &RosRobotModel::curConf, docstring));
docstring =
"\n"
" Maps a link name in the URDF parser to actual robot link name.\n"
"\n";
- addCommand("addJointMapping",
- command::makeCommandVoid2(*this, &RosRobotModel::addJointMapping,
- docstring));
+ addCommand("addJointMapping", command::makeCommandVoid2(*this, &RosRobotModel::addJointMapping, docstring));
}
RosRobotModel::~RosRobotModel() {}
@@ -72,10 +65,8 @@ void RosRobotModel::loadUrdf(const std::string& filename) {
XmlRpc::XmlRpcValue JointsNamesByRank_;
JointsNamesByRank_.setSize(m_model.names.size());
std::vector<std::string>::const_iterator it =
- m_model.names.begin() +
- 2; // first joint is universe, second is freeflyer
- for (int i = 0; it != m_model.names.end(); ++it, ++i)
- JointsNamesByRank_[i] = (*it);
+ m_model.names.begin() + 2; // first joint is universe, second is freeflyer
+ for (int i = 0; it != m_model.names.end(); ++it, ++i) JointsNamesByRank_[i] = (*it);
nh.setParam(jointsParameterName_, JointsNamesByRank_);
}
@@ -85,15 +76,12 @@ void RosRobotModel::loadFromParameterServer() {
rosInit(false);
std::string robotDescription;
ros::param::param<std::string>("/robot_description", robotDescription, "");
- if (robotDescription.empty())
- throw std::runtime_error("No model available as ROS parameter. Fail.");
+ if (robotDescription.empty()) throw std::runtime_error("No model available as ROS parameter. Fail.");
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDF(robotDescription);
if (urdfTree)
- se3::urdf::parseTree(urdfTree->getRoot(), this->m_model,
- se3::SE3::Identity(), false);
+ se3::urdf::parseTree(urdfTree->getRoot(), this->m_model, se3::SE3::Identity(), false);
else {
- const std::string exception_message(
- "robot_description not parsed correctly.");
+ const std::string exception_message("robot_description not parsed correctly.");
throw std::invalid_argument(exception_message);
}
@@ -107,8 +95,7 @@ void RosRobotModel::loadFromParameterServer() {
JointsNamesByRank_.setSize(m_model.names.size());
// first joint is universe, second is freeflyer
std::vector<std::string>::const_iterator it = m_model.names.begin() + 2;
- for (int i = 0; it != m_model.names.end(); ++it, ++i)
- JointsNamesByRank_[i] = (*it);
+ for (int i = 0; it != m_model.names.end(); ++it, ++i) JointsNamesByRank_[i] = (*it);
nh.setParam(jointsParameterName_, JointsNamesByRank_);
}
@@ -137,15 +124,13 @@ Vector RosRobotModel::curConf() const {
else {
// TODO: confirm accesscopy is for asynchronous commands
Vector currConf = jointPositionSIN.accessCopy();
- for (int32_t i = 0; i < ffpose.size(); ++i)
- currConf(i) = static_cast<double>(ffpose[i]);
+ for (int32_t i = 0; i < ffpose.size(); ++i) currConf(i) = static_cast<double>(ffpose[i]);
return currConf;
}
}
-void RosRobotModel::addJointMapping(const std::string& link,
- const std::string& repName) {
+void RosRobotModel::addJointMapping(const std::string& link, const std::string& repName) {
specialJoints_[link] = repName;
}
diff --git a/src/ros_init.cpp b/src/ros_init.cpp
index bad1612..56b868a 100644
--- a/src/ros_init.cpp
+++ b/src/ros_init.cpp
@@ -17,8 +17,7 @@ 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");
@@ -35,14 +34,11 @@ ros::NodeHandle& rosInit(bool createAsyncSpinner,
// 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);
diff --git a/src/ros_interpreter.cpp b/src/ros_interpreter.cpp
index 1b38851..fa6549c 100644
--- a/src/ros_interpreter.cpp
+++ b/src/ros_interpreter.cpp
@@ -4,46 +4,34 @@ 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);
}
-void Interpreter::runPythonFile(std::string ifilename) {
- interpreter_.runPythonFile(ifilename);
-}
+void Interpreter::runPythonFile(std::string ifilename) { interpreter_.runPythonFile(ifilename); }
} // end of namespace dynamicgraph.
diff --git a/src/ros_publish.cpp b/src/ros_publish.cpp
index cbb1dd8..dd5a134 100644
--- a/src/ros_publish.cpp
+++ b/src/ros_publish.cpp
@@ -47,10 +47,7 @@ Value List::doExecute() {
}
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());
@@ -128,8 +125,7 @@ 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);
@@ -174,16 +170,13 @@ 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;
}
@@ -196,9 +189,8 @@ void RosPublish::rm(const std::string& 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++) {
+ for (std::map<std::string, bindedSignal_t>::const_iterator it = bindedSignal_.begin(); it != bindedSignal_.end();
+ it++) {
result += "'" + it->first + "',";
}
result += "]";
diff --git a/src/ros_publish.hh b/src/ros_publish.hh
index 5898072..9b73716 100644
--- a/src/ros_publish.hh
+++ b/src/ros_publish.hh
@@ -50,9 +50,7 @@ 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;
@@ -70,11 +68,8 @@ 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);
diff --git a/src/ros_publish.hxx b/src/ros_publish.hxx
index 97966c8..026b859 100644
--- a/src/ros_publish.hxx
+++ b/src/ros_publish.hxx
@@ -1,93 +1,63 @@
#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HXX
-# define DYNAMIC_GRAPH_ROS_PUBLISH_HXX
-# include <vector>
-# include <std_msgs/Float64.h>
-
-# include "dynamic_graph_bridge_msgs/Matrix.h"
-# include "dynamic_graph_bridge_msgs/Vector.h"
-
-# include "sot_to_ros.hh"
-
-namespace dynamicgraph
-{
- template <>
- inline void
- RosPublish::sendData
- <std::pair<sot::MatrixHomogeneous, Vector> >
- (boost::shared_ptr
- <realtime_tools::RealtimePublisher
- <SotToRos
- <std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> > publisher,
- boost::shared_ptr
- <SotToRos
- <std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t> signal,
- int time)
- {
- SotToRos
- <std::pair
- <sot::MatrixHomogeneous, Vector> >::ros_t result;
- if (publisher->trylock ())
- {
- publisher->msg_.child_frame_id = "/dynamic_graph/world";
- converter (publisher->msg_, signal->access (time));
- publisher->unlockAndPublish ();
- }
+#define DYNAMIC_GRAPH_ROS_PUBLISH_HXX
+#include <vector>
+#include <std_msgs/Float64.h>
+
+#include "dynamic_graph_bridge_msgs/Matrix.h"
+#include "dynamic_graph_bridge_msgs/Vector.h"
+
+#include "sot_to_ros.hh"
+
+namespace dynamicgraph {
+template <>
+inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >(
+ boost::shared_ptr<realtime_tools::RealtimePublisher<SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> >
+ publisher,
+ boost::shared_ptr<SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t> signal, int time) {
+ SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t result;
+ if (publisher->trylock()) {
+ publisher->msg_.child_frame_id = "/dynamic_graph/world";
+ converter(publisher->msg_, signal->access(time));
+ publisher->unlockAndPublish();
}
-
- template <typename T>
- void
- RosPublish::sendData
- (boost::shared_ptr
- <realtime_tools::RealtimePublisher
- <typename SotToRos<T>::ros_t> > publisher,
- boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal,
- int time)
- {
- typename SotToRos<T>::ros_t result;
- if (publisher->trylock ())
- {
- converter (publisher->msg_, signal->access (time));
- publisher->unlockAndPublish ();
- }
+}
+
+template <typename T>
+void RosPublish::sendData(boost::shared_ptr<realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> > publisher,
+ boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time) {
+ typename SotToRos<T>::ros_t result;
+ if (publisher->trylock()) {
+ converter(publisher->msg_, signal->access(time));
+ publisher->unlockAndPublish();
}
+}
- template <typename T>
- void RosPublish::add (const std::string& signal, const std::string& topic)
- {
- typedef typename SotToRos<T>::ros_t ros_t;
- typedef typename SotToRos<T>::signalIn_t signal_t;
+template <typename T>
+void RosPublish::add(const std::string& signal, const std::string& topic) {
+ typedef typename SotToRos<T>::ros_t ros_t;
+ typedef typename SotToRos<T>::signalIn_t signal_t;
- // Initialize the bindedSignal object.
- bindedSignal_t bindedSignal;
+ // Initialize the bindedSignal object.
+ bindedSignal_t bindedSignal;
- // Initialize the publisher.
- boost::shared_ptr
- <realtime_tools::RealtimePublisher<ros_t> >
- pubPtr =
- boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >
- (nh_, topic, 1);
+ // Initialize the publisher.
+ boost::shared_ptr<realtime_tools::RealtimePublisher<ros_t> > pubPtr =
+ boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >(nh_, topic, 1);
- // Initialize the signal.
- boost::shared_ptr<signal_t> signalPtr
- (new signal_t
- (0,
- MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
- boost::get<0> (bindedSignal) = signalPtr;
- SotToRos<T>::setDefault(*signalPtr);
- signalRegistration (*boost::get<0> (bindedSignal));
+ // Initialize the signal.
+ boost::shared_ptr<signal_t> signalPtr(
+ new signal_t(0, MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
+ boost::get<0>(bindedSignal) = signalPtr;
+ SotToRos<T>::setDefault(*signalPtr);
+ signalRegistration(*boost::get<0>(bindedSignal));
- // Initialize the callback.
- callback_t callback = boost::bind
- (&RosPublish::sendData<T>,
- this,
- pubPtr,
- signalPtr,
- _1);
- boost::get<1> (bindedSignal) = callback;
+ // Initialize the callback.
+ callback_t callback = boost::bind(&RosPublish::sendData<T>, this, pubPtr, signalPtr, _1);
+ boost::get<1>(bindedSignal) = callback;
- bindedSignal_[signal] = bindedSignal;
- }
+ bindedSignal_[signal] = bindedSignal;
+}
-} // end of namespace dynamicgraph.
+} // end of namespace dynamicgraph.
-#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HXX
+#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HXX
diff --git a/src/ros_queued_subscribe.cpp b/src/ros_queued_subscribe.cpp
index efcf9c5..66c29b8 100644
--- a/src/ros_queued_subscribe.cpp
+++ b/src/ros_queued_subscribe.cpp
@@ -56,10 +56,7 @@ Value List::doExecute() {
}
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());
@@ -132,10 +129,7 @@ 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"
@@ -177,8 +171,7 @@ RosQueuedSubscribe::RosQueuedSubscribe(const std::string& 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));
+ addCommand("clearQueue", new command::rosQueuedSubscribe::ClearQueue(*this, docstring));
docstring =
"\n"
" Return the queue size of a given signal\n"
@@ -186,8 +179,7 @@ RosQueuedSubscribe::RosQueuedSubscribe(const std::string& 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));
+ addCommand("queueSize", new command::rosQueuedSubscribe::QueueSize(*this, docstring));
docstring =
"\n"
" Whether signals should read values from the queues, and when.\n"
@@ -195,15 +187,12 @@ RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n)
" Input is:\n"
" - int (dynamic graph time at which the reading begin).\n"
"\n";
- addCommand("readQueue",
- new command::rosQueuedSubscribe::ReadQueue(*this, docstring));
+ 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";
@@ -219,9 +208,8 @@ 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++) {
+ for (std::map<std::string, bindedSignal_t>::const_iterator it = bindedSignal_.begin(); it != bindedSignal_.end();
+ it++) {
result += "'" + it->first + "',";
}
result += "]";
@@ -243,8 +231,7 @@ 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();
}
diff --git a/src/ros_queued_subscribe.hh b/src/ros_queued_subscribe.hh
index 8fe46fe..72c36a3 100644
--- a/src/ros_queued_subscribe.hh
+++ b/src/ros_queued_subscribe.hh
@@ -77,11 +77,7 @@ 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();
@@ -153,23 +149,17 @@ 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_; }
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;
diff --git a/src/ros_queued_subscribe.hxx b/src/ros_queued_subscribe.hxx
index f9ac740..8ae034e 100644
--- a/src/ros_queued_subscribe.hxx
+++ b/src/ros_queued_subscribe.hxx
@@ -5,120 +5,108 @@
//
#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
-# define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
-# define ENABLE_RT_LOG
+#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
+#define ENABLE_RT_LOG
-# include <vector>
-# include <boost/bind.hpp>
-# include <boost/date_time/posix_time/posix_time.hpp>
-# include <dynamic-graph/real-time-logger.h>
-# include <dynamic-graph/signal-caster.h>
-# include <dynamic-graph/linear-algebra.h>
-# include <dynamic-graph/signal-cast-helper.h>
-# include <std_msgs/Float64.h>
-# include "dynamic_graph_bridge_msgs/Matrix.h"
-# include "dynamic_graph_bridge_msgs/Vector.h"
+#include <vector>
+#include <boost/bind.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <dynamic-graph/real-time-logger.h>
+#include <dynamic-graph/signal-caster.h>
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/signal-cast-helper.h>
+#include <std_msgs/Float64.h>
+#include "dynamic_graph_bridge_msgs/Matrix.h"
+#include "dynamic_graph_bridge_msgs/Vector.h"
namespace dg = dynamicgraph;
typedef boost::mutex::scoped_lock scoped_lock;
-namespace dynamicgraph
-{
- namespace internal
- {
- static const int BUFFER_SIZE = 1 << 10;
+namespace dynamicgraph {
+namespace internal {
+static const int BUFFER_SIZE = 1 << 10;
- template <typename T>
- struct Add
- {
- void operator () (RosQueuedSubscribe& rosSubscribe,
- const std::string& type,
- const std::string& signal,
- const std::string& topic)
- {
- typedef typename SotToRos<T>::sot_t sot_t;
- typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
- typedef BindedSignal<sot_t, BUFFER_SIZE> BindedSignal_t;
- typedef typename BindedSignal_t::Signal_t Signal_t;
+template <typename T>
+struct Add {
+ void operator()(RosQueuedSubscribe& rosSubscribe, const std::string& type, const std::string& signal,
+ const std::string& topic) {
+ typedef typename SotToRos<T>::sot_t sot_t;
+ typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
+ typedef BindedSignal<sot_t, BUFFER_SIZE> BindedSignal_t;
+ typedef typename BindedSignal_t::Signal_t Signal_t;
- // Initialize the bindedSignal object.
- BindedSignal_t* bs = new BindedSignal_t(&rosSubscribe);
- SotToRos<T>::setDefault (bs->last);
+ // Initialize the bindedSignal object.
+ BindedSignal_t* bs = new BindedSignal_t(&rosSubscribe);
+ SotToRos<T>::setDefault(bs->last);
- // Initialize the signal.
- boost::format signalName ("RosQueuedSubscribe(%1%)::output(%2%)::%3%");
- signalName % rosSubscribe.getName () % type % signal;
+ // Initialize the signal.
+ boost::format signalName("RosQueuedSubscribe(%1%)::output(%2%)::%3%");
+ signalName % rosSubscribe.getName() % type % signal;
- bs->signal.reset (new Signal_t (signalName.str ()));
- bs->signal->setFunction (boost::bind(&BindedSignal_t::reader, bs, _1, _2));
- rosSubscribe.signalRegistration (*bs->signal);
+ bs->signal.reset(new Signal_t(signalName.str()));
+ bs->signal->setFunction(boost::bind(&BindedSignal_t::reader, bs, _1, _2));
+ rosSubscribe.signalRegistration(*bs->signal);
- // Initialize the subscriber.
- typedef boost::function<void (const ros_const_ptr_t& data)> callback_t;
- callback_t callback = boost::bind
- (&BindedSignal_t::template writer<ros_const_ptr_t>, bs, _1);
+ // Initialize the subscriber.
+ typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
+ callback_t callback = boost::bind(&BindedSignal_t::template writer<ros_const_ptr_t>, bs, _1);
- // Keep 50 messages in queue, but only 20 are sent every 100ms
- // -> No message should be lost because of a full buffer
- bs->subscriber =
- boost::make_shared<ros::Subscriber>
- (rosSubscribe.nh ().subscribe (topic, BUFFER_SIZE, callback));
+ // Keep 50 messages in queue, but only 20 are sent every 100ms
+ // -> No message should be lost because of a full buffer
+ bs->subscriber = boost::make_shared<ros::Subscriber>(rosSubscribe.nh().subscribe(topic, BUFFER_SIZE, callback));
- RosQueuedSubscribe::bindedSignal_t bindedSignal (bs);
- rosSubscribe.bindedSignal ()[signal] = bindedSignal;
- }
- };
+ RosQueuedSubscribe::bindedSignal_t bindedSignal(bs);
+ rosSubscribe.bindedSignal()[signal] = bindedSignal;
+ }
+};
- // template <typename T, typename R>
- template <typename T, int N>
- template <typename R>
- void BindedSignal<T, N>::writer (const R& data)
- {
- // synchronize with method clear
- boost::mutex::scoped_lock lock(wmutex);
- if (full()) {
- rmutex.lock();
- frontIdx = (frontIdx + 1) % N;
- rmutex.unlock();
- }
- converter (buffer[backIdx], data);
- // No need to synchronize with reader here because:
- // - if the buffer was not empty, then it stays not empty,
- // - if it was empty, then the current value will be used at next time. It
- // means the transmission bandwidth is too low.
- if (!init) {
- last = buffer[backIdx];
- init = true;
- }
- backIdx = (backIdx+1) % N;
- }
+// template <typename T, typename R>
+template <typename T, int N>
+template <typename R>
+void BindedSignal<T, N>::writer(const R& data) {
+ // synchronize with method clear
+ boost::mutex::scoped_lock lock(wmutex);
+ if (full()) {
+ rmutex.lock();
+ frontIdx = (frontIdx + 1) % N;
+ rmutex.unlock();
+ }
+ converter(buffer[backIdx], data);
+ // No need to synchronize with reader here because:
+ // - if the buffer was not empty, then it stays not empty,
+ // - if it was empty, then the current value will be used at next time. It
+ // means the transmission bandwidth is too low.
+ if (!init) {
+ last = buffer[backIdx];
+ init = true;
+ }
+ backIdx = (backIdx + 1) % N;
+}
- template <typename T, int N>
- T& BindedSignal<T, N>::reader (T& data, int time)
- {
- // synchronize with method clear:
- // If reading from the list cannot be done, then return last value.
- scoped_lock lock(rmutex, boost::try_to_lock);
- if (!lock.owns_lock() || entity->readQueue_ == -1 || time < entity->readQueue_) {
- data = last;
- } else {
- if (empty())
- data = last;
- else {
- data = buffer[frontIdx];
- frontIdx = (frontIdx + 1) % N;
- last = data;
- }
- }
- return data;
+template <typename T, int N>
+T& BindedSignal<T, N>::reader(T& data, int time) {
+ // synchronize with method clear:
+ // If reading from the list cannot be done, then return last value.
+ scoped_lock lock(rmutex, boost::try_to_lock);
+ if (!lock.owns_lock() || entity->readQueue_ == -1 || time < entity->readQueue_) {
+ data = last;
+ } else {
+ if (empty())
+ data = last;
+ else {
+ data = buffer[frontIdx];
+ frontIdx = (frontIdx + 1) % N;
+ last = data;
}
- } // end of namespace internal.
-
- template <typename T>
- void RosQueuedSubscribe::add (const std::string& type, const std::string& signal, const std::string& topic)
- {
- internal::Add<T> () (*this, type, signal, topic);
}
-} // end of namespace dynamicgraph.
+ return data;
+}
+} // end of namespace internal.
+
+template <typename T>
+void RosQueuedSubscribe::add(const std::string& type, const std::string& signal, const std::string& topic) {
+ internal::Add<T>()(*this, type, signal, topic);
+}
+} // end of namespace dynamicgraph.
-#endif //! DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
+#endif //! DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
diff --git a/src/ros_subscribe.cpp b/src/ros_subscribe.cpp
index 113ec2d..f046af6 100644
--- a/src/ros_subscribe.cpp
+++ b/src/ros_subscribe.cpp
@@ -37,10 +37,7 @@ Value List::doExecute() {
}
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());
@@ -95,8 +92,7 @@ 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"
@@ -137,9 +133,7 @@ RosSubscribe::RosSubscribe(const std::string& n)
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";
@@ -155,9 +149,8 @@ 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++) {
+ for (std::map<std::string, bindedSignal_t>::const_iterator it = bindedSignal_.begin(); it != bindedSignal_.end();
+ it++) {
result += "'" + it->first + "',";
}
result += "]";
diff --git a/src/ros_subscribe.hh b/src/ros_subscribe.hh
index 35b9e59..c5fd7db 100644
--- a/src/ros_subscribe.hh
+++ b/src/ros_subscribe.hh
@@ -51,8 +51,7 @@ 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);
@@ -69,20 +68,15 @@ 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;
diff --git a/src/ros_subscribe.hxx b/src/ros_subscribe.hxx
index ded5de1..b3467d4 100644
--- a/src/ros_subscribe.hxx
+++ b/src/ros_subscribe.hxx
@@ -1,163 +1,127 @@
#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 <std_msgs/Float64.h>
-# include "dynamic_graph_bridge_msgs/Matrix.h"
-# include "dynamic_graph_bridge_msgs/Vector.h"
-# include "ros_time.hh"
+#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 <std_msgs/Float64.h>
+#include "dynamic_graph_bridge_msgs/Matrix.h"
+#include "dynamic_graph_bridge_msgs/Vector.h"
+#include "ros_time.hh"
namespace dg = dynamicgraph;
-namespace dynamicgraph
-{
- template <typename R, typename S>
- void
- RosSubscribe::callback
- (boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
- const R& data)
- {
- typedef S sot_t;
- sot_t value;
- converter (value, data);
- signal->setConstant (value);
+namespace dynamicgraph {
+template <typename R, typename S>
+void RosSubscribe::callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data) {
+ typedef S sot_t;
+ sot_t value;
+ converter(value, data);
+ signal->setConstant(value);
+}
+
+template <typename R>
+void RosSubscribe::callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data) {
+ ptime time = rosTimeToPtime(data->header.stamp);
+ signal->setConstant(time);
+}
+
+namespace internal {
+template <typename T>
+struct Add {
+ 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;
+
+ // Initialize the bindedSignal object.
+ RosSubscribe::bindedSignal_t bindedSignal;
+
+ // Initialize the signal.
+ boost::format signalName("RosSubscribe(%1%)::%2%");
+ signalName % RosSubscribe.getName() % signal;
+
+ boost::shared_ptr<signal_t> signal_(new signal_t(0, signalName.str()));
+ SotToRos<T>::setDefault(*signal_);
+ bindedSignal.first = signal_;
+ RosSubscribe.signalRegistration(*bindedSignal.first);
+
+ // 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);
+
+ bindedSignal.second = boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callback));
+
+ RosSubscribe.bindedSignal()[signal] = bindedSignal;
}
+};
- template <typename R>
- void
- RosSubscribe::callbackTimestamp
- (boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
- const R& data)
- {
- ptime time =
- rosTimeToPtime (data->header.stamp);
- signal->setConstant(time);
- }
+template <typename T>
+struct Add<std::pair<T, dg::Vector> > {
+ 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;
+ typedef typename SotToRos<type_t>::ros_const_ptr_t ros_const_ptr_t;
+ typedef typename SotToRos<type_t>::signalIn_t signal_t;
+
+ // Initialize the bindedSignal object.
+ RosSubscribe::bindedSignal_t bindedSignal;
+
+ // Initialize the signal.
+ boost::format signalName("RosSubscribe(%1%)::%2%");
+ signalName % RosSubscribe.getName() % signal;
+
+ boost::shared_ptr<signal_t> signal_(new signal_t(0, signalName.str()));
+ SotToRos<T>::setDefault(*signal_);
+ bindedSignal.first = signal_;
+ RosSubscribe.signalRegistration(*bindedSignal.first);
+
+ // 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);
+
+ bindedSignal.second = boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callback));
+
+ RosSubscribe.bindedSignal()[signal] = bindedSignal;
- namespace internal
- {
- template <typename T>
- struct Add
- {
- 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;
-
- // Initialize the bindedSignal object.
- RosSubscribe::bindedSignal_t bindedSignal;
-
- // Initialize the signal.
- boost::format signalName ("RosSubscribe(%1%)::%2%");
- signalName % RosSubscribe.getName () % signal;
-
- boost::shared_ptr<signal_t> signal_
- (new signal_t (0, signalName.str ()));
- SotToRos<T>::setDefault(*signal_);
- bindedSignal.first = signal_;
- RosSubscribe.signalRegistration (*bindedSignal.first);
-
- // 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);
-
- bindedSignal.second =
- boost::make_shared<ros::Subscriber>
- (RosSubscribe.nh ().subscribe (topic, 1, callback));
-
- RosSubscribe.bindedSignal ()[signal] = bindedSignal;
- }
- };
-
- template <typename T>
- struct Add<std::pair<T, dg::Vector> >
- {
- 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;
- typedef typename SotToRos<type_t>::ros_const_ptr_t ros_const_ptr_t;
- typedef typename SotToRos<type_t>::signalIn_t signal_t;
-
- // Initialize the bindedSignal object.
- RosSubscribe::bindedSignal_t bindedSignal;
-
- // Initialize the signal.
- boost::format signalName ("RosSubscribe(%1%)::%2%");
- signalName % RosSubscribe.getName () % signal;
-
- boost::shared_ptr<signal_t> signal_
- (new signal_t (0, signalName.str ()));
- SotToRos<T>::setDefault(*signal_);
- bindedSignal.first = signal_;
- RosSubscribe.signalRegistration (*bindedSignal.first);
-
- // 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);
-
- 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 ();
-
- // Initialize the bindedSignal object.
- RosSubscribe::bindedSignal_t bindedSignalTimestamp;
-
- // Initialize the signal.
- boost::format signalNameTimestamp ("RosSubscribe(%1%)::%2%");
- signalNameTimestamp % RosSubscribe.name % signalTimestamp;
-
- boost::shared_ptr<signalTimestamp_t> signalTimestamp_
- (new signalTimestamp_t (0, signalNameTimestamp.str ()));
-
- RosSubscribe::ptime zero (rosTimeToPtime (ros::Time (0, 0)));
- signalTimestamp_->setConstant (zero);
- bindedSignalTimestamp.first = signalTimestamp_;
- RosSubscribe.signalRegistration (*bindedSignalTimestamp.first);
-
- // 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);
-
- bindedSignalTimestamp.second =
- boost::make_shared<ros::Subscriber>
- (RosSubscribe.nh ().subscribe (topic, 1, callbackTimestamp));
-
- RosSubscribe.bindedSignal ()[signalTimestamp] = bindedSignalTimestamp;
- }
- };
- } // end of namespace internal.
-
- template <typename T>
- void RosSubscribe::add (const std::string& signal, const std::string& topic)
- {
- internal::Add<T> () (*this, signal, topic);
+ // Timestamp.
+ typedef dynamicgraph::SignalPtr<RosSubscribe::ptime, int> signalTimestamp_t;
+ std::string signalTimestamp = (boost::format("%1%%2%") % signal % "Timestamp").str();
+
+ // Initialize the bindedSignal object.
+ RosSubscribe::bindedSignal_t bindedSignalTimestamp;
+
+ // Initialize the signal.
+ boost::format signalNameTimestamp("RosSubscribe(%1%)::%2%");
+ signalNameTimestamp % RosSubscribe.name % signalTimestamp;
+
+ boost::shared_ptr<signalTimestamp_t> signalTimestamp_(new signalTimestamp_t(0, signalNameTimestamp.str()));
+
+ RosSubscribe::ptime zero(rosTimeToPtime(ros::Time(0, 0)));
+ signalTimestamp_->setConstant(zero);
+ bindedSignalTimestamp.first = signalTimestamp_;
+ RosSubscribe.signalRegistration(*bindedSignalTimestamp.first);
+
+ // 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);
+
+ bindedSignalTimestamp.second =
+ boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp));
+
+ RosSubscribe.bindedSignal()[signalTimestamp] = bindedSignalTimestamp;
}
-} // end of namespace dynamicgraph.
+};
+} // end of namespace internal.
+
+template <typename T>
+void RosSubscribe::add(const std::string& signal, const std::string& topic) {
+ internal::Add<T>()(*this, signal, topic);
+}
+} // end of namespace dynamicgraph.
-#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
+#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
diff --git a/src/ros_tf_listener.cpp b/src/ros_tf_listener.cpp
index da89ca3..2d04a53 100644
--- a/src/ros_tf_listener.cpp
+++ b/src/ros_tf_listener.cpp
@@ -5,8 +5,7 @@
namespace dynamicgraph {
namespace internal {
-sot::MatrixHomogeneous& TransformListenerData::getTransform(
- sot::MatrixHomogeneous& res, int time) {
+sot::MatrixHomogeneous& TransformListenerData::getTransform(sot::MatrixHomogeneous& res, int time) {
static const ros::Time rosTime(0);
try {
listener.lookupTransform(toFrame, fromFrame, rosTime, transform);
@@ -18,8 +17,7 @@ sot::MatrixHomogeneous& TransformListenerData::getTransform(
return res;
}
for (int r = 0; r < 3; ++r) {
- for (int c = 0; c < 3; ++c)
- res.linear()(r, c) = transform.getBasis().getRow(r)[c];
+ for (int c = 0; c < 3; ++c) res.linear()(r, c) = transform.getBasis().getRow(r)[c];
res.translation()[r] = transform.getOrigin()[r];
}
return res;
diff --git a/src/ros_tf_listener.hh b/src/ros_tf_listener.hh
index 6f0b457..c40c2f0 100644
--- a/src/ros_tf_listener.hh
+++ b/src/ros_tf_listener.hh
@@ -24,12 +24,10 @@ struct TransformListenerData {
tf::StampedTransform transform;
signal_t signal;
- TransformListenerData(RosTfListener* e, tf::TransformListener& l,
- const std::string& to, const std::string& from,
+ TransformListenerData(RosTfListener* e, tf::TransformListener& l, const std::string& to, const std::string& from,
const std::string& signame)
: entity(e), listener(l), toFrame(to), fromFrame(from), signal(signame) {
- signal.setFunction(
- boost::bind(&TransformListenerData::getTransform, this, _1, _2));
+ signal.setFunction(boost::bind(&TransformListenerData::getTransform, this, _1, _2));
}
sot::MatrixHomogeneous& getTransform(sot::MatrixHomogeneous& res, int time);
@@ -52,28 +50,21 @@ class RosTfListener : public Entity {
" - from: frame name,\n"
" - signalName: the signal name in dynamic-graph"
"\n";
- addCommand("add", command::makeCommandVoid3(*this, &RosTfListener::add,
- docstring));
+ addCommand("add", command::makeCommandVoid3(*this, &RosTfListener::add, docstring));
}
~RosTfListener() {
- for (Map_t::const_iterator _it = listenerDatas.begin();
- _it != listenerDatas.end(); ++_it)
- delete _it->second;
+ for (Map_t::const_iterator _it = listenerDatas.begin(); _it != listenerDatas.end(); ++_it) delete _it->second;
}
- void add(const std::string& to, const std::string& from,
- const std::string& signame) {
+ void add(const std::string& to, const std::string& from, const std::string& signame) {
if (listenerDatas.find(signame) != listenerDatas.end())
- throw std::invalid_argument("A signal " + signame +
- " already exists in RosTfListener " +
- getName());
+ throw std::invalid_argument("A signal " + signame + " already exists in RosTfListener " + getName());
boost::format signalName("RosTfListener(%1%)::output(MatrixHomo)::%2%");
signalName % getName() % signame;
- TransformListenerData* tld =
- new TransformListenerData(this, listener, to, from, signalName.str());
+ TransformListenerData* tld = new TransformListenerData(this, listener, to, from, signalName.str());
signalRegistration(tld->signal);
listenerDatas[signame] = tld;
}
diff --git a/src/ros_time.cpp b/src/ros_time.cpp
index 278f7be..52c8f76 100644
--- a/src/ros_time.cpp
+++ b/src/ros_time.cpp
@@ -23,8 +23,7 @@ const std::string RosTime::docstring_(
" boost::posix_time::ptime type.\n");
RosTime::RosTime(const std::string& name)
- : Entity(name),
- now_("RosTime(" + name + ")::output(boost::posix_time::ptime)::time") {
+ : Entity(name), now_("RosTime(" + name + ")::output(boost::posix_time::ptime)::time") {
signalRegistration(now_);
now_.setConstant(rosTimeToPtime(ros::Time::now()));
now_.setFunction(boost::bind(&RosTime::update, this, _1, _2));
diff --git a/src/ros_time.hh b/src/ros_time.hh
index 5875985..19c503d 100644
--- a/src/ros_time.hh
+++ b/src/ros_time.hh
@@ -22,8 +22,7 @@ class RosTime : public dynamicgraph::Entity {
virtual std::string getDocString() const;
protected:
- boost::posix_time::ptime& update(boost::posix_time::ptime& time,
- const int& t);
+ boost::posix_time::ptime& update(boost::posix_time::ptime& time, const int& t);
private:
static const std::string docstring_;
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 8a05e3d..3ba0bff 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -76,8 +76,7 @@ void workThreadLoader(SotLoader *aSotLoader) {
aSotLoader->oneIteration();
gettimeofday(&stop, 0);
- dt = 1000000 * (stop.tv_sec - start.tv_sec) +
- (stop.tv_usec - start.tv_usec);
+ dt = 1000000 * (stop.tv_sec - start.tv_sec) + (stop.tv_usec - start.tv_usec);
dataToLog.record((double)dt * 1e-6);
} else
dt = 0;
@@ -110,9 +109,7 @@ SotLoader::~SotLoader() {
thread_.join();
}
-void SotLoader::startControlLoop() {
- thread_ = boost::thread(workThreadLoader, this);
-}
+void SotLoader::startControlLoop() { thread_ = boost::thread(workThreadLoader, this); }
void SotLoader::initializeRosNode(int argc, char *argv[]) {
SotLoaderBasic::initializeRosNode(argc, argv);
@@ -130,8 +127,7 @@ void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
assert(angleControl_.size() == angleEncoder_.size());
sensorsIn["joints"].setName("angle");
- for (unsigned int i = 0; i < angleControl_.size(); i++)
- angleEncoder_[i] = angleControl_[i];
+ for (unsigned int i = 0; i < angleControl_.size(); i++) angleEncoder_[i] = angleControl_[i];
sensorsIn["joints"].setValues(angleEncoder_);
}
@@ -140,8 +136,7 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
angleControl_ = controlValues["control"].getValues();
// Debug
- std::map<std::string, dgs::ControlValues>::iterator it =
- controlValues.begin();
+ std::map<std::string, dgs::ControlValues>::iterator it = controlValues.begin();
sotDEBUG(30) << "ControlValues to be broadcasted:" << std::endl;
for (; it != controlValues.end(); it++) {
sotDEBUG(30) << it->first << ":";
@@ -154,8 +149,8 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
// Check if the size if coherent with the robot description.
if (angleControl_.size() != (unsigned int)nbOfJoints_) {
- std::cerr << " angleControl_" << angleControl_.size() << " and nbOfJoints"
- << (unsigned int)nbOfJoints_ << " are different !" << std::endl;
+ std::cerr << " angleControl_" << angleControl_.size() << " and nbOfJoints" << (unsigned int)nbOfJoints_
+ << " are different !" << std::endl;
exit(-1);
}
// Publish the data.
@@ -165,8 +160,7 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
}
for (unsigned int i = 0; i < parallel_joints_to_state_vector_.size(); i++) {
joint_state_.position[i + nbOfJoints_] =
- coefficient_parallel_joints_[i] *
- angleControl_[parallel_joints_to_state_vector_[i]];
+ coefficient_parallel_joints_[i] * angleControl_[parallel_joints_to_state_vector_[i]];
}
joint_pub_.publish(joint_state_);
@@ -175,14 +169,11 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
// get the robot pose values
std::vector<double> poseValue_ = controlValues["baseff"].getValues();
- freeFlyerPose_.setOrigin(
- tf::Vector3(poseValue_[0], poseValue_[1], poseValue_[2]));
- tf::Quaternion poseQ_(poseValue_[4], poseValue_[5], poseValue_[6],
- poseValue_[3]);
+ freeFlyerPose_.setOrigin(tf::Vector3(poseValue_[0], poseValue_[1], poseValue_[2]));
+ tf::Quaternion poseQ_(poseValue_[4], poseValue_[5], poseValue_[6], poseValue_[3]);
freeFlyerPose_.setRotation(poseQ_);
// Publish
- freeFlyerPublisher_.sendTransform(tf::StampedTransform(
- freeFlyerPose_, ros::Time::now(), "odom", "base_link"));
+ freeFlyerPublisher_.sendTransform(tf::StampedTransform(freeFlyerPose_, ros::Time::now(), "odom", "base_link"));
}
void SotLoader::setup() {
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index cfede40..1e3258d 100644
--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -26,8 +26,7 @@ using namespace std;
using namespace dynamicgraph::sot;
namespace po = boost::program_options;
-SotLoaderBasic::SotLoaderBasic()
- : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
+SotLoaderBasic::SotLoaderBasic() : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
readSotVectorStateParam();
initPublication();
}
@@ -44,16 +43,12 @@ int SotLoaderBasic::initPublication() {
void SotLoaderBasic::initializeRosNode(int, char* []) {
ROS_INFO("Ready to start dynamic graph.");
ros::NodeHandle n;
- service_start_ = n.advertiseService("start_dynamic_graph",
- &SotLoaderBasic::start_dg, this);
+ service_start_ = n.advertiseService("start_dynamic_graph", &SotLoaderBasic::start_dg, this);
- service_stop_ =
- n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);
+ service_stop_ = n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);
}
-void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) {
- dynamicLibraryName_ = afilename;
-}
+void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) { dynamicLibraryName_ = afilename; }
int SotLoaderBasic::readSotVectorStateParam() {
std::map<std::string, int> from_state_name_to_state_vector;
@@ -73,16 +68,12 @@ int SotLoaderBasic::readSotVectorStateParam() {
if (ros::param::has("/sot/joint_state_parallel")) {
XmlRpc::XmlRpcValue joint_state_parallel;
n.getParam("/sot/joint_state_parallel", joint_state_parallel);
- ROS_ASSERT(joint_state_parallel.getType() ==
- XmlRpc::XmlRpcValue::TypeStruct);
- std::cout << "Type of joint_state_parallel:"
- << joint_state_parallel.getType() << std::endl;
+ ROS_ASSERT(joint_state_parallel.getType() == XmlRpc::XmlRpcValue::TypeStruct);
+ std::cout << "Type of joint_state_parallel:" << joint_state_parallel.getType() << std::endl;
- for (XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin();
- it != joint_state_parallel.end(); it++) {
+ for (XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin(); it != joint_state_parallel.end(); it++) {
XmlRpc::XmlRpcValue local_value = it->second;
- std::string final_expression,
- map_expression = static_cast<string>(local_value);
+ std::string final_expression, map_expression = static_cast<string>(local_value);
double final_coefficient = 1.0;
// deal with sign
if (map_expression[0] == '-') {
@@ -92,8 +83,7 @@ int SotLoaderBasic::readSotVectorStateParam() {
final_expression = map_expression;
std::cout << it->first.c_str() << ": " << final_coefficient << std::endl;
- from_parallel_name_to_state_vector_name[it->first.c_str()] =
- final_expression;
+ from_parallel_name_to_state_vector_name[it->first.c_str()] = final_expression;
coefficient_parallel_joints_.push_back(final_coefficient);
}
nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
@@ -115,12 +105,10 @@ int SotLoaderBasic::readSotVectorStateParam() {
// and build map from parallel name to state vector
int i = 0;
parallel_joints_to_state_vector_.resize(nbOfParallelJoints_);
- for (std::map<std::string, std::string>::iterator it =
- from_parallel_name_to_state_vector_name.begin();
+ for (std::map<std::string, std::string>::iterator it = from_parallel_name_to_state_vector_name.begin();
it != from_parallel_name_to_state_vector_name.end(); it++, i++) {
joint_state_.name[i + nbOfJoints_] = it->first.c_str();
- parallel_joints_to_state_vector_[i] =
- from_state_name_to_state_vector[it->second];
+ parallel_joints_to_state_vector_[i] = from_state_name_to_state_vector[it->second];
}
return 0;
@@ -128,8 +116,7 @@ int SotLoaderBasic::readSotVectorStateParam() {
int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
po::options_description desc("Allowed options");
- desc.add_options()("help", "produce help message")(
- "input-file", po::value<string>(), "library to load");
+ desc.add_options()("help", "produce help message")("input-file", po::value<string>(), "library to load");
po::store(po::parse_command_line(argc, argv, desc), vm_);
po::notify(vm_);
@@ -150,8 +137,7 @@ int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
void SotLoaderBasic::Initialization() {
// Load the SotRobotBipedController library.
- sotRobotControllerLibrary_ =
- dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
+ sotRobotControllerLibrary_ = dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
if (!sotRobotControllerLibrary_) {
std::cerr << "Cannot load library: " << dlerror() << '\n';
return;
@@ -161,9 +147,8 @@ void SotLoaderBasic::Initialization() {
dlerror();
// Load the symbols.
- createSotExternalInterface_t* createSot =
- reinterpret_cast<createSotExternalInterface_t*>(reinterpret_cast<long>(
- dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
+ createSotExternalInterface_t* createSot = reinterpret_cast<createSotExternalInterface_t*>(
+ reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
const char* dlsym_error = dlerror();
if (dlsym_error) {
std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
@@ -183,9 +168,8 @@ void SotLoaderBasic::CleanUp() {
// SignalCaster singleton could probably be destroyed.
// Load the symbols.
- destroySotExternalInterface_t* destroySot =
- reinterpret_cast<destroySotExternalInterface_t*>(reinterpret_cast<long>(
- dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
+ destroySotExternalInterface_t* destroySot = reinterpret_cast<destroySotExternalInterface_t*>(
+ reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
const char* dlsym_error = dlerror();
if (dlsym_error) {
std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
@@ -199,14 +183,12 @@ void SotLoaderBasic::CleanUp() {
dlclose(sotRobotControllerLibrary_);
}
-bool SotLoaderBasic::start_dg(std_srvs::Empty::Request&,
- std_srvs::Empty::Response&) {
+bool SotLoaderBasic::start_dg(std_srvs::Empty::Request&, std_srvs::Empty::Response&) {
dynamic_graph_stopped_ = false;
return true;
}
-bool SotLoaderBasic::stop_dg(std_srvs::Empty::Request&,
- std_srvs::Empty::Response&) {
+bool SotLoaderBasic::stop_dg(std_srvs::Empty::Request&, std_srvs::Empty::Response&) {
dynamic_graph_stopped_ = true;
return true;
}
diff --git a/src/sot_to_ros.cpp b/src/sot_to_ros.cpp
index 8b03235..9d6e998 100644
--- a/src/sot_to_ros.cpp
+++ b/src/sot_to_ros.cpp
@@ -12,12 +12,8 @@ const char* SotToRos<Vector>::signalTypeName = "Vector";
const char* SotToRos<specific::Vector3>::signalTypeName = "Vector3";
const char* SotToRos<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
const char* SotToRos<specific::Twist>::signalTypeName = "Twist";
-const char* SotToRos<std::pair<specific::Vector3, Vector> >::signalTypeName =
- "Vector3Stamped";
-const char*
- SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName =
- "MatrixHomo";
-const char* SotToRos<std::pair<specific::Twist, Vector> >::signalTypeName =
- "Twist";
+const char* SotToRos<std::pair<specific::Vector3, Vector> >::signalTypeName = "Vector3Stamped";
+const char* SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName = "MatrixHomo";
+const char* SotToRos<std::pair<specific::Twist, Vector> >::signalTypeName = "Twist";
} // end of namespace dynamicgraph.
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index 6a3de0b..054e6ff 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -24,9 +24,8 @@
#include <dynamic-graph/signal-ptr.h>
#include <sot/core/matrix-geometry.hh>
-#define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME) \
- makeSignalString(typeid(*this).name(), NAME, IS_INPUT, OUTPUT_TYPE, \
- SIGNAL_NAME)
+#define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME) \
+ makeSignalString(typeid(*this).name(), NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)
namespace dynamicgraph {
@@ -301,14 +300,10 @@ struct SotToRos<std::pair<specific::Twist, Vector> > {
}
};
-inline std::string makeSignalString(const std::string& className,
- const std::string& instanceName,
- bool isInputSignal,
- const std::string& signalType,
- const std::string& signalName) {
+inline std::string makeSignalString(const std::string& className, const std::string& instanceName, bool isInputSignal,
+ const std::string& signalType, const std::string& signalName) {
boost::format fmt("%s(%s)::%s(%s)::%s");
- fmt % className % instanceName % (isInputSignal ? "input" : "output") %
- signalType % signalName;
+ fmt % className % instanceName % (isInputSignal ? "input" : "output") % signalType % signalName;
return fmt.str();
}
} // end of namespace dynamicgraph.
--
GitLab