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