Merge pull request #1490 from ros/kinetic-backports

changes between 1.12.13 and 1.13.7 for backporting

clients/roscpp/include/ros/timer.h

@@ -71,6 +71,7 @@ class ROSCPP_DECL Timer
    */
   void setPeriod(const Duration& period, bool reset=true);
 
+  bool hasStarted() const { return impl_->hasStarted(); }
   bool isValid() { return impl_ && impl_->isValid(); }
   operator void*() { return isValid() ? (void*)1 : (void*)0; }
 
@@ -98,6 +99,7 @@ class ROSCPP_DECL Timer
     Impl();
     ~Impl();
 
+    bool hasStarted() const;
     bool isValid();
     bool hasPending();
     void setPeriod(const Duration& period, bool reset=true);

clients/roscpp/include/ros/timer_manager.h

@@ -576,7 +576,7 @@ void TimerManager<T, D, E>::threadFunc()
       {
         // On system time we can simply sleep for the rest of the wait time, since anything else requiring processing will
         // signal the condition variable
-        int32_t remaining_time = std::max((int32_t)((sleep_end - current).toSec() * 1000.0f), 1);
+        int64_t remaining_time = std::max<int64_t>((sleep_end - current).toSec() * 1000.0f, 1);
         timers_cond_.timed_wait(lock, boost::posix_time::milliseconds(remaining_time));
       }
     }

clients/roscpp/src/libros/connection_manager.cpp

