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)
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)
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)