Merge branch 'master' into ip

.travis.yml

@@ -16,8 +16,6 @@ notifications:
   slack: jsk-robotics:Av7tc8wj3IWkLYvlTzHE7x2g
 env:
   global:
-    - ROSWS=wstool
-    - BUILDER=catkin
     - ROS_PARALLEL_JOBS="-j8"
     - ROS_PARALLEL_TEST_JOBS="-j1"
     - CATKIN_PARALLEL_TEST_JOBS="-j1"

jsk_data/node_scripts/data_collection_server.py

@@ -90,16 +90,22 @@ def sub_cb(self, msg, topic_name):
 
     def service_cb(self, req):
         now = rospy.Time.now()
-        while any(msg.header.stamp < now for msg in self.msg.values()):
+        saving_msgs = {}
+        slop = 0.1
+        while len(saving_msgs) < len(self.topics):
+            for topic in self.topics:
+                if topic['name'] in saving_msgs:
+                    continue
+                if ((topic['name'] in self.msg) and
+                     abs(now - self.msg[topic['name']].header.stamp <
+                         rospy.Duration(slop))):
+                    saving_msgs[topic['name']] = self.msg[topic['name']]
             rospy.sleep(0.1)
         save_dir = osp.join(self.save_dir, str(now.to_nsec()))
         if not osp.exists(save_dir):
             os.makedirs(save_dir)
         for topic in self.topics:
-            msg = self.msg.get(topic['name'], None)
-            if msg is None:
-                os.removedirs(save_dir)
-                return TriggerResponse(success=False)
+            msg = saving_msgs[topic['name']]
             if topic['savetype'] == 'ColorImage':
                 bridge = cv_bridge.CvBridge()
                 img = bridge.imgmsg_to_cv2(msg, 'bgr8')
@@ -109,6 +115,10 @@ def service_cb(self, req):
                 depth = bridge.imgmsg_to_cv2(msg)
                 with open(osp.join(save_dir, topic['fname']), 'wb') as f:
                     pkl.dump(depth, f)
+            elif topic['savetype'] == 'LabelImage':
+                bridge = cv_bridge.CvBridge()
+                label = bridge.imgmsg_to_cv2(msg)
+                cv2.imwrite(osp.join(save_dir, topic['fname']), label)
             else:
                 rospy.logerr('Unexpected savetype for topic: {}'
                              .format(topic['savetype']))

jsk_data/src/jsk_data/download_data.py

