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)