Merge commit '9a0332aed9802343b0c7cb8773ed453a5f14f4c0' into ros_drones_ws
This commit is contained in:
David St-Onge
commit
25ed9328da
|
|
@ -6,15 +6,16 @@ include "vec2.bzz"
|
|||
include "update.bzz"
|
||||
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
include "vstigenv.bzz"
|
||||
|
||||
include "graphs/shapes_L.bzz"
|
||||
include "graphs/shapes_Y.bzz"
|
||||
|
||||
ROBOT_RADIUS=50
|
||||
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
|
||||
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
|
||||
|
||||
# max velocity in cm/step
|
||||
ROBOT_MAXVEL = 500.0
|
||||
ROBOT_MAXVEL = 250.0
|
||||
|
||||
#
|
||||
# Global variables
|
||||
|
|
@ -90,8 +91,7 @@ step_cunt=0
|
|||
m_lockstig = 1
|
||||
|
||||
# Lennard-Jones parameters, may need change
|
||||
EPSILON = 13.5 #the LJ parameter for other robots
|
||||
EPSILON_FOR1 = 10.0 #the LJ parameter for the robot labeled 1
|
||||
EPSILON = 4000 #13.5 the LJ parameter for other robots
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
|
||||
|
|
@ -277,7 +277,7 @@ log(id,"I pass value=",recv_value)
|
|||
var return_table={.Label=0.0,.Bearing=0.0}
|
||||
return_table.Label=qian
|
||||
if(bai==1){
|
||||
b=b*-1.0
|
||||
b=b*-1.0
|
||||
}
|
||||
return_table.Bearing=b
|
||||
return return_table
|
||||
|
|
@ -288,10 +288,10 @@ 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
|
||||
if(lock_neighbor_id[i]==nei_id){
|
||||
return_val=lock_neighbor_dis[i]
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
return return_val
|
||||
}
|
||||
|
|
@ -303,9 +303,10 @@ function LJ_vec(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)
|
||||
cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),bearing)
|
||||
}
|
||||
#log(id,"dis=",dis,"target=",target,"label=",nei_id)
|
||||
#log(id,"dis=",dis,"target=",target,"label=",nei_id)
|
||||
#log("x=",cDir.x,"y=",cDir.y)
|
||||
return cDir
|
||||
}
|
||||
#calculate the motion vector
|
||||
|
|
@ -313,12 +314,13 @@ 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
|
||||
#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
|
||||
}
|
||||
|
||||
|
|
@ -531,8 +533,8 @@ function DoFree() {
|
|||
tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
|
||||
m_navigation=math.vec2.add(tempvec_P,tempvec_N)
|
||||
# Limit the mvt
|
||||
if(math.vec2.length(m_navigation)>ROBOT_MAXVEL)
|
||||
m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL/math.vec2.length(m_navigation))
|
||||
if(math.vec2.length(m_navigation)>ROBOT_MAXVEL*2)
|
||||
m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL*2/math.vec2.length(m_navigation))
|
||||
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
||||
}else{ #no joined robots in sight
|
||||
i=0
|
||||
|
|
@ -593,8 +595,11 @@ function DoAsking(){
|
|||
}
|
||||
}
|
||||
else{
|
||||
if(m_MessageReqID[psResponse]!=m_unRequestId)
|
||||
TransitionToFree()
|
||||
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()
|
||||
|
|
@ -662,8 +667,8 @@ function DoJoining(){
|
|||
|
||||
|
||||
#test if is already in desired position
|
||||
if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
|
||||
log("LOCKING! ", math.vec2.length(S2Target), ", ", S2Target_bearing)
|
||||
if(math.vec2.length(S2Target)<m_fTargetDistanceTolerance and S2Target_bearing<m_fTargetAngleTolerance){
|
||||
log("JOINED! ", math.vec2.length(S2Target), ", ", S2Target_bearing)
|
||||
TransitionToJoined()
|
||||
return
|
||||
}
|
||||
|
|
@ -716,7 +721,7 @@ function DoJoined(){
|
|||
}
|
||||
}
|
||||
#if it is the pred
|
||||
if(m_MessageState[i]=="STATE_JOINED" and m_MessageLabel[i]==m_vecNodes[m_nLabel].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
|
||||
}
|
||||
|
|
@ -739,6 +744,8 @@ function DoJoined(){
|
|||
m_selfMessage.ReqLabel=ReqLabel
|
||||
m_selfMessage.ReqID=ReqID
|
||||
m_selfMessage.Response=r2i("REQ_GRANTED")
|
||||
m_vecNodes[ReqLabel].State="ASSIGNING"
|
||||
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
|
||||
}
|
||||
|
||||
#lost pred, wait for some time and transit to free
|
||||
|
|
@ -758,7 +765,7 @@ function DoJoined(){
|
|||
|
||||
#check if should to transists to lock
|
||||
#write statues
|
||||
#v_tag.put(m_nLabel, 1)
|
||||
v_tag.get(m_nLabel)
|
||||
log(v_tag.size(), " of ", ROBOTS, " ready to lock")
|
||||
if(v_tag.size()==ROBOTS){
|
||||
TransitionToLock()
|
||||
|
|
@ -775,9 +782,8 @@ 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.x=0.25 #change value so that robot 0 will move
|
||||
m_navigation.y=0.0
|
||||
log(";",m_nLabel,";",0)
|
||||
}
|
||||
if(m_nLabel!=0){
|
||||
m_navigation=motion_vector()
|
||||
|
|
@ -800,7 +806,8 @@ function init() {
|
|||
#
|
||||
m_unResponseTimeThreshold=10
|
||||
m_unLabelSearchWaitTime=10
|
||||
m_fTargetDistanceTolerance=1
|
||||
m_fTargetDistanceTolerance=50
|
||||
m_fTargetAngleTolerance=0.1
|
||||
m_unJoiningLostPeriod=100
|
||||
|
||||
#
|
||||
|
|
@ -808,6 +815,7 @@ function init() {
|
|||
#
|
||||
uav_initswarm()
|
||||
v_tag = stigmergy.create(m_lockstig)
|
||||
uav_initstig()
|
||||
Reset()
|
||||
TARGET_ALTITUDE = 2.5 + id * 1.5
|
||||
}
|
||||
|
|
@ -818,6 +826,7 @@ function init() {
|
|||
function step(){
|
||||
uav_rccmd()
|
||||
uav_neicmd()
|
||||
uav_updatestig()
|
||||
#update the graph
|
||||
UpdateNodeInfo()
|
||||
#reset message package to be sent
|
||||
|
|
@ -846,8 +855,10 @@ function step(){
|
|||
debug(m_eState,m_nLabel)
|
||||
log("Current state: ", UAVSTATE)
|
||||
log("Swarm size: ", ROBOTS)
|
||||
# if(id==0)
|
||||
# stattab_print()
|
||||
|
||||
#navigation
|
||||
|
||||
#broadcast messag
|
||||
neighbors.broadcast("m",packmessage(m_selfMessage))
|
||||
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ function barrier_wait(threshold, transf, resumef) {
|
|||
barrier.put(id, 1)
|
||||
UAVSTATE = "BARRIERWAIT"
|
||||
if(barrier.size() >= threshold) {
|
||||
#barrier = nil
|
||||
# getlowest()
|
||||
transf()
|
||||
} else if(timeW>=BARRIER_TIMEOUT) {
|
||||
barrier = nil
|
||||
|
|
@ -44,4 +44,19 @@ function barrier_wait(threshold, transf, resumef) {
|
|||
timeW=0
|
||||
}
|
||||
timeW = timeW+1
|
||||
}
|
||||
|
||||
# get the lowest id of the fleet, but requires too much bandwidth
|
||||
function getlowest(){
|
||||
Lid = 20;
|
||||
u=20
|
||||
while(u>=0){
|
||||
tab = barrier.get(u)
|
||||
if(tab!=nil){
|
||||
if(tab<Lid)
|
||||
Lid=tab
|
||||
}
|
||||
u=u-1
|
||||
}
|
||||
log("--> LOWEST ID:",Lid)
|
||||
}
|
||||
|
|
@ -14,7 +14,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of Lab
|
|||
m_vecNodes[1] = {
|
||||
.Label = 1,
|
||||
.Pred = 0,
|
||||
.distance = 2000,
|
||||
.distance = 800,
|
||||
.bearing = 0.0,
|
||||
.height = 5000,
|
||||
.State="UNASSIGNED",
|
||||
|
|
@ -23,7 +23,7 @@ m_vecNodes[1] = {
|
|||
m_vecNodes[2] = {
|
||||
.Label = 2,
|
||||
.Pred = 0,
|
||||
.distance = 2000,
|
||||
.distance = 800,
|
||||
.bearing = 1.57,
|
||||
.height = 7000,
|
||||
.State="UNASSIGNED",
|
||||
|
|
@ -32,7 +32,7 @@ m_vecNodes[2] = {
|
|||
m_vecNodes[3] = {
|
||||
.Label = 3,
|
||||
.Pred = 1,
|
||||
.distance = 2000,
|
||||
.distance = 800,
|
||||
.bearing = 0.0,
|
||||
.height = 9000,
|
||||
.State="UNASSIGNED",
|
||||
|
|
@ -41,7 +41,7 @@ m_vecNodes[3] = {
|
|||
m_vecNodes[4] = {
|
||||
.Label = 4,
|
||||
.Pred = 2,
|
||||
.distance = 2000,
|
||||
.distance = 800,
|
||||
.bearing = 1.57,
|
||||
.height = 11000,
|
||||
.State="UNASSIGNED",
|
||||
|
|
@ -50,7 +50,7 @@ m_vecNodes[4] = {
|
|||
m_vecNodes[5] = {
|
||||
.Label = 5,
|
||||
.Pred = 4,
|
||||
.distance = 2000,
|
||||
.distance = 800,
|
||||
.bearing = 1.57,
|
||||
.height = 14000,
|
||||
.State="UNASSIGNED",
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of Lab
|
|||
m_vecNodes[1] = {
|
||||
.Label = 1,
|
||||
.Pred = 0,
|
||||
.distance = 2000,
|
||||
.distance = 1000,
|
||||
.bearing = 4.71,
|
||||
.height = 5000,
|
||||
.State="UNASSIGNED",
|
||||
|
|
@ -23,7 +23,7 @@ m_vecNodes[1] = {
|
|||
m_vecNodes[2] = {
|
||||
.Label = 2,
|
||||
.Pred = 0,
|
||||
.distance = 1414,
|
||||
.distance = 707,
|
||||
.bearing = 2.36,
|
||||
.height = 7000,
|
||||
.State="UNASSIGNED",
|
||||
|
|
@ -32,7 +32,7 @@ m_vecNodes[2] = {
|
|||
m_vecNodes[3] = {
|
||||
.Label = 3,
|
||||
.Pred = 0,
|
||||
.distance = 1414,
|
||||
.distance = 707,
|
||||
.bearing = 0.79,
|
||||
.height = 9000,
|
||||
.State="UNASSIGNED",
|
||||
|
|
@ -41,7 +41,7 @@ m_vecNodes[3] = {
|
|||
m_vecNodes[4] = {
|
||||
.Label = 4,
|
||||
.Pred = 2,
|
||||
.distance = 1414,
|
||||
.distance = 707,
|
||||
.bearing = 2.36,
|
||||
.height = 11000,
|
||||
.State="UNASSIGNED",
|
||||
|
|
@ -50,7 +50,7 @@ m_vecNodes[4] = {
|
|||
m_vecNodes[5] = {
|
||||
.Label = 5,
|
||||
.Pred = 3,
|
||||
.distance = 1414,
|
||||
.distance = 707,
|
||||
.bearing = 0.79,
|
||||
.height = 14000,
|
||||
.State="UNASSIGNED",
|
||||
|
|
|
|||
|
|
@ -103,6 +103,9 @@ function uav_rccmd() {
|
|||
flight.rc_cmd=0
|
||||
uav_disarm()
|
||||
neighbors.broadcast("cmd", 401)
|
||||
} else if (flight.rc_cmd==666){
|
||||
flight.rc_cmd=0
|
||||
stattab_send()
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@ GROUND_VSTIG = 11
|
|||
WAIT4STEP = 10
|
||||
v_status = {}
|
||||
v_ground = {}
|
||||
b_updating = 0
|
||||
|
||||
function uav_initstig() {
|
||||
v_status = stigmergy.create(STATUS_VSTIG)
|
||||
|
|
@ -11,29 +12,48 @@ function uav_initstig() {
|
|||
|
||||
counter=WAIT4STEP
|
||||
function uav_updatestig() {
|
||||
# TODO: Push values on update only.
|
||||
if(counter<=0) {
|
||||
var ls={.g=0,.b=battery.capacity,.x=xbee_status.rssi,.f=flight.status}
|
||||
b_updating=1
|
||||
#var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status}
|
||||
ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status
|
||||
log("Pushing ", ls, "on vstig with id:", id)
|
||||
v_status.put(id, ls)
|
||||
counter=WAIT4STEP
|
||||
} else
|
||||
} else {
|
||||
b_updating=0
|
||||
counter=counter-1
|
||||
}
|
||||
}
|
||||
|
||||
function unpackstatus(recv_value){
|
||||
gps=(recv_value-recv_value%1000000)/1000000
|
||||
recv_value=recv_value-gps*1000000
|
||||
batt=(recv_value-recv_value%1000)/1000
|
||||
recv_value=recv_value-batt*1000
|
||||
xbee=(recv_value-recv_value%10)/10
|
||||
recv_value=recv_value-xbee*10
|
||||
fc=recv_value
|
||||
log("- GPS ", gps)
|
||||
log("- Battery ", batt)
|
||||
log("- Xbee ", xbee)
|
||||
log("- Status ", fc)
|
||||
}
|
||||
|
||||
function checkusers() {
|
||||
# Read a value from the structure
|
||||
if(size(users)>0)
|
||||
log("Got a user!")
|
||||
# Read a value from the structure
|
||||
if(size(users)>0)
|
||||
log("Got a user!")
|
||||
|
||||
# log(users)
|
||||
# users_print(users.dataG)
|
||||
# if(size(users.dataG)>0)
|
||||
# vt.put("p", users.dataG)
|
||||
|
||||
# Get the number of keys in the structure
|
||||
#log("The vstig has ", vt.size(), " elements")
|
||||
# users_save(vt.get("p"))
|
||||
# table_print(users.dataL)
|
||||
# log(users)
|
||||
# users_print(users.dataG)
|
||||
# if(size(users.dataG)>0)
|
||||
# vt.put("p", users.dataG)
|
||||
|
||||
# Get the number of keys in the structure
|
||||
# log("The vstig has ", vt.size(), " elements")
|
||||
# users_save(vt.get("p"))
|
||||
# table_print(users.dataL)
|
||||
}
|
||||
|
||||
function users_save(t) {
|
||||
|
|
@ -56,17 +76,37 @@ function usertab_print(t) {
|
|||
|
||||
function stattab_print() {
|
||||
if(v_status.size()>0) {
|
||||
u=0
|
||||
while(u<8){
|
||||
tab = v_status.get(u)
|
||||
if(tab!=nil) {
|
||||
log("id: ", u)
|
||||
log("- GPS ", tab.g)
|
||||
log("- Battery ", tab.b)
|
||||
log("- Xbee ", tab.x)
|
||||
log("- Status ", tab.f)
|
||||
if(b_updating==0) {
|
||||
u=0
|
||||
while(u<8){
|
||||
tab = v_status.get(u)
|
||||
if(tab!=nil)
|
||||
unpackstatus(tab)
|
||||
u=u+1
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
function stattab_send() {
|
||||
if(v_status.size()>0) {
|
||||
if(b_updating==0) {
|
||||
u=0
|
||||
while(u<8){
|
||||
tab = v_status.get(u)
|
||||
if(tab!=nil){
|
||||
recv_value=tab
|
||||
gps=(recv_value-recv_value%1000000)/1000000
|
||||
recv_value=recv_value-gps*1000000
|
||||
batt=(recv_value-recv_value%1000)/1000
|
||||
recv_value=recv_value-batt*1000
|
||||
xbee=(recv_value-recv_value%10)/10
|
||||
recv_value=recv_value-xbee*10
|
||||
fc=recv_value
|
||||
add_neighborStatus(u,gps,batt,xbee,fc)
|
||||
}
|
||||
u=u+1
|
||||
}
|
||||
u=u+1
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -29,6 +29,14 @@ struct rb_struct
|
|||
};
|
||||
typedef struct rb_struct RB_struct;
|
||||
|
||||
struct neiStatus
|
||||
{
|
||||
uint gps_strenght = 0;
|
||||
uint batt_lvl = 0;
|
||||
uint xbee = 0;
|
||||
uint flight_status = 0;
|
||||
}; typedef struct neiStatus neighbors_status ;
|
||||
|
||||
|
||||
uint16_t* u64_cvt_u16(uint64_t u64);
|
||||
|
||||
|
|
|
|||
|
|
@ -5,9 +5,9 @@
|
|||
#include <stdio.h>
|
||||
#include "uav_utility.h"
|
||||
#include "mavros_msgs/CommandCode.h"
|
||||
#include "mavros_msgs/Mavlink.h"
|
||||
#include "ros/ros.h"
|
||||
#include "buzz_utility.h"
|
||||
//#include "roscontroller.h"
|
||||
|
||||
#define EARTH_RADIUS (double) 6371000.0
|
||||
#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
|
||||
|
|
@ -61,6 +61,9 @@ void flight_status_update(uint8_t state);
|
|||
/* Update neighbors table */
|
||||
void neighbour_pos_callback(int id, float range, float bearing, float elevation);
|
||||
void update_neighbors(buzzvm_t vm);
|
||||
int buzzuav_addNeiStatus(buzzvm_t vm);
|
||||
mavros_msgs::Mavlink get_status();
|
||||
|
||||
/*Flight status*/
|
||||
void set_obstacle_dist(float dist[]);
|
||||
|
||||
|
|
|
|||
|
|
@ -69,7 +69,7 @@ private:
|
|||
double latitude=0.0;
|
||||
float altitude=0.0;
|
||||
}; typedef struct gps GPS ; // not useful in cpp
|
||||
|
||||
|
||||
GPS target, home, cur_pos;
|
||||
double cur_rel_altitude;
|
||||
|
||||
|
|
@ -91,7 +91,7 @@ private:
|
|||
/*tmp to be corrected*/
|
||||
uint8_t no_cnt=0;
|
||||
uint8_t old_val=0 ;
|
||||
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
|
||||
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,obstacles_topic,stand_by,xbeesrv_name, setpoint_name;
|
||||
std::string stream_client_name;
|
||||
std::string relative_altitude_sub_name;
|
||||
std::string setpoint_nonraw;
|
||||
|
|
@ -102,6 +102,7 @@ private:
|
|||
ros::ServiceClient mav_client;
|
||||
ros::ServiceClient xbeestatus_srv;
|
||||
ros::Publisher payload_pub;
|
||||
ros::Publisher MPpayload_pub;
|
||||
ros::Publisher neigh_pos_pub;
|
||||
ros::Publisher localsetpoint_nonraw_pub;
|
||||
ros::ServiceServer service;
|
||||
|
|
@ -155,6 +156,8 @@ private:
|
|||
/*Neighbours pos publisher*/
|
||||
void neighbours_pos_publisher();
|
||||
|
||||
void send_MPpayload();
|
||||
|
||||
/*Prepare messages and publish*/
|
||||
void prepare_msg_and_publish();
|
||||
|
||||
|
|
|
|||
|
|
@ -325,6 +325,9 @@ void in_message_process(){
|
|||
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus));
|
||||
buzzvm_gstore(VM);
|
||||
|
||||
return VM->state;
|
||||
}
|
||||
|
|
@ -367,6 +370,9 @@ void in_message_process(){
|
|||
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
||||
buzzvm_gstore(VM);
|
||||
|
||||
return VM->state;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,13 +25,15 @@ namespace buzzuav_closures{
|
|||
static int buzz_cmd=0;
|
||||
static float height=0;
|
||||
static bool deque_full = false;
|
||||
static float rssi = 0.0;
|
||||
static int rssi = 0;
|
||||
static int message_number = 0;
|
||||
static float raw_packet_loss = 0.0;
|
||||
static float filtered_packet_loss = 0.0;
|
||||
static int filtered_packet_loss = 0;
|
||||
static float api_rssi = 0.0;
|
||||
|
||||
std::map< int, buzz_utility::RB_struct> targets_map;
|
||||
std::map< int, buzz_utility::Pos_struct> neighbors_map;
|
||||
std::map< int, buzz_utility::neighbors_status> neighbors_status_map;
|
||||
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
|
|
@ -218,6 +220,48 @@ namespace buzzuav_closures{
|
|||
return 0;
|
||||
}
|
||||
|
||||
int buzzuav_addNeiStatus(buzzvm_t vm){
|
||||
buzzvm_lnum_assert(vm, 5);
|
||||
buzzvm_lload(vm, 1); // fc
|
||||
buzzvm_lload(vm, 2); // xbee
|
||||
buzzvm_lload(vm, 3); // batt
|
||||
buzzvm_lload(vm, 4); // gps
|
||||
buzzvm_lload(vm, 5); // id
|
||||
buzzvm_type_assert(vm, 5, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 4, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
|
||||
buzz_utility::neighbors_status newRS;
|
||||
uint8_t id = buzzvm_stack_at(vm, 5)->i.value;
|
||||
newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value;
|
||||
newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value;
|
||||
newRS.xbee= buzzvm_stack_at(vm, 2)->i.value;
|
||||
newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value;
|
||||
map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id);
|
||||
if(it!=neighbors_status_map.end())
|
||||
neighbors_status_map.erase(it);
|
||||
neighbors_status_map.insert(make_pair(id, newRS));
|
||||
return vm->state;
|
||||
}
|
||||
|
||||
mavros_msgs::Mavlink get_status(){
|
||||
mavros_msgs::Mavlink payload_out;
|
||||
map< int, buzz_utility::neighbors_status >::iterator it;
|
||||
for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++it){
|
||||
payload_out.payload64.push_back(it->first);
|
||||
payload_out.payload64.push_back(it->second.gps_strenght);
|
||||
payload_out.payload64.push_back(it->second.batt_lvl);
|
||||
payload_out.payload64.push_back(it->second.xbee);
|
||||
payload_out.payload64.push_back(it->second.flight_status);
|
||||
}
|
||||
/*Add Robot id and message number to the published message*/
|
||||
payload_out.sysid = (uint8_t)neighbors_status_map.size();
|
||||
/*payload_out.msgid = (uint32_t)message_number;
|
||||
|
||||
message_number++;*/
|
||||
return payload_out;
|
||||
}
|
||||
/*----------------------------------------/
|
||||
/ Buzz closure to go directly to a GPS destination from the Mission Planner
|
||||
/----------------------------------------*/
|
||||
|
|
@ -247,7 +291,7 @@ namespace buzzuav_closures{
|
|||
}
|
||||
|
||||
/*---------------------------------------/
|
||||
/ Buss closure for basic UAV commands
|
||||
/ Buzz closure for basic UAV commands
|
||||
/---------------------------------------*/
|
||||
int buzzuav_takeoff(buzzvm_t vm) {
|
||||
buzzvm_lnum_assert(vm, 1);
|
||||
|
|
@ -334,7 +378,7 @@ namespace buzzuav_closures{
|
|||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1));
|
||||
buzzvm_pushf(vm, batt[2]);
|
||||
buzzvm_pushi(vm, (int)batt[2]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_gstore(vm);
|
||||
return vm->state;
|
||||
|
|
@ -347,7 +391,7 @@ namespace buzzuav_closures{
|
|||
|
||||
void set_rssi(float value)
|
||||
{
|
||||
rssi = value;
|
||||
rssi = round(value);
|
||||
}
|
||||
|
||||
void set_raw_packet_loss(float value)
|
||||
|
|
@ -357,7 +401,7 @@ namespace buzzuav_closures{
|
|||
|
||||
void set_filtered_packet_loss(float value)
|
||||
{
|
||||
filtered_packet_loss = value;
|
||||
filtered_packet_loss = round(100*value);
|
||||
}
|
||||
|
||||
void set_api_rssi(float value)
|
||||
|
|
@ -374,7 +418,7 @@ namespace buzzuav_closures{
|
|||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1));
|
||||
buzzvm_pushf(vm, rssi);
|
||||
buzzvm_pushi(vm, rssi);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1));
|
||||
|
|
@ -382,7 +426,7 @@ namespace buzzuav_closures{
|
|||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1));
|
||||
buzzvm_pushf(vm, filtered_packet_loss);
|
||||
buzzvm_pushi(vm, filtered_packet_loss);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1));
|
||||
|
|
|
|||
|
|
@ -172,6 +172,10 @@ bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float &result)
|
|||
return srv_response.success;
|
||||
}
|
||||
|
||||
void roscontroller::send_MPpayload(){
|
||||
MPpayload_pub.publish(buzzuav_closures::get_status());
|
||||
}
|
||||
|
||||
/*-------------------------------------------------
|
||||
/rosbuzz_node loop method executed once every step
|
||||
/--------------------------------------------------*/
|
||||
|
|
@ -193,6 +197,7 @@ void roscontroller::RosControllerRun()
|
|||
// buzz_closure::neighbour_pos_callback(neighbours_pos_map);
|
||||
/*Neighbours of the robot published with id in respective topic*/
|
||||
neighbours_pos_publisher();
|
||||
send_MPpayload();
|
||||
/*Check updater state and step code*/
|
||||
update_routine(bcfname.c_str(), dbgfname.c_str());
|
||||
/*Step buzz script */
|
||||
|
|
@ -387,6 +392,8 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle)
|
|||
ROS_ERROR("Provide a localpos name in YAML file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
|
||||
node_handle.getParam("obstacles", obstacles_topic);
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------
|
||||
|
|
@ -398,23 +405,18 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
|
|||
|
||||
Subscribe(n_c);
|
||||
|
||||
// current_position_sub = n_c.subscribe("/global_position", 1000,
|
||||
// &roscontroller::current_pos,this);
|
||||
// battery_sub = n_c.subscribe("/power_status", 1000,
|
||||
// &roscontroller::battery,this);
|
||||
payload_sub =
|
||||
n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt, this);
|
||||
// flight_status_sub =n_c.subscribe("/flight_status",100,
|
||||
// &roscontroller::flight_extended_status_update,this);
|
||||
// Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000,
|
||||
// &roscontroller::set_robot_id,this);
|
||||
obstacle_sub = n_c.subscribe("guidance/obstacle_distance", 100,
|
||||
n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
|
||||
|
||||
obstacle_sub = n_c.subscribe(obstacles_topic, 5,
|
||||
&roscontroller::obstacle_dist, this);
|
||||
|
||||
/*publishers*/
|
||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
|
||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 1000);
|
||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5);
|
||||
MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
|
||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 5);
|
||||
localsetpoint_nonraw_pub =
|
||||
n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 1000);
|
||||
n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
|
||||
/* Service Clients*/
|
||||
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
|
||||
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
|
||||
|
|
@ -427,8 +429,8 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
|
|||
stream_client =
|
||||
n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
|
||||
|
||||
users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos, this);
|
||||
local_pos_sub = n_c.subscribe(local_pos_sub_name, 1000,
|
||||
users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this);
|
||||
local_pos_sub = n_c.subscribe(local_pos_sub_name, 5,
|
||||
&roscontroller::local_pos_callback, this);
|
||||
|
||||
multi_msg = true;
|
||||
|
|
@ -449,13 +451,13 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
|
|||
it->first, 100, &roscontroller::flight_status_update, this);
|
||||
} else if (it->second == "mavros_msgs/BatteryStatus") {
|
||||
battery_sub =
|
||||
n_c.subscribe(it->first, 1000, &roscontroller::battery, this);
|
||||
n_c.subscribe(it->first, 5, &roscontroller::battery, this);
|
||||
} else if (it->second == "sensor_msgs/NavSatFix") {
|
||||
current_position_sub =
|
||||
n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
|
||||
n_c.subscribe(it->first, 5, &roscontroller::current_pos, this);
|
||||
} else if (it->second == "std_msgs/Float64") {
|
||||
relative_altitude_sub =
|
||||
n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this);
|
||||
n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this);
|
||||
}
|
||||
|
||||
std::cout << "Subscribed to: " << it->first << endl;
|
||||
|
|
@ -1125,14 +1127,17 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
|
|||
rc_goto[0] = req.param5;
|
||||
rc_goto[1] = req.param6;
|
||||
rc_goto[2] = req.param7;
|
||||
// for test
|
||||
// SetLocalPosition(rc_goto[0], rc_goto[1], rc_goto[2], 0);
|
||||
|
||||
buzzuav_closures::rc_set_goto(rc_goto);
|
||||
rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
case 666:
|
||||
ROS_INFO("RC_Call: Update Fleet Status!!!!");
|
||||
rc_cmd = 666;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
default:
|
||||
res.success = false;
|
||||
break;
|
||||
|
|
|
|||
Loading…
Reference in New Issue