def apriltag_callback(self, data): # use apriltag pose detection to find where is the robot # print data for detection in data.detections: if self.idx == 6 and detection.id == 12: self.idx = self.idx + 1 if (self.idx == 5 and detection.id == 4) and ( self.idx == 8 and detection.id == 7): # tag id is the correct one self.detection_id = detection.id poselist_tag_cam = pose2poselist(detection.pose) poselist_tag_base = transformPose(self.lr, poselist_tag_cam, sourceFrame='/camera', targetFrame='/robot_base') poselist_base_tag = invPoselist(poselist_tag_base) poselist_base_map = transformPose(self.lr, poselist_base_tag, sourceFrame='/apriltag', targetFrame='/map') pubFrame(self.br, pose=poselist_base_map, frame_id='/robot_base', parent_frame_id='/map')
def apriltag_callback(data): # use apriltag pose detection to find where is the robot for detection in data.detections: if detection.id == 0: #FIX THIS: change the number to equal the id on the april tag you're using poselist_tag_cam = pose2poselist(detection.pose) #This is the pose you're transforming ## Relevant frames that are already defined: ## camera, map, robot_base, apriltag ## Relevant functions that are defined in a different ros package: ## transformPose(), invPoseList() ## Relevant variables already defined: ## lr #TO DO: Convert the poselist_tag_cam into the pose that we want. #HINT: The first argument for transformPose is lr which is already defined in the beginning of this code. #HINT2: The syntax for transformPose should look like: ## transformPose(lr, poselist_someframe_otherframe, 'otherframe', 'newframe')\ #HINT3: The syntax for invPoselist should look like: ## invPoselist(poselist) ## poselist_tag_base = transformPose(??) #(1) on the lab handout ## poselist_base_tag = invPoselist(??) #(2) on the lab handout ## poselist_base_map = transformPose(??) #(3) on the lab handout pubFrame(br, pose = poselist_base_map, frame_id = '/robot_base', parent_frame_id = '/map')
def process_measurements(self, goal): """ YOUR CODE HERE Main loop of the robot - where all measurements, control, and estimation are done. """ self._apriltag_localization.broadcast_static_tf() odom_meas = self._base_controller.get_measurement() # current wheel angles (wheel_angle_left, wheel_angle_right) tag_meas = self._apriltag_localization.get_measurement() # calculated robot pose(s) based on Apriltag detections (x, y, theta) # Debug # print 'wheel_angle_left = ', self._base_controller._wheel_angle_left # print 'wheel_angle_right = ', self._base_controller._wheel_angle_right # print 'odom_meas = ', odom_meas # print 'tag_meas = ', tag_meas # odom_meas = None # tag_meas = None # Do KalmanFilter step pose_est = self._kalman_filter.step_filter(odom_meas, tag_meas) # print 'pose_est = ', pose_est # print 'goal = ', goal poselist_map2base_ekf = [pose_est[0], pose_est[1], 0, 0, 0, pose_est[2]] pubFrame(self._br, pose = poselist_map2base_ekf, frame_id = '/base_link_ekf', parent_frame_id = '/map') at_goal = False v, omega, at_goal = self._diff_drive_controller.compute_vel(pose_est, goal) cmd_vel_msg = Twist() cmd_vel_msg.linear.x = v cmd_vel_msg.angular.z = omega # self._cmd_vel_pub.publish(cmd_vel_msg) # self._base_controller.command_velocity(v, omega) return at_goal
def main(): rospy.init_node('apriltag_navi', anonymous=True) lr = tf.TransformListener() br = tf.TransformBroadcaster() rospy.sleep(0.1) poselist_tag_map = [0,0,0.44,0.5,0.5,0.5,0.5] print 'poselist_tag_map', poselist_tag_map, '\n' pose_tag_map = poselist2pose(poselist_tag_map) print 'poselist2pose(poselist_tag_map):\n', pose_tag_map, '\n' poselist_map_tag = invPoselist(poselist_tag_map) print 'invPoselist(poselist_tag_map):', poselist_map_tag, '\n' poselist_tag_map_bylookup = lookupTransform(lr, sourceFrame = '/apriltag', targetFrame = '/map') print "lookupTransform(poselist_tag_map, sourceFrame = '/tag', targetFrame = '/map'):", poselist_tag_map_bylookup, '\n' poselist_map_tag_bylookup = lookupTransform(lr, sourceFrame = '/map', targetFrame = '/apriltag') print "lookupTransform(poselist_tag_map, sourceFrame = '/map', targetFrame = '/apriltag'):", poselist_map_tag_bylookup, '\n' poselist_base_tag = [0,0,1,0,0,0,1] poselist_base_map = transformPose(lr, poselist_base_tag, sourceFrame = '/apriltag', targetFrame = '/map') print "transformPose(poselist_tag_map, sourceFrame = '/apriltag', targetFrame = '/map'):", poselist_base_map, '\n' for i in xrange(100): pubFrame(br, pose = poselist_base_map, frame_id = '/robot_base', parent_frame_id = '/map') rospy.sleep(0.1)
def apriltag_callback(data): # use apriltag pose detection to find where is the robot for detection in data.detections: if detection.id == 0: # tag id is the correct one poselist_tag_cam = pose2poselist(detection.pose) poselist_tag_base = transformPose(lr, poselist_tag_cam, sourceFrame = '/camera', targetFrame = '/robot_base') poselist_base_tag = invPoselist(poselist_tag_base) poselist_base_map = transformPose(lr, poselist_base_tag, sourceFrame = '/apriltag', targetFrame = '/map') pubFrame(br, pose = poselist_base_map, frame_id = '/robot_base', parent_frame_id = '/map')
def apriltag_callback(data): # use apriltag pose detection to find where is the robot for detection in data.detections: if detection.id == 0: # tag id is the correct one poselist_tag_cam = pose2poselist(detection.pose) ## poselist_tag_base = transformPose(??) ## poselist_base_tag = invPoselist(??) ## poselist_base_map = transformPose(??) pubFrame(br, pose=poselist_base_map, frame_id='/robot_base', parent_frame_id='/map')
def callback(self, pose_array): """ Convert pose of tag in camera frame to pose of robot in map frame. """ with self._lock: pose_array_msg = PoseArray() # Camera frame to tag frame(s) if (len(pose_array.transforms) == 0): self._pose_detections = None self._tag_pose_pub.publish(pose_array_msg) return pose_detections = np.zeros((len(pose_array.transforms), 3)) for i in range(len(pose_array.transforms)): pose_msg = Pose() tag_id = pose_array.transforms[i].fiducial_id transform_cam2tag = pose_array.transforms[i].transform # print "transform_cam2tag = ", transform_cam2tag poselist_cam2tag = transform2poselist(transform_cam2tag) poselist_base2tag = transformPose(self._lr, poselist_cam2tag, 'camera', 'base_link') poselist_tag2base = invPoselist(poselist_base2tag) # print "poselist_tag2base = ", poselist_tag2base poselist_map2base = transformPose(self._lr, poselist_tag2base, 'apriltag_' + str(tag_id), 'map') # print "poselist_map2base = ", poselist_map2base pubFrame(self._br, pose=poselist_map2base, frame_id='/base_link', parent_frame_id='/map') robot_pose3d = lookupTransform(self._lr, '/map', '/base_link') robot_position2d = robot_pose3d[0:2] robot_yaw = tf.transformations.euler_from_quaternion( robot_pose3d[3:7])[2] robot_pose2d = robot_position2d + [robot_yaw] pose_detections[i] = np.array(robot_pose2d) pose_msg.position.x = robot_pose3d[0] pose_msg.position.y = robot_pose3d[1] pose_msg.orientation.x = robot_pose3d[3] pose_msg.orientation.y = robot_pose3d[4] pose_msg.orientation.z = robot_pose3d[5] pose_msg.orientation.w = robot_pose3d[6] pose_array_msg.poses.append(pose_msg) self._tag_pose_pub.publish(pose_array_msg) self._pose_detections = pose_detections
def apriltag_callback(data): # use apriltag pose detection to find where is the robot if len(data.detections) != 0: # check if apriltag is detected detection = data.detections[0] print detection.pose if detection.id == 21: # tag id is the correct one det_poselist = helper.pose2poselist(detection.pose.pose) basetag_poselist = transformPose(lr, det_poselist, '/camera', '/robot_base') basetag_poselist_inv = invPoselist(basetag_poselist) mapbase_poselist = transformPose(lr, basetag_poselist_inv, '/apriltag', '/map') mapbase_pose = poselist2pose(mapbase_poselist) pubFrame(br, mapbase_pose)
def broadcast_static_tf(self): """ Broadcast static transforms (camera pose in base_link frame, all tag poses in map frame) """ pubFrame(self._br, pose=self._poselist_base2cam, frame_id='/camera', parent_frame_id='/base_link') for tag_id, poselist_map2tag in self._tag_poses.iteritems(): pubFrame(self._br, pose=poselist_map2tag, frame_id='/apriltag_' + str(tag_id), parent_frame_id='/map')
def apriltag_callback(data): # use apriltag pose detection to find where is the robot for detection in data.detections: if detection.id == 0: ## poselist_tag_cam = pose2list(detection.pose) poselist_tag_base = poseTransform(poselist_tag_cam, homeFrame='/camera', targetFrame='/robot_base', listener=lr) poselist_base_tag = invPoseList(poselist_tag_base) poselist_base_map = poseTransform(poselist_base_tag, homeFrame='/apriltag0', targetFrame='/map', listener=lr) pubFrame(br, pose=poselist_base_map, frame_id='/robot_base', parent_frame_id='/map')
def apriltag_callback(data): # use apriltag pose detection to find where is the robot for detection in data.detections: id = detection.id if ((id == 1) or (id == 0) or (id == 2) or (id == 3)): # tag id is one the correct ones that are on game board #somewhere in here we need to add april tag location relative to map poselist_tag_cam = pose2poselist(detection.pose) poselist_tag_base = transformPose(lr, poselist_tag_cam, 'camera', 'robot_base') poselist_base_tag = invPoselist(poselist_tag_base) poselist_base_map = transformPose(lr, poselist_base_tag, 'apriltag' + str(id), 'map') pubFrame(br, pose=poselist_base_map, frame_id='/robot_base_est', parent_frame_id='/map')
def apriltag_callback(data): # use apriltag pose detection to find where is the robot if len(data.detections) != 0: # check if apriltag is detected detection = data.detections[0] print detection.pose if detection.id == 21: print 'detection' # tag id is the correct one # Use the functions in helper.py to do the following # step 1. convert the pose to poselist Hint: pose data => detection.pose.pose A = pose2poselist(detection.pose.pose) # step 2. do the matrix manipulation # A source B target transformPose(,pose,B,A) A = transformPose(lr, A, '/camera', '/base_link') A = invPoselist(A) A = transformPose(lr, A, '/apriltag', '/map') pubFrame(br, A, '/base_link', '/map')
def apriltag_callback(self, data): # use apriltag pose detection to find where is the robot ## for detection in data.detections: if detection.id in range(11): pose_tag_base = helper.poseTransform(helper.pose2list( detection.pose), homeFrame='/camera', targetFrame='/robot_base', listener=self.listener) pose_base_map = helper.poseTransform( helper.invPoseList(pose_tag_base), homeFrame='/apriltag' + str(detection.id), targetFrame='/map', listener=self.listener) helper.pubFrame(self.br, pose=pose_base_map, frame_id='/robot_base', parent_frame_id='/map', npub=1)
def apriltag_callback(self, data): # use apriltag pose detection to find where is the robot # print data for detection in data.detections: print 'apriltag id', detection.id poselist_tag_cam = pose2poselist(detection.pose) poselist_tag_base = transformPose(self.lr, poselist_tag_cam, sourceFrame='/camera', targetFrame='/robot_base') poselist_base_tag = invPoselist(poselist_tag_base) poselist_base_map = transformPose(self.lr, poselist_base_tag, sourceFrame='/apriltag', targetFrame='/map') pubFrame(self.br, pose=poselist_base_map, frame_id='/robot_base', parent_frame_id='/map')
def apriltag_callback(data): # use apriltag pose detection to find where is the robot if len(data.detections) != 0: # check if apriltag is detected detection = data.detections[0] print detection.pose if detection.id == 20: # tag id is the correct one # Use the functions in helper.py to do the following # step 1. convert the pose to poselist Hint: pose data => detection.pose.pose # step 2. do the matrix manipulation # step 3. publish the base frame w.r.t the map frame # note: tf listener and broadcaster are initalize in line 19~20 poselist1 = pose2poselist(detection.pose.pose) poselist1_T2B = transformPose(lr, poselist1, '/camera', '/base_link') poselist1_T2B_inv = invPoselist(poselist1_T2B) poselist1_B2M = transformPose(lr, poselist1_T2B_inv, '/apriltag', 'map') #detection.pose.pose = poselist2pose(poselist1_B2M) #detection is form camera so no need to write back pubFrame(br, poselist1_B2M)
def apriltag_callback(data): # use apriltag pose detection to find where is the robot if len(data.detections) != 0: detection = data.detections[0] print detection.pose if detection.id == 20: # tag id is the correct one poselist_tag_cam = pose2poselist(detection.pose.pose) poselist_tag_base = transformPose(lr, poselist_tag_cam, sourceFrame='camera', targetFrame='base_link') print poselist_tag_base poselist_base_tag = invPoselist(poselist_tag_base) poselist_base_map = transformPose(lr, poselist_base_tag, sourceFrame='apriltag', targetFrame='map') pubFrame(br, pose=poselist_base_map, frame_id='/base_link', parent_frame_id='/map')
def apriltag_callback(data): global lr, br global apriltag_number # use apriltag pose detection to find where is the robot for detection in data.detections: if detection.id == apriltag_number: tagframe = '/apriltag%d' % detection.id ## poselist_tag_cam = pose2list(detection.pose) poselist_tag_base = poseTransform(poselist_tag_cam, homeFrame='/camera', targetFrame='/robot_base', listener=lr) poselist_base_tag = invPoseList(poselist_tag_base) poselist_base_map = poseTransform(poselist_base_tag, homeFrame=tagframe, targetFrame='/map', listener=lr) pubFrame(br, pose=poselist_base_map, frame_id='/robot_base', parent_frame_id='/map') print apriltag_number