[jsk_pcl_ros_utils][polygon_magnifier] Allow negative param value to shrink

doc/jsk_pcl_ros_utils/nodes/polygon_magnifier.md

@@ -1,4 +1,7 @@
 # PolygonMagnifier
+
+![](./images/polygon_magnifier.png)
+
 Magnify polygons by specified length.
 
 ## Subscribing Topic
@@ -14,4 +17,11 @@ Magnify polygons by specified length.
 ## Parameters
 * `~magnify_distance` (Double, default: `0.2`)
 
-  Length to scale polygon
+  Length to scale polygons. Default value `0.2` means the distance of each corresponding edges will be `0.2` m.
+  If this value is less than `0`, output polygons are shrinked.
+
+## Sample
+
+``` bash
+roslaunch jsk_pcl_ros_utils sample_polygon_magnifier.launch
+```

jsk_pcl_ros_utils/CMakeLists.txt

@@ -249,6 +249,7 @@ if (CATKIN_ENABLE_TESTING)
     add_rostest(test/evaluate_voxel_segmentation_by_gt_box.test)
     add_rostest(test/pointcloud_to_mask_image.test)
     add_rostest(test/polygon_array_unwrapper.test)
+    add_rostest(test/polygon_magnifier.test)
     add_rostest(test/subtract_point_indices.test)
     roslaunch_add_file_check(test/subtract_point_indices.test)
   endif()

jsk_pcl_ros_utils/cfg/PolygonMagnifier.cfg

@@ -9,5 +9,5 @@ from math import pi
 
 gen = ParameterGenerator ()
 
-gen.add("magnify_distance", double_t, 0, "Magnify distance", 0.2, 0.0, 10.0)
+gen.add("magnify_distance", double_t, 0, "Magnify distance", 0.2, -10.0, 10.0)
 exit (gen.generate (PACKAGE, "jsk_pcl_ros_utils", "PolygonMagnifier"))

jsk_pcl_ros_utils/sample/config/sample_polygon_magnifier.rviz

@@ -0,0 +1,165 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 1025
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        map:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          base_link:
+            {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Class: jsk_rviz_plugin/PolygonArray
+      Color: 255; 255; 255
+      Enabled: true
+      Name: Original
+      Topic: /polygon_array_publisher/output
+      Unreliable: false
+      Value: true
+      coloring: Flat color
+      normal length: 0.1
+      only border: true
+      show normal: true
+    - Alpha: 1
+      Class: jsk_rviz_plugin/PolygonArray
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Positive
+      Topic: /polygon_magnifier_positive/output
+      Unreliable: false
+      Value: true
+      coloring: Flat color
+      normal length: 0.1
+      only border: true
+      show normal: true
+    - Alpha: 1
+      Class: jsk_rviz_plugin/PolygonArray
+      Color: 255; 25; 0
+      Enabled: true
+      Name: Negative
+      Topic: /polygon_magnifier_negative/output
+      Unreliable: false
+      Value: true
+      coloring: Flat color
+      normal length: 0.1
+      only border: true
+      show normal: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 5.33994
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.590064
+        Y: -0.0526083
+        Z: 0.807601
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.360398
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.7004
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1361
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001d0000004a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000004a7000000f400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000148000004a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000004a7000000d300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000089800000052fc0100000002fb0000000800540069006d0065010000000000000898000004b900fffffffb0000000800540069006d0065010000000000000450000000000000000000000574000004a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2200
+  X: 71
+  Y: 42

jsk_pcl_ros_utils/sample/sample_polygon_magnifier.launch

@@ -0,0 +1,27 @@
+<launch>
+  <arg name="gui" default="true" />
+
+  <include file="$(find jsk_recognition_utils)/sample/sample_polygon_array_publisher.launch">
+    <arg name="gui" value="false" />
+  </include>
+
+  <node name="polygon_magnifier_positive"
+        pkg="jsk_pcl_ros_utils" type="polygon_magnifier">
+    <remap from="~input" to="polygon_array_publisher/output" />
+    <rosparam>
+      magnify_distance: 1.0
+    </rosparam>
+  </node>
+
+  <node name="polygon_magnifier_negative"
+        pkg="jsk_pcl_ros_utils" type="polygon_magnifier">
+    <remap from="~input" to="polygon_array_publisher/output" />
+    <rosparam>
+      magnify_distance: -1.0
+    </rosparam>
+  </node>
+
+  <node if="$(arg gui)"
+        name="rviz" pkg="rviz" type="rviz"
+        args="-d $(find jsk_pcl_ros_utils)/sample/config/sample_polygon_magnifier.rviz" />
+</launch>

jsk_pcl_ros_utils/src/polygon_magnifier_nodelet.cpp

@@ -72,19 +72,24 @@ namespace jsk_pcl_ros_utils
   {
     boost::mutex::scoped_lock lock(mutex_);
     vital_checker_->poke();
-    jsk_recognition_msgs::PolygonArray ret_polygon_array;
-    ret_polygon_array.header = msg->header;
+
+    jsk_recognition_msgs::PolygonArray ret_polygon_array = *msg;
+
     for (size_t i = 0; i < msg->polygons.size(); i++) {
-      geometry_msgs::PolygonStamped magnified_polygon;
-      magnified_polygon.header = msg->polygons[i].header;
-      jsk_recognition_utils::ConvexPolygon poly = jsk_recognition_utils::ConvexPolygon::fromROSMsg(msg->polygons[i].polygon);
-      jsk_recognition_utils::ConvexPolygon::Ptr magnified_poly = poly.magnifyByDistance(magnify_distance_);
-      magnified_polygon.polygon = magnified_poly->toROSMsg();
-      ret_polygon_array.polygons.push_back(magnified_polygon);
+      jsk_recognition_utils::ConvexPolygon poly =
+        jsk_recognition_utils::ConvexPolygon::fromROSMsg(msg->polygons[i].polygon);
+
+      jsk_recognition_utils::ConvexPolygon::Ptr magnified_poly
+        = poly.magnifyByDistance(magnify_distance_);
+
+      if (!magnified_poly->isConvex()) {
+        ROS_WARN("Magnified polygon %ld is not convex.", i);
+      }
+
+      ret_polygon_array.polygons[i].polygon = magnified_poly->toROSMsg();
     }
     pub_.publish(ret_polygon_array);
   }
-
 }
 
 #include <pluginlib/class_list_macros.h>

