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/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_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h

@@ -5,6 +5,7 @@
 
 #include <rtt/RTT.hpp>
 #include <rtt/plugin/ServicePlugin.hpp>
+#include <rtt/internal/GlobalService.hpp>
 
 //! 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<std::string(const std::string&)> resolveName =
+      RTT::internal::GlobalService::Instance()->provides("ros")->getOperation("resolveName");
+
+    ros::NodeHandle nh(resolveName(""));
     server_ = nh.advertiseService(
         service_name, 
         &ROSServiceServerProxy<ROS_SERVICE_T>::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<std::string(const std::string&)> resolveName =
+      RTT::internal::GlobalService::Instance()->provides("ros")->getOperation("resolveName");
+
+    ros::NodeHandle nh(resolveName(""));
     client_ = nh.serviceClient<ROS_SERVICE_T>(service_name);
 
     // Link the operation with the service client

rtt_roscomm/include/rtt_roscomm/rtt_rostopic_ros_msg_transporter.hpp

@@ -53,6 +53,7 @@
 #include <rtt/Port.hpp>
 #include <rtt/TaskContext.hpp>
 #include <rtt/internal/ConnFactory.hpp>
+#include <rtt/internal/GlobalService.hpp>
 #include <ros/ros.h>
 
 #include <rtt_roscomm/rtt_rostopic_ros_publish_activity.hpp>
@@ -66,6 +67,7 @@ namespace rtt_roscomm {
   template<typename T>
   class RosPubChannelElement: public base::ChannelElement<T>,public RosPublisher
   {
+    RTT::OperationCaller<std::string(const std::string&)> 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<typename T>
   class RosSubChannelElement: public base::ChannelElement<T>
   {
+    RTT::OperationCaller<std::string(const std::string&)> 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);

rtt_rosnode/src/ros_plugin.cpp

@@ -26,12 +26,17 @@
  ***************************************************************************/
 
 
+#include <rtt/RTT.hpp>
 #include <rtt/plugin/Plugin.hpp>
 #include <rtt/TaskContext.hpp>
 #include <rtt/Activity.hpp>
 #include <rtt/Logger.hpp>
 #include <rtt/os/startstop.h>
 #include <ros/ros.h>
+#include <ros/names.h>
+#include <ros/this_node.h>
+#include <rtt/internal/GlobalService.hpp>
+#include <string>
 
 using namespace RTT;
 extern "C" {
@@ -55,6 +60,12 @@ extern "C" {
       }
     }
 
+    // Initialize default ns from node name
+    RTT::OperationCaller<std::string(const std::string&)> 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);

rtt_rosparam/src/rtt_rosparam_service.cpp

@@ -2,6 +2,7 @@
 #include <rtt/Property.hpp>
 #include <rtt/plugin/ServicePlugin.hpp>
 #include <rtt/types/PropertyDecomposition.hpp>
+#include <rtt/internal/GlobalService.hpp>
 
 #include <boost/utility/enable_if.hpp>
 #include <boost/type_traits/is_convertible.hpp>
@@ -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<std::string(const std::string&)> 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) << "["<<this->getOwner()->getName()<<"] Resolving ROS param \""<<param_name<<"\" to \""<<resolved_name<<"\"" << RTT::endlog();
 
   return resolved_name;