diff --git a/rtt_ros/CMakeLists.txt b/rtt_ros/CMakeLists.txt
index 2b3c4e74..11f2e94f 100644
--- a/rtt_ros/CMakeLists.txt
+++ b/rtt_ros/CMakeLists.txt
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 2.8.3)
project(rtt_ros)
-find_package(catkin REQUIRED COMPONENTS rostime rospack roslib)
+find_package(catkin REQUIRED COMPONENTS rostime rospack roslib roscpp)
find_package(LibXml2 REQUIRED)
find_package(OROCOS-RTT REQUIRED)
include(${OROCOS-RTT_USE_FILE_PATH}/UseOROCOS-RTT.cmake)
diff --git a/rtt_ros/include/rtt_ros/rtt_ros.h b/rtt_ros/include/rtt_ros/rtt_ros.h
index 4b6353e1..4af1376b 100644
--- a/rtt_ros/include/rtt_ros/rtt_ros.h
+++ b/rtt_ros/include/rtt_ros/rtt_ros.h
@@ -5,9 +5,29 @@
namespace rtt_ros {
+ //! Global ros namespace
+ static std::string active_ns;
+
//! Import a ROS package and all of its rtt_ros/plugin_depend dependencies
bool import(const std::string& package);
+ //! Set the full ROS namespace
+ std::string setNS(const std::string& ns);
+
+ //! Resolve the namespace based on the active namespace
+ std::string resolveName(const std::string& name);
+
+ //! Reset the full ROS namespace
+ std::string resetNS();
+
+ //! Get the full ROS namespace
+ std::string getNS();
+
+ //! Append to the ROS namespace
+ std::string pushNS(const std::string& ns);
+
+ //! Remove the last token from the ROS namespace
+ std::string popNS();
}
#endif // __RTT_ROS_RTT_ROS_H
diff --git a/rtt_ros/launch/rttlua.launch b/rtt_ros/launch/rttlua.launch
new file mode 100644
index 00000000..14054e13
--- /dev/null
+++ b/rtt_ros/launch/rttlua.launch
@@ -0,0 +1,33 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/rtt_ros/scripts/rttlua-debug b/rtt_ros/scripts/rttlua-debug
new file mode 100755
index 00000000..d2df32c9
--- /dev/null
+++ b/rtt_ros/scripts/rttlua-debug
@@ -0,0 +1,9 @@
+#!/usr/bin/env bash
+
+# Wrapper script for the orocos deployer debug
+
+# Use gnulinux if orocos target isn't set
+OROCOS_TARGET=${OROCOS_TARGET:-gnulinux}
+
+# Run with gdb
+gdb -ex run --args rttlua-$OROCOS_TARGET "$@"
diff --git a/rtt_ros/scripts/rttlua-screen b/rtt_ros/scripts/rttlua-screen
new file mode 100755
index 00000000..46fe1538
--- /dev/null
+++ b/rtt_ros/scripts/rttlua-screen
@@ -0,0 +1,24 @@
+#!/usr/bin/env bash
+
+# Wrapper script
+
+SCREEN_NAME=RTTLUA
+screen -dmS $SCREEN_NAME rttlua "$@"
+SCREEN_ID=$(screen -list | grep $SCREEN_NAME | head -n 1 | awk '{print $1}')
+
+cleanup()
+{
+ echo "CLEANING UP!"
+ screen -S $SCREEN_ID -p 0 -X stuff ""
+}
+
+trap cleanup SIGINT
+
+#screen -r $SCREEN_ID
+
+#wait $SCREEN_PID (won't work snce it's not a child process)
+while screen -list | grep -q $SCREEN_ID
+do
+ sleep 1
+done
+
diff --git a/rtt_ros/src/rtt_ros.cpp b/rtt_ros/src/rtt_ros.cpp
index bfa853a1..cc261965 100644
--- a/rtt_ros/src/rtt_ros.cpp
+++ b/rtt_ros/src/rtt_ros.cpp
@@ -22,6 +22,8 @@
#include
#include
+#include
+#include
bool rtt_ros::import(const std::string& package)
@@ -198,3 +200,46 @@ bool rtt_ros::import(const std::string& package)
return missing_packages.size() == 0;
}
+
+static std::string head(const std::string& name)
+{
+ size_t last_token = name.find_last_of("/", name.size()>0 ? name.size() - 1 : name.size());
+ return name.substr(0, last_token);
+}
+
+static std::string tail(const std::string& name)
+{
+ size_t last_token = name.find_last_of("/", name.size()>0 ? name.size() - 1 : name.size());
+ return name.substr(last_token);
+}
+
+std::string rtt_ros::resolveName(const std::string& name)
+{
+ return ros::names::resolve(head(active_ns), name);
+}
+
+std::string rtt_ros::setNS(const std::string& ns)
+{
+ active_ns = ros::names::clean(ns);
+ return active_ns;
+}
+
+std::string rtt_ros::resetNS()
+{
+ return setNS(ros::names::append(ros::this_node::getNamespace(), ros::this_node::getName()));
+}
+
+std::string rtt_ros::getNS()
+{
+ return active_ns;
+}
+
+std::string rtt_ros::pushNS(const std::string& ns)
+{
+ return setNS(ros::names::append(active_ns, ns));
+}
+
+std::string rtt_ros::popNS()
+{
+ return setNS(head(active_ns));
+}
diff --git a/rtt_ros/src/rtt_ros_service.cpp b/rtt_ros/src/rtt_ros_service.cpp
index 9a76f382..7cec892e 100644
--- a/rtt_ros/src/rtt_ros_service.cpp
+++ b/rtt_ros/src/rtt_ros_service.cpp
@@ -18,19 +18,42 @@
#include
#include
+#include
+#include
#include
#include
void loadROSService(){
- RTT::Service::shared_ptr ros = RTT::internal::GlobalService::Instance()->provides("ros");
+ RTT::Service::shared_ptr ros_service = RTT::internal::GlobalService::Instance()->provides("ros");
- ros->doc("RTT service for loading RTT plugins ");
+ ros_service->doc("RTT service for loading RTT plugins ");
// ROS Package-importing
- ros->addOperation("import", &rtt_ros::import).doc(
+ ros_service->addOperation("import", &rtt_ros::import).doc(
"Imports the Orocos plugins from a given ROS package (if found) along with the plugins of all of the package's run or exec dependencies as listed in the package.xml.").arg(
"package", "The ROS package name.");
+
+ // Namespace manipulation
+ ros_service->addOperation("resolveName", &rtt_ros::resolveName).doc(
+ "Resolves a ros name based on the active namespace.");
+
+ ros_service->addOperation("resetNS", &rtt_ros::resetNS).doc(
+ "Resets the global namespace to the original namespace");
+
+ ros_service->addOperation("setNS", &rtt_ros::setNS).doc(
+ "Set the global namespace for all namespaced ROS operations").
+ arg("ns", "The new namespace.");
+
+ ros_service->addOperation("getNS", &rtt_ros::getNS).doc(
+ "Get the global namespace for all namespaced ROS operations");
+
+ ros_service->addOperation("pushNS", &rtt_ros::pushNS).doc(
+ "Append to the global namespace for all namespaced ROS operations").
+ arg("ns", "The new sub-namespace.");
+
+ ros_service->addOperation("popNS", &rtt_ros::popNS).doc(
+ "Remove the last namespace extension which was pushed.");
}
using namespace RTT;
diff --git a/rtt_rosclock/include/rtt_rosclock/prof.h b/rtt_rosclock/include/rtt_rosclock/prof.h
new file mode 100644
index 00000000..7fed9e7a
--- /dev/null
+++ b/rtt_rosclock/include/rtt_rosclock/prof.h
@@ -0,0 +1,89 @@
+#ifndef __RTT_ROSCLOCK_PROF_H__
+#define __RTT_ROSCLOCK_PROF_H__
+
+#include
+#include
+
+namespace rtt_rosclock {
+ class WallProf {
+ public:
+ typedef std::pair TicToc;
+
+ WallProf(double memory) :
+ memory_(memory)
+ {
+
+ }
+
+ void tic()
+ {
+ last_tic = rtt_rosclock::rtt_wall_now();
+ }
+
+ void toc()
+ {
+ ros::Time toc = rtt_rosclock::rtt_wall_now();
+ tictocs_.push_back(std::make_pair(last_tic, toc));
+ while(tictocs_.size() > 1 && (toc - tictocs_.front().first).toSec() > memory_) {
+ tictocs_.pop_front();
+ }
+ }
+
+ ros::Duration last()
+ {
+ TicToc &last_tictoc = tictocs_.back();
+ return last_tictoc.second - last_tictoc.first;
+ }
+
+ void analyze()
+ {
+ if(tictocs_.size() < 1) {
+ return;
+ }
+
+ TicToc &last_tictoc = tictocs_.back();
+
+ double sum = 0.0;
+ double count = tictocs_.size();
+
+ max_ = 0;
+ min_ = last_tictoc.second.toSec();
+
+ for(std::list::const_iterator it=tictocs_.begin(); it!=tictocs_.end(); ++it) {
+ const double tictoc = (it->second - it->first).toSec();
+ sum += tictoc;
+ max_ = std::max(max_, tictoc);
+ min_ = std::min(min_, tictoc);
+ }
+
+ mean_ = sum/count;
+
+ double err_sum = 0.0;
+
+ for(std::list::const_iterator it=tictocs_.begin(); it!=tictocs_.end(); ++it) {
+ const double tictoc = (it->second - it->first).toSec();
+ err_sum += std::pow(mean_ - tictoc, 2.0);
+ }
+
+ stddev_ = sqrt(err_sum/count);
+ }
+
+ double mean() { return mean_; }
+ double min() { return min_; }
+ double max() { return max_; }
+ double stddev() { return stddev_; }
+ size_t n() { return tictocs_.size(); }
+
+ private:
+ ros::Time last_tic;
+ double memory_;
+ std::list tictocs_;
+
+ double mean_;
+ double min_;
+ double max_;
+ double stddev_;
+ };
+}
+
+#endif // ifndef __RTT_ROSCLOCK_PROF_H__
diff --git a/rtt_rosclock/include/rtt_rosclock/throttle.h b/rtt_rosclock/include/rtt_rosclock/throttle.h
new file mode 100644
index 00000000..5ec982a4
--- /dev/null
+++ b/rtt_rosclock/include/rtt_rosclock/throttle.h
@@ -0,0 +1,30 @@
+#ifndef __RTT_ROSCLOCK_THROTTLE_H__
+#define __RTT_ROSCLOCK_THROTTLE_H__
+
+#include
+
+namespace rtt_rosclock {
+ class WallThrottle {
+ public:
+ WallThrottle(ros::Duration period) :
+ last_check_(0),
+ period_(period)
+ { }
+
+ bool ready()
+ {
+ ros::Time now = rtt_rosclock::rtt_wall_now();
+ if(now - last_check_ < period_) {
+ return false;
+ }
+ last_check_ = now;
+ return true;
+ }
+
+ private:
+ ros::Time last_check_;
+ ros::Duration period_;
+ };
+}
+
+#endif // ifndef __RTT_ROSCLOCK_THROTTLE_H__
diff --git a/rtt_roscomm/cmake/create_boost_header.py b/rtt_roscomm/cmake/create_boost_header.py
index c236b0e3..000ff09c 100755
--- a/rtt_roscomm/cmake/create_boost_header.py
+++ b/rtt_roscomm/cmake/create_boost_header.py
@@ -6,7 +6,7 @@
import gencpp
import genmsg
-from roslib import packages,msgs
+from roslib import packages,msgs
import os
from cStringIO import StringIO
@@ -18,7 +18,7 @@
def write_boost_includes(s, spec):
"""
Writes the message-specific includes
-
+
@param s: The stream to write to
@type s: stream
@param spec: The message spec to iterate over
@@ -31,14 +31,14 @@ def write_boost_includes(s, spec):
(pkg, name) = genmsg.names.package_resource_name(field.base_type)
pkg = (pkg or spec.package) # convert '' to this package
s.write('#include <%s/boost/%s.h>\n'%(pkg, name))
-
- s.write('\n')
+
+ s.write('\n')
def write_boost_serialization(s, spec, cpp_name_prefix, file):
"""
Writes the boost::serialize function for a message
-
+
@param s: Stream to write to
@type s: stream
@param spec: The message spec
@@ -47,7 +47,7 @@ def write_boost_serialization(s, spec, cpp_name_prefix, file):
@type cpp_name_prefix: str
"""
(cpp_msg_unqualified, cpp_msg_with_alloc, _) = gencpp.cpp_message_declarations(cpp_name_prefix, spec.short_name)
-
+
s.write("/* Auto-generated by create_boost_header.py for file %s */\n"%(file))
s.write('#ifndef %s_BOOST_SERIALIZATION_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
s.write('#define %s_BOOST_SERIALIZATION_%s_H\n\n'%(spec.package.upper(), spec.short_name.upper()))
@@ -57,25 +57,25 @@ def write_boost_serialization(s, spec, cpp_name_prefix, file):
write_boost_includes(s, spec)
s.write('namespace boost\n{\n')
s.write('namespace serialization\n{\n\n')
-
+
s.write('template\n')
s.write('void serialize(Archive& a, %s & m, unsigned int)\n{\n'%(cpp_msg_with_alloc))
-
+
for field in spec.parsed_fields():
s.write(' a & make_nvp("%s",m.%s);\n'%(field.name,field.name))
s.write('}\n\n')
-
+
s.write('} // namespace serialization\n')
s.write('} // namespace boost\n\n')
s.write('#endif // %s_BOOST_SERIALIZATION_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
-
+
def generate_boost_serialization(package, msg_path, msg_type, boost_header_path):
"""
Generate a boost::serialization header
-
+
@param msg_path: The path to the .msg file
@type msg_path: str
"""
@@ -83,19 +83,19 @@ def generate_boost_serialization(package, msg_path, msg_type, boost_header_path)
spec = genmsg.msg_loader.load_msg_from_file(mc, msg_path, msg_type)
cpp_prefix = '%s::'%(package)
-
+
s = StringIO()
write_boost_serialization(s, spec, cpp_prefix, msg_path)
-
+
(output_dir,filename) = os.path.split(boost_header_path)
try:
os.makedirs(output_dir)
except OSError, e:
pass
-
+
f = open(boost_header_path, 'w')
print >> f, s.getvalue()
-
+
s.close()
diff --git a/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h b/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h
index 73af4a3f..3c55e0cb 100644
--- a/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h
+++ b/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h
@@ -5,6 +5,7 @@
#include
#include
+#include
//! Abstract ROS Service Proxy
class ROSServiceProxyBase
@@ -60,7 +61,10 @@ class ROSServiceServerProxy : public ROSServiceServerProxyBase
proxy_operation_caller_.reset(new ProxyOperationCallerType("ROS_SERVICE_SERVER_PROXY"));
// Construct the ROS service server
- ros::NodeHandle nh;
+ RTT::OperationCaller resolveName =
+ RTT::internal::GlobalService::Instance()->provides("ros")->getOperation("resolveName");
+
+ ros::NodeHandle nh(resolveName(""));
server_ = nh.advertiseService(
service_name,
&ROSServiceServerProxy::ros_service_callback,
@@ -118,7 +122,10 @@ class ROSServiceClientProxy : public ROSServiceClientProxyBase
proxy_operation_.reset(new ProxyOperationType("ROS_SERVICE_CLIENT_PROXY"));
// Construct the underlying service client
- ros::NodeHandle nh;
+ RTT::OperationCaller resolveName =
+ RTT::internal::GlobalService::Instance()->provides("ros")->getOperation("resolveName");
+
+ ros::NodeHandle nh(resolveName(""));
client_ = nh.serviceClient(service_name);
// Link the operation with the service client
diff --git a/rtt_roscomm/include/rtt_roscomm/rtt_rostopic_ros_msg_transporter.hpp b/rtt_roscomm/include/rtt_roscomm/rtt_rostopic_ros_msg_transporter.hpp
index 9da60d57..18f173a9 100644
--- a/rtt_roscomm/include/rtt_roscomm/rtt_rostopic_ros_msg_transporter.hpp
+++ b/rtt_roscomm/include/rtt_roscomm/rtt_rostopic_ros_msg_transporter.hpp
@@ -53,6 +53,7 @@
#include
#include
#include
+#include
#include
#include
@@ -66,6 +67,7 @@ namespace rtt_roscomm {
template
class RosPubChannelElement: public base::ChannelElement,public RosPublisher
{
+ RTT::OperationCaller resolveName;
char hostname[1024];
std::string topicname;
ros::NodeHandle ros_node;
@@ -89,8 +91,9 @@ namespace rtt_roscomm {
* @return ChannelElement that will publish data to topics
*/
RosPubChannelElement(base::PortInterface* port,const ConnPolicy& policy):
- ros_node(),
- ros_node_private("~")
+ resolveName(RTT::internal::GlobalService::Instance()->provides("ros")->getOperation("resolveName")),
+ ros_node(resolveName("")),
+ ros_node_private(resolveName("~"))
{
if ( policy.name_id.empty() ){
std::stringstream namestr;
@@ -183,6 +186,7 @@ namespace rtt_roscomm {
template
class RosSubChannelElement: public base::ChannelElement
{
+ RTT::OperationCaller resolveName;
std::string topicname;
ros::NodeHandle ros_node;
ros::NodeHandle ros_node_private;
@@ -199,8 +203,9 @@ namespace rtt_roscomm {
* @return ChannelElement that will publish data to topics
*/
RosSubChannelElement(base::PortInterface* port, const ConnPolicy& policy) :
- ros_node(),
- ros_node_private("~")
+ resolveName(RTT::internal::GlobalService::Instance()->provides("ros")->getOperation("resolveName")),
+ ros_node(resolveName("")),
+ ros_node_private(resolveName("~"))
{
topicname=policy.name_id;
Logger::In in(topicname);
diff --git a/rtt_rosnode/src/ros_plugin.cpp b/rtt_rosnode/src/ros_plugin.cpp
index 20f3908e..2224e5b3 100644
--- a/rtt_rosnode/src/ros_plugin.cpp
+++ b/rtt_rosnode/src/ros_plugin.cpp
@@ -26,12 +26,17 @@
***************************************************************************/
+#include
#include
#include
#include
#include
#include
#include
+#include
+#include
+#include
+#include
using namespace RTT;
extern "C" {
@@ -55,6 +60,12 @@ extern "C" {
}
}
+ // Initialize default ns from node name
+ RTT::OperationCaller setNS =
+ RTT::internal::GlobalService::Instance()->provides("ros")->getOperation("setNS");
+ setNS(ros::names::append(ros::this_node::getNamespace(), ros::this_node::getName()));
+ //setNS(ros::names::append(ros::this_node::getNamespace(), ros::this_node::getName()));
+
// get number of spinners from parameter server, if available
int thread_count = 1;
ros::param::get("~spinner_threads", thread_count);
diff --git a/rtt_rosparam/src/rtt_rosparam_service.cpp b/rtt_rosparam/src/rtt_rosparam_service.cpp
index 0395a2fc..a17a0f76 100644
--- a/rtt_rosparam/src/rtt_rosparam_service.cpp
+++ b/rtt_rosparam/src/rtt_rosparam_service.cpp
@@ -2,6 +2,7 @@
#include
#include
#include
+#include
#include
#include
@@ -102,6 +103,7 @@ class ROSParamService: public RTT::Service
.doc("Sets one parameter on the ROS param server from the similarly-named property of this component (or stores the properties of a named RTT sub-service) in the component's private namespace.")
.arg("name", "Name of the property / service / parameter.");
+ this->resolveName = RTT::internal::GlobalService::Instance()->provides("ros")->getOperation("resolveName");
}
private:
@@ -145,6 +147,8 @@ class ROSParamService: public RTT::Service
bool setParamAbsolute(const std::string &name) { return set(name, ABSOLUTE); }
bool setParamPrivate(const std::string &name) { return set(name, PRIVATE); }
bool setParamComponentPrivate(const std::string &name) { return set(name, COMPONENT); }
+
+ RTT::OperationCaller resolveName;
};
const std::string ROSParamService::resolvedName(
@@ -173,6 +177,9 @@ const std::string ROSParamService::resolvedName(
break;
};
+ // Apply the active rtt_ros ns
+ resolved_name = this->resolveName(resolved_name);
+
RTT::log(RTT::Debug) << "["<getOwner()->getName()<<"] Resolving ROS param \""<