-
Notifications
You must be signed in to change notification settings - Fork 190
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[jsk_pcl_ros_utils] add sample / test for polygon_magnifier
- Loading branch information
1 parent
9702a7e
commit 0402cbf
Showing
8 changed files
with
470 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
165 changes: 165 additions & 0 deletions
165
jsk_pcl_ros_utils/sample/config/sample_polygon_magnifier.rviz
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
65 changes: 65 additions & 0 deletions
65
jsk_recognition_utils/node_scripts/polygon_array_publisher.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,65 @@ | ||
#!/usr/bin/env python | ||
# -*- coding: utf-8 -*- | ||
# Copyright: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp> | ||
|
||
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(map(lambda p: "label" in p, param)) | ||
has_likelihood = any(map(lambda p: "likelihood" in p, 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 = map(lambda v: Point32(v[0], v[1], v[2]), 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() |
Oops, something went wrong.