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)
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)
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")
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)
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)
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)
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)
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)
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)
