Exemple #1
0
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
Exemple #2
0
    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  
Exemple #3
0
    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"
Exemple #4
0
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
Exemple #5
0
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
Exemple #7
0
    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
Exemple #10
0
	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
Exemple #13
0
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
Exemple #14
0
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
Exemple #15
0
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
Exemple #16
0
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)
Exemple #17
0
 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 _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)
Exemple #20
0
 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)
Exemple #21
0
 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
Exemple #25
0
    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)),
         )
Exemple #27
0
    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
Exemple #28
0
 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
Exemple #29
0
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)
Exemple #30
0
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
Exemple #31
0
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
Exemple #32
0
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
Exemple #36
0
    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)
Exemple #37
0
    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)
Exemple #39
0
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