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

fixed graph formation

This commit is contained in:
dave 2017-06-26 14:09:33 -04:00
parent 78edc3c9f0
commit d7ca910e21
11 changed files with 254 additions and 222 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
include/buzz_utility.h
View File

@ -20,8 +20,15 @@ struct pos_struct
pos_struct(double x,double y,double z):x(x),y(y),z(z){};
pos_struct(){}
};
typedef struct pos_struct Pos_struct;
struct rb_struct
{
double r,b,la,lo;
rb_struct(double la, double lo, double r,double b):la(la),lo(lo),r(r),b(b){};
rb_struct(){}
};
typedef struct rb_struct RB_struct;
uint16_t* u64_cvt_u16(uint64_t u64);

3
include/buzzuav_closures.h
View File

@ -87,7 +87,8 @@ int buzzuav_update_battery(buzzvm_t vm);
* Updates current position in Buzz
*/
int buzzuav_update_currentpos(buzzvm_t vm);
int buzzuav_adduserRB(buzzvm_t vm);
int buzzuav_update_targets(buzzvm_t vm);
int buzzuav_addtargetRB(buzzvm_t vm);
/*
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
* use flight.status for flight status

1
include/roscontroller.h
View File

@ -171,6 +171,7 @@ private:
double longitude,
double altitude);
/*convert from spherical to cartesian coordinate system callback */
float constrainAngle(float x);
void gps_rb(GPS nei_pos, double out[]);
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
void gps_ned_home(float &ned_x, float &ned_y);

3
misc/cmdlinectr.sh
View File

