From 5b5a278defb4cf69fc78f3f8ff0720ecc1080a48 Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Fri, 18 Nov 2011 21:04:50 +0100
Subject: [PATCH] Add support for vector3 and add tf_broadcaster.
---
manifest.xml | 1 +
src/converter.hh | 20 +++++++++++++++++
src/dynamic_graph/ros/ros.py | 26 +++++++++++++++++-----
src/ros_export.cpp | 4 ++++
src/ros_import.cpp | 4 ++++
src/ros_import.hxx | 24 ++++++++++++++++++++
src/sot_to_ros.cpp | 4 ++++
src/sot_to_ros.hh | 43 ++++++++++++++++++++++++++++++++++++
8 files changed, 121 insertions(+), 5 deletions(-)
diff --git a/manifest.xml b/manifest.xml
index 0046dbf..9910664 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -16,6 +16,7 @@
<depend package="geometry_msgs"/>
<depend package="sensor_msgs"/>
<depend package="bullet"/>
+ <depend package="tf"/>
<depend package="realtime_tools"/>
</package>
diff --git a/src/converter.hh b/src/converter.hh
index 4b12fc9..1734644 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -68,6 +68,22 @@ namespace dynamicgraph
dst.elementAt (i) = src.data[i];
}
+ // Vector3
+ SOT_TO_ROS_IMPL(specific::Vector3)
+ {
+ assert (src.size () == 3);
+ dst.x = src.elementAt (0);
+ dst.y = src.elementAt (1);
+ dst.z = src.elementAt (2);
+ }
+
+ ROS_TO_SOT_IMPL(specific::Vector3)
+ {
+ dst.resize (3);
+ dst.elementAt (0) = src.x;
+ dst.elementAt (1) = src.y;
+ dst.elementAt (2) = src.z;
+ }
// Matrix
SOT_TO_ROS_IMPL(ml::Matrix)
@@ -163,6 +179,7 @@ namespace dynamicgraph
} \
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, ;);
@@ -186,6 +203,7 @@ namespace dynamicgraph
DG_BRIDGE_MAKE_SHPTR_IMPL(double);
DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Vector);
+ DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Matrix);
DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
@@ -205,6 +223,7 @@ namespace dynamicgraph
} \
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, ;);
@@ -224,6 +243,7 @@ namespace dynamicgraph
} \
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, ;);
diff --git a/src/dynamic_graph/ros/ros.py b/src/dynamic_graph/ros/ros.py
index d6b5ba3..e0a8210 100644
--- a/src/dynamic_graph/ros/ros.py
+++ b/src/dynamic_graph/ros/ros.py
@@ -10,12 +10,28 @@ class Ros(object):
rosExport = None
rosJointState = None
- def __init__(self, device, suffix = ''):
- self.device = device
+ def __init__(self, robot, suffix = ''):
+ self.robot = robot
self.rosImport = RosImport('rosImport{0}'.format(suffix))
self.rosExport = RosExport('rosExport{0}'.format(suffix))
self.rosJointState = RosJointState('rosJointState{0}'.format(suffix))
- plug(self.device.state, self.rosJointState.state)
- self.device.after.addSignal('{0}.trigger'.format(self.rosImport.name))
- self.device.after.addSignal('{0}.trigger'.format(self.rosJointState.name))
+ plug(self.robot.device.state, self.rosJointState.state)
+ self.robot.device.after.addSignal('{0}.trigger'.format(self.rosImport.name))
+ self.robot.device.after.addSignal('{0}.trigger'.format(self.rosJointState.name))
+
+ # Export base_footprint aka dynamic-graph world frame.
+ self.rosImport.add('matrixHomoStamped', 'base_footprint', 'base_footprint')
+ plug(robot.dynamic.signal('left-ankle'), self.rosImport.base_footprint)
+
+ # Export base_link aka waist frame.
+ self.rosImport.add('matrixHomoStamped', 'base_link', 'base_link')
+ plug(robot.dynamic.waist, self.rosImport.base_link)
+
+ # Export center of mass position.
+ self.rosImport.add('vector3Stamped', 'com', 'com')
+ plug(robot.dynamic.com, self.rosImport.com)
+
+ # Export ZMP position.
+ self.rosImport.add('vector3Stamped', 'zmp', 'zmp')
+ plug(robot.dynamic.zmp, self.rosImport.zmp)
diff --git a/src/ros_export.cpp b/src/ros_export.cpp
index 3f86275..f6d778c 100644
--- a/src/ros_export.cpp
+++ b/src/ros_export.cpp
@@ -78,6 +78,10 @@ namespace dynamicgraph
entity.add<ml::Matrix> (signal, topic);
else if (type == "vector")
entity.add<ml::Vector> (signal, topic);
+ else if (type == "vector3")
+ entity.add<specific::Vector3> (signal, topic);
+ else if (type == "vector3Stamped")
+ entity.add<std::pair<specific::Vector3, ml::Vector> > (signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous> (signal, topic);
else if (type == "matrixHomoStamped")
diff --git a/src/ros_import.cpp b/src/ros_import.cpp
index 1a8af28..3f41fce 100644
--- a/src/ros_import.cpp
+++ b/src/ros_import.cpp
@@ -82,6 +82,10 @@ namespace dynamicgraph
entity.add<ml::Matrix> (signal, topic);
else if (type == "vector")
entity.add<ml::Vector> (signal, topic);
+ else if (type == "vector3")
+ entity.add<specific::Vector3> (signal, topic);
+ else if (type == "vector3Stamped")
+ entity.add<std::pair<specific::Vector3, ml::Vector> > (signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous> (signal, topic);
else if (type == "matrixHomoStamped")
diff --git a/src/ros_import.hxx b/src/ros_import.hxx
index bb224f1..1ac821a 100644
--- a/src/ros_import.hxx
+++ b/src/ros_import.hxx
@@ -12,6 +12,30 @@
namespace dynamicgraph
{
+ template <>
+ inline void
+ RosImport::sendData
+ <std::pair<sot::MatrixHomogeneous, ml::Vector> >
+ (boost::shared_ptr
+ <realtime_tools::RealtimePublisher
+ <SotToRos
+ <std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t> > publisher,
+ boost::shared_ptr
+ <SotToRos
+ <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalIn_t> signal,
+ int time)
+ {
+ SotToRos
+ <std::pair
+ <sot::MatrixHomogeneous, ml::Vector> >::ros_t result;
+ if (publisher->trylock ())
+ {
+ publisher->msg_.child_frame_id = "/dynamic_graph/world";
+ converter (publisher->msg_, signal->access (time));
+ publisher->unlockAndPublish ();
+ }
+ }
+
template <typename T>
void
RosImport::sendData
diff --git a/src/sot_to_ros.cpp b/src/sot_to_ros.cpp
index bfdbc79..1b52ae9 100644
--- a/src/sot_to_ros.cpp
+++ b/src/sot_to_ros.cpp
@@ -6,9 +6,13 @@ namespace dynamicgraph
const char* SotToRos<double>::signalTypeName = "Double";
const char* SotToRos<ml::Matrix>::signalTypeName = "Matrix";
const char* SotToRos<ml::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, ml::Vector> >::signalTypeName
+ = "Vector3Stamped";
+ const char* SotToRos
<std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalTypeName
= "MatrixHomo";
const char* SotToRos
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index 8dbbb35..2088175 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -14,6 +14,7 @@
# 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/signal-ptr.h>
@@ -34,6 +35,7 @@ namespace dynamicgraph
/// For instance a vector of six elements vs a twist coordinate.
namespace specific
{
+ class Vector3 {};
class Twist {};
} // end of namespace specific.
@@ -107,6 +109,27 @@ namespace dynamicgraph
}
};
+ template <>
+ struct SotToRos<specific::Vector3>
+ {
+ typedef ml::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)
+ {
+ ml::Vector v;
+ v.resize (0);
+ s.setConstant (v);
+ }
+ };
+
template <>
struct SotToRos<sot::MatrixHomogeneous>
{
@@ -148,6 +171,26 @@ namespace dynamicgraph
}
};
+ // Stamped vector3
+ template <>
+ struct SotToRos<std::pair<specific::Vector3, ml::Vector> >
+ {
+ typedef ml::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, ml::Vector> >
--
GitLab