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()