@ -11,6 +11,9 @@ function arm {
function disarm {
rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0
}
function testWP {
rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10
}
function record {
rosbag record $1/flight_status $1/global_position $1/users_pos $1/local_position $1/neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info
}

1
script/Graph_drone.graph
View File

@ -2,3 +2,4 @@
1 0 1000.0 0.0
2 0 1000.0 1.57
3 0 1000.0 3.14
4 0 1000.0 4.71

2
script/flock.bzz
View File

@ -6,7 +6,7 @@ include "vstigenv.bzz"
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 14.0
EPSILON = 18.0
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {

40
script/graphform.bzz
View File

@ -7,12 +7,7 @@ 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.
#
#Constant parameters, need to be adjust
#parameters for set_wheels
m_sWheelTurningParams={.MaxSpeed=1.0}
ROBOT_RADIUS=0.5
ROBOT_RADIUS=50
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
@ -102,14 +97,6 @@ var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return mag
}
function Norm(vector) {
var l = math.vec2.length(vector)
return {
.x = vector.x / l,
.y = vector.y / l
}
}
function LimitAngle(angle){
if(angle>2*math.pi)
return angle-2*math.pi
@ -119,15 +106,6 @@ else
return angle
}
#
# Calculates the length of the given vector2.
# PARAM v: The vector2.
# RETURN: The length of the vector.
#
Length = function(v) {
return math.sqrt(v.x * v.x + v.y * v.y)
}
#
# Calculates the angle of the given vector2.
# PARAM v: The vector2.
@ -554,19 +532,18 @@ function DoJoining(){
#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_dis=Length(S2Target)
var S2Target_bearing=Angle(S2Target)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias
m_navigation=math.vec2.newp(S2Target_dis,S2Target_bearing)
#S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17
m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
#test if is already in desired position
if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
log(S2Target_dis,S2Target_bearing)
#TransitionToJoined()
#log(S2Target_dis,S2Target_bearing)
TransitionToJoined()
return
}
} else{ #miss pred, there is a change the another robot block the sight, keep moving as before for sometime
@ -662,7 +639,7 @@ function DoJoined(){
if(v_tag.size()==ROBOTS){
#TransitionToLock()
TransitionToLock()
}
}
@ -733,6 +710,7 @@ uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
function action(){
statef=action
UAVSTATE="GRAPH"
}
#
@ -744,7 +722,7 @@ function init() {
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=150
m_fTargetDistanceTolerance=10
m_unJoiningLostPeriod=100
#
@ -767,7 +745,7 @@ function step(){
#
#act according to current state
#
if(UAVSTATE=="IDLE"){
if(UAVSTATE=="GRAPH"){
if(m_eState=="STATE_FREE")
DoFree()
else if(m_eState=="STATE_ESCAPE")

24
script/include/uavstates.bzz
View File

@ -50,7 +50,7 @@ function land() {
uav_land()
}
else {
barrier_set(ROBOTS,idle,land)
barrier_set(ROBOTS,turnedoff,land)
barrier_ready()
timeW=0
#barrier = nil
@ -58,6 +58,22 @@ function land() {
}
}
function follow() {
if(size(targets)>0) {
UAVSTATE = "FOLLOW"
statef=follow
attractor=math.vec2.newp(0,0)
foreach(targets, function(id, tab) {
force=(0.05)*(tab.range)^4
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
})
uav_moveto(attractor.x, attractor.y)
} else {
log("No target in local table!")
#statef=idle
}
}
function uav_rccmd() {
if(flight.rc_cmd==22) {
log("cmd 22")
@ -74,9 +90,11 @@ function uav_rccmd() {
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
UAVSTATE = "FOLLOW"
log(rc_goto.latitude, " ", rc_goto.longitude)
add_targetrb(0,rc_goto.latitude,rc_goto.longitude)
statef = follow
#uav_goto()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()

54
src/buzz_utility.cpp
View File

@ -40,10 +40,10 @@ namespace buzz_utility{
void update_users(){
if(users_map.size()>0) {
/* Reset users information */
buzzusers_reset();
// Reset users information
// buzzusers_reset();
// create_stig_tables();
/* Get user id and update user information */
// Get user id and update user information
map< int, Pos_struct >::iterator it;
for (it=users_map.begin(); it!=users_map.end(); ++it){
//ROS_INFO("Buzz_utility will save user %i.", it->first);
@ -52,21 +52,20 @@ namespace buzz_utility{
(it->second).y,
(it->second).z);
}
}/*else
ROS_INFO("[%i] No new users",Robot_id);*/
}
}
int buzzusers_reset() {
/*int buzzusers_reset() {
if(VM->state != BUZZVM_STATE_READY) return VM->state;
/* Make new table */
//Make new table
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
//make_table(&t);
if(VM->state != BUZZVM_STATE_READY) return VM->state;
/* Register table as global symbol */
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
//Register table as global symbol
//buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);*/
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
@ -75,55 +74,47 @@ namespace buzz_utility{
//buzzvm_pushi(VM, 2);
//buzzvm_callc(VM);
return VM->state;
}
}*/
int buzzusers_add(int id, double latitude, double longitude, double altitude) {
if(VM->state != BUZZVM_STATE_READY) return VM->state;
/* Get users "p" table */
// Get users "p" table
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
buzzvm_tget(VM);*/
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
//buzzvm_pushi(VM, 1);
//buzzvm_callc(VM);
buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE);
buzzobj_t nbr = buzzvm_stack_at(VM, 1);
/* Get "data" field */
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_tget(VM);
if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) {
//ROS_INFO("Empty data, create a new table");
buzzvm_pop(VM);
buzzvm_push(VM, nbr);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_pusht(VM);
buzzobj_t data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
buzzvm_push(VM, data);
}
/* When we get here, the "data" table is on top of the stack */
/* Push user id */
// When we get here, the "data" table is on top of the stack
// Push user id
buzzvm_pushi(VM, id);
/* Create entry table */
// Create entry table
buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
/* Insert latitude */
// Insert latitude
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1));
buzzvm_pushf(VM, latitude);
buzzvm_tput(VM);
/* Insert longitude */
// Insert longitude
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1));
buzzvm_pushf(VM, longitude);
buzzvm_tput(VM);
/* Insert altitude */
// Insert altitude
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1));
buzzvm_pushf(VM, altitude);
buzzvm_tput(VM);
/* Save entry into data table */
// Save entry into data table
buzzvm_push(VM, entry);
buzzvm_tput(VM);
//ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
@ -329,8 +320,8 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_adduserRB));
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB));
buzzvm_gstore(VM);
return VM->state;
@ -371,7 +362,7 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
@ -692,6 +683,7 @@ int create_stig_tables() {
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM);
buzzuav_closures::buzzuav_update_targets(VM);
//update_users();
buzzuav_closures::buzzuav_update_flight_status(VM);
}

83
src/buzzuav_closures.cpp
View File

