def main(self, **kwargs):
self.is_parser_running_ = False
self.parser_id_ = None
self.prolog = rosprolog.Prolog(wait_for_services=False)
shell = Shell(title='Manipulation Activity Parsing', maximized=True)
self.shell = shell
self.create_tree_page(shell.content)
#
self.token_subscriber = rospy.Subscriber('/parser/token', EventToken,
self.push_token)
#
PARSER_PAGES.append(self)
#
shell.on_resize += shell.dolayout
shell.show(pack=True)
def __init__(self, tf_frequency):
rospy.wait_for_service('/rosprolog/query')
self.tf_frequency = tf_frequency
self.dirty_object_requests = Queue()
self.prolog = rosprolog.Prolog()
self.objects = defaultdict(lambda: PerceivedObject())
self.tf_broadcaster = rospy.Publisher("/tf", TFMessage, queue_size=100)
self.marker_array_publisher = rospy.Publisher(
'/visualization_marker_array', MarkerArray, queue_size=100)
self.object_state_subscriber = rospy.Subscriber(
'/object_state', ObjectStateArray, self.update_state_cb)
self.update_positions_srv = rospy.Service(
'~update_object_positions', Trigger,
self.update_object_positions_cb)
self.dirty_timer = rospy.Timer(rospy.Duration(.01),
self.dirty_timer_cb)
rospy.loginfo('object state publisher is running')
rospy.sleep(1)
#! /usr/bin/env python
import rospy
import rosprolog_client
# marker
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
# object_state
from knowledge_msgs.msg import ObjectStateArray
from knowledge_msgs.msg import ObjectState
prolog_client = rosprolog_client.Prolog()
def callback(data):
rospy.loginfo("test")
for marker in data.markers:
rospy.loginfo(marker)
osa_message = ObjectStateArray()
if marker.action == 0:
osa_message.action = 0
else:
osa_message.action = 1
os = ObjectState()
os.object_id = marker.ns
x = marker.pose.position.x
y = marker.pose.position.y
z = marker.pose.position.z
# query = "hsr_lookup_transform('map',[" + str(x) + "," + str(y) + "," + str(z) + "], PoseTrans, _), hsr_existing_object_at_thr(PoseTrans, 0.009, Instance)."