def __init__(self):
rospy.init_node('align_to_marker', anonymous = True)
self.sub1 = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.get_marker )
self.sub2 = rospy.Subscriber('/bebop/odom', Odometry, self.get_odom, queue_size = 1)
self.sub3 = rospy.Subscriber("bebop/states/ardrone3/PilotingState/AltitudeChanged",
Ardrone3PilotingStateAltitudeChanged,
self.get_alt)
self.pub = rospy.Publisher('bebop/cmd_vel', Twist, queue_size = 1)
self.rate = rospy.Rate(10)
self.tw = Twist()
self.msg = AlvarMarker()
self.msg_tmp = AlvarMarker()
self.target_id = -1
self.theta = 0.0
self.th_odom = 0.0
self.dist = 0.0
self.dist_x = 0.0
self.dist_y = 0.0
self.dir_x = 1.0
self.dir_y = 1.0
self.p = Pose
self.Kp_ang = 1.0
self.Kp_lin = 0.28
self.duration = 0.0
self.time2end = 0.0
self.wise = 1.0
self.marker_found_1st = False
self.marker_found_2nd = False
self.marker_align_finish = False
# step 1
self.is_arrive_middle = False
self.is_odom_received = False
self.is_marker_found = False
self.first_found_marker = False
self.second_found_marker = False
self.th_odom_1 = 0.0
self.th_odom_2 = 0.0
self.th_odom_mid = 0.0
# step 2 (test)
self.theta_1 = 0.0
self.theta_2 = 0.0
self.theta_mid = 0.0
#self.is_faced_marker = False
self.current_alt = 1.0
if(len(msg.markers)):
print('marker found first')
while not rospy.is_shutdown():
if self.is_arrive_middle == False:
self.set_middle()
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 __init__(self, publisher):
self.data = AlvarMarkers()
self.data.markers = [AlvarMarker(), AlvarMarker()]
self.pub = publisher
self.data.markers[0].id = 1
self.data.markers[1].id = 2
self.data.header.frame_id = 'base_footprint'
self.last_update = rospy.Time.now()
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 arMarkerMsgCallback(self, ar_marker_pose_msg):
if len(ar_marker_pose_msg.markers) == 0:
if self.ar_marker_pose != False and self.canot_find_times < 3:
self.canot_find_times += 1
else:
self.ar_marker_pose = False
rospy.loginfo("CANNOT FIND AR POSE")
else:
self.ar_marker_pose = AlvarMarker()
self.ar_marker_pose = ar_marker_pose_msg.markers[0]
def __init__(self):
rospy.init_node("detect_ar_tag")
self.ar_tag_sub = rospy.Subscriber("/ar_pose_marker",
AlvarMarkers,
self.ar_callback,
queue_size=1)
self.artag_exist = False # number of ar tag
self.marker = AlvarMarker() #ar tag
self.id = self.marker.id
self.position = Pose()
self.orientation = Quaternion()
def ar_pose_grabbing(self, data):
num_markers = 0
marker = AlvarMarker()
marker.confidence = 0
for i in data.markers:
marker = i
num_markers = num_markers + 1
# rospy.loginfo('confidence is: %f', i.confidence)
# if i.confidence > self.confidence_th:
# num_markers = num_markers + 1
# if i.confidence > marker.conficence:
# marker = i
# if num_markers > 0:
if num_markers == 1:
if len(self.ar_pose_x_history) < self.ar_pose_history_len:
# Consider to compare the marker id to determine whether to append a marker
self.ar_pose_history_append(marker.pose.pose)
else:
self.ar_pose_history_pop()
self.ar_pose_history_append(marker.pose.pose)
self.update_data_hist(marker.pose.pose)
else:
rospy.loginfo('No marker is reliablly detected!!!')
def __init__(self):
rospy.init_node('align_to_marker', anonymous = True)
self.sub1 = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.get_marker )
self.sub2 = rospy.Subscriber('/bebop/odom', Odometry, self.get_odom, queue_size = 1)
self.pub = rospy.Publisher('bebop/cmd_vel', Twist, queue_size = 1)
self.rate = rospy.Rate(10)
self.tw = Twist()
self.msg = AlvarMarker()
self.target_id = -1
self.theta = 0.0
self.th_odom = 0.0
self.dist = 0.0
self.dist_x = 0.0
self.dist_y = 0.0
self.dir_x = 1.0
self.dir_y = 1.0
self.p = Pose
self.Kp_ang = 1.0
self.Kp_lin = 0.2
self.th_1 = 0.0
self.th_2 = 0.0
self.th_3 = 0.0
self.TurnTh = 0.0
self.duration = 0.0
self.time2end = 0.0
self.wise = 1.0
self.wise_ = 1.0
self.marker_found_1st = False
self.marker_found_2nd = False
self.marker_align_finish = False
import time
import sys
import os
import signal
def signal_handler(sig, frame):
os.system('killall -9 python rosout')
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
image = np.empty(shape=[0])
bridge = CvBridge()
Alvar = AlvarMarker()
pub = None
Width = 640
Height = 480
Offset = 340
Offset2 = 190
Gap = 40
def img_callback(data):
global image
image = bridge.imgmsg_to_cv2(data, "bgr8")
def pose_callback(msg):
def timer_callback(event):
global last_heartbeat, marker
if (rospy.get_time() - last_heartbeat) >= 1:
marker = AlvarMarker()
marker.id = -1
def arMarkerMsgCallback(self, ar_marker_pose_msg):
if len(ar_marker_pose_msg.markers) == 0:
self.ar_marker_pose = False
else:
self.ar_marker_pose = AlvarMarker()
self.ar_marker_pose = ar_marker_pose_msg.markers[0]
def __init__(self):
rospy.init_node('align_to_marker', anonymous=True)
self.sub1 = rospy.Subscriber('/ar_pose_marker', AlvarMarkers,
self.cbGetMarker)
self.sub2 = rospy.Subscriber('/bebop/odom',
Odometry,
self.cbGetOdom,
queue_size=1)
self.sub3 = rospy.Subscriber(
"bebop/states/ardrone3/PilotingState/AltitudeChanged",
Ardrone3PilotingStateAltitudeChanged, self.cbGetAlt)
self.pub = rospy.Publisher('bebop/cmd_vel', Twist, queue_size=1)
self.rate = rospy.Rate(10)
self.tw = Twist()
self.msg = AlvarMarker()
self.msg_tmp = AlvarMarker()
self.target_id = -1
self.theta = 0.0
self.th_odom = 0.0
self.dist = 0.0
self.dist_x = 0.0
self.dist_y = 0.0
self.dir_x = 1.0
self.dir_y = 1.0
self.p = Pose
self.Kp_ang = 1.0
self.Kp_lin = 0.28
self.duration = 0.0
self.time2end = 0.0
self.wise = 1.0
##############################
self.is_cur_seq = 0
self.searching_middle_seq = 0
self.turn_front_seq = 1
self.align_center_seq = 2
self.go_forward_seq = 3
self.up_seq = 4
self.finished_seq = 5
self.testflag1 = False
self.testflag1_flag_1 = False
self.testflag1_flag_2 = False
self.seq1 = 0
self.seq2 = 1
self.seq3 = 2
self.test_seqs_finished = False
self.is_mark_checked = False
self.cnt = 0
self.is_start = False
self.duration_2 = 0.0
self.time2end_2 = 0.0
while not rospy.is_shutdown():
if self.test_seqs_finished == False and self.is_mark_checked == True:
self.fnMain()
