def calc_position(self, markers_detected):
global markers
msg = AlvarMarkers()
pose_array = PoseArray()
#put here the prediction for multiple markers
for marker in markers_detected:
#print marker
if not self.check_marker(marker['name']):
self.create_new_marker(marker)
pose = marker['pose']
name = marker['name']
quat = quat_msg_to_array(pose.pose.orientation)
r, p, theta = tf.transformations.euler_from_quaternion(quat)
q = tf.transformations.quaternion_from_euler(0, 0, theta)
pose.pose.position.z = 0
pose.pose.orientation = Quaternion(*q)
pose = self.predict_pose(markers[name]['kalman'], pose)
markers[name]['last_seen'] = rospy.Time.now()
marker_msg = AlvarMarker()
marker_msg.pose = pose
marker_msg.header = pose.header
marker_msg.id = int(marker['name'])
msg.markers.append(marker_msg)
pose_array.poses.append(pose.pose)
pose_array.header.stamp = rospy.Time.now()
pose_array.header.frame_id = odom
self.array_pub.publish(pose_array)
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = odom
self.marker_pub.publish(msg)
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)
