def visualize(self):
        '''
        Publish various visualization messages.
        '''
        if not self.DO_VIZ:
            return

        if self.pose_pub.get_num_connections() > 0 and isinstance(self.inferred_pose, np.ndarray):
            # Publish the inferred pose for visualization
            ps = PoseStamped()
            ps.header = Utils.make_header("map")
            ps.pose.position.x = self.inferred_pose[0]
            ps.pose.position.y = self.inferred_pose[1]
            ps.pose.orientation = Utils.angle_to_quaternion(self.inferred_pose[2])
            self.pose_pub.publish(ps)

        if self.particle_pub.get_num_connections() > 0:
            # publish a downsampled version of the particle distribution to avoid a lot of latency
            if self.MAX_PARTICLES > self.MAX_VIZ_PARTICLES:
                # randomly downsample particles
                proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES, p=self.weights)
                # proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES)
                self.publish_particles(self.particles[proposal_indices,:])
            else:
                self.publish_particles(self.particles)

        if self.pub_fake_scan.get_num_connections() > 0 and isinstance(self.ranges, np.ndarray):
            # generate the scan from the point of view of the inferred position for visualization
            self.viz_queries[:,0] = self.inferred_pose[0]
            self.viz_queries[:,1] = self.inferred_pose[1]
            self.viz_queries[:,2] = self.downsampled_angles + self.inferred_pose[2]
            self.range_method.calc_range_many(self.viz_queries, self.viz_ranges)
            self.publish_scan(self.downsampled_angles, self.viz_ranges)
Example #2
0
def make_arbiter(name, base_dir, masters, slave=None):
    """Arbiter module generation.
    
    The arbiter module is used to generate all the necessary logic glue to
    allow multi-masters to got access to a bus or a slave.
    
        @param name: arbiter module name
        @param base_dir: destination directory
        @param masters: master interfaces signals
        @param slave: slave interface signals
        @return: vhdl.Entity instance from Arbiter module.
        
        master[0] = InstanceInterface object
        master[1] = {'signal_name' : 'signal attached'}
    """
    base_name = ("arbiter_%s" % name)
    filename = path.join(base_dir, ("%s.vhd" % base_name))

    try:
        vhdl_fd = open(filename, "w")
    except IOError:
        raise ArbiterError("Can't create %s.vhd file." % base_name)

    vhdl_fd.write(make_header("Wishbone Round-Robin arbiter module", 
                                base_name))
    
    # 1. Building Entity declaration
    entity = "entity %s is\n" % base_name
    entity += "  port (\n"
    entity += "    -- Global signals\n"
    entity += "    clk : in std_logic;\n"
    entity += "    reset : in std_logic;\n\n"
    

    entity += "\n  );"
    entity += "\nend entity;\n"

    vhdl_fd.write(entity)

    # 2. Starting Architecture declaration
    vhdl_fd.write(_VHDL_ARCHITECTURE % (base_name))

    # 3. Adding local signals
    vhdl_fd.write(to_comment(['Signals declaration']))
    
    vhdl_fd.write("begin\n")
     
    
    # 7. Closing module
    vhdl_fd.write("end architecture;\n")
    vhdl_fd.close()
    
    # 8. Create vhdl.Entity object
    entity = Entity(StringIO(entity))
    instance = Instance(entity, base_name)
    
    # 9. Adding signals interconnection
    
    # 10. Returning instance
    return instance, ("%s.vhd" % base_name)
