def LargeSquarePolygon(header): p = PolygonStamped() p.header = header p.polygon.points = [ Point32(x=-10.0, y=10.0), Point32(x=-6.0, y=10.0), Point32(x=-2.0, y=10.0), Point32(x=2.0, y=10.0), Point32(x=6.0, y=10.0), Point32(x=10.0, y=10.0), Point32(x=10.0, y=6.0), Point32(x=10.0, y=2.0), Point32(x=10.0, y=-2.0), Point32(x=10.0, y=-6.0), Point32(x=10.0, y=-10.0), Point32(x=6.0, y=-10.0), Point32(x=2.0, y=-10.0), Point32(x=-2.0, y=-10.0), Point32(x=-6.0, y=-10.0), Point32(x=-10.0, y=-10.0), Point32(x=-10.0, y=-6.0), Point32(x=-10.0, y=-2.0), Point32(x=-10.0, y=2.0), Point32(x=-10.0, y=6.0) ] return p
def _create_initial_frontier_goal(self): frontier_gh = ExploreTaskGoal() curr_ts = rospy.Time.now() curr_header = Header() curr_header.frame_id = "map" curr_header.seq = 1 curr_header.stamp = curr_ts # Point Stamped pt_s = PointStamped() pt_s.header = curr_header pt_s.point.x = 1.0 # Polygon Stamped # Note that polygon is not defined so it's an unbounded exploration pg_s = PolygonStamped() pg_s.header = curr_header vertices = [(5.0, 5.0), (-5.0, 5.0), (-5.0, -5.0), (5.0, -5.0)] for vertex in vertices: pg_s.polygon.points.append(Point(x=vertex[0], y=vertex[1])) #frontier_gh.header = curr_header #frontier_gh.goal_id.stamp = curr_ts #frontier_gh.goal_id.id = 'initial_frontier_marco' frontier_gh.explore_boundary = pg_s frontier_gh.explore_center = pt_s rospy.loginfo(frontier_gh) return frontier_gh
def publishMap(self): ''' Takes a mapPath [(x1, y1), (x2, y2), ...] in map coordinates and outputs a PolygonStmped of the points in vesc (world) coordinates ''' poly = PolygonStamped() poly.header = utils_TA.make_header("/map") scale = 0.0504 for i in xrange(len(self.pathTuples)): p = self.pathTuples[i] x = 25.90 - p[0] * scale y = -16.50 + p[1] * scale pt = Point32() pt.x = x pt.y = y pt.z = -1 poly.polygon.points.append(pt) self.path_pub.publish(poly) print poly print "traj pubbed"
def MovingSquare(header): """ Dynamicly moving Square """ p = PolygonStamped() p.header = header #print header.stamp.nsecs/100000000.0 dynamic_position_val = sin(header.stamp.nsecs / 1000000000.0) dynamic_position_cos = cos(header.stamp.nsecs / 1000000000.0) #print dynamic_position_val p.polygon.points = [ Point32(x=dynamic_position_val + 1.0, y=dynamic_position_val + 1.0, z=dynamic_position_val), Point32(x=dynamic_position_val - 1.0, y=dynamic_position_val + 1.0, z=dynamic_position_cos), Point32(x=dynamic_position_val - 1.0, y=dynamic_position_val - 1.0, z=dynamic_position_cos), Point32(x=dynamic_position_val + 1.0, y=dynamic_position_val - 1.0, z=dynamic_position_val) ] return p
def main(pub_path): global m_pose_seen, m_pose_stamped r = rospy.Rate(2) m_polygon = PolygonStamped() header = Header() header.frame_id = "odom" header.stamp = rospy.Time.now() m_polygon.header = header m_polygon = LargeSquarePolygon(header) """ m_polygon = [SquarePolygon(header), RectanglePolygon(header), CirclePolygon(header), StarPolygon(header)][3] """ while not m_pose_seen: rospy.loginfo("NOT pose seen.") r.sleep() rospy.loginfo("pose seen.") res = do_action(m_polygon, m_pose_stamped) rospy.loginfo("len(res.plan.points) == " + str(len(res.plan.points))) rospy.loginfo("do_action(..) seen.") mpath = convert_striping_plan_to_path_client(res) rospy.loginfo("convert_striping_plan_to_path_client(..) seen.") rospy.loginfo(str(mpath)) while not rospy.is_shutdown(): pub_path.publish(mpath.path) r.sleep()
def get_rightwall_endpoint(laser_data): p1 = get_lidar_point_at_angle(laser_data, -90) p2 = get_lidar_point_at_angle(laser_data, -90 + 20) delta_y = p2[1] - p1[1] delta_x = p2[0] - p1[0] denominator = math.sqrt(delta_y**2 + delta_x**2) numerator_const_term = p2[0] * p1[1] - p2[1] * p1[0] wall_at_infinity = p1 + (p2 - p1) * 40 rightwall = PolygonStamped() rightwall.header = Header(stamp=rospy.Time.now(), frame_id="laser") rightwall.polygon.points = [ Point32(x=p1[0], y=p1[1]), Point32(x=wall_at_infinity[0], y=wall_at_infinity[1]) ] pub_rightwall.publish(rightwall) angle = -90 + 20 endpoint = get_lidar_point_at_angle(laser_data, angle) for i in range(20): angle += 5 candid = get_lidar_point_at_angle(laser_data, angle) distance = abs(delta_y * candid[0] - delta_x * candid[1] + numerator_const_term) / denominator if distance < 0.5: endpoint = candid else: break endpoint_msg = PointStamped() endpoint_msg.header = Header(stamp=rospy.Time.now(), frame_id="laser") endpoint_msg.point = Point(endpoint[0], endpoint[1], 0) pub_rightwall_endpoint.publish(endpoint_msg) return endpoint
def pose_callback(self, pose_msg): ''' Uses a pose message to generate an odometry and state message. :param pose_msg: (geometry_msgs/PoseStamped) pose message ''' new_pose_msg, trans, rot = self.tf_to_pose("LO01_base_link", "map", pose_msg.header) if not self.use_amcl: pose_msg = new_pose_msg self.new_pose_pub.publish( pose_msg ) # if using AMCL, this will be the same as the original pose message if trans and rot: # if not getting tf, trans and rot will be None footprint = PolygonStamped() footprint.header = pose_msg.header # has same frame_id (map) and time stamp loomo_points = np.array([[0.16, -0.31], [0.16, 0.31], [-0.16, 0.31], [-0.16, -0.31]]) roll, pitch, yaw = tf.transformations.euler_from_quaternion(rot) rot = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) rotated_points = np.matmul(rot, loomo_points.T) # 2x4 array rot_and_trans = rotated_points + np.array([[trans[0]], [trans[1]]]) polygon = Polygon( points=[Point32(x=x, y=y, z=0) for x, y in rot_and_trans.T]) footprint.polygon = polygon self.footprint_pub.publish(footprint) odom_msg = Odometry() odom_msg.pose.pose = pose_msg.pose odom_msg.header = pose_msg.header self.odom_pub.publish(odom_msg) state_msg = State() state_msg.header = pose_msg.header state_msg.state_stamp = pose_msg.header.stamp state_msg.pos.x = pose_msg.pose.position.x state_msg.pos.y = pose_msg.pose.position.y state_msg.pos.z = pose_msg.pose.position.z state_msg.quat.x = pose_msg.pose.orientation.x state_msg.quat.y = pose_msg.pose.orientation.y state_msg.quat.z = pose_msg.pose.orientation.z state_msg.quat.w = pose_msg.pose.orientation.w if self.last_state_time and self.last_state: dt = pose_msg.header.stamp.nsecs - self.last_state_time state_msg.vel.x = (state_msg.pos.x - self.last_state.pos.x) / (float(dt) / 10**9) state_msg.vel.y = (state_msg.pos.y - self.last_state.pos.y) / (float(dt) / 10**9) state_msg.vel.z = 0 # ground robot not traveling in z direction self.last_state_time = pose_msg.header.stamp.nsecs self.last_state = state_msg self.state_pub.publish(state_msg)
def polygon_cb(msg_var, data_var): global polygon_voronoi_publisher, header idx = data_var polygon_msg = PolygonStamped() polygon_msg.header = header polygon_msg.polygon = msg_var #print data_var,msg_var polygon_voronoi_publisher[idx].publish(polygon_msg)
def RectanglePolygon(header): p = PolygonStamped() p.header = header p.polygon.points = [Point32(x=-1.0, y=1.0, z=1.0), Point32(x=-2.0, y=1.0, z=1.0), Point32(x=-2.0, y=-1.0, z=1.0), Point32(x=-1.0, y=-1.0, z=1.0)] return p
def main_loop(self): rate = rospy.Rate(self.frequency) while not rospy.is_shutdown(): if self.current_velocity is not None and self.current_heading is not None and self.nearest_pedestrian is not None: x = self.nearest_pedestrian[0]/100 y = self.nearest_pedestrian[1]/100 v = self.current_velocity if v > 20: v = 20 h = self.current_heading state = [x,y,v,h] vi, ci, ii = self.check_reachability(state) critical_polygon = PolygonStamped() header = Header() header.stamp = rospy.Time.now() header.frame_id = 'velodyne' critical_polygon.header = header for i in range(len(self.critical_reachability[vi][ci][0])): x = self.critical_reachability[vi][ci][0][i] y = self.critical_reachability[vi][ci][1][i] xr = x*np.cos(h)-y*np.sin(h) yr = x*np.sin(h)+y*np.cos(h) z = 0 critical_polygon.polygon.points.append(Point32(x=xr, y=yr, z=z)) self.critical_polygon_pub.publish(critical_polygon) imminent_polygon = PolygonStamped() header = Header() header.stamp = rospy.Time.now() header.frame_id = 'velodyne' imminent_polygon.header = header for i in range(len(self.imminent_reachability[vi][ii][0])): x = self.imminent_reachability[vi][ii][0][i] y = self.imminent_reachability[vi][ii][1][i] xr = x*np.cos(h)-y*np.sin(h) yr = x*np.sin(h)+y*np.cos(h) z = 0 imminent_polygon.polygon.points.append(Point32(x=xr, y=yr, z=z)) self.imminent_polygon_pub.publish(imminent_polygon) rate.sleep()
def SquarePolygon(header): p = PolygonStamped() p.header = header p.polygon.points = [Point32(x=1.0, y=1.0), Point32(x=-1.0, y=1.0), Point32(x=-1.0, y=-1.0), Point32(x=1.0, y=-1.0)] return p
def CirclePolygon(header): p = PolygonStamped() p.header = header for i in range(100): theta = i / 100.0 * 2.0 * pi x = 1.0 * cos(theta) + 3.0 y = 1.0 * sin(theta) p.polygon.points.append(Point32(x=x, y=y)) return p
def RectanglePolygon(header): p = PolygonStamped() p.header = header p.polygon.points = [ Point32(x=-1.0, y=1.0, z=1.0), Point32(x=-2.0, y=1.0, z=1.0), Point32(x=-2.0, y=-1.0, z=1.0), Point32(x=-1.0, y=-1.0, z=1.0) ] return p
def SquarePolygon(header): p = PolygonStamped() p.header = header p.polygon.points = [ Point32(x=1.0, y=1.0), Point32(x=-1.0, y=1.0), Point32(x=-1.0, y=-1.0), Point32(x=1.0, y=-1.0) ] return p
def calculate_goalpoint_two_boxes(laser_data): poly = PolygonStamped() center = PointStamped() p1 = get_lidar_point_at_angle(laser_data, -80) p2 = get_lidar_point_at_angle(laser_data, -70) p3 = get_lidar_point_at_angle(laser_data, 70) p4 = get_lidar_point_at_angle(laser_data, 80) poly.header = Header(stamp=rospy.Time.now(), frame_id="laser") poly.polygon.points = [Point32(x=p1[0], y=p1[1]), Point32(x=p2[0], y=p2[1]), Point32(x=p3[0], y=p3[1]), Point32(x=p4[0], y=p4[1])] pub_poly1.publish(poly) center1 = (p1 + p2 + p3 + p4) / 4 center.header = Header(stamp=rospy.Time.now(), frame_id="laser") center.point = Point(center1[0], center1[1], 0) pub_point1.publish(center) p5 = get_lidar_point_at_angle(laser_data, -50) p6 = get_lidar_point_at_angle(laser_data, -40) p7 = get_lidar_point_at_angle(laser_data, 40) p8 = get_lidar_point_at_angle(laser_data, 50) poly.header = Header(stamp=rospy.Time.now(), frame_id="laser") poly.polygon.points = [Point32(x=p5[0], y=p5[1]), Point32(x=p6[0], y=p6[1]), Point32(x=p7[0], y=p7[1]), Point32(x=p8[0], y=p8[1])] pub_poly2.publish(poly) center2 = (p5 + p6 + p7 + p8) / 4 center.header = Header(stamp=rospy.Time.now(), frame_id="laser") center.point = Point(center2[0], center2[1], 0) pub_point2.publish(center) centerboxes = (center1+center2)/2 return lidar_to_rear_axle_coords(centerboxes)
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 + rect.width, y=rect.y + rect.height) poly_msg.polygon.points.append(pt0) poly_msg.polygon.points.append(pt1) polys_msg.polygons.append(poly_msg) self.pub.publish(polys_msg)
def convert_rect_to_polygon(rect, header): poly_msg = PolygonStamped() poly_msg.header = 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) return poly_msg
def publish(self): polygon_msg = PolygonStamped() pointcloud_msg = PointCloud() pointcloud_msg.header.frame_id = self.publish_frame pointcloud_msg.header.stamp = rospy.Time.now() pointcloud_msg.points = self.corner_points_transformed self.corner_points_pub.publish(pointcloud_msg) polygon_msg.header = pointcloud_msg.header polygon_msg.polygon.points = self.corner_points_transformed self.foot_state_msg.foot_state.polygon.points = self.corner_points_transformed self.polygon_pub.publish(polygon_msg) self.foot_state_pub.publish(self.foot_state_msg)
def _convert(self, msg): polygon_msg = PolygonStamped() polygon_msg.header = msg.header if self.index < 0: return elif self.index < len(msg.polygons): polygon_msg = msg.polygons[self.index] self.pub.publish(polygon_msg) else: logerr_throttle( 10, 'Invalid index {} is specified ' 'for polygon array whose size is {}'.format( self.index, len(msg.polygons)))
def generate_plane(self, header, xmin=-0.0, xmax=1.0, ymin=-1.0, ymax=1.0): msg = PolygonArray() msg.header = header p = PolygonStamped() p.header = header p.polygon.points = [Point32(x=xmin, y=ymin), Point32(x=xmax, y=ymin), Point32(x=xmax, y=ymax), Point32(x=xmin, y=ymax)] msg.polygons.append(p) return msg
def publish_triangle_polygon(self, pose): pts = [] pts.append(tfx.pose((0,0.0079,0))) pts.append(tfx.pose((0,-0.0079,0))) pts.append(tfx.pose((0,0, -0.013))) for i in range(len(pts)): # IPython.embed() pts[i] = (pose.as_tf()*pts[i]).position.msg.Point() polygon = Polygon() polygon_stamped = PolygonStamped() polygon.points = pts polygon_stamped.polygon = polygon polygon_stamped.header = pose.msg.Header() self.triangle_polygon_publisher.publish(polygon_stamped)
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 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 _convert(self, msg): polygon_msg = PolygonStamped() polygon_msg.header = msg.header if self.index < 0: return elif self.index < len(msg.polygons): polygon_msg = msg.polygons[self.index] self.pub.publish(polygon_msg) else: logerr_throttle( 10, "Invalid index {} is specified " "for polygon array whose size is {}".format(self.index, len(msg.polygons)), )
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 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 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 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 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 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 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 zones_to_polygons(self, zones): polygons = [] for zone in zones.zones: # Expand to a PolygonStamped of 3d points polygon = PolygonStamped() polygon.header = zone.header # copy both tf frame and timestamp # Permute! point = Point32() x, y, z = tuple(dim/2.0 for dim in zone.extents.dimensions) # sides -> half-sides for point.x in [-1*x, x]: for point.y in [-1*y, y]: for point.z in [-1*z, z]: polygon.polygon.points.append(copy.deepcopy(point)) polygons.append(polygon) return polygons
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 _callback_field_boundary(self, msg): field = self.get_plane(msg.header.stamp, 0.0) if field is None: return field_boundary = PolygonStamped() field_boundary.header = msg.header field_boundary.header.frame_id = self._publish_frame for p in msg.polygon.points: p_relative = self._transform_point(p, field, msg.header.stamp) if p_relative is not None: field_boundary.polygon.points.append(p_relative) else: rospy.logwarn_throttle( 5.0, rospy.get_name() + ": At least one point of the Field Boundary could not be transformed," + " dropping message") return self._field_boundary_pub.publish(field_boundary)
def callback_objsBoxs(self, objsBoxs_msg): objsPints = PolygonStamped() objsPints.header = objsBoxs_msg.header for i in range(len(objsBoxs_msg.objects_vector)): x_offset = objsBoxs_msg.objects_vector[i].roi.x_offset y_offset = objsBoxs_msg.objects_vector[i].roi.y_offset height = objsBoxs_msg.objects_vector[i].roi.height width = objsBoxs_msg.objects_vector[i].roi.width # u_center <-- x + w*1/2 # v_center <-- y + h*1/2 u_center = x_offset + width * 1 / 2 v_center = y_offset + height * 1 / 2 d = self.cv_dipth_img[v_center, u_center] if math.isnan(d): rospy.logwarn("=== d is nan ============") else: z = d / self.camera_factor x = (u_center - self.camera_cx) * z / self.camera_fx y = (v_center - self.camera_cy) * z / self.camera_fy objPint = Point32() objPint.x = x objPint.y = y objPint.z = z objsPints.polygon.points.append(objPint) if objsPints.polygon.points: self._pub_objsPoints.publish(objsPints) # rospy.loginfo(objsPints) else: self._pub_objsPoints.publish(objsPints) pass #
def deflect_path(self): # use the nearest colliding point to deflect the path accordingly if self.closest_pt_index is not None: dist = self.laser_ranges[self.closest_pt_index] angle = normalize_angle( self.real_laser_angles[self.closest_pt_index] + self.FORWARD_ANGLE) angle_rel_car_pose = self.laser_angles[self.closest_pt_index] print("angle", angle) print("best", self.best_pt_angle) deflection_dir = 1 if (sign(self.best_pt_angle) < sign(angle) or (sign(self.best_pt_angle) == sign(angle) and self.best_pt_angle - angle > 0)) else -1 deflection_factor = deflection_dir * min( (self.MAX_TURN_ANGLE) / (max(dist, 1.0)**2) * abs(abs(angle_rel_car_pose) - math.pi / 4) / (math.pi / 4), self.MAX_TURN_ANGLE) print("def factor", deflection_factor) new_angle = normalize_angle(self.best_pt_angle + deflection_factor) print("new angle", new_angle) self.deflected_pt = (self.cur_pose.position.x + self.best_pt_dist * math.cos(new_angle), self.cur_pose.position.y + self.best_pt_dist * math.sin(new_angle)) else: self.deflected_pt = self.best_pt path = PolygonStamped() path.header = Utils.make_header("map") path.polygon = Polygon([ Point(self.cur_pose.position.x, self.cur_pose.position.y, 0), Point(self.deflected_pt[0], self.deflected_pt[1], 0) ]) self.path_command_pub.publish(path)
def publish_freespace(Publisher, LF, Header, FrameId): """ Function that publishes a Polygon message showing the current local freespace Input: 1) Publisher: PolygonStamped ROS publisher 2) LF: Local freespace polygon 3) Header: ROS header to be appended to Polygon message 4) FrameId: String defining the frame_id of the Polygon message """ # Publish freespace polygon polygon_msg = GeometryMsgsPolygon() polygon_msg.header = Header polygon_msg.header.frame_id = FrameId if LF.any() and ShapelyPolygon(LF).is_valid: numvertex = LF.shape[0] for i in range(0, numvertex): polygon_msg.polygon.points.append( Point32(x=LF[i][0], y=LF[i][1], z=0.)) else: polygon_msg.polygon.points.append(Point32(x=0., y=0., z=0.)) Publisher.publish(polygon_msg) return