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

field changs plus recording

This commit is contained in:
dave 2017-09-19 16:20:23 -04:00
parent 527569fe71 f2efd71ba6
commit c982c60592
4 changed files with 69 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
buzz_scripts/graphformGPS.bzz
View File

@ -5,11 +5,11 @@ include "string.bzz"
include "vec2.bzz"
include "update.bzz"
include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
#include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "graphs/shapes_Y.bzz"
include "graphs/shapes_L.bzz"
ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
@ -93,7 +93,7 @@ step_cunt=0
m_lockstig = 1
# Lennard-Jones parameters, may need change
EPSILON = 1800 #13.5 the LJ parameter for other robots
EPSILON = 4000 #13.5 the LJ parameter for other robots
# Lennard-Jones interaction magnitude
@ -311,6 +311,8 @@ neighbors.listen("m",
m_MessageRange[m_neighbourCount]=m_receivedMessage.Range
m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
m_messageID[m_neighbourCount]=rid
log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount])
m_neighbourCount=m_neighbourCount+1
})
}
@ -496,19 +498,24 @@ function DoAsking(){
log("get response")
psResponse=i
}}
if(m_MessageState[i]=="STATE_JOINING" and m_MessageLabel[i]==m_nLabel){
TransitionToFree()
return
}
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
}
#if(m_unWaitCount==0){
#TransitionToFree()
#return
#}
}
else{
log("respond id=",m_MessageReqID[psResponse])
@ -528,6 +535,8 @@ function DoAsking(){
}
}
}
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
@ -578,6 +587,9 @@ function set_rc_goto() {
#
#Do joined
#
repeat_assign=0
assign_label=-1
assign_id=-1
function DoJoined(){
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
@ -609,6 +621,12 @@ function DoJoined(){
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
}
}
if(m_MessageState[i]=="STATE_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
repeat_assign=0
}
#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
@ -627,15 +645,20 @@ function DoJoined(){
ReqIndex=i
i=i+1
}
if(repeat_assign==0){
#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
assign_label=ReqLabel
assign_id=ReqID
repeat_assign=1
}
m_selfMessage.ReqLabel=assign_label
m_selfMessage.ReqID=assign_id
m_selfMessage.Response=r2i("REQ_GRANTED")
m_vecNodes[ReqLabel].State="ASSIGNING"
log("Label=",ReqLabel)
log("ID=",ReqID)
#m_vecNodes[ReqLabel].State="ASSIGNING"
log("Label=",assign_label)
log("ID=",assign_id)
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
}
@ -704,9 +727,9 @@ function init() {
#
uav_initswarm()
#v_tag = stigmergy.create(m_lockstig)
uav_initstig()
#uav_initstig()
# go to diff. height since no collision avoidance implemented yet
TARGET_ALTITUDE = 2.5 + id * 1.5
TARGET_ALTITUDE = 3.0 + id * 1.25
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
@ -720,7 +743,7 @@ function step() {
# get the swarm commands
uav_neicmd()
# update the vstig (status/net/batt)
uav_updatestig()
#uav_updatestig()
#update the graph
UpdateNodeInfo()

5
buzz_scripts/include/barrier.bzz
View File

@ -7,7 +7,7 @@
#
# Constants
#
BARRIER_TIMEOUT = 200 # in steps
BARRIER_TIMEOUT = 1200 # in steps
BARRIER_VSTIG = 80
timeW = 0
barrier = nil
@ -52,13 +52,14 @@ function barrier_wait(threshold, transf, resumef, bdt) {
barrier.put(id, 1)
barrier.get(id)
#log("----->BS: ", barrier.size())
log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
barrier.put("d", 1)
timeW = 0
transf()
} else if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!")
barrier=nil
timeW = 0
resumef()
}

2
buzz_scripts/include/uavstates.bzz
View File

@ -152,7 +152,7 @@ function uav_rccmd() {
function uav_neicmd() {
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid, "(", UAVSTATE, ")")
print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")")
if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") {
statef=takeoff
UAVSTATE = "TAKEOFF"

29
src/roscontroller.cpp
View File

@ -3,6 +3,7 @@
namespace rosbzz_node {
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
const bool debug = false;
/*---------------
/Constructor
@ -52,9 +53,11 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv)
path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/";
std::string folder_check="mkdir -p "+path;
system(folder_check.c_str());
rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+".log").c_str(),
(path +"logger_"+ std::to_string((uint8_t)robot_id)+"_old.log").c_str());
path += "logger_"+std::to_string(robot_id)+".log";
for(int i=5;i>0;i--){
rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(),
(path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str());
}
path += "logger_"+std::to_string(robot_id)+"_0.log";
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
}
@ -193,6 +196,8 @@ void roscontroller::RosControllerRun()
/* Set the Buzz bytecode */
if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),
robot_id)) {
int packet_loss_tmp,time_step=0;
double cur_packet_loss=0;
ROS_INFO("[%i] Bytecode file found and set", robot_id);
std::string standby_bo = Compile_bzz(stand_by) + ".bo";
//init_update_monitor(bcfname.c_str(), standby_bo.c_str());
@ -223,15 +228,23 @@ void roscontroller::RosControllerRun()
const uint8_t shrt_id= 0xFF;
float result;
if ( GetFilteredPacketLoss(shrt_id, result) )
log<<result<<",";
else
log<<"0,";
/*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());
if(time_step==BUZZRATE){
time_step=0;
cur_packet_loss= 1 -( (double)packet_loss_tmp/(BUZZRATE*( (int)no_of_robots-1 )) );
packet_loss_tmp=0;
if(cur_packet_loss<0) cur_packet_loss=0;
else if (cur_packet_loss>1) cur_packet_loss=1;
}
else{
packet_loss_tmp+=(int)buzz_utility::get_inmsg_size();
time_step++;
}
if(debug) ROS_WARN("CURRENT PACKET DROP : %f ",cur_packet_loss);
/*Log In Msg queue size*/
log<<(int)buzz_utility::get_inmsg_size()<<",";
/*Step buzz script */
@ -1077,7 +1090,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
gps_rb(nei_pos, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/
uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3));
ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
if(debug) ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
/*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],
cvt_neighbours_pos_payload[1],