Skip to content

Commit d5ea3f3

Browse files
fmessmerfmessmer
committed
add show_grasp_rviz
1 parent e788fea commit d5ea3f3

7 files changed

+376
-1
lines changed

‎cob_grasp_generation/CMakeLists.txt

+2
Original file line numberDiff line numberDiff line change
@@ -33,5 +33,7 @@ install(PROGRAMS
3333
scripts/query_grasps_server.py
3434
scripts/show_grasps_or_client.py
3535
scripts/show_grasps_or_server.py
36+
scripts/show_grasps_rviz_client.py
37+
scripts/show_grasps_rviz_server.py
3638
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
3739
)

‎cob_grasp_generation/launch/show_grasp.rviz

+166
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,166 @@
1+
Panels:
2+
- Class: rviz/Displays
3+
Help Height: 78
4+
Name: Displays
5+
Property Tree Widget:
6+
Expanded:
7+
- /Global Options1
8+
Splitter Ratio: 0.5
9+
Tree Height: 595
10+
- Class: rviz/Selection
11+
Name: Selection
12+
- Class: rviz/Tool Properties
13+
Expanded:
14+
- /2D Pose Estimate1
15+
- /2D Nav Goal1
16+
- /Publish Point1
17+
Name: Tool Properties
18+
Splitter Ratio: 0.588679
19+
- Class: rviz/Views
20+
Expanded:
21+
- /Current View1
22+
Name: Views
23+
Splitter Ratio: 0.5
24+
- Class: rviz/Time
25+
Experimental: false
26+
Name: Time
27+
SyncMode: 0
28+
SyncSource: ""
29+
Visualization Manager:
30+
Class: ""
31+
Displays:
32+
- Alpha: 0.5
33+
Cell Size: 1
34+
Class: rviz/Grid
35+
Color: 160; 160; 164
36+
Enabled: true
37+
Line Style:
38+
Line Width: 0.03
39+
Value: Lines
40+
Name: Grid
41+
Normal Cell Count: 0
42+
Offset:
43+
X: 0
44+
Y: 0
45+
Z: 0
46+
Plane: XY
47+
Plane Cell Count: 10
48+
Reference Frame: <Fixed Frame>
49+
Value: true
50+
- Alpha: 1
51+
Class: rviz/RobotModel
52+
Collision Enabled: false
53+
Enabled: true
54+
Links:
55+
All Links Enabled: true
56+
Expand Joint Details: false
57+
Expand Link Details: false
58+
Expand Tree: false
59+
Link Tree Style: Links in Alphabetic Order
60+
grasp_link:
61+
Alpha: 1
62+
Show Axes: false
63+
Show Trail: false
64+
gripper_base_link:
65+
Alpha: 1
66+
Show Axes: false
67+
Show Trail: false
68+
Value: true
69+
gripper_camera_link:
70+
Alpha: 1
71+
Show Axes: false
72+
Show Trail: false
73+
Value: true
74+
gripper_finger_1_link:
75+
Alpha: 1
76+
Show Axes: false
77+
Show Trail: false
78+
Value: true
79+
gripper_finger_2_link:
80+
Alpha: 1
81+
Show Axes: false
82+
Show Trail: false
83+
Value: true
84+
gripper_grasp_link:
85+
Alpha: 1
86+
Show Axes: false
87+
Show Trail: false
88+
Value: true
89+
gripper_palm_link:
90+
Alpha: 1
91+
Show Axes: false
92+
Show Trail: false
93+
Value: true
94+
Name: RobotModel
95+
Robot Description: robot_description
96+
TF Prefix: ""
97+
Update Interval: 0
98+
Value: true
99+
Visual Enabled: true
100+
- Class: rviz/Marker
101+
Enabled: true
102+
Marker Topic: /object_marker
103+
Name: Marker
104+
Namespaces:
105+
"": true
106+
Queue Size: 100
107+
Value: true
108+
Enabled: true
109+
Global Options:
110+
Background Color: 48; 48; 48
111+
Fixed Frame: object_link
112+
Frame Rate: 30
113+
Name: root
114+
Tools:
115+
- Class: rviz/Interact
116+
Hide Inactive Objects: true
117+
- Class: rviz/MoveCamera
118+
- Class: rviz/Select
119+
- Class: rviz/FocusCamera
120+
- Class: rviz/Measure
121+
- Class: rviz/SetInitialPose
122+
Topic: /initialpose
123+
- Class: rviz/SetGoal
124+
Topic: /move_base_simple/goal
125+
- Class: rviz/PublishPoint
126+
Single click: true
127+
Topic: /clicked_point
128+
Value: true
129+
Views:
130+
Current:
131+
Class: rviz/Orbit
132+
Distance: 0.72402
133+
Enable Stereo Rendering:
134+
Stereo Eye Separation: 0.06
135+
Stereo Focal Distance: 1
136+
Swap Stereo Eyes: false
137+
Value: false
138+
Focal Point:
139+
X: -0.00595993
140+
Y: 0.024299
141+
Z: 0.0573712
142+
Name: Current View
143+
Near Clip Distance: 0.01
144+
Pitch: 0.559796
145+
Target Frame: <Fixed Frame>
146+
Value: Orbit (rviz)
147+
Yaw: 6.18858
148+
Saved: ~
149+
Window Geometry:
150+
Displays:
151+
collapsed: false
152+
Height: 876
153+
Hide Left Dock: false
154+
Hide Right Dock: false
155+
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002e2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000037a000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
156+
Selection:
157+
collapsed: false
158+
Time:
159+
collapsed: false
160+
Tool Properties:
161+
collapsed: false
162+
Views:
163+
collapsed: false
164+
Width: 1535
165+
X: 65
166+
Y: 24

