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

integrated enhancements from dev_kinetic

This commit is contained in:
dave 2018-09-07 00:05:19 -04:00
parent 52357b142d
commit 8580f8f306
10 changed files with 120 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
CMakeLists.txt
View File

@ -5,16 +5,8 @@ if(UNIX)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
endif() endif()
if(SIM) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=${SIM} -DMAVROSKINETIC=${KIN}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1")
else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0")
endif()
if(KIN)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAVROSKINETIC=1")
else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAVROSKINETIC=0")
endif()
## Find catkin macros and libraries ## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
roscpp roscpp

20
buzz_scripts/include/act/states.bzz
View File

@ -97,7 +97,7 @@ function take_picture() {
takepicture() takepicture()
} else if(pic_time>=PICTURE_WAIT) { # wait for the picture } else if(pic_time>=PICTURE_WAIT) { # wait for the picture
BVMSTATE="IDLE" BVMSTATE="IDLE"
pic_time=0 pic_time=0
} }
pic_time=pic_time+1 pic_time=pic_time+1
} }
@ -142,7 +142,7 @@ function aggregate() {
centroid = math.vec2.scale(centroid, 1.0 / neighbors.count()) centroid = math.vec2.scale(centroid, 1.0 / neighbors.count())
cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.0, id * 2 * math.pi / ROBOTS)) cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.0, id * 2 * math.pi / ROBOTS))
cmdbin = LimitSpeed(cmdbin, 1.0/2.0) cmdbin = LimitSpeed(cmdbin, 1.0/2.0)
goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0)
} }
# circle all together around centroid # circle all together around centroid
@ -164,7 +164,7 @@ function pursuit() {
dT = gamma * pc dT = gamma * pc
vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
vfg = LimitSpeed(vfg, 2.0) vfg = LimitSpeed(vfg, 2.0)
goto_abs(vfg.x, vfg.y, 0.0, 0.0) goto_abs(vfg.x, vfg.y, 0.0, 0.0)
} }
# Lennard-Jones interaction magnitude # Lennard-Jones interaction magnitude
@ -174,22 +174,18 @@ function lj_magnitude(dist, target, epsilon) {
lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return lj return lj
} }
# Neighbor data to LJ interaction vector # Neighbor data to LJ interaction vector
function lj_vector(rid, data) { function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
} }
# Accumulator of neighbor LJ interactions # Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) { function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum) return math.vec2.add(data, accum)
} }
# Calculates and actuates the flocking interaction # Calculates and actuates the flocking interaction
<<<<<<< HEAD
old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes
=======
>>>>>>> 064760108611591426d86c679c7789b1a95cebee
function formation() { function formation() {
BVMSTATE="FORMATION" BVMSTATE="FORMATION"
# Calculate accumulator # Calculate accumulator
@ -205,7 +201,7 @@ function rc_cmd_listen() {
if(flight.rc_cmd==22) { if(flight.rc_cmd==22) {
log("cmd 22") log("cmd 22")
flight.rc_cmd=0 flight.rc_cmd=0
BVMSTATE = "LAUNCH" BVMSTATE = "LAUNCH"
neighbors.broadcast("cmd", 22) neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) { } else if(flight.rc_cmd==21) {
flight.rc_cmd=0 flight.rc_cmd=0
@ -308,7 +304,7 @@ function nei_cmd_listen() {
# neighbors.listen("gt",function(vid, value, rid) { # neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", rid) # print("Got (", vid, ",", value, ") from robot #", rid)
# # if(gt.id == id) statef=goto # # if(gt.id == id) statef=goto
# }) # })
} }
} }
}) })

30
buzz_scripts/include/taskallocate/graphformGPS.bzz
View File