@@ -40,20 +40,10 @@
 namespace ros
 {
 
-ConnectionManagerPtr g_connection_manager;
-boost::mutex g_connection_manager_mutex;
 const ConnectionManagerPtr& ConnectionManager::instance()
 {
-  if (!g_connection_manager)
-  {
-    boost::mutex::scoped_lock lock(g_connection_manager_mutex);
-    if (!g_connection_manager)
-    {
-      g_connection_manager = boost::make_shared<ConnectionManager>();
-    }
-  }
-
-  return g_connection_manager;
+  static ConnectionManagerPtr connection_manager = boost::make_shared<ConnectionManager>();
+  return connection_manager;
 }
 
 ConnectionManager::ConnectionManager()

clients/roscpp/src/libros/io.cpp

@@ -119,6 +119,8 @@ void close_socket_watcher(int fd) {
 void add_socket_to_watcher(int epfd, int fd) {
 #if defined(HAVE_EPOLL)
   struct epoll_event ev;
+  bzero(&ev, sizeof(ev));
+
   ev.events = 0;
   ev.data.fd = fd;
 
@@ -147,6 +149,8 @@ void del_socket_from_watcher(int epfd, int fd) {
 void set_events_on_socket(int epfd, int fd, int events) {
 #if defined(HAVE_EPOLL)
   struct epoll_event ev;
+  bzero(&ev, sizeof(ev));
+
   ev.events = events;
   ev.data.fd = fd;
   if (::epoll_ctl(epfd, EPOLL_CTL_MOD, fd, &ev))

clients/roscpp/src/libros/poll_manager.cpp

@@ -33,20 +33,10 @@
 namespace ros
 {
 
-PollManagerPtr g_poll_manager;
-boost::mutex g_poll_manager_mutex;
 const PollManagerPtr& PollManager::instance()
 {
-  if (!g_poll_manager)
-  {
-    boost::mutex::scoped_lock lock(g_poll_manager_mutex);
-    if (!g_poll_manager)
-    {
-      g_poll_manager.reset(new PollManager);
-    }
-  }
-
-  return g_poll_manager;
+  static PollManagerPtr poll_manager = boost::make_shared<PollManager>();
+  return poll_manager;
 }
 
 PollManager::PollManager()

clients/roscpp/src/libros/poll_set.cpp

@@ -139,6 +139,7 @@ bool PollSet::addEvents(int sock, int events)
 
   set_events_on_socket(epfd_, sock, it->second.events_);
 
+  sockets_changed_ = true;
   signal();
 
   return true;
@@ -161,6 +162,7 @@ bool PollSet::delEvents(int sock, int events)
 
   set_events_on_socket(epfd_, sock, it->second.events_);
 
+  sockets_changed_ = true;
   signal();
 
   return true;

clients/roscpp/src/libros/statistics.cpp

@@ -202,7 +202,7 @@ void StatisticsLogger::callback(const boost::shared_ptr<M_string>& connection_he
       msg.period_mean *= 1.0 / (stats.arrival_time_list.size() - 1);
 
       // then, calc the stddev
-      msg.period_stddev = ros::Duration(0);
+      double period_variance = 0.0;
       for(std::list<ros::Time>::iterator it = stats.arrival_time_list.begin(); it != stats.arrival_time_list.end(); it++)
       {
         if (it == stats.arrival_time_list.begin())
@@ -212,11 +212,11 @@ void StatisticsLogger::callback(const boost::shared_ptr<M_string>& connection_he
         }
         ros::Duration period = *it - prev;
         ros::Duration t = msg.period_mean - period;
-        msg.period_stddev += ros::Duration(t.toSec() * t.toSec());
+        period_variance += t.toSec() * t.toSec();
         prev = *it;
       }
-      msg.period_stddev = ros::Duration(sqrt(msg.period_stddev.toSec() / (stats.arrival_time_list.size() - 1)));
-
+      double period_stddev = sqrt(period_variance / (stats.arrival_time_list.size() - 1));
+      msg.period_stddev = ros::Duration(period_stddev);
     }
     else
     {

clients/roscpp/src/libros/timer.cpp

@@ -42,6 +42,11 @@ Timer::Impl::~Impl()
   stop();
 }
 
+bool Timer::Impl::hasStarted() const
+{
+  return started_;
+}
+
 bool Timer::Impl::isValid()
 {
   return !period_.isZero();

clients/roscpp/src/libros/xmlrpc_manager.cpp

@@ -95,20 +95,10 @@ void getPid(const XmlRpcValue& params, XmlRpcValue& result)
 
 const ros::WallDuration CachedXmlRpcClient::s_zombie_time_(30.0); // reap after 30 seconds
 
-XMLRPCManagerPtr g_xmlrpc_manager;
-boost::mutex g_xmlrpc_manager_mutex;
 const XMLRPCManagerPtr& XMLRPCManager::instance()
 {
-  if (!g_xmlrpc_manager)
-  {
-    boost::mutex::scoped_lock lock(g_xmlrpc_manager_mutex);
-    if (!g_xmlrpc_manager)
-    {
-      g_xmlrpc_manager.reset(new XMLRPCManager);
-    }
-  }
-
-  return g_xmlrpc_manager;
+  static XMLRPCManagerPtr xmlrpc_manager = boost::make_shared<XMLRPCManager>();
+  return xmlrpc_manager;
 }
 
 XMLRPCManager::XMLRPCManager()

clients/rospy/src/rospy/topics.py

@@ -1348,7 +1348,7 @@ def get_topics(self):
         @return: list of topic names this node subscribes to/publishes
         @rtype: [str]
         """                
-        return self.topics
+        return self.topics.copy()
 
     def _get_list(self, rmap):
         return [[k, v.type] for k, v in rmap.items()]

test/test_roscpp/test/CMakeLists.txt

@@ -1,6 +1,6 @@
 catkin_add_gtest(${PROJECT_NAME}-test_version test_version.cpp)
 if(TARGET ${PROJECT_NAME}-test_version)
-  target_link_libraries(${PROJECT_NAME}-test_version)
+  target_link_libraries(${PROJECT_NAME}-test_version ${catkin_LIBRARIES})
 endif()
 
 catkin_add_gtest(${PROJECT_NAME}-test_header test_header.cpp)

test/test_roscpp/test_serialization/CMakeLists.txt

@@ -4,24 +4,24 @@ endif()
 
 catkin_add_gtest(${PROJECT_NAME}-serialization src/serialization.cpp)
 if(TARGET ${PROJECT_NAME}-serialization)
-  target_link_libraries(${PROJECT_NAME}-serialization ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
+  target_link_libraries(${PROJECT_NAME}-serialization ${GTEST_LIBRARIES} ${catkin_LIBRARIES})
   add_dependencies(${PROJECT_NAME}-serialization ${${PROJECT_NAME}_EXPORTED_TARGETS})
 endif()
 
 catkin_add_gtest(${PROJECT_NAME}-generated_messages src/generated_messages.cpp)
 if(TARGET ${PROJECT_NAME}-generated_messages)
-  target_link_libraries(${PROJECT_NAME}-generated_messages ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
+  target_link_libraries(${PROJECT_NAME}-generated_messages ${GTEST_LIBRARIES} ${catkin_LIBRARIES})
   add_dependencies(${PROJECT_NAME}-generated_messages ${${PROJECT_NAME}_EXPORTED_TARGETS})
 endif()
 
 add_executable(${PROJECT_NAME}-builtin_types EXCLUDE_FROM_ALL src/builtin_types.cpp)
 add_dependencies(tests ${PROJECT_NAME}-builtin_types) 
-target_link_libraries(${PROJECT_NAME}-builtin_types ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}-builtin_types ${GTEST_LIBRARIES} ${catkin_LIBRARIES})
 add_dependencies(${PROJECT_NAME}-builtin_types ${${PROJECT_NAME}_EXPORTED_TARGETS})
 add_rostest(test/builtin_types.test)
 
 add_executable(${PROJECT_NAME}-pre_deserialize EXCLUDE_FROM_ALL src/pre_deserialize.cpp)
 add_dependencies(tests ${PROJECT_NAME}-pre_deserialize)
-target_link_libraries(${PROJECT_NAME}-pre_deserialize ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}-pre_deserialize ${GTEST_LIBRARIES} ${catkin_LIBRARIES})
 add_dependencies(${PROJECT_NAME}-pre_deserialize ${${PROJECT_NAME}_EXPORTED_TARGETS})
 add_rostest(test/pre_deserialize.test)

tools/rosbag/include/rosbag/recorder.h

@@ -110,6 +110,7 @@ struct ROSBAG_DECL RecorderOptions
     std::string     node;
     unsigned long long min_space;
     std::string min_space_str;
+    ros::TransportHints transport_hints;
 
     std::vector<std::string> topics;
 };

tools/rosbag/src/player.cpp

@@ -305,11 +305,12 @@ void Player::updateRateTopicTime(const ros::MessageEvent<topic_tools::ShapeShift
     std::string s;
     bool flag = false;
     while(std::getline(f, s, '\n')) {
-        if (s.find("#") != 0) {
+        if (!s.empty() && s.find("#") != 0) {
             // Does not start with #, is not a comment.
-            if(s == "Header header") {
+            if (s.find("Header ") == 0) {
                 flag = true;
             }
+            break;
         }
     }
     // If the header is not the first element in the message according to the definition, throw an error.

tools/rosbag/src/record.cpp

@@ -66,7 +66,9 @@ rosbag::RecorderOptions parseOptions(int argc, char** argv) {
       ("topic", po::value< std::vector<std::string> >(), "topic to record")
       ("size", po::value<uint64_t>(), "The maximum size of the bag to record in MB.")
       ("duration", po::value<std::string>(), "Record a bag of maximum duration in seconds, unless 'm', or 'h' is appended.")
-      ("node", po::value<std::string>(), "Record all topics subscribed to by a specific node.");
+      ("node", po::value<std::string>(), "Record all topics subscribed to by a specific node.")
+      ("tcpnodelay", "Use the TCP_NODELAY transport hint when subscribing to topics.")
+      ("udp", "Use the UDP transport hint when subscribing to topics.");
 
 
     po::positional_options_description p;
@@ -238,6 +240,14 @@ rosbag::RecorderOptions parseOptions(int argc, char** argv) {
       opts.node = vm["node"].as<std::string>();
       std::cout << "Recording from: " << opts.node << std::endl;
     }
+    if (vm.count("tcpnodelay"))
+    {
+      opts.transport_hints.tcpNoDelay();
+    }
+    if (vm.count("udp"))
+    {
+      opts.transport_hints.udp();
+    }
 
     // Every non-option argument is assumed to be a topic
     if (vm.count("topic"))

tools/rosbag/src/recorder.cpp

@@ -209,7 +209,7 @@ int Recorder::run() {
 }
 
 shared_ptr<ros::Subscriber> Recorder::subscribe(string const& topic) {
-	ROS_INFO("Subscribing to %s", topic.c_str());
+    ROS_INFO("Subscribing to %s", topic.c_str());
 
     ros::NodeHandle nh;
     shared_ptr<int> count(boost::make_shared<int>(options_.limit));
@@ -223,6 +223,7 @@ shared_ptr<ros::Subscriber> Recorder::subscribe(string const& topic) {
     ops.helper = boost::make_shared<ros::SubscriptionCallbackHelperT<
         const ros::MessageEvent<topic_tools::ShapeShifter const> &> >(
             boost::bind(&Recorder::doQueue, this, _1, topic, sub, count));
+    ops.transport_hints = options_.transport_hints;
     *sub = nh.subscribe(ops);
 
     currently_recording_.insert(topic);

tools/rosbag/src/rosbag/rosbag_main.py

@@ -93,6 +93,8 @@ def record_cmd(argv):
     parser.add_option(      "--node",          dest="node",          default=None,  type='string',action="store", help="record all topics subscribed to by a specific node")
     parser.add_option("-j", "--bz2",           dest="compression",   default=None,  action="store_const", const='bz2', help="use BZ2 compression")
     parser.add_option("--lz4",                 dest="compression",                  action="store_const", const='lz4', help="use LZ4 compression")
+    parser.add_option("--tcpnodelay",          dest="tcpnodelay",                   action="store_true",          help="Use the TCP_NODELAY transport hint when subscribing to topics.")
+    parser.add_option("--udp",                 dest="udp",                          action="store_true",          help="Use the UDP transport hint when subscribing to topics.")
 
     (options, args) = parser.parse_args(argv)
 
@@ -128,6 +130,8 @@ def record_cmd(argv):
     if options.size:        cmd.extend(["--size", str(options.size)])
     if options.node:
         cmd.extend(["--node", options.node])
+    if options.tcpnodelay:  cmd.extend(["--tcpnodelay"])
+    if options.udp:         cmd.extend(["--udp"])
 
     cmd.extend(args)
 

tools/roslaunch/resources/example-args.launch

@@ -0,0 +1,3 @@
+<launch>
+  <arg name="test_foo" />
+</launch>

tools/roslaunch/resources/example-pass_all_args.launch

@@ -0,0 +1,4 @@
+<launch>
+  <arg name="test_foo" default="value_foo" />
+  <include file="$(find roslaunch)/resources/example-args.launch" pass_all_args="true" />
+</launch>

tools/roslaunch/src/roslaunch/depends.py

@@ -170,6 +170,12 @@ def _parse_launch(tags, launch_file, file_deps, verbose, context):
                 else:
                     launch_tag = dom[0]
                     sub_context = _parse_subcontext(tag.childNodes, context)
+                    try:
+                        if tag.attributes['pass_all_args']:
+                            sub_context["arg"] = context["arg"]
+                            sub_context["arg"].update(_parse_subcontext(tag.childNodes, context)["arg"])
+                    except KeyError as e:
+                        pass
                     _parse_launch(launch_tag.childNodes, sub_launch_file, file_deps, verbose, sub_context)
             except IOError as e:
                 raise RoslaunchDepsException("Cannot load roslaunch include '%s' in '%s'"%(sub_launch_file, launch_file))

tools/roslaunch/src/roslaunch/loader.py

@@ -358,7 +358,7 @@ def add_param(self, ros_config, param_name, param_value, verbose=True):
         else:
             ros_config.add_param(Param(param_name, param_value), verbose=verbose)
 
-    def load_rosparam(self, context, ros_config, cmd, param, file_, text, verbose=True):
+    def load_rosparam(self, context, ros_config, cmd, param, file_, text, verbose=True, subst_function=None):
         """
         Load rosparam setting
         
@@ -394,6 +394,8 @@ def load_rosparam(self, context, ros_config, cmd, param, file_, text, verbose=Tr
                 with open(file_, 'r') as f:
                     text = f.read()
 
+            if subst_function is not None:
+                text = subst_function(text)
             # parse YAML text
             # - lazy import
             global yaml

tools/roslaunch/src/roslaunch/xmlloader.py

@@ -233,9 +233,10 @@ def _rosparam_tag(self, tag, context, ros_config, verbose=True):
             # load is the default command            
             cmd = cmd or 'load'
             value = _get_text(tag)
+            subst_function = None
             if subst_value:
-                value = self.resolve_args(value, context)
-            self.load_rosparam(context, ros_config, cmd, param, file, value, verbose=verbose)
+                subst_function = lambda x: self.resolve_args(x, context)
+            self.load_rosparam(context, ros_config, cmd, param, file, value, verbose=verbose, subst_function=subst_function)
 
         except ValueError as e:
             raise loader.LoadException("error loading <rosparam> tag: \n\t"+str(e)+"\nXML is %s"%tag.toxml())

tools/roslaunch/test/params_subst.yaml

@@ -0,0 +1 @@
+string1: $(anon foo)

tools/roslaunch/test/unit/test_roslaunch_rlutil.py

@@ -84,4 +84,8 @@ def test_resolve_launch_arguments(self):
                 self.fail("should have failed")
             except roslaunch.RLException:
                 pass
-
+
+    def test_roslaunch_check_pass_all_args(self):
+        filename = os.path.join(get_example_path(), 'example-pass_all_args.launch')
+        error_msg = roslaunch.rlutil.check_roslaunch(filename)
+        assert error_msg is None

tools/roslaunch/test/unit/test_xmlloader.py

@@ -242,6 +242,10 @@ def test_rosparam_valid(self):
         self.assertEquals('bar', p.value)
         p = [p for p in mock.params if p.key == '/node_rosparam/robots/childparam'][0]
         self.assertEquals('a child namespace parameter', p.value)
+
+        # test substitution in yaml files
+        p = [p for p in mock.params if p.key == '/rosparam_subst/string1'][0]
+        self.assertTrue('$(anon foo)' not in p.value)
 
         exes = [e for e in mock.executables if e.command == 'rosparam']
         self.assertEquals(len(exes), 2, "expected 2 rosparam exes, got %s"%len(exes))
@@ -274,6 +278,10 @@ def test_rosparam_valid(self):
         p = [p for p in mock.params if p.key == '/inline_dict2/key4'][0]
         self.assertEquals('value4', p.value)
 
+        # test substitution in inline yaml
+        p = [p for p in mock.params if p.key == '/inline_subst'][0]
+        self.assertTrue('$(anon foo)' not in p.value)
+
         # verify that later tags override 
         # - key2 is overriden
         self.assertEquals(1, len([p for p in mock.params if p.key == '/override/key1']))