def __init__(self, save_pts=False, config_file_path=None): if config_file_path: pass rospy.init_node("gi_navigator_node") self.dg = DiscreteGridUtils.DiscreteGridUtils(grid_size=0.1) # modify grid size according to different purposes self.rate = rospy.Rate(50) self.driver = astar.driver.Driver() self.controller = Controller() self.mavros_state = "OFFBOARD" self.set_status(status.INITIALIZED) self.save_pts = save_pts self.cur_command_id = 0 self.prev_command_id = 0 self.cur_target_position = None self.task_id = -1 self.obstacle_set_mutex = threading.Lock() # mutex.acquire(timeout);mutex.release() self.nav_command_mutex = threading.Lock() # for nav command in dstar and ros high level command. self.local_pose_d = None self.local_pose_c = None self.navigator_status_pub = rospy.Publisher('/gi/navigator_status', String, queue_size=10) self.path_plan_pub = rospy.Publisher('/gi/navi_path_plan', MarkerArray, queue_size=10) self.path = [] self.path_prune = PathPruning(obstacle_distance=12) self.rs = randomsampling() t1 = threading.Thread(target=self.ros_thread) t1.start()
def __init__(self,config_file_path = None): if config_file_path: pass rospy.init_node("gi_navigator_node") self.dg = DiscreteGridUtils.DiscreteGridUtils(grid_size=0.2)#0.2) self.rate = rospy.Rate(50) self.driver = astar.driver.Driver() self.controller = Controller() self.mavros_state = "OFFBOARD" self.set_status(status.INITIALIZED) self.cur_command_id = 0 self.prev_command_id = 0 self.cur_target_position=None self.task_id = -1 self.obstacle_set_mutex = threading.Lock() # mutex.acquire(timeout);mutex.release() self.nav_command_mutex = threading.Lock() # for nav command in dstar and ros high level command. self.local_pose = None t1 = threading.Thread(target=self.ros_thread) t1.start() self.navigator_status_pub = rospy.Publisher('/gi/navigator_status', String, queue_size=10) self.path_plan_pub = rospy.Publisher('/gi/navi_path_plan',MarkerArray,queue_size=10)
class Navigator: def __init__(self, config_file_path=None): if config_file_path: pass rospy.init_node("gi_navigator_node") self.dg = DiscreteGridUtils.DiscreteGridUtils(grid_size=0.2) #0.2) self.rate = rospy.Rate(50) self.driver = astar.driver.Driver() self.controller = Controller() self.mavros_state = "OFFBOARD" self.set_status(status.INITIALIZED) self.cur_command_id = 0 self.prev_command_id = 0 self.cur_target_position = None self.task_id = -1 self.obstacle_set_mutex = threading.Lock( ) # mutex.acquire(timeout);mutex.release() self.nav_command_mutex = threading.Lock( ) # for nav command in dstar and ros high level command. self.local_pose = None t1 = threading.Thread(target=self.ros_thread) t1.start() self.navigator_status_pub = rospy.Publisher('/gi/navigator_status', String, queue_size=10) self.path_plan_pub = rospy.Publisher('/gi/navi_path_plan', MarkerArray, queue_size=10) #t2 = thread.start_new_thread(self.Dstar_thread, ()) #self.keep_navigating() self.path = [] self.path_prune = PathPruning(obstacle_distance=8) time.sleep(2) ''' Navigating thread ''' def keep_navigating(self): # for debug: #self.algo = astar.astar.A_star((1,0,0)) ### Added, wait for several seconds for mavros switching to offboard for i in range(0, 25): if self.mavros_state == "OFFBOARD": break time.sleep(1) while self.mavros_state == "OFFBOARD" and not (rospy.is_shutdown()): # print ('Inside outer loop!') #print ("Navigator state: ", self.STATUS.data, "Mavros state: ", self.mavros_state) relative_pos = (0, 0, 0) end_pos = self.get_latest_target() current_pos = self.get_current_pose() # TODO:fix this. last_pos = self.get_current_pose() if current_pos is None: #print ('current pose not valid!') continue while current_pos != end_pos and not self.navi_task_terminated( ) and not (rospy.is_shutdown()): # Till task is finished: # print ('Inside inner loop!') current_pos = self.get_current_pose() self.algo = astar.astar.A_star(end_pos) print('Move 1 step') obstacle_map = self.driver.get_obstacles_around( ) # TODO:加入障碍记忆. print('From ', current_pos) t1 = time.time() self.driver.algo = astar.astar.A_star(end_pos) self.path = self.algo.find_path( current_pos, self.driver.get_obstacles_around()) t2 = time.time() print('A* time cost:', (t2 - t1)) if not self.path: # Path no found self.set_status(status.EXPLORATION) print('No path found!') self.do_hover() # TODO time.sleep(0.05) # TODO # find a new nav_point # #continue ''' target_pos = last_pos if target_pos[2] < 1.0: target_pos = (last_pos[0], last_pos[1], 1.0) current_pos = self.get_current_pose() self.controller.mav_move( *self.dg.discrete_to_continuous_target((target_pos[0], target_pos[1], target_pos[2])), abs_mode=True) # TODO:fix this. print("Fly to Last position:", target_pos) while self.distance(target_pos, current_pos) < 0.5: self.controller.mav_move( *self.dg.discrete_to_continuous_target((target_pos[0], target_pos[1], target_pos[2])), abs_mode=True) # TODO:fix this. current_pos = self.get_current_pose() time.sleep(2) ''' else: # Path found. keep state machine and do task step by step. self.set_status(status.GOING_TO_TARGET) self.collear_check_path = self.path_prune.remove_collinear_points( self.path) self.bresenham_check_path = self.path_prune.path_pruning_bresenham3d( self.collear_check_path, obstacle_map) #publish raw path plan. m_arr = MarkerArray() marr_index = 0 for next_move in self.path: point = self.dg.discrete_to_continuous_target( (next_move[0], next_move[1], next_move[2])) mk = Marker() mk.header.frame_id = "map" mk.action = mk.ADD mk.id = marr_index marr_index += 1 mk.color.r = 1.0 mk.color.a = 1.0 mk.type = mk.CUBE mk.scale.x = 0.6 mk.scale.y = 0.6 mk.scale.z = 0.6 mk.pose.position.x = point[0] mk.pose.position.y = point[1] mk.pose.position.z = point[2] m_arr.markers.append(mk) self.path_plan_pub.publish(m_arr) #move_iter = 0 #move_iter_max = 3 for next_move in self.bresenham_check_path: self.path_plan_pub.publish(m_arr) if self.navi_task_terminated(): break print('current_pos:', current_pos) next_pos = next_move relative_pos = (next_pos[0] - current_pos[0], next_pos[1] - current_pos[1], next_pos[2] - current_pos[2]) print('next_move : ', next_move) print("relative_move : ", relative_pos) print("next_pose: ", next_pos) if not self.driver.algo.is_valid( next_pos, self.driver.get_obstacles_around()): print('Path not valid!') break self.current_pos = next_pos #axis transform relative_pos_new = (-relative_pos[0], -relative_pos[1], relative_pos[2]) #self.controller.mav_move(*relative_pos_new,abs_mode=False) # TODO:fix this. print('mav_move() input: relative pos=', next_pos) self.controller.mav_move( *self.dg.discrete_to_continuous_target( (next_pos[0], next_pos[1], next_pos[2])), abs_mode=True) # TODO:fix this. current_pos = self.get_current_pose() time.sleep(2) predict_move = (self.current_pos[0] + relative_pos[0], self.current_pos[1] + relative_pos[1], self.current_pos[2] + relative_pos[2]) print("predict_move : ", predict_move) if not self.algo.path_is_valid( self.bresenham_check_path, self.driver.get_obstacles_around()): print('Path conflict detected!') break #move_iter += 1 #if move_iter >= move_iter_max: # self.do_hover() # TODO # break last_pos = self.get_current_pose() time.sleep(0.05) # wait for new nav task. ''' if self.found_path: print("Found path!") target_position = self.cur_target_position result = False while result is False: result = self.mav_move(target_position[0], target_position[1], target_position[2]) print("Reached Position: ", target_position[0], target_position[1], target_position[2]) print("Finished Current Path") time.sleep(0.2) ''' print("Mavros not in OFFBOARD mode, Disconnected!") ''' move quad in body frame ''' def distance(self, p1, p2): x_distance = (p2[0] - p1[0])**2 y_distance = (p2[1] - p1[1])**2 z_distance = (p2[2] - p1[2])**2 return np.sqrt(x_distance + y_distance + z_distance) def remove_collinear_points(self, original_path): new_path = [] print("original_path length: ", len(original_path)) length = len(original_path) - 2 new_path.append(original_path[0]) # new_path.append(original_path[-1]) print(original_path) for i in range(length): distance13 = self.distance(original_path[i + 2], original_path[i]) distance12 = self.distance(original_path[i + 1], original_path[i]) distance23 = self.distance(original_path[i + 2], original_path[i + 1]) print("distance13 - distance12 - distance23 :", distance13 - distance12 - distance23) if abs(distance13 - distance12 - distance23) < 0.001: # print ("points collinear") continue else: print(original_path[i + 1]) print("not found collinear point") new_path.append(original_path[i + 1]) print("new path length: ", len(new_path)) print(new_path) return new_path def terminate_navigating(self): #TODO pass def resume_navigating(self): #TODO pass def do_hover(self): #TODO pass def set_target_postion(self, target_position): self.found_path = True self.cur_target_position = self.dg.continuous_to_discrete( target_position) print("Current target position in grid: ", self.cur_target_position) #print("Set Current Position to: ", target_position[0], target_position[1], target_position[2]) def get_latest_target(self): return self.cur_target_position def set_vision_target(self): self.set_status(status.GOING_TO_VISION_TARGET) self.set_target_position(xxxxx) #TODO pass def navi_task_terminated(self): if dist(self.local_pose, self.cur_target_position) < 0.25: #TODO: or stop flag is set. return True else: return False ''' Dstar Thread def Dstar_thread(self): while not rospy.is_shutdown(): while status!= xxx:# TODO next_move = xxx return next_move''' '''##For test: target = [0.5, 0.5, 0.5] self.set_target_postion(target) pass''' ''' ROS thread responsible for subscribers and publishers ''' def ros_thread(self): print('ros_thread spawn!!!!') self.octomap_msg = None # subscribers self.slam_sub = rospy.Subscriber("/gi/slam_output/pose", PoseStamped, self.slam_pose_callback) self.vision_target_sub = rospy.Subscriber("/gi/visual_target/pose", PoseStamped, self.vision_target_callback) self.point_cloud_sub = rospy.Subscriber("/camera/left/point_cloud", PointCloud, self.point_cloud_callback) self.octomap_cells_vis = rospy.Subscriber( "/octomap_point_cloud_centers", PointCloud2, self.octomap_update_callback) self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_callback) self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback) # publishers #self.mavros_control_pub = rospy.Publisher('mavros/Command', Command, queue_size=10) self.set_status(status.INITIALIZED) rospy.spin() ''' ROS callbacks ''' def slam_pose_callback(self, msg): self.slam_pose = msg def vision_target_callback(self, msg): self.vision_target = msg #print("Received New Vision Target!") def mavros_state_callback(self, msg): self.mavros_state = msg.mode #print(msg.mode, type(msg.mode)) self.navigator_status_pub.publish(self.STATUS) def point_cloud_callback(self, msg): self.current_point_cloud = msg def octomap_update_callback(self, msg): # as pointcloud2. obs_set = set() for p in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): #print " x : %f y: %f z: %f" % (p[0], p[1], p[2]) point = self.dg.continuous_to_discrete((p[0], p[1], p[2])) #print ('point:',point) obs_set.add(point) acquired = self.obstacle_set_mutex.acquire(True) # blocking. if acquired: #print('octomap updated!') self.driver.set_obstacle_set(obs_set) self.obstacle_set_mutex.release() return else: print('Lock not acquired!') def local_pose_callback(self, msg): pose_ = msg.pose.position #TODO:do fusion with visual slam. self.local_pose = self.dg.continuous_to_discrete( (pose_.x, pose_.y, pose_.z)) #print ('local_pose set!!!') def get_local_pose(self): # in mavros axis.for command. #print ('self.local_pose',self.local_pose) return self.local_pose def get_current_pose(self): # current pose:slam pose(in world axis) return self.get_local_pose() # TODO:do transform T1 ^-1 * T2. ''' helper functions ''' def set_status(self, status): self.STATUS = String(status.name) ''' def reachTargetPosition(self, target, threshold = 0.1): delta_x = math.fabs(self.local_pose.pose.position.x - target.pos_sp[0]) delta_y = math.fabs(self.local_pose.pose.position.y - target.pos_sp[1]) delta_z = math.fabs(self.local_pose.pose.position.z - target.pos_sp[2]) distance = (delta_x + delta_y + delta_z) print("distance: ", distance, "threshold: ", threshold) if distance < threshold: return True else: return False ''' def setMavMode(self, msg): pass
class Navigator: def __init__(self, save_pts=False, config_file_path=None): if config_file_path: pass rospy.init_node("gi_navigator_node") self.dg = DiscreteGridUtils.DiscreteGridUtils( grid_size=0.1) # modify grid size according to different purposes self.rate = rospy.Rate(50) self.driver = astar.driver.Driver() self.controller = Controller() self.mavros_state = "OFFBOARD" self.set_status(status.INITIALIZED) self.save_pts = save_pts self.cur_command_id = 0 self.prev_command_id = 0 self.cur_target_position = None self.task_id = -1 self.obstacle_set_mutex = threading.Lock( ) # mutex.acquire(timeout);mutex.release() self.nav_command_mutex = threading.Lock( ) # for nav command in dstar and ros high level command. self.local_pose_d = None self.local_pose_c = None self.navigator_status_pub = rospy.Publisher('/gi/navigator_status', String, queue_size=10) self.path_plan_pub = rospy.Publisher('/gi/navi_path_plan', MarkerArray, queue_size=10) self.path = [] self.path_prune = PathPruning(obstacle_distance=12) t1 = threading.Thread(target=self.ros_thread) t1.start() ''' Navigating thread ''' def keep_navigating(self): while 1: # get target position and local position in discrete end_pos = self.get_latest_target() current_pos = self.get_local_pose_d() if current_pos is None: print('current pose not valid!') time.sleep(0.5) continue while not self.reachTargetPositionDiscrete(end_pos, 4) \ and (not self.navi_task_terminated()) \ and (not rospy.is_shutdown()): # Till task is finished: print('From ', self.get_local_pose_d()) # This is the place where you modify path planner t1 = time.time() self.set_status(status.LOOKING_FOR_PATH) self.algo = astar.astar.A_star(end_pos) self.driver.algo = astar.astar.A_star(end_pos) self.path = self.algo.find_path( self.get_local_pose_d(), self.driver.get_obstacles_around()) t2 = time.time() print('A* time cost:', (t2 - t1)) if not self.path: self.set_status(status.LOOKING_FOR_PATH_SUCCEED) print('No path found!') time.sleep(0.05) else: # Path found. keep state machine and do task step by step. print("Path found!") self.set_status(status.LOOKING_FOR_PATH_FAILED) self.collear_check_path = self.path_prune.remove_collinear_points( self.path) self.bresenham_check_path = self.path_prune.path_pruning_bresenham3d( self.collear_check_path, self.driver.get_obstacles_around()) # self.bresenham_check_path = self.collear_check_path print("Path finished processing!") # publish raw path, the actual flight path will defer from published raw path # (1, 0, 0) is RED in RGB order self.publish_path(self.path, (1, 0, 0)) # going through each waypoint for next_move in self.bresenham_check_path: if self.navi_task_terminated(): break print('next_move : ', next_move) if not self.driver.algo.is_valid( next_move, self.driver.get_obstacles_around()): print( 'Next waypoint is in collision with obstacle, path not valid!' ) break next_position_continuous = self.dg.discrete_to_continuous_target( next_move) print("local pose: ", self.local_pose_c) print("target pose: ", next_position_continuous) while not self.reachTargetPositionContinuous( next_position_continuous, 0.5): # if not arrived yet self.controller.mav_move( next_position_continuous[0], next_position_continuous[1], next_position_continuous[2], abs_mode=True) time.sleep(0.05) # if not self.algo.path_is_valid(self.bresenham_check_path, self.driver.get_obstacles_around()): # print('Path conflict detected!') # break print("Target Reached!") time.sleep(0.05) # wait for new nav task. print("Mavros not in OFFBOARD mode, Disconnected!") ''' move quad in body frame ''' def terminate_navigating(self): # TODO pass def resume_navigating(self): # TODO pass def set_target_position(self, target_position): if target_position and len(target_position) is 3: self.cur_target_position = self.dg.continuous_to_discrete( target_position) def get_latest_target(self): return self.cur_target_position def set_vision_target(self, vision_target): self.set_status(status.GOING_TO_VISION_TARGET) self.set_target_position(vision_target) def navi_task_terminated(self): if self.dist( self.local_pose_d, self.cur_target_position) < 2: # TODO: or stop flag is set. return True else: return False ''' Dstar Thread def Dstar_thread(self): while not rospy.is_shutdown(): while status!= xxx:# TODO next_move = xxx return next_move''' '''##For test: target = [0.5, 0.5, 0.5] self.set_target_postion(target) pass''' ''' ROS thread responsible for subscribers and publishers ''' def ros_thread(self): print('ros_thread spawn!!!!') self.octomap_msg = None # subscribers self.slam_sub = rospy.Subscriber("/gi/slam_output/pose", PoseStamped, self.slam_pose_callback) self.vision_target_sub = rospy.Subscriber("/gi/visual_target/pose", PoseStamped, self.vision_target_callback) self.point_cloud_sub = rospy.Subscriber("/camera/left/point_cloud", PointCloud, self.point_cloud_callback) self.octomap_cells_vis = rospy.Subscriber( "/octomap_point_cloud_centers", PointCloud2, self.octomap_update_callback) self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_callback) self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback) # publishers # self.mavros_control_pub = rospy.Publisher('mavros/Command', Command, queue_size=10) self.set_status(status.INITIALIZED) rospy.spin() ''' ROS callbacks ''' def slam_pose_callback(self, msg): self.slam_pose = msg def vision_target_callback(self, msg): self.vision_target = msg # print("Received New Vision Target!") def mavros_state_callback(self, msg): self.mavros_state = msg.mode self.navigator_status_pub.publish(self.STATUS) def point_cloud_callback(self, msg): self.current_point_cloud = msg def octomap_update_callback(self, msg): # as pointcloud2. obs_set = set() for p in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): # print " x : %f y: %f z: %f" % (p[0], p[1], p[2]) point = self.dg.continuous_to_discrete((p[0], p[1], p[2])) # print("corresponding discrete value: ", point) obs_set.add(point) # save points set if (self.save_pts): save_points3D(obs_set) acquired = self.obstacle_set_mutex.acquire(True) # blocking. if acquired: # print('octomap updated!') self.driver.set_obstacle_set(obs_set) self.obstacle_set_mutex.release() return else: print('Lock not acquired!') def local_pose_callback(self, msg): pose_ = msg.pose.position # TODO:do fusion with visual slam. self.local_pose_c = (pose_.x, pose_.y, pose_.z) self.local_pose_d = self.dg.continuous_to_discrete( (pose_.x, pose_.y, pose_.z)) # return pose in discrete def get_local_pose_d(self): # in mavros axis.for command. return self.local_pose_d # return pose in continuous def get_local_pose_c(self): return self.local_pose_c ''' helper functions ''' def set_status(self, status): self.STATUS = String(status.name) def dist(sefl, pos1, pos2): if not pos1 or not pos2: return False, 0 else: return True, reduce( lambda x, y: x + y, map(lambda i: (pos1[i] - pos2[i])**2, [0, 1, 2])) # target should be Continuous def reachTargetPositionContinuous(self, target, threshold=0.7): delta_x = math.fabs(self.local_pose_c[0] - target[0]) delta_y = math.fabs(self.local_pose_c[1] - target[1]) delta_z = math.fabs(self.local_pose_c[2] - target[2]) distance = (delta_x + delta_y + delta_z) print("distance: ", distance, "threshold: ", threshold) if distance < threshold: return True else: return False # target should be discrete def reachTargetPositionDiscrete(self, target, threshold=3): delta_x = math.fabs(self.local_pose_d[0] - target[0]) delta_y = math.fabs(self.local_pose_d[1] - target[1]) delta_z = math.fabs(self.local_pose_d[2] - target[2]) distance = (delta_x + delta_y + delta_z) print("distance: ", distance, "threshold: ", threshold) if distance < threshold: return True else: return False def setMavMode(self, msg): pass def do_hover(self): pass def publish_path(self, path, RGB=(1, 0, 0)): m_arr = MarkerArray() marr_index = 0 for next_move in path: point = self.dg.discrete_to_continuous_target( (next_move[0], next_move[1], next_move[2])) mk = Marker() mk.header.frame_id = "map" mk.action = mk.ADD mk.id = marr_index marr_index += 1 mk.color.r = RGB[0] mk.color.g = RGB[1] mk.color.b = RGB[2] mk.color.a = 1.0 mk.type = mk.CUBE mk.scale.x = 0.3 mk.scale.y = 0.3 mk.scale.z = 0.3 mk.pose.position.x = point[0] mk.pose.position.y = point[1] mk.pose.position.z = point[2] m_arr.markers.append(mk) self.path_plan_pub.publish(m_arr)
def __init__(self, config_file_path=None): if config_file_path: pass rospy.init_node("gi_navigator_node") self.ground_h = 10 # 设置地面高度 self.alpha = 0.2 # 设置alpha 的值控制 获取边界 self.dist = 9 # 距离最近点的距离计算下一点 self.error_yaw = 0 # 设置yaw 的偏差 self.current_yaw = 0 # 当前的yaw 值 self.except_dist = 5 # 期望与岸的距离为 0.7 self.bank_status = True # 记录岸的状态 True 左岸,False右岸 self.bank_dist = 0 # 船距离岸的距离 self.bank_parallel_yaw = 0 # 岸的yaw 值 # # 设置yaw pid设置 # self.yaw_pid = PID.PID(P=6.0,I=0.00 ,D=0.00) # # 设置dist_pid pid设置 # self.dist_pid = PID.PID(P=20.0,I=0.0 ,D=0.0) self.arm_state = False # 手动模式切换状态 self.manual_state = False # 获取设置点的位置 self.set_point_pos = None self.dg = DiscreteGridUtils.DiscreteGridUtils(grid_size=0.2) # 0.2 self.rate = rospy.Rate(50) self.driver = astar.driver.Driver() self.controller = Controller() # 设置为手动模式 self.mavros_state = "MANUAL" self.set_status(status.INITIALIZED) self.current_pos = None self.cur_command_id = 0 self.prev_command_id = 0 self.cur_target_position = None self.task_id = -1 # mutex.acquire(timeout);mutex.release() self.obstacle_set_mutex = threading.Lock() # for nav command in dstar and ros high level command. self.nav_command_mutex = threading.Lock() self.points = None # 存储imu roll,pitch情况 self.rollxyz = None self.local_pose = None t1 = threading.Thread(target=self.ros_thread) t1.start() self.navigator_status_pub = rospy.Publisher('/gi/navigator_status', String, queue_size=10) self.path_plan_pub = rospy.Publisher('/gi/navi_path_plan', MarkerArray, queue_size=10) # 偏行角度改变 self.yaw_target_pub = rospy.Publisher('gi/set_pose/orientation', Float32, queue_size=10) #t2 = thread.start_new_thread(self.Dstar_thread, ()) ''' ros publishers ''' # rc重载发布 self.test_pub = rospy.Publisher('/test/point', PointCloud2, queue_size=10) # rc重载发布 self.rc_override_pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=10) ''' ros services ''' self.armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.flightModeService = rospy.ServiceProxy('/mavros/set_mode', SetMode) # self.keep_navigating() self.trans = None self.path = [] self.new_path = [] self.path_prune = PathPruning(obstacle_distance=8) time.sleep(2)
class Navigator: def __init__(self, config_file_path=None): if config_file_path: pass rospy.init_node("gi_navigator_node") self.ground_h = 10 # 设置地面高度 self.alpha = 0.2 # 设置alpha 的值控制 获取边界 self.dist = 9 # 距离最近点的距离计算下一点 self.error_yaw = 0 # 设置yaw 的偏差 self.current_yaw = 0 # 当前的yaw 值 self.except_dist = 5 # 期望与岸的距离为 0.7 self.bank_status = True #记录岸的状态 True 左岸,False右岸 self.bank_dist = 0 #船距离岸的距离 self.bank_parallel_yaw = 0 # 岸的yaw 值 # # 设置yaw pid设置 # self.yaw_pid = PID.PID(P=6.0,I=0.00 ,D=0.00) # # 设置dist_pid pid设置 # self.dist_pid = PID.PID(P=20.0,I=0.0 ,D=0.0) self.arm_state = False # 手动模式切换状态 self.manual_state = False # 获取设置点的位置 self.set_point_pos = None self.dg = DiscreteGridUtils.DiscreteGridUtils(grid_size=0.2) #0.2 self.rate = rospy.Rate(50) self.driver = astar.driver.Driver() self.controller = Controller() # 设置为手动模式 self.mavros_state = "MANUAL" self.set_status(status.INITIALIZED) self.current_pos = None self.cur_command_id = 0 self.prev_command_id = 0 self.cur_target_position = None self.task_id = -1 self.obstacle_set_mutex = threading.Lock( ) # mutex.acquire(timeout);mutex.release() self.nav_command_mutex = threading.Lock( ) # for nav command in dstar and ros high level command. self.local_pose = None t1 = threading.Thread(target=self.ros_thread) t1.start() self.navigator_status_pub = rospy.Publisher('/gi/navigator_status', String, queue_size=10) self.path_plan_pub = rospy.Publisher('/gi/navi_path_plan', MarkerArray, queue_size=10) # 偏行角度改变 self.yaw_target_pub = rospy.Publisher('gi/set_pose/orientation', Float32, queue_size=10) #t2 = thread.start_new_thread(self.Dstar_thread, ()) ''' ros publishers ''' # rc重载发布 self.rc_override_pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=10) ''' ros services ''' self.armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.flightModeService = rospy.ServiceProxy('/mavros/set_mode', SetMode) print("Px4 Controller Initialized!") #self.keep_navigating() self.path = [] self.new_path = [] self.path_prune = PathPruning(obstacle_distance=8) time.sleep(2) def plot_polygon(self): self.driver.simple_plot() ''' Navigating thread ''' def keep_navigating(self): self.error_yaw = self.current_yaw - 180 # com = Commander() t2 = threading.Thread(target=self.plot_polygon) t2.start() t3 = threading.Thread(target=self.find_path) t3.start() # scan_environment = True/ # 将第一次的位置记录到路径中 self.path.append(self.current_pos) while self.mavros_state == "OFFBOARD" and not (rospy.is_shutdown()): # self.driver.set_current_pos(self.current_pos) # 先扫描一下附近的情况将周围环境扫描一边在执行任务 # if scan_environment : # # 旋转一周扫描周围环境 # except_yaw = 90 # com.turn(except_yaw) # # 等待其偏移到指定角度 # self.wait_yaw(except_yaw) # except_yaw = 180 # com.turn(except_yaw) # # 等待其偏移到指定角度 # self.wait_yaw(except_yaw) # except_yaw = -90 # com.turn(except_yaw) # # 等待其偏移到指定角度 # self.wait_yaw(except_yaw) # except_yaw = 0 # com.turn(except_yaw) # # 等待其偏移到指定角度 # self.wait_yaw(except_yaw) # scan_environment = False # current_pos = self.get_current_pose() if not self.new_path: # TODO set status print('No path found!') # self.do_hover() # TODO time.sleep(0.1) # TODO else: m_arr = MarkerArray() marr_index = 0 # 将路径发布出去可以在rviz 中看到 for next_move in self.new_path: point = self.dg.discrete_to_continuous_target( (next_move[0], next_move[1], self.ground_h)) mk = Marker() mk.header.frame_id = "map" mk.action = mk.ADD mk.id = marr_index marr_index += 1 mk.color.r = 1.0 mk.color.a = 1.0 mk.type = mk.CUBE mk.scale.x = 0.3 mk.scale.y = 0.3 mk.scale.z = 0.3 mk.pose.position.x = point[0] mk.pose.position.y = point[1] mk.pose.position.z = point[2] m_arr.markers.append(mk) self.path_plan_pub.publish(m_arr) # 执行移动 for next_move in self.new_path: self.path_plan_pub.publish(m_arr) # if self.navi_task_terminated(): # break # print ('current_pos:', current_pos) # 判断新路径中第一个点与 上一路径转向角度是否大于90度,若大于90度则不使用,使用向量点乘,大于0满足条件,小于0不满足 # if len(self.path)>1 and self.vector_dot_product(self.path[-2],self.path[-1],self.path[-1],next_move) <0 : # print ("跳过") # continue next_pos = next_move # 设置需要移动的点位 relative_pos = (next_pos[0] - self.current_pos[0], next_pos[1] - self.current_pos[1], 0) # print ('next_move : ', next_move) # print ("relative_move : ", relative_pos) # print ("next_pose: ", next_pos) # 判断下一个点会不会在障碍物内 # if not self.driver.algo.is_valid(next_pos, self.driver.get_obstacles_around()): # print ('Path not valid!') # break #axis transform # relative_pos_new = (-relative_pos[0], -relative_pos[1], relative_pos[2]) #self.controller.mav_move(*relative_pos_new,abs_mode=False) # TODO:fix this. # print ('mav_move() input: relative pos=',next_pos) # 目标点转向角度为 angle = None vector = (float(self.new_path[-1][0] - self.current_pos[0]), float(self.new_path[-1][1] - self.current_pos[1])) # print ("vector",vector) if vector[0] != 0: angle = math.atan( vector[1] / vector[0]) * 180 / math.pi #计算出角度 if vector[0] < 0: angle += 180 else: if vector[1] < 0: angle = -90 elif vector[1] > 0: angle = 90 # print ("angle ",angle) # 设置偏移 if angle: self.controller._turn_relative_to_takeoff_abs(angle) self.wait_yaw(angle) # 移动开始 self.controller.mav_move( *self.dg.discrete_to_continuous_target( (next_pos[0], next_pos[1], self.ground_h)), abs_mode=True) # TODO:fix this. time.sleep(2) predict_move = (self.current_pos[0] + relative_pos[0], self.current_pos[1] + relative_pos[1], self.ground_h + relative_pos[2]) # print ("predict_move : ", predict_move) # 等待移动成功 # 当当前位置与期望位置的向量与期望位置向量与当前位置的向量夹角小于90度时认为完成当前任务执行下次任务 while self.distance( self.current_pos, next_pos) > 1 and self.vector_dot_product( next_pos, self.current_pos, self.path[-1], next_pos) < 0: # print ("next_pos : ", next_pos) # print (" self.current_pos : ", self.current_pos) time.sleep(0.1) # 将新路径保存。 if self.path[-1] != self.current_pos: self.path.append(self.current_pos) # print ("self.path",self.path) break # 判断路径有无交叉 # if not self.algo.path_is_valid(self.collear_check_path, self.driver.get_obstacles_around()): # print ('Path conflict detected!') # break def vector_dot_product(self, vector1_start, vector1_end, vector2_start, vector2_end): ''' 计算两向量之间的点乘 vector1_start : 第一个向量的起点 vector1_end : 第一个向量的末点 vector2_start : 第二个向量的起点 vector2_end : 第二个向量的末点 ''' vector1 = (vector1_end[0] - vector1_start[0], vector1_end[1] - vector1_start[1]) vector2 = (vector2_end[0] - vector2_start[0], vector2_end[1] - vector2_start[1]) dot_product = vector1[0] * vector2[0] + vector1[1] * vector2[1] return dot_product def wait_yaw(self, except_yaw): ''' 等待偏移 ''' print("except_yaw", except_yaw) start_time = time.time() while self.driver.angle_diff(self.current_yaw, except_yaw) > 2: time.sleep(0.1) print(time.time() - start_time) # 获取高于某高度的地图使其化为2d 的保留在,x,y 去除高度信息 # map_2d = self.driver.get_octomap_in__h(self.ground_h) if time.time() - start_time > 20: break # 根据2d 的地图提取地图边界 # self.driver.get_octomap_alpha_shape(map_2d, self.alpha) def arrival_position(self, current_pos, max_val): ''' 判断是否到达上次输入目标附近 ''' x = abs(current_pos[0] - self.set_point_pos[0]) y = abs(current_pos[1] - self.set_point_pos[1]) if x < max_val and y < max_val: return False else: return True def find_path(self): ''' 获取船到岸的距离 ''' while True: try: # time1 = time.time() # 获取高于某高度的地图使其化为2d 的保留在,x,y 去除高度信息 map_2d = self.driver.get_octomap_in__h(self.ground_h) # time2 = time.time() if len(map_2d) > 3: # 根据2d 的地图提取地图边界 map_bounds = self.driver.get_octomap_alpha_shape( map_2d, self.alpha) # time3 = time.time() if not map_bounds.is_empty: # 根据多边形边界,获取距离当前位置最近的多边形,和最近多边形上距离当前位置最近的点 nearest_point = self.driver.get_nearest_points_and_polygon( self.current_pos, map_bounds) # time4 = time.time() current_axis_yaw = float(self.current_yaw) # 由当前位置 和指向角度可以计算得到 # time5 = time.time() current_pos_end = ( self.current_pos[0] + 10 * math.cos(current_axis_yaw / 180 * math.pi), self.current_pos[1] + 10 * math.sin(current_axis_yaw / 180 * math.pi)) point = list(nearest_point.coords)[0] # 计算出当前岸线是在左侧还是在船右侧 self.bank_status = self.driver.yaw_left_or_right( self.current_pos, current_pos_end, point) # time6 = time.time() # 根据岸线在船的左右位置判断,将当前位置与多边形最近点所形成的线段./顺时针或逆时针以1度步进不断旋转到90度,获得直线与所有多边形的交点,将交点连成一个或多个线段。表示岸线 path_lines = self.driver.get_polygon_path( self.current_pos, nearest_point, map_bounds, self.bank_status) print path_lines # 将当之间距离很近将路径合并成一条路径,但当路经大于某个值时丢弃当前路径, path = self.driver.get_path(path_lines, self.current_pos, self.bank_status, self.except_dist, 10) if path != None and len(path) > 0: # 移除在同一直线上的路径,并移除掉第一个点 self.new_path = self.path_prune.remove_collinear_points( path) self.driver.set_current_pos( self.current_pos, self.new_path) print("new_path", self.new_path) # time7 = time.time() except Exception as e: print("EXception in find path: ", e) traceback.print_exc() # print ("time1: " ,round((time2-time1),2)) # print ("time2: " ,round((time3-time2),2)) # print ("time3: " ,round((time4-time3),2)) # print ("time4: " ,round((time5-time4),2)) # print ("time5: " ,round((time6-time5),2)) # print ("time6: " ,round((time7-time6),2)) # print ("AllTime : " ,round((time7-time1),2)) # print (" # ===============================================" ) def print_log(self, ): ''' 打印日志 ''' print("self.yaw", self.current_point_cloud) print("self.right_parallel_yaw", self.bank_parallel_yaw) # print ("self.right_K",self.right_K) print("self.right_dist", self.bank_dist) # print ("ground",self.ground_speed) print("=================================") def set_dist_pid_control(self): ''' 根据期望离岸距离 不断调整yaw 到达期望距离 except_dist 期望到达的距离, now_dist 当前距离值, parallel_yaw 车身与岸平行时的yaw 值 rcmsg 设置通道值 ''' self.dist_pid.SetPoint = self.except_dist self.dist_pid.update(float(self.bank_dist)) self.dist_pid.sample_time = 0.1 #采样间隔 # 根据距离最近的岸在船身哪侧获得期望yaw 值 if self.bank_status: except_yaw = int(self.bank_parallel_yaw - self.dist_pid.output) # 得到的期望yaw 值 else: except_yaw = int(self.bank_parallel_yaw + self.dist_pid.output) # 得到的期望yaw 值 # 期望yaw 应在0-360 之间 if except_yaw > 360: except_yaw = except_yaw % 360 elif except_yaw < 0: except_yaw = except_yaw % 360 + 360 print("except_yaw", except_yaw) # 根据期望yaw 值不断调整车身与岸距离 return except_yaw def set_yaw_pid_control(self, except_yaw, rcmsg): ''' 根据期望yaw 值调整pwm 到达期望yaw 值 except_yaw 期望到达的yaw值, rcmsg 设置通道值 ''' self.yaw_pid.SetPoint = except_yaw self.yaw_pid.yaw_update(float( self.current_yaw)) # 针对0 -360 yaw 值针对 设置pid self.yaw_pid.sample_time = 0.1 #采样间隔 rcmsg.channels[0] = int(1500 + self.yaw_pid.output) # 设定pwm 值最大为2000 最小为1000 if rcmsg.channels[0] > 2000: rcmsg.channels[0] = 2000 elif rcmsg.channels[0] < 1000: rcmsg.channels[0] = 1000 def arm(self): if self.armService(True): return True else: print("Vehicle arming failed!") return False def disarm(self): if self.armService(False): return True else: print("Vehicle disarming failed!") return False # 设置为手动模式 def manual(self): if self.flightModeService(custom_mode='MANUAL'): return True else: print("Vechile MANUAL failed") return False ''' move quad in body frame ''' def distance(self, p1, p2): x_distance = (p2[0] - p1[0])**2 y_distance = (p2[1] - p1[1])**2 return np.sqrt(x_distance + y_distance) def remove_collinear_points(self, original_path): new_path = [] print("original_path length: ", len(original_path)) length = len(original_path) - 2 new_path.append(original_path[0]) # new_path.append(original_path[-1]) print(original_path) for i in range(length): distance13 = self.distance(original_path[i + 2], original_path[i]) distance12 = self.distance(original_path[i + 1], original_path[i]) distance23 = self.distance(original_path[i + 2], original_path[i + 1]) print("distance13 - distance12 - distance23 :", distance13 - distance12 - distance23) if abs(distance13 - distance12 - distance23) < 0.001: # print ("points collinear") continue else: print(original_path[i + 1]) print("not found collinear point") new_path.append(original_path[i + 1]) print("new path length: ", len(new_path)) print(new_path) return new_path def terminate_navigating(self): #TODO pass def resume_navigating(self): #TODO pass def do_hover(self): #TODO pass def set_target_postion(self, target_position): self.found_path = True # 当前位置,将目标位置进行离散 self.cur_target_position = target_position print("Current target position in grid: ", self.cur_target_position) #print("Set Current Position to: ", target_position[0], target_position[1], target_position[2]) def set_vision_target(self): self.set_status(status.GOING_TO_VISION_TARGET) self.set_target_position(xxxxx) #TODO pass def navi_task_terminated(self): if dist(self.local_pose, self.cur_target_position) < 0.25: #TODO: or stop flag is set. return True else: return False ''' Dstar Thread def Dstar_thread(self): while not rospy.is_shutdown(): while status!= xxx:# TODO next_move = xxx return next_move''' '''##For test: target = [0.5, 0.5, 0.5] self.set_target_postion(target) pass''' ''' ROS thread responsible for subscribers and publishers ''' def ros_thread(self): print('ros_thread spawn!!!!') self.octomap_msg = None # subscribers self.slam_sub = rospy.Subscriber("/gi/slam_output/pose", PoseStamped, self.slam_pose_callback) self.vision_target_sub = rospy.Subscriber("/gi/visual_target/pose", PoseStamped, self.vision_target_callback) self.point_cloud_sub = rospy.Subscriber("/camera/left/point_cloud", PointCloud, self.point_cloud_callback) # 实时点云数据 # self.point_cloud_sub = rospy.Subscriber("/cloud_in", PointCloud, self.point_cloud_callback) # self.octomap_cells_vis = rospy.Subscriber("/octomap_point_cloud_centers", PointCloud2, self.octomap_update_callback) #订阅八叉树地图 self.octomap_cells_vis = rospy.Subscriber( "/stereo_camera/points2", PointCloud2, self.octomap_update_callback) #订阅八叉树地图 # self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_callback) self.local_pose_sub = rospy.Subscriber("/mavros/global_position/local", Odometry, self.local_pose_callback) self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback) self.imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, self.imu_callback) # 获取罗盘值 # self.compass_hdg_sub = rospy.Subscriber("/mavros/global_position/compass_hdg", Float64, self.compass_hdg_callback) # 获取到设置的坐标以及 yaw self.local_target_sub = rospy.Subscriber('mavros/setpoint_raw/local', PositionTarget, self.set_point_callback) # publishers #self.mavros_control_pub = rospy.Publisher('mavros/Command', Command, queue_size=10) self.set_status(status.INITIALIZED) rospy.spin() ''' ROS callbacks ''' def set_point_callback(self, msg): pose_ = msg.position #TODO:do fusion with visual slam. self.set_point_pos = self.dg.continuous_to_discrete( (pose_.x, pose_.y, pose_.z)) # pass def imu_callback(self, msg): # global global_imu, current_heading # self.imu = msg self.current_yaw = int(self.q2yaw(msg.orientation) * 180 / math.pi) # self.received_imu = True def q2yaw(self, q): if isinstance(q, Quaternion): rotate_z_rad = q.yaw_pitch_roll[0] else: q_ = Quaternion(q.w, q.x, q.y, q.z) rotate_z_rad = q_.yaw_pitch_roll[0] return rotate_z_rad def slam_pose_callback(self, msg): self.slam_pose = msg def vision_target_callback(self, msg): self.vision_target = msg #print("Received New Vision Target!") def mavros_state_callback(self, msg): self.mavros_state = msg.mode #print(msg.mode, type(msg.mode)) self.navigator_status_pub.publish(self.STATUS) def point_cloud_callback(self, msg): self.current_point_cloud = msg def octomap_update_callback(self, msg): # as pointcloud2. # set 里面为 (x,y,z) 的元组 # print (msg) obs_set = set() # a = pc2.read_points(msg) for p in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): #print " x : %f y: %f z: %f" % (p[0], p[1], p[2]) point = self.dg.continuous_to_discrete( (p[0], p[1], p[2])) # 将点进行离散化 #print ('point:',point) obs_set.add(point) acquired = self.obstacle_set_mutex.acquire( True) # blocking. 尝试获得锁定。使线程进入同步阻塞状态 if acquired: #print('octomap updated!') self.driver.set_obstacle_set(obs_set) # 设置八叉树 self.obstacle_set_mutex.release() # 释放锁 return else: print('Lock not acquired!') def local_pose_callback(self, msg): pose_ = msg.pose.pose.position #TODO:do fusion with visual slam. local_pose = self.dg.continuous_to_discrete( (pose_.x, pose_.y, pose_.z)) self.local_pose = (local_pose[0], local_pose[1]) self.current_pos = self.local_pose #print ('local_pose set!!!') # def compass_hdg_callback(self, msg): # ''' # 罗盘数据回调函数 # msg 罗盘获取到的yaw 值 # ''' # self.current_yaw = msg.data def get_local_pose(self): # in mavros axis.for command. #print ('self.local_pose',self.local_pose) return self.local_pose def get_current_pose(self): # current pose:slam pose(in world axis) return self.get_local_pose() # TODO:do transform T1 ^-1 * T2. ''' helper functions ''' def set_status(self, status): self.STATUS = String(status.name) ''' def reachTargetPosition(self, target, threshold = 0.1): delta_x = math.fabs(self.local_pose.pose.position.x - target.pos_sp[0]) delta_y = math.fabs(self.local_pose.pose.position.y - target.pos_sp[1]) delta_z = math.fabs(self.local_pose.pose.position.z - target.pos_sp[2]) distance = (delta_x + delta_y + delta_z) print("distance: ", distance, "threshold: ", threshold) if distance < threshold: return True else: return False ''' def setMavMode(self, msg): pass
def __init__(self, config_file_path=None): if config_file_path: pass # Parameters for debugging self.is_disable_waiting_time = navigator2_config[ 'disable_waiting_time'] self.is_enable_testmap_publishing = navigator2_config[ 'enable_publishing_test_map'] rospy.init_node("gi_navigator_node") self.dg = DiscreteGridUtils.DiscreteGridUtils(grid_size=0.2) #0.2) self.rate = rospy.Rate(50) self.driver = astar.driver.Driver() self.controller = Controller() self.mavros_state = "OFFBOARD" self.set_status(status.INITIALIZED) self.cur_command_id = 0 self.prev_command_id = 0 self.cur_target_position = None # in grids self.cur_target_position_raw = None # in meters self.task_id = -1 self.obstacle_set_mutex = threading.Lock( ) # mutex.acquire(timeout);mutex.release() self.occupancy_grid_mutex = threading.Lock() self.nav_command_mutex = threading.Lock( ) # for nav command in dstar and ros high level command. self.local_pose = None # in meters self.local_pose_raw = None # in meters t1 = threading.Thread(target=self.ros_thread) t1.start() self.obs_set_raw = None self.is_local_pose_updated = False self.is_obstacle_set_updated = False self.navigator_status_pub = rospy.Publisher('/gi/navigator_status', String, queue_size=10) self.path_plan_pub = rospy.Publisher('/gi/navi_path_plan', MarkerArray, queue_size=10) #t2 = thread.start_new_thread(self.Dstar_thread, ()) # Parameters self.is_use_bresenham = navigator2_config[ 'is_use_bresenham'] # 这个开了以后是用3D地图避障,主要是视觉起作用,但是视觉精度比较低,在小环境内不好用 self.is_remove_collinear_pts = navigator2_config[ 'is_remove_collinear_pts'] # 这个开了以后会飞的更快,关了有更好的避障效果 # 避障可能会让飞机在某些地方“飞不动” self.is_rotate_uav = navigator2_config[ 'is_rotate_uav'] # 飞行过程中机头指向目标,开启后能更好发挥视觉作用,但是过快的旋转对激光建图不利 # battery status for emergency landing self.battery_persentage = 1. self.is_enable_low_battery_landing = navigator2_config[ 'enable_low_battery_landing'] self.min_battery_threshold = navigator2_config['battery_min_threshold'] self.path = [] self.path_prune = PathPruning(obstacle_distance=8) self.path_prune_2D = PathPruning_wScale( obstacle_distance=navigator2_config['pathpruning_obstacle_dst'], resolution=navigator2_config['pathpruning_resolution']) time.sleep(2)
class Navigator: def __init__(self, config_file_path=None): if config_file_path: pass # Parameters for debugging self.is_disable_waiting_time = navigator2_config[ 'disable_waiting_time'] self.is_enable_testmap_publishing = navigator2_config[ 'enable_publishing_test_map'] rospy.init_node("gi_navigator_node") self.dg = DiscreteGridUtils.DiscreteGridUtils(grid_size=0.2) #0.2) self.rate = rospy.Rate(50) self.driver = astar.driver.Driver() self.controller = Controller() self.mavros_state = "OFFBOARD" self.set_status(status.INITIALIZED) self.cur_command_id = 0 self.prev_command_id = 0 self.cur_target_position = None # in grids self.cur_target_position_raw = None # in meters self.task_id = -1 self.obstacle_set_mutex = threading.Lock( ) # mutex.acquire(timeout);mutex.release() self.occupancy_grid_mutex = threading.Lock() self.nav_command_mutex = threading.Lock( ) # for nav command in dstar and ros high level command. self.local_pose = None # in meters self.local_pose_raw = None # in meters t1 = threading.Thread(target=self.ros_thread) t1.start() self.obs_set_raw = None self.is_local_pose_updated = False self.is_obstacle_set_updated = False self.navigator_status_pub = rospy.Publisher('/gi/navigator_status', String, queue_size=10) self.path_plan_pub = rospy.Publisher('/gi/navi_path_plan', MarkerArray, queue_size=10) #t2 = thread.start_new_thread(self.Dstar_thread, ()) # Parameters self.is_use_bresenham = navigator2_config[ 'is_use_bresenham'] # 这个开了以后是用3D地图避障,主要是视觉起作用,但是视觉精度比较低,在小环境内不好用 self.is_remove_collinear_pts = navigator2_config[ 'is_remove_collinear_pts'] # 这个开了以后会飞的更快,关了有更好的避障效果 # 避障可能会让飞机在某些地方“飞不动” self.is_rotate_uav = navigator2_config[ 'is_rotate_uav'] # 飞行过程中机头指向目标,开启后能更好发挥视觉作用,但是过快的旋转对激光建图不利 # battery status for emergency landing self.battery_persentage = 1. self.is_enable_low_battery_landing = navigator2_config[ 'enable_low_battery_landing'] self.min_battery_threshold = navigator2_config['battery_min_threshold'] self.path = [] self.path_prune = PathPruning(obstacle_distance=8) self.path_prune_2D = PathPruning_wScale( obstacle_distance=navigator2_config['pathpruning_obstacle_dst'], resolution=navigator2_config['pathpruning_resolution']) time.sleep(2) ''' Navigating thread ''' def keep_navigating(self): ### Added, wait for several seconds for mavros switching to offboard for i in range(0, 25): if self.mavros_state == "OFFBOARD": break time.sleep(1) # wait a second for better initial mapping if not self.is_disable_waiting_time: print("wait for initial mapping...") time.sleep(15) while self.mavros_state == "OFFBOARD" and not (rospy.is_shutdown()): # print ('Inside outer loop!') #print ("Navigator state: ", self.STATUS.data, "Mavros state: ", self.mavros_state) relative_pos = (0, 0, 0) end_pos = self.get_latest_target() current_pos = self.get_current_pose() # TODO:fix this. if current_pos is None: #print ('current pose not valid!') continue while current_pos != end_pos and not self.navi_task_terminated( ) and not (rospy.is_shutdown()): # Till task is finished: ''' 几个比较坑的问题 1 地图提供的origin是地图(0, 0, 0)点对应实际坐标系的位置 2 2D地图和3D地图的分辨率是不同的 核心编程思想: 1 这个函数内所有的量纲都是公制,在各个模块内换成栅格,输出的时候再换回公制 2 打印一定要清晰 ''' if not self.is_local_pose_updated: time.sleep(1) continue else: self.is_local_pose_updated = False if not self.is_obstacle_set_updated: time.sleep(1) continue else: self.is_obstacle_set_updated = False # Land if battery is low if self.battery_persentage < self.min_battery_threshold: if self.is_enable_low_battery_landing: self.controller.mav_move( self.cur_target_position_raw[0], self.cur_target_position_raw[1], 0.) time.sleep(2) continue current_pos = self.get_current_pose() # in grids end_pos = self.dg.continuous_to_discrete( (self.cur_target_position_raw[0], self.cur_target_position_raw[1], self.cur_target_position_raw[2])) # in_grids # A* algorithm self.algo = astar.astar_2d.A_star_2D( self.cur_target_position_raw, vehicle_width=2, vehicle_length=2, resolution=self.occupancy_grid_raw.info.resolution) occupancy_map_resized = occupancy_grid_helper.resize( self.occupancy_grid_raw, 0.2) self.algo.update_map(occupancy_map_resized) # for debugging if self.is_enable_testmap_publishing: data = self.algo.map_list_to_occupancy_grid() self.occupancy_grid_pub.publish(data) path = self.algo.find_path(self.local_pose_raw, self.cur_target_position_raw) if path != None: print("2D path found!") # publish raw path plan. m_arr = MarkerArray() marr_index = 0 for point in alternative_path: mk = Marker() mk.header.frame_id = "map" mk.action = mk.ADD mk.id = marr_index marr_index += 1 mk.color.r = 1.0 mk.color.a = 1.0 mk.type = mk.CUBE mk.scale.x = 0.6 mk.scale.y = 0.6 mk.scale.z = 0.6 mk.pose.position.x = point[0] mk.pose.position.y = point[1] mk.pose.position.z = point[2] m_arr.markers.append(mk) self.path_plan_pub.publish(m_arr) if self.is_remove_collinear_pts: path = self.path_prune_2D.remove_collinear_points(path) if self.is_use_bresenham: path = self.path_prune_2D.path_pruning_bresenham3d( path, self.obs_set_raw) print("Path: ", path) else: print("2D path NOT found!") alternative_path = self.algo.find_alternative_path([ self.cur_target_position_raw[0], self.cur_target_position_raw[1] ]) # publish raw path plan. m_arr = MarkerArray() marr_index = 0 for point in alternative_path: mk = Marker() mk.header.frame_id = "map" mk.action = mk.ADD mk.id = marr_index marr_index += 1 mk.color.r = 1.0 mk.color.a = 1.0 mk.type = mk.CUBE mk.scale.x = 0.6 mk.scale.y = 0.6 mk.scale.z = 0.6 mk.pose.position.x = point[0] mk.pose.position.y = point[1] mk.pose.position.z = point[2] m_arr.markers.append(mk) self.path_plan_pub.publish(m_arr) # 路径简化 if self.is_remove_collinear_pts: alternative_path = self.path_prune_2D.remove_collinear_points( alternative_path) if self.is_use_bresenham: alternative_path = self.path_prune_2D.path_pruning_bresenham3d( alternative_path, self.obs_set_raw) # 因为在闭列表内,所以不可能找不到备用路径的 print("Alternative path found: ", alternative_path) #move_iter = 0 #move_iter_max = 3 for next_move in alternative_path: if self.navi_task_terminated(): break current_pos = self.get_current_pose() # in grids print('current_pos:', current_pos) next_pos = next_move relative_pos = (next_pos[0] - current_pos[0], next_pos[1] - current_pos[1], next_pos[2] - current_pos[2]) self.current_pos = next_pos #axis transform # TODO if not self.algo.is_valid(next_pos[0], next_pos[1], True): print('Path not valid!') break relative_pos_new = (-relative_pos[0], -relative_pos[1], relative_pos[2]) # calculate deired vehicle yaw if self.is_rotate_uav: target_yaw = atan2_yaw( relative_pos[1], relative_pos[0]) * 180. / math.pi else: target_yaw = 0 # alternative: None # move vehicle print('mav_move() input: relative pos=', next_pos) if self.is_use_bresenham: while self.distance(self.local_pose_raw, next_pos) >= 0.66: self.controller.mav_move(next_pos[0], next_pos[1], next_pos[2], abs_mode=True, yaw=target_yaw) time.sleep(2) else: self.controller.mav_move(next_pos[0], next_pos[1], next_pos[2], abs_mode=True, yaw=target_yaw) time.sleep(2) continue for next_move in path: if self.navi_task_terminated(): break current_pos = self.get_current_pose() # in grids print('current_pos:', current_pos) next_pos = next_move relative_pos = (next_pos[0] - current_pos[0], next_pos[1] - current_pos[1], next_pos[2] - current_pos[2]) # TODO if not self.algo.is_valid(next_pos[0], next_pos[1], True): print('Path not valid!') break self.current_pos = next_pos #axis transform relative_pos_new = (-relative_pos[0], -relative_pos[1], relative_pos[2]) # calculate deired vehicle yaw if self.is_rotate_uav: target_yaw = atan2_yaw( relative_pos[1], relative_pos[0]) * 180. / math.pi else: target_yaw = 0 # alternative: None # move vehicle print('mav_move() input: relative pos=', next_pos) if self.is_use_bresenham: while self.distance(self.local_pose_raw, next_pos) >= 0.66: self.controller.mav_move(next_pos[0], next_pos[1], next_pos[2], abs_mode=True, yaw=target_yaw) time.sleep(2) else: self.controller.mav_move(next_pos[0], next_pos[1], next_pos[2], abs_mode=True, yaw=target_yaw) time.sleep(2) current_pos = self.get_current_pose() time.sleep(0.05) # wait for new nav task. return print("Mavros not in OFFBOARD mode, Disconnected!") ''' move quad in body frame ''' def distance(self, p1, p2): x_distance = (p2[0] - p1[0])**2 y_distance = (p2[1] - p1[1])**2 z_distance = (p2[2] - p1[2])**2 return np.sqrt(x_distance + y_distance + z_distance) def terminate_navigating(self): #TODO pass def resume_navigating(self): #TODO pass def do_hover(self): #TODO pass def set_target_postion(self, target_position): self.found_path = True self.cur_target_position_raw = target_position self.cur_target_position = self.dg.continuous_to_discrete( target_position) print("Current target position in grid: ", self.cur_target_position) #print("Set Current Position to: ", target_position[0], target_position[1], target_position[2]) def get_latest_target(self): return self.cur_target_position def set_vision_target(self): self.set_status(status.GOING_TO_VISION_TARGET) self.set_target_position(xxxxx) #TODO pass def navi_task_terminated(self): if dist(self.local_pose, self.cur_target_position) < 0.25: #TODO: or stop flag is set. return True else: return False ''' Dstar Thread def Dstar_thread(self): while not rospy.is_shutdown(): while status!= xxx:# TODO next_move = xxx return next_move''' '''##For test: target = [0.5, 0.5, 0.5] self.set_target_postion(target) pass''' ''' ROS thread responsible for subscribers and publishers ''' def ros_thread(self): print('ros_thread spawn!!!!') self.octomap_msg = None # subscribers self.slam_sub = rospy.Subscriber("/gi/slam_output/pose", PoseStamped, self.slam_pose_callback) self.vision_target_sub = rospy.Subscriber("/gi/visual_target/pose", PoseStamped, self.vision_target_callback) self.point_cloud_sub = rospy.Subscriber("/camera/left/point_cloud", PointCloud, self.point_cloud_callback) self.occupancy_grid_sub = rospy.Subscriber( "/map", OccupancyGrid, self.occupancy_grid_callback) self.octomap_cells_vis = rospy.Subscriber( "/octomap_point_cloud_centers", PointCloud2, self.octomap_update_callback) self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_callback) self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback) self.mavros_sub = rospy.Subscriber("/mavros/battery", BatteryState, self.uav_battery_callback) # publishers #self.mavros_control_pub = rospy.Publisher('mavros/Command', Command, queue_size=10) if self.is_enable_testmap_publishing: self.occupancy_grid_pub = rospy.Publisher('/map_test', OccupancyGrid, queue_size=10) self.set_status(status.INITIALIZED) rospy.spin() ''' ROS callbacks ''' def slam_pose_callback(self, msg): self.slam_pose = msg def vision_target_callback(self, msg): self.vision_target = msg #print("Received New Vision Target!") def mavros_state_callback(self, msg): self.mavros_state = msg.mode #print(msg.mode, type(msg.mode)) self.navigator_status_pub.publish(self.STATUS) def point_cloud_callback(self, msg): self.current_point_cloud = msg def occupancy_grid_callback(self, msg): self.occupancy_grid_raw = msg def octomap_update_callback(self, msg): # as pointcloud2. obs_set = set() obs_set_raw = set() for p in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): #print " x : %f y: %f z: %f" % (p[0], p[1], p[2]) point = self.dg.continuous_to_discrete((p[0], p[1], p[2])) #print ('point:',point) obs_set.add(point) obs_set.add(p) acquired = self.obstacle_set_mutex.acquire(True) # blocking. if acquired: #print('octomap updated!') self.driver.set_obstacle_set(obs_set) self.obs_set_raw = obs_set_raw self.obstacle_set_mutex.release() self.is_obstacle_set_updated = True return else: print('Lock not acquired!') def local_pose_callback(self, msg): pose_ = msg.pose.position #TODO:do fusion with visual slam. self.local_pose_raw = (pose_.x, pose_.y, pose_.z) self.local_pose = self.dg.continuous_to_discrete( (pose_.x, pose_.y, pose_.z)) #print ('local_pose set!!!') self.is_local_pose_updated = True def uav_battery_callback(self, msg): self.battery_persentage = msg.percentage def get_local_pose(self): # in mavros axis.for command. #print ('self.local_pose',self.local_pose) return self.local_pose def get_current_pose(self): # current pose:slam pose(in world axis) return self.get_local_pose() # TODO:do transform T1 ^-1 * T2. ''' helper functions ''' def set_status(self, status): self.STATUS = String(status.name) ''' TODO ''' def reachTargetPosition(self, target, threshold=0.1): delta_x = math.fabs(self.local_pose.pose.position.x - target.pos_sp[0]) delta_y = math.fabs(self.local_pose.pose.position.y - target.pos_sp[1]) delta_z = math.fabs(self.local_pose.pose.position.z - target.pos_sp[2]) distance = (delta_x**2 + delta_y**2 + delta_z**2)**0.5 print("distance: ", distance, "threshold: ", threshold) if distance < threshold: return True else: return False def setMavMode(self, msg): pass