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 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") # initialize and load the trajectory self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) self.pub_topic = "/trajectory/current" self.traj_pub = rospy.Publisher(self.pub_topic, PoseArray, queue_size=1) # need to wait a short period of time before publishing the first message rospy.loginfo("waiting to publish trajectory") time.sleep(3.0) # visualize the loaded trajectory self.trajectory.publish_viz() # send the trajectory self.publish_trajectory() def publish_trajectory(self): print "Publishing trajectory to:", self.pub_topic self.traj_pub.publish(self.trajectory.toPoseArray())
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", default= "/home/nvidia/workspace/catkin_ws/src/ddl_0725/pf_localization/trajectories/2018-08-09-16-16-58.traj" ) self.should_publish = bool(rospy.get_param("~publish", default=True)) self.pub_topic = rospy.get_param( "~topic", default="/loaded_trajectory/recorded_path") # 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, Path, 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) self.duration = rospy.Duration(1) self.times = 0 self.srv_get_raw_path_ = rospy.Service('/get_raw_path', GetRawPath, self.handle_get_raw_path) # send the trajectory if self.should_publish: self.publish_trajectory() def publish_trajectory(self): print "Publishing trajectory to:", self.pub_topic while not rospy.is_shutdown(): if self.traj_pub.get_num_connections() > 0: self.traj_pub.publish(self.trajectory.toPath()) time.sleep(self.duration.to_sec()) self.times = self.times + 1 if self.times > 20: break def handle_get_raw_path(self, req): if not isinstance(self.trajectory, LineTrajectory): self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) return GetRawPathResponse(self.trajectory.toPath())
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 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", default= "/home/lenovo/zh/workspace/catkin_ws/src/ddl_0725/localization_0725/trajectories/2018-08-09-16-16-58.traj" ) self.should_publish = bool(rospy.get_param("~publish", default=True)) self.pub_topic = rospy.get_param( "~topic", default="/loaded_trajectory/recorded_path") # 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, Path, 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) self.duration = rospy.Duration(1) # send the trajectory if self.should_publish: self.publish_trajectory() def publish_trajectory(self): print "Publishing trajectory to:", self.pub_topic while not rospy.is_shutdown(): self.traj_pub.publish(self.trajectory.toPath()) time.sleep(self.duration.to_sec())
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 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())