Example #3
0
 def publish_scan(self, angles, ranges):
     ls = LaserScan()
     ls.header = Utils.make_header("laser", stamp=self.last_stamp)
     ls.angle_min = np.min(angles)
     ls.angle_max = np.max(angles)
     ls.angle_increment = np.abs(angles[0] - angles[1])
     ls.range_min = 0
     ls.range_max = np.max(ranges)
     ls.ranges = ranges
     self.pub_fake_scan.publish(ls)
    def publish_tf(self,pose, stamp=None):
        """ Publish a tf for the car. This tells ROS where the car is with respect to the map. """
        if stamp == None:
            stamp = rospy.Time.now()

        # this may cause issues with the TF tree. If so, see the below code.
        self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]), 
               stamp , "/laser", "/map")

        # also publish odometry to facilitate getting the localization pose
        if self.PUBLISH_ODOM:
            odom = Odometry()
            odom.header = Utils.make_header("/map", stamp)
            odom.pose.pose.position.x = pose[0]
            odom.pose.pose.position.y = pose[1]
            odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2])
            self.odom_pub.publish(odom)
        
        return # below this line is disabled

        """
        Our particle filter provides estimates for the "laser" frame
        since that is where our laser range estimates are measure from. Thus,
        We want to publish a "map" -> "laser" transform.

        However, the car's position is measured with respect to the "base_link"
        frame (it is the root of the TF tree). Thus, we should actually define
        a "map" -> "base_link" transform as to not break the TF tree.
        """

        # Get map -> laser transform.
        map_laser_pos = np.array( (pose[0],pose[1],0) )
        map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2]) )
        # Apply laser -> base_link transform to map -> laser transform
        # This gives a map -> base_link transform
        laser_base_link_offset = (0.265, 0, 0)
        map_laser_pos -= np.dot(tf.transformations.quaternion_matrix(tf.transformations.unit_vector(map_laser_rotation))[:3,:3], laser_base_link_offset).T

        # Publish transform
        self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp , "/base_link", "/map")
Example #5
0
    def update_dynamic_map(self, pose, angles, ranges):
        coords = self.pol2cart(pose[0], pose[1], pose[2],
                             ranges, angles, self.map_info.resolution)
        if (self.clicked and time.time()-self.clicked_time > self.SETTLE_TIME):
            for coord in coords:
                if coord[0] < self.map_info.width and coord[1] < self.map_info.height:
                    if self.dynamic_map[coord[1]][coord[0]] < 10:
                        self.dynamic_map[coord[1]][coord[0]] += OCCUPANCY_INC_PER_SCAN
                    else:
                        self.dynamic_map[coord[1]][coord[0]] = 100

        self.map_msg.data = self.dynamic_map.astype(int).flatten().tolist()
        self.map_debug_pub.publish(self.map_msg)

        if self.cartesian_debug.get_num_connections() > 0:
            msg = Marker()
            msg.header = Utils.make_header("map")
            msg.id = 0
            msg.type = 8  # POINTS
            msg.action = 0  # ADD
            msg.pose.position.x = 0
            msg.pose.position.y = 0
            msg.pose.position.z = 0
            msg.pose.orientation.x = 0.0
            msg.pose.orientation.y = 0.0
            msg.pose.orientation.z = 0.0
            msg.pose.orientation.w = 0.0
            msg.scale.x = 0.1
            msg.scale.y = 0.1
            msg.scale.z = 0.1
            msg.color.a = 1.0
            msg.color.r = 1.0
            msg.color.g = 0.0
            msg.color.b = 0.0
            pts = []
            for coord in coords:
                pts.append(Point(coord[0], coord[1], 0))
            msg.points = pts
            self.cartesian_debug.publish(msg)
 def publish_particles(self, particles):
     # publish the given particles as a PoseArray object
     pa = PoseArray()
     pa.header = Utils.make_header("map")
     pa.poses = Utils.particles_to_poses(particles)
     self.particle_pub.publish(pa)
