Adding ROS namespace manipulation operations

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)

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

rtt_ros/launch/rttlua.launch

@@ -0,0 +1,33 @@
+<launch>
+  <!-- The node name for the deployer -->
+  <arg name="NAME" default="$(anon rttlua)"/>
+  <!-- Additional Orocos deployer args -->
+  <arg name="RTTLUA_ARGS" default=""/>
+  <!-- Orocos target (gnulinux, xenomai, etc) -->
+  <arg name="OROCOS_TARGET" default="$(optenv OROCOS_TARGET)"/>
+  <!-- Path to look for dynamically-loaded plugins and components (something like /lib/orocos) -->
+  <arg name="RTT_COMPONENT_PATH" default="$(env RTT_COMPONENT_PATH)"/>
+
+  <!-- Set DEBUG to true to run in GDB (don't forget to build in Debug mode) -->
+  <arg name="DEBUG" default="false"/>
+  <arg if="$(arg DEBUG)" name="SUFFIX" value="-debug"/>
+  <arg unless="$(arg DEBUG)" name="SUFFIX" value=""/>
+
+  <arg name="SCREEN" default="false"/>
+  <arg if="$(arg SCREEN)" name="SUFFIX2" value="-screen"/>
+  <arg unless="$(arg SCREEN)" name="SUFFIX2" value="$(arg SUFFIX2)"/>
+
+  <arg name="INTERACTIVE" default="true"/>
+  <arg if="$(arg INTERACTIVE)" name="INTERACTIVE_ARG" value="-i"/>
+  <arg unless="$(arg INTERACTIVE)" name="INTERACTIVE_ARG" value=""/>
+
+  <!-- Launch deployer -->
+  <node 
+    name="$(arg NAME)"
+    pkg="rtt_ros" type="rttlua$(arg SUFFIX2)" 
+    args="$(arg INTERACTIVE_ARG) $(arg RTTLUA_ARGS) -"
+    output="screen">
+    <env name="OROCOS_TARGET" value="$(arg OROCOS_TARGET)"/>
+    <env name="RTT_COMPONENT_PATH" value="$(arg RTT_COMPONENT_PATH)"/>
+  </node>
+</launch>

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 "$@"

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
+

rtt_ros/src/rtt_ros.cpp

@@ -22,6 +22,8 @@
 #include <rospack/rospack.h>
 
 #include <rtt_ros/rtt_ros.h>
+#include <ros/names.h>
+#include <ros/this_node.h>
 
 
 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));
+}

rtt_ros/src/rtt_ros_service.cpp

@@ -18,19 +18,42 @@
 #include <rtt/deployment/ComponentLoader.hpp>
 #include <rtt/Logger.hpp>
 
+#include <ros/names.h>
+#include <ros/this_node.h>
 #include <rospack/rospack.h>
 
 #include <rtt_ros/rtt_ros.h>
 
 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;

rtt_rosclock/include/rtt_rosclock/prof.h

@@ -0,0 +1,89 @@
+#ifndef __RTT_ROSCLOCK_PROF_H__
+#define __RTT_ROSCLOCK_PROF_H__
+
+#include <queue>
+#include <rtt_rosclock/rtt_rosclock.h>
+
+namespace rtt_rosclock {
+  class WallProf {
+  public:
+    typedef std::pair<ros::Time, ros::Time> 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<TicToc>::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<TicToc>::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<TicToc> tictocs_;
+
+    double mean_;
+    double min_;
+    double max_;
+    double stddev_;
+  };
+}
+
+#endif // ifndef __RTT_ROSCLOCK_PROF_H__

rtt_rosclock/include/rtt_rosclock/throttle.h

@@ -0,0 +1,30 @@
+#ifndef __RTT_ROSCLOCK_THROTTLE_H__
+#define __RTT_ROSCLOCK_THROTTLE_H__
+
+#include <rtt_rosclock/rtt_rosclock.h>
+
+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__