From 49acf603c975238a256622a33cd1bdd875cce425 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Tue, 22 Oct 2019 23:03:15 +0200
Subject: [PATCH] Fix style
---
include/dynamic_graph_bridge/ros_init.hh | 27 +-
.../dynamic_graph_bridge/ros_interpreter.hh | 117 ++-
include/dynamic_graph_bridge/sot_loader.hh | 37 +-
.../dynamic_graph_bridge/sot_loader_basic.hh | 37 +-
src/converter.hh | 610 +++++++---------
src/geometric_simu.cpp | 13 +-
src/robot_model.cpp | 193 +++--
src/robot_model.hh | 97 ++-
src/ros_init.cpp | 136 ++--
src/ros_interpreter.cpp | 105 ++-
src/ros_publish.cpp | 425 +++++------
src/ros_publish.hh | 193 +++--
src/ros_queued_subscribe.cpp | 449 +++++-------
src/ros_queued_subscribe.hh | 380 +++++-----
src/ros_subscribe.cpp | 305 ++++----
src/ros_subscribe.hh | 190 +++--
src/ros_tf_listener.cpp | 48 +-
src/ros_tf_listener.hh | 169 ++---
src/ros_time.cpp | 56 +-
src/ros_time.hh | 46 +-
src/sot_loader.cpp | 221 +++---
src/sot_loader_basic.cpp | 215 +++---
src/sot_to_ros.cpp | 41 +-
src/sot_to_ros.hh | 689 ++++++++----------
24 files changed, 2155 insertions(+), 2644 deletions(-)
diff --git a/include/dynamic_graph_bridge/ros_init.hh b/include/dynamic_graph_bridge/ros_init.hh
index 5b1de2f..a4d1dd4 100644
--- a/include/dynamic_graph_bridge/ros_init.hh
+++ b/include/dynamic_graph_bridge/ros_init.hh
@@ -1,20 +1,19 @@
#ifndef ROS_INIT_HH
-# define ROS_INIT_HH
-# include <ros/ros.h>
+#define ROS_INIT_HH
+#include <ros/ros.h>
-namespace dynamicgraph
-{
- ros::NodeHandle& rosInit (bool createAsyncSpinner=false, bool createMultiThreadSpinner=true);
+namespace dynamicgraph {
+ros::NodeHandle& rosInit(bool createAsyncSpinner = false,
+ bool createMultiThreadSpinner = true);
- /// \brief Return spinner or throw an exception if spinner
- /// creation has been disabled at startup.
- ros::AsyncSpinner& spinner ();
+/// \brief Return spinner or throw an exception if spinner
+/// creation has been disabled at startup.
+ros::AsyncSpinner& spinner();
- /// \brief Return multi threaded spinner or throw an exception if spinner
- /// creation has been disabled at startup.
- ros::MultiThreadedSpinner& mtSpinner ();
+/// \brief Return multi threaded spinner or throw an exception if spinner
+/// creation has been disabled at startup.
+ros::MultiThreadedSpinner& mtSpinner();
+} // end of namespace dynamicgraph.
-} // end of namespace dynamicgraph.
-
-#endif //! ROS_INIT_HH
+#endif //! ROS_INIT_HH
diff --git a/include/dynamic_graph_bridge/ros_interpreter.hh b/include/dynamic_graph_bridge/ros_interpreter.hh
index 1c8dec1..f4fda6c 100644
--- a/include/dynamic_graph_bridge/ros_interpreter.hh
+++ b/include/dynamic_graph_bridge/ros_interpreter.hh
@@ -1,60 +1,59 @@
#ifndef DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
-# define DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
-# include <ros/ros.h>
-# include <dynamic_graph_bridge_msgs/RunCommand.h>
-# include <dynamic_graph_bridge_msgs/RunPythonFile.h>
-# include <dynamic-graph/python/interpreter.hh>
-
-namespace dynamicgraph
-{
- /// \brief This class wraps the implementation of the runCommand
- /// service.
- ///
- /// This takes as input a ROS node handle and do not handle the
- /// callback so that the service behavior can be controlled from
- /// the outside.
- class Interpreter
- {
- public:
- 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&)>
- 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);
-
- /// \brief Method to parse python scripts.
- /// \param Input file name to parse.
- void runPythonFile(std::string ifilename);
-
- /// Initialize service run_command
- void startRosService ();
-
- protected:
- /// \brief Run a Python command and return result, stderr and stdout.
- bool runCommandCallback (dynamic_graph_bridge_msgs::RunCommand::Request& req,
- 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);
-
- private:
- python::Interpreter interpreter_;
- ros::NodeHandle& nodeHandle_;
- ros::ServiceServer runCommandSrv_;
- ros::ServiceServer runPythonFileSrv_;
- };
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
+#define DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
+#include <ros/ros.h>
+#include <dynamic_graph_bridge_msgs/RunCommand.h>
+#include <dynamic_graph_bridge_msgs/RunPythonFile.h>
+#include <dynamic-graph/python/interpreter.hh>
+
+namespace dynamicgraph {
+/// \brief This class wraps the implementation of the runCommand
+/// service.
+///
+/// This takes as input a ROS node handle and do not handle the
+/// callback so that the service behavior can be controlled from
+/// the outside.
+class Interpreter {
+ public:
+ 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&)>
+ 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);
+
+ /// \brief Method to parse python scripts.
+ /// \param Input file name to parse.
+ void runPythonFile(std::string ifilename);
+
+ /// Initialize service run_command
+ void startRosService();
+
+ protected:
+ /// \brief Run a Python command and return result, stderr and stdout.
+ bool runCommandCallback(dynamic_graph_bridge_msgs::RunCommand::Request& req,
+ 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);
+
+ private:
+ python::Interpreter interpreter_;
+ ros::NodeHandle& nodeHandle_;
+ ros::ServiceServer runCommandSrv_;
+ ros::ServiceServer runPythonFileSrv_;
+};
+} // end of namespace dynamicgraph.
+
+#endif //! DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index 72e93ae..e70ac48 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -41,7 +41,7 @@
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
-// Sot Framework includes
+// Sot Framework includes
#include <sot/core/debug.hh>
#include <sot/core/abstract-sot-external-interface.hh>
@@ -51,20 +51,15 @@
namespace po = boost::program_options;
namespace dgs = dynamicgraph::sot;
-
-class SotLoader: public SotLoaderBasic
-{
-
-protected:
-
-
+class SotLoader : public SotLoaderBasic {
+ protected:
/// Map of sensor readings
- std::map <std::string,dgs::SensorValues> sensorsIn_;
+ std::map<std::string, dgs::SensorValues> sensorsIn_;
/// Map of control values
- std::map<std::string,dgs::ControlValues> controlValues_;
+ std::map<std::string, dgs::ControlValues> controlValues_;
/// Angular values read by encoders
- std::vector <double> angleEncoder_;
+ std::vector<double> angleEncoder_;
/// Angular values sent to motors
std::vector<double> angleControl_;
/// Forces read by force sensors
@@ -74,26 +69,24 @@ protected:
/// Attitude of the robot computed by extended Kalman filter.
std::vector<double> baseAtt_;
/// Accelerations read by Accelerometers
- std::vector <double> accelerometer_;
+ std::vector<double> accelerometer_;
/// Angular velocity read by gyrometers
- std::vector <double> gyrometer_;
+ std::vector<double> gyrometer_;
/// URDF string description of the robot.
std::string robot_desc_string_;
-
/// The thread running dynamic graph
boost::thread thread_;
-
+
// \brief Start control loop
virtual void startControlLoop();
-
- //Robot Pose Publisher
+ // Robot Pose Publisher
tf::TransformBroadcaster freeFlyerPublisher_;
tf::Transform freeFlyerPose_;
-public:
+ public:
SotLoader();
~SotLoader();
@@ -105,17 +98,13 @@ public:
void oneIteration();
// \brief Fill the sensors value for the SoT.
- void fillSensors(std::map<std::string,
- dgs::SensorValues> & sensorsIn);
+ void fillSensors(std::map<std::string, dgs::SensorValues> &sensorsIn);
// \brief Read the control computed by the SoT framework.
- void readControl(std::map<std::string,
- dgs::ControlValues> & controlValues);
+ void readControl(std::map<std::string, dgs::ControlValues> &controlValues);
// \brief Prepare the SoT framework.
void setup();
-
-
};
#endif /* SOT_LOADER_HH_ */
diff --git a/include/dynamic_graph_bridge/sot_loader_basic.hh b/include/dynamic_graph_bridge/sot_loader_basic.hh
index 85de433..8ca2860 100644
--- a/include/dynamic_graph_bridge/sot_loader_basic.hh
+++ b/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -39,28 +39,26 @@
#include "std_srvs/Empty.h"
#include <sensor_msgs/JointState.h>
-// Sot Framework includes
+// Sot Framework includes
#include <sot/core/debug.hh>
#include <sot/core/abstract-sot-external-interface.hh>
namespace po = boost::program_options;
namespace dgs = dynamicgraph::sot;
-
class SotLoaderBasic {
-protected:
-
+ protected:
// Dynamic graph is stopped.
bool dynamic_graph_stopped_;
/// \brief the sot-hrp2 controller
- dgs::AbstractSotExternalInterface * sotController_;
+ dgs::AbstractSotExternalInterface* sotController_;
po::variables_map vm_;
std::string dynamicLibraryName_;
/// \brief Handle on the SoT library.
- void * sotRobotControllerLibrary_;
+ void* sotRobotControllerLibrary_;
/// \brief Map between SoT state vector and some joint_state_links
XmlRpc::XmlRpcValue stateVectorMap_;
@@ -76,10 +74,10 @@ protected:
/// Advertises stop_dynamic_graph services
ros::ServiceServer service_stop_;
-
+
// Joint state publication.
ros::Publisher joint_pub_;
-
+
// Joint state to be published.
sensor_msgs::JointState joint_state_;
@@ -87,13 +85,12 @@ protected:
int nbOfJoints_;
parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
-
-public:
+ public:
SotLoaderBasic();
- ~SotLoaderBasic() {};
+ ~SotLoaderBasic(){};
// \brief Read user input to extract the path of the SoT dynamic library.
- int parseOptions(int argc, char *argv[]);
+ int parseOptions(int argc, char* argv[]);
/// \brief Load the SoT device corresponding to the robot.
void Initialization();
@@ -102,16 +99,15 @@ public:
void CleanUp();
// \brief Create a thread for ROS.
- virtual void initializeRosNode(int argc, char *argv[]);
-
+ virtual void initializeRosNode(int argc, char* argv[]);
// \brief Callback function when starting dynamic graph.
- bool start_dg(std_srvs::Empty::Request& request,
+ 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();
@@ -120,13 +116,10 @@ public:
int initPublication();
// \brief Get Status of dg.
- bool isDynamicGraphStopped()
- { return dynamic_graph_stopped_; }
+ bool isDynamicGraphStopped() { return dynamic_graph_stopped_; }
// \brief Specify the name of the dynamic library.
- void setDynamicLibraryName(std::string &afilename);
-
-
+ void setDynamicLibraryName(std::string& afilename);
};
#endif /* _SOT_LOADER_BASIC_HH_ */
diff --git a/src/converter.hh b/src/converter.hh
index 65e5777..58440a8 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -1,346 +1,284 @@
#ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH
-# define DYNAMIC_GRAPH_ROS_CONVERTER_HH
-# include <stdexcept>
-# include "sot_to_ros.hh"
-
-# include <boost/static_assert.hpp>
-# include <boost/date_time/date.hpp>
-# include <boost/date_time/posix_time/posix_time.hpp>
-
-# include <ros/time.h>
-# include <std_msgs/Header.h>
-
-# 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 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)
- {
- header.seq = 0;
- header.stamp = ros::Time::now ();
- header.frame_id = "/dynamic_graph/world";
+#define DYNAMIC_GRAPH_ROS_CONVERTER_HH
+#include <stdexcept>
+#include "sot_to_ros.hh"
+
+#include <boost/static_assert.hpp>
+#include <boost/date_time/date.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+
+#include <ros/time.h>
+#include <std_msgs/Header.h>
+
+#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 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) {
+ header.seq = 0;
+ header.stamp = ros::Time::now();
+ header.frame_id = "/dynamic_graph/world";
+}
+
+/// \brief Handle ROS <-> dynamic-graph conversion.
+///
+/// Implements all ROS/dynamic-graph conversions required by the
+/// bridge.
+///
+/// Relies on the SotToRos type trait to make sure valid pair of
+/// types are used.
+template <typename D, typename S>
+void converter(D& dst, const S& src);
+
+// Boolean
+SOT_TO_ROS_IMPL(bool) { dst.data = src; }
+
+ROS_TO_SOT_IMPL(bool) { dst = src.data; }
+
+// Double
+SOT_TO_ROS_IMPL(double) { dst.data = src; }
+
+ROS_TO_SOT_IMPL(double) { dst = src.data; }
+
+// Int
+SOT_TO_ROS_IMPL(int) { dst.data = src; }
+
+ROS_TO_SOT_IMPL(int) { dst = src.data; }
+
+// Unsigned
+SOT_TO_ROS_IMPL(unsigned int) { dst.data = src; }
+
+ROS_TO_SOT_IMPL(unsigned int) { dst = src.data; }
+
+// String
+SOT_TO_ROS_IMPL(std::string) { dst.data = src; }
+
+ROS_TO_SOT_IMPL(std::string) { dst = src.data; }
+
+// Vector
+SOT_TO_ROS_IMPL(Vector) {
+ dst.data.resize(src.size());
+ for (int i = 0; i < src.size(); ++i) dst.data[i] = src(i);
+}
+
+ROS_TO_SOT_IMPL(Vector) {
+ dst.resize(src.data.size());
+ for (unsigned int i = 0; i < src.data.size(); ++i) dst(i) = src.data[i];
+}
+
+// Vector3
+SOT_TO_ROS_IMPL(specific::Vector3) {
+ if (src.size() > 0) {
+ dst.x = src(0);
+ if (src.size() > 1) {
+ dst.y = src(1);
+ if (src.size() > 2) dst.z = src(2);
+ }
}
-
- /// \brief Handle ROS <-> dynamic-graph conversion.
- ///
- /// Implements all ROS/dynamic-graph conversions required by the
- /// bridge.
- ///
- ///Relies on the SotToRos type trait to make sure valid pair of
- /// types are used.
- template <typename D, typename S>
- void converter (D& dst, const S& src);
-
- // Boolean
- SOT_TO_ROS_IMPL(bool)
- {
- dst.data = src;
- }
-
- ROS_TO_SOT_IMPL(bool)
- {
- dst = src.data;
- }
-
- // Double
- SOT_TO_ROS_IMPL(double)
- {
- dst.data = src;
- }
-
- ROS_TO_SOT_IMPL(double)
- {
- dst = src.data;
- }
-
- // Int
- SOT_TO_ROS_IMPL(int)
- {
- dst.data = src;
- }
-
- ROS_TO_SOT_IMPL(int)
- {
- dst = src.data;
- }
-
- // Unsigned
- SOT_TO_ROS_IMPL(unsigned int)
- {
- dst.data = src;
- }
-
- ROS_TO_SOT_IMPL(unsigned int)
- {
- dst = src.data;
- }
-
- // String
- SOT_TO_ROS_IMPL(std::string)
- {
- dst.data = src;
- }
-
- ROS_TO_SOT_IMPL(std::string)
- {
- dst = src.data;
- }
-
- // Vector
- SOT_TO_ROS_IMPL(Vector)
- {
- dst.data.resize (src.size ());
- for (int i = 0; i < src.size (); ++i)
- dst.data[i] = src (i);
- }
-
- ROS_TO_SOT_IMPL(Vector)
- {
- dst.resize (src.data.size ());
- for (unsigned int i = 0; i < src.data.size (); ++i)
- dst (i) = src.data[i];
- }
-
- // Vector3
- SOT_TO_ROS_IMPL(specific::Vector3)
- {
- if (src.size () > 0)
- {
- dst.x = src (0);
- if (src.size () > 1)
- {
- dst.y = src (1);
- if (src.size () > 2)
- dst.z = src (2);
- }
- }
- }
-
- ROS_TO_SOT_IMPL(specific::Vector3)
- {
- dst.resize (3);
- dst (0) = src.x;
- dst (1) = src.y;
- dst (2) = src.z;
- }
-
- // Matrix
- SOT_TO_ROS_IMPL(Matrix)
- {
-
- //TODO: Confirm Ros Matrix Storage order. It changes the RosMatrix to ColMajor.
- dst.width = (unsigned int)src.rows ();
- dst.data.resize (src.cols () * src.rows ());
- for (int i = 0; i < src.cols () * src.rows (); ++i)
- dst.data[i] = src.data()[i];
- }
-
- ROS_TO_SOT_IMPL(Matrix)
- {
- 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];
- }
-
- // Homogeneous matrix.
- SOT_TO_ROS_IMPL(sot::MatrixHomogeneous)
- {
-
- sot::VectorQuaternion q(src.linear());
- dst.translation.x = src.translation()(0);
- dst.translation.y = src.translation()(1);
- dst.translation.z = src.translation()(2);
-
- dst.rotation.x = q.x();
- dst.rotation.y = q.y();
- dst.rotation.z = q.z();
- dst.rotation.w = q.w();
-
- }
-
- ROS_TO_SOT_IMPL(sot::MatrixHomogeneous)
- {
- sot::VectorQuaternion q(src.rotation.w,
- src.rotation.x,
- src.rotation.y,
- src.rotation.z);
- dst.linear() = q.matrix();
-
- // Copy the translation component.
- dst.translation()(0) = src.translation.x;
- dst.translation()(1) = src.translation.y;
- dst.translation()(2) = src.translation.z;
- }
-
-
- // Twist.
- SOT_TO_ROS_IMPL(specific::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);
- dst.angular.x = src (3);
- dst.angular.y = src (4);
- dst.angular.z = src (5);
- }
-
- ROS_TO_SOT_IMPL(specific::Twist)
- {
- dst.resize (6);
- dst (0) = src.linear.x;
- dst (1) = src.linear.y;
- dst (2) = src.linear.z;
- dst (3) = src.angular.x;
- dst (4) = src.angular.y;
- dst (5) = src.angular.z;
- }
-
-
- /// \brief This macro generates a converter for a stamped type from
- /// dynamic-graph to ROS. I.e. A data associated with its
- /// timestamp.
-# define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
- template <> \
- inline void converter \
- (SotToRos<std::pair<T, Vector> >::ros_t& dst, \
- const SotToRos<std::pair<T, Vector> >::sot_t& src) \
- { \
- makeHeader(dst.header); \
- converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t> (dst.ATTRIBUTE, src); \
- do { EXTRA } while (0); \
- } \
+}
+
+ROS_TO_SOT_IMPL(specific::Vector3) {
+ dst.resize(3);
+ dst(0) = src.x;
+ dst(1) = src.y;
+ dst(2) = src.z;
+}
+
+// Matrix
+SOT_TO_ROS_IMPL(Matrix) {
+ // TODO: Confirm Ros Matrix Storage order. It changes the RosMatrix to
+ // ColMajor.
+ dst.width = (unsigned int)src.rows();
+ dst.data.resize(src.cols() * src.rows());
+ for (int i = 0; i < src.cols() * src.rows(); ++i) dst.data[i] = src.data()[i];
+}
+
+ROS_TO_SOT_IMPL(Matrix) {
+ 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];
+}
+
+// Homogeneous matrix.
+SOT_TO_ROS_IMPL(sot::MatrixHomogeneous) {
+ sot::VectorQuaternion q(src.linear());
+ dst.translation.x = src.translation()(0);
+ dst.translation.y = src.translation()(1);
+ dst.translation.z = src.translation()(2);
+
+ dst.rotation.x = q.x();
+ dst.rotation.y = q.y();
+ dst.rotation.z = q.z();
+ dst.rotation.w = q.w();
+}
+
+ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
+ sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y,
+ src.rotation.z);
+ dst.linear() = q.matrix();
+
+ // Copy the translation component.
+ dst.translation()(0) = src.translation.x;
+ dst.translation()(1) = src.translation.y;
+ dst.translation()(2) = src.translation.z;
+}
+
+// Twist.
+SOT_TO_ROS_IMPL(specific::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);
+ dst.angular.x = src(3);
+ dst.angular.y = src(4);
+ dst.angular.z = src(5);
+}
+
+ROS_TO_SOT_IMPL(specific::Twist) {
+ dst.resize(6);
+ dst(0) = src.linear.x;
+ dst(1) = src.linear.y;
+ dst(2) = src.linear.z;
+ dst(3) = src.angular.x;
+ dst(4) = src.angular.y;
+ dst(5) = src.angular.z;
+}
+
+/// \brief This macro generates a converter for a stamped type from
+/// dynamic-graph to ROS. I.e. A data associated with its
+/// timestamp.
+#define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
+ template <> \
+ inline void converter(SotToRos<std::pair<T, Vector> >::ros_t& dst, \
+ const SotToRos<std::pair<T, Vector> >::sot_t& src) { \
+ makeHeader(dst.header); \
+ converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t>(dst.ATTRIBUTE, src); \
+ do { \
+ EXTRA \
+ } while (0); \
+ } \
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(specific::Twist, twist, ;);
-
-
- /// \brief This macro generates a converter for a shared pointer on
- /// a ROS type to a dynamic-graph type.
- ///
- /// 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); \
- } \
+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(specific::Twist, twist, ;);
+
+/// \brief This macro generates a converter for a shared pointer on
+/// a ROS type to a dynamic-graph type.
+///
+/// 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); \
+ } \
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
- DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
- DG_BRIDGE_MAKE_SHPTR_IMPL(double);
- DG_BRIDGE_MAKE_SHPTR_IMPL(int);
- DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
- DG_BRIDGE_MAKE_SHPTR_IMPL(std::string);
- DG_BRIDGE_MAKE_SHPTR_IMPL(Vector);
- DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
- DG_BRIDGE_MAKE_SHPTR_IMPL(Matrix);
- DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
- DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
-
- /// \brief This macro generates a converter for 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_IMPL(T, ATTRIBUTE, EXTRA) \
- template <> \
- inline void converter \
- (SotToRos<std::pair<T, Vector> >::sot_t& dst, \
- const SotToRos<std::pair<T, Vector> >::ros_t& src) \
- { \
- converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t> (dst, src.ATTRIBUTE); \
- do { EXTRA } while (0); \
- } \
+DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
+DG_BRIDGE_MAKE_SHPTR_IMPL(double);
+DG_BRIDGE_MAKE_SHPTR_IMPL(int);
+DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
+DG_BRIDGE_MAKE_SHPTR_IMPL(std::string);
+DG_BRIDGE_MAKE_SHPTR_IMPL(Vector);
+DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
+DG_BRIDGE_MAKE_SHPTR_IMPL(Matrix);
+DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
+DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
+
+/// \brief This macro generates a converter for 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_IMPL(T, ATTRIBUTE, EXTRA) \
+ template <> \
+ inline void converter(SotToRos<std::pair<T, Vector> >::sot_t& dst, \
+ const SotToRos<std::pair<T, Vector> >::ros_t& 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_IMPL(specific::Vector3, vector, ;);
- DG_BRIDGE_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, ;);
- DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
-
- /// \brief This macro generates a converter for a shared pointer on
- /// 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); \
- } \
+DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
+DG_BRIDGE_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, ;);
+DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
+
+/// \brief This macro generates a converter for a shared pointer on
+/// 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); \
+ } \
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, ;);
- DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(sot::MatrixHomogeneous, transform, ;);
- DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Twist, twist, ;);
-
-
- /// \brief If an impossible/unimplemented conversion is required, fail.
- ///
- /// IMPORTANT, READ ME:
- ///
- /// If the compiler generates an error in the following function,
- /// this is /normal/.
- ///
- /// This error means that either you try to use an undefined
- /// conversion. You can either fix your code or provide the wanted
- /// conversion by updating this header.
- template <typename U, typename V>
- inline void converter (U& dst, V& src)
- {
- // This will always fail if instantiated.
- BOOST_STATIC_ASSERT (sizeof (U) == 0);
- }
-
- typedef boost::posix_time::ptime ptime;
- typedef boost::posix_time::seconds seconds;
- typedef boost::posix_time::microseconds microseconds;
- 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));
- return time;
- }
-
- 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 nsec = (unsigned int)diff.fractional_seconds();
-
- return ros::Time (sec, nsec);
- }
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH
+DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Vector3, vector, ;);
+DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(sot::MatrixHomogeneous, transform, ;);
+DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Twist, twist, ;);
+
+/// \brief If an impossible/unimplemented conversion is required, fail.
+///
+/// IMPORTANT, READ ME:
+///
+/// If the compiler generates an error in the following function,
+/// this is /normal/.
+///
+/// This error means that either you try to use an undefined
+/// conversion. You can either fix your code or provide the wanted
+/// conversion by updating this header.
+template <typename U, typename V>
+inline void converter(U& dst, V& src) {
+ // This will always fail if instantiated.
+ BOOST_STATIC_ASSERT(sizeof(U) == 0);
+}
+
+typedef boost::posix_time::ptime ptime;
+typedef boost::posix_time::seconds seconds;
+typedef boost::posix_time::microseconds microseconds;
+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));
+ return time;
+}
+
+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 nsec = (unsigned int)diff.fractional_seconds();
+
+ return ros::Time(sec, nsec);
+}
+} // end of namespace dynamicgraph.
+
+#endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH
diff --git a/src/geometric_simu.cpp b/src/geometric_simu.cpp
index 11f9ad2..d8fda01 100644
--- a/src/geometric_simu.cpp
+++ b/src/geometric_simu.cpp
@@ -26,17 +26,14 @@
#include <dynamic_graph_bridge/sot_loader.hh>
-
-int main(int argc, char *argv[])
-{
- dgADD_OSTREAM_TO_RTLOG (std::cerr);
+int main(int argc, char *argv[]) {
+ dgADD_OSTREAM_TO_RTLOG(std::cerr);
ros::init(argc, argv, "sot_ros_encapsulator");
SotLoader aSotLoader;
- if (aSotLoader.parseOptions(argc,argv)<0)
- return -1;
-
- aSotLoader.initializeRosNode(argc,argv);
+ if (aSotLoader.parseOptions(argc, argv) < 0) return -1;
+
+ aSotLoader.initializeRosNode(argc, argv);
ros::spin();
return 0;
diff --git a/src/robot_model.cpp b/src/robot_model.cpp
index 6632b24..23ec297 100644
--- a/src/robot_model.cpp
+++ b/src/robot_model.cpp
@@ -9,111 +9,110 @@
#include <ros/package.h>
-namespace dynamicgraph
-{
+namespace dynamicgraph {
RosRobotModel::RosRobotModel(const std::string& name)
- : Dynamic(name),
- jointsParameterName_("jrl_map"),
- ns_("sot_controller")
-{
-
- std::string docstring;
-
- docstring =
- "\n"
- " Load the robot model from the parameter server.\n"
- "\n"
- " This is the recommended method.\n"
- "\n";
- addCommand("loadFromParameterServer", command::makeCommandVoid0(*this,&RosRobotModel::loadFromParameterServer,docstring));
-
- docstring =
- "\n"
- " Load the robot model from an URDF file.\n"
- "\n";
- addCommand("loadUrdf", command::makeCommandVoid1(*this,&RosRobotModel::loadUrdf,docstring));
-
- docstring =
- "\n"
- " Set the controller namespace."
- "\n";
- addCommand("setNamespace", command::makeCommandVoid1(*this,&RosRobotModel::setNamespace,docstring));
-
- docstring =
- "\n"
- " Get current configuration of the robot.\n"
- "\n";
- addCommand ("curConf", new command::Getter<RosRobotModel,Vector> (*this,&RosRobotModel::curConf,docstring));
-
- docstring =
- "\n"
- " Maps a link name in the URDF parser to actual robot link name.\n"
- "\n";
- addCommand ("addJointMapping", command::makeCommandVoid2(*this,&RosRobotModel::addJointMapping,docstring));
+ : Dynamic(name), jointsParameterName_("jrl_map"), ns_("sot_controller") {
+ std::string docstring;
+
+ docstring =
+ "\n"
+ " Load the robot model from the parameter server.\n"
+ "\n"
+ " This is the recommended method.\n"
+ "\n";
+ addCommand("loadFromParameterServer",
+ command::makeCommandVoid0(
+ *this, &RosRobotModel::loadFromParameterServer, docstring));
+
+ docstring =
+ "\n"
+ " Load the robot model from an URDF file.\n"
+ "\n";
+ addCommand("loadUrdf", command::makeCommandVoid1(
+ *this, &RosRobotModel::loadUrdf, docstring));
+
+ docstring =
+ "\n"
+ " Set the controller namespace."
+ "\n";
+ addCommand("setNamespace",
+ command::makeCommandVoid1(*this, &RosRobotModel::setNamespace,
+ docstring));
+
+ docstring =
+ "\n"
+ " Get current configuration of the robot.\n"
+ "\n";
+ addCommand("curConf", new command::Getter<RosRobotModel, Vector>(
+ *this, &RosRobotModel::curConf, docstring));
+
+ docstring =
+ "\n"
+ " Maps a link name in the URDF parser to actual robot link name.\n"
+ "\n";
+ addCommand("addJointMapping",
+ command::makeCommandVoid2(*this, &RosRobotModel::addJointMapping,
+ docstring));
}
-RosRobotModel::~RosRobotModel()
-{}
+RosRobotModel::~RosRobotModel() {}
-void RosRobotModel::loadUrdf (const std::string& filename)
-{
- rosInit (false);
+void RosRobotModel::loadUrdf(const std::string& filename) {
+ rosInit(false);
m_model = se3::urdf::buildModel(filename);
this->m_urdfPath = filename;
if (m_data) delete m_data;
m_data = new se3::Data(m_model);
- init=true;
+ init = true;
// m_HDR = parser.parse(filename);
ros::NodeHandle nh(ns_);
-
+
XmlRpc::XmlRpcValue JointsNamesByRank_;
JointsNamesByRank_.setSize(m_model.names.size());
- std::vector<std::string>::const_iterator it = m_model.names.begin()+2; //first joint is universe, second is freeflyer
- for (int i=0;it!=m_model.names.end();++it, ++i) JointsNamesByRank_[i]= (*it);
+ std::vector<std::string>::const_iterator it =
+ m_model.names.begin() +
+ 2; // first joint is universe, second is freeflyer
+ for (int i = 0; it != m_model.names.end(); ++it, ++i)
+ JointsNamesByRank_[i] = (*it);
nh.setParam(jointsParameterName_, JointsNamesByRank_);
}
-void RosRobotModel::setNamespace (const std::string& ns)
-{
- ns_ = ns;
-}
+void RosRobotModel::setNamespace(const std::string& ns) { ns_ = ns; }
+
+void RosRobotModel::loadFromParameterServer() {
+ rosInit(false);
+ std::string robotDescription;
+ ros::param::param<std::string>("/robot_description", robotDescription, "");
+ if (robotDescription.empty())
+ throw std::runtime_error("No model available as ROS parameter. Fail.");
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDF(robotDescription);
+ if (urdfTree)
+ se3::urdf::parseTree(urdfTree->getRoot(), this->m_model,
+ se3::SE3::Identity(), false);
+ else {
+ const std::string exception_message(
+ "robot_description not parsed correctly.");
+ throw std::invalid_argument(exception_message);
+ }
-void RosRobotModel::loadFromParameterServer()
-{
- rosInit (false);
- std::string robotDescription;
- ros::param::param<std::string> ("/robot_description", robotDescription, "");
- if (robotDescription.empty ())
- throw std::runtime_error("No model available as ROS parameter. Fail.");
- ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDF (robotDescription);
- if (urdfTree)
- se3::urdf::parseTree(urdfTree->getRoot(),
- this->m_model, se3::SE3::Identity(), false);
- else {
- const std::string exception_message
- ("robot_description not parsed correctly.");
- throw std::invalid_argument(exception_message);
- }
+ this->m_urdfPath = "";
+ if (m_data) delete m_data;
+ m_data = new se3::Data(m_model);
+ init = true;
+ ros::NodeHandle nh(ns_);
- this->m_urdfPath = "";
- if (m_data) delete m_data;
- m_data = new se3::Data(m_model);
- init=true;
- ros::NodeHandle nh(ns_);
-
- XmlRpc::XmlRpcValue JointsNamesByRank_;
- JointsNamesByRank_.setSize(m_model.names.size());
- //first joint is universe, second is freeflyer
- std::vector<std::string>::const_iterator it = m_model.names.begin()+2;
- for (int i=0;it!=m_model.names.end();++it, ++i) JointsNamesByRank_[i]= (*it);
- nh.setParam(jointsParameterName_, JointsNamesByRank_);
+ XmlRpc::XmlRpcValue JointsNamesByRank_;
+ JointsNamesByRank_.setSize(m_model.names.size());
+ // first joint is universe, second is freeflyer
+ std::vector<std::string>::const_iterator it = m_model.names.begin() + 2;
+ for (int i = 0; it != m_model.names.end(); ++it, ++i)
+ JointsNamesByRank_[i] = (*it);
+ nh.setParam(jointsParameterName_, JointsNamesByRank_);
}
-Vector RosRobotModel::curConf() const
-{
-
+Vector RosRobotModel::curConf() const {
// The first 6 dofs are associated to the Freeflyer frame
// Freeflyer reference frame should be the same as global
// frame so that operational point positions correspond to
@@ -121,23 +120,22 @@ Vector RosRobotModel::curConf() const
XmlRpc::XmlRpcValue ffpose;
ros::NodeHandle nh(ns_);
std::string param_name = "ffpose";
- if (nh.hasParam(param_name)){
+ if (nh.hasParam(param_name)) {
nh.getParam(param_name, ffpose);
ROS_ASSERT(ffpose.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(ffpose.size() == 6);
for (int32_t i = 0; i < ffpose.size(); ++i) {
ROS_ASSERT(ffpose[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
}
- }
- else {
+ } else {
ffpose.setSize(6);
- for (int32_t i = 0; i < ffpose.size(); ++i) ffpose[i] = 0.0;
+ for (int32_t i = 0; i < ffpose.size(); ++i) ffpose[i] = 0.0;
}
-
- if (!m_data )
- throw std::runtime_error ("no robot loaded");
+
+ if (!m_data)
+ throw std::runtime_error("no robot loaded");
else {
- //TODO: confirm accesscopy is for asynchronous commands
+ // 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]);
@@ -145,12 +143,11 @@ Vector RosRobotModel::curConf() const
return currConf;
}
}
-
-void
-RosRobotModel::addJointMapping(const std::string &link, const std::string &repName)
-{
- specialJoints_[link] = repName;
+
+void RosRobotModel::addJointMapping(const std::string& link,
+ const std::string& repName) {
+ specialJoints_[link] = repName;
}
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosRobotModel, "RosRobotModel");
-} // end of namespace dynamicgraph.
+} // end of namespace dynamicgraph.
diff --git a/src/robot_model.hh b/src/robot_model.hh
index 646a894..59e68fd 100644
--- a/src/robot_model.hh
+++ b/src/robot_model.hh
@@ -1,58 +1,53 @@
#ifndef DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
-# define DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
+#define DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
-# include <string>
+#include <string>
#include <sot-dynamic/dynamic.h>
#include <dynamic-graph/linear-algebra.h>
#include "XmlRpcValue.h"
-namespace dynamicgraph
-{
- class RosRobotModel;
-
- /// \brief This entity load either the current model available in
- /// the robot_description parameter or a specified file and build
- /// a Dynamic entity
- ///
- /// This relies on pinocchio urdf parser to load the model and pinocchio
- /// to realize the computation.
- class RosRobotModel : public sot::Dynamic
- {
- DYNAMIC_GRAPH_ENTITY_DECL();
- public:
- RosRobotModel(const std::string& n);
- virtual ~RosRobotModel();
-
- void loadUrdf(const std::string& filename);
- void setNamespace (const std::string& ns);
- void loadFromParameterServer();
- Vector curConf() const;
-
- void addJointMapping(const std::string& link, const std::string& repName);
-
- protected:
-
- unsigned getDimension () const
- {
- if (!m_data)
- throw std::runtime_error ("no robot loaded");
- //TODO: Configuration vector dimension or the dof?
- return m_model.nv;
- //return m_model.nq;
- }
-
-
- private:
- /// \brief Name of the parameter where the joints list will be published
- std::string jointsParameterName_;
-
- /// \brief Name of the controller namespace
- std::string ns_;
-
- /// \brief Special joints map for the parser
- std::map<std::string, std::string> specialJoints_;
- };
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
+namespace dynamicgraph {
+class RosRobotModel;
+
+/// \brief This entity load either the current model available in
+/// the robot_description parameter or a specified file and build
+/// a Dynamic entity
+///
+/// This relies on pinocchio urdf parser to load the model and pinocchio
+/// to realize the computation.
+class RosRobotModel : public sot::Dynamic {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+ RosRobotModel(const std::string& n);
+ virtual ~RosRobotModel();
+
+ void loadUrdf(const std::string& filename);
+ void setNamespace(const std::string& ns);
+ void loadFromParameterServer();
+ Vector curConf() const;
+
+ void addJointMapping(const std::string& link, const std::string& repName);
+
+ protected:
+ unsigned getDimension() const {
+ if (!m_data) throw std::runtime_error("no robot loaded");
+ // TODO: Configuration vector dimension or the dof?
+ return m_model.nv;
+ // return m_model.nq;
+ }
+
+ private:
+ /// \brief Name of the parameter where the joints list will be published
+ std::string jointsParameterName_;
+
+ /// \brief Name of the controller namespace
+ std::string ns_;
+
+ /// \brief Special joints map for the parser
+ std::map<std::string, std::string> specialJoints_;
+};
+} // end of namespace dynamicgraph.
+
+#endif //! DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
diff --git a/src/ros_init.cpp b/src/ros_init.cpp
index 0918704..bad1612 100644
--- a/src/ros_init.cpp
+++ b/src/ros_init.cpp
@@ -4,84 +4,74 @@
#include "dynamic_graph_bridge/ros_init.hh"
-namespace dynamicgraph
-{
- struct GlobalRos
- {
- ~GlobalRos ()
- {
- if (spinner)
- spinner->stop ();
- if (nodeHandle)
- nodeHandle->shutdown ();
- }
-
- boost::shared_ptr<ros::NodeHandle> nodeHandle;
- boost::shared_ptr<ros::AsyncSpinner> spinner;
- boost::shared_ptr<ros::MultiThreadedSpinner> mtSpinner;
- };
- GlobalRos ros;
+namespace dynamicgraph {
+struct GlobalRos {
+ ~GlobalRos() {
+ if (spinner) spinner->stop();
+ if (nodeHandle) nodeHandle->shutdown();
+ }
- ros::NodeHandle& rosInit (bool createAsyncSpinner, bool createMultiThreadedSpinner)
- {
- if (!ros.nodeHandle)
- {
- int argc = 1;
- char* arg0 = strdup("dynamic_graph_bridge");
- char* argv[] = {arg0, 0};
- ros::init(argc, argv, "dynamic_graph_bridge");
- free (arg0);
+ boost::shared_ptr<ros::NodeHandle> nodeHandle;
+ boost::shared_ptr<ros::AsyncSpinner> spinner;
+ boost::shared_ptr<ros::MultiThreadedSpinner> mtSpinner;
+};
+GlobalRos ros;
- ros.nodeHandle = boost::make_shared<ros::NodeHandle> ("");
- }
- if (!ros.spinner && createAsyncSpinner)
- {
- ros.spinner = boost::make_shared<ros::AsyncSpinner> (4);
+ros::NodeHandle& rosInit(bool createAsyncSpinner,
+ bool createMultiThreadedSpinner) {
+ if (!ros.nodeHandle) {
+ int argc = 1;
+ char* arg0 = strdup("dynamic_graph_bridge");
+ char* argv[] = {arg0, 0};
+ ros::init(argc, argv, "dynamic_graph_bridge");
+ free(arg0);
- // Change the thread's scheduler from real-time to normal and reduce its priority
- int oldThreadPolicy, newThreadPolicy;
- struct sched_param oldThreadParam, newThreadParam;
- 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 = sched_get_priority_min (newThreadPolicy);
-
- pthread_setschedparam (pthread_self(), newThreadPolicy, &newThreadParam);
- }
-
- // AsyncSpinners are created with the reduced priority
- ros.spinner->start ();
-
- // Switch the priority of the parent thread (this thread) back to real-time.
- pthread_setschedparam (pthread_self(), oldThreadPolicy, &oldThreadParam);
- }
- else
- {
- if (!ros.mtSpinner && createMultiThreadedSpinner)
- {
- // Seems not to be used.
- // If we need to reduce its threads priority, it needs to be done before calling the MultiThreadedSpinner::spin() method
- ros.mtSpinner = boost::make_shared<ros::MultiThreadedSpinner>(4);
- }
- }
- return *ros.nodeHandle;
+ ros.nodeHandle = boost::make_shared<ros::NodeHandle>("");
}
+ if (!ros.spinner && createAsyncSpinner) {
+ ros.spinner = boost::make_shared<ros::AsyncSpinner>(4);
- ros::AsyncSpinner& spinner ()
- {
- if (!ros.spinner)
- throw std::runtime_error ("spinner has not been created");
- return *ros.spinner;
- }
+ // Change the thread's scheduler from real-time to normal and reduce its
+ // priority
+ int oldThreadPolicy, newThreadPolicy;
+ struct sched_param oldThreadParam, newThreadParam;
+ 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 = sched_get_priority_min(newThreadPolicy);
+
+ pthread_setschedparam(pthread_self(), newThreadPolicy, &newThreadParam);
+ }
+
+ // AsyncSpinners are created with the reduced priority
+ ros.spinner->start();
- ros::MultiThreadedSpinner& mtSpinner ()
- {
- if (!ros.mtSpinner)
- throw std::runtime_error ("spinner has not been created");
- return *ros.mtSpinner;
+ // Switch the priority of the parent thread (this thread) back to real-time.
+ pthread_setschedparam(pthread_self(), oldThreadPolicy, &oldThreadParam);
+ } else {
+ if (!ros.mtSpinner && createMultiThreadedSpinner) {
+ // Seems not to be used.
+ // If we need to reduce its threads priority, it needs to be done before
+ // calling the MultiThreadedSpinner::spin() method
+ ros.mtSpinner = boost::make_shared<ros::MultiThreadedSpinner>(4);
+ }
}
+ return *ros.nodeHandle;
+}
+
+ros::AsyncSpinner& spinner() {
+ if (!ros.spinner) throw std::runtime_error("spinner has not been created");
+ return *ros.spinner;
+}
+
+ros::MultiThreadedSpinner& mtSpinner() {
+ if (!ros.mtSpinner) throw std::runtime_error("spinner has not been created");
+ return *ros.mtSpinner;
+}
-} // end of namespace dynamicgraph.
+} // end of namespace dynamicgraph.
diff --git a/src/ros_interpreter.cpp b/src/ros_interpreter.cpp
index c4d58fd..1b38851 100644
--- a/src/ros_interpreter.cpp
+++ b/src/ros_interpreter.cpp
@@ -1,60 +1,49 @@
#include "dynamic_graph_bridge/ros_interpreter.hh"
-namespace dynamicgraph
-{
- static const int queueSize = 5;
-
- Interpreter::Interpreter (ros::NodeHandle& nodeHandle)
- : interpreter_ (),
- nodeHandle_ (nodeHandle),
- runCommandSrv_ (),
- runPythonFileSrv_ ()
- {
- }
-
- void Interpreter::startRosService ()
- {
- 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);
- }
-
- 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)
- {
- 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)
- {
- interpreter_.python(command, result, out, err);
- }
-
- void Interpreter::runPythonFile( std::string ifilename ){
- interpreter_.runPythonFile(ifilename);
- }
-
-} // end of namespace dynamicgraph.
-
+namespace dynamicgraph {
+static const int queueSize = 5;
+
+Interpreter::Interpreter(ros::NodeHandle& nodeHandle)
+ : interpreter_(),
+ nodeHandle_(nodeHandle),
+ runCommandSrv_(),
+ runPythonFileSrv_() {}
+
+void Interpreter::startRosService() {
+ 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);
+}
+
+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) {
+ 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) {
+ interpreter_.python(command, result, out, err);
+}
+
+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 20f865f..cbb1dd8 100644
--- a/src/ros_publish.cpp
+++ b/src/ros_publish.cpp
@@ -21,292 +21,229 @@
#define ENABLE_RT_LOG
#include <dynamic-graph/real-time-logger.h>
-namespace dynamicgraph
-{
-
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish");
- 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)
- {}
-
- Value Add::doExecute ()
- {
- RosPublish& entity =
- static_cast<RosPublish&> (owner ());
- std::vector<Value> values = getParameterValues ();
-
- const std::string& type = values[0].value ();
- const std::string& signal = values[1].value ();
- const std::string& topic = values[2].value ();
-
- if (type == "boolean")
- entity.add<bool> (signal, topic);
- else if (type == "double")
- entity.add<double> (signal, topic);
- else if (type == "unsigned")
- entity.add<unsigned int> (signal, topic);
- else if (type == "int")
- entity.add<int> (signal, topic);
- else if (type == "matrix")
- entity.add<Matrix> (signal, topic);
- else if (type == "vector")
- entity.add<Vector> (signal, topic);
- else if (type == "vector3")
- entity.add<specific::Vector3> (signal, topic);
- else if (type == "vector3Stamped")
- entity.add<std::pair<specific::Vector3, Vector> > (signal, topic);
- else if (type == "matrixHomo")
- entity.add<sot::MatrixHomogeneous> (signal, topic);
- else if (type == "matrixHomoStamped")
- entity.add<std::pair<sot::MatrixHomogeneous, Vector> >
- (signal, topic);
- else if (type == "twist")
- entity.add<specific::Twist> (signal, topic);
- else if (type == "twistStamped")
- entity.add<std::pair<specific::Twist, Vector> > (signal, topic);
- else if (type == "string")
- entity.add<std::string> (signal, topic);
- else
- throw std::runtime_error("bad type");
- return Value ();
- }
-
- Rm::Rm
- (RosPublish& entity, const std::string& docstring)
- : Command
- (entity,
- boost::assign::list_of (Value::STRING),
- docstring)
- {
+namespace dynamicgraph {
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish");
+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) {}
+
+Value Add::doExecute() {
+ RosPublish& entity = static_cast<RosPublish&>(owner());
+ std::vector<Value> values = getParameterValues();
+
+ const std::string& type = values[0].value();
+ const std::string& signal = values[1].value();
+ const std::string& topic = values[2].value();
+
+ if (type == "boolean")
+ entity.add<bool>(signal, topic);
+ else if (type == "double")
+ entity.add<double>(signal, topic);
+ else if (type == "unsigned")
+ entity.add<unsigned int>(signal, topic);
+ else if (type == "int")
+ entity.add<int>(signal, topic);
+ else if (type == "matrix")
+ entity.add<Matrix>(signal, topic);
+ else if (type == "vector")
+ entity.add<Vector>(signal, topic);
+ else if (type == "vector3")
+ entity.add<specific::Vector3>(signal, topic);
+ else if (type == "vector3Stamped")
+ entity.add<std::pair<specific::Vector3, Vector> >(signal, topic);
+ else if (type == "matrixHomo")
+ entity.add<sot::MatrixHomogeneous>(signal, topic);
+ else if (type == "matrixHomoStamped")
+ entity.add<std::pair<sot::MatrixHomogeneous, Vector> >(signal, topic);
+ else if (type == "twist")
+ entity.add<specific::Twist>(signal, topic);
+ else if (type == "twistStamped")
+ entity.add<std::pair<specific::Twist, Vector> >(signal, topic);
+ else if (type == "string")
+ entity.add<std::string>(signal, topic);
+ else
+ throw std::runtime_error("bad type");
+ 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.
- 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 ();
- }
- } // end of errorEstimator.
- } // end of namespace command.
-
- const std::string RosPublish::docstring_
- ("Publish dynamic-graph signals as ROS topics.\n"
- "\n"
- " Use command \"add\" to publish a new ROS topic.\n");
-
- RosPublish::RosPublish (const std::string& n)
+const std::string RosPublish::docstring_(
+ "Publish dynamic-graph signals as ROS topics.\n"
+ "\n"
+ " Use command \"add\" to publish a new ROS topic.\n");
+
+RosPublish::RosPublish(const std::string& n)
: dynamicgraph::Entity(n),
// RosPublish do not use callback so do not create a useless spinner.
- nh_ (rosInit (false)),
- bindedSignal_ (),
- trigger_ (boost::bind (&RosPublish::trigger, this, _1, _2),
- sotNOSIGNAL,
- MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
- rate_ (0,10000000),
- nextPublication_ ()
- {
- aofs_.open("/tmp/ros_publish.txt");
- dgADD_OSTREAM_TO_RTLOG (aofs_);
-
- try {
- if (ros::Time::isSimTime())
- nextPublication_ = ros::Time::now ();
- else
- {
- clock_gettime(CLOCK_REALTIME,&nextPublicationRT_);
- }
- } catch (const std::exception& exc) {
- throw std::runtime_error ("Failed to call ros::Time::now ():" +
- std::string (exc.what ()));
+ nh_(rosInit(false)),
+ bindedSignal_(),
+ trigger_(boost::bind(&RosPublish::trigger, this, _1, _2), sotNOSIGNAL,
+ MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
+ rate_(0, 10000000),
+ nextPublication_() {
+ aofs_.open("/tmp/ros_publish.txt");
+ dgADD_OSTREAM_TO_RTLOG(aofs_);
+
+ try {
+ if (ros::Time::isSimTime())
+ nextPublication_ = ros::Time::now();
+ else {
+ clock_gettime(CLOCK_REALTIME, &nextPublicationRT_);
}
- signalRegistration (trigger_);
- trigger_.setNeedUpdateFromAllChildren (true);
+ } catch (const std::exception& exc) {
+ throw std::runtime_error("Failed to call ros::Time::now ():" +
+ std::string(exc.what()));
+ }
+ signalRegistration(trigger_);
+ trigger_.setNeedUpdateFromAllChildren(true);
- std::string docstring =
+ std::string docstring =
"\n"
" Add a signal writing data to a ROS topic\n"
"\n"
" Input:\n"
" - type: string among ['double', 'matrix', 'vector', 'vector3',\n"
- " 'vector3Stamped', 'matrixHomo', 'matrixHomoStamped',\n"
+ " 'vector3Stamped', 'matrixHomo', "
+ "'matrixHomoStamped',\n"
" 'twist', 'twistStamped'],\n"
" - signal: the signal name in dynamic-graph,\n"
" - topic: the topic name in ROS.\n"
"\n";
- addCommand ("add",
- new command::rosPublish::Add
- (*this, docstring));
- docstring =
+ 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"
+ " - 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 =
+ 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 =
+ 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));
- }
+ addCommand("list", new command::rosPublish::List(*this, docstring));
+}
- RosPublish::~RosPublish ()
- {
- aofs_.close();
- }
+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;
+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;
- return;
- }
+ if (signal == "trigger") {
+ std::cerr << "The trigger signal should not be removed. Aborting."
+ << std::endl;
+ return;
+ }
- //lock the mutex to avoid deleting the signal during a call to trigger
- boost::mutex::scoped_lock lock (mutex_);
+ // lock the mutex to avoid deleting the signal during a call to trigger
+ boost::mutex::scoped_lock lock(mutex_);
+
+ signalDeregistration(signal);
+ bindedSignal_.erase(signal);
+}
- signalDeregistration(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 += "]";
+ return result;
+}
- 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 += "]";
- return result;
- }
-
- void RosPublish::clear ()
- {
- std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
- for(; it!= bindedSignal_.end(); )
- {
- if (it->first != "trigger")
- {
- rm(it->first);
- it = bindedSignal_.begin();
- }
- else
- {
- ++it;
- }
+void RosPublish::clear() {
+ std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
+ for (; it != bindedSignal_.end();) {
+ if (it->first != "trigger") {
+ rm(it->first);
+ it = bindedSignal_.begin();
+ } else {
+ ++it;
}
}
+}
- int& RosPublish::trigger (int& dummy, int t)
- {
-
- typedef std::map<std::string, bindedSignal_t>::iterator iterator_t;
- ros::Time aTime;
- if (ros::Time::isSimTime())
- {
- aTime= ros::Time::now();
- if (aTime <= nextPublication_)
- return dummy;
-
- nextPublication_ = aTime + rate_;
- }
- else
- {
- struct timespec aTimeRT;
- clock_gettime(CLOCK_REALTIME,&aTimeRT);
- nextPublicationRT_.tv_sec = aTimeRT.tv_sec + rate_.sec;
- nextPublicationRT_.tv_nsec = aTimeRT.tv_nsec + rate_.nsec;
- if (nextPublicationRT_.tv_nsec>1000000000)
- {
- nextPublicationRT_.tv_nsec-=1000000000;
- nextPublicationRT_.tv_sec+=1;
- }
- }
-
- boost::mutex::scoped_lock lock (mutex_);
-
- for (iterator_t it = bindedSignal_.begin ();
- it != bindedSignal_.end (); ++it)
- {
- boost::get<1>(it->second) (t);
- }
- return dummy;
+int& RosPublish::trigger(int& dummy, int t) {
+ typedef std::map<std::string, bindedSignal_t>::iterator iterator_t;
+ ros::Time aTime;
+ if (ros::Time::isSimTime()) {
+ aTime = ros::Time::now();
+ if (aTime <= nextPublication_) return dummy;
+
+ nextPublication_ = aTime + rate_;
+ } else {
+ struct timespec aTimeRT;
+ clock_gettime(CLOCK_REALTIME, &aTimeRT);
+ nextPublicationRT_.tv_sec = aTimeRT.tv_sec + rate_.sec;
+ nextPublicationRT_.tv_nsec = aTimeRT.tv_nsec + rate_.nsec;
+ if (nextPublicationRT_.tv_nsec > 1000000000) {
+ nextPublicationRT_.tv_nsec -= 1000000000;
+ nextPublicationRT_.tv_sec += 1;
+ }
}
- std::string RosPublish::getDocString () const
- {
- return docstring_;
+ boost::mutex::scoped_lock lock(mutex_);
+
+ for (iterator_t it = bindedSignal_.begin(); it != bindedSignal_.end(); ++it) {
+ boost::get<1>(it->second)(t);
}
+ return dummy;
+}
+
+std::string RosPublish::getDocString() const { return docstring_; }
-} // end of namespace dynamicgraph.
+} // end of namespace dynamicgraph.
diff --git a/src/ros_publish.hh b/src/ros_publish.hh
index b946d5a..5898072 100644
--- a/src/ros_publish.hh
+++ b/src/ros_publish.hh
@@ -1,105 +1,96 @@
#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/entity.h>
-# include <dynamic-graph/signal-time-dependent.h>
-# include <dynamic-graph/command.h>
-
-# include <ros/ros.h>
-
-# include <realtime_tools/realtime_publisher.h>
-
-# include "converter.hh"
-# include "sot_to_ros.hh"
-# include <fstream>
-
-namespace dynamicgraph
-{
- class RosPublish;
-
- namespace command
- {
- namespace rosPublish
- {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
-
-# define ROS_PUBLISH_MAKE_COMMAND(CMD) \
- class CMD : public Command \
- { \
- public: \
- CMD (RosPublish& entity, \
- const std::string& docstring); \
- virtual Value doExecute (); \
- }
-
- ROS_PUBLISH_MAKE_COMMAND(Add);
- ROS_PUBLISH_MAKE_COMMAND(Clear);
- ROS_PUBLISH_MAKE_COMMAND(List);
- ROS_PUBLISH_MAKE_COMMAND(Rm);
+#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/entity.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <dynamic-graph/command.h>
+
+#include <ros/ros.h>
+
+#include <realtime_tools/realtime_publisher.h>
+
+#include "converter.hh"
+#include "sot_to_ros.hh"
+#include <fstream>
+
+namespace dynamicgraph {
+class RosPublish;
+
+namespace command {
+namespace rosPublish {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+
+#define ROS_PUBLISH_MAKE_COMMAND(CMD) \
+ class CMD : public Command { \
+ public: \
+ CMD(RosPublish& entity, const std::string& docstring); \
+ virtual Value doExecute(); \
+ }
+
+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
- } // end of namespace errorEstimator.
- } // end of namespace command.
-
-
- /// \brief Publish dynamic-graph information into ROS.
- class RosPublish : public dynamicgraph::Entity
- {
- DYNAMIC_GRAPH_ENTITY_DECL();
- public:
- typedef boost::function<void (int)> callback_t;
-
- typedef boost::tuple<
- boost::shared_ptr<dynamicgraph::SignalBase<int> >,
- callback_t>
- bindedSignal_t;
-
- static const double ROS_JOINT_STATE_PUBLISHER_RATE;
-
- RosPublish (const std::string& n);
- virtual ~RosPublish ();
-
- virtual std::string getDocString () const;
- void display (std::ostream& os) const;
-
- void add (const std::string& signal, const std::string& topic);
- void rm (const std::string& signal);
- std::string list () 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);
-
- template <typename T>
- void add (const std::string& signal, const std::string& topic);
-
- private:
- static const std::string docstring_;
- ros::NodeHandle& nh_;
- std::map<std::string, bindedSignal_t> bindedSignal_;
- dynamicgraph::SignalTimeDependent<int,int> trigger_;
- ros::Duration rate_;
- ros::Time nextPublication_;
- boost::mutex mutex_;
- std::ofstream aofs_;
- struct timespec nextPublicationRT_;
- };
-} // end of namespace dynamicgraph.
-
-# include "ros_publish.hxx"
-#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HH
+} // namespace rosPublish
+} // end of namespace command.
+
+/// \brief Publish dynamic-graph information into ROS.
+class RosPublish : public dynamicgraph::Entity {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+ typedef boost::function<void(int)> callback_t;
+
+ typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+ callback_t>
+ bindedSignal_t;
+
+ static const double ROS_JOINT_STATE_PUBLISHER_RATE;
+
+ RosPublish(const std::string& n);
+ virtual ~RosPublish();
+
+ virtual std::string getDocString() const;
+ void display(std::ostream& os) const;
+
+ void add(const std::string& signal, const std::string& topic);
+ void rm(const std::string& signal);
+ std::string list() 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);
+
+ template <typename T>
+ void add(const std::string& signal, const std::string& topic);
+
+ private:
+ static const std::string docstring_;
+ ros::NodeHandle& nh_;
+ std::map<std::string, bindedSignal_t> bindedSignal_;
+ dynamicgraph::SignalTimeDependent<int, int> trigger_;
+ ros::Duration rate_;
+ ros::Time nextPublication_;
+ boost::mutex mutex_;
+ std::ofstream aofs_;
+ struct timespec nextPublicationRT_;
+};
+} // end of namespace dynamicgraph.
+
+#include "ros_publish.hxx"
+#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HH
diff --git a/src/ros_queued_subscribe.cpp b/src/ros_queued_subscribe.cpp
index d5463ac..997a594 100644
--- a/src/ros_queued_subscribe.cpp
+++ b/src/ros_queued_subscribe.cpp
@@ -32,175 +32,124 @@
#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)
- {}
-
- Value Add::doExecute ()
- {
- RosQueuedSubscribe& entity =
- static_cast<RosQueuedSubscribe&> (owner ());
- std::vector<Value> values = getParameterValues ();
-
- const std::string& type = values[0].value ();
- const std::string& signal = values[1].value ();
- const std::string& topic = values[2].value ();
-
- if (type == "double")
- entity.add<double> (type, signal, topic);
- else if (type == "unsigned")
- entity.add<unsigned int> (type, signal, topic);
- else if (type == "matrix")
- entity.add<dg::Matrix> (type, signal, topic);
- else if (type == "vector")
- entity.add<dg::Vector> (type, signal, topic);
- else if (type == "vector3")
- entity.add<specific::Vector3> (type, signal, topic);
- else if (type == "matrixHomo")
- entity.add<sot::MatrixHomogeneous> (type, signal, topic);
- else if (type == "twist")
- entity.add<specific::Twist> (type, signal, topic);
- else
- 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 ();
- }
- } // end of errorEstimator.
- } // end of namespace command.
-
- const std::string RosQueuedSubscribe::docstring_
- ("Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n"
- "\n"
- " Use command \"add\" to subscribe to a new signal.\n");
-
- RosQueuedSubscribe::RosQueuedSubscribe (const std::string& n)
+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) {}
+
+Value Add::doExecute() {
+ RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
+ std::vector<Value> values = getParameterValues();
+
+ const std::string& type = values[0].value();
+ const std::string& signal = values[1].value();
+ const std::string& topic = values[2].value();
+
+ if (type == "double")
+ entity.add<double>(type, signal, topic);
+ else if (type == "unsigned")
+ entity.add<unsigned int>(type, signal, topic);
+ else if (type == "matrix")
+ entity.add<dg::Matrix>(type, signal, topic);
+ else if (type == "vector")
+ entity.add<dg::Vector>(type, signal, topic);
+ else if (type == "vector3")
+ entity.add<specific::Vector3>(type, signal, topic);
+ else if (type == "matrixHomo")
+ entity.add<sot::MatrixHomogeneous>(type, signal, topic);
+ else if (type == "twist")
+ entity.add<specific::Twist>(type, signal, topic);
+ else
+ 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.
+
+const std::string RosQueuedSubscribe::docstring_(
+ "Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n"
+ "\n"
+ " Use command \"add\" to subscribe to a new signal.\n");
+
+RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n)
: dynamicgraph::Entity(n),
- nh_ (rosInit (true)),
- bindedSignal_ (),
- readQueue_ (-1)
- {
- std::string docstring =
+ nh_(rosInit(true)),
+ bindedSignal_(),
+ readQueue_(-1) {
+ std::string docstring =
"\n"
" Add a signal reading data from a ROS topic\n"
"\n"
@@ -210,143 +159,119 @@ namespace dynamicgraph
" - signal: the signal name in dynamic-graph,\n"
" - topic: the topic name in ROS.\n"
"\n";
- addCommand ("add",
- new command::rosQueuedSubscribe::Add
- (*this, docstring));
- docstring =
+ 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"
+ " - 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 =
+ 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 =
+ 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 =
+ 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 =
+ 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 =
+ 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));
- }
+ addCommand("readQueue",
+ new command::rosQueuedSubscribe::ReadQueue(*this, docstring));
+}
- RosQueuedSubscribe::~RosQueuedSubscribe ()
- {}
+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";
+void RosQueuedSubscribe::rm(const std::string& signal) {
+ std::string signalTs = signal + "Timestamp";
- signalDeregistration(signal);
- bindedSignal_.erase (signal);
+ signalDeregistration(signal);
+ bindedSignal_.erase(signal);
- if(bindedSignal_.find(signalTs) != bindedSignal_.end())
- {
- signalDeregistration(signalTs);
- bindedSignal_.erase(signalTs);
- }
+ if (bindedSignal_.find(signalTs) != bindedSignal_.end()) {
+ signalDeregistration(signalTs);
+ bindedSignal_.erase(signalTs);
}
-
- std::string 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 += "]";
- return result;
+}
+
+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 + "',";
}
-
- void RosQueuedSubscribe::clear ()
- {
- std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
- for(; it!= bindedSignal_.end(); )
- {
- rm(it->first);
- it = bindedSignal_.begin();
- }
+ result += "]";
+ return result;
+}
+
+void RosQueuedSubscribe::clear() {
+ std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
+ for (; it != bindedSignal_.end();) {
+ rm(it->first);
+ it = bindedSignal_.begin();
}
+}
- void RosQueuedSubscribe::clearQueue (const std::string& signal)
- {
- if(bindedSignal_.find(signal) != bindedSignal_.end())
- {
- bindedSignal_[signal]->clear();
- }
+void RosQueuedSubscribe::clearQueue(const std::string& signal) {
+ if (bindedSignal_.find(signal) != bindedSignal_.end()) {
+ bindedSignal_[signal]->clear();
}
+}
- std::size_t RosQueuedSubscribe::queueSize (const std::string& signal) const
- {
- std::map<std::string, bindedSignal_t>::const_iterator _bs =
+std::size_t RosQueuedSubscribe::queueSize(const std::string& signal) const {
+ std::map<std::string, bindedSignal_t>::const_iterator _bs =
bindedSignal_.find(signal);
- if(_bs != bindedSignal_.end())
- {
- return _bs->second->size();
- }
- return -1;
- }
-
- void RosQueuedSubscribe::readQueue (int beginReadingAt)
- {
- // Prints signal queues sizes
- /*for (std::map<std::string, bindedSignal_t>::const_iterator it =
- bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
- std::cout << it->first << " : " << it->second->size() << '\n';
- }*/
- readQueue_ = beginReadingAt;
- }
-
- std::string RosQueuedSubscribe::getDocString () const
- {
- return docstring_;
+ if (_bs != bindedSignal_.end()) {
+ return _bs->second->size();
}
-} // end of namespace dynamicgraph.
+ return -1;
+}
+
+void RosQueuedSubscribe::readQueue(int beginReadingAt) {
+ // Prints signal queues sizes
+ /*for (std::map<std::string, bindedSignal_t>::const_iterator it =
+ bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
+ std::cout << it->first << " : " << it->second->size() << '\n';
+ }*/
+ readQueue_ = beginReadingAt;
+}
+
+std::string RosQueuedSubscribe::getDocString() const { return docstring_; }
+} // end of namespace dynamicgraph.
diff --git a/src/ros_queued_subscribe.hh b/src/ros_queued_subscribe.hh
index 755202f..7fb3345 100644
--- a/src/ros_queued_subscribe.hh
+++ b/src/ros_queued_subscribe.hh
@@ -18,207 +18,187 @@
// <http://www.gnu.org/licenses/>.
#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
-# define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
-# include <map>
-
-# 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 <sot/core/matrix-geometry.hh>
-
-# include <ros/ros.h>
-
-# include "converter.hh"
-# include "sot_to_ros.hh"
-
-namespace dynamicgraph
-{
- class RosQueuedSubscribe;
-
- namespace command
- {
- namespace rosQueuedSubscribe
- {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
-
-# define ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(CMD) \
- class CMD : public Command \
- { \
- public: \
- CMD (RosQueuedSubscribe& entity, \
- const std::string& docstring); \
- virtual Value doExecute (); \
- }
-
- 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);
+#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
+#include <map>
+
+#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 <sot/core/matrix-geometry.hh>
+
+#include <ros/ros.h>
+
+#include "converter.hh"
+#include "sot_to_ros.hh"
+
+namespace dynamicgraph {
+class RosQueuedSubscribe;
+
+namespace command {
+namespace rosQueuedSubscribe {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+
+#define ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(CMD) \
+ class CMD : public Command { \
+ public: \
+ CMD(RosQueuedSubscribe& entity, const std::string& docstring); \
+ virtual Value doExecute(); \
+ }
+
+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
- } // end of namespace rosQueuedSubscribe.
- } // end of namespace command.
-
- class RosQueuedSubscribe;
-
- namespace internal
- {
- template <typename T>
- struct Add;
-
- struct BindedSignalBase {
- typedef boost::shared_ptr<ros::Subscriber> Subscriber_t;
-
- BindedSignalBase(RosQueuedSubscribe* e) : entity(e) {}
- virtual ~BindedSignalBase() {}
-
- virtual void clear () = 0;
- virtual std::size_t size () const = 0;
-
- Subscriber_t subscriber;
- RosQueuedSubscribe* entity;
- };
-
- template <typename T, int BufferSize>
- struct BindedSignal : BindedSignalBase {
- typedef dynamicgraph::Signal<T, int> Signal_t;
- typedef boost::shared_ptr<Signal_t> SignalPtr_t;
- typedef std::vector<T> buffer_t;
- typedef typename buffer_t::size_type size_type;
-
- BindedSignal(RosQueuedSubscribe* e)
- : BindedSignalBase (e)
- , frontIdx(0)
- , backIdx(0)
- , buffer (BufferSize)
- , init(false)
- {}
- ~BindedSignal()
- {
- signal.reset();
- clear();
- }
-
- /// See comments in reader and writer for details about synchronisation.
- void clear ()
- {
- // synchronize with method writer
- wmutex.lock();
- if (!empty()) {
- if (backIdx == 0)
- last = buffer[BufferSize-1];
- else
- last = buffer[backIdx-1];
- }
- // synchronize with method reader
- rmutex.lock();
- frontIdx = backIdx = 0;
- rmutex.unlock();
- wmutex.unlock();
- }
-
- bool empty () const
- {
- return frontIdx == backIdx;
- }
-
- bool full () const
- {
- return ((backIdx + 1) % BufferSize) == frontIdx;
- }
-
- size_type size () const
- {
- if (frontIdx <= backIdx)
- return backIdx - frontIdx;
- else
- return backIdx + BufferSize - frontIdx;
- }
-
- SignalPtr_t signal;
- /// Index of the next value to be read.
- size_type frontIdx;
- /// Index of the slot where to write next value (does not contain valid data).
- size_type backIdx;
- buffer_t buffer;
- boost::mutex wmutex, rmutex;
- T last;
- bool init;
-
- template <typename R> void writer (const R& data);
- T& reader (T& val, int time);
- };
- } // end of internal namespace.
-
-
- /// \brief Publish ROS information in the dynamic-graph.
- class RosQueuedSubscribe : public dynamicgraph::Entity
- {
- DYNAMIC_GRAPH_ENTITY_DECL();
- typedef boost::posix_time::ptime ptime;
- public:
- typedef boost::shared_ptr<internal::BindedSignalBase> bindedSignal_t;
-
- RosQueuedSubscribe (const std::string& n);
- virtual ~RosQueuedSubscribe ();
-
- virtual std::string getDocString () const;
- void display (std::ostream& os) const;
-
- void rm (const std::string& signal);
- std::string list ();
- 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);
-
- std::map<std::string, bindedSignal_t>&
- bindedSignal ()
- {
- return bindedSignal_;
+} // end of namespace rosQueuedSubscribe.
+} // end of namespace command.
+
+class RosQueuedSubscribe;
+
+namespace internal {
+template <typename T>
+struct Add;
+
+struct BindedSignalBase {
+ typedef boost::shared_ptr<ros::Subscriber> Subscriber_t;
+
+ BindedSignalBase(RosQueuedSubscribe* e) : entity(e) {}
+ virtual ~BindedSignalBase() {}
+
+ virtual void clear() = 0;
+ virtual std::size_t size() const = 0;
+
+ Subscriber_t subscriber;
+ RosQueuedSubscribe* entity;
+};
+
+template <typename T, int BufferSize>
+struct BindedSignal : BindedSignalBase {
+ typedef dynamicgraph::Signal<T, int> Signal_t;
+ typedef boost::shared_ptr<Signal_t> SignalPtr_t;
+ typedef std::vector<T> buffer_t;
+ typedef typename buffer_t::size_type size_type;
+
+ BindedSignal(RosQueuedSubscribe* e)
+ : BindedSignalBase(e),
+ frontIdx(0),
+ backIdx(0),
+ buffer(BufferSize),
+ init(false) {}
+ ~BindedSignal() {
+ signal.reset();
+ clear();
+ }
+
+ /// See comments in reader and writer for details about synchronisation.
+ void clear() {
+ // synchronize with method writer
+ wmutex.lock();
+ if (!empty()) {
+ if (backIdx == 0)
+ last = buffer[BufferSize - 1];
+ else
+ last = buffer[backIdx - 1];
}
-
- ros::NodeHandle& nh ()
- {
- return nh_;
- }
-
- template <typename R, typename S>
- void callback
- (boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
- const R& data);
-
- template <typename R>
- void callbackTimestamp
- (boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
- const R& data);
-
- template <typename T>
- friend class internal::Add;
- private:
- static const std::string docstring_;
- ros::NodeHandle& nh_;
- std::map<std::string, bindedSignal_t> bindedSignal_;
-
- int readQueue_;
- // Signal<bool, int> readQueue_;
-
- template <typename T>
- friend class internal::BindedSignal;
- };
-} // end of namespace dynamicgraph.
-
-# include "ros_queued_subscribe.hxx"
-#endif //! DYNAMIC_GRAPH_QUEUED_ROS_SUBSCRIBE_HH
+ // synchronize with method reader
+ rmutex.lock();
+ frontIdx = backIdx = 0;
+ rmutex.unlock();
+ wmutex.unlock();
+ }
+
+ bool empty() const { return frontIdx == backIdx; }
+
+ bool full() const { return ((backIdx + 1) % BufferSize) == frontIdx; }
+
+ size_type size() const {
+ if (frontIdx <= backIdx)
+ return backIdx - frontIdx;
+ else
+ return backIdx + BufferSize - frontIdx;
+ }
+
+ SignalPtr_t signal;
+ /// Index of the next value to be read.
+ size_type frontIdx;
+ /// Index of the slot where to write next value (does not contain valid data).
+ size_type backIdx;
+ buffer_t buffer;
+ boost::mutex wmutex, rmutex;
+ T last;
+ bool init;
+
+ template <typename R>
+ void writer(const R& data);
+ T& reader(T& val, int time);
+};
+} // namespace internal
+
+/// \brief Publish ROS information in the dynamic-graph.
+class RosQueuedSubscribe : public dynamicgraph::Entity {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+ typedef boost::posix_time::ptime ptime;
+
+ public:
+ typedef boost::shared_ptr<internal::BindedSignalBase> bindedSignal_t;
+
+ RosQueuedSubscribe(const std::string& n);
+ virtual ~RosQueuedSubscribe();
+
+ virtual std::string getDocString() const;
+ void display(std::ostream& os) const;
+
+ void rm(const std::string& signal);
+ std::string list();
+ 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);
+
+ std::map<std::string, bindedSignal_t>& bindedSignal() {
+ return bindedSignal_;
+ }
+
+ ros::NodeHandle& nh() { return nh_; }
+
+ template <typename R, typename S>
+ void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
+ const R& data);
+
+ template <typename R>
+ void callbackTimestamp(
+ boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
+ const R& data);
+
+ template <typename T>
+ friend class internal::Add;
+
+ private:
+ static const std::string docstring_;
+ ros::NodeHandle& nh_;
+ std::map<std::string, bindedSignal_t> bindedSignal_;
+
+ int readQueue_;
+ // Signal<bool, int> readQueue_;
+
+ template <typename T>
+ friend class internal::BindedSignal;
+};
+} // end of namespace dynamicgraph.
+
+#include "ros_queued_subscribe.hxx"
+#endif //! DYNAMIC_GRAPH_QUEUED_ROS_SUBSCRIBE_HH
diff --git a/src/ros_subscribe.cpp b/src/ros_subscribe.cpp
index cab1457..113ec2d 100644
--- a/src/ros_subscribe.cpp
+++ b/src/ros_subscribe.cpp
@@ -13,213 +13,164 @@
#include "dynamic_graph_bridge/ros_init.hh"
#include "ros_subscribe.hh"
-namespace dynamicgraph
-{
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
-
- namespace command
- {
- namespace rosSubscribe
- {
- Clear::Clear
- (RosSubscribe& entity, const std::string& docstring)
- : Command
- (entity,
- std::vector<Value::Type> (),
- docstring)
- {}
-
- Value Clear::doExecute ()
- {
- RosSubscribe& entity =
- static_cast<RosSubscribe&> (owner ());
-
- entity.clear ();
- return Value ();
- }
-
- List::List
- (RosSubscribe& entity, const std::string& docstring)
- : Command
- (entity,
- std::vector<Value::Type> (),
- docstring)
- {}
-
- Value List::doExecute ()
- {
- RosSubscribe& entity =
- static_cast<RosSubscribe&> (owner ());
- return Value (entity.list ());
- }
-
- Add::Add
- (RosSubscribe& entity, const std::string& docstring)
- : Command
- (entity,
- boost::assign::list_of
- (Value::STRING) (Value::STRING) (Value::STRING),
- docstring)
- {}
-
- Value Add::doExecute ()
- {
- RosSubscribe& entity =
- static_cast<RosSubscribe&> (owner ());
- std::vector<Value> values = getParameterValues ();
-
- const std::string& type = values[0].value ();
- const std::string& signal = values[1].value ();
- const std::string& topic = values[2].value ();
-
- if (type == "double")
- entity.add<double> (signal, topic);
- else if (type == "unsigned")
- entity.add<unsigned int> (signal, topic);
- else if (type == "matrix")
- entity.add<dg::Matrix> (signal, topic);
- else if (type == "vector")
- entity.add<dg::Vector> (signal, topic);
- else if (type == "vector3")
- entity.add<specific::Vector3> (signal, topic);
- else if (type == "vector3Stamped")
- entity.add<std::pair<specific::Vector3, dg::Vector> > (signal, topic);
- else if (type == "matrixHomo")
- entity.add<sot::MatrixHomogeneous> (signal, topic);
- else if (type == "matrixHomoStamped")
- entity.add<std::pair<sot::MatrixHomogeneous, dg::Vector> >
- (signal, topic);
- else if (type == "twist")
- entity.add<specific::Twist> (signal, topic);
- else if (type == "twistStamped")
- entity.add<std::pair<specific::Twist, dg::Vector> >
- (signal, topic);
- else if (type == "string")
- entity.add<std::string> (signal, topic);
- else
- throw std::runtime_error("bad type");
- return Value ();
- }
-
- Rm::Rm
- (RosSubscribe& entity, const std::string& docstring)
- : Command
- (entity,
- boost::assign::list_of (Value::STRING),
- docstring)
- {}
-
- Value Rm::doExecute ()
- {
- RosSubscribe& entity =
- static_cast<RosSubscribe&> (owner ());
- std::vector<Value> values = getParameterValues ();
- const std::string& signal = values[0].value ();
- entity.rm (signal);
- return Value ();
- }
- } // end of errorEstimator.
- } // end of namespace command.
-
- const std::string RosSubscribe::docstring_
- ("Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n"
- "\n"
- " Use command \"add\" to subscribe to a new signal.\n");
-
- RosSubscribe::RosSubscribe (const std::string& n)
- : dynamicgraph::Entity(n),
- nh_ (rosInit (true)),
- bindedSignal_ ()
- {
- std::string docstring =
+namespace dynamicgraph {
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
+
+namespace command {
+namespace rosSubscribe {
+Clear::Clear(RosSubscribe& entity, const std::string& docstring)
+ : Command(entity, std::vector<Value::Type>(), docstring) {}
+
+Value Clear::doExecute() {
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+
+ entity.clear();
+ return Value();
+}
+
+List::List(RosSubscribe& entity, const std::string& docstring)
+ : Command(entity, std::vector<Value::Type>(), docstring) {}
+
+Value List::doExecute() {
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+ return Value(entity.list());
+}
+
+Add::Add(RosSubscribe& entity, const std::string& docstring)
+ : Command(
+ entity,
+ boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
+ docstring) {}
+
+Value Add::doExecute() {
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+ std::vector<Value> values = getParameterValues();
+
+ const std::string& type = values[0].value();
+ const std::string& signal = values[1].value();
+ const std::string& topic = values[2].value();
+
+ if (type == "double")
+ entity.add<double>(signal, topic);
+ else if (type == "unsigned")
+ entity.add<unsigned int>(signal, topic);
+ else if (type == "matrix")
+ entity.add<dg::Matrix>(signal, topic);
+ else if (type == "vector")
+ entity.add<dg::Vector>(signal, topic);
+ else if (type == "vector3")
+ entity.add<specific::Vector3>(signal, topic);
+ else if (type == "vector3Stamped")
+ entity.add<std::pair<specific::Vector3, dg::Vector> >(signal, topic);
+ else if (type == "matrixHomo")
+ entity.add<sot::MatrixHomogeneous>(signal, topic);
+ else if (type == "matrixHomoStamped")
+ entity.add<std::pair<sot::MatrixHomogeneous, dg::Vector> >(signal, topic);
+ else if (type == "twist")
+ entity.add<specific::Twist>(signal, topic);
+ else if (type == "twistStamped")
+ entity.add<std::pair<specific::Twist, dg::Vector> >(signal, topic);
+ else if (type == "string")
+ entity.add<std::string>(signal, topic);
+ else
+ throw std::runtime_error("bad type");
+ return Value();
+}
+
+Rm::Rm(RosSubscribe& entity, const std::string& docstring)
+ : Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
+
+Value Rm::doExecute() {
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+ std::vector<Value> values = getParameterValues();
+ const std::string& signal = values[0].value();
+ entity.rm(signal);
+ return Value();
+}
+} // namespace rosSubscribe
+} // end of namespace command.
+
+const std::string RosSubscribe::docstring_(
+ "Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n"
+ "\n"
+ " Use command \"add\" to subscribe to a new signal.\n");
+
+RosSubscribe::RosSubscribe(const std::string& n)
+ : dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_() {
+ std::string docstring =
"\n"
" Add a signal reading data from a ROS topic\n"
"\n"
" Input:\n"
" - type: string among ['double', 'matrix', 'vector', 'vector3',\n"
- " 'vector3Stamped', 'matrixHomo', 'matrixHomoStamped',\n"
+ " 'vector3Stamped', 'matrixHomo', "
+ "'matrixHomoStamped',\n"
" 'twist', 'twistStamped'],\n"
" - signal: the signal name in dynamic-graph,\n"
" - topic: the topic name in ROS.\n"
"\n";
- addCommand ("add",
- new command::rosSubscribe::Add
- (*this, docstring));
- docstring =
+ 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"
+ " - 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 =
+ 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 =
+ addCommand("clear", new command::rosSubscribe::Clear(*this, docstring));
+ docstring =
"\n"
" List signals reading data from a ROS topic\n"
"\n"
" No input:\n"
"\n";
- addCommand ("list",
- new command::rosSubscribe::List
- (*this, docstring));
- }
+ addCommand("list", new command::rosSubscribe::List(*this, docstring));
+}
- RosSubscribe::~RosSubscribe ()
- {}
+RosSubscribe::~RosSubscribe() {}
- void RosSubscribe::display (std::ostream& os) const
- {
- os << CLASS_NAME << std::endl;
- }
+void RosSubscribe::display(std::ostream& os) const {
+ os << CLASS_NAME << std::endl;
+}
- void RosSubscribe::rm (const std::string& signal)
- {
- std::string signalTs = signal+"Timestamp";
+void RosSubscribe::rm(const std::string& signal) {
+ std::string signalTs = signal + "Timestamp";
- signalDeregistration(signal);
- bindedSignal_.erase (signal);
+ signalDeregistration(signal);
+ bindedSignal_.erase(signal);
- if(bindedSignal_.find(signalTs) != bindedSignal_.end())
- {
- signalDeregistration(signalTs);
- bindedSignal_.erase(signalTs);
- }
+ if (bindedSignal_.find(signalTs) != bindedSignal_.end()) {
+ signalDeregistration(signalTs);
+ bindedSignal_.erase(signalTs);
}
-
- std::string RosSubscribe::list ()
- {
- std::string result("[");
- for (std::map<std::string, bindedSignal_t>::const_iterator it =
- bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
- result += "'" + it->first + "',";
- }
- result += "]";
- return result;
+}
+
+std::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 + "',";
}
-
- void RosSubscribe::clear ()
- {
- std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
- for(; it!= bindedSignal_.end(); )
- {
- rm(it->first);
- it = bindedSignal_.begin();
- }
+ result += "]";
+ return result;
+}
+
+void RosSubscribe::clear() {
+ std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
+ for (; it != bindedSignal_.end();) {
+ rm(it->first);
+ it = bindedSignal_.begin();
}
+}
- std::string RosSubscribe::getDocString () const
- {
- return docstring_;
- }
-} // end of namespace dynamicgraph.
+std::string RosSubscribe::getDocString() const { return docstring_; }
+} // end of namespace dynamicgraph.
diff --git a/src/ros_subscribe.hh b/src/ros_subscribe.hh
index 9aa386d..35b9e59 100644
--- a/src/ros_subscribe.hh
+++ b/src/ros_subscribe.hh
@@ -1,110 +1,98 @@
#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
-# define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
-# include <map>
-
-# include <boost/shared_ptr.hpp>
-
-# include <dynamic-graph/entity.h>
-# include <dynamic-graph/signal-time-dependent.h>
-# include <dynamic-graph/signal-ptr.h>
-# include <dynamic-graph/command.h>
-# include <sot/core/matrix-geometry.hh>
-
-# include <ros/ros.h>
-
-# include "converter.hh"
-# include "sot_to_ros.hh"
-
-namespace dynamicgraph
-{
- class RosSubscribe;
-
- namespace command
- {
- namespace rosSubscribe
- {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
-
-# define ROS_SUBSCRIBE_MAKE_COMMAND(CMD) \
- class CMD : public Command \
- { \
- public: \
- CMD (RosSubscribe& entity, \
- const std::string& docstring); \
- virtual Value doExecute (); \
- }
-
- ROS_SUBSCRIBE_MAKE_COMMAND(Add);
- ROS_SUBSCRIBE_MAKE_COMMAND(Clear);
- ROS_SUBSCRIBE_MAKE_COMMAND(List);
- ROS_SUBSCRIBE_MAKE_COMMAND(Rm);
+#define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
+#include <map>
+
+#include <boost/shared_ptr.hpp>
+
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/command.h>
+#include <sot/core/matrix-geometry.hh>
+
+#include <ros/ros.h>
+
+#include "converter.hh"
+#include "sot_to_ros.hh"
+
+namespace dynamicgraph {
+class RosSubscribe;
+
+namespace command {
+namespace rosSubscribe {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+
+#define ROS_SUBSCRIBE_MAKE_COMMAND(CMD) \
+ class CMD : public Command { \
+ public: \
+ CMD(RosSubscribe& entity, const std::string& docstring); \
+ virtual Value doExecute(); \
+ }
+
+ROS_SUBSCRIBE_MAKE_COMMAND(Add);
+ROS_SUBSCRIBE_MAKE_COMMAND(Clear);
+ROS_SUBSCRIBE_MAKE_COMMAND(List);
+ROS_SUBSCRIBE_MAKE_COMMAND(Rm);
#undef ROS_SUBSCRIBE_MAKE_COMMAND
- } // end of namespace errorEstimator.
- } // end of namespace command.
+} // namespace rosSubscribe
+} // end of namespace command.
+namespace internal {
+template <typename T>
+struct Add;
+} // namespace internal
- namespace internal
- {
- template <typename T>
- struct Add;
- } // end of internal namespace.
+/// \brief Publish ROS information in the dynamic-graph.
+class RosSubscribe : public dynamicgraph::Entity {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+ typedef boost::posix_time::ptime ptime;
- /// \brief Publish ROS information in the dynamic-graph.
- class RosSubscribe : public dynamicgraph::Entity
- {
- DYNAMIC_GRAPH_ENTITY_DECL();
- typedef boost::posix_time::ptime ptime;
- public:
- typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
- boost::shared_ptr<ros::Subscriber> >
+ public:
+ typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+ boost::shared_ptr<ros::Subscriber> >
bindedSignal_t;
- RosSubscribe (const std::string& n);
- virtual ~RosSubscribe ();
-
- virtual std::string getDocString () const;
- void display (std::ostream& os) const;
-
- void add (const std::string& signal, const std::string& topic);
- void rm (const std::string& signal);
- std::string list ();
- void clear ();
-
- template <typename T>
- void add (const std::string& signal, const std::string& topic);
-
- std::map<std::string, bindedSignal_t>&
- bindedSignal ()
- {
- return bindedSignal_;
- }
-
- ros::NodeHandle& nh ()
- {
- return nh_;
- }
-
- template <typename R, typename S>
- void callback
- (boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
- const R& data);
-
- template <typename R>
- void callbackTimestamp
- (boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
- const R& data);
-
- template <typename T>
- friend class internal::Add;
- private:
- static const std::string docstring_;
- ros::NodeHandle& nh_;
- std::map<std::string, bindedSignal_t> bindedSignal_;
- };
-} // end of namespace dynamicgraph.
-
-# include "ros_subscribe.hxx"
-#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
+ RosSubscribe(const std::string& n);
+ virtual ~RosSubscribe();
+
+ virtual std::string getDocString() const;
+ void display(std::ostream& os) const;
+
+ void add(const std::string& signal, const std::string& topic);
+ void rm(const std::string& signal);
+ std::string list();
+ void clear();
+
+ template <typename T>
+ void add(const std::string& signal, const std::string& topic);
+
+ std::map<std::string, bindedSignal_t>& bindedSignal() {
+ return bindedSignal_;
+ }
+
+ ros::NodeHandle& nh() { return nh_; }
+
+ template <typename R, typename S>
+ void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
+ const R& data);
+
+ template <typename R>
+ void callbackTimestamp(
+ boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
+ const R& data);
+
+ template <typename T>
+ friend class internal::Add;
+
+ private:
+ static const std::string docstring_;
+ ros::NodeHandle& nh_;
+ std::map<std::string, bindedSignal_t> bindedSignal_;
+};
+} // end of namespace dynamicgraph.
+
+#include "ros_subscribe.hxx"
+#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
diff --git a/src/ros_tf_listener.cpp b/src/ros_tf_listener.cpp
index e852c22..da89ca3 100644
--- a/src/ros_tf_listener.cpp
+++ b/src/ros_tf_listener.cpp
@@ -3,30 +3,28 @@
#include <dynamic-graph/factory.h>
-namespace dynamicgraph
-{
- namespace internal
- {
- sot::MatrixHomogeneous& TransformListenerData::getTransform (sot::MatrixHomogeneous& res, int time)
- {
- static const ros::Time rosTime(0);
- try {
- listener.lookupTransform (toFrame, fromFrame, rosTime, transform);
- } catch (const tf::TransformException& ex) {
- res.setIdentity();
- std::ostringstream oss;
- oss << "Enable to get transform at time " << time << ": " << ex.what();
- entity->SEND_WARNING_STREAM_MSG(oss.str());
- 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];
- res.translation()[r] = transform.getOrigin()[r];
- }
- return res;
- }
+namespace dynamicgraph {
+namespace internal {
+sot::MatrixHomogeneous& TransformListenerData::getTransform(
+ sot::MatrixHomogeneous& res, int time) {
+ static const ros::Time rosTime(0);
+ try {
+ listener.lookupTransform(toFrame, fromFrame, rosTime, transform);
+ } catch (const tf::TransformException& ex) {
+ res.setIdentity();
+ std::ostringstream oss;
+ oss << "Enable to get transform at time " << time << ": " << ex.what();
+ entity->SEND_WARNING_STREAM_MSG(oss.str());
+ return res;
}
-
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTfListener, "RosTfListener");
+ for (int r = 0; r < 3; ++r) {
+ for (int c = 0; c < 3; ++c)
+ res.linear()(r, c) = transform.getBasis().getRow(r)[c];
+ res.translation()[r] = transform.getOrigin()[r];
+ }
+ return res;
}
+} // namespace internal
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTfListener, "RosTfListener");
+} // namespace dynamicgraph
diff --git a/src/ros_tf_listener.hh b/src/ros_tf_listener.hh
index acf1d76..6f0b457 100644
--- a/src/ros_tf_listener.hh
+++ b/src/ros_tf_listener.hh
@@ -1,95 +1,88 @@
#ifndef DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
-# define DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
+#define DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
-# include <boost/bind.hpp>
+#include <boost/bind.hpp>
-# include <tf/transform_listener.h>
+#include <tf/transform_listener.h>
-# include <dynamic-graph/entity.h>
-# include <dynamic-graph/signal.h>
-# include <dynamic-graph/command-bind.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal.h>
+#include <dynamic-graph/command-bind.h>
-# include <sot/core/matrix-geometry.hh>
+#include <sot/core/matrix-geometry.hh>
namespace dynamicgraph {
- class RosTfListener;
-
- namespace internal
- {
- struct TransformListenerData {
- typedef Signal<sot::MatrixHomogeneous, int> signal_t;
-
- RosTfListener* entity;
- tf::TransformListener& listener;
- const std::string toFrame, fromFrame;
- tf::StampedTransform transform;
- signal_t signal;
-
- 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));
- }
-
- sot::MatrixHomogeneous& getTransform (sot::MatrixHomogeneous& res, int time);
- };
- } // end of internal namespace.
-
- class RosTfListener : public Entity
- {
- DYNAMIC_GRAPH_ENTITY_DECL();
-
- public:
- typedef internal::TransformListenerData TransformListenerData;
-
- RosTfListener (const std::string& name) : Entity (name)
- {
- std::string docstring =
- "\n"
- " Add a signal containing the transform between two frames.\n"
- "\n"
- " Input:\n"
- " - to : frame name\n"
- " - from: frame name,\n"
- " - signalName: the signal name in dynamic-graph"
- "\n";
- addCommand ("add",
- command::makeCommandVoid3(*this, &RosTfListener::add, docstring));
- }
-
- ~RosTfListener ()
- {
- 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)
- {
- if (listenerDatas.find(signame) != listenerDatas.end())
- 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());
- signalRegistration (tld->signal);
- listenerDatas[signame] = tld;
- }
-
- private:
- typedef std::map<std::string, TransformListenerData*> Map_t;
- Map_t listenerDatas;
- tf::TransformListener listener;
- };
-} // end of namespace dynamicgraph.
-
-#endif // DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
+class RosTfListener;
+
+namespace internal {
+struct TransformListenerData {
+ typedef Signal<sot::MatrixHomogeneous, int> signal_t;
+
+ RosTfListener* entity;
+ tf::TransformListener& listener;
+ const std::string toFrame, fromFrame;
+ tf::StampedTransform transform;
+ signal_t signal;
+
+ 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));
+ }
+
+ sot::MatrixHomogeneous& getTransform(sot::MatrixHomogeneous& res, int time);
+};
+} // namespace internal
+
+class RosTfListener : public Entity {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+ typedef internal::TransformListenerData TransformListenerData;
+
+ RosTfListener(const std::string& name) : Entity(name) {
+ std::string docstring =
+ "\n"
+ " Add a signal containing the transform between two frames.\n"
+ "\n"
+ " Input:\n"
+ " - to : frame name\n"
+ " - from: frame name,\n"
+ " - signalName: the signal name in dynamic-graph"
+ "\n";
+ addCommand("add", command::makeCommandVoid3(*this, &RosTfListener::add,
+ docstring));
+ }
+
+ ~RosTfListener() {
+ 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) {
+ if (listenerDatas.find(signame) != listenerDatas.end())
+ 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());
+ signalRegistration(tld->signal);
+ listenerDatas[signame] = tld;
+ }
+
+ private:
+ typedef std::map<std::string, TransformListenerData*> Map_t;
+ Map_t listenerDatas;
+ tf::TransformListener listener;
+};
+} // end of namespace dynamicgraph.
+
+#endif // DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
diff --git a/src/ros_time.cpp b/src/ros_time.cpp
index 0d50559..278f7be 100644
--- a/src/ros_time.cpp
+++ b/src/ros_time.cpp
@@ -12,34 +12,28 @@
namespace dynamicgraph {
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTime, "RosTime");
-
- using namespace boost::posix_time;
-
- const std::string RosTime::docstring_
- ("Export ROS time into dynamic-graph.\n"
- "\n"
- " Signal \"time\" provides time as given by ros::time as\n"
- " boost::posix_time::ptime type.\n");
-
- RosTime::RosTime (const std::string& name) :
- 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));
- }
-
- ptime&
- RosTime::update (ptime& time, const int& )
- {
- time = rosTimeToPtime (ros::Time::now ());
- return time;
- }
-
- std::string RosTime::getDocString () const
- {
- return docstring_;
- }
-} // namespace dynamicgraph
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTime, "RosTime");
+
+using namespace boost::posix_time;
+
+const std::string RosTime::docstring_(
+ "Export ROS time into dynamic-graph.\n"
+ "\n"
+ " Signal \"time\" provides time as given by ros::time as\n"
+ " boost::posix_time::ptime type.\n");
+
+RosTime::RosTime(const std::string& name)
+ : 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));
+}
+
+ptime& RosTime::update(ptime& time, const int&) {
+ time = rosTimeToPtime(ros::Time::now());
+ return time;
+}
+
+std::string RosTime::getDocString() const { return docstring_; }
+} // namespace dynamicgraph
diff --git a/src/ros_time.hh b/src/ros_time.hh
index 23f27f6..5875985 100644
--- a/src/ros_time.hh
+++ b/src/ros_time.hh
@@ -4,29 +4,31 @@
/// Author: Florent Lamiraux
#ifndef DYNAMIC_GRAPH_ROS_TIME_HH
-# define DYNAMIC_GRAPH_ROS_TIME_HH
+#define DYNAMIC_GRAPH_ROS_TIME_HH
-# include <ros/time.h>
-# include <boost/date_time/posix_time/posix_time_types.hpp>
-# include <dynamic-graph/signal.h>
-# include <dynamic-graph/entity.h>
+#include <ros/time.h>
+#include <boost/date_time/posix_time/posix_time_types.hpp>
+#include <dynamic-graph/signal.h>
+#include <dynamic-graph/entity.h>
namespace dynamicgraph {
- class RosTime : public dynamicgraph::Entity
- {
- DYNAMIC_GRAPH_ENTITY_DECL ();
- public:
- Signal <boost::posix_time::ptime, int> now_;
- RosTime (const std::string& name);
- virtual std::string getDocString () const;
- protected:
- boost::posix_time::ptime&
- update (boost::posix_time::ptime& time, const int& t);
- private:
- static const std::string docstring_;
- }; // class RosTime
-
-} // namespace dynamicgraph
-
-#endif // DYNAMIC_GRAPH_ROS_TIME_HH
+class RosTime : public dynamicgraph::Entity {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+ Signal<boost::posix_time::ptime, int> now_;
+ RosTime(const std::string& name);
+ virtual std::string getDocString() const;
+
+ protected:
+ boost::posix_time::ptime& update(boost::posix_time::ptime& time,
+ const int& t);
+
+ private:
+ static const std::string docstring_;
+}; // class RosTime
+
+} // namespace dynamicgraph
+
+#endif // DYNAMIC_GRAPH_ROS_TIME_HH
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 52386d9..f67a80d 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -31,117 +31,103 @@
boost::condition_variable cond;
using namespace std;
-using namespace dynamicgraph::sot;
+using namespace dynamicgraph::sot;
namespace po = boost::program_options;
-struct DataToLog
-{
+struct DataToLog {
const std::size_t N;
std::size_t idx;
std::vector<double> times;
- DataToLog (std::size_t N_)
- : N (N_)
- , idx (0)
- , times (N, 0)
- {}
+ DataToLog(std::size_t N_) : N(N_), idx(0), times(N, 0) {}
- void record (const double t)
- {
+ void record(const double t) {
times[idx] = t;
++idx;
if (idx == N) idx = 0;
}
- void save (const char* prefix)
- {
+ void save(const char *prefix) {
std::ostringstream oss;
oss << prefix << "-times.log";
std::ofstream aof(oss.str().c_str());
if (aof.is_open()) {
for (std::size_t k = 0; k < N; ++k) {
- aof << times[ (idx + k) % N ] << '\n';
+ aof << times[(idx + k) % N] << '\n';
}
}
aof.close();
}
};
-
-void workThreadLoader(SotLoader *aSotLoader)
-{
- unsigned period = 1000; // micro seconds
+void workThreadLoader(SotLoader *aSotLoader) {
+ unsigned period = 1000; // micro seconds
if (ros::param::has("/sot_controller/dt")) {
double periodd;
ros::param::get("/sot_controller/dt", periodd);
period = unsigned(1e6 * periodd);
}
- DataToLog dataToLog (5000);
+ DataToLog dataToLog(5000);
- while(aSotLoader->isDynamicGraphStopped())
- {
- usleep(period);
- }
+ while (aSotLoader->isDynamicGraphStopped()) {
+ usleep(period);
+ }
struct timeval start, stop;
- ros::NodeHandle nh ("/geometric_simu");
+ ros::NodeHandle nh("/geometric_simu");
bool paused;
unsigned long long dt;
- while(!aSotLoader->isDynamicGraphStopped())
- {
- nh.param<bool> ("paused", paused, false);
-
- if (!paused) {
- gettimeofday(&start,0);
- aSotLoader->oneIteration();
- gettimeofday(&stop,0);
-
- dt = 1000000 * (stop.tv_sec - start.tv_sec) + (stop.tv_usec - start.tv_usec);
- dataToLog.record ((double)dt * 1e-6);
- } else
- dt = 0;
- if (period > dt) {
- usleep(period - (unsigned)dt);
- }
+ while (!aSotLoader->isDynamicGraphStopped()) {
+ nh.param<bool>("paused", paused, false);
+
+ if (!paused) {
+ gettimeofday(&start, 0);
+ aSotLoader->oneIteration();
+ gettimeofday(&stop, 0);
+
+ dt = 1000000 * (stop.tv_sec - start.tv_sec) +
+ (stop.tv_usec - start.tv_usec);
+ dataToLog.record((double)dt * 1e-6);
+ } else
+ dt = 0;
+ if (period > dt) {
+ usleep(period - (unsigned)dt);
}
- dataToLog.save ("/tmp/geometric_simu");
+ }
+ dataToLog.save("/tmp/geometric_simu");
cond.notify_all();
ros::waitForShutdown();
}
-SotLoader::SotLoader():
- sensorsIn_ (),
- controlValues_ (),
- angleEncoder_ (),
- angleControl_ (),
- forces_ (),
- torques_ (),
- baseAtt_ (),
- accelerometer_ (3),
- gyrometer_ (3),
- thread_ ()
-{
+SotLoader::SotLoader()
+ : sensorsIn_(),
+ controlValues_(),
+ angleEncoder_(),
+ angleControl_(),
+ forces_(),
+ torques_(),
+ baseAtt_(),
+ accelerometer_(3),
+ gyrometer_(3),
+ thread_() {
readSotVectorStateParam();
initPublication();
}
-SotLoader::~SotLoader()
-{
+SotLoader::~SotLoader() {
dynamic_graph_stopped_ = true;
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[])
-{
+void SotLoader::initializeRosNode(int argc, char *argv[]) {
SotLoaderBasic::initializeRosNode(argc, argv);
- //Temporary fix. TODO: where should nbOfJoints_ be initialized from?
+ // Temporary fix. TODO: where should nbOfJoints_ be initialized from?
if (ros::param::has("/sot/state_vector_map")) {
angleEncoder_.resize(nbOfJoints_);
angleControl_.resize(nbOfJoints_);
@@ -150,102 +136,81 @@ void SotLoader::initializeRosNode(int argc, char *argv[])
startControlLoop();
}
-void
-SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
-{
+void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
// Update joint values.w
assert(angleControl_.size() == angleEncoder_.size());
sensorsIn["joints"].setName("angle");
- for(unsigned int i=0;i<angleControl_.size();i++)
+ for (unsigned int i = 0; i < angleControl_.size(); i++)
angleEncoder_[i] = angleControl_[i];
- sensorsIn["joints"].setValues(angleEncoder_);
+ sensorsIn["joints"].setValues(angleEncoder_);
}
-void
-SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
-{
-
-
+void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
// Update joint values.
angleControl_ = controlValues["control"].getValues();
- //Debug
- 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<<":";
+ // Debug
+ 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 << ":";
std::vector<double> ctrlValues_ = it->second.getValues();
std::vector<double>::iterator it_d = ctrlValues_.begin();
- for(;it_d!=ctrlValues_.end();it_d++) sotDEBUG (30)<<*it_d<<" ";
- sotDEBUG (30)<<std::endl;
+ for (; it_d != ctrlValues_.end(); it_d++) sotDEBUG(30) << *it_d << " ";
+ sotDEBUG(30) << std::endl;
}
- sotDEBUG (30)<<"End ControlValues"<<std::endl;
-
+ sotDEBUG(30) << "End ControlValues" << std::endl;
// 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;
- exit(-1);
- }
+ if (angleControl_.size() != (unsigned int)nbOfJoints_) {
+ std::cerr << " angleControl_" << angleControl_.size() << " and nbOfJoints"
+ << (unsigned int)nbOfJoints_ << " are different !" << std::endl;
+ exit(-1);
+ }
// Publish the data.
- joint_state_.header.stamp = ros::Time::now();
- for(int i=0;i<nbOfJoints_;i++)
- {
- joint_state_.position[i] = angleControl_[i];
- }
- 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]];
- }
+ joint_state_.header.stamp = ros::Time::now();
+ for (int i = 0; i < nbOfJoints_; i++) {
+ joint_state_.position[i] = angleControl_[i];
+ }
+ 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]];
+ }
joint_pub_.publish(joint_state_);
- //Publish robot pose
- //get the robot pose values
+ // Publish robot pose
+ // 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"));
-
+ // Publish
+ freeFlyerPublisher_.sendTransform(tf::StampedTransform(
+ freeFlyerPose_, ros::Time::now(), "odom", "base_link"));
}
-void SotLoader::setup()
-{
-
+void SotLoader::setup() {
fillSensors(sensorsIn_);
sotController_->setupSetSensors(sensorsIn_);
- sotController_->getControl(controlValues_);
- readControl(controlValues_);
+ sotController_->getControl(controlValues_);
+ readControl(controlValues_);
}
-void SotLoader::oneIteration()
-{
+void SotLoader::oneIteration() {
fillSensors(sensorsIn_);
- try
- {
- sotController_->nominalSetSensors(sensorsIn_);
- sotController_->getControl(controlValues_);
- }
- catch(std::exception &e) { throw e;}
-
+ try {
+ sotController_->nominalSetSensors(sensorsIn_);
+ sotController_->getControl(controlValues_);
+ } catch (std::exception &e) {
+ throw e;
+ }
+
readControl(controlValues_);
}
-
-
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index fd903ef..687916c 100644
--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -34,21 +34,17 @@
boost::mutex mut;
using namespace std;
-using namespace dynamicgraph::sot;
+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();
}
-int SotLoaderBasic::initPublication()
-{
- ros::NodeHandle & n = dynamicgraph::rosInit(false);
-
+int SotLoaderBasic::initPublication() {
+ ros::NodeHandle& n = dynamicgraph::rosInit(false);
// Prepare message to be published
joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
@@ -56,115 +52,99 @@ int SotLoaderBasic::initPublication()
return 0;
}
-
-void SotLoaderBasic::initializeRosNode(int , char *[])
-{
+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_stop_ = n.advertiseService("stop_dynamic_graph",
- &SotLoaderBasic::stop_dg,
- this);
-
-}
+ &SotLoaderBasic::start_dg, this);
+ service_stop_ =
+ n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);
+}
-void SotLoaderBasic::setDynamicLibraryName(std::string &afilename)
-{
+void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) {
dynamicLibraryName_ = afilename;
}
-int SotLoaderBasic::readSotVectorStateParam()
-{
- std::map<std::string,int> from_state_name_to_state_vector;
- std::map<std::string,std::string> from_parallel_name_to_state_vector_name;
+int SotLoaderBasic::readSotVectorStateParam() {
+ std::map<std::string, int> from_state_name_to_state_vector;
+ std::map<std::string, std::string> from_parallel_name_to_state_vector_name;
ros::NodeHandle n;
- if (!ros::param::has("/sot/state_vector_map"))
- {
- std::cerr<< " Read Sot Vector State Param " << std::endl;
- return 1;
- }
+ if (!ros::param::has("/sot/state_vector_map")) {
+ std::cerr << " Read Sot Vector State Param " << std::endl;
+ return 1;
+ }
n.getParam("/sot/state_vector_map", stateVectorMap_);
ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
nbOfJoints_ = stateVectorMap_.size();
nbOfParallelJoints_ = 0;
- 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;
-
- 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);
- double final_coefficient = 1.0;
- // deal with sign
- if (map_expression[0]=='-')
- {
- final_expression = map_expression.substr(1,map_expression.size()-1);
- final_coefficient = -1.0;
- }
- else
- 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;
- coefficient_parallel_joints_.push_back(final_coefficient);
-
- }
- nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
+ 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;
+
+ 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);
+ double final_coefficient = 1.0;
+ // deal with sign
+ if (map_expression[0] == '-') {
+ final_expression = map_expression.substr(1, map_expression.size() - 1);
+ final_coefficient = -1.0;
+ } else
+ 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;
+ coefficient_parallel_joints_.push_back(final_coefficient);
}
+ nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
+ }
// Prepare joint_state according to robot description.
- joint_state_.name.resize(nbOfJoints_+nbOfParallelJoints_);
- joint_state_.position.resize(nbOfJoints_+nbOfParallelJoints_);
+ joint_state_.name.resize(nbOfJoints_ + nbOfParallelJoints_);
+ joint_state_.position.resize(nbOfJoints_ + nbOfParallelJoints_);
// Fill in the name of the joints from the state vector.
// and build local map from state name to state vector
- for (int32_t i = 0; i < stateVectorMap_.size(); ++i)
- {
- joint_state_.name[i]= static_cast<string>(stateVectorMap_[i]);
+ for (int32_t i = 0; i < stateVectorMap_.size(); ++i) {
+ joint_state_.name[i] = static_cast<string>(stateVectorMap_[i]);
- from_state_name_to_state_vector[joint_state_.name[i]] = i;
- }
+ from_state_name_to_state_vector[joint_state_.name[i]] = i;
+ }
// Fill in the name of the joints from the parallel joint vector.
// and build map from parallel name to state vector
- int i=0;
+ 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();
- 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];
- }
+ 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];
+ }
return 0;
}
-
-int SotLoaderBasic::parseOptions(int argc, char *argv[])
-{
+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_);
-
+ po::notify(vm_);
+
if (vm_.count("help")) {
cout << desc << "\n";
return -1;
@@ -172,47 +152,41 @@ int SotLoaderBasic::parseOptions(int argc, char *argv[])
if (!vm_.count("input-file")) {
cout << "No filename specified\n";
return -1;
- }
- else
- dynamicLibraryName_= vm_["input-file"].as< string >();
-
+ } else
+ dynamicLibraryName_ = vm_["input-file"].as<string>();
+
Initialization();
return 0;
}
-void SotLoaderBasic::Initialization()
-{
-
+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 ;
+ return;
}
-
+
// reset errors
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';
- return ;
+ return;
}
-
+
// Create robot-controller
sotController_ = createSot();
- cout <<"Went out from Initialization." << endl;
+ cout << "Went out from Initialization." << endl;
}
-void SotLoaderBasic::CleanUp()
-{
+void SotLoaderBasic::CleanUp() {
dynamicgraph::PoolStorage::destroy();
// We do not destroy the FactoryStorage singleton because the module will not
// be reloaded at next initialization (because Python C API cannot safely
@@ -220,37 +194,30 @@ 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';
- return ;
+ return;
}
- destroySot (sotController_);
+ destroySot(sotController_);
sotController_ = NULL;
/// Uncount the number of access to this library.
dlclose(sotRobotControllerLibrary_);
}
-
-
-bool SotLoaderBasic::start_dg(std_srvs::Empty::Request& ,
- std_srvs::Empty::Response& )
-{
- dynamic_graph_stopped_=false;
+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 8c08f63..8b03235 100644
--- a/src/sot_to_ros.cpp
+++ b/src/sot_to_ros.cpp
@@ -1,26 +1,23 @@
#include "sot_to_ros.hh"
-namespace dynamicgraph
-{
+namespace dynamicgraph {
- const char* SotToRos<bool>::signalTypeName = "bool";
- const char* SotToRos<double>::signalTypeName = "Double";
- const char* SotToRos<int>::signalTypeName = "int";
- const char* SotToRos<std::string>::signalTypeName = "string";
- const char* SotToRos<unsigned int>::signalTypeName = "Unsigned";
- const char* SotToRos<Matrix>::signalTypeName = "Matrix";
- 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<bool>::signalTypeName = "bool";
+const char* SotToRos<double>::signalTypeName = "Double";
+const char* SotToRos<int>::signalTypeName = "int";
+const char* SotToRos<std::string>::signalTypeName = "string";
+const char* SotToRos<unsigned int>::signalTypeName = "Unsigned";
+const char* SotToRos<Matrix>::signalTypeName = "Matrix";
+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";
-} // end of namespace dynamicgraph.
+} // end of namespace dynamicgraph.
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index 9895aaa..6a3de0b 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -1,379 +1,316 @@
#ifndef DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
-# define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
-# include <vector>
-# include <utility>
-
-# include <boost/format.hpp>
-
-# include <std_msgs/Bool.h>
-# include <std_msgs/Float64.h>
-# include <std_msgs/UInt32.h>
-# include <std_msgs/Int32.h>
-# include <std_msgs/String.h>
-# include "dynamic_graph_bridge_msgs/Matrix.h"
-# include "dynamic_graph_bridge_msgs/Vector.h"
-
-# include "geometry_msgs/Transform.h"
-# include "geometry_msgs/TransformStamped.h"
-# include "geometry_msgs/Twist.h"
-# include "geometry_msgs/TwistStamped.h"
-# include "geometry_msgs/Vector3Stamped.h"
-
-# include <dynamic-graph/signal-time-dependent.h>
-# include <dynamic-graph/linear-algebra.h>
-# 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)
-
-namespace dynamicgraph
-{
-
- /// \brief Types dedicated to identify pairs of (dg,ros) data.
- ///
- /// They do not contain any data but allow to make the difference
- /// between different types with the same structure.
- /// For instance a vector of six elements vs a twist coordinate.
- namespace specific
- {
- class Vector3 {};
- class Twist {};
- } // end of namespace specific.
-
- /// \brief SotToRos trait type.
- ///
- /// This trait provides types associated to a dynamic-graph type:
- /// - ROS corresponding type,
- /// - signal type / input signal type,
- /// - ROS callback type.
- template <typename SotType>
- class SotToRos;
-
- template <>
- struct SotToRos<bool>
- {
- typedef bool sot_t;
- typedef std_msgs::Bool ros_t;
- typedef std_msgs::BoolConstPtr ros_const_ptr_t;
- typedef dynamicgraph::Signal<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- s.setConstant (false);
- }
-
- static void setDefault(sot_t& s)
- {
- s = false;
- }
- };
-
- template <>
- struct SotToRos<double>
- {
- typedef double sot_t;
- typedef std_msgs::Float64 ros_t;
- typedef std_msgs::Float64ConstPtr ros_const_ptr_t;
- typedef dynamicgraph::Signal<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- s.setConstant (0.);
- }
-
- static void setDefault(sot_t& s)
- {
- s = 0.;
- }
- };
-
- template <>
- struct SotToRos<unsigned int>
- {
- typedef unsigned int sot_t;
- typedef std_msgs::UInt32 ros_t;
- typedef std_msgs::UInt32ConstPtr ros_const_ptr_t;
- typedef dynamicgraph::Signal<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- s.setConstant (0);
- }
-
- static void setDefault(sot_t& s)
- {
- s = 0;
- }
- };
-
- template <>
- struct SotToRos<int>
- {
- typedef int sot_t;
- typedef std_msgs::Int32 ros_t;
- typedef std_msgs::Int32ConstPtr ros_const_ptr_t;
- typedef dynamicgraph::Signal<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- s.setConstant (0);
- }
-
- static void setDefault(sot_t& s)
- {
- s = 0;
- }
- };
-
- template <>
- struct SotToRos<std::string>
- {
- typedef std::string sot_t;
- typedef std_msgs::String ros_t;
- typedef std_msgs::StringConstPtr ros_const_ptr_t;
- typedef dynamicgraph::Signal<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- s.setConstant ("");
- }
-
- static void setDefault(sot_t& s)
- {
- s = std::string ();
- }
- };
-
- template <>
- struct SotToRos<Matrix>
- {
- typedef Matrix sot_t;
- typedef dynamic_graph_bridge_msgs::Matrix ros_t;
- typedef dynamic_graph_bridge_msgs::MatrixConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- Matrix m;
- m.resize(0, 0);
- s.setConstant (m);
- }
-
- static void setDefault(sot_t& s)
- {
- s.resize(0,0);
- }
- };
-
- template <>
- struct SotToRos<Vector>
- {
- typedef Vector sot_t;
- typedef dynamic_graph_bridge_msgs::Vector ros_t;
- typedef dynamic_graph_bridge_msgs::VectorConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- Vector v;
- v.resize (0);
- s.setConstant (v);
- }
-
- static void setDefault(sot_t& s)
- {
- s.resize(0);
- }
- };
-
- template <>
- struct SotToRos<specific::Vector3>
- {
- typedef Vector sot_t;
- typedef geometry_msgs::Vector3 ros_t;
- typedef geometry_msgs::Vector3ConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- Vector v (Vector::Zero(3));
- s.setConstant (v);
- }
-
- static void setDefault(sot_t& s)
- {
- s = Vector::Zero(3);
- }
- };
-
- template <>
- struct SotToRos<sot::MatrixHomogeneous>
- {
- typedef sot::MatrixHomogeneous sot_t;
- typedef geometry_msgs::Transform ros_t;
- typedef geometry_msgs::TransformConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- sot::MatrixHomogeneous m;
- s.setConstant (m);
- }
-
- static void setDefault(sot_t& s)
- {
- s.setIdentity();
- }
- };
-
- template <>
- struct SotToRos<specific::Twist>
- {
- typedef Vector sot_t;
- typedef geometry_msgs::Twist ros_t;
- typedef geometry_msgs::TwistConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- Vector v (6);
- v.setZero ();
- s.setConstant (v);
- }
-
- static void setDefault(sot_t& s)
- {
- s = Vector::Zero(6);
- }
- };
-
- // Stamped vector3
- template <>
- struct SotToRos<std::pair<specific::Vector3, Vector> >
- {
- typedef Vector sot_t;
- typedef geometry_msgs::Vector3Stamped ros_t;
- typedef geometry_msgs::Vector3StampedConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- SotToRos<sot_t>::setDefault(s);
- }
- };
-
- // Stamped transformation
- template <>
- struct SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >
- {
- typedef sot::MatrixHomogeneous sot_t;
- typedef geometry_msgs::TransformStamped ros_t;
- typedef geometry_msgs::TransformStampedConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- SotToRos<sot_t>::setDefault(s);
- }
- };
-
- // Stamped twist
- template <>
- struct SotToRos<std::pair<specific::Twist, Vector> >
- {
- typedef Vector sot_t;
- typedef geometry_msgs::TwistStamped ros_t;
- typedef geometry_msgs::TwistStampedConstPtr ros_const_ptr_t;
- typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
- typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
- typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
- static const char* signalTypeName;
-
- template <typename S>
- static void setDefault(S& s)
- {
- SotToRos<sot_t>::setDefault(s);
- }
- };
-
- 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;
- return fmt.str ();
+#define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
+#include <vector>
+#include <utility>
+
+#include <boost/format.hpp>
+
+#include <std_msgs/Bool.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/UInt32.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
+#include "dynamic_graph_bridge_msgs/Matrix.h"
+#include "dynamic_graph_bridge_msgs/Vector.h"
+
+#include "geometry_msgs/Transform.h"
+#include "geometry_msgs/TransformStamped.h"
+#include "geometry_msgs/Twist.h"
+#include "geometry_msgs/TwistStamped.h"
+#include "geometry_msgs/Vector3Stamped.h"
+
+#include <dynamic-graph/signal-time-dependent.h>
+#include <dynamic-graph/linear-algebra.h>
+#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)
+
+namespace dynamicgraph {
+
+/// \brief Types dedicated to identify pairs of (dg,ros) data.
+///
+/// They do not contain any data but allow to make the difference
+/// between different types with the same structure.
+/// For instance a vector of six elements vs a twist coordinate.
+namespace specific {
+class Vector3 {};
+class Twist {};
+} // end of namespace specific.
+
+/// \brief SotToRos trait type.
+///
+/// This trait provides types associated to a dynamic-graph type:
+/// - ROS corresponding type,
+/// - signal type / input signal type,
+/// - ROS callback type.
+template <typename SotType>
+class SotToRos;
+
+template <>
+struct SotToRos<bool> {
+ typedef bool sot_t;
+ typedef std_msgs::Bool ros_t;
+ typedef std_msgs::BoolConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::Signal<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ s.setConstant(false);
}
-} // end of namespace dynamicgraph.
-#endif //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
+ static void setDefault(sot_t& s) { s = false; }
+};
+
+template <>
+struct SotToRos<double> {
+ typedef double sot_t;
+ typedef std_msgs::Float64 ros_t;
+ typedef std_msgs::Float64ConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::Signal<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ s.setConstant(0.);
+ }
+
+ static void setDefault(sot_t& s) { s = 0.; }
+};
+
+template <>
+struct SotToRos<unsigned int> {
+ typedef unsigned int sot_t;
+ typedef std_msgs::UInt32 ros_t;
+ typedef std_msgs::UInt32ConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::Signal<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ s.setConstant(0);
+ }
+
+ static void setDefault(sot_t& s) { s = 0; }
+};
+
+template <>
+struct SotToRos<int> {
+ typedef int sot_t;
+ typedef std_msgs::Int32 ros_t;
+ typedef std_msgs::Int32ConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::Signal<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ s.setConstant(0);
+ }
+
+ static void setDefault(sot_t& s) { s = 0; }
+};
+
+template <>
+struct SotToRos<std::string> {
+ typedef std::string sot_t;
+ typedef std_msgs::String ros_t;
+ typedef std_msgs::StringConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::Signal<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ s.setConstant("");
+ }
+
+ static void setDefault(sot_t& s) { s = std::string(); }
+};
+
+template <>
+struct SotToRos<Matrix> {
+ typedef Matrix sot_t;
+ typedef dynamic_graph_bridge_msgs::Matrix ros_t;
+ typedef dynamic_graph_bridge_msgs::MatrixConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ Matrix m;
+ m.resize(0, 0);
+ s.setConstant(m);
+ }
+
+ static void setDefault(sot_t& s) { s.resize(0, 0); }
+};
+
+template <>
+struct SotToRos<Vector> {
+ typedef Vector sot_t;
+ typedef dynamic_graph_bridge_msgs::Vector ros_t;
+ typedef dynamic_graph_bridge_msgs::VectorConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ Vector v;
+ v.resize(0);
+ s.setConstant(v);
+ }
+
+ static void setDefault(sot_t& s) { s.resize(0); }
+};
+
+template <>
+struct SotToRos<specific::Vector3> {
+ typedef Vector sot_t;
+ typedef geometry_msgs::Vector3 ros_t;
+ typedef geometry_msgs::Vector3ConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ Vector v(Vector::Zero(3));
+ s.setConstant(v);
+ }
+
+ static void setDefault(sot_t& s) { s = Vector::Zero(3); }
+};
+
+template <>
+struct SotToRos<sot::MatrixHomogeneous> {
+ typedef sot::MatrixHomogeneous sot_t;
+ typedef geometry_msgs::Transform ros_t;
+ typedef geometry_msgs::TransformConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ sot::MatrixHomogeneous m;
+ s.setConstant(m);
+ }
+
+ static void setDefault(sot_t& s) { s.setIdentity(); }
+};
+
+template <>
+struct SotToRos<specific::Twist> {
+ typedef Vector sot_t;
+ typedef geometry_msgs::Twist ros_t;
+ typedef geometry_msgs::TwistConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ Vector v(6);
+ v.setZero();
+ s.setConstant(v);
+ }
+
+ static void setDefault(sot_t& s) { s = Vector::Zero(6); }
+};
+
+// Stamped vector3
+template <>
+struct SotToRos<std::pair<specific::Vector3, Vector> > {
+ typedef Vector sot_t;
+ typedef geometry_msgs::Vector3Stamped ros_t;
+ typedef geometry_msgs::Vector3StampedConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ SotToRos<sot_t>::setDefault(s);
+ }
+};
+
+// Stamped transformation
+template <>
+struct SotToRos<std::pair<sot::MatrixHomogeneous, Vector> > {
+ typedef sot::MatrixHomogeneous sot_t;
+ typedef geometry_msgs::TransformStamped ros_t;
+ typedef geometry_msgs::TransformStampedConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ SotToRos<sot_t>::setDefault(s);
+ }
+};
+
+// Stamped twist
+template <>
+struct SotToRos<std::pair<specific::Twist, Vector> > {
+ typedef Vector sot_t;
+ typedef geometry_msgs::TwistStamped ros_t;
+ typedef geometry_msgs::TwistStampedConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s) {
+ SotToRos<sot_t>::setDefault(s);
+ }
+};
+
+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;
+ return fmt.str();
+}
+} // end of namespace dynamicgraph.
+
+#endif //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
--
GitLab