Skip to content

Commit be3aca1

Browse files
daniel86daniel86
authored
Merge branch 'dev' into test-query
2 parents e4507b2 + 15318b9 commit be3aca1

12 files changed

+476
-542
lines changed

‎CMakeLists.txt‎

Lines changed: 11 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -5,35 +5,32 @@ project(knowrob_designator)
55
find_package(catkin REQUIRED COMPONENTS
66
rospy
77
std_msgs
8-
actionlib
9-
actionlib_msgs
108
message_generation
119
)
1210

1311
catkin_python_setup()
1412

15-
## Declare ROS actions
16-
add_action_files(
17-
DIRECTORY action
13+
## Declare ROS messages
14+
add_message_files(
15+
DIRECTORY msg
1816
FILES
19-
PushObjectDesignator.action
20-
DesignatorInit.action
21-
DesignatorResolutionStart.action
22-
DesignatorResolutionFinished.action
23-
DesignatorExecutionStart.action
24-
DesignatorExecutionFinished.action
17+
PushObjectDesignator.msg
18+
DesignatorInit.msg
19+
DesignatorResolutionStart.msg
20+
DesignatorResolutionFinished.msg
21+
DesignatorExecutionStart.msg
22+
DesignatorExecutionFinished.msg
2523
)
2624

27-
## Generate messages from actions
25+
## Generate messages
2826
generate_messages(
2927
DEPENDENCIES
3028
std_msgs
31-
actionlib_msgs
3229
)
3330

3431
## Declare catkin package
3532
catkin_package(
36-
CATKIN_DEPENDS rospy std_msgs actionlib actionlib_msgs message_runtime
33+
CATKIN_DEPENDS rospy std_msgs message_runtime
3734
)
3835

3936
# list all executable python and shell scripts for installation

‎action/PushObjectDesignator.action‎

Lines changed: 0 additions & 8 deletions
This file was deleted.

action/DesignatorExecutionFinished.action renamed to msg/DesignatorExecutionFinished.msg

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -4,9 +4,4 @@
44
# - `timestamp`: Time of completion
55
string designator_id
66
string json_designator
7-
time stamp
8-
---
9-
bool success
10-
string message
11-
---
12-
string status
7+
time stamp

action/DesignatorExecutionStart.action renamed to msg/DesignatorExecutionStart.msg

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -4,9 +4,4 @@
44
# - `timestamp`: Time of update
55
string designator_id
66
string json_designator
7-
time stamp
8-
---
9-
bool success
10-
string message
11-
---
12-
string status
7+
time stamp

action/DesignatorInit.action renamed to msg/DesignatorInit.msg

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,4 @@
66
string designator_id
77
string parent_id
88
string json_designator
9-
time stamp
10-
---
11-
bool success
12-
string message
13-
---
14-
string status
9+
time stamp

action/DesignatorResolutionFinished.action renamed to msg/DesignatorResolutionFinished.msg

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,4 @@
66
string designator_id
77
string resolved_from_id
88
string json_designator
9-
time stamp
10-
---
11-
bool success
12-
string message
13-
---
14-
string status
9+
time stamp

action/DesignatorResolutionStart.action renamed to msg/DesignatorResolutionStart.msg

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -4,9 +4,4 @@
44
# - `timestamp`: Time of execution logging
55
string designator_id
66
string json_designator
7-
time stamp
8-
---
9-
bool success
10-
string message
11-
---
12-
string status
7+
time stamp

‎msg/PushObjectDesignator.msg‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
# Add or modify and object
2+
string json_designator
3+
time stamp

‎package.xml‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -15,12 +15,8 @@
1515
<buildtool_depend>catkin</buildtool_depend>
1616
<build_depend>message_generation</build_depend>
1717
<build_depend>std_msgs</build_depend>
18-
<build_depend>actionlib_msgs</build_depend>
19-
<build_depend>actionlib</build_depend>
2018
<build_depend>visualization_msgs</build_depend>
2119
<exec_depend>message_runtime</exec_depend>
22-
<exec_depend>actionlib</exec_depend>
23-
<exec_depend>actionlib_msgs</exec_depend>
2420
<exec_depend>std_msgs</exec_depend>
2521
<test_depend>rostest</test_depend>
2622
<test_depend>python3</test_depend>

‎scripts/knowrob_designator_client.py‎

Lines changed: 91 additions & 88 deletions
Original file line numberDiff line numberDiff line change
@@ -1,31 +1,22 @@
11
#!/usr/bin/env python3
22
# -*- coding: utf-8 -*-
3-
# Full client for all KnowRob designator actions
3+
# Topic-based client for all KnowRob designator messages
44

55
import rospy
6-
import actionlib
76
import uuid
87
from time import sleep
8+
from std_msgs.msg import Header
99
from knowrob_designator.msg import (
10-
PushObjectDesignatorAction, PushObjectDesignatorGoal,
11-
DesignatorInitAction, DesignatorInitGoal,
12-
DesignatorResolutionStartAction, DesignatorResolutionStartGoal,
13-
DesignatorResolutionFinishedAction, DesignatorResolutionFinishedGoal,
14-
DesignatorExecutionStartAction, DesignatorExecutionStartGoal,
15-
DesignatorExecutionFinishedAction, DesignatorExecutionFinishedGoal
10+
PushObjectDesignator,
11+
DesignatorInit,
12+
DesignatorResolutionStart,
13+
DesignatorResolutionFinished,
14+
DesignatorExecutionStart,
15+
DesignatorExecutionFinished
1616
)
1717
from knowrob_ros.knowrob_ros_lib import KnowRobRosLib
1818
from knowrob_ros.knowrob_ros_lib import get_default_modalframe
1919

20-
def send_action(client, goal, label):
21-
rospy.loginfo(f"[{label}] Waiting for action server...")
22-
client.wait_for_server()
23-
rospy.loginfo(f"[{label}] Sending goal...")
24-
client.send_goal(goal)
25-
client.wait_for_result()
26-
result = client.get_result()
27-
rospy.loginfo(f"[{label}] Result: success={result.success}, message='{result.message}''")
28-
2920
def testQueryDesig():
3021
know = KnowRobRosLib()
3122
know.init_clients() # After rospy.init_node()
@@ -36,39 +27,53 @@ def testQueryDesig():
3627
rospy.loginfo(f"response: [{result}]")
3728

3829
def main():
39-
rospy.init_node('knowrob_designator_full_test_client')
40-
30+
rospy.init_node('knowrob_designator_topic_client')
31+
now = rospy.Time.now()
32+
33+
# Publishers
34+
push_pub = rospy.Publisher('/knowrob/designator/push_object_designator', PushObjectDesignator, queue_size=10)
35+
init_pub = rospy.Publisher('/knowrob/designator/init', DesignatorInit, queue_size=10)
36+
resolve_start_pub = rospy.Publisher('/knowrob/designator/resolving_started', DesignatorResolutionStart, queue_size=10)
37+
resolve_finished_pub = rospy.Publisher('/knowrob/designator/resolving_finished', DesignatorResolutionFinished, queue_size=10)
38+
exec_start_pub = rospy.Publisher('/knowrob/designator/execution_start', DesignatorExecutionStart, queue_size=10)
39+
exec_finished_pub = rospy.Publisher('/knowrob/designator/execution_finished', DesignatorExecutionFinished, queue_size=10)
40+
41+
rospy.sleep(1.0) # Wait for publishers to register
42+
4143
##########################################################
42-
############### Object Designators ########################
43-
44-
# 0. PushObjectDesignator
45-
push_client = actionlib.SimpleActionClient('/knowrob/designator/push_object_designator', PushObjectDesignatorAction)
46-
push_goal = PushObjectDesignatorGoal()
47-
push_goal.json_designator = """
44+
############### Object Designator ########################
45+
46+
push_msg = PushObjectDesignator()
47+
push_msg.stamp = now
48+
push_msg.json_designator = """
4849
{
4950
"anObject": {
50-
"type": "Milk"
51+
"name": "Milk1",
52+
"type": "Milk",
5153
"pose": {
52-
"x": 1.0,
53-
"y": 0.5,
54-
"z": 0.75,
54+
"px": 1.0,
55+
"py": 1.0,
56+
"pz": 0.0,
57+
"rx": 0.0,
58+
"ry": 0.0,
59+
"rz": 0.0,
60+
"rw": 1.0,
5561
"frame": "map"
5662
}
5763
}
5864
}
5965
"""
60-
push_goal.stamp = rospy.Time.now()
61-
send_action(push_client, push_goal, "PushObjectDesignator")
62-
66+
rospy.loginfo("Publishing PushObjectDesignator...")
67+
push_pub.publish(push_msg)
68+
6369
##########################################################
6470
############### Action Designators ########################
65-
66-
# Create a designator ID and JSON designator
71+
6772
json_designator = """
6873
{
6974
"anAction": {
7075
"type": "Transporting",
71-
"objectActedOn": {
76+
"object_designator": {
7277
"anObject": {
7378
"type": "Milk"
7479
}
@@ -85,74 +90,72 @@ def main():
8590
}
8691
}
8792
"""
88-
designator_id = f"desig_{uuid.uuid4()}"
89-
resolved_id = f"desig_{uuid.uuid4()}"
90-
now = rospy.Time.now()
91-
92-
# 1. DesignatorInit
93-
init_client = actionlib.SimpleActionClient('/knowrob/designator/init', DesignatorInitAction)
94-
init_goal = DesignatorInitGoal()
95-
init_goal.designator_id = designator_id
96-
init_goal.parent_id = "" # root designator
97-
init_goal.json_designator = json_designator
98-
init_goal.stamp = now
99-
send_action(init_client, init_goal, "Init")
100-
101-
# 2. DesignatorResolvingStarted
102-
resolving_client = actionlib.SimpleActionClient('/knowrob/designator/resolving_started', DesignatorResolutionStartAction)
103-
resolving_goal = DesignatorResolutionStartGoal()
104-
resolving_goal.designator_id = designator_id
105-
resolving_goal.json_designator = json_designator
106-
resolving_goal.stamp = now
107-
send_action(resolving_client, resolving_goal, "ResolveStart")
108-
109-
# 3. DesignatorResolutionFinished with resolved target
110-
resolved_client = actionlib.SimpleActionClient('/knowrob/designator/resolving_finished', DesignatorResolutionFinishedAction)
11193

11294
resolved_designator = """
11395
{
11496
"anAction": {
11597
"type": "Transporting",
116-
"objectActedOn": {
98+
"object_designator": {
11799
"anObject": {
118100
"type": "Milk"
119101
}
120102
},
121-
"target": {
122-
"pose": {
123-
"x": 1.2,
124-
"y": 0.8,
125-
"z": 0.75,
126-
"frame": "map"
103+
"target_location": {
104+
'px': 2.1,
105+
'py': 2.35,
106+
'pz': 0.8,
107+
'rx': 0.0,
108+
'ry': 0.0,
109+
'rz': 0.0,
110+
'rw': 1.0,
111+
'frame': 'map'
127112
}
128113
}
129114
}
130115
}
131116
"""
132117

133-
resolved_goal = DesignatorResolutionFinishedGoal()
134-
resolved_goal.designator_id = resolved_id
135-
resolved_goal.resolved_from_id = designator_id
136-
resolved_goal.json_designator = resolved_designator
137-
resolved_goal.stamp = rospy.Time.now()
138-
send_action(resolved_client, resolved_goal, "ResolveFinished")
139-
140-
141-
# 4. DesignatorExecutionStart
142-
exec_start_client = actionlib.SimpleActionClient('/knowrob/designator/execution_start', DesignatorExecutionStartAction)
143-
exec_start_goal = DesignatorExecutionStartGoal()
144-
exec_start_goal.designator_id = resolved_id
145-
exec_start_goal.json_designator = resolved_designator
146-
exec_start_goal.stamp = now
147-
send_action(exec_start_client, exec_start_goal, "ExecutionStart")
148-
149-
# 5. DesignatorExecutionFinished
150-
exec_finished_client = actionlib.SimpleActionClient('/knowrob/designator/execution_finished', DesignatorExecutionFinishedAction)
151-
exec_finished_goal = DesignatorExecutionFinishedGoal()
152-
exec_finished_goal.designator_id = resolved_id
153-
exec_finished_goal.json_designator = resolved_designator
154-
exec_finished_goal.stamp = now
155-
send_action(exec_finished_client, exec_finished_goal, "ExecutionFinished")
118+
designator_id = f"desig_{uuid.uuid4()}"
119+
resolved_id = f"desig_{uuid.uuid4()}"
120+
121+
init_msg = DesignatorInit()
122+
init_msg.stamp = now
123+
init_msg.designator_id = designator_id
124+
init_msg.parent_id = ""
125+
init_msg.json_designator = json_designator
126+
rospy.loginfo("Publishing DesignatorInit...")
127+
init_pub.publish(init_msg)
128+
129+
resolve_start_msg = DesignatorResolutionStart()
130+
resolve_start_msg.stamp = now
131+
resolve_start_msg.designator_id = designator_id
132+
resolve_start_msg.json_designator = json_designator
133+
rospy.loginfo("Publishing DesignatorResolutionStart...")
134+
resolve_start_pub.publish(resolve_start_msg)
135+
136+
rospy.sleep(1.0)
137+
138+
resolve_finished_msg = DesignatorResolutionFinished()
139+
resolve_finished_msg.stamp = rospy.Time.now()
140+
resolve_finished_msg.designator_id = resolved_id
141+
resolve_finished_msg.resolved_from_id = designator_id
142+
resolve_finished_msg.json_designator = resolved_designator
143+
rospy.loginfo("Publishing DesignatorResolutionFinished...")
144+
resolve_finished_pub.publish(resolve_finished_msg)
145+
146+
exec_start_msg = DesignatorExecutionStart()
147+
exec_start_msg.stamp = now
148+
exec_start_msg.designator_id = resolved_id
149+
exec_start_msg.json_designator = resolved_designator
150+
rospy.loginfo("Publishing DesignatorExecutionStart...")
151+
exec_start_pub.publish(exec_start_msg)
152+
153+
exec_finished_msg = DesignatorExecutionFinished()
154+
exec_finished_msg.stamp = now
155+
exec_finished_msg.designator_id = resolved_id
156+
exec_finished_msg.json_designator = resolved_designator
157+
rospy.loginfo("Publishing DesignatorExecutionFinished...")
158+
exec_finished_pub.publish(exec_finished_msg)
156159

157160
# Finally do some testing queries with KnowRob
158161
testQueryDesig()

0 commit comments

Comments
 (0)