예제 #1
0
    def createPointMarker2(self, points, marker_id, rgba = None, pose=[0,0,0,0,0,0,1], frame_id = '/world'):
        marker = Marker()
        marker.header.frame_id = "/map"
        marker.type = marker.POINTS
        marker.scale.x = 0.01
        marker.scale.y = 0.01
        marker.scale.z = 0.01
        marker.id = marker_id

        n = len(points)
        sub = 1
    
        if rgba is not None:
            marker.color.r, marker.color.g, marker.color.b, marker.color.a = tuple(rgba)

        for i in xrange(0,n,sub):
            p = Point()
            p.x = points[i][0]
            p.y = points[i][1]
            p.z = points[i][2]
            marker.points.append(p)


        if rgba is None:
            for i in xrange(0,n,sub):
                p = ColorRGBA()
                p.r = points[i][3]
                p.g = points[i][4]
                p.b = points[i][5]
                p.a = points[i][6]
                marker.colors.append(p)

        # marker.pose = poselist2pose(pose)

        return marker
예제 #2
0
            def recur(node):
                if self.enable_vis:
                    TEMP = Point()
                    TEMP.x = node.value[0]
                    TEMP.y = node.value[1]
                    TEMP.z = .05
                    marker.points.append(TEMP)

                    TEMP = ColorRGBA()
                    TEMP.r = 1
                    TEMP.g = 0
                    TEMP.b = 0
                    TEMP.a = 1

                    marker.colors.append(TEMP)

                self.trajectory.points.append([node.value[0], node.value[1]])
                parent = node.parent
                if parent is not None:
                    recur(parent)
 def marker_stuff(self, x_obs, y_obs):
     marker = Marker()
     marker.header.frame_id = "map"
     marker.type = marker.POINTS
     marker.action = marker.ADD
     
     marker.scale.x = 0.5
     marker.scale.y = 0.5
     for i in range(len(x_obs)):
         TEMP = Point()
         TEMP.x = x_obs[i]
         TEMP.y = y_obs[i]
         TEMP.z = .05
         marker.points.append(TEMP)
     
         TEMP = ColorRGBA()
         TEMP.r = 1
         TEMP.g = 0
         TEMP.b = 0
         TEMP.a = 1
         
         marker.colors.append(TEMP)
     
     self.vis_pub.publish(marker)
예제 #4
0
    def lidar_cb(self, msg):
        rospy.loginfo("starting lidar cb")
        if self.road_map is None:
            return

        rospy.loginfo("got map")
        ranges = np.array(msg.ranges)
        num_ranges = len(ranges)
        angles = (np.ones(num_ranges) * msg.angle_min) + (
            np.arange(num_ranges) * msg.angle_increment)

        # Discard extraneous data (< range_min or > range_max)
        angles = angles[ranges > msg.range_min]
        ranges = ranges[ranges > msg.range_min]
        angles = angles[ranges < msg.range_max]
        ranges = ranges[ranges < msg.range_max]

        # Remove data points not in the viable range in front of the car, ie theta E (-viable_angle_range, viable_angle_range)
        ranges = ranges[angles > -self.viable_angle_range]
        angles = angles[angles > -self.viable_angle_range]
        ranges = ranges[angles < self.viable_angle_range]
        angles = angles[angles < self.viable_angle_range]

        # Remove data points outside self.in_range
        angles = angles[ranges < self.in_range]
        ranges = ranges[ranges < self.in_range]

        # Convert to cartesian, LIDAR-frame positions
        x = ranges * np.cos(angles)
        y = ranges * np.sin(angles)
        new_num_ranges = len(x)
        arr = np.vstack((x, y))

        # Find transform from LIDAR frame to map frame
        try:
            (trans,
             rot) = self.listener.lookupTransform(msg.header.frame_id,
                                                  self.road_map.frame,
                                                  rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException,
                tf.ExtrapolationException):
            rate.sleep()
            return

        # Convert translation + quaternion to homogeneous transformation matrix
        T_lidar_map = transformations.quaternion_matrix(rot)
        T_lidar_map = T_lidar_map.T
        TEMP = transformations.quaternion_matrix(rot)
        TEMP = np.linalg.inv(TEMP[:3, :3])
        T_lidar_map[:3, 3] = np.matmul(TEMP, -np.array(trans).T)
        #T_lidar_map = T_lidar_map.T

        # Convert LIDAR points from LIDAR frame to map frame
        arr_LIDAR = np.vstack(
            (x, y, np.zeros(new_num_ranges), np.ones(new_num_ranges)))
        arr_MAP = np.matmul(T_lidar_map, arr_LIDAR)
        print(T_lidar_map)
        arr_MAP = arr_MAP[:2]

        x_map = arr_MAP[0]
        y_map = arr_MAP[1]

        # Find which points are on the roads
        on_road = self.road_map.check_free(arr_MAP)

        x_obs = x_map[on_road]
        y_obs = y_map[on_road]
        """        
        # Find occupancy grid matrix with detected obstacles filled in
        new_occ_matrix = self.road_map.add_obstructions(np.vstack((x_obs, y_obs)))
        
        
        # Creates and publishes OccupancyGrid message with obstacles to /obstacles
        obs_msg = OccupancyGrid()
        
        # now = rospy.get_time()
        # obs_msg.header.stamp = now
        obs_msg.header.frame_id = self.road_map.frame
        
        obs_msg.info = self.road_map.msg.info
        # obs_msg.info.map_load_time = rospy.Time(0) # Probably unnecessary?
        
        obs_msg.data = np.ravel(new_occ_matrix)
        rospy.loginfo("publishing")
        
        self.obs_pub.publish(obs_msg)
        
        print("test")
        """
        marker = Marker()
        marker.header.frame_id = "map"
        marker.type = marker.POINTS
        marker.action = marker.ADD

        marker.scale.x = 0.5
        marker.scale.y = 0.5
        for i in range(len(x_obs)):
            TEMP = Point()
            TEMP.x = x_obs[i]
            TEMP.y = y_obs[i]
            TEMP.z = .05
            marker.points.append(TEMP)

            TEMP = ColorRGBA()
            TEMP.r = 1
            TEMP.g = 0
            TEMP.b = 0
            TEMP.a = 1

            marker.colors.append(TEMP)

        self.vis_pub.publish(marker)