jsk_pcl_ros_utils/test/polygon_magnifier.test

@@ -0,0 +1,16 @@
+<launch>
+  <include file="$(find jsk_pcl_ros_utils)/sample/sample_polygon_magnifier.launch">
+    <arg name="gui" value="false" />
+  </include>
+
+  <test test-name="test_polygon_magnifier"
+        pkg="jsk_tools" type="test_topic_published.py"
+        time-limit="360" retry="2">
+    <rosparam>
+      topic_0: /polygon_magnifier_positive/output
+      timeout_0: 10
+      topic_1: /polygon_magnifier_negative/output
+      timeout_1: 10
+    </rosparam>
+  </test>
+</launch>

jsk_recognition_utils/node_scripts/polygon_array_publisher.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import rospy
+from geometry_msgs.msg import PolygonStamped, Point32
+from jsk_recognition_msgs.msg import PolygonArray
+
+
+class PolygonArrayPublisher(object):
+    def __init__(self):
+        publish_rate = rospy.get_param("~publish_rate", 1.0)
+
+        frame_id = rospy.get_param("~frame_id")
+        param = rospy.get_param("~polygons")
+        self.msg = self.parse_params(param, frame_id)
+
+        self.pub_polygons = rospy.Publisher("~output", PolygonArray, queue_size=1)
+
+        self.timer = rospy.Timer(rospy.Duration(1. / publish_rate), self.publish)
+
+    def parse_params(self, param, frame_id):
+        # validate params
+        assert isinstance(param, list), "polygons must be list"
+
+        has_label = any("label" in p for p in param)
+        has_likelihood = any("likelihood" in p for p in param)
+
+        for polygon in param:
+            assert "points" in polygon and isinstance(polygon["points"], list),\
+                   "each polygon must have at least 1 'points'"
+            for point in polygon["points"]:
+                assert len(point) == 3,\
+                       "element of 'points' must be list of 3 numbers"
+            if has_label:
+                assert "label" in polygon, "missing 'label'"
+            if has_likelihood:
+                assert "likelihood" in polygon, "missing 'likelihood'"
+
+        # parse params
+        msg = PolygonArray()
+        msg.header.frame_id = frame_id
+        for polygon in param:
+            ps = PolygonStamped()
+            ps.header.frame_id = frame_id
+            ps.polygon.points = [Point32(*v) for v in polygon["points"]]
+            msg.polygons.append(ps)
+            if has_label:
+                msg.labels.append(polygon["label"])
+            if has_likelihood:
+                msg.likelihood.append(polygon["likelihood"])
+
+        return msg
+
+    def publish(self, event):
+        # update timestamp
+        self.msg.header.stamp = event.current_real
+        for p in self.msg.polygons:
+            p.header.stamp = event.current_real
+
+        self.pub_polygons.publish(self.msg)
+
+if __name__ == '__main__':
+    rospy.init_node("polygon_array_publisher")
+    p = PolygonArrayPublisher()
+    rospy.spin()