@@ -78,14 +78,14 @@ def download_data(pkg_name, path, url, md5, download_client=None,
             download_client = 'wget'
     if compressed_bags is None:
         compressed_bags = []
-    # get package path
-    rp = rospkg.RosPack()
-    pkg_path = rp.get_path(pkg_name)
     if not osp.isabs(path):
+        # get package path
+        rp = rospkg.RosPack()
+        pkg_path = rp.get_path(pkg_name)
         path = osp.join(pkg_path, path)
     # prepare cache dir
     ros_home = os.getenv('ROS_HOME', osp.expanduser('~/.ros'))
-    cache_dir = osp.join(ros_home, 'data')
+    cache_dir = osp.join(ros_home, 'data', pkg_name)
     if not osp.exists(cache_dir):
         os.makedirs(cache_dir)
     cache_file = osp.join(cache_dir, osp.basename(path))
@@ -109,5 +109,8 @@ def download_data(pkg_name, path, url, md5, download_client=None,
     if extract:
         extract_file(path, to_directory=osp.dirname(path))
     for compressed_bag in compressed_bags:
-        compressed_bag = osp.join(pkg_path, compressed_bag)
+        if not osp.isabs(compressed_bag):
+            rp = rospkg.RosPack()
+            pkg_path = rp.get_path(pkg_name)
+            compressed_bag = osp.join(pkg_path, compressed_bag)
         decompress_rosbag(compressed_bag, quiet=quiet)

jsk_tools/launch/record_axis_camera.launch

@@ -1,5 +1,7 @@
 <launch>
+  <arg name="stamped_filename" default="false" />
   <arg name="filename" default="output.avi" />
+  <arg name="fps" default="15" />
   <arg name="hostname" default="133.11.216.141" />
   <arg name="username" default="root" />
   <arg name="camera_info_url" default="package://jsk_tools/info/133_11_216_141.yaml" />
@@ -23,8 +25,12 @@
     <node name="image_proc" pkg="image_proc" type="image_proc" />
     <node name="video_recorder" pkg="image_view" type="video_recorder" output="screen">
       <remap from="image" to="image_rect_color" />
-      <param name="filename" value="$(arg filename)" />
-      <param name="max_depth_range" value="0.0" />
+      <rosparam subst_value="true">
+        fps: $(arg fps)
+        stamped_filename: $(arg stamped_filename)
+        filename: $(arg filename)
+        max_depth_range: 0.0
+      </rosparam>
     </node>
   </group>
 </launch>

jsk_topic_tools/CMakeLists.txt

@@ -125,6 +125,16 @@ if (CATKIN_ENABLE_TESTING)
   target_link_libraries(test_rosparam_utils ${PROJECT_NAME})
   add_rostest_gtest(test_log_utils test/test_log_utils.launch src/tests/test_log_utils.cpp)
   target_link_libraries(test_log_utils ${PROJECT_NAME})
+
+  add_library(jsk_topic_tools_test
+    src/tests/test_nodelet_log_utils.cpp
+    src/log_utils.cpp)
+  target_link_libraries(jsk_topic_tools_test
+    ${catkin_LIBRARIES} ${BOOST_LIBRARIES})
+  add_executable(test_nodelet_log EXCLUDE_FROM_ALL test/test_nodelet_log.cpp)
+  target_link_libraries(test_nodelet_log ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
+  add_dependencies(tests test_nodelet_log)
+  add_rostest(test/test_nodelet_log.test)
   if("$ENV{ROS_DISTRO}" STRGREATER "hydro")
     # FIXME: jsk_tools/test_topic_published.py does not work on hydro travis/jenkins
     # https://github.com/jsk-ros-pkg/jsk_common/pull/1293#issuecomment-164158260

jsk_topic_tools/include/jsk_topic_tools/log_utils.h

@@ -83,6 +83,28 @@ namespace jsk_topic_tools
 #define JSK_NODELET_FATAL_STREAM(...) \
   NODELET_FATAL_STREAM("[" << jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str() << "] " << __VA_ARGS__)
 
+#define JSK_NODELET_DEBUG_THROTTLE(rate, str, ...) \
+  NODELET_DEBUG_THROTTLE(rate, "[%s] " str, jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str(), ##__VA_ARGS__)
+#define JSK_NODELET_INFO_THROTTLE(rate, str, ...) \
+  NODELET_INFO_THROTTLE(rate, "[%s] " str, jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str(), ##__VA_ARGS__)
+#define JSK_NODELET_WARN_THROTTLE(rate, str, ...) \
+  NODELET_WARN_THROTTLE(rate, "[%s] " str, jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str(), ##__VA_ARGS__)
+#define JSK_NODELET_ERROR_THROTTLE(rate, str, ...) \
+  NODELET_ERROR_THROTTLE(rate, "[%s] " str, jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str(), ##__VA_ARGS__)
+#define JSK_NODELET_FATAL_THROTTLE(rate, str, ...) \
+  NODELET_FATAL_THROTTLE(rate, "[%s] " str, jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str(), ##__VA_ARGS__)
+
+#define JSK_NODELET_DEBUG_STREAM_THROTTLE(rate,...) \
+  NODELET_DEBUG_STREAM_THROTTLE(rate, "[" << jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str() << "] " << __VA_ARGS__)
+#define JSK_NODELET_INFO_STREAM_THROTTLE(rate,...) \
+  NODELET_INFO_STREAM_THROTTLE(rate, "[" << jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str() << "] " << __VA_ARGS__)
+#define JSK_NODELET_WARN_STREAM_THROTTLE(rate,...) \
+  NODELET_WARN_STREAM_THROTTLE(rate, "[" << jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str() << "] " << __VA_ARGS__)
+#define JSK_NODELET_ERROR_STREAM_THROTTLE(rate,...) \
+  NODELET_ERROR_STREAM_THROTTLE(rate, "[" << jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str() << "] " << __VA_ARGS__)
+#define JSK_NODELET_FATAL_STREAM_THROTTLE(rate,...) \
+  NODELET_FATAL_STREAM_THROTTLE(rate, "[" << jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str() << "] " << __VA_ARGS__)
+
 #define JSK_ROS_DEBUG(str,...) \
   ROS_DEBUG("[%s] " str, jsk_topic_tools::getFunctionName(__PRETTY_FUNCTION__).c_str(), ##__VA_ARGS__)
 #define JSK_ROS_INFO(str,...) \

jsk_topic_tools/jsk_topic_tools_test_nodelet.xml

@@ -0,0 +1,8 @@
+<library path="lib/libjsk_topic_tools_test">
+  <class name="jsk_topic_tools_test/LogUtils" type="LogUtils"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      Test nodelet for logging macros.
+    </description>
+  </class>
+</library>

jsk_topic_tools/package.xml

@@ -56,5 +56,6 @@
   <test_depend>roslint</test_depend>
   <export>
     <nodelet plugin="${prefix}/jsk_topic_tools_nodelet.xml"/>
+    <nodelet plugin="${prefix}/jsk_topic_tools_test_nodelet.xml"/>
   </export>
 </package>

jsk_topic_tools/src/tests/test_nodelet_log_utils.cpp

@@ -0,0 +1,43 @@
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include <jsk_topic_tools/log_utils.h>
+
+namespace jsk_topic_tools
+{
+  class LogUtils : public nodelet::Nodelet
+  {
+  private:
+    virtual void onInit()
+      {
+        JSK_NODELET_DEBUG("DEBUG output");
+        JSK_NODELET_DEBUG_STREAM("DEBUG" << " output");
+        JSK_NODELET_DEBUG_THROTTLE(10.0, "DEBUG output");
+        JSK_NODELET_DEBUG_STREAM_THROTTLE(10.0, "DEBUG" << " output");
+
+        JSK_NODELET_INFO("INFO output");
+        JSK_NODELET_INFO_STREAM("INFO" << " output");
+        JSK_NODELET_INFO_THROTTLE(10.0, "INFO output");
+        JSK_NODELET_INFO_STREAM_THROTTLE(10.0, "INFO" << " output");
+
+        JSK_NODELET_WARN("WARN output");
+        JSK_NODELET_WARN_STREAM("WARN" << " output");
+        JSK_NODELET_WARN_THROTTLE(10.0, "WARN output");
+        JSK_NODELET_WARN_STREAM_THROTTLE(10.0, "WARN" << " output");
+
+        JSK_NODELET_ERROR("ERROR output");
+        JSK_NODELET_ERROR_STREAM("ERROR" << " output");
+        JSK_NODELET_ERROR_THROTTLE(10.0, "ERROR output");
+        JSK_NODELET_ERROR_STREAM_THROTTLE(10.0, "ERROR" << " output");
+
+        JSK_NODELET_FATAL("FATAL output");
+        JSK_NODELET_FATAL_STREAM("FATAL" << " output");
+        JSK_NODELET_FATAL_THROTTLE(10.0, "FATAL output");
+        JSK_NODELET_FATAL_STREAM_THROTTLE(10.0, "FATAL" << " output");
+      }
+  };
+}
+
+typedef jsk_topic_tools::LogUtils LogUtils;
+PLUGINLIB_EXPORT_CLASS(LogUtils, nodelet::Nodelet)

jsk_topic_tools/test/test_nodelet_log.cpp

@@ -0,0 +1,28 @@
+/*
+ * test_nodelet_log.cpp
+ * Author: Yuki Furuta <[email protected]>
+ */
+
+#include <gtest/gtest.h>
+#include <nodelet/nodelet.h>
+#include <nodelet/loader.h>
+#include <ros/ros.h>
+#include <string>
+#include <vector>
+
+TEST(JSKNodeletLog, TEST_LOG)
+{
+  nodelet::Loader n(false);
+  ros::M_string remappings;
+  std::string nodelet_name = ros::this_node::getName();
+  std::string nodelet_type = "jsk_topic_tools_test/LogUtils";
+  std::vector<std::string> argv;
+  EXPECT_TRUE(n.load(nodelet_name, nodelet_type, remappings, argv));
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "test_nodelet_log_utils");
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

jsk_topic_tools/test/test_nodelet_log.test

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="nodelet_log_test" pkg="jsk_topic_tools" type="test_nodelet_log" />
+</launch>