def speed_limit(new_goal, prev_goal, dt, max_speed): """ :type prev_goal: Goal :type _goal: Goal :param dt: float :param max_speed: float :return: limit goal """ limit_goal = Goal() vec = [ new_goal.pose.point.x - prev_goal.pose.point.x, new_goal.pose.point.y - prev_goal.pose.point.y, new_goal.pose.point.z - prev_goal.pose.point.z ] dist = np.linalg.norm(vec) if dist > max_speed: res = vec / dist * max_speed * dt limit_goal.pose.point.x = res[0] + prev_goal.pose.point.x limit_goal.pose.point.y = res[1] + prev_goal.pose.point.y limit_goal.pose.point.z = res[2] + prev_goal.pose.point.z return limit_goal else: return new_goal
def rotate_vect(a, b, rot): """ Поворачиваем b относительно a на угол rot :type a: Point :type b: Point :type rot: float :type dit: float :param a: common goal pose :param b: drone_0_offset of drone goal :param rot: rotate of current pose :return: возвращаем точку повёрнутую на нужный угол """ new_goal = Goal() rotate = np.array([[math.cos(rot), -math.sin(rot)], [math.sin(rot), math.cos(rot)]]) pos = np.array([[b.x], [b.y]]) res = np.dot(rotate, pos) res[0] += a.x res[1] += a.y new_goal.pose.point.x = float(res[0]) new_goal.pose.point.y = float(res[1]) new_goal.pose.point.z = float(a.z + b.z) new_goal.pose.course = float(rot) return new_goal
def goal_clb(data): """ Get common goal pose :type data:Goal :return: """ global goal_common_msgs, drone_offset_list, markers_lerp, markers_goal, old_time, state_init_flag goal_common_msgs = data drone_goal_msgs = Goal() name = "" dt = rospy.get_time() - old_time # Перебираем массив всех дронов for i in range(len(drone_offset_list)): name_of_drone = drone_offset_list[i][0] # получаем целевую точку куда нужно двигаться drone_goal_msgs = rotate_vect(goal_common_msgs.pose.point, drone_offset_list[i][1], goal_common_msgs.pose.course) drone_goal_msgs.pose.course = data.pose.course # Интерполируем точку от текущей до целевой if state_init_flag: lerp_goal = speed_limit(drone_goal_msgs, drone_offset_list[i][2], dt, max_vel) lerp_goal.pose.course = data.pose.course else: lerp_goal = drone_goal_msgs # отталкиваемся от соседей repel_vec = repel_from_near(lerp_goal, i, free_zone) lerp_goal.pose.point.x += repel_vec[0] * dt * 1.5 lerp_goal.pose.point.y += repel_vec[1] * dt * 1.5 lerp_goal.pose.point.z += repel_vec[2] * dt * 1.5 # указываем интерполированную точку как предыдущую drone_offset_list[i][2] = lerp_goal markers_lerp.markers[i] = setup_market(name_of_drone, lerp_goal.pose.point, i, [0.0, 0.0, 1.0, 1.0]) markers_goal.markers[i] = setup_market(name_of_drone, drone_goal_msgs.pose.point, i, [0.0, 1.0, 0.0, 1.0]) rospy.Publisher(name_of_drone + "/geo/goal_pose", Goal, queue_size=10).publish(lerp_goal) # print("len array:", len(drone_offset_list), size_of_drone) pub_marker_goal_array.publish(markers_goal) pub_marker_goal_lerp_array.publish(markers_lerp) old_time = rospy.get_time() state_init_flag = True
def MarkerFeedbackClb(data): """ Geet marker feedback :type data: InteractiveMarkerFeedback :return: """ global goal_pose, target_course if goal_pose == None: goal_pose = Goal() goal_pose.pose.point = data.pose.position target_course = get_yaw_from_quat( (data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w))
def load_params(path): """ Load data of drones from yaml :type path: str :return: """ global size_of_drone, drone_offset_list, markers_goal try: file = open(path, "r") data = str(file.read()) file.close() except: rospy.logerr("Param error path") sys.exit() # set data "name" and "offset" to list size_of_drone = yaml.load(data)['size'] i = 0 while len(drone_offset_list) < size_of_drone: try: offset_str = yaml.load(data)[tag + str(i)] offset_data = Point() offset_data.x = offset_str['x'] offset_data.y = offset_str['y'] offset_data.z = offset_str['z'] new_goal = Goal() new_goal.pose.point.x = offset_data.x new_goal.pose.point.y = offset_data.y new_goal.pose.point.z = offset_data.z drone_offset_list.append([tag + str(i), offset_data, new_goal]) markers_goal.markers.append(Marker()) markers_lerp.markers.append(Marker()) except: rospy.logerr("%s:param not found" % (tag + str(i))) i += 1
# класс хранящий основные параметры найденных контуров class contour_obj: # конструктор def __init__(self): self.name = None self.cords = [] self.mask = [] # флаги view_window_flag = False # фдаг отображения окон с результатами обработки изображений сделано для отладки # переменные drone_alt = 0.0 # текущая высота дрона drone_pose = PoseStamped() # текущая позиция дрона в глобальной системе координат goal_point = Goal() # целевая точка, в которую должен лететь дрон max_resize = (64, 64) # задаем максимальный размер кадра для "ресайза" выделенных контуров # названия путей logo_of_object = 'land_point_blue.png' camera_file_port = "/dev/video2" # stereo elp >> /dev/video2, /dev/video4 # topics alt_topic = "/drone/alt" # топик текущей высоты drone_pose_topic = "/mavros/local_position/pose" # топик текущей позиции drone_goal_pose = "/goal_pose" # топик целевой точки camera_server_topic = "/camera_server" # топик передачи картинки на сервер просмотра(для удаленного отображения картинки на ПК управления) # делаем захват видео с камеры в переменную cap cap = cv.VideoCapture(camera_file_port) cap.set(cv.CAP_PROP_FPS, 24) # Частота кадров
#!/usr/bin/env python # coding: utf-8 import rospy from geometry_msgs.msg import Pose, PoseStamped from drone_msgs.msg import Goal import tf import numpy as np from std_srvs.srv import Empty goal_pose = Goal() init_goal_flag = False resetPoseTopic = "/drone/goal_pose/reset" localPoseTopic = "/mavros/local_position/pose" localPose = None def goal_clb(data): """ Get goal pose """ global goal_pose, init_goal_flag goal_pose= data init_goal_flag = True def resetCb(req): print("reset goal pose") global localPose, goal_pose
local_pose_topic = "/mavros/local_position/pose" geo_pose_topic = "/geo/local_pose" ctr_type = Goal.POSE # По умолчанию используем управление по координатам drone_geo_pose = Pose() # Координаты дрона drone_local_pose = Pose() # Координаты дрона geo_offset = [0.0, 0.0, 0.0] # смещение глобальных координат относительно локальных goal_msgs = Pose() goal_msgs = PoseStamped() goal_msgs.header.frame_id = 'map' goal_point = Goal() drone_status = {'armed': False, 'mode': None, 'landed': False} goal_timer = 0.0 geo_pose_timer = 0.0 local_pose_timer = 0.0 goal_lost_time = 1.0 pose_lost_time = 0.5 print_delay = 3. def nav_geo_pos_cb(data): global drone_geo_pose, geo_pose_timer drone_geo_pose = data.pose
beta_max = k_buff * (r_view - rc_min) # максимально возможная бета dist_offset = 1.0 # дистанция на которую ставится виртуальная точка от дрона # топики лазерскан и поинтклауд lidar_topic = "/scan" octomap_topic = "/scan_matched_points2" goal_sub_topic = "/goal_pose" goal_pub_topic = "/goal_pose_to_reg" pose_topic = "/mavros/local_position/pose" velocity_topic = "/mavros/local_position/velocity" marker_topic = "/marker_target_point" lp = lg.LaserProjection() target_pose = Goal() # целевая точка virtual_target_pose = Goal() # виртуальная ЦТ current_pose = [0.0, 0.0, 0.0] # текущая позиция дрона current_course = 0.0 # текущий курс дрона vec_to_goal = [0.0, 0.0, 0.0] # вектор между дроном и ЦТ vel_vec_2d = [0.0, 0.0] # вектор скорости в плоскости min_dist = 999.0 # расстояние до ближайщего препятствия min_dist_index = 0 # угол по лидару до ближ. преп. angle_rad = 0.0 # угол на который надо повернуть init_server = False active_flag = True # состояние, включен ли планировщик yaml_path = os.path.dirname(__file__) + '/../../cfg/params.yaml' cfg_srv = None
def get_drone_pose(common_goal, offset): new_pose = Goal() return new_pose