Example #7
0
def make_intercon(name, base_dir, master, slaves):
    """Intercon module generation.
    
    The intercon module is used to generate all the necessary logic glue to
    allow master interface to communicate with each slave.
    
        @param name: intercon module name
        @param base_dir: destination directory
        @param masters: master interfaces signals
        @param slaves: slave interfaces signals
        @return: vhdl.Entity instance from Intercom module.
        
        master[0] = InstanceInterface object
        master[1] = {'signal_name' : 'signal attached'}
    """
    base_name = ("intercon_%s" % name)
    filename = path.join(base_dir, ("%s.vhd" % base_name))

    # Generate master bytes selection signals list
    _, sel_width = master.wb_signal_width("SEL")
    byte_width = master.data_width // 8

    master_sel = [ mk_sel_bit(master, idx, sel_width, byte_width)
                    for idx in range(byte_width)
                 ]
    
    try:
        vhdl_fd = open(filename, "w")
    except IOError:
        raise InterconError("Can't create %s.vhd file." % base_name)

    vhdl_fd.write(make_header("Wishbone address decoder module", base_name))
    
    # 1. Building Entity declaration
    entity = "entity %s is\n" % base_name
    entity += "  port (\n"
    #entity += "    -- Global signals\n"
    #entity += "    clk : in std_logic;\n"
    #entity += "    reset : in std_logic;\n\n"
    
    entity += "    -- Master signals\n"
    
    # for signal in master.
    entity_ports =  dict(port_matrix(master, signal, "wbs_master") 
                            for _, signal in master.signals.iteritems()
                            )
    ports = ["    %s : %s" % (key, value[0]) 
                for key, value in entity_ports.iteritems()
            ]
    entity += ";\n".join(ports)

    slaves_max_width = 8
    for slave in slaves:
        entity += ";\n\n    -- Slave %s.%s signals\n" % (slave.instance_name,
                                                         slave.name)
        
        if slave.data_width > slaves_max_width:
            slaves_max_width = slave.data_width 
        # for signal out slave.

        slave_ports =  dict(port_matrix(slave, signal) 
                                for _, signal in slave.signals.iteritems()
                                )
        entity_ports.update(slave_ports)
        entity += ";\n".join(["    %s : %s" % (key, value[0]) 
                              for key, value in slave_ports.iteritems()
                              ])

    entity += "\n  );"
    entity += "\nend entity;\n"

    vhdl_fd.write(entity)

    # 2. Starting Architecture declaration
    vhdl_fd.write(_VHDL_ARCHITECTURE % (base_name))

    slaves_max_width //= 8
    slaves_max_sel = 0
    while 2**slaves_max_sel < slaves_max_width:
        slaves_max_sel += 1
    
    master_sels = [mk_sel_array(master, master_sel, 2**idx) 
                        for idx in range(slaves_max_sel+1)]
    
    if(len(slaves) > 1):
        slave_cnx = [InterconRegisterIface(slave, master, 
                                           "slave_sel(%d)"%idx,
                                           master_sels)
                        for idx, slave in enumerate(slaves)
                    ]
    else:
        slave_cnx = [InterconRegisterIface(slaves[0], master, 
                                           "slave_sel",
                                           master_sels)
                    ]

    # 3. Adding local signals
    vhdl_fd.write(to_comment(['Signals declaration']))
    if(len(slaves) > 1):
        vhdl_fd.write("  signal slave_sel : std_logic_vector(%u downto 0);\n" % 
                      (len(slaves) - 1))
    else:
        vhdl_fd.write("  signal slave_sel : std_logic;\n") 

    # 3.1 Adding bytes selection signals
    for idx, sels in enumerate(master_sels):
        (_, signals) = sels
        if len(signals) == 1:
            vhdl_fd.write("  signal master_sel%u : std_logic;\n" % (8 * (2 ** idx)))
        else:
            vhdl_fd.write("  signal master_sel%u : std_logic_vector(%d downto 0);\n" % 
                          (8 * (2 ** idx),  len(signals) - 1))
    
    vhdl_fd.write("\nbegin\n")
     
    # 4. Building address decoders
    vhdl_fd.write(to_comment(['Byte selection signals']))
    for idx, sels in enumerate(master_sels):
        def _to_signal(sig):
            if sig == None:
                return "'1'"
            else:
                return sig
            
        (_, signals) = sels
        if len(signals) == 1:
            vhdl_fd.write("  master_sel%u <= %s;\n" % 
                          (8 * (2 ** idx), _to_signal(signals[0])))
        else:
            vhdl = ["  master_sel%u(%u) <= %s;\n" % 
                          (8 * (2 ** idx), idx2, _to_signal(signals[idx2]))
                          for idx2 in range(len(signals))]
            vhdl_fd.write("".join(vhdl))

    # 4. Building address decoders
    vhdl_fd.write(to_comment(['Address decoders']))
    vhdl = [slave.addr_sel for slave in slave_cnx]
    vhdl_fd.write("  %s;\n" % ";\n  ".join(vhdl))

    vhdl = [slave.addr for slave in slave_cnx]
    vhdl_fd.write("  %s;\n" % ";\n  ".join(vhdl))
    
    # 5. Building acknowledge signal
    vhdl_fd.write(to_comment(['Control signals']))
    vhdl = [slave.ack for slave in slave_cnx]
    vhdl.append("'0'")
    vhdl_fd.write("  wbs_master_ack_o <= %s;\n" % 
                  "\n                      else ".join(vhdl))

    vhdl = ["  %s;\n" % ";\n  ".join(slave.control) for slave in slave_cnx]
    vhdl_fd.write("\n".join(vhdl))
    
    # 6. Building datapath
    vhdl_fd.write(to_comment(['Datapath wrapping']))
    vhdl = [slave.byte_sel for slave in slave_cnx if slave.byte_ok]
    if len(vhdl):
        vhdl_fd.write("  %s;\n" % ";\n  ".join(vhdl))

    vhdl = [slave.writedata for slave in slave_cnx if slave.writedata_ok]
    if len(vhdl):
        vhdl_fd.write("  %s;\n" % ";\n  ".join(vhdl))

    if master.has_signal("dat", True):
        for idx in range(byte_width):
            vhdl = [slave.readdata(idx) for slave in slave_cnx 
                                            if slave.readdata_ok(idx)
                    ]
            vhdl = [elmt for elmt in reduce(list.__add__, vhdl) if elmt != None]
            name = "wbs_master_dat_o(%d downto %d)" % ((idx*8)+7, idx*8)
            if len(vhdl) == 0:
                vhdl_fd.write("  %s <= (others => '0');\n" % name)
            else:
                vhdl.append("(others => '0')")
                sep = "\n%s else " % (" " * (len(name) + 5))
                vhdl_fd.write("  %s <= %s;\n" % 
                              (name, sep.join(vhdl)))

    # 7. Closing module
    vhdl_fd.write("\nend architecture;\n")
    vhdl_fd.close()
    
    # 8. Create vhdl.Entity object
    entity = Entity(StringIO(entity))
    instance = Instance(entity, "CP_" + base_name)
    
    # 9. Adding signals interconnection
    ports = dict((key, value[1]) 
                 for key, value in entity_ports.iteritems())

    instance.setPorts(ports)

    # 10. Returning instance
    return instance, base_name+".vhd"
    def handle_smooth_path(self, req):
        ''' 1. Save raw path to raw_path_.
            2. Smooth path by gradient descending.
            3. Remove point from smoothed path which is too close to its neighborhood points ,or its neighborhood points is too close(which means there is probably a peak in path).
            4. Publish the result path
        '''
        raw_path = req.raw_path
        tolerance = self.param_tolerance_
        max_iterations = self.param_iterations_
        alpha = self.param_alpha_
        beta = self.param_beta_
        margin = self.param_margin_
        min_point_dist = self.param_min_point_dist_

        if not isinstance(self.map_data_, OccupancyGrid):
            print 'Received smooth path request, but cannot smooth when map data not received.'
            return

        diff = tolerance + 1
        step = 0
        np_path = self.makeNpArray(raw_path)
        if not isinstance(np_path, object):
            return
        new_path = deepcopy(np_path)

        while step < max_iterations:
            if diff < tolerance:
                print 'smooth ok'
                break

            step += 1
            diff = 0.
            pre_path = deepcopy(new_path)

            i = 1
            while i != new_path.shape[0] - 2:
                new_path[i] += alpha * (pre_path[i] - new_path[i]) + beta * (
                    new_path[i - 1] + new_path[i + 1] - 2 * new_path[i])
                if self.isCollision(new_path[i], self.map_data_, margin):
                    new_path[i] = deepcopy(pre_path[i])
                    i += 1
                    continue
                if np.sum(
                    (new_path[i - 1] - new_path[i + 1])**2) < min_point_dist:
                    new_path = np.delete(new_path, i, axis=0)
                    pre_path = np.delete(pre_path, i, axis=0)
                    i -= 1
                i += 1

            diff += np.sum((new_path - pre_path)**2)

        print 'round: ', step, '; diff: ', diff, '; origin # of points: ', len(
            req.raw_path.poses
        ), '; result # of points: ', new_path.shape[
            0], '; # of deleted points: ', np_path.shape[0] - new_path.shape[0]

        smoothed_path = Path()
        smoothed_path.header = make_header(self.param_map_frame_)
        for i in xrange(new_path.shape[0]):
            pose = PoseStamped()
            pose.pose.position.x = new_path[i][0]
            pose.pose.position.y = new_path[i][1]
            pose.pose.position.z = 0
            pose.pose.orientation = angle_to_quaternion(0)
            smoothed_path.poses.append(pose)
        resp = SmoothPathResponse(smoothed_path)
        if self.param_save:
            trajectory = LineTrajectory("/bulid_trajectory")
            trajectory.fromPath(smoothed_path)
            trajectory.save(self.param_save_path)
        return resp
