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

graph test

This commit is contained in:
dave 2017-07-22 14:14:06 -04:00
parent 05c584d085 f24fb63723
commit d31154602e
14 changed files with 270 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
buzz_scripts/graphform.bzz
View File

@ -175,7 +175,6 @@ function i2r(value){
else if(value==2){
return "REQ_GRANTED"
}
}
#map from response to int
function r2i(value){
@ -320,7 +319,6 @@ function motion_vector(){
return m_vector
}
function start_listen(){
neighbors.listen("m",
function(vid,value,rid){
@ -468,7 +466,7 @@ while(i<m_neighbourCunt){
}
m_navigation.x=0.0
m_navigation.y=0.0
goto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x,m_navigation.y)
}
#
@ -746,9 +744,9 @@ function DoJoined(){
#check if should to transists to lock
if(v_tag.size()==ROBOTS){
TransitionToLock()
}
# if(v_tag.size()==ROBOTS){
# TransitionToLock()
# }
}
#
@ -769,7 +767,7 @@ if(m_nLabel!=0){
m_navigation=motion_vector()
}
#move
goto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x,m_navigation.y)
}
function action(){
@ -793,7 +791,8 @@ function init() {
# Join Swarm
#
uav_initswarm()
Reset();
Reset()
Read_Graph()
}
#

5
buzz_scripts/include/Graph_drone.graph Normal file
View File

@ -0,0 +1,5 @@
0 -1 -1 -1 3000.0
1 0 1000.0 0.0 5000.0
2 0 1000.0 1.57 7000.0
3 0 1000.0 3.14 9000.0
4 0 1000.0 4.71 11000.0

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

@ -1,6 +1,7 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
@ -55,3 +56,4 @@ m_vecNodes[5] = {
.State="UNASSIGNED",
.StateAge=0
}
}

2
buzz_scripts/include/graphs/shapes_O.bzz
View File

@ -1,6 +1,7 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
@ -55,3 +56,4 @@ m_vecNodes[5] = {
.State="UNASSIGNED",
.StateAge=0
}
}

5
buzz_scripts/include/graphs/shapes_P.bzz
View File

@ -1,6 +1,6 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
@ -30,7 +30,7 @@ m_vecNodes[2] = {
}
m_vecNodes[3] = {
.Lable = 3,
.Pred = 1,
.Pred = 0,
.distance = 2000,
.bearing = 4.71,
.height = 9000,
@ -55,3 +55,4 @@ m_vecNodes[5] = {
.State="UNASSIGNED",
.StateAge=0
}
}

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

@ -1,6 +1,6 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
@ -55,3 +55,4 @@ m_vecNodes[5] = {
.State="UNASSIGNED",
.StateAge=0
}
}

65
buzz_scripts/include/graphs/shapes_square.bzz
View File

@ -1,6 +1,8 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
@ -47,65 +49,4 @@ m_vecNodes[4] = {
.StateAge=0
}
#
# Graph parsing
#
function parse_graph(fname) {
# Graph data
var gd = {}
# Open the file
var fd = io.fopen(fname, "r")
if(not fd) {
log("Can't open '", fname, "'")
return nil
}
# Parse the file line by line
var rrec # Record read from line
var grec # Record parsed into graph
io.fforeach(fd, function(line) {
# Parse file line
rrec = string.split(line, "\t ")
# Make record
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = string.toint(rrec[0]), # Lable of the point
.Pred = string.toint(rrec[1]), # Lable of its predecessor
.distance = string.tofloat(rrec[2]), # distance to the predecessor
.bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot
.height = string.tofloat(rrec[4]), # height of this dot
.State="UNASSIGNED",
.StateAge=0
}})
# All done
io.fclose(fd)
return gd
}
function parse_graph_fixed(fname) {
# Graph data
var gd = {}
# Open the file
var fd = io.fopen(fname, "r")
if(not fd) {
log("Can't open '", fname, "'")
return nil
}
# Parse the file line by line
var rrec # Record read from line
var grec # Record parsed into graph
io.fforeach(fd, function(line) {
# Parse file line
rrec = string.split(line, "\t ")
# Make record
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2
.Pred1 = string.toint(rrec[1]), # Pred 1 lable
.Pred2 = string.toint(rrec[3]), # Pred 2 lable
.d1 = string.tofloat(rrec[2]), # Pred 1 distance
.d2 = string.tofloat(rrec[4]), # Pred 2 distance
.Lable=string.toint(rrec[0]),
.State="UNASSIGNED",
.StateAge=0
}})
# All done
io.fclose(fd)
return gd
}

111
buzz_scripts/include/shapes.bzz Normal file
View File

