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)