From 8d94003cc23258b0b173bc1fb31306df47e5376b Mon Sep 17 00:00:00 2001 From: Alex Spitzer Date: Mon, 23 Apr 2018 16:27:03 -0400 Subject: [PATCH 01/13] Add TransportHint options --tcpnodelay and --udp (#1295) --- tools/rosbag/include/rosbag/recorder.h | 1 + tools/rosbag/src/record.cpp | 12 +++++++++++- tools/rosbag/src/recorder.cpp | 3 ++- tools/rosbag/src/rosbag/rosbag_main.py | 4 ++++ 4 files changed, 18 insertions(+), 2 deletions(-) diff --git a/tools/rosbag/include/rosbag/recorder.h b/tools/rosbag/include/rosbag/recorder.h index 508e709e2e..8fe6a91487 100644 --- a/tools/rosbag/include/rosbag/recorder.h +++ b/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 topics; }; diff --git a/tools/rosbag/src/record.cpp b/tools/rosbag/src/record.cpp index b624e4e986..f6cbb2067f 100644 --- a/tools/rosbag/src/record.cpp +++ b/tools/rosbag/src/record.cpp @@ -66,7 +66,9 @@ rosbag::RecorderOptions parseOptions(int argc, char** argv) { ("topic", po::value< std::vector >(), "topic to record") ("size", po::value(), "The maximum size of the bag to record in MB.") ("duration", po::value(), "Record a bag of maximum duration in seconds, unless 'm', or 'h' is appended.") - ("node", po::value(), "Record all topics subscribed to by a specific node."); + ("node", po::value(), "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::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")) diff --git a/tools/rosbag/src/recorder.cpp b/tools/rosbag/src/recorder.cpp index 86cd8cade1..247488b8fd 100644 --- a/tools/rosbag/src/recorder.cpp +++ b/tools/rosbag/src/recorder.cpp @@ -209,7 +209,7 @@ int Recorder::run() { } shared_ptr 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 count(boost::make_shared(options_.limit)); @@ -223,6 +223,7 @@ shared_ptr Recorder::subscribe(string const& topic) { ops.helper = boost::make_shared &> >( boost::bind(&Recorder::doQueue, this, _1, topic, sub, count)); + ops.transport_hints = options_.transport_hints; *sub = nh.subscribe(ops); currently_recording_.insert(topic); diff --git a/tools/rosbag/src/rosbag/rosbag_main.py b/tools/rosbag/src/rosbag/rosbag_main.py index e855b1a8d9..42df3ea833 100644 --- a/tools/rosbag/src/rosbag/rosbag_main.py +++ b/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) From 017ce50ca0ba607abb192ebf1b98b47930ab76c3 Mon Sep 17 00:00:00 2001 From: Laurence de Bruxelles Date: Tue, 24 Apr 2018 15:15:18 +0100 Subject: [PATCH 02/13] topic_tools: Check that output topic is valid in demux. Fixes #1366 (#1367) --- tools/topic_tools/src/demux.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/tools/topic_tools/src/demux.cpp b/tools/topic_tools/src/demux.cpp index 8d414d4cad..6511c25a65 100644 --- a/tools/topic_tools/src/demux.cpp +++ b/tools/topic_tools/src/demux.cpp @@ -152,6 +152,9 @@ void in_cb(const boost::shared_ptr& msg) ros::Duration(0.5).sleep(); } + // check that we have a valid topic + if (!g_selected->pub) return; + // finally: send out the message over the active publisher g_selected->pub->publish(msg); ROS_DEBUG("... and sent it out again!"); @@ -307,4 +310,3 @@ int main(int argc, char **argv) g_pubs.clear(); return 0; } - From 4744c900e8bc9270d8d5f1557258322aac21e7d6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9=20Santos?= Date: Tue, 24 Apr 2018 15:41:33 +0100 Subject: [PATCH 03/13] Replaced DCL pattern with static variable. Fixes #1364 (#1365) --- clients/roscpp/src/libros/connection_manager.cpp | 14 ++------------ clients/roscpp/src/libros/poll_manager.cpp | 14 ++------------ clients/roscpp/src/libros/xmlrpc_manager.cpp | 14 ++------------ 3 files changed, 6 insertions(+), 36 deletions(-) diff --git a/clients/roscpp/src/libros/connection_manager.cpp b/clients/roscpp/src/libros/connection_manager.cpp index 98789b66ae..19fdb90cce 100644 --- a/clients/roscpp/src/libros/connection_manager.cpp +++ b/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(); - } - } - - return g_connection_manager; + static ConnectionManagerPtr connection_manager = boost::make_shared(); + return connection_manager; } ConnectionManager::ConnectionManager() diff --git a/clients/roscpp/src/libros/poll_manager.cpp b/clients/roscpp/src/libros/poll_manager.cpp index 569b9c53c7..4910d24c50 100644 --- a/clients/roscpp/src/libros/poll_manager.cpp +++ b/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(); + return poll_manager; } PollManager::PollManager() diff --git a/clients/roscpp/src/libros/xmlrpc_manager.cpp b/clients/roscpp/src/libros/xmlrpc_manager.cpp index 1aee46b1a5..15175ff5e7 100644 --- a/clients/roscpp/src/libros/xmlrpc_manager.cpp +++ b/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(); + return xmlrpc_manager; } XMLRPCManager::XMLRPCManager() From 7dca2a7ca53bce3b31af07f67beef8cb731d554a Mon Sep 17 00:00:00 2001 From: Jannik Abbenseth Date: Thu, 26 Apr 2018 20:58:48 +0200 Subject: [PATCH 04/13] Fix "pass_all_args" for roslaunch-check, add nosetest (#1368) --- tools/roslaunch/resources/example-args.launch | 3 +++ tools/roslaunch/resources/example-pass_all_args.launch | 4 ++++ tools/roslaunch/src/roslaunch/depends.py | 6 ++++++ tools/roslaunch/test/unit/test_roslaunch_rlutil.py | 6 +++++- 4 files changed, 18 insertions(+), 1 deletion(-) create mode 100644 tools/roslaunch/resources/example-args.launch create mode 100644 tools/roslaunch/resources/example-pass_all_args.launch diff --git a/tools/roslaunch/resources/example-args.launch b/tools/roslaunch/resources/example-args.launch new file mode 100644 index 0000000000..4bcf561453 --- /dev/null +++ b/tools/roslaunch/resources/example-args.launch @@ -0,0 +1,3 @@ + + + diff --git a/tools/roslaunch/resources/example-pass_all_args.launch b/tools/roslaunch/resources/example-pass_all_args.launch new file mode 100644 index 0000000000..c13e85e74d --- /dev/null +++ b/tools/roslaunch/resources/example-pass_all_args.launch @@ -0,0 +1,4 @@ + + + + diff --git a/tools/roslaunch/src/roslaunch/depends.py b/tools/roslaunch/src/roslaunch/depends.py index 79eb050d53..7e3583770f 100644 --- a/tools/roslaunch/src/roslaunch/depends.py +++ b/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)) diff --git a/tools/roslaunch/test/unit/test_roslaunch_rlutil.py b/tools/roslaunch/test/unit/test_roslaunch_rlutil.py index bd828c1f97..4950b6b470 100644 --- a/tools/roslaunch/test/unit/test_roslaunch_rlutil.py +++ b/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 From 3ce36e37ff63156671326be73787f0c44d36e51f Mon Sep 17 00:00:00 2001 From: Denise Eng Date: Fri, 27 Apr 2018 15:19:46 -0400 Subject: [PATCH 05/13] Convert the period standard deviation in StatisticsLogger to Duration at the end. (#1361) Fixes #1360 --- clients/roscpp/src/libros/statistics.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/clients/roscpp/src/libros/statistics.cpp b/clients/roscpp/src/libros/statistics.cpp index 2b0a1c299b..a618936fa8 100644 --- a/clients/roscpp/src/libros/statistics.cpp +++ b/clients/roscpp/src/libros/statistics.cpp @@ -202,7 +202,7 @@ void StatisticsLogger::callback(const boost::shared_ptr& 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::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& 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 { From 7d9e4073ab353ec9393f235ad2c703c22bb5ffed Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Thu, 10 May 2018 22:07:35 +0200 Subject: [PATCH 06/13] Fix integer overflow for oneshot timers. (#1382) * Template std::max instead of casting explicitly. --- clients/roscpp/include/ros/timer_manager.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clients/roscpp/include/ros/timer_manager.h b/clients/roscpp/include/ros/timer_manager.h index 27f8ca1f64..354c7daad1 100644 --- a/clients/roscpp/include/ros/timer_manager.h +++ b/clients/roscpp/include/ros/timer_manager.h @@ -576,7 +576,7 @@ void TimerManager::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((sleep_end - current).toSec() * 1000.0f, 1); timers_cond_.timed_wait(lock, boost::posix_time::milliseconds(remaining_time)); } } From 0463bc9a1d65c0064dce623e87bf555dad8d9268 Mon Sep 17 00:00:00 2001 From: Guillaume Autran Date: Thu, 10 May 2018 16:13:36 -0400 Subject: [PATCH 07/13] Change to force a rebuild of the pollset on flag changes (#1393) * Change to force a rebuild of the pollset on flag changes This change fixes issues seen in #1357, resulting from #1281 --- clients/roscpp/src/libros/io.cpp | 4 ++++ clients/roscpp/src/libros/poll_set.cpp | 2 ++ test/test_roscpp/test/CMakeLists.txt | 2 +- test/test_roscpp/test_serialization/CMakeLists.txt | 8 ++++---- 4 files changed, 11 insertions(+), 5 deletions(-) diff --git a/clients/roscpp/src/libros/io.cpp b/clients/roscpp/src/libros/io.cpp index 66c4a9993a..d508cca817 100644 --- a/clients/roscpp/src/libros/io.cpp +++ b/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)) diff --git a/clients/roscpp/src/libros/poll_set.cpp b/clients/roscpp/src/libros/poll_set.cpp index 7ae227d48d..50580ee806 100644 --- a/clients/roscpp/src/libros/poll_set.cpp +++ b/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; diff --git a/test/test_roscpp/test/CMakeLists.txt b/test/test_roscpp/test/CMakeLists.txt index 7328a59828..05dae977d3 100644 --- a/test/test_roscpp/test/CMakeLists.txt +++ b/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) diff --git a/test/test_roscpp/test_serialization/CMakeLists.txt b/test/test_roscpp/test_serialization/CMakeLists.txt index 27e35175d2..be7c426f60 100644 --- a/test/test_roscpp/test_serialization/CMakeLists.txt +++ b/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) From 7d860bb3fdf07907c66bf40840416c4693f594bd Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Fri, 18 May 2018 15:54:37 -0700 Subject: [PATCH 08/13] XmlRpcpp fixes for OSX (#1402) * Handle RLIM_INFINITY * XmlRpcServer: use unsigned int for rlimit * Properly link xmlrpcpp tests * Exclude test libraries from all target * Symbol overriding for OSX * fix signed / unsigned comparison warning --- utilities/xmlrpcpp/src/XmlRpcServer.cpp | 12 +++++++-- utilities/xmlrpcpp/test/CMakeLists.txt | 29 ++++++++++++++++----- utilities/xmlrpcpp/test/mock_socket.cpp | 4 +-- utilities/xmlrpcpp/test/test_dispatch.cpp | 6 ++--- utilities/xmlrpcpp/test/test_system_mocks.c | 9 +++---- 5 files changed, 40 insertions(+), 20 deletions(-) diff --git a/utilities/xmlrpcpp/src/XmlRpcServer.cpp b/utilities/xmlrpcpp/src/XmlRpcServer.cpp index 99154efacb..3b84286780 100644 --- a/utilities/xmlrpcpp/src/XmlRpcServer.cpp +++ b/utilities/xmlrpcpp/src/XmlRpcServer.cpp @@ -27,15 +27,18 @@ XmlRpcServer::XmlRpcServer() _accept_retry_time_sec(0.0) { struct rlimit limit = { .rlim_cur = 0, .rlim_max = 0 }; - int max_files = 1024; + unsigned int max_files = 1024; if(getrlimit(RLIMIT_NOFILE, &limit) == 0) { max_files = limit.rlim_max; + if( limit.rlim_max == RLIM_INFINITY ) { + max_files = 0; + } } else { XmlRpcUtil::error("Could not get open file limit: %s", strerror(errno)); } pollfds.resize(max_files); - for(int i=0; i close_calls; void XmlRpcSocket::close(int fd) { - EXPECT_LE(1, close_calls.size()); + EXPECT_LE(1u, close_calls.size()); if (close_calls.size() > 0) { int close_fd = close_calls.front(); close_calls.pop_front(); @@ -299,7 +299,7 @@ void MockSocketTest::TearDown() { void MockSocketTest::CheckCalls() { // Check that call counters and queues are empty. EXPECT_EQ(0, socket_calls); - EXPECT_EQ(0, close_calls.size()); + EXPECT_EQ(0u, close_calls.size()); EXPECT_EQ(0, setNonBlocking_calls); EXPECT_EQ(0, setReuseAddr_calls); EXPECT_EQ(0, bind_calls); diff --git a/utilities/xmlrpcpp/test/test_dispatch.cpp b/utilities/xmlrpcpp/test/test_dispatch.cpp index 0caeb29a35..364367286a 100644 --- a/utilities/xmlrpcpp/test/test_dispatch.cpp +++ b/utilities/xmlrpcpp/test/test_dispatch.cpp @@ -46,15 +46,13 @@ // those symbols instead use __wrap_xxx extern "C" { // Mock for poll -int __real_poll(struct pollfd *fds, nfds_t nfds, int timeout); - int (*fake_poll)(struct pollfd *, nfds_t, int) = 0; int __wrap_poll(struct pollfd *fds, nfds_t nfds, int timeout) { if(fake_poll) { return fake_poll(fds, nfds, timeout); } else { - return __real_poll(fds, nfds, timeout); + return 0; } } @@ -72,7 +70,7 @@ int mock_poll(struct pollfd *fds, nfds_t nfds, int timeout) { EXPECT_EQ(poll_fds.size(), nfds); EXPECT_EQ(poll_timeout, timeout); - for(nfds_t i=0; i Date: Wed, 23 May 2018 14:40:16 -0400 Subject: [PATCH 09/13] Fix the count of subscribers. (#1407) Signed-off-by: Chris Lalancette --- tools/rostopic/src/rostopic/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/rostopic/src/rostopic/__init__.py b/tools/rostopic/src/rostopic/__init__.py index a903b68664..fab37992fd 100644 --- a/tools/rostopic/src/rostopic/__init__.py +++ b/tools/rostopic/src/rostopic/__init__.py @@ -1137,7 +1137,7 @@ def _sub_rostopic_list(master, pubs, subs, publishers_only, subscribers_only, ve print(indent+"Subscribed topics:") for t, ttype, tlist in subs: if len(tlist) > 1: - print(indent+" * %s [%s] %s subscribers"%(t, ttype, len(llist))) + print(indent+" * %s [%s] %s subscribers"%(t, ttype, len(tlist))) else: print(indent+" * %s [%s] 1 subscriber"%(t, ttype)) print('') From 647ffe1c34f12e8d65da5f0931b9558318194673 Mon Sep 17 00:00:00 2001 From: Peter Baughman Date: Fri, 1 Jun 2018 12:18:08 -0700 Subject: [PATCH 10/13] Fix Issue 1346 - Make a copy of the topic list (#1416) * Make a copy of the topic list get_topics() returns a set, which is iterated by the Log constructor. Make a copy of the set (atomic in cpython!) so nobody adds or removes something while the Log constructor iterates over it * Revert "Make a copy of the topic list", Fix This reverts commit 0e019e0a604e7c49836a6254c8cffcac933ee2cc. Move the fix to the _TopicManager so anybody else calling get_topics doesn't get burned by the same issue as _rosout --- clients/rospy/src/rospy/topics.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clients/rospy/src/rospy/topics.py b/clients/rospy/src/rospy/topics.py index b61a4ecadc..4f2d8d76d7 100644 --- a/clients/rospy/src/rospy/topics.py +++ b/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()] From 38eb3382198e36702a279261b744a9e7b1b19cd1 Mon Sep 17 00:00:00 2001 From: Sam Pfeiffer Date: Thu, 7 Jun 2018 01:53:49 +1000 Subject: [PATCH 11/13] Fix some errors in some probably not frequented code paths (#1415) --- clients/rospy/src/rospy/impl/tcpros_pubsub.py | 2 +- clients/rospy/src/rospy/names.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/clients/rospy/src/rospy/impl/tcpros_pubsub.py b/clients/rospy/src/rospy/impl/tcpros_pubsub.py index a30d1b6d7a..9344156784 100644 --- a/clients/rospy/src/rospy/impl/tcpros_pubsub.py +++ b/clients/rospy/src/rospy/impl/tcpros_pubsub.py @@ -242,7 +242,7 @@ def create_transport(self, resolved_name, pub_uri, protocol_params): if type(protocol_params) != list or len(protocol_params) != 3: return 0, "ERROR: invalid TCPROS parameters", 0 if protocol_params[0] != TCPROS: - return 0, "INTERNAL ERROR: protocol id is not TCPROS: %s"%id, 0 + return 0, "INTERNAL ERROR: protocol id is not TCPROS: %s"%protocol_params[0], 0 id, dest_addr, dest_port = protocol_params sub = rospy.impl.registration.get_topic_manager().get_subscriber_impl(resolved_name) diff --git a/clients/rospy/src/rospy/names.py b/clients/rospy/src/rospy/names.py index c6c4921259..8949dcb759 100644 --- a/clients/rospy/src/rospy/names.py +++ b/clients/rospy/src/rospy/names.py @@ -46,7 +46,7 @@ is_global, is_private import rosgraph.names -from rospy.exceptions import ROSException +from rospy.exceptions import ROSException, ROSInitException from rospy.impl.validators import ParameterInvalid TOPIC_ANYTYPE = ANYTYPE #indicates that a subscriber will connect any datatype given to it From f92e9e33f4237037c8df09066336f468562bbaf2 Mon Sep 17 00:00:00 2001 From: Ollin Boer Bohan Date: Fri, 3 Aug 2018 15:56:40 -0700 Subject: [PATCH 12/13] Rename python message_filters.Cache.getLastestTime to getLatestTime (#1450) * Rename getLastestTime to getLatestTime Per directions [here](https://github.com/ros/ros_comm/issues/1449#issuecomment-403118429). * Test getLatestTime instead of getLastestTime per https://github.com/ros/ros_comm/issues/1449#issuecomment-403118429 --- utilities/message_filters/src/message_filters/__init__.py | 6 +++++- .../message_filters/test/test_message_filters_cache.py | 6 ++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/utilities/message_filters/src/message_filters/__init__.py b/utilities/message_filters/src/message_filters/__init__.py index 999e6d9b2a..0f2bf6c968 100644 --- a/utilities/message_filters/src/message_filters/__init__.py +++ b/utilities/message_filters/src/message_filters/__init__.py @@ -156,12 +156,16 @@ def getElemBeforeTime(self, stamp): return None return older[-1] - def getLastestTime(self): + def getLatestTime(self): """Return the newest recorded timestamp.""" if not self.cache_times: return None return self.cache_times[-1] + def getLastestTime(self): + """Return the newest recorded timestamp (equivalent to `getLatestTime()`, but included for backwards compatibility).""" + return self.getLatestTime() + def getOldestTime(self): """Return the oldest recorded timestamp.""" if not self.cache_times: diff --git a/utilities/message_filters/test/test_message_filters_cache.py b/utilities/message_filters/test/test_message_filters_cache.py index dabda595a8..2fabb1a7f9 100644 --- a/utilities/message_filters/test/test_message_filters_cache.py +++ b/utilities/message_filters/test/test_message_filters_cache.py @@ -103,9 +103,11 @@ def test_all_funcs(self): self.assertEqual(s, rospy.Time(3), "invalid msg return by getElemBeforeTime") - s = cache.getLastestTime() + s = cache.getLatestTime() self.assertEqual(s, rospy.Time(4), - "invalid stamp return by getLastestTime") + "invalid stamp return by getLatestTime") + self.assertEqual(s, cache.getLastestTime(), + "stamps returned by getLatestTime and getLastestTime don't match") s = cache.getOldestTime() self.assertEqual(s, rospy.Time(0), From d80aa6d4ab9a28705149ae766ab329051b8d22ad Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 3 Aug 2018 23:46:47 +0100 Subject: [PATCH 13/13] Add hasStarted()const to Timer API (#1464) --- clients/roscpp/include/ros/timer.h | 2 ++ clients/roscpp/src/libros/timer.cpp | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/clients/roscpp/include/ros/timer.h b/clients/roscpp/include/ros/timer.h index 68498751ca..2d5663e443 100644 --- a/clients/roscpp/include/ros/timer.h +++ b/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); diff --git a/clients/roscpp/src/libros/timer.cpp b/clients/roscpp/src/libros/timer.cpp index 6db917dd27..a063bc480c 100644 --- a/clients/roscpp/src/libros/timer.cpp +++ b/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();