class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join( rospy.get_param("~save_path"), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") self.trajectory = LineTrajectory("/built_trajectory") # receive either goal points or clicked points self.publish_point_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_point_callback, queue_size=1) self.nav_goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.clicked_point_callback, queue_size=1) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def clicked_point_callback(self, msg): if isinstance(msg, PointStamped): self.trajectory.addPoint(msg.point) elif isinstance(msg, PoseStamped): self.trajectory.addPoint(msg.pose.position) # publish visualization of the path being built self.trajectory.publish_viz() def saveTrajectory(self): self.trajectory.save(self.save_path)
class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join( rospy.get_param("~save_path"), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") self.trajectory = LineTrajectory("/built_trajectory") ''' Insert appropriate subscribers/publishers here ''' self.point_listener = rospy.Subscriber("/clicked_point", PointStamped, self.point_cb, queue_size=1) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def point_cb(self, clicked_point): self.trajectory.addPoint(clicked_point.point) self.trajectory.publish_start_point() self.trajectory.publish_end_point() self.trajectory.publish_trajectory() def saveTrajectory(self): self.trajectory.save(self.save_path)
class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join("trajectories/", time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") #%Y-%m-%d-%H-%M-%S self.trajectory = LineTrajectory("/built_trajectory") self.data_points = [] self.count = 0 self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10) self.trajectory_points = rospy.Publisher("/traj_pts", Marker, queue_size=20) self.trajectory.publish_viz() # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def publish_trajectory(self): self.traj_pub.publish(self.trajectory.toPoseArray()) def saveTrajectory(self): self.trajectory.save(self.save_path) def clicked_pose(self, msg): self.count += 1 point = Point() point.x = msg.point.x point.y = msg.point.y self.trajectory.addPoint(point) self.data_points.append(point) self.mark_pt(self.trajectory_points, (0, 1, 0), self.data_points) if self.count > 2: rospy.loginfo("PUBLISH TRAJ") self.publish_trajectory() self.trajectory.publish_viz() def mark_pt(self, subscriber, color_tup, data): mark_pt = Marker() mark_pt.header.frame_id = "/map" mark_pt.header.stamp = rospy.Time.now() mark_pt.type = mark_pt.SPHERE_LIST mark_pt.action = mark_pt.ADD mark_pt.scale.x = .5 mark_pt.scale.y = .5 mark_pt.scale.z = .5 mark_pt.color.a = 1.0 mark_pt.color.r = color_tup[0] mark_pt.color.g = color_tup[1] mark_pt.color.b = color_tup[2] mark_pt.points = data subscriber.publish(mark_pt)
class PathBuilder(object): def __init__(self): self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry, self.odom_callback) self.traj = LineTrajectory() rospack = rospkg.RosPack() fc_path = rospack.get_path("final_challenge") self.save_path = os.path.join( fc_path + "/trajectories/", time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") rospy.on_shutdown(self.traj.save(self.save_path)) def odom_callback(self, msg): point = msg.pose.pose.position #get Point from Odometry msg self.traj.addPoint(point)
class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join( rospy.get_param( "~save_path", default= "/home/nvidia/workspace/catkin_ws/src/ddl_0725/pf_localization/trajectories" ), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") self.trajectory = LineTrajectory("/built_trajectory") self.odom_topic = rospy.get_param("~odom_topic", default="/pf/odom") self.sub_odom = rospy.Subscriber(self.odom_topic, Odometry, self.odomCB, queue_size=2) self.sub_pose = rospy.Subscriber("/current_pose", PoseStamped, self.poseCB, queue_size=1) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def odomCB(self, msg): self.trajectory.addPoint( msg.pose.pose.position.x, msg.pose.pose.position.y, quaternion_to_angle(msg.pose.pose.orientation)) self.trajectory.publish_viz() def poseCB(self, msg): self.trajectory.addPoint(msg.pose.position.x, msg.pose.position.y, quaternion_to_angle(msg.pose.orientation)) self.trajectory.publish_viz() def saveTrajectory(self): self.trajectory.save(self.save_path)
class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join( rospy.get_param( "~save_path", default= "/home/lenovo/zh/workspace/catkin_ws/src/ddl_0725/localization_0725/trajectories" ), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") self.trajectory = LineTrajectory("/built_trajectory") self.odom_topic = rospy.get_param("~odom_topic", default="/pf/odom") self.sub_odom = rospy.Subscriber(self.odom_topic, Odometry, self.odomCB, queue_size=2) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def clicked_point_callback(self, msg): if isinstance(msg, PointStamped): self.trajectory.addPoint(msg.point) elif isinstance(msg, PoseStamped): self.trajectory.addPoint(msg.pose.position) # publish visualization of the path being built self.trajectory.publish_viz() def odomCB(self, msg): self.trajectory.addPoint( msg.pose.pose.position.x, msg.pose.pose.position.y, quaternion_to_angle(msg.pose.pose.orientation)) self.trajectory.publish_viz() def saveTrajectory(self): self.trajectory.save(self.save_path)
class TableFilteringV6(): def __init__(self): # Define Adjustable Parameters # - Scan Region (Rectangular) self.x_min = float(rospy.get_param("~x_min")) # [m] self.x_max = float(rospy.get_param("~x_max")) # [m] self.y_min = float(rospy.get_param("~y_min")) # [m] self.y_max = float(rospy.get_param("~y_max")) # [m] # - Table Size self.table_width = float(rospy.get_param("~table_width")) # [m] self.table_length = float(rospy.get_param("~table_length")) # [m] self.table_diagonal = float(rospy.get_param("~table_diagonal")) # [m] self.table_tolerance = float(rospy.get_param("~table_tolerance")) # [m] # - Route Trim self.route_trim_x = rospy.get_param("~route_trim_x") # [m] self.route_trim_y = rospy.get_param("~route_trim_y") # [m] # Internal Use Variables - Do not modify without consultation self.init = False # Need to be "False" when integrated with others self.cluster_tolerance = 0.1 # [m] self.table = False self.trajectory = LineTrajectory("/table_trajectory") self.tfTL = TransformListener() self.pathpoint_1 = None self.pathpoint_2 = None self.refresh_rate = rospy.Rate(10) self.stop_counter = 0 self.centerpoint = None # Subscribers self.scan_sub = rospy.Subscriber(rospy.get_param("~scan_topic"), LaserScan, self.scanCB, queue_size=1) self.pose_sub = rospy.Subscriber(rospy.get_param("~pose_topic"), Odometry, self.poseCB, queue_size=1) self.init_table_sub = rospy.Subscriber(rospy.get_param("~init_table_topic"), Bool, self.init_tableCB, queue_size=1) # Publishers self.route_pub = rospy.Publisher(rospy.get_param("~route_topic"), PolygonStamped, queue_size=1) self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"), Twist, queue_size=1) self.finish_table_pub = rospy.Publisher(rospy.get_param("~finish_table_topic"), Bool, queue_size=1) self.viz_table_pub = rospy.Publisher(rospy.get_param("~viz_table_topic"), Marker, queue_size=1) self.viz_points_pub = rospy.Publisher(rospy.get_param("~viz_points_topic"), Marker, queue_size=1) # Publish Route for Navigating Under Table self.route_publisher() # Check when to start the Table-Filtering def init_tableCB(self, msg): self.init = msg.data if(msg.data != True): self.table = False # Receive every LaserScan and do Filtering to find out Table def scanCB(self, msg): if(self.init == True): maybe_pts = [] clusters = [] mean_pts = [] precheck_pts = [] checked_pts = [] table_pts = [] # Finding Points within selected Scan_Region (Rectangular) for i in xrange(len(msg.ranges)): pt = Point() pt.x = msg.ranges[i]*np.cos(msg.angle_min + msg.angle_increment*i) pt.y = msg.ranges[i]*np.sin(msg.angle_min + msg.angle_increment*i) pt.z = 0.0 if(self.x_min <= pt.x <= self.x_max and self.y_min <= pt.y <= self.y_max): maybe_pts.append(pt) # Filter the maybe_pts into clusters of points for j in range(0,10): cluster_counter = 0 temp_cluster = [] if(len(maybe_pts) == 0): break for i in xrange(len(maybe_pts)): if(self.dist(maybe_pts[i-1], maybe_pts[i]) <= self.cluster_tolerance): temp_cluster.append(maybe_pts[i-1]) if(self.dist(maybe_pts[i-1], maybe_pts[i]) > self.cluster_tolerance): cluster_counter = cluster_counter + 1 if(cluster_counter == 2): temp_cluster.append(maybe_pts[i-1]) break clusters.append(temp_cluster) # Save each clusters that meet the requirement for i in xrange(len(temp_cluster)): del maybe_pts[0] # Filter and Find the Mean Point of survived clusters (possible as table legs) for i in xrange(len(clusters)): if(len(clusters[i]) > 5): mean_pts.append(self.mean_point(clusters[i])) # Visualize the detected Clusters for i in xrange(len(mean_pts)): self.viz_point(i, mean_pts[i], ColorRGBA(1,0,0,1)) # Filter the Possibilities and Find out which Sets of Clusters is equivalent as Table for j in xrange(len(mean_pts)): correct_counter = 0 for i in xrange(len(mean_pts)): if(self.check_length_width_diagonal(mean_pts[j], mean_pts[i])): correct_counter = correct_counter + 1 if(correct_counter == 3): precheck_pts.append(mean_pts[j]) break checked_pts = self.remove_duplicates(precheck_pts) # Visualize the Table print len(checked_pts) if(len(checked_pts) == 4): for i in xrange(len(checked_pts)): table_pts.append(checked_pts[i]) table_pts.append(checked_pts[0]) self.viz_table(table_pts) self.table = True self.pathpoint_1 = self.middle_point(checked_pts[0], checked_pts[3]) self.pathpoint_2 = self.middle_point(checked_pts[1], checked_pts[2]) else: self.pathpoint_1 = self.pathpoint_1 self.pathpoint_2 = self.pathpoint_2 # Checking Position (Map Frame) for when to stop def poseCB(self, msg): if(self.init == True): current_pose = msg.pose.pose.position current_yaw = utils.quaternion_to_angle(msg.pose.pose.orientation) if(self.centerpoint != None): # print "current_pose: " + str(current_pose) # print "centerpoint : " + str(self.centerpoint) print self.dist(current_pose, self.centerpoint) if(self.dist(current_pose, self.centerpoint) <= 0.17): # 0.141 / 0.145 / 0.17 / 0.28 / 0.31 self.stopping() self.stop_counter = self.stop_counter + 1 if(self.stop_counter == 30): self.finish_table_pub.publish(True) self.stop_counter = 0 print "----------------------------- reporting finish --------------------------------------" # Testing USE - find hidden bug # Keep on Publishing the Latest Route for the Table-Navi def route_publisher(self): while not rospy.is_shutdown(): if(self.tfTL.frameExists("base_link")): if(self.init == True and self.pathpoint_1 != None and self.pathpoint_2 != None and self.table == True): self.trajectory.clear() # Transform the table mid-pts (base_link) into (map) traj_pt_1 = self.transforming_point(self.pathpoint_1, "/map") traj_pt_1.x += self.route_trim_x traj_pt_1.y += self.route_trim_y traj_pt_2 = self.transforming_point(self.pathpoint_2, "/map") traj_pt_2.x += self.route_trim_x traj_pt_2.y += self.route_trim_y # Line Extension(2m before the table) traj_pt_0 = Point() traj_pt_0.x = traj_pt_1.x + (2.0*(traj_pt_1.x - traj_pt_2.x)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_x traj_pt_0.y = traj_pt_1.y + (2.0*(traj_pt_1.y - traj_pt_2.y)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_y traj_pt_0.z = 0.0 # Line Extension (0.3m after the table) traj_pt_3 = Point() traj_pt_3.x = traj_pt_2.x - (0.3*(traj_pt_1.x - traj_pt_2.x)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_x traj_pt_3.y = traj_pt_2.y - (0.3*(traj_pt_1.y - traj_pt_2.y)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_y traj_pt_3.z = 0.0 self.trajectory.addPoint(traj_pt_0) self.trajectory.addPoint(traj_pt_1) self.trajectory.addPoint(traj_pt_2) self.trajectory.addPoint(traj_pt_3) self.route_pub.publish(self.trajectory.toPolygon()) self.centerpoint = self.middle_point(traj_pt_1, traj_pt_2) # Visualize the Path Points self.viz_point(100, traj_pt_0, ColorRGBA(0,0,1,1)) self.viz_point(101, traj_pt_1, ColorRGBA(0,0,1,1)) self.viz_point(102, traj_pt_2, ColorRGBA(0,0,1,1)) self.viz_point(103, traj_pt_3, ColorRGBA(0,0,1,1)) else: self.centerpoint = self.centerpoint self.refresh_rate.sleep() # Distance Calculator def dist(self, a, b): distance = np.sqrt((a.x-b.x)**2 + (a.y-b.y)**2) return distance # Mean Point Calculator for each Cluster def mean_point(self, point_set): mean_pt = Point() for i in xrange(len(point_set)): mean_pt.x = mean_pt.x + point_set[i].x # x-position mean_pt.y = mean_pt.y + point_set[i].y # y-position mean_pt.x = mean_pt.x/len(point_set) mean_pt.y = mean_pt.y/len(point_set) mean_pt.z = 0.0 return mean_pt # a & b Middle Point Calculator def middle_point(self, a, b): middle_point = Point((a.x + b.x)/2.0, (a.y + b.y)/2.0, 0.0) return middle_point # Table Length or Width Checker def check_length_width_diagonal(self, a, b): if(0.0001<abs(self.dist(a,b) - self.table_length)<=self.table_tolerance or 0.0001<abs(self.dist(a,b) - self.table_width)<=self.table_tolerance or 0.0001<abs(self.dist(a,b) - self.table_diagonal)<self.table_tolerance): return True else: return False # Duplicates Remover def remove_duplicates(self, values): output = [] seen = set() for value in values: if value not in seen: output.append(value) seen.add(value) return output # Transform Point from coordinate frame (base_link) to (map) def transforming_point(self, pre_pos, frame): pre_transform_pt = PointStamped() pre_transform_pt.header.frame_id = "base_link" pre_transform_pt.header.stamp = rospy.Time(0) pre_transform_pt.point = pre_pos transformed_pt = self.tfTL.transformPoint(frame, pre_transform_pt) after_transform_pt = Point(transformed_pt.point.x, transformed_pt.point.y, 0.0) return after_transform_pt # Use for Visualizing the Points def viz_point(self, sid, pt_position, color): point = Marker() point.header.frame_id = "base_link" point.header.stamp = rospy.Time.now() point.ns = "points" point.id = sid point.type = 1 # CUBE point.action = 0 # add/modify point.lifetime = rospy.Duration(0.1) point.scale = Vector3(0.1,0.1,0.1) point.color = color point.pose.orientation = Quaternion(0,0,0,1) point.pose.position = pt_position self.viz_points_pub.publish(point) # Use for Visualizing the Table def viz_table(self, table_points): table = Marker() table.header.frame_id = "base_link" table.header.stamp = rospy.Time.now() table.ns = "table" table.id = 0 table.type = 4 # LINE STRIP table.action = 0 # add/modify table.lifetime = rospy.Duration(0.1) table.scale = Vector3(0.05,0,0) table.color = ColorRGBA(1,1,0,1) # Yellow table.pose.orientation = Quaternion(0,0,0,1) for p in table_points: t_point = Point(p.x, p.y, 0.0) table.points.append(t_point) self.viz_table_pub.publish(table) # Stopping the vehicle def stopping(self): stop_cmd = Twist() stop_cmd.linear = Vector3(0, 0, 0) stop_cmd.angular = Vector3(0, 0, 0) self.drive_pub.publish(stop_cmd)
class PathPlan(object): """ Listens for goal pose published by RViz and uses it to plan a path from current car pose. """ def __init__(self): self.current_pose = None self.goal_pose = None # The map data, in row-major order, starting with (0,0). Occupancy # probabilities are in the range [0,100]. Unknown is -1. self.map_occupancy_prob = None self.obstacles = None # The origin of the map [m, m, rad]. This is the real-world pose of the # cell (0,0) in the map. self.map_origin_pose = None # Map width, height [cells] self.map_dimensions = (0, 0) # The map resolution [m/cell] self.map_res = None # MAKE SURE YOU UNCOMMENT THIS AND SET THE ODOM TOPIC RIGHT # self.odom_topic = rospy.get_param("/particle_filter/odom_topic", "/odom") self.trajectory = LineTrajectory("/planned_trajectory") self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb) self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10) self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=10) self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry, self.odom_cb) def map_cb(self, msg): self.map_origin_pose = msg.info.origin _, _, self.theta = tf.transformations.euler_from_quaternion( (self.map_origin_pose.orientation.x, self.map_origin_pose.orientation.y, self.map_origin_pose.orientation.z, self.map_origin_pose.orientation.w)) self.map_dimensions = (msg.info.width, msg.info.height) map = np.reshape(msg.data, (msg.info.height, msg.info.width)) self.map_occupancy_prob = dilation(map, square(4)) print(self.map_occupancy_prob[339][359]) self.map_res = msg.info.resolution obstacles1 = np.where(self.map_occupancy_prob == 100) obstacles2 = np.where(self.map_occupancy_prob == -1) self.obstacles = set(zip(obstacles1[1], obstacles1[0])).union( set(zip(obstacles2[1], obstacles2[0]))) def odom_cb(self, msg): if self.current_pose is None: print("intialize current pose") #Has .position (Point) and .orientation (Quaternion) attributes self.current_pose = msg.pose.pose pixel_coord = self.map_to_pixel( (self.current_pose.position.x, self.current_pose.position.y)) self.current_pose.position.x = pixel_coord[0] self.current_pose.position.y = pixel_coord[1] def goal_cb(self, msg): print("intialize goal pose") #Has .position (Point) and .orientation (Quaternion) attributes self.goal_pose = msg.pose pixel_coord = self.map_to_pixel( (self.goal_pose.position.x, self.goal_pose.position.y)) self.goal_pose.position.x = pixel_coord[0] self.goal_pose.position.y = pixel_coord[1] while self.current_pose is None or self.goal_pose is None or self.obstacles is None: print('waiting for all the required info') self.plan_path( (self.current_pose.position.x, self.current_pose.position.y), (self.goal_pose.position.x, self.goal_pose.position.y)) # def trace_path(self, node, path = []): # point = Point() # map_coord = self.pixel_to_map((node[0][0], node[0][1])) # point.x = map_coord[0] # point.y = map_coord[1] # path.append(point) # if node[1] != None: # return self.trace_path(node[1], path) # else: # path.reverse() # return path def trace_path(self, start, end, parent): path = [] point, node = Point(), end map_coord = self.pixel_to_map((node[0], node[1])) point.x = map_coord[0] point.y = map_coord[1] path.append(point) while parent[node] != node: node = parent[node] point = Point() map_coord = self.pixel_to_map((node[0], node[1])) point.x = map_coord[0] point.y = map_coord[1] path.append(point) return path[-1::-1] def astar(self, start_position, end_position, obstacles): queue = [] heapq.heappush(queue, (0, start_position)) visited = set() parent = {start_position: start_position} # for path reconstruction mins = {start_position: 0} while queue: _, node = heapq.heappop(queue) if node in visited: continue if node == end_position: return self.trace_path(start_position, end_position, parent) visited.add(node) for neighbor in self.adj_nodes(node): if neighbor in visited or neighbor in obstacles: continue new_cost = mins[node] + self.euclidian_cost(node, neighbor) if neighbor not in mins or mins[neighbor] > new_cost: mins[neighbor] = new_cost priority = new_cost + self.euclidian_cost( neighbor, end_position) heapq.heappush(queue, (priority, neighbor)) parent[neighbor] = node return [] def dijkstra(self, start_position, end_position, obstacles): queue = [] heapq.heappush(queue, (0, start_position)) visited = set() parent = {start_position: start_position} # for path reconstruction mins = {start_position: 0} while queue: _, node = heapq.heappop(queue) if node in visited: continue if node == end_position: return self.trace_path(start_position, end_position, parent) visited.add(node) for neighbor in self.adj_nodes(node): if neighbor in visited or neighbor in obstacles: continue new_cost = mins[node] + self.euclidian_cost(node, neighbor) if neighbor not in mins or mins[neighbor] > new_cost: mins[neighbor] = new_cost heapq.heappush(queue, (new_cost, neighbor)) parent[neighbor] = node return [] # def astar(self, start_position, end_position, obstacles): # print("starting astar") # # queue = [] # heapq.heappush(queue, (0, 0, (start_position, None))) # visited = set() # while queue: # _, cost, path = heapq.heappop(queue) # current = path[0] # if current in visited: # continue # if current == end_position: # return self.trace_path(path, []) # visited.add(current) # # for neighbor in self.adj_nodes(current): # if neighbor in visited or neighbor in obstacles: # continue # # new_cost = cost + self.euclidian_cost(current, neighbor) # priority = new_cost + self.euclidian_cost(neighbor, end_position) # heapq.heappush(queue, (priority, new_cost, (neighbor, path))) # return [] def plan_path(self, start_position, end_position): print('planning path') if end_position in self.obstacles: print("End goal is occupied") return path = self.dijkstra(start_position, end_position, self.obstacles) if not path: print("trajectory not found") self.trajectory.clear() for point in path: self.trajectory.addPoint(point) self.traj_pub.publish(self.trajectory.toPoseArray()) # visualize trajectory Markers self.trajectory.publish_viz() def euclidian_cost(self, position1, position2): #Idk how the dubin curve thing right now so here's a 2D Euclidan heuristic lol return ((position2[0] - position1[0]))**2 + ( (position2[1] - position1[1]))**2 # def dubin_cost(self, pose1, pose2): # x0 = pose1.position.x # y0 = pose1.position.y # _, _, theta0 = tf.transformations.euler_from_quaternion((pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w)) # # x1 = pose2.position.x # y1 = pose2.position.y # _, _, theta1 = tf.transformations.euler_from_quaternion((pose2.orientation.x, pose2.orientation.y, pose2.orientation.z, pose2.orientation.w)) # q0 = (x0, y0, theta0) # q1 = (x1, y1, theta1) # turning_radius = 0.5 # step_size = 0.1 # path = dubins.shortest_path(q0, q1, turning_radius) # configurations, _ = path.sample_many(step_size) def adj_nodes(self, point): #return nodes that are adjacent to point adj_nodes = [] for i in range(-1, 2): for j in range(-1, 2): if 0 <= point[0] + i < self.map_dimensions[ 0] and 0 <= point[1] + j < self.map_dimensions[1]: adj_nodes.append((point[0] + i, point[1] + j)) return adj_nodes def map_to_pixel(self, c): #converts a pose to a pixel shift = np.array([[self.map_origin_pose.position.x], [self.map_origin_pose.position.y]]) c_np = np.array([[c[0]], [c[1]]]) res = self.map_res rotation = np.array([[math.cos(self.theta), math.sin(self.theta)], [-1 * math.sin(self.theta), math.cos(self.theta)]]) p = (np.linalg.inv(rotation).dot((c_np - shift))) / res p = p.flatten() return round(p[0]), round(p[1]) def pixel_to_map(self, c): #converts a pose to a pixel shift = np.array([[self.map_origin_pose.position.x], [self.map_origin_pose.position.y]]) c_np = np.array([[c[0]], [c[1]]]) res = self.map_res rotation = np.array([[math.cos(self.theta), math.sin(self.theta)], [-1 * math.sin(self.theta), math.cos(self.theta)]]) m = res * rotation.dot(c_np) + shift m = m.flatten() return m[0], m[1]