def ar_pose_switch(self, msg):
#create and publish an Alvar message
old_msg=msg
new_msg=AlvarMarkers()
new_msg.header=old_msg.header
marker=AlvarMarker()
marker.header=old_msg.header
marker.id=old_msg.id
marker.confidence=old_msg.confidence
marker.pose.header=old_msg.header
marker.pose.pose=old_msg.pose.pose
new_msg.markers=[marker]
self.marker_repub.publish(new_msg)
def ar_pose_switch(self, msg):
old_msg=msg
new_msg=AlvarMarkers()
new_msg.header=old_msg.header
marker=AlvarMarker()
marker.header=old_msg.header
marker.id=old_msg.id
marker.confidence=old_msg.confidence
marker.pose.header=old_msg.header
marker.pose.pose=old_msg.pose.pose
new_msg.markers=[marker]
print"Republishing"
self.marker_repub.publish(new_msg)
def read(filename):
yaml_data = None
with open(filename) as f:
yaml_data = yaml.load(f)
anns_list = []
data_list = []
for t in yaml_data:
ann = Annotation()
ann.timestamp = rospy.Time.now()
ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id))
ann.data_id = unique_id.toMsg(unique_id.fromRandom())
ann.id = unique_id.toMsg(unique_id.fromRandom())
ann.name = t['name']
ann.type = 'ar_track_alvar/AlvarMarker'
for i in range(0, random.randint(0,11)):
ann.keywords.append('kw'+str(random.randint(1,11)))
# if 'prev_id' in vars():
# ann.relationships.append(prev_id)
# prev_id = ann.id
ann.shape = 1 # CUBE
ann.color.r = 1.0
ann.color.g = 1.0
ann.color.b = 1.0
ann.color.a = 1.0
ann.size.x = 0.18
ann.size.y = 0.18
ann.size.z = 0.01
ann.pose.header.frame_id = t['frame_id']
ann.pose.header.stamp = rospy.Time.now()
ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
anns_list.append(ann)
object = AlvarMarker()
object.id = t['id']
object.confidence = t['confidence']
object.pose.header.frame_id = t['frame_id']
object.pose.header.stamp = rospy.Time.now()
object.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
data = AnnotationData()
data.id = ann.data_id
data.data = pickle.dumps(object)
data_list.append(data)
print ann, object, data
return anns_list, data_list
def publish(markers):
ar_list = AlvarMarkers()
for m in markers:
ar = AlvarMarker()
ar.id = m['id']
ar.confidence = m['confidence']
ar.pose.header.frame_id = m['frame_id']
ar.pose.header.stamp = rospy.Time.now()
ar.pose.pose = message_converter.convert_dictionary_to_ros_message(
'geometry_msgs/Pose', m['pose'])
ar_list.markers.append(ar)
ar_pub.publish(ar_list)
return
def read(filename):
yaml_data = None
with open(filename) as f:
yaml_data = yaml.load(f)
ar_list = AlvarMarkers()
for m in yaml_data:
ar = AlvarMarker()
ar.id = m['id']
ar.confidence = m['confidence']
ar.pose.header.frame_id = m['frame_id']
ar.pose.header.stamp = rospy.Time.now()
ar.pose.pose = message_converter.convert_dictionary_to_ros_message(
'geometry_msgs/Pose', m['pose'])
ar_list.markers.append(ar)
return ar_list
def publish(self):
seq = 0
while not rospy.is_shutdown():
markers = AlvarMarkers()
markers.header.stamp = rospy.get_rostime()
markers.header.seq = seq
markers.header.frame_id = _frame_id
for name in self.objs.keys():
if name == 'COCA-COLA-CAN-250ML':
self.objs[name].position.z += 0.08
if name in self.objDict.keys():
################################
# publish AR marker
################################
marker = AlvarMarker()
marker.id = self.objDict[name]
marker.pose.pose = self.objs[name]
marker.confidence = 1
marker.header.frame_id = _frame_id
markers.markers.append(marker)
################################
# publish tf
################################
translation = (self.objs[name].position.x,
self.objs[name].position.y,
self.objs[name].position.z)
rotation = (self.objs[name].orientation.x,
self.objs[name].orientation.y,
self.objs[name].orientation.z,
self.objs[name].orientation.w)
self._tf_broadcaster.sendTransform(translation, rotation,
markers.header.stamp,
name, _frame_id)
################################
# publish rviz marker
################################
rviz_marker = Marker()
rviz_marker.header = markers.header
rviz_marker.id = marker.id
rviz_marker.pose = self.objs[name]
rviz_marker.lifetime = rospy.Duration(1.0)
rviz_marker.scale.x = 0.03
rviz_marker.scale.y = 0.03
rviz_marker.scale.z = 0.015
rviz_marker.ns = "basic_shapes"
rviz_marker.type = Marker.CUBE
rviz_marker.action = Marker.ADD
if rviz_marker.id == 1:
# red
rviz_marker.color.r = 1.0
rviz_marker.color.g = 0.0
rviz_marker.color.b = 0.0
rviz_marker.color.a = 1.0
elif rviz_marker.id == 2:
# blue
rviz_marker.color.r = 0.0
rviz_marker.color.g = 0.0
rviz_marker.color.b = 1.0
rviz_marker.color.a = 1.0
else:
# green
rviz_marker.color.r = 0.0
rviz_marker.color.g = 1.0
rviz_marker.color.b = 0.0
rviz_marker.color.a = 1.0
self._visualPub.publish(rviz_marker)
self._pub.publish(markers)
seq = seq + 1
# no spinning needed in rospy
rospy.sleep(1 / _pub_freq)
def publish(self):
seq = 0
while not rospy.is_shutdown():
markers = AlvarMarkers()
markers.header.stamp = rospy.get_rostime()
markers.header.seq = seq
markers.header.frame_id = _frame_id
for name in self.objs.keys():
if name == 'COCA-COLA-CAN-250ML':
self.objs[name].position.z += 0.08
if name in self.objDict.keys():
################################
# publish AR marker
################################
marker = AlvarMarker()
marker.id = self.objDict[name]
marker.pose.pose = self.objs[name]
marker.confidence = 1
marker.header.frame_id = _frame_id
markers.markers.append(marker)
################################
# publish tf
################################
translation = (self.objs[name].position.x,
self.objs[name].position.y,
self.objs[name].position.z)
rotation = (self.objs[name].orientation.x,
self.objs[name].orientation.y,
self.objs[name].orientation.z,
self.objs[name].orientation.w)
self._tf_broadcaster.sendTransform(translation,
rotation,
markers.header.stamp,
name,
_frame_id)
################################
# publish rviz marker
################################
rviz_marker = Marker()
rviz_marker.header = markers.header
rviz_marker.id = marker.id
rviz_marker.pose = self.objs[name]
rviz_marker.lifetime = rospy.Duration(1.0)
rviz_marker.scale.x = 0.03
rviz_marker.scale.y = 0.03
rviz_marker.scale.z = 0.015
rviz_marker.ns = "basic_shapes"
rviz_marker.type = Marker.CUBE
rviz_marker.action = Marker.ADD
if rviz_marker.id == 1:
# red
rviz_marker.color.r = 1.0
rviz_marker.color.g = 0.0
rviz_marker.color.b = 0.0
rviz_marker.color.a = 1.0
elif rviz_marker.id == 2:
# blue
rviz_marker.color.r = 0.0
rviz_marker.color.g = 0.0
rviz_marker.color.b = 1.0
rviz_marker.color.a = 1.0
else:
# green
rviz_marker.color.r = 0.0
rviz_marker.color.g = 1.0
rviz_marker.color.b = 0.0
rviz_marker.color.a = 1.0
self._visualPub.publish(rviz_marker)
self._pub.publish(markers)
seq = seq + 1
# no spinning needed in rospy
rospy.sleep(1 / _pub_freq)
