force push from ros_drones_ws

This commit is contained in:
dave 2018-09-07 00:17:30 -04:00
parent 0647601086
commit 8cdd5eb105
13 changed files with 200 additions and 230 deletions

View File

@ -5,11 +5,8 @@ if(UNIX)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
endif()
if(SIM)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1 -DMAVROSKINETIC=1")
else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0 -DMAVROSKINETIC=1")
endif()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=${SIM} -DMAVROSKINETIC=${KIN}")
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
@ -70,6 +67,11 @@ install(TARGETS rosbuzz_node
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Install project namespaced headers
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE)
find_package(catkin REQUIRED COMPONENTS roslaunch)
roslaunch_add_file_check(launch)

View File

@ -4,12 +4,14 @@
#
########################################
include "utils/vec2.bzz"
include "act/old_barrier.bzz"
include "act/barrier.bzz"
#include "act/old_barrier.bzz"
include "utils/conversions.bzz"
TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 0.5 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 1.0 # m.
@ -27,6 +29,7 @@ function idle() {
BVMSTATE = "IDLE"
}
# Custom function for the user.
function cusfun(){
BVMSTATE="CUSFUN"
@ -94,7 +97,7 @@ function take_picture() {
takepicture()
} else if(pic_time>=PICTURE_WAIT) { # wait for the picture
BVMSTATE="IDLE"
pic_time=0
pic_time=0
}
pic_time=pic_time+1
}
@ -108,9 +111,7 @@ function goto_gps(transf) {
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
transf()
else {
log("Waypoint before scale : x ",m_navigation.x, " Y ",m_navigation.y )
m_navigation = LimitSpeed(m_navigation, 1.0)
log("Waypoint after scale : x ",m_navigation.x, " Y ",m_navigation.y )
m_navigation = LimitSpeed(m_navigation, 1.0)
goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
}
}
@ -141,7 +142,7 @@ function aggregate() {
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 = 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
@ -163,7 +164,7 @@ function pursuit() {
dT = gamma * pc
vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
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
@ -173,17 +174,17 @@ function lj_magnitude(dist, target, epsilon) {
lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return lj
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Calculates and actuates the flocking interaction
function formation() {
BVMSTATE="FORMATION"
@ -200,7 +201,7 @@ function rc_cmd_listen() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
BVMSTATE = "LAUNCH"
BVMSTATE = "LAUNCH"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
flight.rc_cmd=0
@ -303,7 +304,7 @@ function nei_cmd_listen() {
# neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", rid)
# # if(gt.id == id) statef=goto
# })
# })
}
}
})

View File

@ -190,7 +190,7 @@ function pack_guide_msg(send_table){
else{
pon=1
}
var b=math.abs(send_table.Bearing)
var b=math.abs(send_table.Bearing)
send_value=r_id*1000+pon*100+b
return send_value
}
@ -257,10 +257,10 @@ function LJ_vec(i){
var target=target4label(nei_id)
var cDir={.x=0.0,.y=0.0}
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("x=",cDir.x,"y=",cDir.y)
#log("x=",cDir.x,"y=",cDir.y)
return cDir
}
#
@ -272,11 +272,11 @@ function motion_vector(){
while(i<m_neighbourCount){
#calculate and add the motion vector
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
}
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
}
#
@ -373,7 +373,7 @@ function TransitionToAsking(un_label){
#
function TransitionToJoining(){
BVMSTATE="GRAPH_JOINING"
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
}
@ -389,7 +389,7 @@ function TransitionToJoined(){
#write statues
#v_tag.put(m_nLabel, m_lockstig)
barrier_create()
barrier_ready(940)
barrier_ready(900+50)
m_navigation.x=0.0
m_navigation.y=0.0
@ -416,7 +416,7 @@ function TransitionToLock(){
m_navigation.y=0.0
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
#stop listening
#stop listening
neighbors.ignore("m")
}
#
@ -464,7 +464,7 @@ function DoFree() {
#set message
m_selfMessage.State=s2i(BVMSTATE)
BroadcastGraph()
}
#
@ -487,14 +487,14 @@ function DoAsking(){
}}
if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){
TransitionToFree()
return
return
}
i=i+1
}
#analyse response
if(psResponse==-1){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(BVMSTATE)
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)))
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
}
}
@ -610,7 +610,7 @@ function DoJoined(){
if(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
repeat_assign=0
}
#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){
@ -677,6 +677,7 @@ function DoLock() {
m_navigation=motion_vector()
}
# #move
# goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
BroadcastGraph()
@ -716,7 +717,7 @@ function UpdateGraph() {
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
}
function BroadcastGraph() {
function BroadcastGraph() {
#navigation
#broadcast message
neighbors.broadcast("m",packmessage(m_selfMessage))
@ -792,6 +793,6 @@ function destroyGraph() {
m_navigation.y=0.0
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
m_vecNodes={}
#stop listening
#stop listening
neighbors.ignore("m")
}

View File

@ -44,9 +44,9 @@ function vec_from_gps(lat, lon, home_ref) {
}
ned_x = d_lat/180*math.pi * 6371000.0;
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
m_Lgoal_range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
m_Lgoal_bearing = LimitAngle(math.atan(ned_y,ned_x));
return math.vec2.newp(m_Lgoal_range,m_Lgoal_bearing)
#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
return math.vec2.new(ned_x,ned_y)
}
function gps_from_vec(vec) {

View File

@ -27,14 +27,15 @@ goal_list = {
.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
}
# Executed once at init time.
function init() {
init_stig()
init_swarm()
initGraph()
# initGraph()
TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m
loop = 1
# start the swarm command listener
@ -109,10 +110,8 @@ function step() {
# Executed once when the robot (or the simulator) is reset.
function reset() {
resetGraph()
}
# Executed once at the end of experiment.
function destroy() {
destroyGraph()
}

View File

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

View File

@ -6,6 +6,7 @@
#include "mavros_msgs/Mavlink.h"
#include "ros/ros.h"
#include "buzz_utility.h"
#include "rosbuzz/mavrosCC.h"
#define EARTH_RADIUS (double)6371000.0
#define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0)))
@ -13,19 +14,6 @@
namespace buzzuav_closures
{
typedef enum {
COMMAND_NIL = 0, // Dummy command
COMMAND_TAKEOFF, // Take off
COMMAND_LAND,
COMMAND_GOHOME,
COMMAND_ARM,
COMMAND_DISARM,
COMMAND_GOTO,
COMMAND_MOVETO,
COMMAND_PICTURE,
COMMAND_GIMBAL,
} Custom_CommandCode;
/*
* prextern int() function in Buzz
* This function is used to print data from buzz
@ -89,6 +77,7 @@ void set_filtered_packet_loss(float value);
/*
* sets current position
*/
void set_currentNEDpos(double x, double y);
void set_currentpos(double latitude, double longitude, float altitude, float yaw);
@ -101,6 +90,7 @@ double* getgoto();
*/
std::map<int, std::map<int,int>> getgrid();
/*
* returns the gimbal commands
*/

View File

@ -0,0 +1,24 @@
#pragma once
#if MAVROSKINETIC
const short MISSION_START = mavros_msgs::CommandCode::MISSION_START;
const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
const short COMPONENT_ARM_DISARM = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
#else
const short MISSION_START = mavros_msgs::CommandCode::CMD_MISSION_START;
const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
const short COMPONENT_ARM_DISARM = CMD_COMPONENT_ARM_DISARM;
#endif
const short NAV_TAKEOFF = mavros_msgs::CommandCode::NAV_TAKEOFF;
const short NAV_LAND = mavros_msgs::CommandCode::NAV_LAND;
const short NAV_RETURN_TO_LAUNCH = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
const short NAV_SPLINE_WAYPOINT = mavros_msgs::CommandCode::NAV_SPLINE_WAYPOINT;
const short NAV_WAYPOINT = mavros_msgs::CommandCode::NAV_WAYPOINT;
const short IMAGE_START_CAPTURE = mavros_msgs::CommandCode::IMAGE_START_CAPTURE;
const short CMD_REQUEST_UPDATE = 666;
const short CMD_SYNC_CLOCK = 777;

View File

@ -35,8 +35,8 @@
#include <signal.h>
#include <ostream>
#include <map>
#include <cmath>
#include "buzzuav_closures.h"
#include "rosbuzz/mavrosCC.h"
/*
* ROSBuzz message types
@ -44,24 +44,22 @@
typedef enum {
ROS_BUZZ_MSG_NIL = 0, // dummy 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
} rosbuzz_msgtype;
// Time sync algo. constants
#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 MAX_NUMBER_OF_ROBOTS 10
#define TIMEOUT 60
#define BUZZRATE 10
#define CMD_REQUEST_UPDATE 666
#define CMD_SYNC_CLOCK 777
using namespace std;
namespace rosbzz_node
namespace rosbuzz_node
{
class roscontroller
{
@ -99,6 +97,8 @@ private:
};
typedef struct POSE ros_pose;
ros_pose target, home, cur_pos;
struct MsgData
{
int msgid;
@ -106,14 +106,12 @@ private:
uint16_t size;
double sent_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){};
MsgData(){};
};
typedef struct MsgData msg_data;
ros_pose target, home, cur_pos;
uint64_t payload;
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
@ -122,8 +120,8 @@ private:
int robot_id = 0;
ros::Time logical_clock;
ros::Time previous_step_time;
std::vector<msg_data> inmsgdata;
uint64_t out_msg_time;
std::vector<msg_data> inmsgdata;
uint64_t out_msg_time;
double logical_time_rate;
bool time_sync_jumped;
std::string robot_name = "";
@ -133,7 +131,6 @@ private:
int armstate;
int barrier;
int update;
int statepub_active;
int message_number = 0;
uint8_t no_of_robots = 0;
bool rcclient;
@ -141,6 +138,7 @@ private:
bool multi_msg;
uint8_t no_cnt = 0;
uint8_t old_val = 0;
bool debug = false;
std::string bzzfile_name;
std::string fcclient_name;
std::string armclient;
@ -164,7 +162,7 @@ private:
ros::Publisher payload_pub;
ros::Publisher MPpayload_pub;
ros::Publisher neigh_pos_pub;
ros::Publisher uavstate_pub;
ros::Publisher bvmstate_pub;
ros::Publisher grid_pub;
ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service;
@ -212,7 +210,7 @@ private:
void neighbours_pos_publisher();
/*UAVState publisher*/
void uavstate_publisher();
void state_publisher();
/*Grid publisher*/
void grid_publisher();
@ -302,6 +300,7 @@ private:
bool GetRawPacketLoss(const uint8_t short_id, float& result);
bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
void get_xbee_status();
void time_sync_step();
void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr);
uint64_t get_logical_time();

View File

@ -204,25 +204,25 @@ static int buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
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_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_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_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_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_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
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_gstore(VM);
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_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
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_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_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_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_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_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
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_gstore(VM);
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
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_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
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_gstore(VM);
@ -539,14 +539,6 @@ int update_step_test()
return a == BUZZVM_STATE_READY;
}
buzzvm_t get_vm()
/*
/ return the BVM
----------------*/
{
return VM;
}
void set_robot_var(int ROBOTS)
/*
/ set swarm size in the BVM
@ -569,7 +561,15 @@ std::vector<uint8_t*> get_inmsg_vector(){
return IN_MSG;
}
string getuavstate()
buzzvm_t get_vm()
/*
/ return the BVM
----------------*/
{
return VM;
}
string get_bvmstate()
/*
/ return current BVM state
---------------------------------------*/
@ -579,12 +579,19 @@ string getuavstate()
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_gload(VM);
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);
}
return uav_state;
}
int get_swarmsize() {
return (int)buzzdict_size(VM->swarmmembers) + 1;
}
int get_timesync_state()
/*
/ return time sync state from bzz script

View File

@ -105,10 +105,10 @@ float constrainAngle(float x)
/ Wrap the angle between -pi, pi
----------------------------------------------------------- */
{
x = fmod(x, 2 * M_PI);
x = fmod(x + M_PI, 2 * M_PI);
if (x < 0.0)
x += 2 * M_PI;
return x;
return x - M_PI;
}
void rb_from_gps(double nei[], double out[], double cur[])
@ -229,9 +229,9 @@ int buzzuav_moveto(buzzvm_t vm)
goto_pos[2] = height + dh;
goto_pos[3] = dyaw;
// DEBUG
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; // TODO: standard mavros?
// 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 = NAV_SPLINE_WAYPOINT; // TODO: standard mavros?
return buzzvm_ret0(vm);
}
@ -332,7 +332,7 @@ int buzzuav_takepicture(buzzvm_t vm)
/ Buzz closure to take a picture here.
/----------------------------------------*/
{
buzz_cmd = COMMAND_PICTURE;
buzz_cmd = IMAGE_START_CAPTURE;
return buzzvm_ret0(vm);
}
@ -356,7 +356,7 @@ int buzzuav_setgimbal(buzzvm_t vm)
rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value;
ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]);
buzz_cmd = COMMAND_GIMBAL;
buzz_cmd = DO_MOUNT_CONTROL;
return buzzvm_ret0(vm);
}
@ -389,10 +389,10 @@ int buzzuav_storegoal(buzzvm_t vm)
double rb[3];
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]);
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);
}
@ -401,13 +401,9 @@ int buzzuav_arm(buzzvm_t vm)
/ Buzz closure to arm
/---------------------------------------*/
{
#ifdef MAVROSKINETIC
cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
#else
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
#endif
cur_cmd = COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n");
buzz_cmd = COMMAND_ARM;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -416,13 +412,9 @@ int buzzuav_disarm(buzzvm_t vm)
/ Buzz closure to disarm
/---------------------------------------*/
{
#ifdef MAVROSKINETIC
cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1;
#else
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
#endif
cur_cmd = COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n");
buzz_cmd = COMMAND_DISARM;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -436,9 +428,9 @@ int buzzuav_takeoff(buzzvm_t vm)
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
goto_pos[2] = buzzvm_stack_at(vm, 1)->f.value;
height = goto_pos[2];
cur_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
cur_cmd = NAV_TAKEOFF;
printf(" Buzz requested Take off !!! \n");
buzz_cmd = COMMAND_TAKEOFF;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -447,9 +439,9 @@ int buzzuav_land(buzzvm_t vm)
/ Buzz closure to land
/-------------------------------------------------------------*/
{
cur_cmd = mavros_msgs::CommandCode::NAV_LAND;
cur_cmd = NAV_LAND;
printf(" Buzz requested Land !!! \n");
buzz_cmd = COMMAND_LAND;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -458,9 +450,9 @@ int buzzuav_gohome(buzzvm_t vm)
/ Buzz closure to return Home
/-------------------------------------------------------------*/
{
cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
cur_cmd = NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n");
buzz_cmd = COMMAND_GOHOME;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -683,7 +675,7 @@ void update_neighbors(buzzvm_t vm)
}
}
// Clear neighbours pos
// Clear neighbours pos
void clear_neighbours_pos(){
neighbors_map.clear();
}

View File

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

View File

@ -8,12 +8,11 @@
#include "roscontroller.h"
#include <thread>
namespace rosbzz_node
namespace rosbuzz_node
{
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())
/*
/ roscontroller class Constructor
@ -132,23 +131,15 @@ void roscontroller::RosControllerRun()
// set ROS loop rate
ros::Rate loop_rate(BUZZRATE);
// check for BVMSTATE variable
buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
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.");
}
if(buzz_utility::get_bvmstate()=="Not Available")
ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE.");
// DEBUG
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
while (ros::ok() && !buzz_utility::buzz_script_done())
{
// Publish topics
neighbours_pos_publisher();
if(statepub_active) uavstate_publisher();
state_publisher();
grid_publisher();
send_MPpayload();
// Check updater state and step code
@ -184,10 +175,6 @@ void roscontroller::RosControllerRun()
<< cur_pos.altitude * 100000 << ",";
log << (int)no_of_robots<<",";
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<<",";
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
@ -202,7 +189,9 @@ void roscontroller::RosControllerRun()
<<","<<it->received_time;
}
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();
// if(debug)
@ -227,7 +216,7 @@ void roscontroller::RosControllerRun()
loop_rate.cycleTime().toSec());
// Safety land if the data coming from the flight controller are too old
if (fcu_timeout <= 0)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
buzzuav_closures::rc_call(NAV_LAND);
else
fcu_timeout -= 1 / BUZZRATE;
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");
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
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"));
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);
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);
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);
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);
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);
}
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);
}
@ -506,14 +503,14 @@ void roscontroller::neighbours_pos_publisher()
neigh_pos_pub.publish(neigh_pos_array);
}
void roscontroller::uavstate_publisher()
void roscontroller::state_publisher()
/*
/ Publish current UAVState from Buzz script
/----------------------------------------------------*/
{
std_msgs::String uavstate_msg;
uavstate_msg.data = buzz_utility::getuavstate();
uavstate_pub.publish(uavstate_msg);
std_msgs::String state_msg;
state_msg.data = buzz_utility::get_bvmstate();
bvmstate_pub.publish(state_msg);
}
void roscontroller::grid_publisher()
@ -619,7 +616,7 @@ with size......... | /
rheader[0]=0;
payload_out.sysid = (uint8_t)robot_id;
payload_out.msgid = (uint32_t)message_number;
if(buzz_utility::get_timesync_state()){
// prepare rosbuzz msg header
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[2]);
//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(logical_clock.toNSec());
//uint64_t ltrate64 = 0;
@ -639,7 +636,7 @@ with size......... | /
}
else{
// 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));
// push header into the buffer
payload_out.payload64.push_back(rheader[0]);
@ -714,7 +711,7 @@ script
float* gimbal;
switch (buzzuav_closures::bzz_cmd())
{
case buzzuav_closures::COMMAND_TAKEOFF:
case NAV_TAKEOFF:
goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
@ -737,7 +734,7 @@ script
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_LAND:
case NAV_LAND:
cmd_srv.request.command = buzzuav_closures::getcmd();
if (current_mode != "LAND")
{
@ -756,7 +753,7 @@ script
armstate = 0;
break;
case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
case NAV_RETURN_TO_LAUNCH: // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
cmd_srv.request.param5 = home.latitude;
cmd_srv.request.param6 = home.longitude;
cmd_srv.request.param7 = home.altitude;
@ -769,32 +766,7 @@ script
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!!
goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv))
{
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
}
else
ROS_ERROR("Failed to call service from flight controller");
#ifdef MAVROSKINETIC
cmd_srv.request.command = mavros_msgs::CommandCode::MISSION_START;
#else
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
#endif
if (mav_client.call(cmd_srv))
{
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
}
else
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_ARM:
case COMPONENT_ARM_DISARM:
if (!armstate)
{
SetMode("LOITER", 0);
@ -803,7 +775,7 @@ script
}
break;
case buzzuav_closures::COMMAND_DISARM:
case COMPONENT_ARM_DISARM+1:
if (armstate)
{
armstate = 0;
@ -812,22 +784,18 @@ script
}
break;
case buzzuav_closures::COMMAND_MOVETO:
case NAV_SPLINE_WAYPOINT:
goto_pos = buzzuav_closures::getgoto();
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
break;
case buzzuav_closures::COMMAND_GIMBAL:
case DO_MOUNT_CONTROL:
gimbal = buzzuav_closures::getgimbal();
cmd_srv.request.param1 = gimbal[0];
cmd_srv.request.param2 = gimbal[1];
cmd_srv.request.param3 = gimbal[2];
cmd_srv.request.param4 = gimbal[3];
#ifdef MAVROSKINETIC
cmd_srv.request.command = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
#else
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
#endif
cmd_srv.request.command = DO_MOUNT_CONTROL;
if (mav_client.call(cmd_srv))
{
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
@ -836,7 +804,7 @@ script
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_PICTURE:
case IMAGE_START_CAPTURE:
ROS_INFO("TAKING A PICTURE HERE!! --------------");
mavros_msgs::CommandBool capture_command;
if (capture_srv.call(capture_command))
@ -902,10 +870,10 @@ float roscontroller::constrainAngle(float x)
/ Wrap the angle between -pi, pi
----------------------------------------------------------- */
{
x = fmod(x, 2 * M_PI);
x = fmod(x + M_PI, 2 * M_PI);
if (x < 0.0)
x += 2 * M_PI;
return x;
return x - M_PI;
}
void roscontroller::gps_rb(POSE nei_pos, double out[])
@ -917,10 +885,7 @@ void roscontroller::gps_rb(POSE 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;
}
@ -996,6 +961,7 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
{
cur_pos.x = msg->pose.position.x;
cur_pos.y = msg->pose.position.y;
buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x);
// cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
tf::Quaternion q(
@ -1127,7 +1093,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
uint16_t mtype = r16head[0];
uint16_t mid = r16head[1];
uint32_t temptime=0;
memcpy(&temptime, r16head+2, sizeof(uint32_t));
memcpy(&temptime, r16head+2, sizeof(uint32_t));
float stime = (float)temptime/(float)100000;
// 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
@ -1156,7 +1122,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
}
// BVM FIFO message
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];
// Go throught the obtained payload
@ -1217,25 +1183,20 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
int rc_cmd;
switch (req.command)
{
case mavros_msgs::CommandCode::NAV_TAKEOFF:
case NAV_TAKEOFF:
ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
rc_cmd = NAV_TAKEOFF;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_LAND:
case NAV_LAND:
ROS_INFO("RC_Call: LAND!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_LAND;
rc_cmd = NAV_LAND;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
#ifdef MAVROSKINETIC
case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM:
rc_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
#else
case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
#endif
case COMPONENT_ARM_DISARM:
rc_cmd = COMPONENT_ARM_DISARM;
armstate = req.param1;
if (armstate)
{
@ -1250,31 +1211,23 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
res.success = true;
}
break;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
case NAV_RETURN_TO_LAUNCH:
ROS_INFO("RC_Call: GO HOME!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
rc_cmd = NAV_RETURN_TO_LAUNCH;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
case NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! ");
buzzuav_closures::rc_set_goto(req.param1, req.param5, req.param6, req.param7);
rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
rc_cmd = NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
#ifdef MAVROSKINETIC
case mavros_msgs::CommandCode::DO_MOUNT_CONTROL:
#else
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
#endif
case DO_MOUNT_CONTROL:
ROS_INFO("RC_Call: Gimbal!!!! ");
buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5);
#ifdef MAVROSKINETIC
rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
#else
rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
#endif
rc_cmd = DO_MOUNT_CONTROL;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
@ -1303,7 +1256,7 @@ void roscontroller::get_number_of_robots()
/ 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)
{
no_of_robots = cur_robots;
@ -1483,7 +1436,7 @@ void roscontroller::get_xbee_status()
void roscontroller::time_sync_step()
/*
* Steps the time syncronization algorithm
* Steps the time syncronization algorithm
------------------------------------------------------------------ */
{
//ROS_INFO("Stepping time sync : %f ", logical_clock.toSec());
@ -1495,13 +1448,13 @@ void roscontroller::time_sync_step()
{
avgRate += (it->second).relative_rate;
// 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;
offsetCount++;
if((it->second).age > BUZZRATE){
neighbours_time_map.erase(it++);
}
else{
else{
(it->second).age++;
++it;
}
@ -1519,10 +1472,10 @@ void roscontroller::time_sync_step()
//neighbours_time_map.clear();
logical_time_rate = avgRate;
}
}
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);
@ -1532,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);
double currentRate = 0;
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;
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);
neighbours_time_map.erase(it);
}