def perform(self, reevaluate=False): stand_pose = PoseStamped() stand_pose.header.stamp = rospy.Time.now() stand_pose.header.frame_id = 'base_footprint' stand_pose.pose.orientation.w = 1 self.blackboard.pathfinding.publish(stand_pose) if not self.blackboard.blackboard.is_currently_walking(): self.pop()
def pose2d_to_pose(self, pose2d): pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = 'base_footprint' pose.pose.position.x = pose2d.x pose.pose.position.y = pose2d.y # Euler -> Quaternion (a lot simpler without roll and pitch) pose.pose.orientation.z = math.sin(0.5 * pose2d.theta) pose.pose.orientation.w = math.cos(0.5 * pose2d.theta) return pose
def add_crosses(): global tf_buffer global cam_info crosses = ["crossL", "crossR", "crossTM", "crosMM", "crossBM"] for cross in crosses: try: trans = tfBuffer.lookup_transform("camera_optical_frame", str(cross), rospy.Time(0)) corner_stamped = PoseStamped() corner_stamped.header.stamp = self.get_clock().now() corner_stamped.header.frame_id = "camera_optical_frame" corner_stamped.pose.position.x = trans.transform.translation.x corner_stamped.pose.position.y = trans.transform.translation.y corner_stamped.pose.position.z = trans.transform.translation.z corner_stamped.pose.orientation = trans.transform.rotation p = [ corner_stamped.pose.position.x, corner_stamped.pose.position.y, corner_stamped.pose.position.z ] k = np.reshape(cam_info.K, (3, 3)) p_pixel = np.matmul(k, p) if p_pixel[2] > 0: p_pixel = p_pixel * (1 / p_pixel[2]) if p_pixel[0] > 0 and p_pixel[0] <= cam_info.width and p_pixel[ 1] > 0 and p_pixel[1] <= cam_info.height: if np.random.uniform(10) < 8 or True: pose = tfBuffer.transform( corner_stamped, "base_footprint", timeout=Duration(seconds=0.5)) point_msg = Point() point_msg.x = add_gaussian_noise(pose.pose.position.x) point_msg.y = add_gaussian_noise(pose.pose.position.y) point_msg.z = pose.pose.position.z #todo set to 0? pixel_msg = LineIntersectionRelative() pixel_msg.pose.pose.pose.position = point_msg pixels.type = pixel_msg.L message_container.intersections.append(pixel_msg) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue message_container.header.stamp = self.get_clock().now()
def ScanMap(self): if self.odom is None: return # Transform: odom frame -> map frame poseStamped = PoseStamped() poseStamped.header = self.odom.header poseStamped.pose = self.odom.pose.pose try: poseStamped = self.tfBuffer.transform(poseStamped, self.map_frame, timeout=rospy.Duration(1.0 / self.rate)) except: # rospy.logerr('Transform failed') return # Scan yaw = tf.transformations.euler_from_quaternion([poseStamped.pose.orientation.x, poseStamped.pose.orientation.y, poseStamped.pose.orientation.z, poseStamped.pose.orientation.w])[2] R = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) T = np.array([poseStamped.pose.position.x, poseStamped.pose.position.y]) scan_buffer = (np.matmul(self.scan_buffer, R.T) + T) scan_buffer = self.occGridParam.map2ImageTransform(scan_buffer) scan_buffer = np.int32(np.round(scan_buffer)) scan_buffer[:, :, 0] = np.clip(scan_buffer[:, :, 0], 0, self.width - 1) scan_buffer[:, :, 1] = np.clip(scan_buffer[:, :, 1], 0, self.height - 1) pixel_values = self.occMap[scan_buffer[:, :, 1], scan_buffer[:, :, 0]] mask = pixel_values > 0 indices = np.where(mask.any(axis=1), mask.argmax(axis=1), -1) pointclouds = [] for index, scan_points in zip(indices, scan_buffer): if index == -1: pointcloud = [np.inf, np.inf, np.inf] else: pointcloud = [scan_points[index, 0], scan_points[index, 1], 0.0] pointclouds.append(pointcloud) pointclouds = np.asarray(pointclouds) pointclouds[:, :2] = self.occGridParam.image2MapTransform(pointclouds[:, :2]) self.map_pointclouds = pointclouds
def ObstacleCallback(self, msg): if self.odom is None: return # Transform poseStamped = PoseStamped() poseStamped.header = msg.header poseStamped.pose = msg.pose.pose try: poseStamped = self.tfBuffer.transform(poseStamped, self.odom_frame, timeout=rospy.Duration(1.0 / self.rate)) except: # rospy.logerr('Transform failed') return x, y, z = poseStamped.pose.position.x, poseStamped.pose.position.y, poseStamped.pose.position.z dx, dy = x - self.odom.pose.pose.position.x, y - self.odom.pose.pose.position.y dist = np.sqrt(dx ** 2 + dy ** 2) if dist < self.min_range or dist > self.max_range: self.obstacle_pointclouds = None return pointclouds = np.array([[x, y, z]]) self.obstacle_pointclouds = pointclouds
def send_goal(checkpoint): goal = PoseStamped() goal.header.stamp = rospy.Time.now() goal.header.frame_id = 'cf1/base_link' goal.pose.position.x = checkpoint[0] goal.pose.position.y = checkpoint[1] goal.pose.position.z = checkpoint[2] x, y, z, w = quaternion_from_euler(0, 0, checkpoint[3]) goal.pose.orientation.x = x goal.pose.orientation.y = y goal.pose.orientation.z = z goal.pose.orientation.w = w pose_pub.publish(goal)
def yaw_towards_frame(pose, target_frame): """Return the yaw towards frame represented in a quaternion (list) """ # TODO: send in transform tf_buf as argument instead of calculating here if not tf_buf.can_transform(target_frame, 'map', rospy.Time(0)): rospy.logwarn_throttle(5.0, 'No transform from %s to map' % target_frame) return #rospy.loginfo(['-'*20,pose]) if pose.header.frame_id != "map": pose = tf_buf.transform(pose, 'map') #rospy.loginfo(euler_from_quaternion([pose.pose.orientation.x,pose.pose.orientation.y,pose.pose.orientation.z,pose.pose.orientation.w])) frame_pose = PoseStamped() # [0, 0, 0] frame_pose.header.stamp = rospy.Time(0) frame_pose.header.frame_id = target_frame frame_pose.pose.orientation.w = 1 #rospy.loginfo(tf_buf.registration.print_me()) p = tf_buf.transform(frame_pose, 'map') frame_pos = np.array( [p.pose.position.x, p.pose.position.y, p.pose.position.z]) curr = np.array( [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]) rospy.loginfo(curr) rospy.loginfo(frame_pos) diff = frame_pos - curr rospy.loginfo(diff) theta = np.arctan2(diff[1], diff[0]) rospy.loginfo(theta) q_rot = quaternion_from_euler(0, 0, theta, "sxyz") return q_rot
def perform(self, reevaluate=False): # get ball pos ball_pos = self.blackboard.world_model.get_ball_position_xy() our_pose = self.blackboard.world_model.get_current_position() # decide on side if our_pose[1] < ball_pos[1]: side_sign = -1 else: side_sign = 1 # compute goal goal_x = ball_pos[0] if self.accept: goal_x += self.pass_pos_x # Limit the x position, so that we are not running into the enemy goal goal_x = min(self.max_x, goal_x) goal_y = ball_pos[1] + side_sign * self.pass_pos_y goal_yaw = 0 pose_msg = PoseStamped() pose_msg.header.stamp = rospy.Time.now() pose_msg.header.frame_id = self.blackboard.map_frame pose_msg.pose.position.x = goal_x pose_msg.pose.position.y = goal_y quaternion = quaternion_from_euler(0, 0, goal_yaw) pose_msg.pose.orientation.x = quaternion[0] pose_msg.pose.orientation.y = quaternion[1] pose_msg.pose.orientation.z = quaternion[2] pose_msg.pose.orientation.w = quaternion[3] self.blackboard.pathfinding.publish(pose_msg) if self.blackboard.pathfinding.status in [GoalStatus.SUCCEEDED, GoalStatus.ABORTED]: self.pop()
def get_bbox_pose(self, bbox, cloud): # Initialize variables n = 0 cog = [0] * 3 min_dim = [None] * 3 max_dim = [None] * 3 cloud_dim = [0] * 3 # Get the width and height of the bbox width = bbox[2] - bbox[0] height = bbox[3] - bbox[1] # Shrink the bbox by 10% to minimize background points xmin = bbox[0] + int(0.1 * width) xmax = bbox[2] - int(0.1 * width) ymin = bbox[1] + int(0.1 * height) ymax = bbox[3] - int(0.1 * height) # Place all the box points in an array to be used by the read_points function below bbox_points = [[x, y] for x in range(xmin, xmax) for y in range(ymin, ymax)] # Clear the current list person_points = list() # Read in the x, y, z coordinates of all bbox points in the cloud for point in point_cloud2.read_points(cloud, skip_nans=True, uvs=bbox_points): try: for i in range(3): if min_dim[i] is None or point[i] < min_dim[i]: min_dim[i] = point[i] if max_dim[i] is None or point[i] > max_dim[i]: max_dim[i] = point[i] cog[i] += point[i] n += 1 #person_points.append(point) except: pass # Publish the pointcloud for this person #person_cloud = point_cloud2.create_cloud(cloud.header, cloud.fields, person_points) #self.people_cloud_pub.publish(person_cloud) # If we don't have any points, just return empty handed if n == 0: return (None, None) # Compute the COG and dimensions of the bounding box for i in range(3): cog[i] /= n cloud_dim[i] = max_dim[i] - min_dim[i] # Fill in the person pose message person_cog = PoseStamped() person_cog.header.frame_id = self.depth_frame person_cog.header.stamp = rospy.Time.now() person_cog.pose.position.x = cog[0] person_cog.pose.position.y = cog[1] person_cog.pose.position.z = cog[2] # Project the COG onto the base frame try: person_in_base_frame = self.tfBuffer.transform( person_cog, self.base_link) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: return (None, None) # Set the person on the ground person_in_base_frame.pose.position.z = 0.0 person_in_base_frame.pose.orientation.x = 0.707 person_in_base_frame.pose.orientation.y = 0.0 person_in_base_frame.pose.orientation.z = 0.707 person_in_base_frame.pose.orientation.w = 0.0 # Fill in the BoundBox message person_bounding_box = BoundingBox() person_bounding_box.header.frame_id = self.depth_frame person_bounding_box.header.stamp = rospy.Time.now() person_bounding_box.pose = person_in_base_frame.pose person_bounding_box.dimensions.x = cloud_dim[0] person_bounding_box.dimensions.y = cloud_dim[1] person_bounding_box.dimensions.z = cloud_dim[2] return (person_in_base_frame, person_bounding_box)
def followperson(data): global detect global time_last_detection global lock global navigator lock.acquire(blocking=False) #print("data",data) dictionary = message_converter.convert_ros_message_to_dictionary(data) pixel_yolo = dictionary['data'] pixel_yolo = eval(pixel_yolo) #print("pixel yolo",pixel_yolo) if pixel_yolo == (-1, -1): now = rospy.Time.now() #print(now) if (detect) and ((now - time_last_detection).to_nsec() < 1000000000): print("Personne perdue de vue") msg = Twist() msg.angular.z = 0.2 cmd_pub.publish(msg) else: print("retour à la ronde") if (lock.locked()): lock.release() ''' try : # print("le robot doit s'arrêter'") #cancel_msg = GoalID() #cancel_pub.publish(cancel_msg) #p_stop = PoseStamped() # p_stop.header.frame_id = 'base_link' # p_stop.pose.position.x = 0 # p_stop.pose.position.y = 0 # p_stop.pose.position.z = 0 # p_stop.pose.orientation.w = 0 # p_stop_map = tf_listener.transformPose('map', p_stop) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print("erreur") pass print("publie STOP") #yolo_goal.publish(p_stop_map) navigator.shutdown() fin = rospy.get_rostime() #print("fin", fin,"debut", debut) pass ''' elif pixel_yolo == (-2, -2): if id_check.verif == None: pass elif id_check.verif == "OK": if (lock.locked()): lock.release() detect = False elif if_check.verif == "Probleme": print("ERREUR D'IDENTIFICATION") if (lock.locked()): lock.release() if (pixel_yolo != (-1, -1)): detect = True time_last_detection = rospy.Time.now() try: point_camera = PointFromPixel(pixel_yolo, model) p_camera = PoseStamped() p_camera.header.frame_id = 'camera_rgb_optical_frame' p_camera.pose.position.x = point_camera[0] p_camera.pose.position.y = point_camera[1] p_camera.pose.position.z = point_camera[2] p_map = tf_listener.transformPose('map', p_camera) p_map.pose.orientation.x = 0 p_map.pose.orientation.y = 0 p_map.pose.orientation.z = 0 p_map.pose.orientation.w = 1 print(p_map) pos = {'x': p_map.pose.position.x, 'y': p_map.pose.position.y} quat = { 'r1': p_map.pose.orientation.x, 'r2': p_map.pose.orientation.y, 'r3': p_map.pose.orientation.z, 'r4': p_map.pose.orientation.w } except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print("erreur") pass print("publie") # yolo_goal.publish(p_map) success = navigator.goto(pos, quat) ''' else: #cancel_msg = GoalID() #cancel_pub.publish(cancel_msg) # even if it's called shutdown, should just cancel actual goal without shuting down the system navigator.shutdown() ''' # quand il arrive à la personne... # envoyer information via serveur pour executer security4.py if (lock.locked()): lock.release()
def publish_goals(): global tf_buffer global cam_info global pubGoals #todo really necessary?! detected_goals = [] goals_msg = GoalRelative() goals = ["goalLB", "goalLT", "goalRB", "goalRT"] for goal in goals: try: trans = tfBuffer.lookup_transform("camera_optical_frame", str(goal), rospy.Time(0)) corner_stamped = PoseStamped() corner_stamped.header.frame_id = "camera_optical_frame" corner_stamped.pose.position.x = trans.transform.translation.x corner_stamped.pose.position.y = trans.transform.translation.y corner_stamped.pose.position.z = trans.transform.translation.z corner_stamped.pose.orientation = trans.transform.rotation p = [ corner_stamped.pose.position.x, corner_stamped.pose.position.y, corner_stamped.pose.position.z ] k = np.reshape(cam_info.K, (3, 3)) p_pixel = np.matmul(k, p) if p_pixel[2] > 0: p_pixel = p_pixel * (1 / p_pixel[2]) if p_pixel[0] > 0 and p_pixel[0] <= cam_info.width and p_pixel[ 1] > 0 and p_pixel[1] <= cam_info.height: if np.random.uniform(10) < 8 or True: pose = tfBuffer.transform( corner_stamped, "base_footprint", timeout=Duration(seconds=0.5)) point_msg = Point() point_msg.x = add_gaussian_noise(pose.pose.position.x) point_msg.y = add_gaussian_noise(pose.pose.position.y) point_msg.z = pose.pose.position.z #todo set to 0? detected_goals.append(point_msg) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue if len(detected_goals) == 1: goals_msg.left_post = detected_goals[0] goals_msg.right_post = detected_goals[0] elif len(detected_goals) == 2: goals_msg.left_post = detected_goals[0] goals_msg.right_post = detected_goals[1] goals_msg.header.stamp = self.get_clock().now() pubGoals.publish(goals_msg)