From 6014f830d758ed07c353a2021acd9b595f2b36ae Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Fri, 26 Nov 2010 22:48:02 +0100
Subject: [PATCH] Add export node.
---
CMakeLists.txt | 16 ++++++-
src/ros_export.cpp | 105 +++++++++++++++++++++++++++++++++++++++++++++
src/ros_export.hh | 48 +++++++++++++++++++++
src/ros_export.hxx | 16 +++++++
4 files changed, 183 insertions(+), 2 deletions(-)
create mode 100644 src/ros_export.cpp
create mode 100644 src/ros_export.hh
create mode 100644 src/ros_export.hxx
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 95802ed..d8b177f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,6 +1,8 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+include(FindPkgConfig)
+
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
@@ -29,7 +31,17 @@ rosbuild_genmsg()
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
+pkg_check_modules(JRL_MAL REQUIRED jrl-mal)
+pkg_check_modules(DYNAMIC_GRAPH REQUIRED dynamic-graph)
+
rosbuild_add_library(ros_import src/ros_import.cpp)
-target_link_libraries(ros_import dynamic-graph)
+rosbuild_add_compile_flags(ros_import ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS})
+rosbuild_add_link_flags(ros_import ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS})
+target_link_libraries(ros_import
+ ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES})
-# rosbuild_add_library(sot_to_ros src/sot_to_ros.cpp)
+rosbuild_add_library(ros_export src/ros_export.cpp)
+rosbuild_add_compile_flags(ros_export ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS})
+rosbuild_add_link_flags(ros_export ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS})
+target_link_libraries(ros_export
+ ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES})
diff --git a/src/ros_export.cpp b/src/ros_export.cpp
new file mode 100644
index 0000000..74458fa
--- /dev/null
+++ b/src/ros_export.cpp
@@ -0,0 +1,105 @@
+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+#include <boost/function.hpp>
+#include <boost/make_shared.hpp>
+
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+
+#include <dynamic-graph/factory.h>
+
+#include "ros_export.hh"
+
+namespace dynamicgraph
+{
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosExport, "RosExport");
+
+ const char* rosInit()
+ {
+ int argc = 1;
+ char* arg0 = strdup("ros_export");
+ char* argv[] = {arg0, 0};
+ ros::init(argc, argv, "ros_export");
+ free (arg0);
+ return "dynamic_graph";
+ }
+
+ RosExport::RosExport (const std::string& n)
+ : dynamicgraph::Entity(n),
+ nh_ (rosInit ()),
+ bindedSignal_ ()
+ {
+ }
+
+ RosExport::~RosExport ()
+ {
+ }
+
+ void RosExport::display (std::ostream& os) const
+ {
+ os << CLASS_NAME << std::endl;
+ }
+
+ void RosExport::commandLine (const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os)
+ {
+ std::string type;
+ std::string signal;
+ std::string topic;
+
+ if (cmdLine == "help")
+ {
+ os << "RosExport: "<< std::endl
+ << " - add <TYPE> <SIGNAL> <TOPIC>" << std::endl
+ << " - rm <SIGNAL>" << std::endl
+ << " - clear" << std::endl
+ << " - list" << std::endl;
+ Entity::commandLine (cmdLine, cmdArgs, os);
+ }
+ else if (cmdLine == "add")
+ {
+ cmdArgs >> type >> signal >> topic;
+ if (type == "double")
+ ;
+ else if (type == "matrix")
+ ;
+ else if (type == "vector")
+ ;
+ else
+ throw "bad type";
+ }
+ else if (cmdLine == "rm")
+ {
+ cmdArgs >> signal;
+ rm (signal);
+ }
+ else if (cmdLine == "clear")
+ clear ();
+ else if (cmdLine == "list")
+ list ();
+ else
+ Entity::commandLine (cmdLine, cmdArgs, os);
+ }
+
+ const std::string& RosExport::getClassName ()
+ {
+ return CLASS_NAME;
+ }
+
+ void RosExport::rm (const std::string& signal)
+ {
+ bindedSignal_.erase (signal);
+ }
+
+ void RosExport::list ()
+ {
+ std::cout << CLASS_NAME << std::endl;
+ }
+
+ void RosExport::clear ()
+ {
+ bindedSignal_.clear ();
+ }
+
+} // end of namespace dynamicgraph.
diff --git a/src/ros_export.hh b/src/ros_export.hh
new file mode 100644
index 0000000..12c915c
--- /dev/null
+++ b/src/ros_export.hh
@@ -0,0 +1,48 @@
+#ifndef DYNAMIC_GRAPH_ROS_EXPORT_HH
+# define DYNAMIC_GRAPH_ROS_EXPORT_HH
+# include <iostream>
+# include <map>
+
+# include <boost/shared_ptr.hpp>
+
+# include <dynamic-graph/entity.h>
+# include <dynamic-graph/signal-time-dependent.h>
+
+# include <ros/ros.h>
+
+namespace dynamicgraph
+{
+ class RosExport : public dynamicgraph::Entity
+ {
+ public:
+ typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+ boost::shared_ptr<ros::Publisher> >
+ bindedSignal_t;
+
+ static const std::string CLASS_NAME;
+
+ RosExport (const std::string& n);
+ virtual ~RosExport ();
+
+ void display (std::ostream& os) const;
+ virtual void
+ commandLine (const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os);
+
+
+ virtual const std::string& getClassName ();
+
+ private:
+ void add (const std::string& signal, const std::string& topic);
+ void rm (const std::string& signal);
+ void list ();
+ void clear ();
+
+ ros::NodeHandle nh_;
+ std::map<std::string, bindedSignal_t> bindedSignal_;
+ };
+} // end of namespace dynamicgraph.
+
+# include "ros_export.hxx"
+#endif //! DYNAMIC_GRAPH_ROS_EXPORT_HH
diff --git a/src/ros_export.hxx b/src/ros_export.hxx
new file mode 100644
index 0000000..97154a0
--- /dev/null
+++ b/src/ros_export.hxx
@@ -0,0 +1,16 @@
+#ifndef DYNAMIC_GRAPH_ROS_EXPORT_HXX
+# define DYNAMIC_GRAPH_ROS_EXPORT_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
+{
+} // end of namespace dynamicgraph.
+
+#endif //! DYNAMIC_GRAPH_ROS_EXPORT_HXX
--
GitLab