Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

tweak graph control, fix obstacles and and rc_client fleet_status

This commit is contained in:
dave 2017-08-09 19:23:42 -04:00
parent 74846c5df8
commit 9a0332aed9
12 changed files with 179 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
buzz_scripts/graphform.bzz
View File

@ -8,14 +8,14 @@ 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 = 200.0
ROBOT_MAXVEL = 250.0
#
# Global variables
@ -91,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
@ -278,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
@ -289,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
}
@ -304,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
@ -314,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
}
@ -532,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
@ -594,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()
@ -663,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
}
@ -717,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
}
@ -740,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
@ -759,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()
@ -776,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()
@ -801,7 +806,8 @@ function init() {
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=1
m_fTargetDistanceTolerance=50
m_fTargetAngleTolerance=0.1
m_unJoiningLostPeriod=100
#
@ -849,8 +855,8 @@ function step(){
debug(m_eState,m_nLabel)
log("Current state: ", UAVSTATE)
log("Swarm size: ", ROBOTS)
if(id==0)
stattab_print()
# if(id==0)
# stattab_print()
#navigation
#broadcast messag

17
buzz_scripts/include/barrier.bzz
View File

@ -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)
}

10
buzz_scripts/include/graphs/shapes_L.bzz
View File

@ -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",

10
buzz_scripts/include/graphs/shapes_Y.bzz
View File

@ -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",

3
buzz_scripts/include/uavstates.bzz
View File

@ -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()
}
}

23
buzz_scripts/include/vstigenv.bzz
View File

@ -86,4 +86,27 @@ function stattab_print() {
}
}
}
}
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
}
}
}
}

8
include/buzz_utility.h
View File

@ -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
include/buzzuav_closures.h
View File

@ -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[]);

7
include/roscontroller.h
View File

@ -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();

6
src/buzz_utility.cpp
View File

@ -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;
}

44
src/buzzuav_closures.cpp
View File

@ -26,12 +26,14 @@ namespace buzzuav_closures{
static float height=0;
static bool deque_full = false;
static int rssi = 0;
static int message_number = 0;
static float raw_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
/----------------------------------------*/

47
src/roscontroller.cpp
View File

@ -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;