def ar_demo():
# Initialize this ROS node
rospy.init_node('ar_demo', anonymous=True)
global marker
# Create publisher for command velocity
global cmd_vel_pub
cmd_vel_pub = rospy.Publisher('/robotont/cmd_vel', Twist, queue_size=1)
# initialize heartbeat
global last_heartbeat
last_heartbeat = rospy.get_time()
# Set up subscriber for /ar_pose_marker
rospy.loginfo("Subscribing to /ar_pose_marker")
rospy.Subscriber("/ar_pose_marker", AlvarMarkers, callback)
# Register heartbeat timer
t = rospy.Timer(rospy.Duration(0.1), timer_callback)
marker = AlvarMarker()
marker.id = -1
step = 0
while (not rospy.is_shutdown()):
#rospy.loginfo(rospy.get_caller_id() + " I heard %s", marker)
x = marker.pose.pose.position.x
y = marker.pose.pose.position.y
z = marker.pose.pose.position.z
twist_msg = Twist()
angle = atan2(y, x)
rospy.loginfo("Marker ID: %s", marker.id)
rospy.loginfo("Marker: X %s Y %s Z %s", x, y, z)
rospy.loginfo("Angle from camera: %s", angle)
quaternion = (marker.pose.pose.orientation.x,
marker.pose.pose.orientation.y,
marker.pose.pose.orientation.z,
marker.pose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
rospy.loginfo("RPY: %s %s %s", roll, pitch, yaw)
# YOUR CODE HERE
# YOUR CODE HERE END
# limiting
twist_msg.linear.x = min(
twist_msg.linear.x,
MAX_X_SPEED) if twist_msg.linear.x > 0 else max(
twist_msg.linear.x, -MAX_X_SPEED)
twist_msg.linear.y = min(
twist_msg.linear.y,
MAX_Y_SPEED) if twist_msg.linear.y > 0 else max(
twist_msg.linear.y, -MAX_Y_SPEED)
twist_msg.angular.z = min(
twist_msg.angular.z,
MAX_Z_SPEED) if twist_msg.angular.z > 0 else max(
twist_msg.angular.z, -MAX_Z_SPEED)
cmd_vel_pub.publish(twist_msg)
rospy.sleep(0.05)
def loadMarkersFromFile(self, folder, filename):
"""
Load markers from the config files
"""
file_path = folder + '/' + filename
with open(file_path, 'r') as stream:
try:
rospy.loginfo('%s::loadMarkersFromFile: opening file %s',
self.node_name, file_path)
config_yaml = yaml.load(stream)
except yaml.YAMLError, e:
rospy.logerr(
'%s:loadMarkersFromFile: error parsing yaml file %s. %s' %
(self.node_name, file_path, e))
return -1
# clear the current list
self._marker_dict.clear()
#print config_yaml
try:
for marker_in_config in config_yaml:
new_marker = AlvarMarker()
new_marker.header.frame_id = marker_in_config['frame']
new_marker.id = marker_in_config['id']
new_marker.id = marker_in_config['id']
new_marker.pose.header.frame_id = marker_in_config['frame']
new_marker.pose.pose.position.x = marker_in_config[
'position'][0]
new_marker.pose.pose.position.y = marker_in_config[
'position'][1]
new_marker.pose.pose.position.z = marker_in_config[
'position'][2]
new_marker.pose.pose.orientation.x = marker_in_config[
'orientation'][0]
new_marker.pose.pose.orientation.y = marker_in_config[
'orientation'][1]
new_marker.pose.pose.orientation.z = marker_in_config[
'orientation'][2]
new_marker.pose.pose.orientation.w = marker_in_config[
'orientation'][3]
# inserting the new marker
self._marker_dict[new_marker.id] = new_marker
self.msg_state.ids_recorded.append(new_marker.id)
#print self._marker_dict[new_marker.id]
except KeyError, e:
self._marker_dict.clear()
rospy.logerr(
'%s:loadMarkersFromFile: error parsing yaml file %s. %s' %
(self.node_name, file_path, e))
return -1
def read(file):
yaml_data = None
with open(file) as f:
yaml_data = yaml.load(f)
anns_list = []
data_list = []
for t in yaml_data:
ann = Annotation()
ann.timestamp = rospy.Time.now()
ann.data_id = unique_id.toMsg(unique_id.fromRandom())
ann.id = unique_id.toMsg(unique_id.fromRandom())
ann.world = world
ann.name = t['name']
ann.type = 'ar_track_alvar_msgs/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.type = ann.type
data.data = serialize_msg(object)
data_list.append(data)
return anns_list, data_list
def read(file):
yaml_data = None
with open(file) as f:
yaml_data = yaml.load(f)
anns_list = []
data_list = []
for t in yaml_data:
ann = Annotation()
ann.timestamp = rospy.Time.now()
ann.data_id = unique_id.toMsg(unique_id.fromRandom())
ann.id = unique_id.toMsg(unique_id.fromRandom())
ann.world = world
ann.name = t['name']
ann.type = 'ar_track_alvar_msgs/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.type = ann.type
data.data = serialize_msg(object)
data_list.append(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 timer_callback(event):
global last_heartbeat, marker
if (rospy.get_time() - last_heartbeat) >= 1:
marker = AlvarMarker()
marker.id = -1
