def cb_segments(self, segment_list_msg):
if not self.switch or self.sleep:
return
good_seg_count = 0
stop_line_x_accumulator = 0.0
stop_line_y_accumulator = 0.0
for segment in segment_list_msg.segments:
if segment.color != segment.RED:
continue
if segment.points[0].x < 0 or segment.points[1].x < 0: # the point is behind us
continue
p1_lane = self.to_lane_frame(segment.points[0])
p2_lane = self.to_lane_frame(segment.points[1])
avg_x = 0.5 * (p1_lane[0] + p2_lane[0])
avg_y = 0.5 * (p1_lane[1] + p2_lane[1])
stop_line_x_accumulator += avg_x
stop_line_y_accumulator += avg_y # TODO output covariance and not just mean
good_seg_count += 1.0
stop_line_reading_msg = StopLineReading()
stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
if good_seg_count < self.min_segs.value:
stop_line_reading_msg.stop_line_detected = False
stop_line_reading_msg.at_stop_line = False
self.pub_stop_line_reading.publish(stop_line_reading_msg)
# ### CRITICAL: publish false to at stop line output_topic
# msg = BoolStamped()
# msg.header.stamp = stop_line_reading_msg.header.stamp
# msg.data = False
# self.pub_at_stop_line.publish(msg)
# ### CRITICAL END
else:
stop_line_reading_msg.stop_line_detected = True
stop_line_point = Point()
stop_line_point.x = stop_line_x_accumulator / good_seg_count
stop_line_point.y = stop_line_y_accumulator / good_seg_count
stop_line_reading_msg.stop_line_point = stop_line_point
# Only detect redline if y is within max_y distance:
stop_line_reading_msg.at_stop_line = (
stop_line_point.x < self.stop_distance.value and np.abs(stop_line_point.y) < self.max_y.value
)
self.pub_stop_line_reading.publish(stop_line_reading_msg)
if stop_line_reading_msg.at_stop_line:
msg = BoolStamped()
msg.header.stamp = stop_line_reading_msg.header.stamp
msg.data = True
self.pub_at_stop_line.publish(msg)
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
): #Process segment list and decide whether there is a stop line.
if not self.active or self.sleep:
return
good_seg_count = 0
stop_line_x_accumulator = 0.0
stop_line_y_accumulator = 0.0
for segment in segment_list_msg.segments:
if segment.color != segment.RED:
continue
if segment.points[0].x < 0 or segment.points[
1].x < 0: # the point is behind us
continue
###Transform segment list to lane pose frame
p1_lane = self.to_lane_frame(segment.points[0])
p2_lane = self.to_lane_frame(segment.points[1])
###Find average x and y of the red line segment
avg_x = 0.5 * (p1_lane[0] + p2_lane[0])
avg_y = 0.5 * (p1_lane[1] + p2_lane[1])
stop_line_x_accumulator += avg_x
stop_line_y_accumulator += avg_y # TODO output covariance and not just mean
good_seg_count += 1.0
stop_line_reading_msg = StopLineReading()
stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
if (
good_seg_count < self.min_segs
): #If red line segment is lower that threshold, then, there is no stop line
stop_line_reading_msg.stop_line_detected = False
stop_line_reading_msg.at_stop_line = False
self.pub_stop_line_reading.publish(stop_line_reading_msg)
return
else:
stop_line_reading_msg.stop_line_detected = True
stop_line_point = Point()
stop_line_point.x = stop_line_x_accumulator / good_seg_count
stop_line_point.y = stop_line_y_accumulator / good_seg_count
stop_line_reading_msg.stop_line_point = stop_line_point
stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs(
stop_line_point.y) < self.lanewidth / 4
self.pub_stop_line_reading.publish(stop_line_reading_msg)
if stop_line_reading_msg.at_stop_line:
###Task: assign fsm stop line message, if message data is true, then it will trigger stop event
###Problem
msg = BoolStamped() #Declaration
msg.header.stamp = segment_list_msg.header.stamp #Assign segment_list time stamp
msg.data = true #If you want to trigger the event at_stop_line, message data must be "true"
print "Event stop line fiter is triggered ? :" + msg.data
self.pub_at_stop_line.publish(msg)
def processSegments(self, segment_list_msg):
#print ("-*/*/*/*/*&&&&&&&&&&*/*/*/*/-")
if not self.active or self.sleep:
return
good_seg_count = 0
stop_line_x_accumulator = 0.0
stop_line_y_accumulator = 0.0
for segment in segment_list_msg.segments:
if segment.color != segment.RED and segment.color != segment.BLUE:
continue
if segment.points[0].x < 0 or segment.points[
1].x < 0: # the point is behind us
continue
p1_lane = self.to_lane_frame(segment.points[0])
p2_lane = self.to_lane_frame(segment.points[1])
avg_x = 0.5 * (p1_lane[0] + p2_lane[0])
avg_y = 0.5 * (p1_lane[1] + p2_lane[1])
stop_line_x_accumulator += avg_x
stop_line_y_accumulator += avg_y # TODO output covariance and not just mean
good_seg_count += 1.0
stop_line_reading_msg = StopLineReading()
stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
if (good_seg_count < self.min_segs):
stop_line_reading_msg.stop_line_detected = False
stop_line_reading_msg.at_stop_line = False
self.pub_stop_line_reading.publish(stop_line_reading_msg)
return
stop_line_reading_msg.stop_line_detected = True
stop_line_point = Point()
stop_line_point.x = stop_line_x_accumulator / good_seg_count
stop_line_point.y = stop_line_y_accumulator / good_seg_count
stop_line_reading_msg.stop_line_point = stop_line_point
stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs(
stop_line_point.y) < self.lanewidth / 4
self.pub_stop_line_reading.publish(stop_line_reading_msg)
if stop_line_reading_msg.at_stop_line:
msg = BoolStamped()
msg.header.stamp = stop_line_reading_msg.header.stamp
msg.data = True
print "-------at stop line-------"
if segment.color == segment.BLUE:
self.pub_sendmessage = rospy.Publisher(
"/arg2/stop_line_filter_node/send",
BoolStamped,
queue_size=1)
self.pub_sendmessage.publish(msg)
elif segment.color == segment.RED:
self.pub_at_stop_line.publish(msg)
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):
good_seg_count = 0
stop_line_x_accumulator = 0.0
stop_line_y_accumulator = 0.0
for segment in segment_list_msg.segments:
if segment.color != segment.RED:
continue
if segment.points[0].x < 0 or segment.points[
1].x < 0: # the point is behind us
continue
if not self.active or self.sleep:
continue
p1_lane = self.to_lane_frame(segment.points[0])
p2_lane = self.to_lane_frame(segment.points[1])
avg_x = 0.5 * (p1_lane[0] + p2_lane[0])
avg_y = 0.5 * (p1_lane[1] + p2_lane[1])
stop_line_x_accumulator += avg_x
stop_line_y_accumulator += avg_y # TODO output covariance and not just mean
good_seg_count += 1.0
stop_line_reading_msg = StopLineReading()
stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
if (good_seg_count < self.min_segs):
stop_line_reading_msg.stop_line_detected = False
stop_line_reading_msg.at_stop_line = False
self.pub_stop_line_reading.publish(stop_line_reading_msg)
return
stop_line_reading_msg.stop_line_detected = True
stop_line_point = Point()
stop_line_point.x = stop_line_x_accumulator / good_seg_count
stop_line_point.y = stop_line_y_accumulator / good_seg_count
stop_line_reading_msg.stop_line_point = stop_line_point
stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs(
stop_line_point.y) < self.lanewidth / 4
self.pub_stop_line_reading.publish(stop_line_reading_msg)
if stop_line_reading_msg.at_stop_line:
msg = BoolStamped()
msg.header.stamp = stop_line_reading_msg.header.stamp
msg.data = True
self.pub_at_stop_line.publish(msg)
else:
msg = BoolStamped()
msg.header.stamp = stop_line_reading_msg.header.stamp
msg.data = False
self.pub_at_stop_line.publish(msg)
def processSegments(self, segment_list_msg):
good_seg_count=0
stop_line_x_accumulator=0.0
stop_line_y_accumulator=0.0
for segment in segment_list_msg.segments:
if segment.color != segment.RED:
continue
if segment.points[0].x < 0 or segment.points[1].x < 0: # the point is behind us
continue
if not self.active or self.sleep:
continue
p1_lane = self.to_lane_frame(segment.points[0])
p2_lane = self.to_lane_frame(segment.points[1])
avg_x = 0.5*(p1_lane[0] + p2_lane[0])
avg_y = 0.5*(p1_lane[1] + p2_lane[1])
stop_line_x_accumulator += avg_x
stop_line_y_accumulator += avg_y # TODO output covariance and not just mean
good_seg_count += 1.0
stop_line_reading_msg = StopLineReading()
stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
if (good_seg_count < self.min_segs):
stop_line_reading_msg.stop_line_detected = False
stop_line_reading_msg.at_stop_line = False
self.pub_stop_line_reading.publish(stop_line_reading_msg)
return
stop_line_reading_msg.stop_line_detected = True
stop_line_point = Point()
stop_line_point.x = stop_line_x_accumulator/good_seg_count
stop_line_point.y = stop_line_y_accumulator/good_seg_count
stop_line_reading_msg.stop_line_point = stop_line_point
stop_line_reading_msg.at_stop_line = stop_line_point.x < self.stop_distance and math.fabs(stop_line_point.y) < self.lanewidth/4
self.pub_stop_line_reading.publish(stop_line_reading_msg)
if stop_line_reading_msg.at_stop_line:
msg = BoolStamped()
msg.header.stamp = stop_line_reading_msg.header.stamp
msg.data = True
self.pub_at_stop_line.publish(msg)
else:
msg = BoolStamped()
msg.header.stamp = stop_line_reading_msg.header.stamp
msg.data = False
self.pub_at_stop_line.publish(msg)
