#!/usr/bin/env?python

#?title:?meta-Cluster?Location?Publisher
#?Description:?Clusters?the?clusters?into?metaclusters
#?Engineer:?Jonathan?Lwowski?
#?Email:?jonathan.lwowski@gmail.com
#?Lab:?Autonomous?Controls?Lab?The?University?of?Texas?at?San?Antonio


#########??????????Libraries?????????###################
import?sys
from?std_msgs.msg?import?String
from?std_msgs.msg?import?Header
import?rospy
import?math
import?numpy?as?np
import?time
from?gazebo_msgs.msg?import?ModelStates
from?geometry_msgs.msg?import?PoseArray
from?geometry_msgs.msg?import?Pose
from?geometry_msgs.msg?import?Point
from?geometry_msgs.msg?import?Quaternion
from?sklearn?import?cluster
from?gazebo_msgs.srv?import?SpawnModel
from?gazebo_msgs.srv?import?DeleteModel
from?cooperative_mav_asv.msg?import?*
from?cooperative_mav_asv.srv?import?*


###?Functions?to?Find?Smalled?Bounding?Circle??###

def?make_circle（points）:
????#?Convert?to?float?and?randomize?order
????shuffled?=?[（float（p[0]）?float（p[1]））?for?p?in?points]
?
????
????#?Progressively?add?points?to?circle?or?recompute?circle
????c?=?None
????for?（i?p）?in?enumerate（shuffled）:
????????if?c?is?None?or?not?_is_in_circle（c?p）:
????????????c?=?_make_circle_one_point（shuffled[0?:?i?+?1]?p）
????return?c


#?One?boundary?point?known
def?_make_circle_one_point（points?p）:
????c?=?（p[0]?p[1]?0.0）
????for?（i?q）?in?enumerate（points）:
????????if?not?_is_in_circle（c?q）:
????????????if?c[2]?==?0.0:
????????????????c?=?_make_diameter（p?q）
????????????else:
????????????????c?=?_make_circle_two_points（points[0?:?i?+?1]?p?q）
????return?c


#?Two?boundary?points?known
def?_make_circle_two_points（points?p?q）:
????diameter?=?_make_diameter（p?q）
????if?all（_is_in_circle（diameter?r）?for?r?in?points）:
????????return?diameter
????
????left?=?None
????right?=?None
????for?r?in?points:
????????cross?=?_cross_product（p[0]?p[1]?q[0]?q[1]?r[0]?r[1]）
????????c?=?_make_circumcircle（p?q?r）
????????if?c?is?None:
????????????continue
????????elif?cross?>?0.0?and?（left?is?None?or?_cross_product（p[0]?p[1]?q[0]?q[1]?c[0]?c[1]）?>?_cross_product（p[0]?p[1]?q[0]?q[1]?left[0]?left[1]））:
????????????left?=?c
????????elif?cross?????????????right?=?c
????return?left?if?（right?is?None?or?（left?is?not?None?and?left[2]?<=?right[2]））?else?right


def?_make_circumcircle（p0?p1?p2）:
????#?Mathematical?algorithm?from?Wikipedia:?Circumscribed?circle
????ax?=?p0[0];?ay?=?p0[1]
????bx?=?p1[0];?by?=?p1[1]
????cx?=?p2[0];?cy?=?p2[1]
????d?=?（ax?*?（by?-?cy）?+?bx?*?（cy?-?ay）?+?cx?*?（ay?-?by））?*?2.0
????if?d?==?0.0:
????????return?None
????x?=?（（ax?*?ax?+?ay?*?ay）?*?（by?-?cy）?+?（bx?*?bx?+?by?*?by）?*?（cy?-?ay）?+?（cx?*?cx?+?cy?*?cy）?*?（ay?-?by））?/?d
????y?=?（（ax?*?ax?+?ay?*?ay）?*?（cx?-?bx）?+?（bx?*?bx?+?by?*?by）?*?（ax?-?cx）?+?（cx?*?cx?+?cy?*?cy）?*?（bx?-?ax））?/?d
????return?（x?y?math.hypot（x?-?ax?y?-?ay））


d

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2016-10-21?16:34??DeepLearningPID-master\
?????文件????????6217??2016-10-21?16:34??DeepLearningPID-master\CMakeLists.txt
?????文件???????35141??2016-10-21?16:34??DeepLearningPID-master\LICENSE
?????文件?????????320??2016-10-21?16:34??DeepLearningPID-master\README.md
?????文件?????????536??2016-10-21?16:34??DeepLearningPID-master\README.md~
?????目錄???????????0??2016-10-21?16:34??DeepLearningPID-master\launch\
?????文件????????1302??2016-10-21?16:34??DeepLearningPID-master\launch\deep_learning_pid.launch
?????文件????????1166??2016-10-21?16:34??DeepLearningPID-master\launch\deep_learning_pid.launch~
?????文件?????????976??2016-10-21?16:34??DeepLearningPID-master\launch\empty_world.launch~
?????文件????????1331??2016-10-21?16:34??DeepLearningPID-master\launch\p3dx.gazebo.launch
?????文件????????1327??2016-10-21?16:34??DeepLearningPID-master\launch\p3dx.gazebo.launch~
?????文件????????1223??2016-10-21?16:34??DeepLearningPID-master\launch\spawn_quadrotor_with_downward_cam.launch
?????文件????????1223??2016-10-21?16:34??DeepLearningPID-master\launch\spawn_quadrotor_with_downward_stereo_cam.launch
?????文件????????5199??2016-10-21?16:34??DeepLearningPID-master\launch\universal_boat_world_multiple_mav_local_cloud.launch~
?????文件????????5207??2016-10-21?16:34??DeepLearningPID-master\launch\universal_boat_world_multiple_mav_server_cloud.launch~
?????目錄???????????0??2016-10-21?16:34??DeepLearningPID-master\msg\
?????文件??????????28??2016-10-21?16:34??DeepLearningPID-master\msg\AuctionAssignments.msg
?????文件??????????70??2016-10-21?16:34??DeepLearningPID-master\msg\BoatAuctionInfo.msg
?????文件??????????65??2016-10-21?16:34??DeepLearningPID-master\msg\ClusterCenter.msg
?????文件??????????26??2016-10-21?16:34??DeepLearningPID-master\msg\ClusterCenters.msg
?????文件??????????87??2016-10-21?16:34??DeepLearningPID-master\msg\metaCluster.msg
?????文件??????????28??2016-10-21?16:34??DeepLearningPID-master\msg\metaClusters.msg
?????文件????????2141??2016-10-21?16:34??DeepLearningPID-master\package.xml
?????目錄???????????0??2016-10-21?16:34??DeepLearningPID-master\rviz\
?????文件????????4973??2016-10-21?16:34??DeepLearningPID-master\rviz\rviz_config.rviz
?????目錄???????????0??2016-10-21?16:34??DeepLearningPID-master\scripts\
?????目錄???????????0??2016-10-21?16:34??DeepLearningPID-master\scripts\backup\
?????文件????????4733??2016-10-21?16:34??DeepLearningPID-master\scripts\backup\cluster_location_service.py~
?????文件????????7564??2016-10-21?16:34??DeepLearningPID-master\scripts\backup\clusters_of_clusters_service.py~
?????文件????????4320??2016-10-21?16:34??DeepLearningPID-master\scripts\backup\universal_boat_auctioner_service.py~
?????文件???????12301??2016-10-21?16:34??DeepLearningPID-master\scripts\backup\universal_boat_service.py~
............此處省略77個文件信息