class LoadTrajectory(object): """ Loads a trajectory from the file system and publishes it to a ROS topic. """ def __init__(self): self.path = rospy.get_param("~trajectory") self.should_publish = bool(rospy.get_param("~publish")) self.pub_topic = rospy.get_param("~topic") # initialize and load the trajectory self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) if self.should_publish: self.traj_pub = rospy.Publisher(self.pub_topic, PolygonStamped, queue_size=1) # need to wait a short period of time before publishing the first message time.sleep(0.5) # visualize the loaded trajectory for 5 seconds self.trajectory.publish_viz(duration=3.0) # send the trajectory if self.should_publish: self.publish_trajectory() def publish_trajectory(self): print "Publishing trajectory to:", self.pub_topic self.traj_pub.publish(self.trajectory.toPolygon())
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") #%Y-%m-%d-%H-%M-%S self.trajectory = LineTrajectory("/built_trajectory") ''' Insert appropriate subscribers/publishers here ''' 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", PolygonStamped, queue_size=10) self.trajectory_points = rospy.Publisher("/traj_pts", Marker, queue_size=20) self.trajectory.publish_viz() #duration=40.0 # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def publish_trajectory(self): self.traj_pub.publish(self.trajectory.toPolygon()) 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") print("publish traj") self.publish_trajectory() 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 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 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") #%Y-%m-%d-%H-%M-%S self.trajectory = LineTrajectory("/built_trajectory") ''' Insert appropriate subscribers/publishers here ''' self.data_points = [] self.count = 0 self.num_waypoints = 4 self.waypoints = [] self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/current", PolygonStamped, queue_size=10) self.trajectory_points = rospy.Publisher("/traj_pts", Marker, queue_size=20) self.trajectory.publish_viz() #duration=40.0 self.rtt_pub = rospy.Publisher("/rtt/final", Marker, queue_size = 10) self.rtt_tree_pub = rospy.Publisher("/rtt/tree", Marker, queue_size = 10000) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) # Create a simulated laser scan self.scan_sim = PyScanSimulator2D( 2, # number of beams to send out 0, # field of view centered around theta = 0 0, # don't add noise 0.01, # used as an epsilon 500) # discretize the theta space for faster ray tracing #subscribe to map self.map_set = False self.permissible_region = None self.resolution = None self.origin = None self.width = None self.height = None self.permissible_indices = None rospy.Subscriber( "/map", OccupancyGrid, self.map_callback, queue_size = 1) self.rrt = RRT(self.rtt_tree_pub,self.scan_sim) def map_callback(self,map_msg): """ #convert the map to a numpy array print('HI') self.resolution = map_msg.info.resolution self.width = map_msg.info.width self.height = map_msg.info.height #ccmap_ = np.array(map_msg.data, np.double)/100 #map_ = np.clip(map_,0,1) #print(map_.shape) """ # assign map-based attributes self.resolution = map_msg.info.resolution self.width = map_msg.info.width self.height = map_msg.info.height # Convert the origin to a tuple origin_p = map_msg.info.origin.position origin_o = map_msg.info.origin.orientation origin_o = tf.transformations.euler_from_quaternion(( origin_o.x, origin_o.y, origin_o.z, origin_o.w)) self.origin = (origin_p.x, origin_p.y, origin_o[2]) # 0: permissible, -1: unmapped, 100: blocked array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width)) # 0: not permissible, 1: permissible self.permissible_region = np.zeros_like(array_255, dtype = bool) self.permissible_region[array_255 == 0] = 1 # dilate the maps self.permissible_region = 1 - binary_dilation(1 - self.permissible_region, square(15)) scan_map = 1 - np.array(self.permissible_region.reshape((self.permissible_region.size,)), np.double) # Initialize a map with the laser scan self.scan_sim.set_map( scan_map, map_msg.info.height, map_msg.info.width, map_msg.info.resolution, self.origin, 0.5) # Consider anything < 0.5 to be free self.permissible_indices = [(r, c) for r in xrange(self.height) for c in xrange(self.width) if self.permissible_region[r][c] == 1] self.map_set = True print("Map initialized") def publish_trajectory(self): self.traj_pub.publish(self.trajectory.toPolygon()) def saveTrajectory(self): self.trajectory.save(self.save_path) def clicked_pose(self,msg): """ 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") print("publish traj") self.publish_trajectory() """ point = Point() point.x = msg.point.x point.y = msg.point.y self.waypoints.append(point) self.mark_pt(self.trajectory_points, (0,0,1), [point]) self.count +=1 print("Point detected") print(self.count) print(self.waypoints) if self.count == self.num_waypoints: #do stuff self.data_points = [] start = self.waypoints[0] for pt in self.waypoints[1:]: rospy.loginfo("Building rrt") result = self.rrt.run(start,pt) self.data_points = self.data_points + result start = result[-1] print("Publishing") print(result) mark = Marker() mark.header.frame_id = "/map" mark.header.stamp = rospy.Time.now() mark.type = Marker.LINE_STRIP mark.ns = "final_rtt_path" mark.id = 9000 mark.scale.x = 0.5 mark.scale.y = 0.5 mark.scale.z = 0.5 mark.points = self.data_points mark.color = ColorRGBA(1,0,0,1) self.rtt_pub.publish(mark) print("published...") self.count = 0 self.waypoints = [] def mark_pt(self, subscriber, color_tup, data): mark_pt = Marker() mark_pt.header.frame_id = "/map" mark_pt.id = self.count mark_pt.header.stamp = rospy.Time.now() mark_pt.type = mark_pt.SPHERE_LIST mark_pt.action = mark_pt.ADD mark_pt.scale.x = 1.0 mark_pt.scale.y = 1.0 mark_pt.scale.z= 1.0 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 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 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())