From fe409907cad62cbb665bf811b7c6563395807c87 Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Fri, 26 Nov 2010 22:21:29 +0100
Subject: [PATCH] Bind vector and matrix MAL types.
---
.gitignore | 2 +-
CMakeLists.txt | 2 +-
manifest.xml | 1 +
msg/Matrix.msg | 2 ++
msg/Vector.msg | 1 +
src/ros_import.cpp | 11 +++++++++--
src/ros_import.hh | 1 +
src/ros_import.hxx | 46 ++++++++++++++++++++++++++++++++++++++++++++++
8 files changed, 62 insertions(+), 4 deletions(-)
create mode 100644 msg/Matrix.msg
create mode 100644 msg/Vector.msg
diff --git a/.gitignore b/.gitignore
index 17c2398..0061e71 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,5 +1,5 @@
lib/
msg/lisp/
msg_gen/
-src/hueblob/
+src/dynamic_graph/
bin/
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2dd9b2d..95802ed 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,7 +17,7 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
-#rosbuild_genmsg()
+rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
diff --git a/manifest.xml b/manifest.xml
index 251f60f..afb07d0 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -12,5 +12,6 @@
<depend package="std_msgs"/>
<depend package="roscpp"/>
+ <rosdep name="jrl-mal"/>
<rosdep name="dynamic-graph"/>
</package>
diff --git a/msg/Matrix.msg b/msg/Matrix.msg
new file mode 100644
index 0000000..323f50b
--- /dev/null
+++ b/msg/Matrix.msg
@@ -0,0 +1,2 @@
+float64[] data
+uint32 width
diff --git a/msg/Vector.msg b/msg/Vector.msg
new file mode 100644
index 0000000..922220f
--- /dev/null
+++ b/msg/Vector.msg
@@ -0,0 +1 @@
+float64[] data
diff --git a/src/ros_import.cpp b/src/ros_import.cpp
index c24c5da..38e236e 100644
--- a/src/ros_import.cpp
+++ b/src/ros_import.cpp
@@ -56,8 +56,15 @@ namespace dynamicgraph
}
else if (cmdLine == "add")
{
- cmdArgs >> signal >> topic;
- add<double> (signal, topic);
+ cmdArgs >> type >> signal >> topic;
+ if (type == "double")
+ add<double> (signal, topic);
+ else if (type == "matrix")
+ add<ml::Matrix> (signal, topic);
+ else if (type == "vector")
+ add<ml::Vector> (signal, topic);
+ else
+ throw "bad type";
}
else if (cmdLine == "rm")
{
diff --git a/src/ros_import.hh b/src/ros_import.hh
index 75afcc3..6d53529 100644
--- a/src/ros_import.hh
+++ b/src/ros_import.hh
@@ -1,6 +1,7 @@
#ifndef DYNAMIC_GRAPH_ROS_IMPORT_HH
# define DYNAMIC_GRAPH_ROS_IMPORT_HH
# include <iostream>
+# include <map>
# include <boost/shared_ptr.hpp>
diff --git a/src/ros_import.hxx b/src/ros_import.hxx
index f49a270..f4f4656 100644
--- a/src/ros_import.hxx
+++ b/src/ros_import.hxx
@@ -1,6 +1,13 @@
#ifndef DYNAMIC_GRAPH_ROS_IMPORT_HXX
# define DYNAMIC_GRAPH_ROS_IMPORT_HXX
+# include <vector>
+# include <jrl/mal/boost.hh>
# include <std_msgs/Float64.h>
+# include "dynamic_graph/Matrix.h"
+# include "dynamic_graph/Vector.h"
+
+
+namespace ml = maal::boost;
namespace dynamicgraph
{
@@ -13,6 +20,25 @@ namespace dynamicgraph
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
};
+ template <>
+ struct SotToRos<ml::Matrix>
+ {
+ typedef ml::Matrix sot_t;
+ typedef dynamic_graph::Matrix ros_t;
+ typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef boost::function<sot_t& (sot_t&, int)> callback_t;
+ };
+
+ template <>
+ struct SotToRos<ml::Vector>
+ {
+ typedef ml::Vector sot_t;
+ typedef dynamic_graph::Vector ros_t;
+ typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef boost::function<sot_t& (sot_t&, int)> callback_t;
+ };
+
+
template <>
void converter (SotToRos<double>::ros_t& dst,
const SotToRos<double>::sot_t& src)
@@ -20,6 +46,26 @@ namespace dynamicgraph
dst.data = src;
}
+ template <>
+ void converter (SotToRos<ml::Matrix>::ros_t& dst,
+ const SotToRos<ml::Matrix>::sot_t& src)
+ {
+ dst.width = src.nbRows ();
+ dst.data.resize (src.nbCols () * src.nbRows ());
+ for (unsigned i = 0; i < src.nbCols () * src.nbRows (); ++i)
+ dst.data[i] = src.elementAt (i);
+ }
+
+ template <>
+ void converter (SotToRos<ml::Vector>::ros_t& dst,
+ const SotToRos<ml::Vector>::sot_t& src)
+ {
+ dst.data.resize (src.size ());
+ for (unsigned i = 0; i < src.size (); ++i)
+ dst.data[i] = src.elementAt (i);
+ }
+
+
template <typename T>
T&
RosImport::sendData (boost::shared_ptr<ros::Publisher> publisher,
--
GitLab