def unitTest(): print "Unit Test running..." req = PathToPoseRequest() # Build a sample border for this test req.border = PolygonStamped() center = Point32() center.x = 0.0 center.y = 0.0 center.z = 0.0 buildShapeH(req.border.polygon, center) req.start = PoseStamped() req.start.pose.position.x = -3.0 req.start.pose.position.y = 3.0 req.start.pose.position.z = 0.0 req.goal = PoseStamped() req.goal.pose.position.x = 3.0 req.goal.pose.position.y = -3.0 req.goal.pose.position.z = 0.0 edges = buildCollisionEdges(req) for edge in edges: drawLine(edge[0], edge[1], white) path = astarFindPath(req) startPoint = path[0] for p in path[1::]: drawLine(Point(startPoint[0], startPoint[1], 0), Point(p[0], p[1], 0), red) startPoint = p result = PathToPose() result.path = Path() for p in path: pose = Pose() pose.position.x = p[0] pose.position.y = p[1] pose.position.z = 0 result.path.poses.append(pose) print "Unit Test completed..." while True: for e in pygame.event.get(): if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE): sys.exit("Leaving because you requested it.")
def handle_msg(msg): goal_tf = "goal%d_ring" % msg.data now = rospy.Time.now() try: listener.waitForTransform("base_link", goal_tf, now, rospy.Duration(1.5)) (goal_pos, _) = listener.lookupTransform("map", goal_tf, now) (robot_trans, rot) = listener.lookupTransform("base_link", goal_tf, now) rot = tf.transformations.euler_from_quaternion(rot) a = math.atan2(robot_trans[1], robot_trans[0]) angle_base.publish(Float32(a)) (launcher_pos, _) = listener.lookupTransform("map", "launcher_base", now) v0, phi, t = launcher.findvelocityandangle(launcher_pos[0], launcher_pos[1], launcher_pos[2], Point32(*goal_pos)) angle_up.publish(Float32(-phi)) msg = PolygonStamped() msg.header.frame_id = 'map' msg.header.stamp = now now = rospy.Time.now() listener.waitForTransform("map", "launcher_tip", now, rospy.Duration(1.5)) (launcher_pos, rot) = listener.lookupTransform("map", "launcher_tip", now) (_, phi, a) = tf.transformations.euler_from_quaternion(rot) x0 = np.array(launcher_pos) f0 = np.array([math.cos(a), math.sin(a), math.sin(-phi)]) * v0 for t in np.arange(0, 2, 0.01): xt = f0 * t + launcher_pos xt[2] -= 0.5 * t * t * 9.81 msg.polygon.points.append(Point32(*xt)) ball_trajectory.publish(msg) except Exception as e: rospy.logerr(e) pass
def __init__(self): self.polygon1 = PolygonStamped() self.polygon1.header.frame_id = "map" points1 = [[0, 0], [6, 0], [6, 2], [2, 2], [2, 4], [6, 4], [6, 6], [0, 6]] for point in points1: new_point = Point32() new_point.x = point[0] new_point.y = point[1] self.polygon1.polygon.points.append(new_point) self.polygon2 = PolygonStamped() self.polygon2.header.frame_id = "map" points2 = [[0, 0], [6, 0], [6, 6], [0, 6], [0, 4], [5, 4], [5, 2], [0, 2]] for point in points2: new_point = Point32() new_point.x = point[0] new_point.y = point[1] self.polygon2.polygon.points.append(new_point) self.pose = Odometry() self.pose.header.frame_id = "map" self.pose.pose.pose.position.x = 5.5 self.pose.pose.pose.position.y = 1 self.pose2 = Odometry() self.pose2.header.frame_id = "map" self.pose2.pose.pose.position.x = 4.9 self.pose2.pose.pose.position.y = 1.1 self.setpoint = PoseStamped() self.setpoint.header.frame_id = "map" self.setpoint.pose.position.x = 5.5 self.setpoint.pose.position.y = 5 self.ticker = 0
def convert_shapely_as_ros_polygon(shapely_polygon): polygon_stamped = PolygonStamped() polygon_stamped.header.seq = 1 polygon_stamped.header.stamp = rospy.Time.now() polygon_stamped.header.frame_id = "/map" ros_polygon = RosPolygon() coords = list(shapely_polygon.exterior.coords) for coord in coords: point = Point32() point.x = coord[0] point.y = coord[1] ros_polygon.points.append(point) polygon_stamped.polygon = ros_polygon return polygon_stamped
def DynamicPolygon(self, header, p_list): """ Dynamicly changing poligon """ p = PolygonStamped() p.header = header # Minimum 3, otherwise the normals can't be calculated and gives error. for i in p_list: point_object = Point32(x=i[0], y=i[1]) p.polygon.points.append(point_object) return p
def publish_visible_area(self, t, o): visible_area = self.cleaningManager.mapManager.get_visible_area_corners( t, o) points = [] for a_point in visible_area: point = Point32() point.x = a_point[0] point.y = a_point[1] points.append(point) rect = PolygonStamped() rect.header.frame_id = "/map" rect.polygon.points = points if len(points) == 4: self.visible_area_pub.publish(rect)
def path_to_polygon(self, path): path_msg = PolygonStamped() path_msg.header.frame_id = "/map" path_msg.header.stamp = rospy.Time.now() list_of_poses = [] for path_index in range(len(path)): temp_pose = Point32() temp_pose.x = path[path_index][0] temp_pose.y = path[path_index][1] temp_pose.z = -1 list_of_poses.append(temp_pose) path_msg.polygon.points = list_of_poses return path_msg
def execute_cb(self, goal): # Create polygon p = PolygonStamped() p.header.frame_id = goal.target_pose.header.frame_id pose = goal.target_pose.pose gx = pose.position.x gy = pose.position.y rpy = euler_from_quaternion([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) gt = rpy[2] for i in range(4): angle = i * pi / 2 + gt + pi / 4 x = gx + FOOTPRINT_RADIUS * cos(angle) y = gy + FOOTPRINT_RADIUS * sin(angle) p.polygon.points.append(Point32(x,y,0)) # front point x = gx + FOOTPRINT_RADIUS * cos(gt) y = gy + FOOTPRINT_RADIUS * sin(gt) p.polygon.points.append(Point32(x,y,0)) rate = rospy.Rate(5) while not rospy.is_shutdown(): if self._as.is_preempt_requested(): self._as.set_preempted() return self.pub.publish(p) t, pose = get_time_and_pose(self.tf, p.header.frame_id, self.target_frame) if pose is not None: if close(pose, gx, gy, gt): break rate.sleep() self._as.set_succeeded() p.polygon.points = [] N = 10 for i in range(N): angle = i * PIx2 / N x = gx + FOOTPRINT_RADIUS * cos(angle) y = gy + FOOTPRINT_RADIUS * sin(angle) p.polygon.points.append(Point32(x,y,0)) for i in range(5): self.pub.publish(p) rate.sleep()
def publish_obstacle_msg(): rospy.init_node("test_obstacle_msg") obstacle_list = []; #list of obstacles pub_list = []; #list of publishers topic_list = []; #for debugging file = open(os.path.join(os.path.dirname(__file__),'./files','obstacles.txt'), 'r'); for line in file: #extract line from file obstacle_msg = line.strip('\n').split(',') #prepare the publisher for a certain polygon topic topic = obstacle_msg[0] topic_list.append(topic) pub = rospy.Publisher('/'+topic, PolygonStamped, queue_size=1) point_list = []; for i in range(1, len(obstacle_msg)): point_i = Point32(); point_i.x = float(obstacle_msg[i].strip().split(' ')[0]) point_i.y = float(obstacle_msg[i].strip().split(' ')[1]) point_list.append(point_i) obstacle = PolygonStamped() obstacle.header.stamp = rospy.Time.now() obstacle.header.frame_id = "map" # CHANGE HERE: odom/map obstacle.polygon.points = point_list obstacle_list.append(obstacle) pub_list.append(pub) file.close(); r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): for i in range(0, len(obstacle_list)): pub_list[i].publish(obstacle_list[i]) print(topic_list[i]) print(obstacle_list[i]) r.sleep()
def __init__(self): """Fake publisher for cart joint states Arguments: - `self`: """ self._joint_state_pub = rospy.Publisher("cart_joint_states", JointState, queue_size=10) self.cart_footprint_pub = rospy.Publisher("/cart_footprint", PolygonStamped, queue_size=10) self.tb = tf.TransformBroadcaster() # Make the message just once since nothing is changing... self.jointstate = JointState() self.jointstate.header.stamp = rospy.Time.now() self.jointstate.name = [ "lf_caster_joint", "lf_wheel_joint", "rf_caster_joint", "rf_wheel_joint", "lb_caster_joint", "lb_wheel_joint", "rb_caster_joint", "rb_wheel_joint" ] self.jointstate.position = [0.0] * len(self.jointstate.name) # Cart footprint message self.cart_footprint = PolygonStamped() self.cart_footprint.header.stamp = rospy.Time.now() self.cart_footprint.header.frame_id = "cart_base_link" self.cart_footprint.polygon.points = [ Point(0.225, 0.225, 0.0), Point(0.225, -0.225, 0.0), Point(-0.225, -0.225, 0.0), Point(-0.225, 0.225, 0.0) ] # Set a transform from the "base" link in the cart urdf to the "cart" frame self.base_pose = PoseStamped( pose=Pose(position=Point(1.15, 0.0, 0.0), orientation=Quaternion(0.0, 0.0, 0.0, 1.0))) self.base_pose.header.stamp = rospy.Time.now() self.base_pose_tup = self.pose_msg_to_tuple(self.base_pose.pose) # set up thread self.thread = threading.Thread(target=self._pub_thread, args=(10, )) self.thread.setDaemon(1) self.alive = threading.Event() self.alive.set() self.thread.start()
def run_detector(self, image, header=None): result = self.detect(image) rois = result['rois'] masks = result['masks'] class_ids = result['class_ids'] scores = result['scores'] rect_array = RectArray() for m in range(masks.shape[2]): if scores[m] > self.__prob_thresh: object_name, color = self.__objects_meta[class_ids[m]] y1, x1, y2, x2 = rois[m] poly = PolygonStamped() poly.polygon.points.append(Point32(x=x1, y=y1)) poly.polygon.points.append(Point32(x=x2, y=y2)) if header is not None: poly.header = header rect_array.polygon.append(poly) rect_array.labels.append(int(class_ids[m])) rect_array.likelihood.append(scores[m]) rect_array.names.append(object_name) im_mask = np.zeros(image.shape[:2], np.int64) im_mask[masks[:,:,m]==True] = np.int64(class_ids[m]) ## temp for j in range(im_mask.shape[0]): for i in range(im_mask.shape[1]): rect_array.indices.append(im_mask[j, i]) if self.__debug: im_plot = self.plot_detection(image, result, self.__labels) cv_result = np.zeros(shape=im_plot.shape, dtype=np.uint8) cv.convertScaleAbs(im_plot, cv_result) print ('Scores: {} {} {}'.format(scores, class_ids, image.shape)) wname = 'image' cv.namedWindow(wname, cv.WINDOW_NORMAL) cv.imshow(wname, cv_result) if cv.waitKey(3) == 27: cv.destroyAllWindows() sys.exit() return rect_array
def make_polys(lines, tf): obs = [PolygonStamped() for _ in range(len(lines))] obs_centers_areas = list() lines = parallel(lines, 0.3) for i, line in enumerate(lines): poly = obs[i].polygon poly.points = [Point32() for _ in range(4)] # we need to make sure that we "extend" the square out in the correct # direction. first, we place the opposite edge of the square: line_len = norm(line) line1 = shift(line, line_len, dir=1) line2 = shift(line, line_len, dir=-1) if dist_to_robot(line1, tf) > dist_to_robot(line2, tf): opp_line = line1 else: opp_line = line2 # now we know the four points, so we can finish this polygon: p0, p1 = line p2, p3 = opp_line obs_x = 0 obs_y = 0 for j, pt in enumerate((p0, p1, p3, p2)): ppt = poly.points[j] x, y = pt ppt.x = x ppt.y = y obst = Obstacle(obs[i]) #check if similar to previous objects flag = False for data in obs_centers_areas: flag = obst.sim_check(data) if not flag: obs_centers_areas.append(obst) ret = Polygons() ret.polygons = obs #print("NUMBER OF LINES DETECTED", len(obs_centers_areas)) return ret, obs_centers_areas
def toPolygon(self): poly = PolygonStamped() poly.header = self.make_header("/map") use_speed_profile = len(self.speed_profile) == len(self.points) for i in xrange(len(self.points)): p = self.points[i] pt = Point32() pt.x = p[0] pt.y = p[1] if use_speed_profile: pt.z = self.speed_profile[i] else: pt.z = -1 poly.polygon.points.append(pt) return poly
def publish_object_found_area(self, px, py, pz): # Publishes a circle around the area where the object is placed msg = PolygonStamped() msg.header.frame_id = "/map" r = 3.0 marker = Marker(type=Marker.CUBE, id=0, lifetime=rospy.Duration(1.0), pose=Pose(Point(px, py, pz), Quaternion(0, 0, 0, 1)), scale=Vector3(r, r, 2.0), header=Header(frame_id='map'), color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text="text") self.pub_object_found_area.publish(marker)
def callback(data): blue_coords, yellow_coords = find_cone_coords(data) middle_x, middle_y = find_middle_points(blue_coords, yellow_coords) middle_points_interpolated = interpolate_middle_points(middle_x, middle_y) if ENABLE_PLOTTING: plt.axes().set_aspect('equal') plt.grid(True) plt.show() trajectory_polygon = PolygonStamped() trajectory_polygon.polygon.points = middle_points_interpolated trajectory_polygon.header.frame_id = "map" trajectory_polygon.header.stamp = rospy.get_rostime() publisher.publish(trajectory_polygon)
def publishPolygons(self, event=None): pmsg = PolygonArray() pmsg.header.stamp = rospy.Time.now() cmsg = ModelCoefficientsArray() cmsg.header.stamp = pmsg.header.stamp for i in range(10): pmsg.polygons.append(PolygonStamped()) pmsg.polygons[i].header.stamp = pmsg.header.stamp pmsg.polygons[i].header.frame_id = str(i) cmsg.coefficients.append(ModelCoefficients()) cmsg.coefficients[i].header.stamp = cmsg.header.stamp cmsg.coefficients[i].header.frame_id = str(i) pmsg.likelihood = [1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0] self.pub_polygon.publish(pmsg) self.pub_coefficients.publish(cmsg)
def robot_tracking(self, TH=0.3): ''' mu_t = np.mean(self.PI_t[:,0:2], axis = 0) cov_t = np.cov(self.PI_t[:,0:2]) max_x = mu_t [0] + np.sqrt(cov_t[0,0]) * self.Sm min_x = mu_t [0] - np.sqrt(cov_t[0,0]) * self.Sm max_y = mu_t [1] + np.sqrt(cov_t[1,1]) * self.Sm min_y = mu_t [1] - np.sqrt(cov_t[1,1]) * self.Sm ''' max_x = np.max(self.PI_t[:, 0]) min_x = np.min(self.PI_t[:, 0]) max_y = np.max(self.PI_t[:, 1]) min_y = np.min(self.PI_t[:, 1]) poly = PolygonStamped() poly.header.frame_id = "map" poly.polygon.points = [] x = np.array([max_x, max_x, min_x, min_x]) y = np.array([max_y, min_y, min_y, max_y]) #print x, y for i in range(4): points = Point32() points.x = x[i] points.y = y[i] points.z = 0 poly.polygon.points.append(points) self.scope_pub.publish(poly) mu_s = np.mean(self.PI_s, axis=0) z = self.scan.scan2cart(mu_s).T * self.scan.map.map.info.resolution z = z[z[:, 0] < max_x] z = z[z[:, 0] > min_x] z = z[z[:, 1] < max_y] z = z[z[:, 1] > min_y] if len(z) > 4: ob = self.scan.obs() _, indices = self.nbrs.kneighbors(z) z_n = ob[indices].reshape(z.shape) z = z[np.linalg.norm(z - z_n, axis=1) > TH] if len(z) > 4: self.mu_t_hat = np.mean(z, axis=0) cov_t_hat = np.cov(z) self.send_robot_estimated(self.mu_t_hat) self.send_other_robot_observation(z)
def DynamicPolygon(header): """ Dynamicly changing poligon """ p = PolygonStamped() p.header = header # Minimum 3, otherwise the normals can't be calculated and gives error. random_number_edges = np.random.randint(low=3, high=15, size=1)[0] for i in range(random_number_edges): point_object = Point32(x=np.random.ranf(), y=np.random.ranf()) p.polygon.points.append(point_object) rospy.loginfo(p) return p
def calculate_goalpoint_one_box(laser_data): p0 = get_lidar_point_at_angle(laser_data, -80) p1 = get_lidar_point_at_angle(laser_data, -70) p2 = get_lidar_point_at_angle(laser_data, 70) p3 = get_lidar_point_at_angle(laser_data, 80) poly = PolygonStamped() poly.header = Header(stamp=rospy.Time.now(), frame_id="laser") poly.polygon.points = [Point32(x=p0[0], y=p0[1]), Point32(x=p1[0], y=p1[1]), Point32(x=p2[0], y=p2[1]), Point32(x=p3[0], y=p3[1])] pub_poly1.publish(poly) p = (p0 + p1 + p2 + p3) / 4 return lidar_to_rear_axle_coords(p)
def __init__(self): self.detected = [] self.people_pub = rospy.Publisher('people', AggregatedPoseTwist, queue_size=1) self.tf_listener = tf.TransformListener() self.footprint = PolygonStamped() num_points = 32 for i in xrange(num_points): p = Point32() p.x = radius * cos((2. * pi * i) / num_points) p.y = radius * sin((2. * pi * i) / num_points) self.footprint.polygon.points.append(p) rospy.sleep(0.5) rospy.Subscriber('tracked_persons', TrackedPersons, self.detected_persons)
def StarPolygon(header): p = PolygonStamped() p.header = header p.polygon.points = [ Point32(x=.0000, y=1.0000 + 3.0), Point32(x=.2245, y=.3090 + 3.0), Point32(x=.9511, y=.3090 + 3.0), Point32(x=.3633, y=-.1180 + 3.0), Point32(x=.5878, y=-.8090 + 3.0), Point32(x=.0000, y=-.3820 + 3.0), Point32(x=-.5878, y=-.8090 + 3.0), Point32(x=-.3633, y=-.1180 + 3.0), Point32(x=-.9511, y=.3090 + 3.0), Point32(x=-.2245, y=.3090 + 3.0) ] return p
def pose_callback(self, msg: PoseStamped): # transform the footprint into the pose original = np.matrix(self.footprint).T rotated = get_rot_matrix(msg) * original + get_trans_vector(msg) # transform so we can iterate simpler rotated = rotated.T # generate the polygon out = PolygonStamped() out.header.frame_id = msg.header.frame_id out.polygon.points = [ Point32(x=col.item(0), y=col.item(1)) for col in rotated ] # publish the result self.pub.publish(out)
def _convert(self, msg): polys_msg = PolygonArray() polys_msg.header = msg.header for rect in msg.rects: poly_msg = PolygonStamped() poly_msg.header = msg.header pt0 = Point32(x=rect.x, y=rect.y) pt1 = Point32(x=rect.x, y=rect.y + rect.height) pt2 = Point32(x=rect.x + rect.width, y=rect.y + rect.height) pt3 = Point32(x=rect.x + rect.width, y=rect.y) poly_msg.polygon.points.append(pt0) poly_msg.polygon.points.append(pt1) poly_msg.polygon.points.append(pt2) poly_msg.polygon.points.append(pt3) polys_msg.polygons.append(poly_msg) self.pub.publish(polys_msg)
def publish(self): """Publish zone information""" # last zone that the receiver was in last_zone = None while not rospy.is_shutdown(): # get the current zone zone = self.positioning.get_zone( self.msg_buffer) if self.msg_buffer else None # check if zone change occurred if zone != last_zone: # publish zone change event event = StringStamped() event.header.stamp = self.last_time # only zone leave if zone is None: event.data = last_zone.name self.zone_leave_pub.publish(event) # only zone enter elif last_zone is None: event.data = zone.name self.zone_enter_pub.publish(event) # leave on zone and enter another else: event.data = last_zone.name self.zone_leave_pub.publish(event) event.data = zone.name self.zone_enter_pub.publish(event) if zone is not None: # publish zone name name = StringStamped() name.header.stamp = self.last_time name.header.frame_id = zone.frame_id name.data = zone.name self.zone_name_pub.publish(name) # publish zone polygon polygon = PolygonStamped() polygon.header.stamp = self.last_time polygon.header.frame_id = zone.frame_id points = [] for p in zone.polygon: points.append(Point32(p[0], p[1], p[2])) polygon.polygon.points = points self.zone_polygon_pub.publish(polygon) # set current zone to last zone last_zone = zone # wait to start next iteration self.rate.sleep()
def __init__(self): # TODO : non-hardcoded footprint (possibly load from .yaml files given as a rosparam) self.fpt_ = np.asarray( [[0.25649121, -0.17213032], [0.17213032, -0.17468672], [0.0562406, -0.17468672], [-0.00596491, -0.14315788], [-0.05027569, -0.09117794], [-0.06987468, -0.00426065], [-0.05027569, 0.07243107], [0.00170426, 0.13804510], [0.05624060, 0.16616541], [0.16957393, 0.16957393], [0.25478697, 0.16786967]], dtype=np.float32) self.fpt_msg_ = PolygonStamped() self.fpt_msg_.header.stamp = rospy.Time.now() self.fpt_msg_.header.frame_id = 'base_link' self.fpt_msg_.polygon = Polygon( [Point32(x, y, 0) for (x, y) in self.fpt_]) self.fpt_pub_ = rospy.Publisher('fpt', PolygonStamped, queue_size=2)
def path_rosvis(path, topic_base_name="static_path_"): pts_all = [] pose_all = [] path_all = [] for segment in path: if type(segment) == Line2D: pts_1seg = segment.to_ros(output_type=Point32) poses_1seg = segment.to_ros(output_type=Pose) path_1seg = segment.to_ros(output_type=PoseStamped) elif type(segment) == Arc2D: pts_1seg = segment.to_ros_div_len(0.5, output_type=Point32) poses_1seg = segment.to_ros_div_len(0.5, output_type=Pose) path_1seg = segment.to_ros_div_len(0.5, output_type=PoseStamped) pts_all = pts_all + pts_1seg pose_all = pose_all + poses_1seg path_all = path_all + path_1seg header = Header() header.seq = 0 header.stamp = rospy.Time.now() # rviz内部でfloat32を使っているらしくUTM座標系の表示は桁落ちでパスがガタガタになる # 32bitのraspi用の ubuntu Mate だけでなく64bitPC版のUbuntuでも同様の現象になる # header.frame_id = UTM_FRAME_NAME header.frame_id = WORK_ORIGIN_FRAME_NAME static_path_poly = PolygonStamped() static_path_poly.header = header static_path_poly.polygon.points = pts_all pub_poly=rospy.Publisher(topic_base_name+"_poly", \ PolygonStamped, queue_size=2, latch=True) pub_poly.publish(static_path_poly) static_path_parr = PoseArray() static_path_parr.header = header static_path_parr.poses = pose_all pub_parr=rospy.Publisher(topic_base_name+"_parr", \ PoseArray, queue_size=2, latch=True) pub_parr.publish(static_path_parr) static_path = Path() static_path.header = header static_path.poses = path_all pub_path = rospy.Publisher(topic_base_name, Path, queue_size=2, latch=True) pub_path.publish(static_path)
def __init__(self): # ros interface self.pub = rospy.Publisher('footprint', Polygon, queue_size=10) self.pub_stamped = rospy.Publisher('footprint_stamped', PolygonStamped, queue_size=10) self.pub_original_stamped = rospy.Publisher('footprint_base', PolygonStamped, queue_size=10) self.tf_listener = tf.TransformListener() # params self.footprint = rospy.get_param("~footprint") self.footprint_frame = rospy.get_param("~footprint_frame") self.target_frames = rospy.get_param("~target_frames") self.inflation_radius = rospy.get_param("~inflation_radius", 0.1) # build footprint (for assertion purposes and base footprint generation) base_hull = self.build_base_hull(self.footprint) self.base_msg = PolygonStamped() self.base_msg.header.frame_id = self.footprint_frame self.base_msg.polygon = self.build_polygon_msg(base_hull) # setup rospy.loginfo("Waiting for initial transform from frames '" + str(self.target_frames) + "' to '" + self.footprint_frame + "'") while not rospy.is_shutdown(): success = True for target_frame in self.target_frames: try: self.tf_listener.waitForTransform(target_frame, self.footprint_frame, rospy.Time(), rospy.Duration(2)) except tf.Exception: rospy.logwarn("Unavailable transform from frame '" + str(target_frame) + "' to '" + self.footprint_frame + "'. Retrying ...") success = False if success: break rospy.loginfo("Ready to work ...")
def createExploreTaskGoal(points): polygon = PolygonStamped() polygon.header.stamp = rospy.Time.now() polygon.header.frame_id = mapFrame polygon.polygon.points = [] # Make explore corner points into a correct ROS message for point in points: x, y = point polygon.polygon.points.append(Point32(x=x, y=y)) # Add start point for exploration startPoint = PointStamped() startPoint.header.stamp = rospy.Time.now() startPoint.header.frame_id = mapFrame startPoint.point = Point(x=0, y=0) return ExploreTaskGoal(explore_boundary=polygon, explore_center=startPoint)
def rectangle(header, width, height, center): poly = PolygonStamped() poly.header = deepcopy(header) x, y, z = center.x, center.y, center.z # this is for doing it in image coordinates # poly.polygon.points.append(Point(x,y-width/2,z-height/2)) # poly.polygon.points.append(Point(x,y+width/2,z-height/2)) # poly.polygon.points.append(Point(x,y+width/2,z+height/2)) # poly.polygon.points.append(Point(x,y-width/2,z+height/2)) # normal frame alignment poly.polygon.points.append(Point(x - width / 2, y - height / 2, z)) poly.polygon.points.append(Point(x + width / 2, y - height / 2, z)) poly.polygon.points.append(Point(x + width / 2, y + height / 2, z)) poly.polygon.points.append(Point(x - width / 2, y + height / 2, z)) return poly
def paint(self, fieldOfView_, parent_frame_, publisher_) : poly = PolygonStamped() poly.header.frame_id=parent_frame_ poly.header.stamp = rospy.Time.now() poly.polygon.points = [Point32() for i in range(4)] poly.polygon.points[0].x = fieldOfView_.x_low poly.polygon.points[0].y = fieldOfView_.y_low poly.polygon.points[0].z = 1.0 poly.polygon.points[1].x = fieldOfView_.x_low poly.polygon.points[1].y = fieldOfView_.y_high poly.polygon.points[1].z = 1.0 poly.polygon.points[2].x = fieldOfView_.x_high poly.polygon.points[2].y = fieldOfView_.y_high poly.polygon.points[2].z = 1.0 poly.polygon.points[3].x = fieldOfView_.x_high poly.polygon.points[3].y = fieldOfView_.y_low poly.polygon.points[3].z = 1.0 publisher_.publish(poly)