From c20ac4e671ffd3c632f9243447fa748fc755a56d Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Tue, 5 Aug 2014 14:16:59 -0400 Subject: [PATCH 1/6] rtt_actionlib: adding virtual destructor --- rtt_actionlib/include/rtt_actionlib/rtt_action_server.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rtt_actionlib/include/rtt_actionlib/rtt_action_server.h b/rtt_actionlib/include/rtt_actionlib/rtt_action_server.h index 2887fd87..2e28c931 100644 --- a/rtt_actionlib/include/rtt_actionlib/rtt_action_server.h +++ b/rtt_actionlib/include/rtt_actionlib/rtt_action_server.h @@ -35,6 +35,8 @@ namespace rtt_actionlib { virtual ~RTTActionServerStatusTimer() { } + virtual ~RTTActionServerStatusTimer() { } + //! Publish the action server status virtual void timeout(RTT::os::Timer::TimerId timer_id) { server_.publishStatus(); From 629ff037411be341ca4cd3a5d2a35fd61e2db0d5 Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Tue, 5 Aug 2014 15:12:43 -0400 Subject: [PATCH 2/6] rtt_actionlib: fixing bad merge --- rtt_actionlib/include/rtt_actionlib/rtt_action_server.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/rtt_actionlib/include/rtt_actionlib/rtt_action_server.h b/rtt_actionlib/include/rtt_actionlib/rtt_action_server.h index 2e28c931..2887fd87 100644 --- a/rtt_actionlib/include/rtt_actionlib/rtt_action_server.h +++ b/rtt_actionlib/include/rtt_actionlib/rtt_action_server.h @@ -35,8 +35,6 @@ namespace rtt_actionlib { virtual ~RTTActionServerStatusTimer() { } - virtual ~RTTActionServerStatusTimer() { } - //! Publish the action server status virtual void timeout(RTT::os::Timer::TimerId timer_id) { server_.publishStatus(); From 7ad3b666c09f1df252fb4c92a5212ee7e47f2a8a Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Wed, 22 Oct 2014 11:31:52 -0400 Subject: [PATCH 3/6] fixup whitespace --- rtt_roscomm/cmake/create_boost_header.py | 30 ++++++++++++------------ 1 file changed, 15 insertions(+), 15 deletions(-) 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() From 50aa60a1330b740ee5d023623a55ef7209ef9594 Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Mon, 9 Feb 2015 11:41:35 -0500 Subject: [PATCH 4/6] rtt_ros: re-adding rttlua scripts --- rtt_ros/launch/rttlua.launch | 33 +++++++++++++++++++++++++++++++++ rtt_ros/scripts/rttlua-debug | 9 +++++++++ rtt_ros/scripts/rttlua-screen | 24 ++++++++++++++++++++++++ 3 files changed, 66 insertions(+) create mode 100644 rtt_ros/launch/rttlua.launch create mode 100755 rtt_ros/scripts/rttlua-debug create mode 100755 rtt_ros/scripts/rttlua-screen 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 + From 1390b990c7c4bc5cfbac666bbb39ff4b6ec6acfc Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Wed, 8 Oct 2014 17:21:58 -0400 Subject: [PATCH 5/6] Adding ROS namespace manipulation operations Conflicts resolved: rtt_ros/src/rtt_ros_service.cpp --- rtt_ros/CMakeLists.txt | 2 +- rtt_ros/include/rtt_ros/rtt_ros.h | 20 +++++++++ rtt_ros/src/rtt_ros.cpp | 45 +++++++++++++++++++ rtt_ros/src/rtt_ros_service.cpp | 29 ++++++++++-- .../rtt_roscomm/rtt_rosservice_proxy.h | 11 ++++- .../rtt_rostopic_ros_msg_transporter.hpp | 13 ++++-- rtt_rosnode/src/ros_plugin.cpp | 11 +++++ rtt_rosparam/src/rtt_rosparam_service.cpp | 7 +++ 8 files changed, 128 insertions(+), 10 deletions(-) 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/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_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 \""< Date: Wed, 29 Apr 2015 15:04:43 -0400 Subject: [PATCH 6/6] adding rtt ros profiler and throttle --- rtt_rosclock/include/rtt_rosclock/prof.h | 89 ++++++++++++++++++++ rtt_rosclock/include/rtt_rosclock/throttle.h | 30 +++++++ 2 files changed, 119 insertions(+) create mode 100644 rtt_rosclock/include/rtt_rosclock/prof.h create mode 100644 rtt_rosclock/include/rtt_rosclock/throttle.h 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__