@ -0,0 +1,111 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = 0, # Lable of the point
.Pred = -1, # Lable of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to this dot
.height = 3000, # height of this dot
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[1] = {
.Lable = 1,
.Pred = 0,
.distance = 1000,
.bearing = 0.0,
.height = 5000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[2] = {
.Lable = 2,
.Pred = 0,
.distance = 1000,
.bearing = 1.57,
.height = 7000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[3] = {
.Lable = 3,
.Pred = 0,
.distance = 1000,
.bearing = 3.14,
.height = 9000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[4] = {
.Lable = 4,
.Pred = 0,
.distance = 1000,
.bearing = 4.71,
.height = 11000,
.State="UNASSIGNED",
.StateAge=0
}
#
# Graph parsing
#
function parse_graph(fname) {
# Graph data
var gd = {}
# Open the file
var fd = io.fopen(fname, "r")
if(not fd) {
log("Can't open '", fname, "'")
return nil
}
# Parse the file line by line
var rrec # Record read from line
var grec # Record parsed into graph
io.fforeach(fd, function(line) {
# Parse file line
rrec = string.split(line, "\t ")
# Make record
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = string.toint(rrec[0]), # Lable of the point
.Pred = string.toint(rrec[1]), # Lable of its predecessor
.distance = string.tofloat(rrec[2]), # distance to the predecessor
.bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot
.height = string.tofloat(rrec[4]), # height of this dot
.State="UNASSIGNED",
.StateAge=0
}})
# All done
io.fclose(fd)
return gd
}
function parse_graph_fixed(fname) {
# Graph data
var gd = {}
# Open the file
var fd = io.fopen(fname, "r")
if(not fd) {
log("Can't open '", fname, "'")
return nil
}
# Parse the file line by line
var rrec # Record read from line
var grec # Record parsed into graph
io.fforeach(fd, function(line) {
# Parse file line
rrec = string.split(line, "\t ")
# Make record
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2
.Pred1 = string.toint(rrec[1]), # Pred 1 lable
.Pred2 = string.toint(rrec[3]), # Pred 2 lable
.d1 = string.tofloat(rrec[2]), # Pred 1 distance
.d2 = string.tofloat(rrec[4]), # Pred 2 distance
.Lable=string.toint(rrec[0]),
.State="UNASSIGNED",
.StateAge=0
}})
# All done
io.fclose(fd)
return gd
}

9
include/buzzuav_closures.h
View File

@ -47,6 +47,11 @@ void rc_set_goto(double pos[]);
void rc_call(int rc_cmd);
/* sets the battery state */
void set_battery(float voltage,float current,float remaining);
void set_deque_full(bool state);
void set_rssi(float value);
void set_raw_packet_loss(float value);
void set_filtered_packet_loss(float value);
void set_api_rssi(float value);
/* sets current position */
void set_currentpos(double latitude, double longitude, double altitude);
/*retuns the current go to position */
@ -83,6 +88,10 @@ int buzzuav_gohome(buzzvm_t vm);
* Updates battery information in Buzz
*/
int buzzuav_update_battery(buzzvm_t vm);
/*
* Updates xbee_status information in Buzz
*/
int buzzuav_update_xbee_status(buzzvm_t vm);
/*
* Updates current position in Buzz
*/

2
include/roscontroller.h
View File

@ -244,6 +244,8 @@ private:
bool GetRawPacketLoss(const uint8_t short_id, float &result);
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
void get_xbee_status();
};
}

4
launch/rosbuzz-solo.launch
View File

@ -43,7 +43,7 @@
<!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.bzz" />
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/flock.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
@ -51,7 +51,7 @@
<param name="xbee_status_srv" value="xbee_status"/>
<param name="xbee_plugged" value="true"/>
<param name="name" value="solos1"/>
<param name="stand_by" value="$(find rosbuzz)/script/stand_by.bzz"/>
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
</node>
</launch>

4
src/buzz_utility.cpp
View File

