def toLanePose(self,vicon_pose_msg): quat = vicon_pose_msg.pose.orientation euler_angles = tf.transformations.euler_from_quaternion([quat.x, quat.y,quat.z,quat.w]) lane_pose = LanePose() lane_pose.d = vicon_pose_msg.pose.position.x - self.x_goal lane_pose.phi = euler_angles[2] - self.phi_goal lane_pose.header.stamp = vicon_pose_msg.header.stamp return lane_pose
def toLanePose(self, vicon_pose_msg): quat = vicon_pose_msg.pose.orientation euler_angles = tf.transformations.euler_from_quaternion( [quat.x, quat.y, quat.z, quat.w]) lane_pose = LanePose() lane_pose.d = vicon_pose_msg.pose.position.x - self.x_goal lane_pose.phi = euler_angles[2] - self.phi_goal lane_pose.header.stamp = vicon_pose_msg.header.stamp return lane_pose
def test_v_dot_simple(self): # publish wheel_cmd_executed wheels_cmd = WheelsCmdStamped() rate = rospy.Rate(10) for i in range(10): wheels_cmd.header.stamp = rospy.Time.now() wheels_cmd.vel_left = 0.5 wheels_cmd.vel_right = 0.5 self.pub_wheels_cmd_executed.publish(wheels_cmd) rate.sleep() # publish LanePose lane_pose = LanePose() lane_pose.d = 0 lane_pose.sigma_d = 0 lane_pose.phi = 0 lane_pose.sigma_phi = 0 lane_pose.status = 0 lane_pose.in_lane = True for i in [1, 2]: lane_pose.header.stamp = rospy.Time.now() self.pub_lane_pose.publish(lane_pose) rate.sleep() # publish StopLineReading stop_line_reading = StopLineReading() stop_line_reading.stop_line_detected = True stop_line_reading.at_stop_line = False stop_line_reading.stop_line_point = Point() for x in [0.51, 0.5]: stop_line_reading.header.stamp = rospy.Time.now() stop_line_reading.stop_line_point.x = x self.pub_stop_line_reading.publish(stop_line_reading) rate.sleep() # Wait for the odometry_training_pairs_node to finish starting up timeout = time.time() + 2.0 while not self.sub_v_sample.get_num_connections() and \ not rospy.is_shutdown() and not time.time() > timeout: rospy.sleep(0.1) msg = self.v_received # self.assertEqual(hasattr(msg,'d_L'),True) self.assertEqual(msg.d_L, 0.5, 'd_L = %s' % msg.d_L) self.assertEqual(msg.d_R, 0.5, 'd_R = %s' % msg.d_R) self.assertAlmostEqual(msg.dt, 0.1, 2, 'dt = %s' % msg.dt) self.assertEqual( msg.theta_angle_pose_delta, 0, 'theta_angle_pose_delta = %s' % msg.theta_angle_pose_delta) self.assertAlmostEqual(msg.x_axis_pose_delta, 0.01, 5, 'x = %s' % msg.x_axis_pose_delta) self.assertEqual(msg.y_axis_pose_delta, 0, 'y = %s' % msg.y_axis_pose_delta)
def test_v_dot_simple(self): # publish wheel_cmd_executed wheels_cmd = WheelsCmdStamped() rate = rospy.Rate(10) for i in range(10): wheels_cmd.header.stamp = rospy.Time.now() wheels_cmd.vel_left = 0.5 wheels_cmd.vel_right = 0.5 self.pub_wheels_cmd_executed.publish(wheels_cmd) rate.sleep() # publish LanePose lane_pose = LanePose() lane_pose.d = 0 lane_pose.sigma_d = 0 lane_pose.phi = 0 lane_pose.sigma_phi = 0 lane_pose.status = 0 lane_pose.in_lane = True for i in [1, 2]: lane_pose.header.stamp = rospy.Time.now() self.pub_lane_pose.publish(lane_pose) rate.sleep() # publish StopLineReading stop_line_reading = StopLineReading() stop_line_reading.stop_line_detected = True stop_line_reading.at_stop_line = False stop_line_reading.stop_line_point = Point() for x in [0.51, 0.5]: stop_line_reading.header.stamp = rospy.Time.now() stop_line_reading.stop_line_point.x = x self.pub_stop_line_reading.publish(stop_line_reading) rate.sleep() # Wait for the odometry_training_pairs_node to finish starting up timeout = time.time()+2.0 while not self.sub_v_sample.get_num_connections() and \ not rospy.is_shutdown() and not time.time() > timeout: rospy.sleep(0.1) msg = self.v_received # self.assertEqual(hasattr(msg,'d_L'),True) self.assertEqual(msg.d_L, 0.5, 'd_L = %s' % msg.d_L) self.assertEqual(msg.d_R, 0.5, 'd_R = %s' % msg.d_R) self.assertAlmostEqual(msg.dt, 0.1, 2,'dt = %s' % msg.dt) self.assertEqual(msg.theta_angle_pose_delta, 0, 'theta_angle_pose_delta = %s' % msg.theta_angle_pose_delta) self.assertAlmostEqual(msg.x_axis_pose_delta, 0.01, 5, 'x = %s' % msg.x_axis_pose_delta) self.assertEqual(msg.y_axis_pose_delta, 0, 'y = %s' % msg.y_axis_pose_delta)
def processSegments(self, segment_list_msg): if not self.active: return current_time = time.time() self.filter.predict(dt=current_time - self.t_last_update, v=self.velocity.v, w=self.velocity.omega) self.t_last_update = current_time ml = self.filter.update(segment_list_msg.segments) if ml is not None: ml_img = self.getDistributionImage(ml, segment_list_msg.header.stamp) self.pub_ml_img.publish(ml_img) [d_max, phi_max] = self.filter.getEstimate() max_val = self.filter.getMax() in_lane = bool(max_val > self.filter.min_max) lanePose = LanePose() lanePose.header.stamp = segment_list_msg.header.stamp lanePose.d = d_max lanePose.phi = phi_max lanePose.in_lane = in_lane lanePose.status = lanePose.NORMAL # publish the belief image belief_img = self.getDistributionImage(self.filter.belief, segment_list_msg.header.stamp) current_time_print = time.time() message_time = segment_list_msg.header.stamp.sec + segment_list_msg.header.stamp.nanosec * 1e-9 delay = current_time_print - message_time #print("wheels_cmd timestamp: " + str(segment_list_msg.header.stamp)) #print("current time: " + str(current_time_print)) #print("delay: " + str(delay)) self.pub_lane_pose.publish(lanePose) self.pub_belief_img.publish(belief_img) # also publishing a separate Bool for the FSM in_lane_msg = BoolStamped() in_lane_msg.header.stamp = segment_list_msg.header.stamp in_lane_msg.data = in_lane self.pub_in_lane.publish(in_lane_msg)
def cbProcessSegments(self, segment_list_msg): """Callback to process the segments Args: segment_list_msg (:obj:`SegmentList`): message containing list of processed segments """ # Get actual timestamp for latency measurement timestamp_before_processing = rospy.Time.now() # Step 1: predict current_time = rospy.get_time() if self.currentVelocity: dt = current_time - self.t_last_update self.filter.predict(dt=dt, v=self.currentVelocity.v, w=self.currentVelocity.omega) self.t_last_update = current_time # Step 2: update self.filter.update(segment_list_msg.segments) # Step 3: build messages and publish things [d_max, phi_max] = self.filter.getEstimate() # print "d_max = ", d_max # print "phi_max = ", phi_max # Getting the highest belief value from the belief matrix max_val = self.filter.getMax() # Comparing it to a minimum belief threshold to make sure we are certain enough of our estimate in_lane = max_val > self.filter.min_max # build lane pose message to send lanePose = LanePose() lanePose.header.stamp = segment_list_msg.header.stamp lanePose.d = d_max lanePose.phi = phi_max lanePose.in_lane = in_lane # XXX: is it always NORMAL? lanePose.status = lanePose.NORMAL self.pub_lane_pose.publish(lanePose) self.debugOutput(segment_list_msg, d_max, phi_max, timestamp_before_processing)
def publishEstimate(self, segment_list_msg=None): belief = self.filter.getEstimate() # build lane pose message to send lanePose = LanePose() lanePose.header.stamp = self.last_update_stamp lanePose.d = belief['mean'][0] lanePose.phi = belief['mean'][1] lanePose.d_phi_covariance = [ belief['covariance'][0][0], belief['covariance'][0][1], belief['covariance'][1][0], belief['covariance'][1][1] ] lanePose.in_lane = True lanePose.status = lanePose.NORMAL self.pub_lane_pose.publish(lanePose) if segment_list_msg is not None: self.debugOutput(segment_list_msg, lanePose.d, lanePose.phi)
def processSegments(self, segment_list_msg): if not self.active: return # Step 1: predict current_time = rospy.get_time() self.filter.predict(dt=current_time - self.t_last_update, v=self.velocity.v, w=self.velocity.omega) self.t_last_update = current_time # Step 2: update ml = self.filter.update(segment_list_msg.segments) if ml is not None: ml_img = self.getDistributionImage(ml, segment_list_msg.header.stamp) self.pub_ml_img.publish(ml_img) # Step 3: build messages and publish things [d_max, phi_max] = self.filter.getEstimate() max_val = self.filter.getMax() in_lane = max_val > self.filter.min_max # build lane pose message to send lanePose = LanePose() lanePose.header.stamp = segment_list_msg.header.stamp lanePose.d = d_max lanePose.phi = phi_max lanePose.in_lane = in_lane lanePose.status = lanePose.NORMAL # publish the belief image belief_img = self.getDistributionImage(self.filter.belief, segment_list_msg.header.stamp) self.pub_lane_pose.publish(lanePose) self.pub_belief_img.publish(belief_img) # also publishing a separate Bool for the FSM in_lane_msg = BoolStamped() in_lane_msg.header.stamp = segment_list_msg.header.stamp in_lane_msg.data = in_lane self.pub_in_lane.publish(in_lane_msg)
def cb_intpose(self, int_pose): ''' cb_intpose Function, that subscribes to the incoming PoseStamped (position and orientation of duckiebot) message and publishes the LanePose message with the entries d and phi, that can be used for the controller :param int_pose: PoseStamped ros message :return: None ''' # TODO Change to debug level if self.verbose: self.log('Received intersection pose.') if self.parametersChanged: self.log('Parameters changed.', 'info') self.refresh_parameters() self.parametersChanged = False trajectory = self.trajectories[self.trajectory] track = trajectory['track'] tangent_angle = trajectory['tangent_angle'] curvature = trajectory['curvature'] # compute distance d and angle phi, same as in lane following d, phi, curv = self.relative_pose(int_pose, track, tangent_angle, curvature) # convert to LanePose() message lane_pose = LanePose() lane_pose.header.stamp = int_pose.header.stamp lane_pose.d = d lane_pose.phi = phi lane_pose.curvature = curv # publish lane pose msg self.pub_lanepose.publish(lane_pose) if self.verbose: self.log("distance: " + str(d)) self.log("angle: " + str(phi)) if self.track_publishing_skipped_iterations % 10 == 0: self.track_publishing_skipped_iterations = 0 self.publish_track(track, tangent_angle) self.track_publishing_skipped_iterations += 1
def processSegments(self, segment_list_msg): if not self.active: return # Step 1: predict current_time = rospy.get_time() self.filter.predict(dt=current_time - self.t_last_update, v=self.velocity.v, w=self.velocity.omega) self.t_last_update = current_time # Step 2: update self.filter.update(segment_list_msg.segments) # Step 3: build messages and publish things [d_max, phi_max] = self.filter.getEstimate() max_val = self.filter.getMax() in_lane = max_val > self.filter.min_max # build lane pose message to send lanePose = LanePose() lanePose.header.stamp = segment_list_msg.header.stamp lanePose.d = d_max lanePose.phi = phi_max lanePose.in_lane = in_lane lanePose.status = lanePose.NORMAL # publish the belief image bridge = CvBridge() belief_img = bridge.cv2_to_imgmsg( (255 * self.filter.belief).astype('uint8'), "mono8") belief_img.header.stamp = segment_list_msg.header.stamp self.pub_lane_pose.publish(lanePose) self.pub_belief_img.publish(belief_img) # also publishing a separate Bool for the FSM in_lane_msg = BoolStamped() in_lane_msg.header.stamp = segment_list_msg.header.stamp in_lane_msg.data = in_lane self.pub_in_lane.publish(in_lane_msg)
def processSegments(self, segment_list_msg): if not self.active: return # Get actual timestamp for latency measurement timestamp_now = rospy.Time.now() # TODO-TAL double call to param server ... --> see TODO in the readme, not only is it never updated, but it is alwas 0 # Step 0: get values from server if (rospy.get_param('~curvature_res') is not self.curvature_res): self.curvature_res = rospy.get_param('~curvature_res') self.filter.updateRangeArray(self.curvature_res) # Step 1: predict current_time = rospy.get_time() dt = current_time - self.t_last_update v = self.velocity.v w = self.velocity.omega self.filter.predict(dt=dt, v=v, w=w) self.t_last_update = current_time # Step 2: update self.filter.update(segment_list_msg.segments) parking_detected = BoolStamped() parking_detected.header.stamp = segment_list_msg.header.stamp parking_detected.data = self.filter.parking_detected self.pub_parking_detection.publish(parking_detected) parking_on = BoolStamped() parking_on.header.stamp = segment_list_msg.header.stamp parking_on.data = True self.pub_parking_on.publish(parking_on) if self.active_mode: # Step 3: build messages and publish things [d_max, phi_max] = self.filter.getEstimate() # print "d_max = ", d_max # print "phi_max = ", phi_max inlier_segments = self.filter.get_inlier_segments( segment_list_msg.segments, d_max, phi_max) inlier_segments_msg = SegmentList() inlier_segments_msg.header = segment_list_msg.header inlier_segments_msg.segments = inlier_segments self.pub_seglist_filtered.publish(inlier_segments_msg) #max_val = self.filter.getMax() #in_lane = max_val > self.filter.min_max # build lane pose message to send lanePose = LanePose() lanePose.header.stamp = segment_list_msg.header.stamp lanePose.d = d_max[0] lanePose.phi = phi_max[0] #lanePose.in_lane = in_lane # XXX: is it always NORMAL? lanePose.status = lanePose.NORMAL if self.curvature_res > 0: lanePose.curvature = self.filter.getCurvature( d_max[1:], phi_max[1:]) self.pub_lane_pose.publish(lanePose) # TODO-TAL add a debug param to not publish the image !! # TODO-TAL also, the CvBridge is re instantiated every time... # publish the belief image bridge = CvBridge() belief_img = bridge.cv2_to_imgmsg( np.array(255 * self.filter.beliefArray[0]).astype("uint8"), "mono8") belief_img.header.stamp = segment_list_msg.header.stamp self.pub_belief_img.publish(belief_img) # Latency of Estimation including curvature estimation estimation_latency_stamp = rospy.Time.now() - timestamp_now estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9 self.latencyArray.append(estimation_latency) if (len(self.latencyArray) >= 20): self.latencyArray.pop(0) # print "Latency of segment list: ", segment_latency # print("Mean latency of Estimation:................. %s" % np.mean(self.latencyArray)) # also publishing a separate Bool for the FSM in_lane_msg = BoolStamped() in_lane_msg.header.stamp = segment_list_msg.header.stamp in_lane_msg.data = self.filter.inLane if (self.filter.inLane): self.pub_in_lane.publish(in_lane_msg)
def processSegments(self, segment_list_msg): # Get actual timestamp for latency measurement timestamp_now = rospy.Time.now() if not self.active: return # TODO-TAL double call to param server ... --> see TODO in the readme, not only is it never updated, but it is alwas 0 # Step 0: get values from server if (rospy.get_param('~curvature_res') is not self.curvature_res): self.curvature_res = rospy.get_param('~curvature_res') self.filter.updateRangeArray(self.curvature_res) ## ####################################################### # ######################################################## # Parameters # ######################################################## lane_width = 0.23 # [m] valid_rad = 0.5 # [m] lookahead = 0.1 # [m] n_test_points = 50 max_buffer_size = 25 # ######################################################## # Process inputs # ######################################################## # Generate an xy list from the data white_xy = [] yellow_xy = [] for segment in segment_list_msg.segments: # White points if segment.color == 0: white_xy.append([segment.points[0].x, segment.points[0].y]) # Yellow points elif segment.color == 1: yellow_xy.append([segment.points[0].x, segment.points[0].y]) else: print("Unrecognized segment color") # Turn into numpy arrays white_xy = np.array(white_xy) yellow_xy = np.array(yellow_xy) # ######################################################## # Process buffer # ######################################################## # Integrate the odometry current_time = rospy.get_time() dt = current_time - self.t_last_update self.t_last_update = current_time delta_x = self.velocity.v * dt delta_theta = self.velocity.omega * dt # Form the displacement delta_r = delta_x * np.array( [-np.sin(delta_theta), np.cos(delta_theta)]) # If buffers contains data, propogate it BACKWARD if len(self.buffer_white) > 0: self.buffer_white = self.buffer_white - delta_r if len(self.buffer_yellow) > 0: self.buffer_yellow = self.buffer_yellow - delta_r # If new observations were received, then append them to the top of the list if len(white_xy) > 0: if len(self.buffer_white) > 0: self.buffer_white = np.vstack((white_xy, self.buffer_white)) else: self.buffer_white = white_xy if len(yellow_xy) > 0: if len(self.buffer_yellow) > 0: self.buffer_yellow = np.vstack((yellow_xy, self.buffer_yellow)) else: self.buffer_yellow = yellow_xy # ######################################################## # Trim training points to within a valid radius # ######################################################## valid_white_xy = [] valid_yellow_xy = [] # Select points within rad if len(self.buffer_white) > 0: tf_valid_white = self.computeRad(self.buffer_white) <= valid_rad valid_white_xy = self.buffer_white[tf_valid_white, :] if len(self.buffer_yellow) > 0: tf_valid_yellow = self.computeRad(self.buffer_yellow) <= valid_rad valid_yellow_xy = self.buffer_yellow[tf_valid_yellow, :] # ######################################################## # Fit GPs # ######################################################## # Set up a linspace for prediction if len(valid_white_xy) > 0: x_pred_white = np.linspace(0, np.minimum(valid_rad, np.amax(valid_white_xy[:, 0])), \ num=n_test_points+1).reshape(-1, 1) else: x_pred_white = np.linspace(0, valid_rad, num=n_test_points + 1).reshape(-1, 1) if len(valid_yellow_xy) > 0: x_pred_yellow = np.linspace(0, np.minimum(valid_rad, np.amax(valid_yellow_xy[:, 0])), \ num=n_test_points+1).reshape(-1, 1) else: x_pred_yellow = np.linspace(0, valid_rad, num=n_test_points + 1).reshape(-1, 1) # Start with the prior y_pred_white = -lane_width / 2.0 * np.ones((len(x_pred_white), 1)) y_pred_yellow = lane_width / 2.0 * np.ones((len(x_pred_yellow), 1)) t_start = time.time() # White GP. Note that we're fitting y = f(x) gpWhite = [] if len(valid_white_xy) > 2: x_train_white = valid_white_xy[:, 0].reshape(-1, 1) y_train_white = valid_white_xy[:, 1] whiteKernel = C(0.01, (-lane_width, 0)) * RBF(5, (0.3, 0.4)) # whiteKernel = C(1.0, (1e-3, 1e3)) * RBF(5, (1e-2, 1e2)) gpWhite = GaussianProcessRegressor(kernel=whiteKernel, optimizer=None) # x = f(y) gpWhite.fit(x_train_white, y_train_white) # Predict y_pred_white = gpWhite.predict(x_pred_white) # Yellow GP gpYellow = [] if len(valid_yellow_xy) > 2: x_train_yellow = valid_yellow_xy[:, 0].reshape(-1, 1) y_train_yellow = valid_yellow_xy[:, 1] # yellowKernel = C(lane_width / 2.0, (0, lane_width)) * RBF(5, (0.3, # 0.4)) yellowKernel = C(0.01, (0, lane_width)) * RBF(5, (0.3, 0.4)) gpYellow = GaussianProcessRegressor(kernel=yellowKernel, optimizer=None) gpYellow.fit(x_train_yellow, y_train_yellow) # Predict y_pred_yellow = gpYellow.predict(x_pred_yellow) t_end = time.time() # print("MODEL BUILDING TIME: ", t_end - t_start) # Make xy point arrays xy_gp_white = np.hstack((x_pred_white, y_pred_white.reshape(-1, 1))) xy_gp_yellow = np.hstack((x_pred_yellow, y_pred_yellow.reshape(-1, 1))) # Trim predicted values to valid radius # Logical conditions tf_valid_white = self.computeRad(xy_gp_white) <= valid_rad tf_valid_yellow = self.computeRad(xy_gp_yellow) <= valid_rad # Select points within rad valid_xy_gp_white = xy_gp_white[tf_valid_white, :] valid_xy_gp_yellow = xy_gp_yellow[tf_valid_yellow, :] # ######################################################## # Display # ######################################################## self.visualizeCurves(valid_xy_gp_white, valid_xy_gp_yellow) # ######################################################## # Compute d and \phi # ######################################################## # Evaluate each GP at the lookahead distance and average to get d # Start with prior y_white = -lane_width / 2 y_yellow = lane_width / 2 if gpWhite: y_white = gpWhite.predict(np.array([lookahead]).reshape(-1, 1)) if gpYellow: y_yellow = gpYellow.predict(np.array([lookahead]).reshape(-1, 1)) # Take the average, and negate disp = np.mean((y_white, y_yellow)) d = -disp # Compute phi from the lookahead distance and d L = np.sqrt(d**2 + lookahead**2) phi = disp / L print("D is: ", d) print("phi is: ", phi) # ######################################################## # Publish the lanePose message # ######################################################## lanePose = LanePose() lanePose.header.stamp = segment_list_msg.header.stamp lanePose.d = d lanePose.phi = phi lanePose.in_lane = True lanePose.status = lanePose.NORMAL lanePose.curvature = 0 self.pub_lane_pose.publish(lanePose) # ######################################################## # Save valid training points to buffer # ######################################################## # Cap the buffer at max_buffer_size white_max = np.minimum(len(self.buffer_white), max_buffer_size) yellow_max = np.minimum(len(self.buffer_yellow), max_buffer_size) self.buffer_white = self.buffer_white[0:white_max - 1] self.buffer_yellow = self.buffer_yellow[0:yellow_max - 1] # print ("SIZE of white buffer: ", len(self.buffer_white)) # print ("YELLOW of yellow buffer: ", len(self.buffer_yellow)) ## ####################################################### # Step 1: predict current_time = rospy.get_time() dt = current_time - self.t_last_update v = self.velocity.v w = self.velocity.omega self.filter.predict(dt=dt, v=v, w=w) # self.t_last_update = current_time # Step 2: update self.filter.update(segment_list_msg.segments) # Step 3: build messages and publish things [d_max, phi_max] = self.filter.getEstimate() # print "d_max = ", d_max # print "phi_max = ", phi_max inlier_segments = self.filter.get_inlier_segments( segment_list_msg.segments, d_max, phi_max) inlier_segments_msg = SegmentList() inlier_segments_msg.header = segment_list_msg.header inlier_segments_msg.segments = inlier_segments self.pub_seglist_filtered.publish(inlier_segments_msg) max_val = self.filter.getMax() in_lane = max_val > self.filter.min_max # build lane pose message to send # lanePose = LanePose() # lanePose.header.stamp = segment_list_msg.header.stamp # lanePose.d = d_max[0] # lanePose.phi = phi_max[0] # lanePose.in_lane = in_lane # # XXX: is it always NORMAL? # lanePose.status = lanePose.NORMAL # if self.curvature_res > 0: # lanePose.curvature = self.filter.getCurvature(d_max[1:], phi_max[1:]) # self.pub_lane_pose.publish(lanePose) # TODO-TAL add a debug param to not publish the image !! # TODO-TAL also, the CvBridge is re instantiated every time... # publish the belief image bridge = CvBridge() belief_img = bridge.cv2_to_imgmsg( np.array(255 * self.filter.beliefArray[0]).astype("uint8"), "mono8") belief_img.header.stamp = segment_list_msg.header.stamp self.pub_belief_img.publish(belief_img) # Latency of Estimation including curvature estimation estimation_latency_stamp = rospy.Time.now() - timestamp_now estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9 self.latencyArray.append(estimation_latency) if (len(self.latencyArray) >= 20): self.latencyArray.pop(0) # print "Latency of segment list: ", segment_latency # print("Mean latency of Estimation:................. %s" % np.mean(self.latencyArray)) # also publishing a separate Bool for the FSM in_lane_msg = BoolStamped() in_lane_msg.header.stamp = segment_list_msg.header.stamp in_lane_msg.data = True #TODO-TAL change with in_lane. Is this messqge useful since it is alwas true ? self.pub_in_lane.publish(in_lane_msg)
def processSegments(self, segment_list_msg): # Get actual timestamp for latency measurement timestamp_now = rospy.Time.now() if not self.active: return # Step 0: get values from server if (rospy.get_param('~curvature_res') is not self.curvature_res): self.curvature_res = rospy.get_param('~curvature_res') self.filter.updateRangeArray(self.curvature_res) # Step 1: predict current_time = rospy.get_time() dt = current_time - self.t_last_update v = self.velocity.v w = self.velocity.omega self.filter.predict(dt=dt, v=v, w=w) self.t_last_update = current_time # Step 2: update self.filter.update(segment_list_msg.segments) # Step 3: build messages and publish things [d_max, phi_max] = self.filter.getEstimate() # print "d_max = ", d_max # print "phi_max = ", phi_max max_val = self.filter.getMax() in_lane = max_val > self.filter.min_max # build lane pose message to send lanePose = LanePose() lanePose.header.stamp = segment_list_msg.header.stamp lanePose.d = d_max[0] lanePose.phi = phi_max[0] lanePose.in_lane = in_lane # XXX: is it always NORMAL? lanePose.status = lanePose.NORMAL if self.curvature_res > 0: lanePose.curvature = self.filter.getCurvature( d_max[1:], phi_max[1:]) # publish the belief image belief_img = self.getDistributionImage(self.filter.belief, segment_list_msg.header.stamp) self.pub_lane_pose.publish(lanePose) self.pub_belief_img.publish(belief_img) def getDistributionImage(self, mat, stamp): bridge = CvBridge() img = bridge.cv2_to_imgmsg((255 * mat).astype('uint8'), "mono8") img.header.stamp = stamp return img # Latency of Estimation including curvature estimation estimation_latency_stamp = rospy.Time.now() - timestamp_now estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9 self.latencyArray.append(estimation_latency) if (len(self.latencyArray) >= 20): self.latencyArray.pop(0) # print "Latency of segment list: ", segment_latency # print("Mean latency of Estimation:................. %s" % np.mean(self.latencyArray)) # TODO (1): see above, method does not exist #self.pub_belief_img.publish(belief_img) # also publishing a separate Bool for the FSM in_lane_msg = BoolStamped() in_lane_msg.header.stamp = segment_list_msg.header.stamp in_lane_msg.data = True #TODO change with in_lane self.pub_in_lane.publish(in_lane_msg)
def processSegments(self, segment_list_msg): # Get actual timestamp for latency measurement timestamp_now = rospy.Time.now() if not self.active: return # TODO-TAL double call to param server ... --> see TODO in the readme, not only is it never updated, but it is alwas 0 # Step 0: get values from server if (rospy.get_param('~curvature_res') is not self.curvature_res): self.curvature_res = rospy.get_param('~curvature_res') self.filter.updateRangeArray(self.curvature_res) # Step 1: predict current_time = rospy.get_time() dt = current_time - self.t_last_update v = self.velocity.v w = self.velocity.omega # ==== HERE THE UPPER BOUNDS ======== ub_d = dt * self.omega_max * self.gain * 1000 ub_phi = dt * self.v_ref * self.gain * 1000 self.filter.predict(dt=dt, v=v, w=w) self.t_last_update = current_time # Step 2: update self.filter.update(segment_list_msg.segments, self.lane_offset) # Step 3: build messages and publish things [d_max, phi_max] = self.filter.getEstimate() inlier_segments = self.filter.get_inlier_segments( segment_list_msg.segments, d_max, phi_max) inlier_segments_msg = SegmentList() inlier_segments_msg.header = segment_list_msg.header inlier_segments_msg.segments = inlier_segments self.pub_seglist_filtered.publish(inlier_segments_msg) max_val = self.filter.getMax() in_lane = max_val > self.filter.min_max # build lane pose message to send lanePose = LanePose() lanePose.header.stamp = segment_list_msg.header.stamp lanePose.d = d_max[0] if not self.last_d: self.last_d = lanePose.d if lanePose.d - self.last_d > ub_d: lanePose.d = self.last_d + ub_d rospy.loginfo("d changed too much") if lanePose.d - self.last_d < -ub_d: lanePose.d = self.last_d - ub_d rospy.loginfo("d changed too much") lanePose.phi = phi_max[0] if not self.last_phi: self.last_phi = lanePose.phi if lanePose.phi - self.last_phi > ub_phi: lanePose.phi = self.last_phi + ub_phi rospy.loginfo("phi changed too much") if lanePose.phi - self.last_phi < -ub_phi: lanePose.phi = self.last_phi - ub_phi rospy.loginfo("phi changed too much") lanePose.in_lane = in_lane # XXX: is it always NORMAL? lanePose.status = lanePose.NORMAL if self.curvature_res > 0: lanePose.curvature = self.filter.getCurvature( d_max[1:], phi_max[1:]) self.pub_lane_pose.publish(lanePose) # Save for next iteration <========= self.last_d = lanePose.d self.last_phi = lanePose.phi # publish the belief image bridge = CvBridge() belief_img = bridge.cv2_to_imgmsg( np.array(255 * self.filter.beliefArray[0]).astype("uint8"), "mono8") belief_img.header.stamp = segment_list_msg.header.stamp self.pub_belief_img.publish(belief_img) # Latency of Estimation including curvature estimation estimation_latency_stamp = rospy.Time.now() - timestamp_now estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9 self.latencyArray.append(estimation_latency) if (len(self.latencyArray) >= 20): self.latencyArray.pop(0) # also publishing a separate Bool for the FSM in_lane_msg = BoolStamped() in_lane_msg.header.stamp = segment_list_msg.header.stamp in_lane_msg.data = True #TODO-TAL change with in_lane. Is this messqge useful since it is alwas true ? self.pub_in_lane.publish(in_lane_msg)
def processSegments(self,segment_list_msg): # Get actual timestamp for latency measurement timestamp_now = rospy.Time.now() if not self.active: return # Step 1: predict current_time = rospy.get_time() dt = current_time - self.t_last_update v = self.velocity.v w = self.velocity.omega self.filter.predict(dt=dt, v=v, w=w) self.t_last_update = current_time # Step 2: update self.filter.update(segment_list_msg.segments) # Step 3: build messages and publish things [d_max, phi_max] = self.filter.getEstimate() ## Inlier segments inlier_segments = self.filter.get_inlier_segments(segment_list_msg.segments, d_max, phi_max) inlier_segments_msg = SegmentList() inlier_segments_msg.header = segment_list_msg.header inlier_segments_msg.segments = inlier_segments self.pub_seglist_filtered.publish(inlier_segments_msg) ## Lane pose in_lane = self.filter.isInLane() lanePose = LanePose() lanePose.header.stamp = segment_list_msg.header.stamp lanePose.d = d_max lanePose.phi = phi_max lanePose.in_lane = in_lane lanePose.status = lanePose.NORMAL self.pub_lane_pose.publish(lanePose) ## Belief image bridge = CvBridge() if self.mode == 'histogram': belief_img = bridge.cv2_to_imgmsg(np.array(255 * self.filter.beliefArray).astype("uint8"), "mono8") elif self.mode == 'particle': belief_img = bridge.cv2_to_imgmsg(np.array(255 * self.filter.getBeliefArray()).astype("uint8"), "mono8") belief_img.header.stamp = segment_list_msg.header.stamp self.pub_belief_img.publish(belief_img) ## Latency of Estimation estimation_latency_stamp = rospy.Time.now() - timestamp_now estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs/1e9 self.latencyArray.append(estimation_latency) if (len(self.latencyArray) >= 20): self.latencyArray.pop(0) # Separate Bool for the FSM in_lane_msg = BoolStamped() in_lane_msg.header.stamp = segment_list_msg.header.stamp in_lane_msg.data = True #TODO-TAL change with in_lane. Is this messqge useful since it is alwas true ? self.pub_in_lane.publish(in_lane_msg)
def updateEstimate(self, segListMsg): self.testCount = self.testCount + 1 # Just some counter, gives an idea of the speed. # ######## EXTRACT ######### data from segments back into separated arrays. whitePointsArray, yellowPointsArray = self.segList2Array(segListMsg) nWhite = whitePointsArray.shape[0] nYellow = yellowPointsArray.shape[0] # ###### REMOVE WHITE POINTS OF LEFT LANE ###### if nYellow >= 2: # Need minimum of two points to define a line. self.ransacY.fit( np.reshape(yellowPointsArray[:, 0], (-1, 1)), np.array(np.reshape(yellowPointsArray[:, 1], (-1, 1)))) zYellowOnly = np.array([ self.ransacY.estimator_.coef_, self.ransacY.estimator_.intercept_ ]) whitePointsArray = self.removeLeftLane(zYellowOnly, whitePointsArray) nWhite = whitePointsArray.shape[0] nYellow = yellowPointsArray.shape[0] # ###### FIT TWO LANES ######## if nWhite >= 2 and nYellow >= 2: z = self.fitTwoLanes(whitePointsArray, yellowPointsArray) #self.zTwoLane = z m = z[0] c = z[1] zWhite = np.array([m, c]) zYellow = np.array([m, c + self.laneWidth * np.sqrt(m**2 + 1)]) elif nYellow >= 2: zWhite = zYellowOnly elif nWhite >= 2: self.ransacW.fit(np.reshape(whitePointsArray[:, 0], (-1, 1)), np.reshape(whitePointsArray[:, 1], (-1, 1))) zWhite = np.array([ self.ransacW.estimator_.coef_, self.ransacW.estimator_.intercept_ ]) if nWhite >= 2 or nYellow >= 2: # ######### Calculate d and phi from geometry ########## m = np.asscalar(zWhite[0]) c = np.asscalar(zWhite[1]) phiWhite = np.arcsin(-m / (np.sqrt(1 + m**2))) r_wz_b = np.array([ [-c * m / (1 + m**2)], [c - (c * m**2 / (1 + m**2))] ]) # Position vector to nearest point on the line (normal to line) C_wb = np.array([[np.cos(phiWhite), -np.sin(phiWhite)], [np.sin(phiWhite), np.cos(phiWhite)]]) r_wz_w = np.matmul( C_wb, r_wz_b ) # Distance TO white lane from duckiebot. This should be negative if nWhite < 2 and nYellow >= 2: # Then the line we have is actually yellow r_dw_w = np.array([[0], [-self.laneWidth / 2]]) else: if r_wz_w[1] >= 0: # Then white line is on our left. # This can occur in two situations # 1) We are in the other lane # 2) We are turning a right corner. # In either case, it is actually the OTHER white line r_dw_w = np.array([[0], [-1.5 * self.laneWidth]]) else: r_dw_w = np.array([[0], [self.laneWidth / 2]]) r_zd_w = -r_wz_w - r_dw_w distWhite = r_zd_w[1] # Distance TO duckiebot from desired point. phi = phiWhite d = distWhite else: d = 0 phi = 0 # ######### VISUALIZE ########### if nWhite >= 2 and nYellow >= 2: pa1 = self.line2PointArray(zWhite, 0, 1) pa2 = self.line2PointArray(zYellow, 0, 1) self.visualizeCurves(pa1, pa2) elif nWhite >= 2 or nYellow >= 2: pa1 = self.line2PointArray(zWhite, 0, 1) self.visualizeCurves(pa1) # ###### PUBLISH LANE POSE ####### lanePose = LanePose() lanePose.header.stamp = segListMsg.header.stamp # Complimentary filter lol lanePose.d = 0.8 * float(d) + 0.2 * float(self.d_baseline) lanePose.phi = 0.8 * float(phi) + 0.2 * float(self.phi_baseline) lanePose.in_lane = True lanePose.status = lanePose.NORMAL self.correct(lanePose) # Kalman Filter Modifications lanePose.d = np.asscalar(self.x_k[0][0]) lanePose.phi = np.asscalar(self.x_k[1][0]) self.pub_lane_pose.publish(lanePose)
def MainLoop(self): if not self.active: return rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): # run state machine if self.state == self.state_dict['IDLE']: # waiting for FSMState to tell us that Duckiebot is at an intersection (see FSMCallback) pass elif self.state == self.state_dict['INITIALIZING_LOCALIZATION']: if self.InitializeLocalization(): self.state = self.state_dict['INITIALIZING_PATH'] rospy.loginfo( "[%s] Initialized intersection localization, initializing path." % (self.node_name)) elif self.state == self.state_dict['INITIALIZING_PATH']: if self.InitializePath(): self.state = self.state_dict['TRAVERSING'] self.s = 0.0 self.traversing_start = rospy.Time.now() rospy.loginfo( "[%s] Initialized path, waiting for go signal." % (self.node_name)) else: rospy.loginfo( "[%s] Could not initialize path. Trying again." % (self.node_name)) elif self.state == self.state_dict['TRAVERSING']: '''open-loop''' if self.open_loop: msg_cmds = Twist2DStamped() msg_cmds.header.stamp = rospy.Time.now() if 1.0 < (rospy.Time.now() - self.traversing_start).to_sec() and self.go: pos, vel = self.pathPlanner.EvaluatePath(self.s) dt = 0.01 _, vel2 = self.pathPlanner.EvaluatePath(self.s + dt) self.alpha = self.v / np.linalg.norm(vel) dir = vel / np.linalg.norm(vel) dir2 = vel2 / np.linalg.norm(vel2) theta = np.arctan2(dir[1], dir[0]) theta2 = np.arctan2(dir2[1], dir2[0]) omega = (theta2 - theta) / dt msg_cmds.v = self.v msg_cmds.omega = self.alpha * omega if (msg_cmds.v - 0.5 * math.fabs(msg_cmds.omega) * 0.1) < 0.065: msg_cmds.v = 0.065 + 0.5 * math.fabs( msg_cmds.omega) * 0.1 self.alpha = self.alpha * msg_cmds.v / self.v msg_cmds.v = msg_cmds.v * self.v_scale msg_cmds.omega = msg_cmds.omega * self.omega_scale self.s = self.s + self.alpha * (rospy.Time.now( ) - self.traversing_last_time).to_sec() if (self.s > 0.99): msg_cmds.v = self.v msg_cmds.omega = 0.0 self.state = self.state_dict['DONE'] self.done_time = rospy.Time.now() else: msg_cmds.v = 0.0 msg_cmds.omega = 0.0 self.traversing_last_time = rospy.Time.now() self.pub_cmds.publish(msg_cmds) else: '''closed-loop''' msg_lane_pose = LanePose() msg_lane_pose.header.stamp = rospy.Time.now() if 1.0 < (rospy.Time.now() - self.traversing_start).to_sec() and self.go: pose, _ = self.poseEstimator.PredictState( msg_lane_pose.header.stamp) dist, theta, curvature, self.s = self.pathPlanner.ComputeLaneError( pose, self.s) #rospy.loginfo("the s is: "+str(self.s)) if (self.s > self.s_max): msg_lane_pose.v_ref = self.v msg_lane_pose.d = 0.0 msg_lane_pose.d_ref = 0.0 msg_lane_pose.phi = 0.0 msg_lane_pose.curvature_ref = 0.0 self.state = self.state_dict['DONE'] self.done_time = rospy.Time.now() else: msg_lane_pose.v_ref = self.v msg_lane_pose.d = dist msg_lane_pose.d_ref = 0.0 msg_lane_pose.phi = theta msg_lane_pose.curvature_ref = curvature else: msg_lane_pose.v_ref = 0.0 msg_lane_pose.d = 0.0 msg_lane_pose.d_ref = 0.0 msg_lane_pose.phi = 0.0 msg_lane_pose.curvature_ref = 0.0 self.pub_lane_pose.publish(msg_lane_pose) elif self.state == self.state_dict['DONE']: # switch back to lane following if in lane if self.in_lane and (rospy.Time.now() - self.in_lane_time ).to_sec() < self.in_lane_timeout: msg_done = BoolStamped() msg_done.header.stamp = rospy.Time.now() msg_done.data = True self.pub_done.publish(msg_done) self.state = self.state_dict['IDLE'] # go straight for 2.0 secs if not in lane yet. After 2 secs stop. else: '''open-loop''' if self.open_loop: if (rospy.Time.now() - self.done_time ).to_sec() < self.in_lane_wait_time: msg_cmds = Twist2DStamped() msg_cmds.header.stamp = rospy.Time.now() msg_cmds.v = self.v * self.v_scale msg_cmds.omega = 0.0 * self.omega_scale self.pub_cmds.publish(msg_cmds) else: msg_cmds = Twist2DStamped() msg_cmds.header.stamp = rospy.Time.now() msg_cmds.v = 0.0 * self.v_scale msg_cmds.omega = 0.0 * self.omega_scale self.pub_cmds.publish(msg_cmds) rospy.loginfo( "[%s] Could not find lane. Stopping now." % (self.node_name)) self.state = self.state_dict['ERROR'] else: '''closed-loop''' if (rospy.Time.now() - self.done_time ).to_sec() < self.in_lane_wait_time: msg_lane_pose = LanePose() msg_lane_pose.header.stamp = rospy.Time.now() msg_lane_pose.v_ref = self.v msg_lane_pose.d = 0.0 msg_lane_pose.d_ref = 0.0 msg_lane_pose.phi = 0.0 msg_lane_pose.curvature_ref = 0.0 self.pub_lane_pose.publish(msg_lane_pose) else: msg_lane_pose = LanePose() msg_lane_pose.header.stamp = rospy.Time.now() msg_lane_pose.v_ref = 0.0 msg_lane_pose.d = 0.0 msg_lane_pose.d_ref = 0.0 msg_lane_pose.phi = 0.0 msg_lane_pose.curvature_ref = 0.0 self.pub_lane_pose.publish(msg_lane_pose) rospy.loginfo( "[%s] Could not find lane. Stopping now." % (self.node_name)) self.state = self.state_dict['ERROR'] else: rospy.loginfo("[%s] Something went wrong." % (self.node_name)) rate.sleep()
def processSegments(self, segment_list_msg): if not self.active: return # Step 1: predict current_time = rospy.get_time() dt = current_time - self.t_last_update v = self.velocity.v w = self.velocity.omega self.filter.predict(dt=dt, v=v, w=w) self.t_last_update = current_time # Step 2: update self.filter.update(segment_list_msg.segments) # Step 3: build messages and publish things [d_max, phi_max] = self.filter.getEstimateList() #print "d_max = ", d_max #print "phi_max = ", phi_max # sum_phi_l = np.sum(phi_max[1:self.filter.num_belief]) # sum_d_l = np.sum(d_max[1:self.filter.num_belief]) # av_phi_l = np.average(phi_max[1:self.filter.num_belief]) # av_d_l = np.average(d_max[1:self.filter.num_belief]) max_val = self.filter.getMax() in_lane = max_val > self.filter.min_max #if (sum_phi_l<-1.6 and av_d_l>0.05): # print "I see a left curve" #elif (sum_phi_l>1.6 and av_d_l <-0.05): # print "I see a right curve" #else: # print "I am on a straight line" delta_dmax = np.median(d_max[1:]) # - d_max[0] delta_phimax = np.median(phi_max[1:]) #- phi_max[0] if len(self.d_median) >= 5: self.d_median.pop(0) self.phi_median.pop(0) self.d_median.append(delta_dmax) self.phi_median.append(delta_phimax) # build lane pose message to send lanePose = LanePose() lanePose.header.stamp = segment_list_msg.header.stamp lanePose.d = d_max[0] lanePose.phi = phi_max[0] lanePose.in_lane = in_lane # XXX: is it always NORMAL? lanePose.status = lanePose.NORMAL #print "Delta dmax", delta_dmax #print "Delta phimax", delta_phimax CURVATURE_LEFT = 0.025 CURVATURE_RIGHT = -0.054 CURVATURE_STRAIGHT = 0 if np.median(self.phi_median) < -0.3 and np.median( self.d_median) > 0.05: print "left curve" lanePose.curvature = CURVATURE_LEFT elif np.median(self.phi_median) > 0.2 and np.median( self.d_median) < -0.02: print "right curve" lanePose.curvature = CURVATURE_RIGHT else: print "straight line" lanePose.curvature = CURVATURE_STRAIGHT # publish the belief image belief_img = self.getDistributionImage(self.filter.belief, segment_list_msg.header.stamp) self.pub_lane_pose.publish(lanePose) self.pub_belief_img.publish(belief_img) # also publishing a separate Bool for the FSM in_lane_msg = BoolStamped() in_lane_msg.header.stamp = segment_list_msg.header.stamp in_lane_msg.data = in_lane self.pub_in_lane.publish(in_lane_msg)