From 75b3acf33d2ec91fe1bd930b66e1754f3f1ea5e8 Mon Sep 17 00:00:00 2001
From: Peter Soetens <peter@thesourceworks.com>
Date: Tue, 5 Apr 2011 17:55:27 +0200
Subject: [PATCH] ros service: initial ros utility service

Creates a topic connection policy for talking to ROS nodes

Signed-off-by: Peter Soetens <peter@thesourceworks.com>
---
 rtt_ros_service/CMakeLists.txt      |   58 +++++++++++++++++++++++++++++++++++
 rtt_ros_service/Makefile            |   14 ++++++++
 rtt_ros_service/manifest.xml        |   12 +++++++
 rtt_ros_service/rtt_ros_service.cpp |   55 +++++++++++++++++++++++++++++++++
 4 files changed, 139 insertions(+), 0 deletions(-)
 create mode 100644 rtt_ros_service/CMakeLists.txt
 create mode 100644 rtt_ros_service/Makefile
 create mode 100644 rtt_ros_service/manifest.xml
 create mode 100644 rtt_ros_service/rtt_ros_service.cpp

diff --git a/rtt_ros_service/CMakeLists.txt b/rtt_ros_service/CMakeLists.txt
new file mode 100644
index 0000000..4a7a061
--- /dev/null
+++ b/rtt_ros_service/CMakeLists.txt
@@ -0,0 +1,58 @@
+#
+# The find_package macro for Orocos-RTT works best with
+# cmake >= 2.6.3
+#
+cmake_minimum_required(VERSION 2.6.3)
+
+#
+# This creates a standard cmake project. You may extend this file with
+# any cmake macro you see fit.
+#
+project(rtt_ros_service)
+
+#
+# Do setup in case of ros package, If ROS_ROOT is set, it is
+# recommended to use RTT/OCL through the ros packages.
+#
+set (ROS_ROOT $ENV{ROS_ROOT} )
+if (ROS_ROOT)
+  include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+  rosbuild_init()
+  rosbuild_find_ros_package( rtt )
+  set( RTT_HINTS HINTS ${rtt_PACKAGE_PATH}/install )
+endif()
+
+# Set the CMAKE_PREFIX_PATH in case you're not using Orocos through ROS
+# for helping these find commands find RTT.
+find_package(OROCOS-RTT REQUIRED ${RTT_HINTS})
+
+# Defines the orocos_* cmake macros. See that file for additional
+# documentation.
+include(${OROCOS-RTT_USE_FILE_PATH}/UseOROCOS-RTT.cmake)
+
+#
+# Components, types and plugins.
+#
+# The CMake 'target' names are identical to the first argument of the
+# macros below, except for orocos_typegen_headers, where the target is fully
+# controlled by generated code of 'typegen'.
+#
+
+#
+# Building a Plugin or Service (optional):
+#
+# Creates a plugin library librtt_ros_service-<target>.so
+# and installs in the directory lib/orocos/rtt_ros_service/plugins/
+#
+# Be aware that a plugin may only have the loadRTTPlugin() function once defined in a .cpp file.
+# This function is defined by the plugin and service CPP macros.
+#
+orocos_service(rtt_ros_service rtt_ros_service.cpp) # ...only one service per library !
+#
+# You may add multiple orocos_plugin/orocos_service statements.
+
+#
+# Generates and installs our package. Must be the last statement such
+# that it can pick up all above settings.
+#
+orocos_generate_package()
diff --git a/rtt_ros_service/Makefile b/rtt_ros_service/Makefile
new file mode 100644
index 0000000..b897264
--- /dev/null
+++ b/rtt_ros_service/Makefile
@@ -0,0 +1,14 @@
+# This Makefile is here for 'rosmake' like systems. In case you don't use
+# ROS, it will create a build directory for you and build the package with
+# default settings. It will install it at the same location as the RTT is installed.
+ifdef ROS_ROOT
+include $(shell rospack find mk)/cmake.mk
+else
+$(warning This Makefile builds this package with default settings)
+all:
+	mkdir -p build
+	cd build ; cmake .. -DINSTALL_PATH=orocos && make
+	echo -e "\n Now do 'make install' to install this package.\n"
+install: all
+	cd build ; make install
+endif
\ No newline at end of file
diff --git a/rtt_ros_service/manifest.xml b/rtt_ros_service/manifest.xml
new file mode 100644
index 0000000..1b1fbe8
--- /dev/null
+++ b/rtt_ros_service/manifest.xml
@@ -0,0 +1,12 @@
+<package>
+    <description brief="Orocos RTT-ROS integration tools">
+        This package contains a service you can use to quickly connect
+				to ROS topics and other utility functions for in an RTT-ROS environment.
+    </description>
+    <!--NOTE: set the license and author before you publish this code-->
+    <license>BSD</license>
+    <author>Peter Soetens</author>
+    <depend package="rtt" />
+    <depend package="rtt_ros_integration" />
+</package>
+
diff --git a/rtt_ros_service/rtt_ros_service.cpp b/rtt_ros_service/rtt_ros_service.cpp
new file mode 100644
index 0000000..5ad35ae
--- /dev/null
+++ b/rtt_ros_service/rtt_ros_service.cpp
@@ -0,0 +1,55 @@
+#include <rtt/RTT.hpp>
+#include <rtt/plugin/ServicePlugin.hpp>
+#include <RosLib.hpp> // todo: put RosLib.hpp in include dir of rtt_ros_integration package
+
+using namespace RTT;
+using namespace std;
+
+/**
+ * The globally loadable ROS service.
+ */
+class ROSService : public RTT::Service {
+public:
+    int protocol_id;
+    /**
+     * Instantiates this service.
+     * @param owner The owner or null in case of global.
+     */
+    ROSService(TaskContext* owner) 
+        : Service("ros", owner),
+          protocol_id(ORO_ROS_PROTOCOL_ID)
+    {
+        // stream("Simulation.ctrl", ros.topic("/cmd_vel") )
+        this->addOperation("topic", &ROSService::topic, this).doc("Creates a ConnPolicy for subscribing to or publishing a topic. No buffering is done, only the last sample is kept.").arg("name", "The ros topic name");
+        this->addOperation("topicBuffer", &ROSService::topicBuffer, this).doc("Creates a ConnPolicy for subscribing to or publishing a topic.").arg("name", "The ros topic name").arg("size","The size of the buffer.");
+        this->addConstant("protocol_id", protocol_id );
+    }
+
+    /**
+     * Returns a ConnPolicy object for streaming to or from 
+     * the given ROS topic. No buffering is done.
+     */
+    ConnPolicy topic(const std::string& name) {
+        ConnPolicy cp = ConnPolicy::data();
+        cp.transport = ORO_ROS_PROTOCOL_ID;
+        cp.name_id = name;
+        return cp;
+    }
+
+    /**
+     * Returns a ConnPolicy object for streaming to or from 
+     * the given ROS topic. Also specifies the buffer size of
+     * the connection to be created.
+     */
+    ConnPolicy topicBuffer(const std::string& name, int size) {
+        ConnPolicy cp = ConnPolicy::buffer(size);
+        cp.transport = ORO_ROS_PROTOCOL_ID;
+        cp.name_id = name;
+        return cp;
+    }
+};
+
+/* For consistency reasons, it's better to name the
+ * service the same as in the class above.
+ */
+ORO_SERVICE_NAMED_PLUGIN(ROSService, "ros")
-- 
1.7.0.4