@ -182,6 +182,8 @@ namespace buzz_utility{
uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]);
/*Size is at first 2 bytes*/
uint16_t size=data[0]*sizeof(uint64_t);
uint16_t neigh_id = data[1];
ROS_WARN("NEIG ID %i",neigh_id);
delete[] data;
/*size and robot id read*/
size_t tot = sizeof(uint32_t);
@ -195,6 +197,7 @@ namespace buzz_utility{
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
buzzinmsg_queue_append(VM,
neigh_id,
buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize));
tot += unMsgSize;
}
@ -681,6 +684,7 @@ int create_stig_tables() {
void update_sensors(){
/* Update sensors*/
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_xbee_status(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM);

60
src/buzzuav_closures.cpp
View File

@ -24,6 +24,11 @@ namespace buzzuav_closures{
static int rc_cmd=0;
static int buzz_cmd=0;
static float height=0;
static bool deque_full = false;
static float rssi = 0.0;
static float raw_packet_loss = 0.0;
static float filtered_packet_loss = 0.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;
@ -334,7 +339,60 @@ namespace buzzuav_closures{
buzzvm_gstore(vm);
return vm->state;
}
/****************************************/
void set_deque_full(bool state)
{
deque_full = state;
}
void set_rssi(float value)
{
rssi = value;
}
void set_raw_packet_loss(float value)
{
raw_packet_loss = value;
}
void set_filtered_packet_loss(float value)
{
filtered_packet_loss = value;
}
void set_api_rssi(float value)
{
api_rssi = value;
}
int buzzuav_update_xbee_status(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "deque_full", 1));
buzzvm_pushi(vm, static_cast<uint8_t>(deque_full));
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1));
buzzvm_pushf(vm, rssi);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1));
buzzvm_pushf(vm, raw_packet_loss);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1));
buzzvm_pushf(vm, filtered_packet_loss);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1));
buzzvm_pushf(vm, api_rssi);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
/***************************************/
/*current pos update*/
/***************************************/
void set_currentpos(double latitude, double longitude, double altitude){

51
src/roscontroller.cpp
View File

@ -113,7 +113,7 @@ bool roscontroller::TriggerAPIRssi(const uint8_t short_id)
srv_request.param_id = "trig_rssi_api_" + std::to_string(short_id);
}
mavros_msgs::ParamGet::Response srv_response;
if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
if(!xbeestatus_srv.call(srv_request, srv_response)){return false;}
return srv_response.success;
}
@ -130,7 +130,7 @@ bool roscontroller::GetAPIRssi(const uint8_t short_id, float &result)
srv_request.param_id = "get_rssi_api_" + std::to_string(short_id);
}
mavros_msgs::ParamGet::Response srv_response;
if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
if(!xbeestatus_srv.call(srv_request, srv_response)){return false;}
result = srv_response.value.real;
return srv_response.success;
@ -148,7 +148,7 @@ bool roscontroller::GetRawPacketLoss(const uint8_t short_id, float &result)
srv_request.param_id = "pl_raw_" + std::to_string(short_id);
}
mavros_msgs::ParamGet::Response srv_response;
if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
if(!xbeestatus_srv.call(srv_request, srv_response)){return false;}
result = srv_response.value.real;
return srv_response.success;
@ -166,7 +166,7 @@ bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float &result)
srv_request.param_id = "pl_filtered_" + std::to_string(short_id);
}
mavros_msgs::ParamGet::Response srv_response;
if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
if(!xbeestatus_srv.call(srv_request, srv_response)){return false;}
result = srv_response.value.real;
return srv_response.success;
@ -231,6 +231,7 @@ void roscontroller::RosControllerRun()
updates_set_robots(no_of_robots);
// ROS_INFO("ROBOTS: %i , acutal :
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
get_xbee_status();
/*run once*/
ros::spinOnce();
/*loop rate of ros*/
@ -1138,14 +1139,7 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
}
return true;
}
/*-----------------------------------------------------
/Obtain robot id by subscribing to xbee robot id topic
/ TODO: check for integrity of this subscriber call back
/----------------------------------------------------*/
/*void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
}*/
void roscontroller::get_number_of_robots() {
int cur_robots = (int)buzzdict_size(buzz_utility::get_vm()->swarmmembers) + 1;
if (no_of_robots == 0) {
@ -1202,4 +1196,39 @@ void roscontroller::get_number_of_robots() {
}
*/
}
void roscontroller::get_xbee_status()
/* Description:
* Call all the xbee node services and update the xbee status
------------------------------------------------------------------ */
{
bool result_bool;
float result_float;
const uint8_t all_ids = 0xFF;
if(GetDequeFull(result_bool))
{
buzzuav_closures::set_deque_full(result_bool);
}
if(GetRssi(result_float))
{
buzzuav_closures::set_rssi(result_float);
}
if(GetRawPacketLoss(all_ids, result_float))
{
buzzuav_closures::set_raw_packet_loss(result_float);
}
if(GetFilteredPacketLoss(all_ids, result_float))
{
buzzuav_closures::set_filtered_packet_loss(result_float);
}
// This part needs testing since it can overload the xbee module
/*
* if(GetAPIRssi(all_ids, result_float))
* {
* buzzuav_closures::set_api_rssi(result_float);
* }
* TriggerAPIRssi(all_ids);
*/
}
}