class Smoothing(object):
    def __init__(self):
        self.param_margin_ = rospy.get_param('~margin', default=0.3)
        self.param_alpha_ = rospy.get_param('~alpha', default=0.1)
        self.param_beta_ = rospy.get_param('~beta', default=0.3)
        self.param_iterations_ = rospy.get_param('~iterations', default=50)
        self.param_tolerance_ = rospy.get_param('~tolerance', default=0.5)
        self.param_min_point_dist_ = rospy.get_param('~min_point_dist',
                                                     default=0.5)
        self.param_min_point_dist_ = self.param_min_point_dist_**2
        self.param_map_frame_ = rospy.get_param('~map_frame', default='/map')
        self.param_should_publish_ = rospy.get_param('~should_publish',
                                                     default=True)
        self.param_save_path = os.path.join(
            rospy.get_param(
                "~save_path",
                default=
                "/home/nvidia/workspace/catkin_ws/src/ddl_0725/pf_localization/trajectories"
            ), "smoothed-" + time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj")
        self.param_save = rospy.get_param("~save", default=False)
        self.param_load_or_not = rospy.get_param('~load_or_not', default=False)

        self.map_data_ = None
        self.raw_path_ = None
        self.smoothed_path_ = None

        self.srv_smooth_path_ = rospy.Service("/smooth_path", SmoothPath,
                                              self.handle_smooth_path)
        print 'init ok.'

        # wait for map data, and save it to self.map_data_
        rospy.wait_for_service('/static_map')
        try:
            getmap_srv = rospy.ServiceProxy('/static_map', GetMap)
            resp = getmap_srv()
            self.map_data_ = resp.map
            self.param_map_frame_ = self.map_data_.header.frame_id
            print 'Map data set.'
        except rospy.ServiceException, e:
            print "Service call failed: ", e

        if self.param_load_or_not:
            # wait for raw path(which is need to be smoothed), and save it to self.raw_path_
            rospy.wait_for_service('/get_raw_path')
            try:
                get_raw_path_srv = rospy.ServiceProxy('/get_raw_path',
                                                      GetRawPath)
                resp = get_raw_path_srv()
                self.raw_path_ = resp.raw_path
                self.raw_path_.header = make_header(self.param_map_frame_)
                print 'Raw path set.'
            except rospy.ServiceException, e:
                print 'Service call failed: ', e

            if self.param_should_publish_:
                rospy.wait_for_service('/smooth_path')
                try:
                    ''' 1. Smooth path by gradient descending.
                        2. Remove point from smoothed path which is too close to its neighborhood points ,or its neighborhood points is too close(which means there is probably a peak in path).
                        3. Publish the result path
                    '''
                    smooth_path_srv = rospy.ServiceProxy(
                        '/smooth_path', SmoothPath)
                    resp = smooth_path_srv(self.raw_path_)
                    self.smoothed_path_ = resp.smoothed_path
                    self.smoothed_path_.header = make_header(
                        self.param_map_frame_)
                except rospy.ServiceException, e:
                    print 'Service call failed: ', e
                    return
                self.pub_smoothed_path_ = rospy.Publisher('/smoothed_path',
                                                          Path,
                                                          queue_size=2)
                while not rospy.is_shutdown():
                    self.pub_smoothed_path_.publish(self.smoothed_path_)
                    time.sleep(0.2)
 def publish_particles(self, particles):
     # publish the given particles as a PoseArray object
     pa = PoseArray()
     pa.header = Utils.make_header('map')
     pa.poses = Utils.particles_to_poses(particles)
     self.particle_pub.publish(pa)
Example #11
0
 def publish_particles(self, particles):
     pa = PoseArray()
     pa.header = Utils.make_header("map")
     pa.poses = Utils.particles_to_poses(particles)
     self.particle_pub.publish(pa)
Example #12
0
class VeloscanProcessor:
    def __init__(self):
        self.velodyne_sub = rospy.Subscriber("/racecar/velodyne_points", PointCloud2, self.velodyne_cb, queue_size=1)
        self.cert_pub = rospy.Publisher("/racecar/cert_points", Marker, queue_size=1)
        self.snow_pub = rospy.Publisher("/racecar/snow_points", Marker, queue_size=1)
        self.image_pub = rospy.Publisher("/output/image_raw/compressed", CompressedImage, queue_size=1)
        self.first_scan = False
        self.scan_data = None
        self.last_scan_t = None
        self.centroid = (int(.5*320), int(.5*240))
        self.image_np = None
        self.np_arr = None
        self.hsv = None
        self.mask = None

    def image_to_laser(self, centroid, laser_data):
        laser_vertical_lower = -15.0 * np.pi / 180.0
        laser_vertical_upper = -laser_vertical_lower
        laser_fov_left = -30.0 * np.pi / 180.0
        laser_fov_right = -laser_fov_left
        h = 480.0
        w = 640.0
        nearest_ring = laser_vertical_upper - 30.0*np.pi/180.0 * (centroid[1] / h) 

    def velodyne_cb(self, msg):
        rings = {x:[] for x in range(0,16)}
        for p in pc2.read_points(msg, field_names = ("x", "y", "z", "ring"), skip_nans=True):
            rings[p[3]].append(list(p[0:3]))
        pt = transform(self.centroid, (640, 480), rings)

        m = Marker()
        m.header = utils.make_header("velodyne")
        m.type = 8
        m.color.r = 1.
        m.color.g = 0.
        m.color.b = 0.
        m.color.a = 1.
        m.scale.x = .05
        m.scale.y = .05
        
        m.points = [Point(pt[0], pt[1], pt[2])]
        self.cert_pub.publish(m)

    return
        c = controller.controller(data)
        print ""
        print c.action
        r = interlock.interlock(c)
        print ""
        pts = []

        m = Marker()
        m.header = utils.make_header("velodyne")
        m.type = 8
        m.color.r = 1. if not r else 0.
        m.color.g = 0. if not r else 1.
        m.color.b = 0.
        m.color.a = 1.
        m.scale.x = .02
        m.scale.y = .02
        
        m.points = [Point(x[0], x[1], x[2]) for x in c.points]
        self.cert_pub.publish(m)

        snow_pts = []
        n = Marker()
        n.header = utils.make_header("velodyne")
        n.type = 8
        n.color.r = .75
        n.color.g = 0. 
        n.color.b = 0.75
        n.color.a = 1.
        n.scale.x = .02
        n.scale.y = .02
        
        n.points = [Point(x[0], x[1], x[2]) for x in c.snow_points]
        self.snow_pub.publish(n)
Example #13
0
def make_testbench(project):
    """Testbench module generation.
    
    The testbench module is used to generate all the necessary code to
    create a instance of System on Chip and to start simulation.
    
        @param project: System on Chip project settings 
    """
    base_name = ("%s_tb" % project.name)
    project.entity.name = base_name
    filename = path.join(project.path, ("%s.vhd" % base_name))

    try:
        vhdl_fd = open(filename, "w")
    except IOError:
        raise TestbenchError("Can't create %s.vhd file." % base_name)

    vhdl_fd.write(make_header("System on Chip testbench skeleton.", 
                                base_name))
    
    # 1. Building Entity declaration
    vhdl_fd.write("entity %s is\nend entity;\n" % base_name)

    # 2. Starting Architecture declaration
    vhdl_fd.write(_VHDL_ARCHITECTURE % (base_name))

    # 3. Adding project constants
    vhdl_fd.write(to_comment(['Project constants']))
    reset_min = 0
    const_def = []
    
    # 3.1 Define constant for each clock frequency
    for clk in project.soc.clocks.iteritems():
        const_def.append("  constant %s_PERIOD : time := 1 sec / %d;" % 
                          (clk.name.upper(), clk.frequency))
        if clk.frequency < reset_min or reset_min == 0:
            reset_min = clk.frequency
    
    # 3.2 Define reset timing
    const_def.append("  constant RESET_ON : time := 1 sec / %d;" % 
                     (reset_min * 3))
    const_def.append("  constant RESET_OFF : time := RESET_ON * 5;")
    
    # 3.3 Define constant for each slave interface base address
    for inst in project.instances.itervalues():
        for iface in inst.interfaces.itervalues():
            if iface.type == "WBS":
                const_def.append("  constant %s_%s_BASE_ADDR : integer := %d;" % 
                                 (inst.name.upper(), iface.name.upper(),
                                  iface.offset))
    
    vhdl_fd.write("\n".join(const_def))
    vhdl_fd.write("\n")
    
    # 4. Adding local signals
    vhdl_fd.write(to_comment(['Signals declaration']))
    vhdl_fd.write("  signal reset : std_logic;\n")
    vhdl_fd.write("".join(["  signal %s : %s := '0';\n" % (name, sig_type)
                           for name, sig_type in project.clock_list.iteritems()
                           ]))
    vhdl_fd.write("".join(["  signal %s : %s;\n" % (name, sig_type)
                            for name, sig_type in project.port_list.iteritems()
                            ]))

    # 4.1. Connecting signals to Top entity
    project.entity.setPorts(dict((name, name) 
                                 for name in project.port_list.iterkeys()))
    
    project.entity.setPorts(dict((name, name) 
                                 for name in project.clock_list.iterkeys()))
    project.entity.setPorts({"reset":"reset"}) 

    # 5. Adding entity declaration
    vhdl_fd.write(to_comment(['Components declaration']))
    vhdl_fd.write(project.entity.asComponent)

    vhdl_fd.write("\n\nbegin\n")
    
    # 6. Clocks and reset signals generation
    vhdl_fd.write("  reset <= '0', '1' after RESET_ON, '0' after RESET_OFF;\n")
    vhdl_fd.write("".join(["  %s <= not %s after %s_PERIOD/2;\n" % 
                           (name, name, name)
                            for name in project.clock_list.keys()
                            ]))
    
    # 7. Adding SoC Top entity
    vhdl_fd.write(str(project.entity))
    
    # 8. Closing testbench
    vhdl_fd.write("end architecture;\n")
    vhdl_fd.close()
    
    # 9. Returning file name
    return ("%s.vhd" % base_name)
    def find_best_angle(self):
        abs_real_angles = np.absolute(self.real_laser_angles)
        relevant_ranges = self.laser_ranges.copy()
        relevant_ranges[abs_real_angles > math.pi / 2] = 0.5
        abs_real_angles *= -1
        #self.laser_ranges[abs_real_angles > math.pi/2] = 0.5  # don't consider points behind car
        best_dist_index = np.lexsort((abs_real_angles, relevant_ranges))[-1]
        dist = self.laser_ranges[best_dist_index]
        angle = normalize_angle(self.real_laser_angles[best_dist_index] +
                                self.FORWARD_ANGLE)

        # check the scan to the left and scan to the right
        # bias [angle proportional to distance] toward the scan with larger dist

        if self.best_pt_valid:
            # update the best_pt info (but not the actual best_pt) since we've moved closer to it
            self.best_pt_dist = np.hypot(
                self.cur_pose.position.x - self.best_pt[0],
                self.cur_pose.position.y - self.best_pt[1])
            self.best_pt_angle = normalize_angle(math.atan2(self.best_pt[1] - self.cur_pose.position.y , \
                                            self.best_pt[0] - self.cur_pose.position.x))
            if self.best_pt_dist > 11:  # out of range
                print "condition 1"
                self.best_pt_valid = False
            else:
                lidar_min = self.cur_angle + self.laser_scan_min  # verified
                #index = int(np.hypot((self.best_pt_angle+4*math.pi)%(2*math.pi) ,(lidar_min+4*math.pi)%(2*math.pi)) / (self.STEP_SIZE * self.laser_scan_inc))
                positive_best_pt_angle = self.best_pt_angle if self.best_pt_angle > 0 else self.best_pt_angle + 2 * math.pi
                positive_lidar_min = lidar_min if lidar_min > 0 else lidar_min + 2 * math.pi
                index = int((positive_best_pt_angle - positive_lidar_min) /
                            (self.STEP_SIZE * self.laser_scan_inc))

                if index > (self.real_laser_angles_size - 1) or index < 0:
                    self.best_pt_valid = False
                    print "condition 2"
                else:
                    thresh = self.BLOCKED_TRAJ_THRESH
                    self.best_pt_valid = (abs(self.laser_ranges[index-1] - self.best_pt_dist) < thresh or \
                                    abs(self.laser_ranges[index] - self.best_pt_dist) < thresh or \
                                    abs(self.laser_ranges[index+1] - self.best_pt_dist) < thresh)
                    if not self.best_pt_valid:
                        print "condition 3"

                    #ps = PoseStamped()
                    #ps.header = Utils.make_header("map")
                    #ps.pose.position.x = self.cur_pose.position.x
                    #ps.pose.position.y = self.cur_pose.position.y
                    #ps.pose.orientation = Utils.angle_to_quaternion(self.cur_angle + self.forward_dir_delta)
                    ##ps.pose.orientation = Utils.angle_to_quaternion(self.best_pt_angle if self.best_pt_angle > 0 else self.best_pt_angle)
                    #self.forward_dir_pub.publish(ps)

        #or abs(abs(self.best_pt_angle - self.FORWARD_ANGLE) - \
        #        abs(angle - self.FORWARD_ANGLE)) > self.NEW_PT_ANGLE_THRESH \

        if (not self.best_pt_valid
                or abs(self.best_pt_dist - dist) > self.NEW_PT_DIST_THRESH):
            print "condition 3"
            self.best_pt_dist = dist
            self.best_pt_angle = angle
            self.best_pt = (self.cur_pose.position.x + dist*math.cos(angle), \
                    self.cur_pose.position.y + dist*math.sin(angle))
            self.best_pt_valid = True

        #left_scan = self.real_laser_angles[best_dist_index-1] if best_dist_index > 0 else 0
        #right_scan = self.real_laser_angles[best_dist_index+1] if best_dist_index < self.real_laser_angles_size else 0
        #best_dist_angle = best_dist_angle + (-1 if left_scan < right_scan else 1) * 0.1*((math.pi/2)/((max(min(left_scan, right_scan), 0))+1))

        if self.best_pt is not None:
            traj = Marker()
            traj.header = Utils.make_header("map")
            traj.ns = "my_namespace"
            traj.id = 0
            traj.type = 5  #LINELIST
            traj.action = 0  # ADD
            traj.pose.position.x = 0
            traj.pose.position.y = 0
            traj.pose.position.z = 0
            traj.pose.orientation.x = 0.0
            traj.pose.orientation.y = 0.0
            traj.pose.orientation.z = 0.0
            traj.pose.orientation.w = 0.0
            traj.scale.x = 0.1
            traj.scale.y = 0.1
            traj.scale.z = 0.1
            traj.color.a = 1.0
            traj.color.r = 1.0
            traj.color.g = 0.0
            traj.color.b = 0.0
            traj.points = [
                Point(self.cur_pose.position.x, self.cur_pose.position.y, 0),
                Point(self.best_pt[0], self.best_pt[1], 0)
            ]

            collide = Marker()
            collide.header = Utils.make_header("map")
            collide.ns = "my_namespace2"
            collide.id = 0
            collide.type = 8  #POINT CLOUD
            collide.action = 0  # ADD
            collide.pose.position.x = 0
            collide.pose.position.y = 0
            collide.pose.position.z = 0
            collide.pose.orientation.x = 0.0
            collide.pose.orientation.y = 0.0
            collide.pose.orientation.z = 0.0
            collide.pose.orientation.w = 0.0
            collide.scale.x = 0.2
            collide.scale.y = 0.2
            collide.scale.z = 0.2
            collide.color.a = 1.0
            collide.color.r = 1.0
            collide.color.g = 1.0
            collide.color.b = 0.0
            #points = [Point(x[0], x[1], 0) for x in self.pol2cart(self.cur_pose.position.x, self.cur_pose.position.y, self.FORWARD_ANGLE, self.laser_ranges[self.points_in_box], self.real_laser_angles[self.points_in_box])]
            #print self.points_in_box
            #print self.closest_pt_index
            if self.closest_pt_index is not None:
                points = [
                    Point(x[0], x[1], 0) for x in self.pol2cart(
                        self.cur_pose.position.x, self.cur_pose.position.y,
                        self.FORWARD_ANGLE, self.laser_ranges[
                            self.closest_pt_index], self.real_laser_angles[
                                self.closest_pt_index])
                ]
            else:
                points = []
            collide.points = points

            deflected = Marker()
            deflected.header = Utils.make_header("map")
            deflected.ns = "my_namespace3"
            deflected.id = 0
            deflected.type = 5  #LINELIST
            deflected.action = 0  # ADD
            deflected.pose.position.x = 0
            deflected.pose.position.y = 0
            deflected.pose.position.z = 0
            deflected.pose.orientation.x = 0.0
            deflected.pose.orientation.y = 0.0
            deflected.pose.orientation.z = 0.0
            deflected.pose.orientation.w = 0.0
            deflected.scale.x = 0.1
            deflected.scale.y = 0.1
            deflected.scale.z = 0.1
            deflected.color.a = 1.0
            deflected.color.r = 0.5
            deflected.color.g = 1.0
            deflected.color.b = 0.75
            if self.deflected_pt is not None:
                deflected.points = [
                    Point(self.cur_pose.position.x, self.cur_pose.position.y,
                          0),
                    Point(self.deflected_pt[0], self.deflected_pt[1], 0)
                ]
            markers = MarkerArray([collide, traj, deflected])
            self.free_space_path_pub.publish(markers)
Example #15
0
 def publish_particles(self, particles):
     pa = PoseArray()
     pa.header = Utils.make_header("map")
     pa.poses = Utils.particles_to_poses(particles)
     self.particle_pub.publish(pa)