@ -190,7 +190,7 @@ function pack_guide_msg(send_table){
else{ else{
pon=1 pon=1
} }
var b=math.abs(send_table.Bearing) var b=math.abs(send_table.Bearing)
send_value=r_id*1000+pon*100+b send_value=r_id*1000+pon*100+b
return send_value return send_value
} }
@ -257,10 +257,10 @@ function LJ_vec(i){
var target=target4label(nei_id) var target=target4label(nei_id)
var cDir={.x=0.0,.y=0.0} var cDir={.x=0.0,.y=0.0}
if(target!="miss"){ if(target!="miss"){
cDir=math.vec2.newp(lj_magnitude(dis,target,EPSILON_GRAPH), ljbearing) cDir=math.vec2.newp(lj_magnitude(dis,target,EPSILON_GRAPH), ljbearing)
} }
#log(id,"dis=",dis,"target=",target,"label=",nei_id) #log(id,"dis=",dis,"target=",target,"label=",nei_id)
#log("x=",cDir.x,"y=",cDir.y) #log("x=",cDir.x,"y=",cDir.y)
return cDir return cDir
} }
# #
@ -272,11 +272,11 @@ function motion_vector(){
while(i<m_neighbourCount){ while(i<m_neighbourCount){
#calculate and add the motion vector #calculate and add the motion vector
m_vector=math.vec2.add(m_vector,LJ_vec(i)) m_vector=math.vec2.add(m_vector,LJ_vec(i))
#log(id,"x=",m_vector.x,"y=",m_vector.y) #log(id,"x=",m_vector.x,"y=",m_vector.y)
i=i+1 i=i+1
} }
m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount) m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
#log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y) #log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y)
return m_vector return m_vector
} }
# #
@ -373,7 +373,7 @@ function TransitionToAsking(un_label){
# #
function TransitionToJoining(){ function TransitionToJoining(){
BVMSTATE="GRAPH_JOINING" BVMSTATE="GRAPH_JOINING"
m_selfMessage.State=s2i(BVMSTATE) m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod m_unWaitCount=m_unJoiningLostPeriod
} }
@ -389,7 +389,7 @@ function TransitionToJoined(){
#write statues #write statues
#v_tag.put(m_nLabel, m_lockstig) #v_tag.put(m_nLabel, m_lockstig)
barrier_create() barrier_create()
barrier_ready(940) barrier_ready(900+50)
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
@ -416,7 +416,7 @@ function TransitionToLock(){
m_navigation.y=0.0 m_navigation.y=0.0
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
#stop listening #stop listening
neighbors.ignore("m") neighbors.ignore("m")
} }
# #
@ -464,7 +464,7 @@ function DoFree() {
#set message #set message
m_selfMessage.State=s2i(BVMSTATE) m_selfMessage.State=s2i(BVMSTATE)
BroadcastGraph() BroadcastGraph()
} }
# #
@ -487,14 +487,14 @@ function DoAsking(){
}} }}
if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){ if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){
TransitionToFree() TransitionToFree()
return return
} }
i=i+1 i=i+1
} }
#analyse response #analyse response
if(psResponse==-1){ if(psResponse==-1){
#no response, wait #no response, wait
m_unWaitCount=m_unWaitCount-1 m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(BVMSTATE) m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqLabel=m_nLabel
@ -568,7 +568,7 @@ function set_rc_goto() {
goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x))) goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
print("Saving GPS goal: ",goal.latitude, goal.longitude) print("Saving GPS goal: ",goal.latitude, goal.longitude)
uav_storegoal(goal.latitude, goal.longitude, pose.position.altitude) storegoal(goal.latitude, goal.longitude, pose.position.altitude)
m_gotjoinedparent = 1 m_gotjoinedparent = 1
} }
} }
@ -610,7 +610,7 @@ function DoJoined(){
if(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){ if(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
repeat_assign=0 repeat_assign=0
} }
#if it is the pred #if it is the pred
if((m_MessageState[i]=="GRAPH_JOINED" or m_MessageState[i]=="GRAPH_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){ if((m_MessageState[i]=="GRAPH_JOINED" or m_MessageState[i]=="GRAPH_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
@ -717,7 +717,7 @@ function UpdateGraph() {
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
} }
function BroadcastGraph() { function BroadcastGraph() {
#navigation #navigation
#broadcast message #broadcast message
neighbors.broadcast("m",packmessage(m_selfMessage)) neighbors.broadcast("m",packmessage(m_selfMessage))
@ -793,6 +793,6 @@ function destroyGraph() {
m_navigation.y=0.0 m_navigation.y=0.0
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0) goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
m_vecNodes={} m_vecNodes={}
#stop listening #stop listening
neighbors.ignore("m") neighbors.ignore("m")
} }

4
include/buzz_utility.h
View File

@ -90,6 +90,8 @@ int update_step_test();
int get_robotid(); int get_robotid();
int get_swarmsize();
buzzvm_t get_vm(); buzzvm_t get_vm();
void set_robot_var(int ROBOTS); void set_robot_var(int ROBOTS);
@ -98,7 +100,7 @@ int get_inmsg_size();
std::vector<uint8_t*> get_inmsg_vector(); std::vector<uint8_t*> get_inmsg_vector();
std::string getuavstate(); std::string get_bvmstate();
int get_timesync_state(); int get_timesync_state();

2
include/rosbuzz/mavrosCC.h
View File

@ -1,6 +1,6 @@
#pragma once #pragma once
#ifdef MAVROSKINETIC #if MAVROSKINETIC
const short MISSION_START = mavros_msgs::CommandCode::MISSION_START; const short MISSION_START = mavros_msgs::CommandCode::MISSION_START;
const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;

22
include/roscontroller.h
View File

@ -35,7 +35,6 @@
#include <signal.h> #include <signal.h>
#include <ostream> #include <ostream>
#include <map> #include <map>
#include <cmath>
#include "buzzuav_closures.h" #include "buzzuav_closures.h"
#include "rosbuzz/mavrosCC.h" #include "rosbuzz/mavrosCC.h"
@ -45,13 +44,13 @@
typedef enum { typedef enum {
ROS_BUZZ_MSG_NIL = 0, // dummy msg ROS_BUZZ_MSG_NIL = 0, // dummy msg
UPDATER_MESSAGE, // Update msg UPDATER_MESSAGE, // Update msg
BUZZ_MESSAGE_WTO_TIME, // Broadcast message wihout time info BUZZ_MESSAGE_NO_TIME, // Broadcast message wihout time info
BUZZ_MESSAGE_TIME, // Broadcast message with time info BUZZ_MESSAGE_TIME, // Broadcast message with time info
} rosbuzz_msgtype; } rosbuzz_msgtype;
// Time sync algo. constants // Time sync algo. constants
#define COM_DELAY 100000000 // in nano seconds i.e 100 ms #define COM_DELAY 100000000 // in nano seconds i.e 100 ms
#define TIME_SYNC_JUMP_THR 500000000 #define TIME_SYNC_JUMP_THR 500000000
#define MOVING_AVERAGE_ALPHA 0.1 #define MOVING_AVERAGE_ALPHA 0.1
#define MAX_NUMBER_OF_ROBOTS 10 #define MAX_NUMBER_OF_ROBOTS 10
@ -60,7 +59,7 @@ typedef enum {
using namespace std; using namespace std;
namespace rosbzz_node namespace rosbuzz_node
{ {
class roscontroller class roscontroller
{ {
@ -98,6 +97,7 @@ private:
}; };
typedef struct POSE ros_pose; typedef struct POSE ros_pose;
ros_pose target, home, cur_pos;
struct MsgData struct MsgData
{ {
@ -106,14 +106,12 @@ private:
uint16_t size; uint16_t size;
double sent_time; double sent_time;
uint64_t received_time; uint64_t received_time;
MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt): MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt):
msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){}; msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){};
MsgData(){}; MsgData(){};
}; };
typedef struct MsgData msg_data; typedef struct MsgData msg_data;
ros_pose target, home, cur_pos;
uint64_t payload; uint64_t payload;
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map; std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map; std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
@ -122,8 +120,8 @@ private:
int robot_id = 0; int robot_id = 0;
ros::Time logical_clock; ros::Time logical_clock;
ros::Time previous_step_time; ros::Time previous_step_time;
std::vector<msg_data> inmsgdata; std::vector<msg_data> inmsgdata;
uint64_t out_msg_time; uint64_t out_msg_time;
double logical_time_rate; double logical_time_rate;
bool time_sync_jumped; bool time_sync_jumped;
std::string robot_name = ""; std::string robot_name = "";
@ -133,7 +131,6 @@ private:
int armstate; int armstate;
int barrier; int barrier;
int update; int update;
int statepub_active;
int message_number = 0; int message_number = 0;
uint8_t no_of_robots = 0; uint8_t no_of_robots = 0;
bool rcclient; bool rcclient;
@ -141,6 +138,7 @@ private:
bool multi_msg; bool multi_msg;
uint8_t no_cnt = 0; uint8_t no_cnt = 0;
uint8_t old_val = 0; uint8_t old_val = 0;
bool debug = false;
std::string bzzfile_name; std::string bzzfile_name;
std::string fcclient_name; std::string fcclient_name;
std::string armclient; std::string armclient;
@ -164,7 +162,7 @@ private:
ros::Publisher payload_pub; ros::Publisher payload_pub;
ros::Publisher MPpayload_pub; ros::Publisher MPpayload_pub;
ros::Publisher neigh_pos_pub; ros::Publisher neigh_pos_pub;
ros::Publisher uavstate_pub; ros::Publisher bvmstate_pub;
ros::Publisher grid_pub; ros::Publisher grid_pub;
ros::Publisher localsetpoint_nonraw_pub; ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service; ros::ServiceServer service;
@ -212,7 +210,7 @@ private:
void neighbours_pos_publisher(); void neighbours_pos_publisher();
/*UAVState publisher*/ /*UAVState publisher*/
void uavstate_publisher(); void state_publisher();
/*Grid publisher*/ /*Grid publisher*/
void grid_publisher(); void grid_publisher();

55
src/buzz_utility.cpp
View File

@ -204,25 +204,25 @@ static int buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
@ -258,25 +258,25 @@ static int testing_buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
@ -366,7 +366,7 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_
} }
// Initialize UAVSTATE variable // Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "BMVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM); buzzvm_gstore(VM);
@ -426,7 +426,7 @@ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t
} }
// Initialize UAVSTATE variable // Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM); buzzvm_gstore(VM);
@ -539,14 +539,6 @@ int update_step_test()
return a == BUZZVM_STATE_READY; return a == BUZZVM_STATE_READY;
} }
buzzvm_t get_vm()
/*
/ return the BVM
----------------*/
{
return VM;
}
void set_robot_var(int ROBOTS) void set_robot_var(int ROBOTS)
/* /*
/ set swarm size in the BVM / set swarm size in the BVM
@ -569,7 +561,15 @@ std::vector<uint8_t*> get_inmsg_vector(){
return IN_MSG; return IN_MSG;
} }
string getuavstate() buzzvm_t get_vm()
/*
/ return the BVM
----------------*/
{
return VM;
}
string get_bvmstate()
/* /*
/ return current BVM state / return current BVM state
---------------------------------------*/ ---------------------------------------*/
@ -579,12 +579,19 @@ string getuavstate()
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_gload(VM); buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1); buzzobj_t obj = buzzvm_stack_at(VM, 1);
uav_state = string(obj->s.value.str); if(obj->o.type == BUZZTYPE_STRING)
uav_state = string(obj->s.value.str);
else
uav_state = "Not Available";
buzzvm_pop(VM); buzzvm_pop(VM);
} }
return uav_state; return uav_state;
} }
int get_swarmsize() {
return (int)buzzdict_size(VM->swarmmembers) + 1;
}
int get_timesync_state() int get_timesync_state()
/* /*
/ return time sync state from bzz script / return time sync state from bzz script

16
src/buzzuav_closures.cpp
View File

@ -105,10 +105,10 @@ float constrainAngle(float x)
/ Wrap the angle between -pi, pi / Wrap the angle between -pi, pi
----------------------------------------------------------- */ ----------------------------------------------------------- */
{ {
x = fmod(x, 2 * M_PI); x = fmod(x + M_PI, 2 * M_PI);
if (x < 0.0) if (x < 0.0)
x += 2 * M_PI; x += 2 * M_PI;
return x; return x - M_PI;
} }
void rb_from_gps(double nei[], double out[], double cur[]) void rb_from_gps(double nei[], double out[], double cur[])
@ -229,8 +229,8 @@ int buzzuav_moveto(buzzvm_t vm)
goto_pos[2] = height + dh; goto_pos[2] = height + dh;
goto_pos[3] = dyaw; goto_pos[3] = dyaw;
// DEBUG // DEBUG
ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], // 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]); // goto_pos[1], goto_pos[2]);
buzz_cmd = NAV_SPLINE_WAYPOINT; // TODO: standard mavros? buzz_cmd = NAV_SPLINE_WAYPOINT; // TODO: standard mavros?
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -389,10 +389,10 @@ int buzzuav_storegoal(buzzvm_t vm)
double rb[3]; double rb[3];
rb_from_gps(goal, rb, cur_pos); rb_from_gps(goal, rb, cur_pos);
if (fabs(rb[0]) < 150.0) if (fabs(rb[0]) < 150.0) {
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); }
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -675,7 +675,7 @@ void update_neighbors(buzzvm_t vm)
} }
} }
// Clear neighbours pos // Clear neighbours pos
void clear_neighbours_pos(){ void clear_neighbours_pos(){
neighbors_map.clear(); neighbors_map.clear();
} }

