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

implement vstig users

Browse Source
This commit is contained in:
dave 2017-04-24 14:20:59 -04:00
parent 421d78bacc
commit 61d1cab41b
7 changed files with 148 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/buzz_utility.h
View File

@ -30,6 +30,8 @@ int buzz_listen(const char* type,
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
int buzz_update_users_stigmergy(buzzobj_t tbl);
void in_msg_process(uint64_t* payload);
uint64_t* out_msg_process();

3
include/buzzuav_closures.h
View File

@ -46,6 +46,7 @@ void rc_call(int rc_cmd);
void set_battery(float voltage,float current,float remaining);
/* sets current position */
void set_currentpos(double latitude, double longitude, double altitude);
void set_userspos(double latitude, double longitude, double altitude);
/*retuns the current go to position */
double* getgoto();
/* updates flight status*/
@ -82,7 +83,7 @@ int buzzuav_update_battery(buzzvm_t vm);
* Updates current position in Buzz
*/
int buzzuav_update_currentpos(buzzvm_t vm);
buzzobj_t buzzuav_update_userspos(buzzvm_t vm);
/*
* Updates flight status and rc command in Buzz, put it in a tabel to acess it

15
include/roscontroller.h
View File

@ -39,19 +39,6 @@
#define XBEE_STOP_TRANSMISSION 4355356352
#define TIMEOUT 60
/** Semi-major axis of the Earth, \f$ a \f$, in meters.
* This is a defining parameter of the WGS84 ellipsoid. */
#define WGS84_A 6378137.0
/** Inverse flattening of the Earth, \f$ 1/f \f$.
* This is a defining parameter of the WGS84 ellipsoid. */
#define WGS84_IF 298.257223563
/** The flattening of the Earth, \f$ f \f$. */
#define WGS84_F (1/WGS84_IF)
/** Semi-minor axis of the Earth in meters, \f$ b = a(1-f) \f$. */
#define WGS84_B (WGS84_A*(1-WGS84_F))
/** Eccentricity of the Earth, \f$ e \f$ where \f$ e^2 = 2f - f^2 \f$ */
#define WGS84_E (sqrt(2*WGS84_F - WGS84_F*WGS84_F))
using namespace std;
namespace rosbzz_node{
@ -117,6 +104,7 @@ private:
ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service;
ros::Subscriber current_position_sub;
ros::Subscriber users_sub;
ros::Subscriber battery_sub;
ros::Subscriber payload_sub;
ros::Subscriber flight_status_sub;
@ -189,6 +177,7 @@ private:
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
void users_pos(const rosbuzz::neigh_pos msg);
/*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);

47
src/buzz_utility.cpp
View File

@ -20,6 +20,8 @@ namespace buzz_utility{
static uint8_t MSG_SIZE = 50; // Only 100 bytes of Buzz messages every step
static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static int Robot_id = 0;
buzzobj_t usersvstig;
buzzobj_t key;
/****************************************/
/*adds neighbours position*/
@ -302,6 +304,30 @@ namespace buzz_utility{
fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename);
return 0;
}
buzzvm_dup(VM);
// usersvstig = stigmergy.create(123)
buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
// value of the stigmergy id
buzzvm_pushi(VM, 5);
// get the stigmergy table from the global scope
// buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
// buzzvm_gload(VM);
// get the create method from the stigmergy table
// buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
// buzzvm_tget(VM);
// call the stigmergy.create() method
// buzzvm_closure_call(VM, 1);
// now the stigmergy is at the top of the stack - save it for future reference
// usersvstig = buzzvm_stack_at(VM, 0);
//buzzvm_pop(VM);
// assign the virtual stigmergy to the global symbol v
// create also a global variable for it, so the garbage collector does not remove it
//buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
//buzzvm_push(VM, usersvstig);
buzzvm_gstore(VM);
/* Save bytecode file name */
BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */
@ -309,9 +335,25 @@ namespace buzz_utility{
/* Call the Init() function */
buzzvm_function_call(VM, "init", 0);
/* All OK */
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
int buzz_update_users_stigmergy(buzzobj_t tbl){
// push the key (here it's an int with value 46)
buzzvm_pushi(VM, 46);
// push the table
buzzvm_push(VM, tbl);
// the stigmergy is stored in the vstig variable
// let's push it on the stack
buzzvm_push(VM, usersvstig);
// get the put method from myvstig
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
// call the vstig.put() method
buzzvm_closure_call(VM, 2);
return 1;
}
/****************************************/
/*Sets a new update */
/****************************************/
@ -447,7 +489,12 @@ namespace buzz_utility{
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
//buzz_update_users_stigmergy(tbl);
/*
* Call Buzz step() function
*/

26
src/buzzuav_closures.cpp
View File

@ -19,6 +19,7 @@ namespace buzzuav_closures{
static float batt[3];
static float obst[5]={0,0,0,0,0};
static double cur_pos[3];
static double users_pos[3];
static uint8_t status;
static int cur_cmd = 0;
static int rc_cmd=0;
@ -244,6 +245,11 @@ namespace buzzuav_closures{
cur_pos[1]=longitude;
cur_pos[2]=altitude;
}
void set_userspos(double latitude, double longitude, double altitude){
users_pos[0]=latitude;
users_pos[1]=longitude;
users_pos[2]=altitude;
}
/****************************************/
int buzzuav_update_currentpos(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
@ -263,6 +269,26 @@ namespace buzzuav_closures{
buzzvm_gstore(vm);
return vm->state;
}
buzzobj_t buzzuav_update_userspos(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
buzzvm_pushf(vm, users_pos[0]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
buzzvm_pushf(vm, users_pos[1]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1));
buzzvm_pushf(vm, users_pos[2]);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return buzzvm_stack_at(vm, 0);
//return vm->state;
}
void flight_status_update(uint8_t state){
status=state;

60
src/roscontroller.cpp
View File

@ -224,6 +224,8 @@ namespace rosbzz_node{
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>("/"+robot_name+xbeesrv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>("/"+robot_name+stream_client_name);
users_sub = n_c.subscribe("/"+robot_name+"/users_pos", 1000, &roscontroller::users_pos,this);
multi_msg=true;
}
/*---------------------------------------
@ -424,6 +426,8 @@ namespace rosbzz_node{
Arm();
ros::Duration(0.5).sleep();
}
// Registering HOME POINT.
home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2];
if(current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
if (mav_client.call(cmd_srv))
@ -578,12 +582,24 @@ namespace rosbzz_node{
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
double ned[3];
matrix_multiply(3, 3, 1, (double *)M, ecef, ned);*/
matrix_multiply(3, 3, 1, (double *)M, ecef, ned);
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[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned_y,ned_x);
out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;*/
out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]);
out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned[1],ned[0]);
out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;
}
void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
@ -617,7 +633,15 @@ namespace rosbzz_node{
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/
matrix_multiply(3, 3, 1, (double *)M, ecef, out);
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;*/
}
/*------------------------------------------------------
@ -657,14 +681,35 @@ namespace rosbzz_node{
fcu_timeout = TIMEOUT;
double lat = std::floor(msg->latitude * 1000000) / 1000000;
double lon = std::floor(msg->longitude * 1000000) / 1000000;
if(cur_rel_altitude<0.2){
/*if(cur_rel_altitude<1.2){
home[0]=lat;
home[1]=lon;
home[2]=cur_rel_altitude;
}
}*/
set_cur_pos(lat,lon, cur_rel_altitude);//msg->altitude);
buzzuav_closures::set_currentpos(lat,lon, cur_rel_altitude);//msg->altitude);
}
void roscontroller::users_pos(const rosbuzz::neigh_pos data){
//ROS_INFO("Altitude out: %f", cur_rel_altitude);
double us[3];
int n = data.pos_neigh.size();
// ROS_INFO("Neighbors array size: %i\n", n);
if(n>0)
{
for(int it=0; it<n; ++it)
{
us[0] = data.pos_neigh[it].latitude;
us[1] = data.pos_neigh[it].longitude;
us[2] = data.pos_neigh[it].altitude;
double out[3];
cvt_rangebearing_coordinates(us, out, cur_pos);
buzzuav_closures::set_userspos(out[0], out[1], out[2]);
}
}
}
/*-------------------------------------------------------------
/ Update altitude into BVM from subscriber
/-------------------------------------------------------------*/
@ -694,6 +739,7 @@ namespace rosbzz_node{
double local_pos[3];
cvt_ned_coordinates(cur_pos,local_pos,home);
ROS_INFO("ROSBuzz LocalPos: %.7f,%.7f",local_pos[0],local_pos[1]);
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
moveMsg.pose.position.x = local_pos[1]+y;
moveMsg.pose.position.y = local_pos[0]+x;
@ -704,11 +750,11 @@ namespace rosbzz_node{
moveMsg.pose.orientation.z = 0;
moveMsg.pose.orientation.w = 1;
//if(fabs(x)>0.01 || fabs(y)>0.01) {
// To prevent drifting from stable position.
if(fabs(x)>0.01 || fabs(y)>0.01) {
localsetpoint_nonraw_pub.publish(moveMsg);
ROS_INFO("Sent local NON RAW position message!");
//}
}
}
void roscontroller::SetMode(std::string mode, int delay_miliseconds){

10
src/testflocksim.bzz
View File

@ -11,11 +11,11 @@ function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 5.0
TARGET_ALTITUDE = 10.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 10.0
TARGET = 12.0
EPSILON = 8.0
# Lennard-Jones interaction magnitude
@ -196,6 +196,12 @@ neighbors.listen("cmd",
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
log("User position: ", users.range)
# Read a value from the structure
#x = usersvstig.get(46)
# Get the number of keys in the structure
#log("The vstig has ", usersvstig.size(), " elements")
}
# Executed once when the robot (or the simulator) is reset.