changes in barrier and new graph from GPS

This commit is contained in:
dave 2017-09-05 00:34:59 -04:00
parent 7268209d35
commit ccdb80a4cf
8 changed files with 980 additions and 106 deletions

View File

@ -44,7 +44,7 @@ m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Respo
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#Current robot state #Current robot state
m_eState="STATE_FREE" # m_eState="STATE_FREE" # replace with default UAVSTATE
#navigation vector #navigation vector
m_navigation={.x=0,.y=0} m_navigation={.x=0,.y=0}
@ -361,19 +361,19 @@ function UpdateNodeInfo(){
#Transistion to state free #Transistion to state free
# #
function TransitionToFree(){ function TransitionToFree(){
m_eState="STATE_FREE" UAVSTATE="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(UAVSTATE)
} }
# #
#Transistion to state asking #Transistion to state asking
# #
function TransitionToAsking(un_label){ function TransitionToAsking(un_label){
m_eState="STATE_ASKING" UAVSTATE="STATE_ASKING"
m_nLabel=un_label m_nLabel=un_label
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId m_selfMessage.ReqID=m_unRequestId
@ -384,8 +384,8 @@ function TransitionToAsking(un_label){
#Transistion to state joining #Transistion to state joining
# #
function TransitionToJoining(){ function TransitionToJoining(){
m_eState="STATE_JOINING" UAVSTATE="STATE_JOINING"
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod m_unWaitCount=m_unJoiningLostPeriod
@ -405,8 +405,8 @@ function TransitionToJoining(){
#Transistion to state joined #Transistion to state joined
# #
function TransitionToJoined(){ function TransitionToJoined(){
m_eState="STATE_JOINED" UAVSTATE="STATE_JOINED"
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED" m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("r") neighbors.ignore("r")
@ -426,8 +426,8 @@ function TransitionToJoined(){
#Transistion to state Lock, lock the current formation #Transistion to state Lock, lock the current formation
# #
function TransitionToLock(){ function TransitionToLock(){
m_eState="STATE_LOCK" UAVSTATE="STATE_LOCK"
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED" m_vecNodes[m_nLabel].State="ASSIGNED"
#record neighbor distance #record neighbor distance
@ -448,7 +448,7 @@ while(i<m_neighbourCount){
# Do free # Do free
# #
function DoFree() { function DoFree() {
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(UAVSTATE)
#wait for a while before looking for a Label #wait for a while before looking for a Label
if(m_unWaitCount>0) if(m_unWaitCount>0)
@ -527,7 +527,7 @@ function DoFree() {
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
} }
#set message #set message
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(UAVSTATE)
} }
@ -558,7 +558,7 @@ function DoAsking(){
if(psResponse==-1){ if(psResponse==-1){
#no response, wait #no response, wait
m_unWaitCount=m_unWaitCount-1 m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId m_selfMessage.ReqID=m_unRequestId
if(m_unWaitCount==0){ if(m_unWaitCount==0){
@ -576,7 +576,6 @@ function DoAsking(){
if(m_MessageReqID[psResponse]==m_unRequestId){ if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){ if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining() TransitionToJoining()
#TransitionToJoined()
return return
} }
else{ else{
@ -656,7 +655,7 @@ function DoJoining(){
} }
#pack the communication package #pack the communication package
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
} }
@ -665,7 +664,7 @@ function DoJoining(){
#Do joined #Do joined
# #
function DoJoined(){ function DoJoined(){
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
#collect all requests #collect all requests
@ -754,7 +753,7 @@ function DoJoined(){
#Do Lock #Do Lock
# #
function DoLock(){ function DoLock(){
m_selfMessage.State=s2i(m_eState) m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
@ -772,7 +771,10 @@ uav_moveto(m_navigation.x, m_navigation.y, 0.0)
function action(){ function action(){
statef=action statef=action
UAVSTATE="GRAPH" UAVSTATE="STATE_FREE"
# reset the graph
Reset()
} }
# #
@ -792,27 +794,24 @@ function init() {
# Join Swarm # Join Swarm
# #
uav_initswarm() uav_initswarm()
v_tag = stigmergy.create(m_lockstig) v_tag = stigmergy.create(m_lockstig)
uav_initstig() uav_initstig()
# go to diff. height since no collision avoidance implemented yet # go to diff. height since no collision avoidance implemented yet
TARGET_ALTITUDE = 2.5 + id * 1.5 TARGET_ALTITUDE = 2.5 + id * 1.5
statef=turnedoff statef=turnedoff
UAVSTATE = "TURNEDOFF" UAVSTATE = "TURNEDOFF"
# reset the graph
Reset()
} }
# #
# Executed every step # Executed every step
# #
function step(){ function step() {
# listen to potential RC # listen to potential RC
uav_rccmd() uav_rccmd()
# get the swarm commands # get the swarm commands
uav_neicmd() uav_neicmd()
# update the vstig (status/net/batt) # update the vstig (status/net/batt)
uav_updatestig() uav_updatestig()
#update the graph #update the graph
UpdateNodeInfo() UpdateNodeInfo()
@ -821,27 +820,23 @@ function step(){
# #
# graph state machine # graph state machine
# #
if(UAVSTATE=="GRAPH"){ if(UAVSTATE=="STATE_FREE")
if(m_eState=="STATE_FREE") statef=DoFree
DoFree() else if(UAVSTATE=="STATE_ESCAPE")
else if(m_eState=="STATE_ESCAPE") statef=DoEscape
DoEscape() else if(UAVSTATE=="STATE_ASKING")
else if(m_eState=="STATE_ASKING") statef=DoAsking
DoAsking() else if(UAVSTATE=="STATE_JOINING")
else if(m_eState=="STATE_JOINING") statef=DoJoining
DoJoining() else if(UAVSTATE=="STATE_JOINED")
else if(m_eState=="STATE_JOINED") statef=DoJoined
DoJoined() else if(UAVSTATE=="STATE_LOCK")
else if(m_eState=="STATE_LOCK") statef=DoLock
DoLock()
}
# high level UAV state machine # high level UAV state machine
statef() statef()
log("Current state: ", UAVSTATE, " and label: ", m_nLabel)
debug(m_eState,m_nLabel)
log("Current state: ", UAVSTATE)
log("Swarm size: ", ROBOTS) log("Swarm size: ", ROBOTS)
#navigation #navigation
@ -868,12 +863,8 @@ function step(){
# Executed when reset # Executed when reset
# #
function Reset(){ function Reset(){
# m_vecNodes={}
# m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary
# m_vecNodes_fixed={}
# m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph")
Read_Graph() Read_Graph()
m_nLabel=-1 m_nLabel=-1
#start listening #start listening
start_listen() start_listen()

View File

@ -0,0 +1,829 @@
#
# Include files
#
include "string.bzz"
include "vec2.bzz"
include "update.bzz"
include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
include "barrier.bzz" # reserve stigmergy id=11 for this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "graphs/shapes_Y.bzz"
ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
ROOT_ID = 2
# max velocity in cm/step
ROBOT_MAXVEL = 150.0
#
# Global variables
#
#
# Save message from all neighours
#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot
m_MessageState={}#store received neighbour message
m_MessageLabel={}#store received neighbour message
m_MessageReqLabel={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCount=0#used to cunt neighbours
#Save message from one neighbour
#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing
m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
#
#Save the message to send
#The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#Current robot state
# m_eState="STATE_FREE" # replace with default UAVSTATE
#navigation vector
m_navigation={.x=0,.y=0}
#Debug message to be displayed in qt-opengl
#m_ossDebugMsg
#Debug vector to draw
#CVector2 m_cDebugVector
#Current label being requested or chosen (-1 when none)
m_nLabel=-1
m_messageID={}
#neighbor distance to lock the current pattern
lock_neighbor_id={}
lock_neighbor_dis={}
#Label request id
m_unRequestId=0
#Global bias, used to map local coordinate to global coordinate
m_bias=0
#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate
m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
#Counter to wait for something to happen
m_unWaitCount=0
#Number of steps to wait before looking for a free label
m_unLabelSearchWaitTime=0
#Number of steps to wait for an answer to be received
m_unResponseTimeThreshold=0
#Number of steps to wait until giving up joining
m_unJoiningLostPeriod=0
#Tolerance distance to a target location
m_fTargetDistanceTolerance=0
#step cunt
step_cunt=0
# virtual stigmergy for the LOCK barrier.
m_lockstig = 1
# Lennard-Jones parameters, may need change
EPSILON = 1800 #13.5 the LJ parameter for other robots
# Lennard-Jones interaction magnitude
function FlockInteraction(dist,target,epsilon){
var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return mag
}
#
#return the number of value in table
#
function count(table,value){
var number=0
var i=0
while(i<size(table)){
if(table[i]==value){
number=number+1
}
i=i+1
}
return number
}
#map from int to state
function i2s(value){
if(value==1){
return "STATE_FREE"
}
else if(value==2){
return "STATE_ASKING"
}
else if(value==3){
return "STATE_JOINING"
}
else if(value==4){
return "STATE_JOINED"
}
else if(value==5){
return "STATE_LOCK"
}
}
#map from state to int
function s2i(value){
if(value=="STATE_FREE"){
return 1
}
else if(value=="STATE_ASKING"){
return 2
}
else if(value=="STATE_JOINING"){
return 3
}
else if(value=="STATE_JOINED"){
return 4
}
else if(value=="STATE_LOCK"){
return 5
}
}
#map form int to response
function i2r(value){
if(value==1){
return "REQ_NONE"
}
else if(value==2){
return "REQ_GRANTED"
}
}
#map from response to int
function r2i(value){
if(value=="REQ_NONE"){
return 1
}
else if(value=="REQ_GRANTED"){
return 2
}
}
#
#return the index of value
#
function find(table,value){
var index=nil
var i=0
while(i<size(table)){
if(table[i]==value)
index=i
i=i+1
}
return index
}
#
#pack message into 1 number
#
function packmessage(send_table){
var send_value
send_value=100000*send_table.State+10000*send_table.Label+1000*send_table.ReqLabel+10*send_table.ReqID+send_table.Response
return send_value
}
#
#pack guide message into 1 number
#
function pack_guide_msg(send_table){
var send_value
var r_id=send_table.Label#id of target robot
var pon#positive or negative ,0 postive, 1 negative
if(send_table.Bearing>=0){
pon=0
}
else{
pon=1
}
var b=math.abs(send_table.Bearing)
send_value=r_id*1000+pon*100+b
return send_value
}
#
#unpack message
#
function unpackmessage(recv_value){
var wan=(recv_value-recv_value%100000)/100000
recv_value=recv_value-wan*100000
var qian=(recv_value-recv_value%10000)/10000
recv_value=recv_value-qian*10000
var bai=(recv_value-recv_value%1000)/1000
recv_value=recv_value-bai*1000
var shi=(recv_value-recv_value%10)/10
recv_value=recv_value-shi*10
var ge=recv_value
var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0}
return_table.State=wan
return_table.Label=qian
return_table.ReqLabel=bai
return_table.ReqID=shi
return_table.Response=ge
return return_table
}
#
#unpack guide message
#
function unpack_guide_msg(recv_value){
log(id,"I pass value=",recv_value)
var qian=(recv_value-recv_value%1000)/1000
recv_value=recv_value-qian*1000
var bai=(recv_value-recv_value%100)/100
recv_value=recv_value-bai*100
var b=recv_value
var return_table={.Label=0.0,.Bearing=0.0}
return_table.Label=qian
if(bai==1){
b=b*-1.0
}
return_table.Bearing=b
return return_table
}
#get the target distance to neighbr nei_id
function target4label(nei_id){
var return_val="miss"
var i=0
while(i<size(lock_neighbor_id)){
if(lock_neighbor_id[i]==nei_id){
return_val=lock_neighbor_dis[i]
}
i=i+1
}
return return_val
}
#calculate LJ vector for neibhor stored in i
function LJ_vec(i){
var dis=m_MessageRange[i]
var bearing=m_MessageBearing[i]
var nei_id=m_messageID[i]
var target=target4label(nei_id)
var cDir={.x=0.0,.y=0.0}
if(target!="miss"){
cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),bearing)
}
#log(id,"dis=",dis,"target=",target,"label=",nei_id)
#log("x=",cDir.x,"y=",cDir.y)
return cDir
}
#calculate the motion vector
function motion_vector(){
var i=0
var m_vector={.x=0.0,.y=0.0}
while(i<m_neighbourCount){
#calculate and add the motion vector
m_vector=math.vec2.add(m_vector,LJ_vec(i))
#log(id,"x=",m_vector.x,"y=",m_vector.y)
i=i+1
}
m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
#log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y)
return m_vector
}
function start_listen(){
neighbors.listen("m",
function(vid,value,rid){
#store the received message
var temp_id=rid
var recv_val=unpackmessage(value)
Get_DisAndAzi(temp_id)
#add the received message
#
m_MessageState[m_neighbourCount]=i2s(recv_val.State)
m_MessageLabel[m_neighbourCount]=recv_val.Label
m_MessageReqLabel[m_neighbourCount]=recv_val.ReqLabel
m_MessageReqID[m_neighbourCount]=recv_val.ReqID
m_MessageResponse[m_neighbourCount]=i2r(recv_val.Response)
m_MessageRange[m_neighbourCount]=m_receivedMessage.Range
m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
m_messageID[m_neighbourCount]=rid
m_neighbourCount=m_neighbourCount+1
})
}
#
#Function used to get the station info of the sender of the message
function Get_DisAndAzi(id){
neighbors.foreach(
function(rid, data) {
if(rid==id){
m_receivedMessage.Range=data.distance*100.0
m_receivedMessage.Bearing=data.azimuth
}
})
}
#
#Update node info according to neighbour robots
#
function UpdateNodeInfo(){
#Collect informaiton
#Update information
var i=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_JOINED"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
}
else if(m_MessageState[i]=="STATE_JOINING"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
}
i=i+1
}
#Forget old information
i=0
while(i<size(m_vecNodes)){
if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
if(m_vecNodes[i].StateAge==0)
m_vecNodes[i].State="UNASSIGNED"
}
i=i+1
}
}
#
#Transistion to state free
#
function TransitionToFree(){
UAVSTATE="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(UAVSTATE)
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
UAVSTATE="STATE_ASKING"
m_nLabel=un_label
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_unWaitCount=m_unResponseTimeThreshold
}
#
#Transistion to state joining
#
function set_rc_goto() {
#get information of pred
var i=0
var IDofPred=-1
while(i<m_neighbourCount and IDofPred==-1){
if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
IDofPred=i
i=i+1
}
#found pred
if(IDofPred!=-1){
m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
#attention, m_cMeToPred.GlobalBearing is the bearing of self wrt pred in global coordinate
var S2PGlobalBearing=0
m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing)
if(m_cMeToPred.GlobalBearing>math.pi)
S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi
else
S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi
var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing)
#the vector from self to target in global coordinate
var S2Target=math.vec2.add(S2Pred,P2Target)
#change the vector to local coordinate of self
var S2Target_bearing=math.atan(S2Target.y, S2Target.x)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias
gps_from_rb(math.vec2.length(S2Target)/100.0, S2Target_bearing)
uav_storegoal(goal.latitude, goal.longitude, position.altitude)
}
}
function TransitionToJoining(){
UAVSTATE="STATE_JOINING"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
set_rc_goto()
neighbors.listen("r",
function(vid,value,rid){
var recv_table={.Label=0,.Bearing=0.0}
recv_table=unpack_guide_msg(value)
#store the received message
if(recv_table.Label==m_nLabel){
m_cMeToPred.GlobalBearing=recv_table.Bearing
}
})
}
#
#Transistion to state joined
#
function TransitionToJoined(){
UAVSTATE="STATE_JOINED"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("r")
#write statues
#v_tag.put(m_nLabel, m_lockstig)
barrier_create(m_lockstig)
barrier_ready()
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
UAVSTATE="STATE_LOCK"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#record neighbor distance
lock_neighbor_id={}
lock_neighbor_dis={}
var i=0
while(i<m_neighbourCount){
lock_neighbor_id[i]=m_messageID[i]
lock_neighbor_dis[i]=m_MessageRange[i]
i=i+1
}
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
}
#
# Do free
#
function DoFree() {
m_selfMessage.State=s2i(UAVSTATE)
#wait for a while before looking for a Label
if(m_unWaitCount>0)
m_unWaitCount=m_unWaitCount-1
#find a set of joined robots
var setJoinedLabels={}
var setJoinedIndexes={}
var i=0
var j=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_JOINED"){
setJoinedLabels[j]=m_MessageLabel[i]
setJoinedIndexes[j]=i
j=j+1
}
i=i+1
}
#go through the graph to look for a proper Label
var unFoundLabel=0
# var IDofPred=0
i=1
while(i<size(m_vecNodes) and (unFoundLabel==0)){
#if the node is unassigned and the predecessor is insight
if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLabels,m_vecNodes[i].Pred)==1){
unFoundLabel=m_vecNodes[i].Label
# IDofPred=find(m_MessageLabel,m_vecNodes[unFoundLabel].Pred)
}
i=i+1
}
if(unFoundLabel>0){
TransitionToAsking(unFoundLabel)
return
}
#set message
m_selfMessage.State=s2i(UAVSTATE)
}
#
#Do asking
#
function DoAsking(){
#look for response from predecessor
var i=0
var psResponse=-1
while(i<m_neighbourCount and psResponse==-1){
#the respond robot in joined state
#the request Label be the same as requesed
#get a respond
if(m_MessageState[i]=="STATE_JOINED"){
log("received label = ",m_MessageReqLabel[i])
if(m_MessageReqLabel[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
log("get response")
psResponse=i
}}
i=i+1
}
#analyse response
if(psResponse==-1){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
else{
log("respond id=",m_MessageReqID[psResponse])
if(m_MessageReqID[psResponse]!=m_unRequestId){
m_vecNodes[m_nLabel].State="ASSIGNING"
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
TransitionToFree()
}
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining()
return
}
else{
TransitionToAsking(m_nLabel)
return
}
}
}
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#
#Do joining
#
function DoJoining(){
gotoWP(TransitionToJoined)
#pack the communication package
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
}
#
#Do joined
#
function DoJoined(){
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
#collect all requests
var mapRequests={}
var i=0
var j=0
var ReqLabel
var JoiningLabel
var seenPred=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_ASKING"){
ReqLabel=m_MessageReqLabel[i]
log("ReqLabel var:",ReqLabel)
log("M_vec var",m_vecNodes[ReqLabel].State)
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
#is a request, store the index
mapRequests[j]=i
log("Into if m_nLabel")
j=j+1
}
}
if(m_MessageState[i]=="STATE_JOINING"){
JoiningLabel=m_MessageLabel[i]
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
##joining wrt this dot,send the global bearing
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
neighbors.broadcast("r",pack_guide_msg(m_messageForJoining))
}
}
#if it is the pred
if((m_MessageState[i]=="STATE_JOINED" or m_MessageState[i]=="STATE_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
seenPred=1
m_unWaitCount=m_unJoiningLostPeriod
}
i=i+1
}
#get request
if(size(mapRequests)!=0){
i=1
var ReqIndex=0
while(i<size(mapRequests)){
#compare the distance
if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
ReqIndex=i
i=i+1
}
#get the best index, whose ReqLabel and Reqid are
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
m_selfMessage.ReqLabel=ReqLabel
m_selfMessage.ReqID=ReqID
m_selfMessage.Response=r2i("REQ_GRANTED")
m_vecNodes[ReqLabel].State="ASSIGNING"
log("Label=",ReqLabel)
log("ID=",ReqID)
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
}
#lost pred, wait for some time and transit to free
if(seenPred==0){
m_unWaitCount=m_unWaitCount-1
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
#check if should to transists to lock
#write statues
#v_tag.get(m_nLabel)
#log(v_tag.size(), " of ", ROBOTS, " ready to lock")
#if(v_tag.size()==ROBOTS){
# TransitionToLock()
#}
barrier_wait(ROBOTS, TransitionToLock, DoJoined)
}
#
#Do Lock
#
function DoLock(){
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
#calculate motion vection
if(m_nLabel==0){
m_navigation.x=0.0 #change value so that robot 0 will move
m_navigation.y=0.0
}
if(m_nLabel!=0){
m_navigation=motion_vector()
}
#move
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
}
function action(){
statef=action
UAVSTATE="STATE_FREE"
# reset the graph
Reset()
}
#
# Executed at init
#
function init() {
#
# Global parameters for graph formation
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=100
m_fTargetAngleTolerance=0.1
m_unJoiningLostPeriod=100
#
# Join Swarm
#
uav_initswarm()
#v_tag = stigmergy.create(m_lockstig)
uav_initstig()
# go to diff. height since no collision avoidance implemented yet
TARGET_ALTITUDE = 2.5 + id * 1.5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
#
# Executed every step
#
function step() {
# listen to potential RC
uav_rccmd()
# get the swarm commands
uav_neicmd()
# update the vstig (status/net/batt)
uav_updatestig()
#update the graph
UpdateNodeInfo()
#reset message package to be sent
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#
# graph state machine
#
if(UAVSTATE=="STATE_FREE")
statef=DoFree
else if(UAVSTATE=="STATE_ESCAPE")
statef=DoEscape
else if(UAVSTATE=="STATE_ASKING")
statef=DoAsking
else if(UAVSTATE=="STATE_JOINING")
statef=DoJoining
else if(UAVSTATE=="STATE_JOINED")
statef=DoJoined
else if(UAVSTATE=="STATE_LOCK")
statef=DoLock
# high level UAV state machine
statef()
log("Current state: ", UAVSTATE, " and label: ", m_nLabel)
log("Swarm size: ", ROBOTS)
#navigation
#broadcast message
neighbors.broadcast("m",packmessage(m_selfMessage))
#
#clean message storage
m_MessageState={}#store received neighbour message
m_MessageLabel={}#store received neighbour message
m_MessageReqLabel={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCount=0
#step cunt+1
step_cunt=step_cunt+1
}
#
# Executed when reset
#
function Reset(){
Read_Graph()
m_nLabel=-1
#start listening
start_listen()
#
#set initial state, only one robot choose [A], while the rest choose [B]
#
#[A]The robot used to triger the formation process is defined as joined,
if(id==ROOT_ID){
m_nLabel=0
TransitionToJoined()
}
#[B]Other robots are defined as free.
else{
TransitionToFree()
}
}
#
# Executed upon destroy
#
function destroy() {
#clear neighbour message
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
m_vecNodes={}
#stop listening
neighbors.ignore("m")
}

View File

@ -7,44 +7,53 @@
# #
# Constants # Constants
# #
BARRIER_VSTIG = 11
BARRIER_TIMEOUT = 200 # in steps BARRIER_TIMEOUT = 200 # in steps
timeW = 0
# #
# Sets a barrier # Sets a barrier
# #
function barrier_set(threshold, transf, resumef, bdt) { function barrier_create(vstig_nb) {
# reset
timeW = 0
# create barrier vstig
barrier = stigmergy.create(vstig_nb)
}
function barrier_set(threshold, transf, resumef, vstig_nb) {
statef = function() { statef = function() {
barrier_wait(threshold, transf, resumef, bdt); barrier_wait(threshold, transf, resumef);
} }
barrier = stigmergy.create(BARRIER_VSTIG) UAVSTATE = "BARRIERWAIT"
barrier_create(vstig_nb)
} }
# #
# Make yourself ready # Make yourself ready
# #
function barrier_ready() { function barrier_ready() {
log("BARRIER READY -------")
barrier.put(id, 1) barrier.put(id, 1)
barrier.put("d", 0)
} }
# #
# Executes the barrier # Executes the barrier
# #
timeW=0 function barrier_wait(threshold, transf, resumef) {
function barrier_wait(threshold, transf, resumef, bdt) {
barrier.put(id, 1) barrier.put(id, 1)
UAVSTATE = "BARRIERWAIT"
barrier.get(id) barrier.get(id)
if(barrier.size() >= threshold) { if(barrier.size() >= threshold or barrier.get("d")==1) {
# getlowest() barrier.put("d", 1)
# barrier = nil
timeW = 0
transf() transf()
} else if(timeW >= BARRIER_TIMEOUT) { } else if(timeW >= BARRIER_TIMEOUT) {
barrier = nil timeW = 0
# barrier = nil
resumef() resumef()
timeW=0 }
} else if(bdt!=-1)
neighbors.broadcast("cmd", bdt)
timeW = timeW+1 timeW = timeW+1
} }

View File

@ -6,7 +6,8 @@
TARGET_ALTITUDE = 5.0 TARGET_ALTITUDE = 5.0
UAVSTATE = "TURNEDOFF" UAVSTATE = "TURNEDOFF"
GOTO_MAXVEL = 2 GOTO_MAXVEL = 2
goal = {.range=0, .bearing=0} TAKEOFF_VSTIG = 11
goal = {.range=0, .bearing=0, .latitude=0, .longitude=0}
function uav_initswarm() { function uav_initswarm() {
s = swarm.create(1) s = swarm.create(1)
@ -23,60 +24,76 @@ function idle() {
UAVSTATE = "IDLE" UAVSTATE = "IDLE"
} }
firstpassT = 1
function takeoff() { function takeoff() {
UAVSTATE = "TAKEOFF" UAVSTATE = "TAKEOFF"
statef=takeoff statef=takeoff
if(firstpassT) {
barrier_create(TAKEOFF_VSTIG)
firstpassT = 0
}
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,action,land,-1) barrier_wait(ROBOTS, action, land)
barrier_ready() } else {
} log("Altitude: ", TARGET_ALTITUDE)
else { neighbors.broadcast("cmd", 22)
log("Altitude: ", TARGET_ALTITUDE) uav_takeoff(TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22) }
uav_takeoff(TARGET_ALTITUDE)
}
} }
firstpassL = 1
function land() { function land() {
UAVSTATE = "LAND" UAVSTATE = "LAND"
statef=land statef=land
if(flight.status == 2 or flight.status == 3){ if(firstpassL) {
neighbors.broadcast("cmd", 21) barrier_create(TAKEOFF_VSTIG+1)
uav_land() firstpassL = 0
} }
else {
barrier_set(ROBOTS,turnedoff,land,21) neighbors.broadcast("cmd", 21)
barrier_ready() uav_land()
timeW=0
#barrier = nil if(flight.status != 2 and flight.status != 3){
#statef=idle barrier_wait(ROBOTS,turnedoff,land)
} }
} }
function goto() { function goto() {
UAVSTATE = "GOTO" UAVSTATE = "GOTO"
statef=goto statef=goto
# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
if(rc_goto.id==id){ if(rc_goto.id==id){
s.leave() s.leave()
rb_from_gps(rc_goto.latitude, rc_goto.longitude) gotoWP(picture)
print(id, " has to move ", goal.range)
m_navigation = math.vec2.newp(goal.range, goal.bearing)
if(math.vec2.length(m_navigation)>GOTO_MAXVEL) {
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
} else if(math.vec2.length(m_navigation)>0.25)
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
else
statef = idle
} else { } else {
neighbors.broadcast("cmd", 16) neighbors.broadcast("cmd", 16)
neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
} }
} }
function picture() {
#TODO: change gimbal orientation
uav_takepicture()
statef=idle
}
function gotoWP(transf) {
# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
rb_from_gps(rc_goto.latitude, rc_goto.longitude)
print(id, " has to move ", goal.range)
m_navigation = math.vec2.newp(goal.range, goal.bearing)
if(math.vec2.length(m_navigation)>GOTO_MAXVEL) {
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
} else if(math.vec2.length(m_navigation)>0.25)
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
else
transf()
}
function follow() { function follow() {
if(size(targets)>0) { if(size(targets)>0) {
UAVSTATE = "FOLLOW" UAVSTATE = "FOLLOW"
@ -156,7 +173,7 @@ function LimitAngle(angle){
} }
function rb_from_gps(lat, lon) { function rb_from_gps(lat, lon) {
print("rb dbg: ",lat,lon,position.latitude,position.longitude) print("gps2rb: ",lat,lon,position.latitude,position.longitude)
d_lon = lon - position.longitude d_lon = lon - position.longitude
d_lat = lat - position.latitude d_lat = lat - position.latitude
ned_x = d_lat/180*math.pi * 6371000.0; ned_x = d_lat/180*math.pi * 6371000.0;
@ -164,3 +181,13 @@ function rb_from_gps(lat, lon) {
goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
goal.bearing = LimitAngle(math.atan(ned_y,ned_x)); goal.bearing = LimitAngle(math.atan(ned_y,ned_x));
} }
function gps_from_rb(range, bearing) {
print("rb2gps: ",range,bearing,position.latitude,position.longitude)
latR = position.latitude*math.pi/180.0;
lonR = position.longitude*math.pi/180.0;
target_lat = math.asin(math.sin(latR) * math.cos(range/6371000.0) + math.cos(latR) * math.sin(range/6371000.0) * math.cos(bearing));
target_lon = lonR + math.atan(math.sin(bearing) * math.sin(range/6371000.0) * math.cos(latR), math.cos(range/6371000.0) - math.sin(latR) * math.sin(target_lat));
goal.latitude = target_lat*180.0/math.pi;
goal.longitude = target_lon*180.0/math.pi;
}

View File

@ -22,6 +22,7 @@ namespace buzzuav_closures{
COMMAND_DISARM, COMMAND_DISARM,
COMMAND_GOTO, COMMAND_GOTO,
COMMAND_MOVETO, COMMAND_MOVETO,
COMMAND_PICTURE,
} Custom_CommandCode; } Custom_CommandCode;
/* /*
@ -37,6 +38,7 @@ int buzzros_print(buzzvm_t vm);
*/ */
int buzzuav_moveto(buzzvm_t vm); int buzzuav_moveto(buzzvm_t vm);
int buzzuav_storegoal(buzzvm_t vm); int buzzuav_storegoal(buzzvm_t vm);
int buzzuav_takepicture(buzzvm_t vm);
/* Returns the current command from local variable*/ /* Returns the current command from local variable*/
int getcmd(); int getcmd();
/*Sets goto position */ /*Sets goto position */

View File

@ -304,9 +304,12 @@ void in_message_process(){
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
buzzvm_gstore(VM); buzzvm_gstore(VM);
@ -349,7 +352,7 @@ void in_message_process(){
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));

View File

@ -142,7 +142,7 @@ namespace buzzuav_closures{
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
//printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
//ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]); //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]);
buzz_cmd= COMMAND_MOVETO; // TO DO what should we use buzz_cmd = COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -264,19 +264,32 @@ namespace buzzuav_closures{
message_number++;*/ message_number++;*/
return payload_out; return payload_out;
} }
/*----------------------------------------/
/ Buzz closure to take a picture here.
/----------------------------------------*/
int buzzuav_takepicture(buzzvm_t vm) {
cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
buzz_cmd = COMMAND_PICTURE;
return buzzvm_ret0(vm);
}
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to store locally a GPS destination from the fleet / Buzz closure to store locally a GPS destination from the fleet
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_storegoal(buzzvm_t vm) { int buzzuav_storegoal(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 3); buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // latitude buzzvm_lload(vm, 1); // altitude
buzzvm_lload(vm, 2); // longitude buzzvm_lload(vm, 2); // longitude
buzzvm_lload(vm, 3); // altitude buzzvm_lload(vm, 3); // latitude
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
rc_set_goto(buzzvm_stack_at(vm, 1)->f.value, buzzvm_stack_at(vm, 2)->f.value, buzzvm_stack_at(vm, 3)->f.value, (int)buzz_utility::get_robotid()); float lat = buzzvm_stack_at(vm, 3)->f.value;
return buzzvm_ret0(vm); float lon = buzzvm_stack_at(vm, 2)->f.value;
float alt = buzzvm_stack_at(vm, 1)->f.value;
ROS_ERROR("DEBUG ---- %f %f %f",lat,lon,alt);
rc_set_goto((int)buzz_utility::get_robotid(), lat, lon, alt);
return buzzvm_ret0(vm);
} }
/*----------------------------------------/ /*----------------------------------------/

View File

@ -750,9 +750,9 @@ void roscontroller::flight_controller_service_call()
} }
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
} else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/
// roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], ROS_INFO("TAKING A PICTURE HERE!! --------------")
// goto_pos[2], 0); ;
} }
} }
/*---------------------------------------------- /*----------------------------------------------