예제 #5
0
    def plan_path(self, start_point, end_point, map_obj):
        """
        RRT*. Tries to find a collision free path from start to goal.
        """
        # STUFF FOR TESTING
        self.trajectory.clear()
        if self.enable_vis:
            marker = Marker()
            marker.header.frame_id = "/map"
            marker.type = marker.POINTS
            marker.action = marker.ADD

            marker.scale.x = 0.1
            marker.scale.y = 0.1
            self.vis_pub.publish(marker)

        exploration_bias = 1.0 - self.goal_bias
        final_node = None
        num_existing_path_points_added = 0

        self.rrt_star = RRTStar(Node(start_point))
        self.max_iterations = self.rrt_star.max_size
        while self.rrt_star.size <= self.max_iterations:
            p = np.random.uniform()
            if p < exploration_bias:

                x_rand = self.map.sample_free_space()
            else:
                if final_node is None:
                    x_rand = end_point
                else:
                    x_rand = self.branched_from_existing_path(
                        final_node,
                        depth_underestimate=num_existing_path_points_added)
                    num_existing_path_points_added += 1

            x_nearest = self.rrt_star.nearest(
                x_rand)  # Find the nearest node to x_rand

            path = self.map.generate_line_path(x_nearest.value,
                                               x_rand,
                                               eta=self.eta)
            if path is not None:  # no obstacles between x_nearest and x_rand
                x_new = path[-1]
                X_nearby_connectable = self.find_nearby_connectable(
                    x_nearest, x_new)

                cost_min, node_min = self.find_best_parent(
                    X_nearby_connectable, x_new)

                X_nearby_connectable.remove(
                    node_min
                )  # Remove x_new's parent node from the list of nearby nodes so it is not considered for rewiring

                # Create the new node at x_new!
                node_new = self.rrt_star.add_config(node_min, x_new)

                if self.enable_vis:
                    # FOR TESTING ONLY #
                    # Code to publish marker for new node
                    ###########################################################################################
                    TEMP = Point()
                    TEMP.x = x_new[0]
                    TEMP.y = x_new[1]
                    TEMP.z = .05
                    marker.points.append(TEMP)

                    TEMP = ColorRGBA()
                    TEMP.r = 1
                    TEMP.g = 0
                    TEMP.b = 0
                    TEMP.a = 1

                    marker.colors.append(TEMP)

                    self.vis_pub.publish(marker)
                    ###########################################################################################

                self.rewire(cost_min, node_new, X_nearby_connectable)

                if np.allclose(node_new.value, end_point, .05, 0) and (
                        final_node is
                        None):  #np.array_equal(node_new.value, end_point):
                    final_node = node_new
                    # reduce exploration bias so that we reinforce the existing path
                    exploration_bias = .5
                    if VERBOSE:
                        print("Path found!!!!")
                        print(final_node.cost)
                if rospy.get_time() - self.start_time > self.time_thresh:
                    if VERBOSE:
                        print(self.rrt_star.size)
                    break

        if final_node is not None:
            if self.enable_vis:
                marker = Marker()
                marker.header.frame_id = "/map"
                marker.type = marker.POINTS
                marker.action = marker.ADD

                marker.scale.x = 0.1
                marker.scale.y = 0.1
                marker.points = []
                marker.colors = []

            def recur(node):
                if self.enable_vis:
                    TEMP = Point()
                    TEMP.x = node.value[0]
                    TEMP.y = node.value[1]
                    TEMP.z = .05
                    marker.points.append(TEMP)

                    TEMP = ColorRGBA()
                    TEMP.r = 1
                    TEMP.g = 0
                    TEMP.b = 0
                    TEMP.a = 1

                    marker.colors.append(TEMP)

                self.trajectory.points.append([node.value[0], node.value[1]])
                parent = node.parent
                if parent is not None:
                    recur(parent)

            recur(final_node)
            self.trajectory.points.reverse()
            if self.enable_vis:
                self.vis_pub.publish(marker)
            if VERBOSE:
                print(final_node.depth)
        else:
            # TODO:give best guess- find closest node to goal
            if VERBOSE:
                print("No path found! Please try again.")