def getOutputMsg(self, gate_name, inputs):
bool_list = list()
latest_time_stamp = rospy.Time(0)
for event_name, event_msg in self.event_msg_dict.items():
if event_msg is None:
return None
if event_name in inputs:
if (event_msg.data == self.event_trigger_dict[event_name]):
bool_list.append(True)
else:
bool_list.append(False)
# Keeps track of latest timestamp
if event_msg.header.stamp >= latest_time_stamp:
latest_time_stamp = event_msg.header.stamp
# Perform logic operation
msg = BoolStamped()
msg.header.stamp = latest_time_stamp
gate = self.gates_dict.get(gate_name)
gate_type = gate.get("gate_type")
if gate_type == "AND":
msg.data = all(bool_list)
elif gate_type == "OR":
msg.data = any(bool_list)
print bool_list
return msg
def processImage(self, image_msg): if self.lock.testandset(): vehicle_detected_msg_out = BoolStamped() try: image_cv=self.bridge.imgmsg_to_cv2(image_msg,"bgr8") except CvBridgeError as e: print e start = rospy.Time.now() params = cv2.SimpleBlobDetector_Params() params.minArea = self.blobdetector_min_area params.minDistBetweenBlobs = self.blobdetector_min_dist_between_blobs simple_blob_detector = cv2.SimpleBlobDetector(params) (detection, corners) = cv2.findCirclesGrid(image_cv, self.circlepattern_dims, flags=cv2.CALIB_CB_SYMMETRIC_GRID, blobDetector=simple_blob_detector) elapsed_time = (rospy.Time.now() - start).to_sec() self.pub_time_elapsed.publish(elapsed_time) vehicle_detected_msg_out.data = detection self.pub_detection.publish(vehicle_detected_msg_out) if self.publish_circles: cv2.drawChessboardCorners(image_cv, self.circlepattern_dims, corners, detection) image_msg_out = self.bridge.cv2_to_imgmsg(image_cv, "bgr8") self.pub_circlepattern_image.publish(image_msg_out) self.lock.unlock()
def publishDoneMsg(self):
if self.mode == "INTERSECTION_CONTROL":
msg = BoolStamped()
msg.header.stamp = rospy.Time.now()
msg.data = True
self.pub_done.publish(msg)
rospy.loginfo("[%s] interesction_done!" %(self.node_name))
def toSegmentMsg(self, lines, normals, color):
arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff))
arr_ratio = np.array((1./self.image_size[1], 1./self.image_size[0], 1./self.image_size[1], 1./self.image_size[0]))
segmentMsgList = []
for x1,y1,x2,y2,norm_x,norm_y in np.hstack((lines,normals)):
ox= int((x1+x2)/2)
oy= int((y1+y2)/2)
cx = (ox+3*norm_x).astype('int')
cy = (oy+3*norm_y).astype('int')
ccx = (ox-3*norm_x).astype('int')
ccy = (oy-3*norm_y).astype('int')
if cx >158:
cx = 158
elif cx <1:
cx = 1
if ccx >158:
ccx = 158
elif ccx <1:
ccx = 1
if cy >=79:
cy = 79
elif cy <1:
cy = 1
if ccy >=79:
ccy = 79
elif ccy <1:
ccy = 1
if (self.blue[cy, cx] == 255 and self.yellow[ccy,ccx] ==255) or (self.yellow[cy, cx] == 255 and self.blue[ccy,ccx] ==255):
[x1,y1,x2,y2] = (([x1,y1,x2,y2] + arr_cutoff) * arr_ratio)
segment = Segment()
segment.color = color
segment.pixels_normalized[0].x = x1
segment.pixels_normalized[0].y = y1
segment.pixels_normalized[1].x = x2
segment.pixels_normalized[1].y = y2
segment.normal.x = norm_x
segment.normal.y = norm_y
segmentMsgList.append(segment)
if self.time_switch == True:
msgg = BoolStamped()
msgg.data = True
self.pub_lane.publish(msgg)
self.time_switch = False
self.count = 0
return segmentMsgList
def pub_confrim_msg(self):
# Finish planning, set fsm to next state
finish_msg = BoolStamped()
finish_msg.header.stamp = rospy.Time.now()
finish_msg.data = True
self.pub_confrim.publish(finish_msg)
# set status buffer
self.move_ready = False
self.path_ready = False
def updateActuatorLimits(self, msg_actuator_limits):
self.actuator_limits = msg_actuator_limits
rospy.logdebug("actuator limits updated to: ")
rospy.logdebug("actuator_limits.v: " + str(self.actuator_limits.v))
rospy.logdebug("actuator_limits.omega: " +
str(self.actuator_limits.omega))
msg_actuator_limits_received = BoolStamped()
msg_actuator_limits_received.data = True
self.pub_actuator_limits_received.publish(msg_actuator_limits_received)
def callback(self, data):
vehicle_too_close = BoolStamped()
vehicle_too_close.header.stamp = data.header.stamp
if not data.data:
vehicle_too_close.data = False
else:
vehicle_too_close.data = True
self.publishCmd(data.header.stamp)
self.vehicle_detected_pub.publish(vehicle_too_close)
def processImage(self, image_msg):
if not self.active:
return
now = rospy.Time.now()
if now - self.last_stamp < self.publish_duration:
return
else:
self.last_stamp = now
vehicle_detected_msg_out = BoolStamped()
vehicle_corners_msg_out = VehicleCorners()
try:
image_cv = self.bridge.compressed_imgmsg_to_cv2(
image_msg, "bgr8")
except CvBridgeError as e:
print e
start = rospy.Time.now()
params = cv2.SimpleBlobDetector_Params()
params.minArea = self.blobdetector_min_area
params.minDistBetweenBlobs = self.blobdetector_min_dist_between_blobs
simple_blob_detector = cv2.SimpleBlobDetector_create(params)
(detection, corners) = cv2.findCirclesGrid(image_cv,
self.circlepattern_dims, flags=cv2.CALIB_CB_SYMMETRIC_GRID,
blobDetector=simple_blob_detector)
# print(corners)
vehicle_detected_msg_out.data = detection
self.pub_detection.publish(vehicle_detected_msg_out)
if detection:
# print(corners)
points_list = []
for point in corners:
corner = Point32()
# print(point[0])
corner.x = point[0, 0]
# print(point[0,1])
corner.y = point[0, 1]
corner.z = 0
points_list.append(corner)
vehicle_corners_msg_out.header.stamp = rospy.Time.now()
vehicle_corners_msg_out.corners = points_list
vehicle_corners_msg_out.detection.data = detection
vehicle_corners_msg_out.H = self.circlepattern_dims[1]
vehicle_corners_msg_out.W = self.circlepattern_dims[0]
self.pub_corners.publish(vehicle_corners_msg_out)
elapsed_time = (rospy.Time.now() - start).to_sec()
self.pub_time_elapsed.publish(elapsed_time)
if self.publish_circles:
cv2.drawChessboardCorners(image_cv,
self.circlepattern_dims, corners, detection)
image_msg_out = self.bridge.cv2_to_imgmsg(image_cv, "bgr8")
self.pub_circlepattern_image.publish(image_msg_out)
def cb_image(self, image_msg):
"""
Callback for processing a image which potentially contains a back pattern. Processes the image only if
sufficient time has passed since processing the previous image (relative to the chosen processing frequency).
The pattern detection is performed using OpenCV's `findCirclesGrid <https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=solvepnp#findcirclesgrid>`_ function.
Args:
image_msg (:obj:`sensor_msgs.msg.CompressedImage`): Input image
"""
now = rospy.Time.now()
if now - self.last_stamp < self.publish_duration:
return
else:
self.last_stamp = now
vehicle_centers_msg_out = VehicleCorners()
detection_flag_msg_out = BoolStamped()
image_cv = self.bridge.compressed_imgmsg_to_cv2(image_msg, "bgr8")
(detection, centers) = cv2.findCirclesGrid(image_cv,
patternSize=tuple(self.circlepattern_dims.value),
flags=cv2.CALIB_CB_SYMMETRIC_GRID,
blobDetector=self.simple_blob_detector)
# if the pattern is detected, cv2.findCirclesGrid returns a non-zero result, otherwise it returns 0
# vehicle_detected_msg_out.data = detection > 0
# self.pub_detection.publish(vehicle_detected_msg_out)
vehicle_centers_msg_out.header = image_msg.header
vehicle_centers_msg_out.detection.data = detection > 0
detection_flag_msg_out.header = image_msg.header
detection_flag_msg_out.data = detection > 0
# if the detection is successful add the information about it,
# otherwise publish a message saying that it was unsuccessful
if detection > 0:
points_list = []
for point in centers:
center = Point32()
center.x = point[0, 0]
center.y = point[0, 1]
center.z = 0
points_list.append(center)
vehicle_centers_msg_out.corners = points_list
vehicle_centers_msg_out.H = self.circlepattern_dims.value[1]
vehicle_centers_msg_out.W = self.circlepattern_dims.value[0]
self.pub_centers.publish(vehicle_centers_msg_out)
self.pub_detection_flag.publish(detection_flag_msg_out)
if self.pub_circlepattern_image.get_num_connections() > 0:
cv2.drawChessboardCorners(image_cv,
tuple(self.circlepattern_dims.value), centers, detection)
image_msg_out = self.bridge.cv2_to_compressed_imgmsg(image_cv)
self.pub_circlepattern_image.publish(image_msg_out)
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " % (self.node_name))
self.veh_name = self.node_name.split("/")[1]
self.joy = None
self.last_pub_msg = None
self.last_pub_time = rospy.Time.now()
# Setup Parameters
self.v_gain = self.setupParam("~speed_gain", 2.41)
self.omega_gain = self.setupParam("~steer_gain", 8.3)
self.bicycle_kinematics = self.setupParam("~bicycle_kinematics", 0)
self.steer_angle_gain = self.setupParam("~steer_angle_gain", 1)
self.simulated_vehicle_length = self.setupParam(
"~simulated_vehicle_length", 0.18)
# Publications
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1)
self.pub_joy_override = rospy.Publisher("~joystick_override",
BoolStamped,
queue_size=1)
self.pub_parallel_autonomy = rospy.Publisher("~parallel_autonomy",
BoolStamped,
queue_size=1)
self.pub_anti_instagram = rospy.Publisher("anti_instagram_node/click",
BoolStamped,
queue_size=1)
self.pub_e_stop = rospy.Publisher("wheels_driver_node/emergency_stop",
BoolStamped,
queue_size=1)
self.pub_avoidance = rospy.Publisher("~start_avoidance",
BoolStamped,
queue_size=1)
self.pub_speed_sate = rospy.Publisher("~speed_state",
JoystickSpeedStamped,
queue_size=1)
# Subscriptions
self.sub_joy_ = rospy.Subscriber("joy", Joy, self.cbJoy, queue_size=1)
# timer
# self.pub_timer = rospy.Timer(rospy.Duration.from_sec(self.pub_timestep),self.publishControl)
self.param_timer = rospy.Timer(rospy.Duration.from_sec(1.0),
self.cbParamTimer)
self.has_complained = False
self.state_parallel_autonomy = False
self.state_verbose = False
self.pub_speed_sate.speed_state = 0
pub_msg = BoolStamped()
pub_msg.data = self.state_parallel_autonomy
pub_msg.header.stamp = self.last_pub_time
self.pub_parallel_autonomy.publish(pub_msg)
def processTimer(self, msgg): if self.active: # fsm switch on self.count_time() if not self.active: # fsm switch off # publish timer is off and reset timer state to not starting msg = BoolStamped() msg.data = False self.pub_time_is_up.publish(msg) self.start = False
def checkProtocol(self, tag_id):
# A fake protocol for test
if tag_id == 1:
task_success = True
else:
task_success = False
coord_msg = BoolStamped()
coord_msg.header.stamp = rospy.Time.now()
coord_msg.data = task_success
self.pub_coord.publish(coord_msg)
def callback(self, msg): if(self.active.data): self.id = msg self.car_cmd() self.stop() finish = BoolStamped() finish.data = True self.pub_finish_turn.publish(finish) else: return
def car_read_action(self):
while True:
strhttp = 'http://192.168.0.100/tsp/read_car_action.php?car_id=1'
req = urllib2.Request(strhttp)
response = urllib2.urlopen(req)
the_page = response.read()
if (self.car_action_check == 1 and int(the_page) == 0
and self.last_commodity_tag != 0):
self.car_action_check = int(the_page)
e_stop_msg = BoolStamped()
e_stop_msg.data = int(the_page)
self.pub_lanefollowing.publish(e_stop_msg)
if self.last_action == "B":
stop_line_reading_msg = StopLineReading()
stop_line_reading_msg.stop_line_detected = True
stop_line_reading_msg.at_stop_line = 1
self.pub_stop_line_reading.publish(stop_line_reading_msg)
stop_msg = BoolStamped()
stop_msg.data = True
self.pub_at_stop_back.publish(stop_msg)
time.sleep(1)
self.turn = 3
self.type_back.publish(self.turn)
self.last_commodity_tag = 0
self.car_action_check = int(the_page)
e_stop_msg = BoolStamped()
e_stop_msg.data = int(the_page)
self.pub_lanefollowing.publish(e_stop_msg)
time.sleep(1)
def processButtons(self, joy_msg):
if (joy_msg.buttons[6] == 1): # The back button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = True
rospy.loginfo('joystick_control = True')
self.joy_control = 1
self.ncs_control = 0
self.pub_joy_override.publish(override_msg)
elif (joy_msg.buttons[7] == 1): # the start button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = False
rospy.loginfo('deep_lanefollowing = True')
self.joy_control = 0
self.ncs_control = 1
self.pub_joy_override.publish(override_msg)
elif (joy_msg.buttons[3] == 1):
anti_instagram_msg = BoolStamped()
anti_instagram_msg.header.stamp = self.joy.header.stamp
anti_instagram_msg.data = True
rospy.loginfo('anti_instagram message')
self.pub_anti_instagram.publish(anti_instagram_msg)
def cb_mode(self, fsm_state_msg):
if fsm_state_msg.state == "INTERSECTION_COORDINATION":
self.log('Setting up for intersection')
rospy.sleep(self.stop_time)
self.reset()
# For this task other duckiebots on the same road are excluded so it
# just waits 2 seconds then this node gives the GO signal
if self.parameters['~demo']:
go_msg = BoolStamped()
go_msg.data = True
self.intersection_go.publish(go_msg)
def publishControl(self):
for b, state in self.button2command.items():
if self.joy.buttons[b] == 1:
if self.state == state:
#self.forward()
go_msg = BoolStamped()
go_msg.header.stamp = self.joy.header.stamp
go_msg.data = True
self.pub_go.publish(go_msg)
rospy.loginfo("Publishing forward command")
def processTimer(self, msgg): if self.active: # fsm switch on if self.state == "LANE_RECOVERY": self.lane_recovery() if not self.active: # fsm switch off # publish timer is off and reset timer state to not starting msg = BoolStamped() msg.data = False self.pub_time_is_up.publish(msg) self.start = False
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 cbNextmove(self, msg):
if not self.active:
return
# Pop next move
try:
# If we can pop another move
self.move = self.moves.pop()
except:
# Cannot pop another move, reach destination
reach_msg = BoolStamped()
reach_msg.header.stamp = rospy.Time.now()
reach_msg.data = True
def car_cmd_pub(self,stop_time): car_control_msg = Twist2DStamped() car_control_msg.v = 0.0 car_control_msg.omega = 0.0 self.pub_car_cmd.publish(car_control_msg) print "**************** wait for ",stop_time," sec ****************\n" time.sleep(stop_time) #print "**************** stop time finished ****************" msg = BoolStamped() msg.data = True self.pub_lane_recovery.publish(msg)
def cbFSMState(self, msg):
if (not self.mode == "CALIBRATING") and msg.state == "CALIBRATING":
# Switch into CALIBRATING mode
self.mode = msg.state
self.stopped = False
rospy.loginfo("[%s] %s triggered." % (self.node_name, self.mode))
#Node shuts itself down after the calculations are complete
if self.calib_done:
done = BoolStamped()
done.data = True
self.pub_stop.publish(done)
self.mode = msg.state
def getOutputMsg(self, gate_name, inputs):
bool_list = list()
latest_time_stamp = rospy.Time(0)
for event_name, event_msg in list(self.event_msg_dict.items()):
if event_name in inputs: # one of the inputs to gate
if self.debugging:
print(("sub-event: " + event_name))
if event_msg is None:
if "default" in self.events_dict[event_name]:
bool_list.append(
self.events_dict[event_name]["default"])
else:
bool_list.append(False)
else:
if "field" in self.events_dict[
event_name]: # if special type of message
if (getattr(event_msg,
self.events_dict[event_name]["field"]) ==
self.event_trigger_dict[event_name]):
bool_list.append(True)
else:
bool_list.append(False)
else: # else BoolStamped
if event_msg.data == self.event_trigger_dict[
event_name]:
bool_list.append(True)
else:
bool_list.append(False)
# Keeps track of latest timestamp
if event_msg.header.stamp >= latest_time_stamp:
latest_time_stamp = event_msg.header.stamp
# Perform logic operation
msg = BoolStamped()
msg.header.stamp = latest_time_stamp
gate = self.gates_dict.get(gate_name)
gate_type = gate.get("gate_type")
if gate_type == "AND":
msg.data = all(bool_list)
elif gate_type == "OR":
msg.data = any(bool_list)
if self.debugging:
print((bool_list, "->", msg.data))
# print bool_list, msg.data
return msg
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 processImage(self, image_msg): if self.lock.testandset(): #mulai = rospy.Time.now() vehicle_detected_msg_out = BoolStamped() distance_value = Twist2DStamped() try: image_cv=self.bridge.compressed_imgmsg_to_cv2(image_msg,"bgr8") except CvBridgeError as e: print e start = rospy.Time.now() params = cv2.SimpleBlobDetector_Params() params.minArea = self.blobdetector_min_area params.minDistBetweenBlobs = self.blobdetector_min_dist_between_blobs simple_blob_detector = cv2.SimpleBlobDetector_create(params) (detection, corners) = cv2.findCirclesGrid(image_cv, self.circlepattern_dims, flags=cv2.CALIB_CB_SYMMETRIC_GRID, blobDetector=simple_blob_detector) if detection == True: a = (corners[1][0][0])-(corners[0][0][0]) b = (corners[2][0][0])-(corners[1][0][0]) c = (corners[3][0][0])-(corners[2][0][0]) d = (corners[4][0][0])-(corners[3][0][0]) e = (corners[5][0][0])-(corners[4][0][0]) f = (corners[6][0][0])-(corners[5][0][0]) x = a+b+c+d+e+f m = x/6 distance = (1/m)*431.8993525-1.884667361 distance_value.v = distance self.pub_distance.publish(distance_value) elapsed_time = (rospy.Time.now() - start).to_sec() self.pub_time_elapsed.publish(elapsed_time) vehicle_detected_msg_out.data = detection self.pub_detection.publish(vehicle_detected_msg_out) else: elapsed_time = (rospy.Time.now() - start).to_sec() self.pub_time_elapsed.publish(elapsed_time) vehicle_detected_msg_out.data = detection self.pub_detection.publish(vehicle_detected_msg_out) if self.publish_circles: cv2.drawChessboardCorners(image_cv, self.circlepattern_dims, corners, detection) image_msg_out = self.bridge.cv2_to_compressed_imgmsg(image_cv, dst_format='jpeg') self.pub_circlepattern_image.publish(image_msg_out) #selang_waktu = (rospy.Time.now() - mulai).to_sec() #print selang_waktu, 'waktu proses deteksi jarak' self.lock.unlock()
def callback(self, data):
vehicle_detected_msg_out = BoolStamped()
vehicle_detected_msg_out.header.stamp = data.header.stamp
vehicle_detected_msg_out.data = data.data
self.vehicle_detected_pub.publish(vehicle_detected_msg_out)
self.detection_prev = self.detection
self.detection = data.data
if not data.data:
#self.v_gain = 1
#self.P = 0
self.I = 0
def remote_steering_complete_cb(self, data):
# this should clear the block incomming flag
if data.data == True:
# allow lf override msgs
self.blockLfOverrideMsg = False
# publish message to start lane follow
override_msg = BoolStamped()
override_msg.header.stamp = data.header.stamp
override_msg.data = data.data
self.pub_joy_override.publish(override_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 on_parameters_changed(self, first_time, _updated):
if first_time:
self.has_complained = False
self.state_parallel_autonomy = False
self.state_verbose = False
pub_msg = BoolStamped()
pub_msg.data = self.state_parallel_autonomy
pub_msg.header.stamp = self.last_pub_time
self.publishers.parallel_autonomy.publish(pub_msg)
else:
pass
def lane_recovery(self): if not self.start: # if timer not start yet self.timer_start = time.time() # record start time self.start = True # change timer state to start print "start time: ", self.timer_start self.timer_end = time.time() # record time now print "time: ", self.timer_end - self.timer_start if (self.timer_end - self.timer_start) > 1: #if time duration between start time and time now bigger than 2 seconsds # publish time is up msg = BoolStamped() msg.data = True self.pub_time_is_up.publish(msg) print "time is up"
def pose(data):
global min_dist_lane, time_out, flag, pub
dis = data.position.z
if dis > min_dist_lane:
rospy.loginfo('Tag 1 is still far away enough: %s m', dis)
else:
rospy.loginfo('Tag 1 is too close: %s m', dis)
time_out = 0
flag = True
b = BoolStamped()
b.data = True
pub.publish(b)
def cbIntersectionGo(self, msg):
if not self.active:
return
if not msg.data: return
while self.turn_type == -1:
if not self.active:
return
rospy.loginfo("Requested to start intersection, but we do not see an april tag yet.")
rospy.sleep(2)
tag_id = self.tag_id
turn_type = self.turn_type
sleeptimes = [self.time_left_turn, self.time_straight_turn, self.time_right_turn]
LFparams = [self.LFparams_left, self.LFparams_straight, self.LFparams_right]
omega_ffs = [self.ff_left, self.ff_straight, self.ff_right]
omega_maxs = [self.omega_max_left, self.omega_max_straight, self.omega_max_right]
omega_mins = [self.omega_min_left, self.omega_min_straight, self.omega_min_right]
self.changeLFParams(LFparams[turn_type], sleeptimes[turn_type]+1.0)
rospy.set_param("~lane_controller/omega_ff", omega_ffs[turn_type])
rospy.set_param("~lane_controller/omega_max", omega_maxs[turn_type])
rospy.set_param("~lane_controller/omega_min", omega_mins[turn_type])
# Waiting for LF to adapt to new params
rospy.sleep(1)
rospy.loginfo("Starting intersection control - driving to " + str(turn_type))
self.forward_pose = True
rospy.sleep(sleeptimes[turn_type])
self.forward_pose = False
rospy.set_param("~lane_controller/omega_ff", 0)
rospy.set_param("~lane_controller/omega_max", 999)
rospy.set_param("~lane_controller/omega_min", -999)
# Publish intersection done
msg_done = BoolStamped()
msg_done.data = True
self.pub_int_done.publish(msg_done)
# Publish intersection done detailed
msg_done_detailed = TurnIDandType()
msg_done_detailed.tag_id = tag_id
msg_done_detailed.turn_type = turn_type
self.pub_int_done_detailed.publish(msg_done_detailed)
def image_cb(self, image_msg):
if not self.initialized:
return
if self.frame_id != 0:
return
self.frame_id += 1
self.frame_id = self.frame_id % (1 + NUMBER_FRAMES_SKIPPED())
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"],
image
)
image = cv2.resize(image, (416,416))
bboxes, classes, scores = self.model_wrapper.predict(image)
msg = BoolStamped()
msg.header = image_msg.header
msg.data = self.det2bool(bboxes, classes, scores)
self.pub_obj_dets.publish(msg)
if self._debug:
colors = {0: (0, 255, 255), 1: (0, 165, 255), 2: (0, 250, 0), 3: (0, 0, 255)}
names = {0: "duckie", 1: "cone", 2: "truck", 3: "bus"}
font = cv2.FONT_HERSHEY_SIMPLEX
for clas, box in zip(classes, bboxes):
pt1 = np.array([int(box[0]), int(box[1])])
pt2 = np.array([int(box[2]), int(box[3])])
pt1 = tuple(pt1)
pt2 = tuple(pt2)
color = colors[clas.item()]
name = names[clas.item()]
image = cv2.rectangle(image, pt1, pt2, color, 2)
text_location = (pt1[0], min(416, pt1[1]+20))
image = cv2.putText(image, name, text_location, font, 1, color, thickness=3)
obj_det_img = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")
self.pub_detections_image.publish(obj_det_img)
def publish_topics(self):
now = rospy.Time.now()
self.clearance_to_go_pub.publish(CoordinationClearance(status=self.clearance_to_go))
# Publish intersection_go flag
if (self.clearance_to_go == CoordinationClearance.GO):
msg = BoolStamped()
msg.header.stamp = now
msg.data = True
self.pub_intersection_go.publish(msg)
# TODO: publish intersection go only once.
self.roof_light_pub.publish(CoordinationSignal(signal=self.roof_light))
car_cmd_msg = Twist2DStamped(v=0.0,omega=0.0)
car_cmd_msg.header.stamp = now
self.pub_coord_cmd.publish(car_cmd_msg)
def relative_pose(self, pose, track, tangent_angle, curvature):
'''
This function uses the current position of the duckiebot in the intersection-stop-line frame to find the closest
distance to the optimal trajectory and the angle difference between the closest point of the optimal trajectory
and current orientation of the duckiebot.
:param pose: current position of the duckiebot in the intersection-stop-line frame
:return: distance d and angle phi compared to the optimal trajectory --> same as for line following
'''
position, orientation = utils.pose_to_tuple(pose.pose)
_, _, yaw = euler_from_quaternion(orientation)
car_pos = np.array(position[0:2])
idx_min_dist, min_dist = self.closest_track_point(track[:-1], car_pos)
# if the distance from the car position to the end of the optimal trajectory is less than 28 cm
# and the difference between the angle of the car and end of the optimal trajectory is less than 15 degrees
# switch back to lane following
distance_to_end = LA.norm(track[-1, :] - car_pos)
angle_to_end = abs(tangent_angle[-1] - yaw)
end_condition_angle_rad = np.deg2rad(self.end_condition_angle_deg)
distance_good_enough = distance_to_end < self.end_condition_distance
angle_good_enough = angle_to_end < end_condition_angle_rad
switch = BoolStamped()
switch.header.stamp = rospy.Time(0)
switch.data = distance_good_enough and angle_good_enough
self.pub_switch2lanefollow.publish(switch)
if switch.data:
self.log("SWITCH BACK TO LANE FOLLOWING!!")
# publish lane pose msg
closest_pos = track[idx_min_dist]
closest_angle = tangent_angle[idx_min_dist]
next_pos = track[idx_min_dist + 1]
if self.verbose:
self.log("distance to end: {}".format(distance_to_end))
self.log("angle to end: {}".format(angle_to_end))
self.publish_closest(closest_pos, car_pos)
side = self.track_side(closest_pos, next_pos, car_pos)
d = -min_dist * side
phi = yaw - closest_angle
curv = curvature[idx_min_dist]
return d, phi, curv
def joy_cb(self, joy_msg):
"""
Callback that process the received :obj:`Joy` messages.
Args:
joy_msg (:obj:`Joy`): the joystick message to process.
"""
# Navigation buttons
car_cmd_msg = Twist2DStamped()
car_cmd_msg.header.stamp = rospy.get_rostime()
# Left stick V-axis. Up is positive
car_cmd_msg.v = joy_msg.axes[1] * self._speed_gain
if self._bicycle_kinematics:
# Implements Bicycle Kinematics - Nonholonomic Kinematics
# see https://inst.eecs.berkeley.edu/~ee192/sp13/pdf/steer-control.pdf
steering_angle = joy_msg.axes[3] * self._steer_gain
car_cmd_msg.omega = \
car_cmd_msg.v / self._simulated_vehicle_length * \
math.tan(steering_angle)
else:
# Holonomic Kinematics for Normal Driving
car_cmd_msg.omega = joy_msg.axes[3] * self._steer_gain
self.pub_car_cmd.publish(car_cmd_msg)
# Back button: Stop LF
if joy_msg.buttons[6] == 1:
override_msg = BoolStamped()
override_msg.header.stamp = joy_msg.header.stamp
override_msg.data = True
self.log('override_msg = True')
self.pub_joy_override.publish(override_msg)
# Start button: Start LF
elif joy_msg.buttons[7] == 1:
override_msg = BoolStamped()
override_msg.header.stamp = joy_msg.header.stamp
override_msg.data = False
self.log('override_msg = False')
self.pub_joy_override.publish(override_msg)
# Y button: Emergency Stop
elif joy_msg.buttons[3] == 1:
self.e_stop = not self.e_stop
estop_msg = BoolStamped()
estop_msg.header.stamp = joy_msg.header.stamp
estop_msg.data = self.e_stop
self.pub_e_stop.publish(estop_msg)
else:
some_active = sum(joy_msg.buttons) > 0
if some_active:
self.logwarn('No binding for joy_msg.buttons = %s' %
str(joy_msg.buttons))
def publishBools(self):
msg = BoolStamped()
msg.header.stamp = self.state_msg.header.stamp
active_nodes = self._getActiveNodesOfState(self.state_msg.state)
for node_name, node_pub in self.pub_dict.items():
if self.active_nodes is not None:
if (node_name in active_nodes) == (node_name in self.active_nodes):
# Skip publishing if hasn't changed
continue
msg.data = bool(node_name in active_nodes)
node_state = "ON" if msg.data else "OFF"
node_pub.publish(msg)
rospy.loginfo("[%s] Node %s set to %s." %(self.node_name, node_name, node_state))
# Save current active nodes
self.active_nodes = copy.deepcopy(active_nodes)
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 publishBools(self):
active_nodes = self._getActiveNodesOfState(self.state_msg.state)
for node_name, node_pub in self.pub_dict.items():
msg = BoolStamped()
msg.header.stamp = self.state_msg.header.stamp
msg.data = bool(node_name in active_nodes)
node_state = "ON" if msg.data else "OFF"
rospy.loginfo("[%s] Node %s is %s in %s" %(self.node_name, node_name, node_state, self.state_msg.state))
if self.active_nodes is not None:
if (node_name in active_nodes) == (node_name in self.active_nodes):
continue
# else:
# rospy.logwarn("[%s] self.active_nodes is None!" %(self.node_name))
# continue
node_pub.publish(msg)
# rospy.loginfo("[%s] node %s msg %s" %(self.node_name, node_name, msg))
# rospy.loginfo("[%s] Node %s set to %s." %(self.node_name, node_name, node_state))
self.active_nodes = copy.deepcopy(active_nodes)
def getOutputMsg(self): bool_list = list() latest_time_stamp = rospy.Time(0) for input_name, input_msg in self.input_msg_dict.items(): if input_msg is None: return None bool_list.append(input_msg.data) # Keeps track of latest timestamp if input_msg.header.stamp >= latest_time_stamp: latest_time_stamp = input_msg.header.stamp # Perform logic operation msg = BoolStamped() msg.header.stamp = latest_time_stamp if self.gate_type == "AND": msg.data = all(bool_list) elif self.gate_type == "OR": msg.data = any(bool_list) return msg
def cbMode(self, mode_msg):
self.fsm_mode = mode_msg.state
if (self.fsm_mode == "INTERSECTION_CONTROL"):
# self.availableTurns = [0,1,2] # just to test without april tags, set to [] for actual
# brake lights
self.led.setRGB(3, [1, 0, 0])
self.led.setRGB(1, [1, 0, 0])
# force to stop for 2 seconds
rospy.sleep(2)
# default to straight if nothing pressed
self.turn_direction = self.turn_NONE
# if no straight turn avaliable wait until another is choosen
# publish once user presses forward on joystick
time_max = rospy.get_time() + self.timeout
while (self.turn_direction == self.turn_NONE or not self.joy_forward > 0) and not (rospy.get_time() > time_max and self.availableTurns ==[]):
pass
# turn off brake lights
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(3, [.3, 0, 0])
if self.availableTurns == []:
#if timeout and no available turns, just leave instersection control
done = BoolStamped()
done.header.stamp = rospy.Time.now()
done.data = True
self.pub_done.publish(done)
else:
self.pub_turn_type.publish(self.turn_direction)
rospy.loginfo("[%s] Turn type: %i" % (self.node_name, self.turn_direction))
if self.fsm_mode != "INTERSECTION_CONTROL":
# on exit intersection control, stop blinking
self.availableTurns = []
self.turn_direction = self.turn_NONE
# led setup
self.led.setRGB(4, [.2, .2, .2])
self.led.setRGB(2, [.2, .2, .2])
self.led.setRGB(3, [.3, 0, 0])
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(0, [.2, .2, .2])
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " %(self.node_name))
self.joy = None
self.last_pub_msg = None
self.last_pub_time = rospy.Time.now()
# Setup Parameters
self.v_gain = self.setupParam("~speed_gain", 0.41)
self.omega_gain = self.setupParam("~steer_gain", 8.3)
self.bicycle_kinematics = self.setupParam("~bicycle_kinematics", 0)
self.steer_angle_gain = self.setupParam("~steer_angle_gain", 1)
self.simulated_vehicle_length = self.setupParam("~simulated_vehicle_length", 0.18)
# Publications
self.pub_car_cmd = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1)
self.pub_joy_override = rospy.Publisher("~joystick_override", BoolStamped, queue_size=1)
self.pub_parallel_autonomy = rospy.Publisher("~parallel_autonomy",BoolStamped, queue_size=1)
self.pub_anti_instagram = rospy.Publisher("anti_instagram_node/click",BoolStamped, queue_size=1)
self.pub_e_stop = rospy.Publisher("wheels_driver_node/emergency_stop",BoolStamped,queue_size=1)
self.pub_avoidance = rospy.Publisher("~start_avoidance",BoolStamped,queue_size=1)
# Subscriptions
self.sub_joy_ = rospy.Subscriber("joy", Joy, self.cbJoy, queue_size=1)
# timer
# self.pub_timer = rospy.Timer(rospy.Duration.from_sec(self.pub_timestep),self.publishControl)
self.param_timer = rospy.Timer(rospy.Duration.from_sec(1.0),self.cbParamTimer)
self.has_complained = False
self.state_parallel_autonomy = False
self.state_verbose = False
pub_msg = BoolStamped()
pub_msg.data = self.state_parallel_autonomy
pub_msg.header.stamp = self.last_pub_time
self.pub_parallel_autonomy.publish(pub_msg)
def processTimer(self, msg):
if self.active == True:
print "##################1. fsm state is " + `self.state`
if self.i ==0:
if self.state == "STOP":
self.timer_start = time.time()
self.i = 1
print "####################Timer start######################"
self.timer_end = time.time()
if (self.timer_end - self.timer_start) > 5:
print "####################Timer End##########################"
msg = BoolStamped()
msg.data = True
self.pub_time_is_up.publish(msg)
if not self.active:
self.timer_start = time.time()
msg = BoolStamped()
msg.data = False
self.pub_time_is_up.publish(msg)
self.i = 0
def processButtons(self, joy_msg):
if (joy_msg.buttons[6] == 1): #The back button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = True
self.pub_joy_override.publish(override_msg)
elif (joy_msg.buttons[7] == 1): #the start button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = False
self.pub_joy_override.publish(override_msg)
elif (joy_msg.buttons[5] == 1): # Right back button
self.state_verbose ^= True
rospy.loginfo('state_verbose = %s' % self.state_verbose)
rospy.set_param('line_detector_node/verbose', self.state_verbose)
elif (joy_msg.buttons[4] == 1): #Left back button
self.state_parallel_autonomy ^= True
rospy.loginfo('state_parallel_autonomy = %s' % self.state_parallel_autonomy)
parallel_autonomy_msg = BoolStamped()
parallel_autonomy_msg.header.stamp = self.joy.header.stamp
parallel_autonomy_msg.data = self.state_parallel_autonomy
self.pub_parallel_autonomy.publish(parallel_autonomy_msg)
elif (joy_msg.buttons[3] == 1):
anti_instagram_msg = BoolStamped()
anti_instagram_msg.header.stamp = self.joy.header.stamp
anti_instagram_msg.data = True
self.pub_anti_instagram.publish(anti_instagram_msg)
elif (joy_msg.buttons[8] == 1): #power button (middle)
e_stop_msg = BoolStamped()
e_stop_msg.header.stamp = self.joy.header.stamp
e_stop_msg.data = True # note that this is toggle (actual value doesn't matter)
self.pub_e_stop.publish(e_stop_msg)
else:
some_active = sum(joy_msg.buttons) > 0
if some_active:
rospy.loginfo('No binding for joy_msg.buttons = %s' % str(joy_msg.buttons))
def processButtons(self, joy_msg):
if joy_msg.buttons[6] == 1: # The back button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = True
self.pub_joy_override.publish(override_msg)
elif joy_msg.buttons[7] == 1: # the start button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = False
self.pub_joy_override.publish(override_msg)
elif joy_msg.buttons[5] == 1: # Right back button
self.state_verbose ^= True
rospy.loginfo("state_verbose = %s" % self.state_verbose)
rospy.set_param(
"line_detector_node/verbose", self.state_verbose
) # bad - should be published for all to hear - not set a specific param
elif joy_msg.buttons[4] == 1: # Left back button
self.state_parallel_autonomy ^= True
rospy.loginfo("state_parallel_autonomy = %s" % self.state_parallel_autonomy)
parallel_autonomy_msg = BoolStamped()
parallel_autonomy_msg.header.stamp = self.joy.header.stamp
parallel_autonomy_msg.data = self.state_parallel_autonomy
self.pub_parallel_autonomy.publish(parallel_autonomy_msg)
elif joy_msg.buttons[3] == 1:
anti_instagram_msg = BoolStamped()
anti_instagram_msg.header.stamp = self.joy.header.stamp
anti_instagram_msg.data = True
self.pub_anti_instagram.publish(anti_instagram_msg)
elif joy_msg.buttons[8] == 1: # power button (middle)
e_stop_msg = BoolStamped()
e_stop_msg.header.stamp = self.joy.header.stamp
e_stop_msg.data = True # note that this is toggle (actual value doesn't matter)
self.pub_e_stop.publish(e_stop_msg)
elif joy_msg.buttons[9] == 1: # push left joystick button
avoidance_msg = BoolStamped()
rospy.loginfo("start lane following with avoidance mode")
avoidance_msg.header.stamp = self.joy.header.stamp
avoidance_msg.data = True
self.pub_avoidance.publish(avoidance_msg)
else:
some_active = sum(joy_msg.buttons) > 0
if some_active:
rospy.loginfo("No binding for joy_msg.buttons = %s" % str(joy_msg.buttons))
def processButtons(self, joy_msg):
if (joy_msg.buttons[6] == 1): #The back button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = True
self.pub_joy_override.publish(override_msg)
elif (joy_msg.buttons[7] == 1): #the start button
override_msg = BoolStamped()
override_msg.header.stamp = self.joy.header.stamp
override_msg.data = False
self.pub_joy_override.publish(override_msg)
elif (joy_msg.buttons[5] == 1): # Right back button
self.state_verbose ^= True
rospy.loginfo('state_verbose = %s' % self.state_verbose)
rospy.set_param('line_detector_node/verbose', self.state_verbose) # bad - should be published for all to hear - not set a specific param
elif (joy_msg.buttons[4] == 1): #Left back button
self.state_parallel_autonomy ^= True
rospy.loginfo('state_parallel_autonomy = %s' % self.state_parallel_autonomy)
parallel_autonomy_msg = BoolStamped()
parallel_autonomy_msg.header.stamp = self.joy.header.stamp
parallel_autonomy_msg.data = self.state_parallel_autonomy
self.pub_parallel_autonomy.publish(parallel_autonomy_msg)
elif (joy_msg.buttons[3] == 1):
anti_instagram_msg = BoolStamped()
anti_instagram_msg.header.stamp = self.joy.header.stamp
anti_instagram_msg.data = True
self.pub_anti_instagram.publish(anti_instagram_msg)
elif (joy_msg.buttons[8] == 1): #power button (middle)
e_stop_msg = BoolStamped()
e_stop_msg.header.stamp = self.joy.header.stamp
e_stop_msg.data = True # note that this is toggle (actual value doesn't matter)
self.pub_e_stop.publish(e_stop_msg)
elif (joy_msg.buttons[9] == 1): #push left joystick button
avoidance_msg = BoolStamped()
rospy.loginfo('start lane following with avoidance mode')
avoidance_msg.header.stamp = self.joy.header.stamp
avoidance_msg.data = True
self.pub_avoidance.publish(avoidance_msg)
# new code to run Python file to make duckiebot follow a preplanned map
elif (joy_msg.buttons[0] == 1):
print "the button A was pressed!"
PPMprogram.PPMap()
else:
some_active = sum(joy_msg.buttons) > 0
if some_active:
rospy.loginfo('No binding for joy_msg.buttons = %s' % str(joy_msg.buttons))
def setAprilTagSwitch(self,state):
aprilTagOn = BoolStamped()
aprilTagOn.header.stamp = rospy.Time.now()
aprilTagOn.data = state
self.pub_topic_april_tags_switch.publish(aprilTagOn)
def processImage_(self, image_msg):
self.stats.processed()
if self.intermittent_log_now():
self.intermittent_log(self.stats.info())
self.stats.reset()
tk = TimeKeeper(image_msg)
self.intermittent_counter += 1
# Decode from compressed image with OpenCV
try:
image_cv = image_cv_from_jpg(image_msg.data)
except ValueError as e:
self.loginfo('Could not decode image: %s' % e)
return
tk.completed('decoded')
# Resize and crop image
hei_original, wid_original = image_cv.shape[0:2]
if self.image_size[0] != hei_original or self.image_size[1] != wid_original:
# image_cv = cv2.GaussianBlur(image_cv, (5,5), 2)
image_cv = cv2.resize(image_cv, (self.image_size[1], self.image_size[0]),
interpolation=cv2.INTER_NEAREST)
image_cv = image_cv[self.top_cutoff:,:,:]
tk.completed('resized')
# apply color correction: AntiInstagram
image_cv_corr = self.ai.applyTransform(image_cv)
image_cv_corr = cv2.convertScaleAbs(image_cv_corr)
tk.completed('corrected')
# set up parameter
hsv_blue1 = (100,50,50)
hsv_blue2 = (150,255,255)
hsv_yellow1 = (25,50,50)
hsv_yellow2 = (45,255,255)
# Set the image to be detected
gray = cv2.cvtColor(image_cv_corr,cv2.COLOR_BGR2GRAY)
edges = cv2.Canny(gray, 80, 200, apertureSize = 3)
hsv = cv2.cvtColor(image_cv_corr, cv2.COLOR_BGR2HSV)
yellow = cv2.inRange(hsv, hsv_yellow1, hsv_yellow2)
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(3, 3))
yellow = cv2.dilate(yellow, kernel)
blue = cv2.inRange(hsv, hsv_blue1, hsv_blue2)
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(3, 3))
blue = cv2.dilate(blue, kernel)
# Detect lines and normals
edge_color_yellow = cv2.bitwise_and(yellow, edges)
lines_yellow = cv2.HoughLinesP(edge_color_yellow, 1, np.pi/180, 10, np.empty(1), 3, 1)
if lines_yellow is not None:
lines_yellow = np.array(lines_yellow[0])
else:
lines_yellow = []
#edge_color_blue = cv2.bitwise_and(blue, edges)
#lines_blue = cv2.HoughLinesP(edge_color_blue, 1, np.pi/180, 10, np.empty(1), 3, 1)
#if lines_blue is not None:
#lines_blue = np.array(lines_blue[0])
#else:
#lines_blue = []
#print "***************** ",image_cv.shape," *********************"
#bw_yellow = yellow
#bw_blue = blue
self.blue = blue
self.yellow = yellow
if len(lines_yellow) > 0:
lines_yellow,normals_yellow = self.normals(lines_yellow,yellow)
#if len(lines_blue) > 0:
#lines_blue,normals_blue = self.normals(lines_blue,bw_blue)
tk.completed('detected')
# SegmentList constructor
segmentList = SegmentList()
segmentList.header.stamp = image_msg.header.stamp
# Convert to normalized pixel coordinates, and add segments to segmentList
if len(lines_yellow) > 0:
segmentList.segments.extend(self.toSegmentMsg(lines_yellow, normals_yellow, Segment.YELLOW))
if len(segmentList.segments) == 0:
if self.time_switch == False:
msgg = BoolStamped()
msgg.data = False
self.pub_lane.publish(msgg)
self.time_switch = True
car_control_msg = Twist2DStamped()
car_control_msg.v = 0.0
car_control_msg.omega = 0.0
self.pub_car_cmd.publish(car_control_msg)
#if len(lines_blue) > 0:
#segmentList.segments.extend(self.toSegmentMsg(lines_blue, normals_blue, Segment.YELLOW))
self.intermittent_log('# segments:yellow %3d' % (len(lines_yellow)))
tk.completed('prepared')
# Publish segmentList
self.pub_lines.publish(segmentList)
# VISUALIZATION only below
if self.verbose:
# Draw lines and normals
image_with_lines = np.copy(image_cv_corr)
for x1,y1,x2,y2,norm_x,norm_y in np.hstack((lines_yellow,normals_yellow)):
x1 = int(x1)
x2 = int(x2)
y1 = int(y1)
y2 = int(y2)
ox= int((x1+x2)/2)
oy= int((y1+y2)/2)
cx = (ox+3*norm_x).astype('int')
cy = (oy+3*norm_y).astype('int')
ccx = (ox-3*norm_x).astype('int')
ccy = (oy-3*norm_y).astype('int')
if cx >158:
cx = 158
elif cx <1:
cx = 1
if ccx >158:
ccx = 158
elif ccx <1:
ccx = 1
if cy >=79:
cy = 79
elif cy <1:
cy = 1
if ccy >=79:
ccy = 79
elif ccy <1:
ccy = 1
if (blue[cy, cx] == 255 and yellow[ccy,ccx] ==255) or (yellow[cy, cx] == 255 and blue[ccy,ccx] ==255):
cv2.line(image_with_lines, (x1,y1), (x2,y2), (0,0,255), 2)
cv2.circle(image_with_lines, (x1,y1), 2, (0,255,0))
cv2.circle(image_with_lines, (x2,y2), 2, (255,0,0))
tk.completed('drawn')
# Publish the frame with lines
image_msg_out = self.bridge.cv2_to_imgmsg(image_with_lines, "bgr8")
image_msg_out.header.stamp = image_msg.header.stamp
self.pub_image.publish(image_msg_out)
tk.completed('pub_image')
# if self.verbose:
#colorSegment = color_segment(white.area, red.area, yellow.area)
#edge_msg_out = self.bridge.cv2_to_imgmsg(self.detector.edges, "mono8")
#colorSegment_msg_out = self.bridge.cv2_to_imgmsg(colorSegment, "bgr8")
#self.pub_edge.publish(edge_msg_out)
#self.pub_colorSegment.publish(colorSegment_msg_out)
tk.completed('pub_edge/pub_segment')
self.intermittent_log(tk.getall())
def processSegments(self,segment_list_msg):
if not self.active:
return
t_start = rospy.get_time()
if self.use_propagation:
self.propagateBelief()
self.t_last_update = rospy.get_time()
# initialize measurement likelihood
measurement_likelihood = np.zeros(self.d.shape)
for segment in segment_list_msg.segments:
if segment.color != segment.WHITE and segment.color != segment.YELLOW:
continue
if segment.points[0].x < 0 or segment.points[1].x < 0:
continue
d_i,phi_i,l_i = self.generateVote(segment)
if d_i > self.d_max or d_i < self.d_min or phi_i < self.phi_min or phi_i>self.phi_max:
continue
if self.use_max_segment_dist and (l_i > self.max_segment_dist):
continue
i = floor((d_i - self.d_min)/self.delta_d)
j = floor((phi_i - self.phi_min)/self.delta_phi)
if self.use_distance_weighting:
dist_weight = self.dwa*l_i**3+self.dwb*l_i**2+self.dwc*l_i+self.zero_val
if dist_weight < 0:
continue
measurement_likelihood[i,j] = measurement_likelihood[i,j] + dist_weight
else:
measurement_likelihood[i,j] = measurement_likelihood[i,j] + 1/(l_i)
if np.linalg.norm(measurement_likelihood) == 0:
return
measurement_likelihood = measurement_likelihood/np.sum(measurement_likelihood)
if self.use_propagation:
self.updateBelief(measurement_likelihood)
else:
self.beliefRV = measurement_likelihood
# TODO entropy test:
#print self.beliefRV.argmax()
maxids = np.unravel_index(self.beliefRV.argmax(),self.beliefRV.shape)
# rospy.loginfo('maxids: %s' % maxids)
self.lanePose.header.stamp = segment_list_msg.header.stamp
self.lanePose.d = self.d_min + maxids[0]*self.delta_d
self.lanePose.phi = self.phi_min + maxids[1]*self.delta_phi
self.lanePose.status = self.lanePose.NORMAL
# publish the belief image
bridge = CvBridge()
belief_img = bridge.cv2_to_imgmsg((255*self.beliefRV).astype('uint8'), "mono8")
belief_img.header.stamp = segment_list_msg.header.stamp
max_val = self.beliefRV.max()
self.lanePose.in_lane = max_val > self.min_max and len(segment_list_msg.segments) > self.min_segs and np.linalg.norm(measurement_likelihood) != 0
self.pub_lane_pose.publish(self.lanePose)
self.pub_belief_img.publish(belief_img)
# print "time to process segments:"
# print rospy.get_time() - t_start
# Publish in_lane according to the ent
in_lane_msg = BoolStamped()
in_lane_msg.header.stamp = segment_list_msg.header.stamp
in_lane_msg.data = self.lanePose.in_lane
# ent = entropy(self.beliefRV)
# if (ent < self.max_entropy):
# in_lane_msg.data = True
# else:
# in_lane_msg.data = False
self.pub_in_lane.publish(in_lane_msg)
def pubLocalized(self):
msg = BoolStamped()
msg.data = True
self.pub_localized.publish(msg)
def process(self, msgg):
if self.active == True:
print "##################fsm state is " + `self.state`
if self.state == "RESET":
self.stack = ['B']
time.sleep(2)
elif self.state == "FORWARD":
self.motion_planning("F")
self.stack.append('F')
time.sleep(2)
elif self.state == "TURN_RIGHT":
if self.stack.pop() == 'R':
self.stack.append('R')
msg = BoolStamped()
msg.data = True
self.pub_time_is_up.publish(msg)
else:
self.motion_planning("R")
self.stack.append('R')
time.sleep(2)
elif self.state == "TURN_BACK":
if self.stack.pop() == 'L':
self.stack.append('L')
msg = BoolStamped()
msg.data = True
self.pub_time_is_up.publish(msg)
else:
self.motion_planning("RR")
self.stack.append('L')
time.sleep(2)
elif self.state == "TRACE_BACK":
self.motion_planning("RB")
self.stack.pop()
c = self.stack.pop()
if c == 'F':
self.stack.append('F')
msg = BoolStamped()
msg.data = True
self.pub_last_is_forward.publish(msg)
msg.data = False
self.pub_last_is_turn.publish(msg)
elif c == 'R':
msg = BoolStamped()
msg.data = False
self.pub_last_is_forward.publish(msg)
msg.data = True
self.pub_last_is_turn.publish(msg)
elif c == 'L':
msg = BoolStamped()
msg.data = False
self.pub_last_is_forward.publish(msg)
msg.data = False
self.pub_last_is_turn.publish(msg)
else:
self.stack.append('B')
print "Bottom of stack"
time.sleep(2)
else:
print "Wrong State"
self.printroute()
if not self.active:
msg = BoolStamped()
msg.data = False
self.pub_last_is_forward.publish(msg)
self.pub_last_is_turn.publish(msg)
print "#####################Project not active######################"
def processImage_(self, image_msg):
self.stats.processed()
if self.intermittent_log_now():
self.intermittent_log(self.stats.info())
self.stats.reset()
tk = TimeKeeper(image_msg)
self.intermittent_counter += 1
# Decode from compressed image with OpenCV
try:
image_cv = image_cv_from_jpg(image_msg.data)
except ValueError as e:
self.loginfo('Could not decode image: %s' % e)
return
tk.completed('decoded')
# Resize and crop image
hei_original, wid_original = image_cv.shape[0:2]
if self.image_size[0] != hei_original or self.image_size[1] != wid_original:
# image_cv = cv2.GaussianBlur(image_cv, (5,5), 2)
image_cv = cv2.resize(image_cv, (self.image_size[1], self.image_size[0]),
interpolation=cv2.INTER_NEAREST)
image_cv = image_cv[self.top_cutoff:,:,:]
tk.completed('resized')
# apply color correction: AntiInstagram
image_cv_corr = self.ai.applyTransform(image_cv)
image_cv_corr = cv2.convertScaleAbs(image_cv_corr)
tk.completed('corrected')
# set up parameter
hsv_green1 = np.array([70,100,60])
hsv_green2 = np.array([90,255,255])
hsv_blue1 = np.array([90,80,50])
hsv_blue2 = np.array([110,255,255])
# Set the image to be detected
hsv = cv2.cvtColor(image_cv_corr,cv2.COLOR_BGR2HSV)
green = cv2.inRange(hsv,hsv_green1,hsv_green2)
blue = cv2.inRange(hsv,hsv_blue1,hsv_blue2)
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(3, 3))
green = cv2.dilate(green, kernel)
blue = cv2.dilate(blue, kernel)
x = green[90:120,:]
y = blue[90:120,:]
msgg = BoolStamped()
if (x==255).sum() > 250:
print "green line detected!"
time.sleep(4)
print " 4 sec finish"
msgg.data = True
self.pub_lane_recovery.publish(msgg)
elif (y==255).sum() > 250:
print "blue line detected!"
time.sleep(7)
print " 7 sec finish"
msgg.data = True
self.pub_lane_recovery.publish(msgg)
else:
print "only red line detected"
time.sleep(1)
print " 1 sec finish"
msgg.data = True
self.pub_lane_recovery.publish(msgg)
tk.completed('prepared')
# VISUALIZATION only below
if self.verbose:
tk.completed('drawn')
# Publish the frame with lines
image_msg_out_green = self.bridge.cv2_to_imgmsg(green, "mono8")
image_msg_out_green.header.stamp = image_msg.header.stamp
self.pub_image_green.publish(image_msg_out_green)
image_msg_out_blue = self.bridge.cv2_to_imgmsg(blue, "mono8")
image_msg_out_blue.header.stamp = image_msg.header.stamp
self.pub_image_blue.publish(image_msg_out_blue)
tk.completed('pub_image')
self.intermittent_log(tk.getall())
def cbDetectionsList(self, detections_msg):
#For ground projection uncomment the next lines
marker_array = MarkerArray()
p = Vector2D()
p2 = Vector2D()
count = 0;
projection_list = ObstacleProjectedDetectionList()
projection_list.list = []
projection_list.header = detections_msg.header
minDist = 999999999999999999999999.0
dists = []
width = detections_msg.imwidth
height = detections_msg.imheight
too_close = False
for obstacle in detections_msg.list:
marker = Marker()
rect = obstacle.bounding_box
p.x = float(rect.x)/float(width)
p.y = float(rect.y)/float(height)
p2.x = float(rect.x + rect.w)/float(width)
p2.y = p.y
projected_point = self.ground_proj(p)
projected_point2 = self.ground_proj(p2)
obj_width = (projected_point2.gp.y - projected_point.gp.y)**2 + (projected_point2.gp.x - projected_point.gp.x)**2
obj_width = obj_width ** 0.5
rospy.loginfo("[%s]Width of object: %f" % (self.name,obj_width))
projection = ObstacleProjectedDetection()
projection.location = projected_point.gp
projection.type = obstacle.type
dist = projected_point.gp.x**2 + projected_point.gp.y**2 + projected_point.gp.z**2
dist = dist ** 0.5
if dist<minDist:
minDist = dist
if dist<self.closeness_threshold:
# Trying not to falsely detect the lane lines as duckies that are too close
if obstacle.type.type == ObstacleType.DUCKIE and projected_point.gp.y < 0.18:
# rospy.loginfo("Duckie too close y: %f dist: %f" %(projected_point.gp.y, minDist))
too_close = True
elif obstacle.type.type == ObstacleType.CONE and obj_width<0.3: # and -0.0785< projected_point.gp.y < 0.18:
# rospy.loginfo("Cone too close y: %f dist: %f" %(projected_point.gp.y, minDist))
too_close = True
projection.distance = dist
projection_list.list.append(projection)
#print projected_point.gp
marker.header = detections_msg.header
marker.header.frame_id = self.veh_name
marker.type = marker.ARROW
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.01
marker.scale.z = 0.01
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.5
marker.color.b = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = -0.7071
marker.pose.orientation.z = 0
marker.pose.orientation.w = 0.7071
marker.pose.position.x = projected_point.gp.x
marker.pose.position.y = projected_point.gp.y
marker.pose.position.z = projected_point.gp.z
marker.id = count
count = count +1
marker_array.markers.append(marker)
if count > self.maxMarkers:
self.maxMarkers = count
while count <= self.maxMarkers:
marker = Marker()
marker.action = 2
marker.id = count
marker_array.markers.append(marker)
count = count+1
b = BoolStamped()
b.header = detections_msg.header
b.data = too_close
self.pub_too_close.publish(b)
self.pub_projections.publish(projection_list)
self.pub_markers.publish(marker_array)