2
src/rosbuzz.cpp
View File

@ -17,7 +17,7 @@ int main(int argc, char** argv)
ros::init(argc, argv, "rosBuzz"); ros::init(argc, argv, "rosBuzz");
ros::NodeHandle nh_priv("~"); ros::NodeHandle nh_priv("~");
ros::NodeHandle nh; ros::NodeHandle nh;
rosbzz_node::roscontroller RosController(nh, nh_priv); rosbuzz_node::roscontroller RosController(nh, nh_priv);
RosController.RosControllerRun(); RosController.RosControllerRun();
return 0; return 0;

88
src/roscontroller.cpp
View File

@ -8,12 +8,11 @@
#include "roscontroller.h" #include "roscontroller.h"
#include <thread> #include <thread>
namespace rosbzz_node namespace rosbuzz_node
{ {
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
const bool debug = true;
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv): roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv):
logical_clock(ros::Time()), previous_step_time(ros::Time()) logical_clock(ros::Time()), previous_step_time(ros::Time())
/* /*
/ roscontroller class Constructor / roscontroller class Constructor
@ -132,23 +131,15 @@ void roscontroller::RosControllerRun()
// set ROS loop rate // set ROS loop rate
ros::Rate loop_rate(BUZZRATE); ros::Rate loop_rate(BUZZRATE);
// check for BVMSTATE variable // check for BVMSTATE variable
buzzvm_t VM = buzz_utility::get_vm(); if(buzz_utility::get_bvmstate()=="Not Available")
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE.");
buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1);
if(obj->o.type == BUZZTYPE_STRING) statepub_active = 1;
else
{
statepub_active = 0;
ROS_ERROR("BVMSTATE undeclared in .bzz file, BVMSTATE pusblisher disabled.");
}
// DEBUG // DEBUG
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
while (ros::ok() && !buzz_utility::buzz_script_done()) while (ros::ok() && !buzz_utility::buzz_script_done())
{ {
// Publish topics // Publish topics
neighbours_pos_publisher(); neighbours_pos_publisher();
if(statepub_active) uavstate_publisher(); state_publisher();
grid_publisher(); grid_publisher();
send_MPpayload(); send_MPpayload();
// Check updater state and step code // Check updater state and step code
@ -184,10 +175,6 @@ void roscontroller::RosControllerRun()
<< cur_pos.altitude * 100000 << ","; << cur_pos.altitude * 100000 << ",";
log << (int)no_of_robots<<","; log << (int)no_of_robots<<",";
log << neighbours_pos_map.size()<< ","; log << neighbours_pos_map.size()<< ",";
log<<(int)inmsgdata.size()<<",";
log<< message_number<<",";
log<< out_msg_time<<",";
log <<buzz_utility::getuavstate();
// if(neighbours_pos_map.size() > 0)log<<","; // if(neighbours_pos_map.size() > 0)log<<",";
map<int, buzz_utility::Pos_struct>::iterator it = map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin(); neighbours_pos_map.begin();
@ -202,7 +189,9 @@ void roscontroller::RosControllerRun()
<<","<<it->received_time; <<","<<it->received_time;
} }
inmsgdata.clear(); inmsgdata.clear();
log<<std::endl; log<<(int)inmsgdata.size()<<","<< message_number<<",";
log<< out_msg_time<<",";
log <<buzz_utility::get_bvmstate()<<std::endl;
// time_sync_step(); // time_sync_step();
// if(debug) // if(debug)
@ -227,7 +216,7 @@ void roscontroller::RosControllerRun()
loop_rate.cycleTime().toSec()); loop_rate.cycleTime().toSec());
// Safety land if the data coming from the flight controller are too old // Safety land if the data coming from the flight controller are too old
if (fcu_timeout <= 0) if (fcu_timeout <= 0)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); buzzuav_closures::rc_call(NAV_LAND);
else else
fcu_timeout -= 1 / BUZZRATE; fcu_timeout -= 1 / BUZZRATE;
timer_step += 1; timer_step += 1;
@ -252,6 +241,14 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
ROS_ERROR("Provide a .bzz file to run in Launch file"); ROS_ERROR("Provide a .bzz file to run in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
// Obtain debug mode from launch file parameter
if (n_c.getParam("debug", debug))
;
else
{
ROS_ERROR("Provide a debug mode in Launch file");
system("rosnode kill rosbuzz_node");
}
// Obtain rc service option from parameter server // Obtain rc service option from parameter server
if (n_c.getParam("rcclient", rcclient)) if (n_c.getParam("rcclient", rcclient))
{ {
@ -335,7 +332,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/LaserScan")); m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/LaserScan"));
node_handle.getParam("topics/battery", topic); node_handle.getParam("topics/battery", topic);
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/BatteryStatus")); m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/BatteryState"));
node_handle.getParam("topics/status", topic); node_handle.getParam("topics/status", topic);
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/State")); m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/State"));
@ -398,7 +395,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5); payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5);
MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5); MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", MAX_NUMBER_OF_ROBOTS); neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", MAX_NUMBER_OF_ROBOTS);
uavstate_pub = n_c.advertise<std_msgs::String>("uavstate", 5); bvmstate_pub = n_c.advertise<std_msgs::String>("bvmstate", 5);
grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5); grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5);
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5); localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
@ -431,7 +428,7 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c)
{ {
flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this);
} }
else if (it->second == "mavros_msgs/BatteryStatus") else if (it->second == "sensor_msgs/BatteryState")
{ {
battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this);
} }
@ -506,14 +503,14 @@ void roscontroller::neighbours_pos_publisher()
neigh_pos_pub.publish(neigh_pos_array); neigh_pos_pub.publish(neigh_pos_array);
} }
void roscontroller::uavstate_publisher() void roscontroller::state_publisher()
/* /*
/ Publish current UAVState from Buzz script / Publish current UAVState from Buzz script
/----------------------------------------------------*/ /----------------------------------------------------*/
{ {
std_msgs::String uavstate_msg; std_msgs::String state_msg;
uavstate_msg.data = buzz_utility::getuavstate(); state_msg.data = buzz_utility::get_bvmstate();
uavstate_pub.publish(uavstate_msg); bvmstate_pub.publish(state_msg);
} }
void roscontroller::grid_publisher() void roscontroller::grid_publisher()
@ -619,7 +616,7 @@ with size......... | /
rheader[0]=0; rheader[0]=0;
payload_out.sysid = (uint8_t)robot_id; payload_out.sysid = (uint8_t)robot_id;
payload_out.msgid = (uint32_t)message_number; payload_out.msgid = (uint32_t)message_number;
if(buzz_utility::get_timesync_state()){ if(buzz_utility::get_timesync_state()){
// prepare rosbuzz msg header // prepare rosbuzz msg header
tmphead[0] = (uint8_t)BUZZ_MESSAGE_TIME; tmphead[0] = (uint8_t)BUZZ_MESSAGE_TIME;
@ -630,7 +627,7 @@ with size......... | /
payload_out.payload64.push_back(position[1]); payload_out.payload64.push_back(position[1]);
payload_out.payload64.push_back(position[2]); payload_out.payload64.push_back(position[2]);
//payload_out.payload64.push_back((uint64_t)message_number); //payload_out.payload64.push_back((uint64_t)message_number);
// add time sync algo data // add time sync algo data
payload_out.payload64.push_back(ros::Time::now().toNSec()); payload_out.payload64.push_back(ros::Time::now().toNSec());
payload_out.payload64.push_back(logical_clock.toNSec()); payload_out.payload64.push_back(logical_clock.toNSec());
//uint64_t ltrate64 = 0; //uint64_t ltrate64 = 0;
@ -639,7 +636,7 @@ with size......... | /
} }
else{ else{
// prepare rosbuzz msg header // prepare rosbuzz msg header
tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME; tmphead[0] = (uint8_t)BUZZ_MESSAGE_NO_TIME;
memcpy(rheader, tmphead, sizeof(uint64_t)); memcpy(rheader, tmphead, sizeof(uint64_t));
// push header into the buffer // push header into the buffer
payload_out.payload64.push_back(rheader[0]); payload_out.payload64.push_back(rheader[0]);
@ -873,10 +870,10 @@ float roscontroller::constrainAngle(float x)
/ Wrap the angle between -pi, pi / Wrap the angle between -pi, pi
----------------------------------------------------------- */ ----------------------------------------------------------- */
{ {
x = fmod(x, 2 * M_PI); x = fmod(x + M_PI, 2 * M_PI);
if (x < 0.0) if (x < 0.0)
x += 2 * M_PI; x += 2 * M_PI;
return x; return x - M_PI;
} }
void roscontroller::gps_rb(POSE nei_pos, double out[]) void roscontroller::gps_rb(POSE nei_pos, double out[])
@ -888,10 +885,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[])
float ned_x = 0.0, ned_y = 0.0; float ned_x = 0.0, ned_y = 0.0;
gps_ned_cur(ned_x, ned_y, nei_pos); gps_ned_cur(ned_x, ned_y, nei_pos);
out[0] = sqrt(ned_x * ned_x + ned_y * ned_y); 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] = 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; out[2] = 0.0;
} }
@ -967,7 +961,7 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
{ {
cur_pos.x = msg->pose.position.x; cur_pos.x = msg->pose.position.x;
cur_pos.y = msg->pose.position.y; cur_pos.y = msg->pose.position.y;
buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x); buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x);
// cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead // cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
tf::Quaternion q( tf::Quaternion q(
@ -1099,7 +1093,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
uint16_t mtype = r16head[0]; uint16_t mtype = r16head[0];
uint16_t mid = r16head[1]; uint16_t mid = r16head[1];
uint32_t temptime=0; uint32_t temptime=0;
memcpy(&temptime, r16head+2, sizeof(uint32_t)); memcpy(&temptime, r16head+2, sizeof(uint32_t));
float stime = (float)temptime/(float)100000; float stime = (float)temptime/(float)100000;
// if(debug) ROS_INFO("Received Msg: sent time %f for id %u",stime, mid); // if(debug) ROS_INFO("Received Msg: sent time %f for id %u",stime, mid);
// Check for Updater message, if updater message push it into updater FIFO // Check for Updater message, if updater message push it into updater FIFO
@ -1128,7 +1122,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
} }
// BVM FIFO message // BVM FIFO message
else if ((int)mtype == (int)BUZZ_MESSAGE_TIME || else if ((int)mtype == (int)BUZZ_MESSAGE_TIME ||
(int)mtype == (int)BUZZ_MESSAGE_WTO_TIME) (int)mtype == (int)BUZZ_MESSAGE_NO_TIME)
{ {
uint64_t message_obt[msg->payload64.size() - 1]; uint64_t message_obt[msg->payload64.size() - 1];
// Go throught the obtained payload // Go throught the obtained payload
@ -1262,7 +1256,7 @@ void roscontroller::get_number_of_robots()
/ Garbage collector for the number of robots in the swarm / Garbage collector for the number of robots in the swarm
--------------------------------------------------------------------------*/ --------------------------------------------------------------------------*/
{ {
int cur_robots = (int)buzzdict_size(buzz_utility::get_vm()->swarmmembers) + 1; int cur_robots = buzz_utility::get_swarmsize();
if (no_of_robots == 0) if (no_of_robots == 0)
{ {
no_of_robots = cur_robots; no_of_robots = cur_robots;
@ -1442,7 +1436,7 @@ void roscontroller::get_xbee_status()
void roscontroller::time_sync_step() void roscontroller::time_sync_step()
/* /*
* Steps the time syncronization algorithm * Steps the time syncronization algorithm
------------------------------------------------------------------ */ ------------------------------------------------------------------ */
{ {
//ROS_INFO("Stepping time sync : %f ", logical_clock.toSec()); //ROS_INFO("Stepping time sync : %f ", logical_clock.toSec());
@ -1454,13 +1448,13 @@ void roscontroller::time_sync_step()
{ {
avgRate += (it->second).relative_rate; avgRate += (it->second).relative_rate;
// estimate current offset // estimate current offset
int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time;
avgOffset = avgOffset + offset; avgOffset = avgOffset + offset;
offsetCount++; offsetCount++;
if((it->second).age > BUZZRATE){ if((it->second).age > BUZZRATE){
neighbours_time_map.erase(it++); neighbours_time_map.erase(it++);
} }
else{ else{
(it->second).age++; (it->second).age++;
++it; ++it;
} }
@ -1478,10 +1472,10 @@ void roscontroller::time_sync_step()
//neighbours_time_map.clear(); //neighbours_time_map.clear();
logical_time_rate = avgRate; logical_time_rate = avgRate;
} }
void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr) void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr)
/* /*
* pushes a time syncronization msg into its data slot * pushes a time syncronization msg into its data slot
------------------------------------------------------------------ */ ------------------------------------------------------------------ */
{ {
map<int, buzz_utility::neighbor_time>::iterator it = neighbours_time_map.find(nid); map<int, buzz_utility::neighbor_time>::iterator it = neighbours_time_map.find(nid);
@ -1491,10 +1485,10 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou
int64_t delatNei = round(nh - (it->second).nei_hardware_time); int64_t delatNei = round(nh - (it->second).nei_hardware_time);
double currentRate = 0; double currentRate = 0;
if(deltaLocal !=0 && delatNei !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; if(deltaLocal !=0 && delatNei !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal;
relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate)
+ (1- MOVING_AVERAGE_ALPHA)*currentRate; + (1- MOVING_AVERAGE_ALPHA)*currentRate;
ROS_INFO("SYNC MSG RECEIVED deltaLocal %" PRIu64 ", delatNei %" PRId64 " , currentrate %f , this relative rate %f, final relativeRate %f", ROS_INFO("SYNC MSG RECEIVED deltaLocal %" PRIu64 ", delatNei %" PRId64 " , currentrate %f , this relative rate %f, final relativeRate %f",
deltaLocal, delatNei, currentRate, (it->second).relative_rate, relativeRate); deltaLocal, delatNei, currentRate, (it->second).relative_rate, relativeRate);
neighbours_time_map.erase(it); neighbours_time_map.erase(it);
} }