‎cob_grasp_generation/launch/show_grasp_rviz.launch

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
<?xml version="1.0"?>
2+
<launch>
3+
4+
<arg name="gripper"/>
5+
<arg name="object"/>
6+
7+
<!-- upload robot description -->
8+
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find cob_grasp_generation)/files/urdf/$(arg gripper).urdf.xacro'"/>
9+
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/>
10+
11+
<node pkg="cob_grasp_generation" type="show_grasps_rviz_server.py" name="show_grasps_rviz_server" output="screen"/>
12+
13+
<!-- rviz visualization -->
14+
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find cob_grasp_generation)/launch/show_grasp.rviz" required="true" />
15+
16+
</launch>

‎cob_grasp_generation/package.xml

+9
Original file line numberDiff line numberDiff line change
@@ -23,12 +23,21 @@
2323
<depend>moveit_msgs</depend>
2424

2525
<exec_depend>actionlib</exec_depend>
26+
<exec_depend>cob_description</exec_depend>
2627
<exec_depend>geometry_msgs</exec_depend>
2728
<exec_depend>python-scipy</exec_depend>
29+
<exec_depend>robot_state_publisher</exec_depend>
2830
<exec_depend>roslib</exec_depend>
2931
<exec_depend>rospy</exec_depend>
32+
<exec_depend>rviz</exec_depend>
33+
<exec_depend>schunk_description</exec_depend>
34+
<exec_depend>sensor_msgs</exec_depend>
35+
<exec_depend>std_msgs</exec_depend>
3036
<exec_depend>tf</exec_depend>
37+
<exec_depend>tf2_ros</exec_depend>
3138
<exec_depend>trajectory_msgs</exec_depend>
39+
<exec_depend>visualization_msgs</exec_depend>
40+
<exec_depend>xacro</exec_depend>
3241
<!--<exec_depend>openrave</exec_depend>-->
3342

3443
</package>

‎cob_grasp_generation/scripts/show_grasps_or_client.py

+5-1
Original file line numberDiff line numberDiff line change
@@ -40,10 +40,14 @@ def show_grasps_client():
4040

4141
client.send_goal(goal)
4242
client.wait_for_result()
43-
print client.get_result()
43+
success = client.get_result().success
44+
if not success:
45+
break
4446

4547
raw_input("Enter for next grasp...")
4648
grasp_id = grasp_id + 1
49+
50+
print "no more grasps"
4751

4852

4953
if __name__ == '__main__':

‎cob_grasp_generation/scripts/show_grasps_rviz_client.py

+56
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
#!/usr/bin/env python
2+
#
3+
# Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
4+
#
5+
# Licensed under the Apache License, Version 2.0 (the "License");
6+
# you may not use this file except in compliance with the License.
7+
# You may obtain a copy of the License at
8+
#
9+
# http://www.apache.org/licenses/LICENSE-2.0
10+
#
11+
# Unless required by applicable law or agreed to in writing, software
12+
# distributed under the License is distributed on an "AS IS" BASIS,
13+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
# See the License for the specific language governing permissions and
15+
# limitations under the License.
16+
17+
18+
import rospy
19+
20+
import actionlib
21+
import cob_grasp_generation.msg
22+
23+
def show_grasps_client():
24+
client = actionlib.SimpleActionClient('show_grasps_rviz', cob_grasp_generation.msg.ShowGraspsAction)
25+
client.wait_for_server()
26+
27+
object_name = raw_input("Insert object name: ")
28+
gripper_type = raw_input("Insert gripper_type: ")
29+
#object_name = "yellowsaltcube"
30+
#gripper_type = "sdhx"
31+
grasp_id = 0
32+
33+
while not rospy.is_shutdown():
34+
print grasp_id
35+
36+
# Set the goal here: object_name, grasp_id, sort-by-quality
37+
goal = cob_grasp_generation.msg.ShowGraspsGoal(object_name, gripper_type, grasp_id, True)
38+
39+
client.send_goal(goal)
40+
client.wait_for_result()
41+
success = client.get_result().success
42+
if not success:
43+
break
44+
45+
raw_input("Enter for next grasp...")
46+
grasp_id = grasp_id + 1
47+
48+
print "no more grasps"
49+
50+
51+
if __name__ == '__main__':
52+
try:
53+
rospy.init_node('show_grasp_client')
54+
result = show_grasps_client()
55+
except rospy.ROSInterruptException:
56+
print "program interrupted before completion"

0 commit comments

Comments
 (0)