class TrajectoryCaller(object): """ Call a pre-defined trajectory from the file system and publishes it to a ROS topic. """ def __init__(self): # Define Topics for Publishing and Subscribing Messages self.sub_topic = rospy.get_param("~sub_topic", "trajectory/new") self.pub_topic = rospy.get_param("~pub_topic", "trajectory/current") # Internal Use Variables - Do not modify without consultation self.counts = 0 # Initialize and Load the trajectory self.trajectory = LineTrajectory("/called_trajectory") # Subscriber self.traj_sub = rospy.Subscriber(self.sub_topic, String, self.trajCB, queue_size=1) # Publisher self.traj_pub = rospy.Publisher(self.pub_topic, PolygonStamped, queue_size=1) # Callback Function for receiving path def trajCB(self, msg): path_id = msg.data if self.counts == 1: self.publish_trajectory(path_id) self.counts = 0 self.counts = self.counts + 1 # Republish the trajectory after being called def publish_trajectory(self, path_name): # clear the trajectory whenever going to load new path self.trajectory.clear() # load a new received path self.trajectory.load(path_name) print "Publishing trajectory to:", self.pub_topic self.traj_pub.publish(self.trajectory.toPolygon())
class AutoCharging(): def __init__(self): # Define Topics for publishing or subscribing messages self.init_charging_topic = rospy.get_param("~init_charging_topic") self.battery_topic = rospy.get_param("~battery_topic") self.pose_topic = rospy.get_param("~pose_topic") self.drive_topic = rospy.get_param("~drive_topic") self.mag_drive_topic = rospy.get_param("~mag_drive_topic") self.contactor_topic = rospy.get_param("~contactor_topic") self.stop_charging_topic = rospy.get_param("~stop_charging_topic") self.finish_charging_topic = rospy.get_param("~finish_charging_topic") self.suspend_topic = rospy.get_param("~suspend_topic") self.notify_topic = rospy.get_param("~notify_topic") self.magnetic_topic = rospy.get_param("~magnetic_topic") # Define Adjustable Parameters self.stop_pt = Point(float(rospy.get_param("~stop_pt_x")), float(rospy.get_param("~stop_pt_y")), 0) self.check_pt = Point(float(rospy.get_param("~check_pt_x")), float(rospy.get_param("~check_pt_y")), 0) self.finish_pt = Point(float(rospy.get_param("~finish_pt_x")), float(rospy.get_param("~finish_pt_y")), 0) self.heading_min = float(rospy.get_param("~heading_min")) self.heading_max = float(rospy.get_param("~heading_max")) self.charging_route = rospy.get_param("~charging_route") self.charge_stop_voltage = int(rospy.get_param("~charge_stop_voltage")) self.charge_stop_offset = int(rospy.get_param("~charge_stop_offset")) # Internal Use Variables - Do not modify without consultation self.trajectory = LineTrajectory("/charging_trajectory") self.init = False self.step_counter = 0 self.first_time = True self.contactor_counter = 0 self.finish_charging_counter = 0 self.mag_data = [] self.mag_end = False self.cnt = 0 self.voltage_percent = 0 self.spike_cnt = 0 self.charge_OK = False # Subscribers self.init_charging_sub = rospy.Subscriber(self.init_charging_topic, Bool, self.init_chargingCB, queue_size=1) self.battery_sub = rospy.Subscriber(self.battery_topic, String, self.batteryCB, queue_size=1) self.pose_sub = rospy.Subscriber(self.pose_topic, Odometry, self.poseCB, queue_size=1) self.mag_sub = rospy.Subscriber(self.magnetic_topic, String, self.magCB, queue_size=1) # Publishers self.drive_pub = rospy.Publisher(self.drive_topic, Twist, queue_size=1) self.mag_drive_pub = rospy.Publisher(self.mag_drive_topic, Twist, queue_size=1) self.contactor_pub = rospy.Publisher(self.contactor_topic, Bool, queue_size=1) self.stop_charging_pub = rospy.Publisher(self.stop_charging_topic, Bool, queue_size=1) self.finish_charging_pub = rospy.Publisher(self.finish_charging_topic, Bool, queue_size=1) self.traj_pub = rospy.Publisher("/charging_navi/trajectory", PolygonStamped, queue_size=1) self.suspend_pub = rospy.Publisher(self.suspend_topic, Bool, queue_size=1) self.notify_pub = rospy.Publisher(self.notify_topic, String, queue_size=1) def init_chargingCB(self, msg): self.init = msg.data def batteryCB(self, msg): self.voltage_percent = float(msg.data) def poseCB(self, msg): print "step_counter : " + str(self.step_counter) + ", Batt(%):" + str( self.voltage_percent) now_position = msg.pose.pose.position now_heading = self.quaternion_to_angle(msg.pose.pose.orientation) # Initiate self charging procedure if (self.init == True and self.first_time == True): self.step_counter = 1 self.trajectory.clear() print "Step 0 - Load Trajectory" self.trajectory.load(self.charging_route) time.sleep(0.5) self.traj_pub.publish(self.trajectory.toPolygon()) self.first_time = False self.stop_charging_pub.publish(False) # Step 1 - When arriving check point, disable the charging navi and start to tune heading if (self.step_counter == 1 and self.dist_checker( now_position, self.check_pt, 0.25) == True): print "Step 1 - Tune heading" print np.rad2deg(now_heading) self.init = False if (now_heading > np.deg2rad(self.heading_max) or now_heading < np.deg2rad(self.heading_min)): self.step_counter = 2 else: self.tuning_heading(0.3) self.stop_charging_pub.publish(True) self.finish_charging_pub.publish(False) # Step 2 - Continue enable Charging Navi after the checking point elif (self.step_counter == 2 and self.dist_checker(now_position, self.stop_pt, 0.1) == False): print "Step 2 - Continue enable Charging Navi" self.stop_charging_pub.publish(False) self.step_counter = 3 #------------------Stat # Step 3 - Stop elif (self.step_counter == 3 and self.dist_checker(now_position, self.stop_pt, 0.1) == True): print "Step 3 - Stop Vehicle at Charging Point" self.stopping() self.stop_charging_pub.publish(True) self.step_counter = 4 # Step 4 - Python Magnetic elif (self.step_counter == 4): if (self.mag_end == True): print "Magnetic Drive... Charging" self.contactor_pub.publish(True) self.stopping() self.stop_charging_pub.publish(True) if (self.contactor_counter >= 60): self.step_counter = 5 self.contactor_counter = 0 self.mag_end = False self.contactor_counter = self.contactor_counter + 1 else: self.notify_pub.publish("1") self.mag_drive() print "Magnetic Drive... Moving To Charger" #Step 5 - Turn Off the Contactor after finish charging elif (self.step_counter == 5 and self.charge_OK == True): print "Step 5 - Turn OFF Contactor-----" + str( self.finish_charging_counter) if (self.finish_charging_counter >= 30): self.contactor_pub.publish(False) self.charge_OK = False self.step_counter = 6 self.finish_charging_counter = 0 self.finish_charging_counter = self.finish_charging_counter + 1 #------------------END elif (self.step_counter == 6): print "Step 6 - Navi to Finish Location" self.stop_charging_pub.publish(False) self.step_counter = 7 # Step 7 - Report finish self_charging procedure, Reset counter elif (self.step_counter == 7 and self.dist_checker( now_position, self.finish_pt, 1.0) == True): print "Step 7 - Finish self_charging" self.finish_charging_pub.publish(True) self.step_counter = 0 self.first_time = True self.contactor_counter = 0 self.finish_charging_counter = 0 #measure voltage 10 times before confirming it full prevent false reading from spike if (self.voltage_percent >= (self.charge_stop_voltage + self.charge_stop_offset)): if (self.spike_cnt > 100): self.charge_OK = True self.spike_cnt = 0 self.spike_cnt = self.spike_cnt + 1 print "Done Charged Count:" + str(self.spike_cnt) else: print "Done Charge:" + str(self.charge_OK) def magCB(self, msg): raw = map(int, msg.data.split("/")) self.mag_data = raw def mag_drive(self): x = 0 z = 0 mg = self.mag_data j = [i for i in mg if i > 150] self.cnt = self.cnt + 1 if (len(j) > 8): #stop self.notify_pub.publish("0,0") #Boot = 0, Low_level_Drive = 0 x = 0 z = 0 self.mag_end = True print str(len(j)) + "----STOP, " + str(self.mag_end) else: self.notify_pub.publish("0,1") #Boot = 0, Low_level_Drive = 1 # line_trace speed set x = 0.18 drive_msg = Twist() drive_msg.linear = Vector3(x, 0, 0) drive_msg.angular = Vector3(0, 0, z) self.mag_drive_pub.publish(drive_msg) # Distance Checker, Checking whether 2 points are within distance tolerance or not def dist_checker(self, a, b, dist_tolerance): distance = np.sqrt((a.x - b.x)**2 + (a.y - b.y)**2) if (distance <= dist_tolerance): return True else: return False # Tuning the Heading of Vehicle def tuning_heading(self, rotspeed): drive_msg = Twist() drive_msg.linear = Vector3(0, 0, 0) drive_msg.angular = Vector3(0, 0, rotspeed) self.drive_pub.publish(drive_msg) # Stopping Vehicle def stopping(self): drive_msg = Twist() drive_msg.linear = Vector3(0, 0, 0) drive_msg.angular = Vector3(0, 0, 0) self.drive_pub.publish(drive_msg) # Convert Quaternion to Angle def quaternion_to_angle(self, q): x, y, z, w = q.x, q.y, q.z, q.w roll, pitch, yaw = tf.transformations.euler_from_quaternion( (x, y, z, w)) return yaw
class PsaAutoChargingV2(): def __init__(self): # Define Adjustable Parameters self.stop_pt = Point(float(rospy.get_param("~stop_pt_x")), float(rospy.get_param("~stop_pt_y")), 0) self.check_pt = Point(float(rospy.get_param("~check_pt_x")), float(rospy.get_param("~check_pt_y")), 0) self.finish_pt = Point(float(rospy.get_param("~finish_pt_x")), float(rospy.get_param("~finish_pt_y")), 0) self.heading_min = float(rospy.get_param("~heading_min")) self.heading_max = float(rospy.get_param("~heading_max")) self.charging_route = rospy.get_param("~charging_route") self.charge_stop_voltage = int(rospy.get_param("~charge_stop_voltage")) self.charge_stop_offset = int(rospy.get_param("~charge_stop_offset")) # Internal Use Variables - Do not modify without consultation self.trajectory = LineTrajectory("/charging_trajectory") self.init = False self.step_counter = 0 self.first_time = True self.contactor_counter = 0 self.finish_charging_counter = 0 self.mag_data = [] self.mag_end = False self.cnt = 0 self.voltage_percent = 0 self.spike_cnt = 0 self.charge_OK = False self.iters = 0 # Subscribers self.init_charging_sub = rospy.Subscriber( rospy.get_param("~init_charging_topic"), Bool, self.init_chargingCB, queue_size=1) self.battery_sub = rospy.Subscriber(rospy.get_param("~battery_topic"), String, self.batteryCB, queue_size=1) self.pose_sub = rospy.Subscriber(rospy.get_param("~pose_topic"), Odometry, self.poseCB, queue_size=1) self.amcl_sub = rospy.Subscriber(rospy.get_param("~amcl_topic"), PoseWithCovarianceStamped, self.poseCB, queue_size=1) self.magnetic_sub = rospy.Subscriber( rospy.get_param("~magnetic_topic"), String, self.magCB, queue_size=1) # Publishers self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"), Twist, queue_size=1) self.mag_drive_pub = rospy.Publisher( rospy.get_param("~mag_drive_topic"), Twist, queue_size=1) self.contactor_pub = rospy.Publisher( rospy.get_param("~contactor_topic"), Bool, queue_size=1) self.stop_charging_pub = rospy.Publisher( rospy.get_param("~stop_charging_topic"), Bool, queue_size=1) self.finish_charging_pub = rospy.Publisher( rospy.get_param("~finish_charging_topic"), Bool, queue_size=1) self.traj_pub = rospy.Publisher("/charging_navi/trajectory", PolygonStamped, queue_size=1) self.suspend_pub = rospy.Publisher(rospy.get_param("~suspend_topic"), Bool, queue_size=1) self.notify_pub = rospy.Publisher(rospy.get_param("~notify_topic"), String, queue_size=1) self.init_turn_pub = rospy.Publisher("/turning/init", String, queue_size=1) def init_chargingCB(self, msg): self.init = msg.data def batteryCB(self, msg): self.voltage_percent = float(msg.data) def poseCB(self, msg): print "step_counter : " + str(self.step_counter) + ", Batt(%):" + str( self.voltage_percent) if isinstance(msg, Odometry): now_position = msg.pose.pose.position now_heading = self.quaternion_to_yaw(msg.pose.pose.orientation) elif isinstance(msg, PoseWithCovarianceStamped): now_position = msg.pose.pose.position now_heading = self.quaternion_to_yaw(msg.pose.pose.orientation) # Initiate self charging procedure if (self.init == True and self.first_time == True): self.step_counter = 1 self.trajectory.clear() print "Step 0 - Load Trajectory" self.trajectory.load(self.charging_route) time.sleep(0.5) self.traj_pub.publish(self.trajectory.toPolygon()) self.first_time = False self.stop_charging_pub.publish(False) # Step 1 - When arriving check point, disable the charging navi and start to tune heading if (self.step_counter == 1 and self.dist_checker( now_position, self.check_pt, 0.25) == True): print "Step 1 - Tune heading" print "current_heading : " + str(now_heading) self.init = False if (self.heading_min < now_heading < self.heading_max): self.step_counter = 2 else: self.init_turn_pub.publish("90") self.stop_charging_pub.publish(True) self.finish_charging_pub.publish(False) # Step 2 - Continue enable Charging Navi after the checking point elif (self.step_counter == 2 and self.dist_checker(now_position, self.stop_pt, 0.1) == False): print "Step 2 - Continue enable Charging Navi" self.stop_charging_pub.publish(False) self.step_counter = 3 # Step 3 - Pass over to Magnetic Following Mode elif (self.step_counter == 3 and self.dist_checker(now_position, self.stop_pt, 0.1) == True): print "Step 3 - Ready to Swap into Magnetic Following" self.stopping() self.stop_charging_pub.publish(True) self.step_counter = 4 # Step 4 - Magnetic Following and ON Charging after arriving the end of magnetic line elif (self.step_counter == 4): if (self.mag_end == True): print "Turn ON - Charging" if (self.contactor_counter % 50 == 0): self.contactor_pub.publish(True) self.stopping() self.stop_charging_pub.publish(True) if (self.contactor_counter >= 60 and self.charge_OK == True): self.step_counter = 5 self.contactor_counter = 0 self.mag_end = False self.contactor_counter += 1 else: self.mag_drive() print "Step 4 - Magnetic Drive... Moving To Charger" # Step 5 - Turn Off the Contactor after finish charging elif (self.step_counter == 5 and self.charge_OK == True): print "Step 5 - Turn OFF Contactor-----" + str( self.finish_charging_counter) self.contactor_pub.publish(False) if (self.finish_charging_counter >= 50): self.charge_OK = False self.step_counter = 6 self.finish_charging_counter = -1 self.finish_charging_counter += 1 # Step 6 - Revert back to LIDAR Navigation Mode elif (self.step_counter == 6): print "Step 6 - LIDAR Navi to Finished Location" self.stop_charging_pub.publish(False) self.step_counter = 7 # Step 7 - Report finish self_charging procedure, Reset counter elif (self.step_counter == 7 and self.dist_checker( now_position, self.finish_pt, 1.0) == True): print "Step 7 - Finish self_charging" self.finish_charging_pub.publish(True) self.step_counter = 0 self.first_time = True self.contactor_counter = 0 self.finish_charging_counter = 0 # Measure voltage 100 times before confirming it full prevent false reading from spike if (self.voltage_percent >= (self.charge_stop_voltage + self.charge_stop_offset)): if (self.spike_cnt > 100): self.charge_OK = True self.spike_cnt = 0 self.spike_cnt += 1 print "Done Charged Count:" + str(self.spike_cnt) else: print "Done Charge:" + str(self.charge_OK) def magCB(self, msg): raw = map(int, msg.data.split("/")) self.mag_data = raw def mag_drive(self): x = 0 z = 0 mg = self.mag_data j = [i for i in mg if i > 130] #threshold between magnetic tape or not self.cnt = self.cnt + 1 if (len(j) > 8): #if magnetic detected more than 8 line #stop self.notify_pub.publish("0,0") #Boot = 0, Low_level_Drive = 0 x = 0 self.mag_end = True print str(len(j)) + "----STOP, " + str(self.mag_end) else: self.notify_pub.publish("0,1") #Boot = 0, Low_level_Drive = 1 # line_trace speed set x = 0.2 print str(len(j)) + "----MOVING, " + str(self.mag_end) drive_msg = Twist() drive_msg.linear = Vector3(x, 0, 0) drive_msg.angular = Vector3(0, 0, 0) self.mag_drive_pub.publish(drive_msg) # Distance Checker, Checking whether 2 points are within distance tolerance or not def dist_checker(self, a, b, dist_tolerance): distance = np.sqrt((a.x - b.x)**2 + (a.y - b.y)**2) if (distance <= dist_tolerance): return True else: return False # Stopping Vehicle def stopping(self): drive_msg = Twist() drive_msg.linear = Vector3(0, 0, 0) drive_msg.angular = Vector3(0, 0, 0) self.drive_pub.publish(drive_msg) # Convert Quaternion to Yaw Angle [deg] def quaternion_to_yaw(self, q): x, y, z, w = q.x, q.y, q.z, q.w roll, pitch, yaw = np.rad2deg(tftr.euler_from_quaternion((x, y, z, w))) return yaw
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.odom_topic = rospy.get_param("~odom_topic") self.map_sub = rospy.Subscriber("/obstacles", OccupancyGrid, self.obstacle_map_cb) self.trajectory = LineTrajectory("/final_trajectory") self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_cb, queue_size=10) self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/final", PoseArray, queue_size=10) self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry, self.odom_cb, queue_size=10) self.rest_of_trajectory = LineTrajectory("/rest_of_trajectory") self.planning_dist = 50.0 # FOR TESTING # self.vis_pub = rospy.Publisher("/particles", Marker, queue_size=1) self.nearest_pub = rospy.Publisher("/nearest", Marker, queue_size=1) self.start_pose = None self.goal_pose = None #self.max_iterations = 1000 # Max size of RRT* tree before breaking. self.dim = 2 # Configuration space dimensionality ([x, y] -> 2, [x, y, theta] -> 3) self.gamma = 10 # I don't actually know what this is called lol, it's used in rrt_star.radius() self.eta = .75 self.goal_bias = 0.15 self.map = None # FOR TESTING PURPOSES ONLY -- set to true if you want to see the tree grow self.enable_vis = True def obstacle_map_cb(self, msg): """ Convert from OccupancyGrid to actual array. """ self.map = Map(msg, dilate=5) if True: rospy.loginfo("Obstacle map received! Origin: " + str(msg.info.origin)) self.pick_intermediate_goal(self.map) if True: rospy.loginfo("start pose: " + str(self.start_pose)) rospy.loginfo("end pose: " + str(self.end_pose)) self.start_time = rospy.get_time() self.time_thresh = 0.5 self.plan_path(self.start_pose, self.end_pose, self.map) self.extend_trajectory(self.rest_of_trajectory) # publish trajectory self.traj_pub.publish(self.trajectory.toPoseArray()) # visualize trajectory Markers self.trajectory.publish_viz() def extend_trajectory(self): """ adds rest of trajectory after intermediate points to the planned obstacle avoiding trajectory """ traj_poses = self.trajectory.toPoseArray() rest_poses = self.rest_of_trajectory.toPoseArray() self.trajectory.fromPoseArray(traj_poses.poses + rest_poses.poses) def pick_intermediate_goal(self): """ precondition: self.trajectory is populated w/new obstacle-agnostic trajectory takes in current planned path, and obstacle map (self.map) finds first segment within planning_dist that is blocked by obstacle + replans to endpoint of segment return false if we can just use current traj otherwise returns true&saves goal to self.end_pose, save rest of previous path to something else """ first_i, _ = self.get_closest_seg(self.start_pose) if first_i + 1 >= self.num_pts: raise Exception("something bad happened") if self.map.generate_line_path_unlimited( self.start_pose, self.trajectory.np_points[first_i + 1]): safe_pt = first_i + 1 while self.trajectory.distance_along_trajectory( safe_pt) - self.trajectory.distance_along_trajectory( first_i) < self.planning_dist: if safe_pt + 1 >= self.num_pts: return False if not self.map.generate_line_path_unlimited( self.trajectory.np_points[safe_pt], self.trajectory.np_points[safe_pt + 1]): self.end_pose = self.trajectory.np_points(safe_pt + 1) self.rest_of_trajectory.clear() poseArr = self.trajectory.toPoseArray() self.rest_of_trajectory.fromPoseArray( poseArr.poses[safe_pt + 2:]) return True safe_pt += 1 else: self.end_pose = self.trajectory.np_points(first_i + 1) self.rest_of_trajectory.clear() poseArr = self.trajectory.toPoseArray() self.rest_of_trajectory.fromPoseArray(poseArr.poses[first_i + 2:]) return True def trajectory_cb(self, msg): if self.trajectory.empty(): self.trajectory.fromPoseArray(msg) self.trajectory.make_np_array() #TODO: extend last pt?? self.num_pts = len(self.trajectory.np_points) def next_pt(i): return (i + 1) % self.num_pts # n entries, one for each line segment + connect the last to the first point self.trajectory_dists = np.array([ np.linalg.norm(self.trajectory.np_points[next_pt(i)] - self.trajectory.np_points[i])**2 for i in range(self.num_pts) ]) # also vectors bw points can be calculated once only self.wv = np.array([ self.trajectory.np_points[next_pt(i)] - self.trajectory.np_points[i] for i in range(self.num_pts) ]) def get_closest_seg(self, pose_np): """ given 1x2 numpy pose array, find closest segment from nx2 trajectory.np_points return 2 points from line segment (?) may need to be optimized let the point the robot is at be p, and for each line segment the start and end points be v and w respect~ """ pv = pose_np - self.trajectory.np_points[:last_pt] # self.num_pts x 2 # projecting p onto line bw vw, constraining b/w 0 and 1 to keep within line segment t = np.maximum( 0, np.minimum(1, (pv * self.wv[:last_pt]).sum(axis=-1) / self.trajectory_dists[:last_pt])) proj = self.trajectory.np_points[: last_pt] + self.wv[: last_pt] * t[:, np. newaxis] # return index of minimum distance, and closest pt minseg = np.argmin(np.linalg.norm(pose_np - proj, axis=1)) return minseg, proj[minseg] def odom_cb(self, msg): """ """ self.start_pose = np.array( [msg.pose.pose.position.x, msg.pose.pose.position.y]) def goal_cb(self, msg): self.end_pose = np.array([msg.pose.position.x, msg.pose.position.y]) if VERBOSE: rospy.loginfo("start pose: " + str(self.start_pose)) rospy.loginfo("end pose: " + str(self.end_pose)) self.start_time = rospy.get_time() self.time_thresh = 117. self.plan_path(self.start_pose, self.end_pose, self.map) def branched_from_existing_path(self, final_node, depth_underestimate=0): ''' depth_underestimate: bias to add to depth. Because we don't recursively update it, depth will steadily get less and less accurate as the path is denser than expected due to rewirings. Return a random free space that was near the existing path up to the final_node ''' def find_ancestor(node, n): if n <= 0 or node.parent is None: return node else: return find_ancestor(node.parent, n - 1) # back_track_depth = np.random.choice(int(final_node.depth + depth_underestimate)) back_track_depth = np.random.choice(int(final_node.depth)) x_temp = find_ancestor(final_node, back_track_depth) if x_temp.parent is None: # No point in exploring directly from the origin return self.map.sample_free_space() extend = 1 frac = .1 * extend while extend > 0: rand = np.random.uniform() * 2 * np.pi free_space_near_path = x_temp.value + extend * np.array( [np.cos(rand), np.sin(rand)]) if self.map.check_free(free_space_near_path): return free_space_near_path else: extend -= frac # unlucky -- just give a random free point return self.map.sample_free_space() def find_nearby_connectable(self, x_nearest, x_new): radius = self.rrt_star.radius(self.gamma, self.dim) X_nearby = self.rrt_star.nearest_in_n_sphere( x_new, 1) #min(radius, self.eta)) # Find all nodes near x_new # Find all X_near nodes that can actually be connected to x_new X_nearby_connectable = [x_nearest ] # List of candidate parent nodes for x_new for node in X_nearby: TEMP_PATH = self.map.generate_line_path_unlimited( node.value, x_new) if (TEMP_PATH is not None) and ( node != x_nearest ): # If the path exists between node and x_new, append node to list of candidate parents. (x_nearest starts in the list, so it's gonna be excluded here.) X_nearby_connectable.append(node) return X_nearby_connectable def find_best_parent(self, X_nearby_connectable, x_new): cost_min = np.inf node_min = None # Find which candidate parent node will impart the minimum cost to the x_new node when connected for node in X_nearby_connectable: TEMP_COST = node.cost + self.rrt_star.cost(node.value, x_new) if TEMP_COST < cost_min: cost_min = TEMP_COST node_min = node return cost_min, node_min def rewire(self, cost_min, node_new, X_nearby_connectable): # Rewire to minimize costs for node in X_nearby_connectable: potential_cost = cost_min + self.rrt_star.cost( node_new.value, node.value) if potential_cost < node.cost: self.rrt_star.adoption(node, node_new) def plan_path(self, start_point, end_point, map_obj): """ RRT*. Tries to find a collision free path from start to goal. """ # STUFF FOR TESTING self.trajectory.clear() if self.enable_vis: marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.action = marker.ADD marker.scale.x = 0.1 marker.scale.y = 0.1 self.vis_pub.publish(marker) exploration_bias = 1.0 - self.goal_bias final_node = None num_existing_path_points_added = 0 self.rrt_star = RRTStar(Node(start_point)) self.max_iterations = self.rrt_star.max_size while self.rrt_star.size <= self.max_iterations: p = np.random.uniform() if p < exploration_bias: x_rand = self.map.sample_free_space() else: if final_node is None: x_rand = end_point else: x_rand = self.branched_from_existing_path( final_node, depth_underestimate=num_existing_path_points_added) num_existing_path_points_added += 1 x_nearest = self.rrt_star.nearest( x_rand) # Find the nearest node to x_rand path = self.map.generate_line_path(x_nearest.value, x_rand, eta=self.eta) if path is not None: # no obstacles between x_nearest and x_rand x_new = path[-1] X_nearby_connectable = self.find_nearby_connectable( x_nearest, x_new) cost_min, node_min = self.find_best_parent( X_nearby_connectable, x_new) X_nearby_connectable.remove( node_min ) # Remove x_new's parent node from the list of nearby nodes so it is not considered for rewiring # Create the new node at x_new! node_new = self.rrt_star.add_config(node_min, x_new) if self.enable_vis: # FOR TESTING ONLY # # Code to publish marker for new node ########################################################################################### TEMP = Point() TEMP.x = x_new[0] TEMP.y = x_new[1] TEMP.z = .05 marker.points.append(TEMP) TEMP = ColorRGBA() TEMP.r = 1 TEMP.g = 0 TEMP.b = 0 TEMP.a = 1 marker.colors.append(TEMP) self.vis_pub.publish(marker) ########################################################################################### self.rewire(cost_min, node_new, X_nearby_connectable) if np.allclose(node_new.value, end_point, .05, 0) and ( final_node is None): #np.array_equal(node_new.value, end_point): final_node = node_new # reduce exploration bias so that we reinforce the existing path exploration_bias = .5 if VERBOSE: print("Path found!!!!") print(final_node.cost) if rospy.get_time() - self.start_time > self.time_thresh: if VERBOSE: print(self.rrt_star.size) break if final_node is not None: if self.enable_vis: marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.action = marker.ADD marker.scale.x = 0.1 marker.scale.y = 0.1 marker.points = [] marker.colors = [] def recur(node): if self.enable_vis: TEMP = Point() TEMP.x = node.value[0] TEMP.y = node.value[1] TEMP.z = .05 marker.points.append(TEMP) TEMP = ColorRGBA() TEMP.r = 1 TEMP.g = 0 TEMP.b = 0 TEMP.a = 1 marker.colors.append(TEMP) self.trajectory.points.append([node.value[0], node.value[1]]) parent = node.parent if parent is not None: recur(parent) recur(final_node) self.trajectory.points.reverse() if self.enable_vis: self.vis_pub.publish(marker) if VERBOSE: print(final_node.depth) else: # TODO:give best guess- find closest node to goal if VERBOSE: print("No path found! Please try again.")
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 TrajectoryWithSpeedLoader(): def __init__(self): # Define Adjustaable Parameters self.min_speed = rospy.get_param("~min_speed") self.max_speed = rospy.get_param("~max_speed") self.max_acceleration = rospy.get_param("~max_acceleration") self.max_decceleration = rospy.get_param("~max_decceleration") # Internal USE Variables - Do not modify without consultation self.dynamic_model = utils.ApproximateDynamicsModel() self.ackermann_model = utils.AckermannModel(0.36) self.trajectory = LineTrajectory("/speed_track") self.counts = 0 # Publishers self.viz_track_pub = rospy.Publisher("/speed_track/viz", MarkerArray, queue_size=1) self.traj_pub = rospy.Publisher(rospy.get_param("~pub_topic"), PolygonStamped, queue_size=1) # Subscriber self.traj_sub = rospy.Subscriber(rospy.get_param("~sub_topic"), String, self.trajCB, queue_size=1) print "Initialized. Waiting on messages..." # need to wait a short period of time before publishing the first message time.sleep(0.5) # Callback Function for receiving path def trajCB(self, msg): path_id = msg.data if (self.counts == 1): self.generate_speed_profile(path_id) self.counts = 0 self.counts += 1 # Generating Speed Profile def generate_speed_profile(self, path): self.trajectory.clear() self.trajectory.load(path) self.trajectory.make_np_array() points = self.trajectory.np_points local_polar_points = utils.piecewise_linear_local_waypoints_polar( points) speeds = np.zeros(points.shape[0]) for i in xrange(local_polar_points.shape[0]): max_speed = self.dynamic_model.max_speed_dist( local_polar_points[i, 0]) speeds[i] = max_speed speeds[0] = 0 # speed of the beginning of the track speeds[-1] = 0 # speed of the last of the track speeds = np.clip(speeds, self.min_speed, self.max_speed) # ensure that acceleration bounds are respected for i in xrange(speeds.shape[0] - 1): x = local_polar_points[i, 0] v_i = speeds[i] # initial speed of a straight line v_f = speeds[i + 1] # final speed of a straight line a = (v_f**2 - v_i**2) / (2.0 * x) # vf^2 - vi^2 = 2ax if a > self.max_acceleration: a = self.max_acceleration v_f = np.sqrt(v_i**2 + 2.0 * a * x) speeds[i + 1] = v_f # ensure that decceleration bounds are respected for i in xrange(speeds.shape[0] - 2, -1, -1): x = local_polar_points[i, 0] v_i = speeds[i] v_f = speeds[i + 1] a = (v_f**2 - v_i**2) / (2.0 * x) # vf^2 - vi^2 = 2ax if a < self.max_decceleration: a = self.max_decceleration v_i = np.sqrt(v_f**2 - 2.0 * a * x) speeds[i] = v_i # counting estimated travesal time t_traversal = 0.0 for i in xrange(speeds.shape[0] - 1): v, x = speeds[i], local_polar_points[i, 0] t = x / v t_traversal += t print "Estimated traversal time: ", t_traversal print speeds # Publish the generated Speed Profile self.trajectory.speed_profile = speeds.tolist() self.viz_track_pub.publish( viz_speed_track(self.trajectory.speed_profile, self.trajectory.np_points)) self.traj_pub.publish(self.trajectory.toPolygon())
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]