@ -26,7 +26,7 @@ namespace buzzuav_closures{
static int buzz_cmd=0;
static float height=0;
std::map< int, buzz_utility::RB_struct> targets_map;
std::map< int, buzz_utility::Pos_struct> neighbors_map;
/****************************************/
@ -89,13 +89,20 @@ namespace buzzuav_closures{
out[2] = height; //constant height.
}
float constrainAngle(float x){
x = fmod(x,2*M_PI);
if (x < 0.0)
x += 2*M_PI;
return x;
}
void rb_from_gps(double nei[], double out[], double cur[]){
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
out[1] = atan2(ned_y,ned_x);
out[1] = constrainAngle(atan2(ned_y,ned_x));
out[2] = 0.0;
}
@ -126,57 +133,57 @@ namespace buzzuav_closures{
gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
//printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
//printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",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
return buzzvm_ret0(vm);
}
int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) {
int buzzuav_update_targets(buzzvm_t vm) {
if(vm->state != BUZZVM_STATE_READY) return vm->state;
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
buzzvm_gload(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
buzzobj_t nbr = buzzvm_stack_at(vm, 1);
/* Get "data" field */
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
buzzvm_tget(vm);
if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) {
//ROS_INFO("Empty data, create a new table");
buzzvm_pop(vm);
buzzvm_push(vm, nbr);
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1));
//buzzobj_t t = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
//buzzvm_push(vm, t);
buzzvm_pusht(vm);
buzzobj_t data = buzzvm_stack_at(vm, 1);
buzzvm_tput(vm);
buzzvm_push(vm, data);
}
/* When we get here, the "data" table is on top of the stack */
buzzobj_t targettbl = buzzvm_stack_at(vm, 1);
//buzzvm_tput(vm);
//buzzvm_dup(vm);
double rb[3], tmp[3];
map< int, buzz_utility::RB_struct >::iterator it;
for (it=targets_map.begin(); it!=targets_map.end(); ++it){
tmp[0]=(it->second).la;tmp[1]=(it->second).lo;tmp[2]=height;
rb_from_gps(tmp, rb, cur_pos);
ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]);
buzzvm_push(vm, targettbl);
/* When we get here, the "targets" table is on top of the stack */
//ROS_INFO("Buzz_utility will save user %i.", it->first);
/* Push user id */
buzzvm_pushi(vm, id);
buzzvm_pushi(vm, it->first);
/* Create entry table */
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
/* Insert range */
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1));
buzzvm_pushf(vm, range);
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
buzzvm_pushf(vm, rb[0]);
buzzvm_tput(vm);
/* Insert longitude */
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1));
buzzvm_pushf(vm, bearing);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
buzzvm_pushf(vm, rb[1]);
buzzvm_tput(vm);
/* Save entry into data table */
buzzvm_push(vm, entry);
buzzvm_tput(vm);
//printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing);
}
buzzvm_gstore(vm);
return vm->state;
}
int buzzuav_adduserRB(buzzvm_t vm) {
int buzzuav_addtargetRB(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* longitude */
buzzvm_lload(vm, 2); /* latitude */
buzzvm_lload(vm, 3); /* id */
buzzvm_lload(vm, 1); // longitude
buzzvm_lload(vm, 2); // latitude
buzzvm_lload(vm, 3); // id
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
@ -190,9 +197,19 @@ namespace buzzuav_closures{
rb_from_gps(tmp, rb, cur_pos);
if(fabs(rb[0])<100.0) {
//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
return users_add2localtable(vm, uid, rb[0], rb[1]);
buzz_utility::RB_struct RB_arr;
RB_arr.la=tmp[0];
RB_arr.lo=tmp[1];
RB_arr.r=rb[0];
RB_arr.b=rb[1];
map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(uid);
if(it!=targets_map.end())
targets_map.erase(it);
targets_map.insert(make_pair(uid, RB_arr));
//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
return vm->state;
} else
printf(" ---------- User too far %f\n",rb[0]);
printf(" ---------- Target too far %f\n",rb[0]);
return 0;
}

18
src/roscontroller.cpp
View File

@ -92,22 +92,28 @@ namespace rosbzz_node{
///////////////////////////////////////////////////////
// MAIN LOOP
//////////////////////////////////////////////////////
ROS_INFO("[%i] STARTING MAIN LOOP!", robot_id);
//ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
while (ros::ok() && !buzz_utility::buzz_script_done())
{
/*Update neighbors position inside Buzz*/
//buzz_closure::neighbour_pos_callback(neighbours_pos_map);
/*Neighbours of the robot published with id in respective topic*/
//ROS_WARN("[%i]-----------NEIG", robot_id);
neighbours_pos_publisher();
/*Check updater state and step code*/
//ROS_WARN("[%i]-----------UPD", robot_id);
update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */
//ROS_WARN("[%i]-----------STEP", robot_id);
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
//ROS_WARN("[%i]-----------MSG", robot_id);
prepare_msg_and_publish();
/*call flight controler service to set command long*/
//ROS_WARN("[%i]-----------FC", robot_id);
flight_controller_service_call();
/*Set multi message available after update*/
//ROS_WARN("[%i]-----------UPD", robot_id);
if(get_update_status()){
set_read_update_status();
multi_msg=true;
@ -122,6 +128,7 @@ namespace rosbzz_node{
}
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
//no_of_robots=get_number_of_robots();
//ROS_WARN("[%i]-----------ROBOTS", robot_id);
get_number_of_robots();
buzz_utility::set_robot_var(no_of_robots);
//if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
@ -603,13 +610,20 @@ namespace rosbzz_node{
}
}
float roscontroller::constrainAngle(float x){
x = fmod(x,2*M_PI);
if (x < 0.0)
x += 2*M_PI;
return x;
}
void roscontroller::gps_rb(GPS nei_pos, double out[])
{
float ned_x=0.0, ned_y=0.0;
gps_ned_cur(ned_x, ned_y, nei_pos);
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
//out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned_y,ned_x);
out[1] = constrainAngle(atan2(ned_y,ned_x));
//out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;
}