Merge remote-tracking branch 'refs/remotes/origin/dev'
Conflicts: src/roscontroller.cpp
This commit is contained in:
isvogor
commit
9dc997e45c
|
|
@ -129,6 +129,8 @@ int get_update_status();
|
|||
|
||||
void set_read_update_status();
|
||||
|
||||
int compile_bzz();
|
||||
|
||||
void updates_set_robots(int robots);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -21,12 +21,6 @@ struct pos_struct
|
|||
pos_struct(){}
|
||||
};
|
||||
|
||||
#define function_register(TABLE, FNAME, FPOINTER) \
|
||||
buzzvm_push(VM, (TABLE)); \
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, (FNAME), 1)); \
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, (FPOINTER))); \
|
||||
buzzvm_tput(VM);
|
||||
|
||||
typedef struct pos_struct Pos_struct ;
|
||||
|
||||
uint16_t* u64_cvt_u16(uint64_t u64);
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ public:
|
|||
void RosControllerRun();
|
||||
|
||||
private:
|
||||
struct num_robot_count
|
||||
struct num_robot_count
|
||||
{
|
||||
uint8_t history[10];
|
||||
uint8_t index=0;
|
||||
|
|
@ -61,18 +61,16 @@ private:
|
|||
|
||||
}; typedef struct num_robot_count Num_robot_count ;
|
||||
|
||||
// WGS84 constants
|
||||
double equatorial_radius = 6378137.0;
|
||||
double flattening = 1.0/298.257223563;
|
||||
double excentrity2 = 2*flattening - flattening*flattening;
|
||||
// default reference position
|
||||
double DEFAULT_REFERENCE_LATITUDE = 45.457817;
|
||||
double DEFAULT_REFERENCE_LONGITUDE = -73.636075;
|
||||
|
||||
double target[3];
|
||||
double cur_pos[3];
|
||||
double home[3];
|
||||
struct gps
|
||||
{
|
||||
double longitude=0.0;
|
||||
double latitude=0.0;
|
||||
float altitude=0.0;
|
||||
}; typedef struct gps GPS ;
|
||||
|
||||
GPS target, home, cur_pos;
|
||||
double cur_rel_altitude;
|
||||
|
||||
uint64_t payload;
|
||||
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
|
||||
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
|
||||
|
|
@ -159,13 +157,17 @@ private:
|
|||
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
|
||||
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
||||
|
||||
/*Set the current position of the robot callback*/
|
||||
/*Set the current position of the robot callback*/
|
||||
void set_cur_pos(double latitude,
|
||||
double longitude,
|
||||
double altitude);
|
||||
/*convert from spherical to cartesian coordinate system callback */
|
||||
void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
|
||||
void cvt_ned_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
|
||||
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);
|
||||
void gps_convert_ned(float &ned_x, float &ned_y,
|
||||
double gps_t_lon, double gps_t_lat,
|
||||
double gps_r_lon, double gps_r_lat);
|
||||
|
||||
/*battery status callback*/
|
||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||
|
|
@ -178,7 +180,7 @@ private:
|
|||
|
||||
/*current position callback*/
|
||||
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||
void users_pos(const rosbuzz::neigh_pos msg);
|
||||
void users_pos(const rosbuzz::neigh_pos msg);
|
||||
|
||||
/*current relative altitude callback*/
|
||||
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
|
||||
|
|
|
|||
|
|
@ -9,6 +9,8 @@ if(r. data < l. data or (r. data == l. data )) return l
|
|||
else return r
|
||||
})
|
||||
|
||||
s = swarm.create(1)
|
||||
s.join()
|
||||
}
|
||||
|
||||
function stand_by(){
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
|
||||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
|
|
|
|||
|
|
@ -0,0 +1,92 @@
|
|||
#
|
||||
# Returns the string character at the given position.
|
||||
# PARAM s: The string
|
||||
# PARAM n: The position of the wanted character
|
||||
# RETURN The character at the wanted position, or nil
|
||||
#
|
||||
string.charat = function(s, n) {
|
||||
return string.sub(s, n, n+1)
|
||||
}
|
||||
|
||||
#
|
||||
# Returns the index of the first occurrence of the given string m
|
||||
# within another string s. If none is found, this function returns
|
||||
# nil.
|
||||
# PARAM s: The string
|
||||
# PARAM m: The string to match
|
||||
# RETURN: The position of the first match, or nil
|
||||
#
|
||||
string.indexoffirst = function(s, m) {
|
||||
var ls = string.length(s)
|
||||
var lm = string.length(m)
|
||||
var i = 0
|
||||
while(i < ls-lm+1) {
|
||||
if(string.sub(s, i, i+lm) == m) return i
|
||||
i = i + 1
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
#
|
||||
# Returns the index of the last occurrence of the given string m
|
||||
# within another string s. If none is found, this function returns
|
||||
# nil.
|
||||
# PARAM s: The string
|
||||
# PARAM m: The string to match
|
||||
# RETURN: The position of the last match, or nil
|
||||
#
|
||||
string.indexoflast = function(s, m) {
|
||||
var ls = string.length(s)
|
||||
var lm = string.length(m)
|
||||
var i = ls - lm + 1
|
||||
while(i >= 0) {
|
||||
if(string.sub(s, i, i+lm) == m) return i
|
||||
i = i - 1
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
# Splits a string s using the delimiters in d. The string list is
|
||||
# returned in a table indexed by value (starting at 0).
|
||||
# PARAM s: The string
|
||||
# PARAM d: A string containing the delimiters
|
||||
# RETURN: A table containing the tokens
|
||||
string.split = function(s, d) {
|
||||
var i1 = 0 # index to move along s (token start)
|
||||
var i2 = 0 # index to move along s (token end)
|
||||
var c = 0 # token count
|
||||
var t = {} # token list
|
||||
var ls = string.length(s)
|
||||
var ld = string.length(d)
|
||||
# Go through string s
|
||||
while(i2 < ls) {
|
||||
# Try every delimiter
|
||||
var j = 0 # index to move along d
|
||||
var f = nil # whether the delimiter was found or not
|
||||
while(j < ld and (not f)) {
|
||||
if(string.charat(s, i2) == string.charat(d, j)) {
|
||||
# Delimiter found
|
||||
f = 1
|
||||
# Is it worth adding a new token?
|
||||
if(i2 > i1) {
|
||||
t[c] = string.sub(s, i1, i2)
|
||||
c = c + 1
|
||||
}
|
||||
# Start new token
|
||||
i1 = i2 + 1
|
||||
}
|
||||
else {
|
||||
# Next delimiter
|
||||
j = j + 1
|
||||
}
|
||||
}
|
||||
# Next string character
|
||||
i2 = i2 + 1
|
||||
}
|
||||
# Is it worth adding a new token?
|
||||
if(i2 > i1) {
|
||||
t[c] = string.sub(s, i1, i2)
|
||||
}
|
||||
# Return token list
|
||||
return t;
|
||||
}
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
#
|
||||
# Create a new namespace for vector2 functions
|
||||
#
|
||||
math.vec2 = {}
|
||||
|
||||
#
|
||||
# Creates a new vector2.
|
||||
# PARAM x: The x coordinate.
|
||||
# PARAM y: The y coordinate.
|
||||
# RETURN: A new vector2.
|
||||
#
|
||||
math.vec2.new = function(x, y) {
|
||||
return { .x = x, .y = y }
|
||||
}
|
||||
|
||||
#
|
||||
# Creates a new vector2 from polar coordinates.
|
||||
# PARAM l: The length of the vector2.
|
||||
# PARAM a: The angle of the vector2.
|
||||
# RETURN: A new vector2.
|
||||
#
|
||||
math.vec2.newp = function(l, a) {
|
||||
return {
|
||||
.x = l * math.cos(a),
|
||||
.y = l * math.sin(a)
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Calculates the length of the given vector2.
|
||||
# PARAM v: The vector2.
|
||||
# RETURN: The length of the vector.
|
||||
#
|
||||
math.vec2.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.
|
||||
# RETURN: The angle of the vector.
|
||||
#
|
||||
math.vec2.angle = function(v) {
|
||||
return math.atan2(v.y, v.x)
|
||||
}
|
||||
|
||||
#
|
||||
# Returns the normalized form of a vector2.
|
||||
# PARAM v: The vector2.
|
||||
# RETURN: The normalized form.
|
||||
#
|
||||
math.vec2.norm = function(v) {
|
||||
var l = math.length(v)
|
||||
return {
|
||||
.x = v.x / l,
|
||||
.y = v.y / l
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Calculates v1 + v2.
|
||||
# PARAM v1: A vector2.
|
||||
# PARAM v2: A vector2.
|
||||
# RETURN: v1 + v2
|
||||
#
|
||||
math.vec2.add = function(v1, v2) {
|
||||
return {
|
||||
.x = v1.x + v2.x,
|
||||
.y = v1.y + v2.y
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Calculates v1 - v2.
|
||||
# PARAM v1: A vector2.
|
||||
# PARAM v2: A vector2.
|
||||
# RETURN: v1 + v2
|
||||
#
|
||||
math.vec2.sub = function(v1, v2) {
|
||||
return {
|
||||
.x = v1.x - v2.x,
|
||||
.y = v1.y - v2.y
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Scales a vector by a numeric constant.
|
||||
# PARAM v: A vector2.
|
||||
# PARAM s: A number (float or int).
|
||||
# RETURN: s * v
|
||||
#
|
||||
math.vec2.scale = function(v, s) {
|
||||
return {
|
||||
.x = v.x * s,
|
||||
.y = v.y * s
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Calculates v1 . v2 (the dot product)
|
||||
# PARAM v1: A vector2.
|
||||
# PARAM v2: A vector2.
|
||||
# RETURN: v1 . v2
|
||||
#
|
||||
math.vec2.dot = function(v1, v2) {
|
||||
return v1.x * v2.x + v1.y * v2.y
|
||||
}
|
||||
|
|
@ -1,4 +1,5 @@
|
|||
# We need this for 2D vectors
|
||||
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
####################################################################################################
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
include "vec2.bzz"
|
||||
####################################################################################################
|
||||
# Updater related
|
||||
# This should be here for the updater to work, changing position of code will crash the updater
|
||||
|
|
@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.0
|
|||
CURSTATE = "TURNEDOFF"
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 10.0 #0.000001001
|
||||
EPSILON = 18.0 #0.001
|
||||
TARGET = 12.0 #0.000001001
|
||||
EPSILON = 3.0 #0.001
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
|
|
@ -48,9 +48,12 @@ function hexagon() {
|
|||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
# timeW =0
|
||||
# statef=land
|
||||
# } else if(timeW>=WAIT_TIMEOUT/2) {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.06,0.0)
|
||||
# } else {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,0.0)
|
||||
# uav_moveto(0.0,0.06)
|
||||
# }
|
||||
}
|
||||
|
||||
|
|
@ -142,11 +145,32 @@ function land() {
|
|||
}
|
||||
}
|
||||
|
||||
function users_save(t) {
|
||||
if(size(t)>0) {
|
||||
foreach(t, function(id, tab) {
|
||||
#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
|
||||
add_user_rb(id,tab.la,tab.lo)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
function table_print(t) {
|
||||
if(size(t)>0) {
|
||||
foreach(t, function(u, tab) {
|
||||
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
s = swarm.create(1)
|
||||
# s.select(1)
|
||||
s.join()
|
||||
|
||||
vt = stigmergy.create(5)
|
||||
t = {}
|
||||
vt.put("p",t)
|
||||
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
}
|
||||
|
|
@ -197,6 +221,14 @@ neighbors.listen("cmd",
|
|||
statef()
|
||||
log("Current state: ", CURSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
|
||||
# Check local users and push to v.stig
|
||||
if(size(users.dataG)>0)
|
||||
vt.put("p", users.dataG)
|
||||
|
||||
# Save locally the users and print RG
|
||||
users_save(vt.get("p"))
|
||||
table_print(users.dataL)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
include "vec2.bzz"
|
||||
####################################################################################################
|
||||
# Updater related
|
||||
# This should be here for the updater to work, changing position of code will crash the updater
|
||||
|
|
@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.0
|
|||
CURSTATE = "TURNEDOFF"
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 10.0 #0.000001001
|
||||
EPSILON = 18.0 #0.001
|
||||
TARGET = 12.0 #0.000001001
|
||||
EPSILON = 3.0 #0.001
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
|
|
@ -48,9 +48,12 @@ function hexagon() {
|
|||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
# timeW =0
|
||||
# statef=land
|
||||
# } else if(timeW>=WAIT_TIMEOUT/2) {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.06,0.0)
|
||||
# } else {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,0.0)
|
||||
# uav_moveto(0.0,0.06)
|
||||
# }
|
||||
}
|
||||
|
||||
|
|
@ -142,11 +145,32 @@ function land() {
|
|||
}
|
||||
}
|
||||
|
||||
function users_save(t) {
|
||||
if(size(t)>0) {
|
||||
foreach(t, function(id, tab) {
|
||||
#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
|
||||
add_user_rb(id,tab.la,tab.lo)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
function table_print(t) {
|
||||
if(size(t)>0) {
|
||||
foreach(t, function(u, tab) {
|
||||
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
s = swarm.create(1)
|
||||
# s.select(1)
|
||||
s.join()
|
||||
|
||||
vt = stigmergy.create(5)
|
||||
t = {}
|
||||
vt.put("p",t)
|
||||
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
}
|
||||
|
|
@ -197,6 +221,14 @@ neighbors.listen("cmd",
|
|||
statef()
|
||||
log("Current state: ", CURSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
|
||||
# Check local users and push to v.stig
|
||||
if(size(users.dataG)>0)
|
||||
vt.put("p", users.dataG)
|
||||
|
||||
# Save locally the users and print RG
|
||||
users_save(vt.get("p"))
|
||||
table_print(users.dataL)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
|
|
|
|||
Loading…
Reference in New Issue