def spoof(self):
rate=rospy.Rate(1)
goal=AlvarMarkers()
self.broadcaster.sendTransform((1.6557879542599779, 1.024584175222559287, 1.2336137059145866),( 0.38268343, -0.92387953, 0.38268343, 0.92387953), rospy.Time.now(),"/ar_marker_0", "/base_link")
while not rospy.is_shutdown():
#Send a transform for a AR Tag
#self.broadcaster.sendTransform((0.6557879542599779, 0.024584175222559287, 1.2336137059145866),(0.0, -1, 0.0, 1), rospy.Time.now(),"/ar_marker_1", "/base_link")
try:
now=rospy.Time.now()
#self.listener.waitForTransform('/base_link', '/ar_marker_0', now, rospy.Duration(1))
#trans, rot=self.listener.lookupTransform('/base_link', '/ar_marker_0', now)
goal.header.stamp=rospy.Time.now()
goal.header.frame_id='base_link'
marker=AlvarMarker()
marker.header.stamp=rospy.Time.now()
marker.header.frame_id='base_link'
marker.id=0
marker.pose.header.stamp=rospy.Time.now()
marker.pose.header.frame_id='base_link'
marker.pose.pose.position.x=1.6557879542599779-0.5
marker.pose.pose.position.y= 1.024584175222559287
marker.pose.pose.position.z=1.2336137059145866
marker.pose.pose.orientation.x=0.38268343
marker.pose.pose.orientation.y=-0.92387953
marker.pose.pose.orientation.z=0.38268343
marker.pose.pose.orientation.w=0.92387953
goal.markers=[marker]
self.ar_pub.publish(goal)
except(tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception):
print"TF exception, try again"
rate.sleep()
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 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 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)