integrated enhancements from dev_kinetic
This commit is contained in:
dave
parent
52357b142d
commit
8580f8f306
|
|
@ -5,16 +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")
|
||||
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()
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=${SIM} -DMAVROSKINETIC=${KIN}")
|
||||
|
||||
## Find catkin macros and libraries
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
|
|
|
|||
|
|
@ -97,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
|
||||
}
|
||||
|
|
@ -142,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
|
||||
|
|
@ -164,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
|
||||
|
|
@ -174,22 +174,18 @@ 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
|
||||
<<<<<<< HEAD
|
||||
old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes
|
||||
=======
|
||||
>>>>>>> 064760108611591426d86c679c7789b1a95cebee
|
||||
function formation() {
|
||||
BVMSTATE="FORMATION"
|
||||
# Calculate accumulator
|
||||
|
|
@ -205,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
|
||||
|
|
@ -308,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
|
||||
# })
|
||||
# })
|
||||
}
|
||||
}
|
||||
})
|
||||
|
|
|
|||
|
|
@ -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){
|
||||
|
|
@ -717,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))
|
||||
|
|
@ -793,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")
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#pragma once
|
||||
|
||||
#ifdef MAVROSKINETIC
|
||||
#if MAVROSKINETIC
|
||||
|
||||
const short MISSION_START = mavros_msgs::CommandCode::MISSION_START;
|
||||
const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
|
||||
|
|
|
|||
|
|
@ -35,7 +35,6 @@
|
|||
#include <signal.h>
|
||||
#include <ostream>
|
||||
#include <map>
|
||||
#include <cmath>
|
||||
#include "buzzuav_closures.h"
|
||||
#include "rosbuzz/mavrosCC.h"
|
||||
|
||||
|
|
@ -45,13 +44,13 @@
|
|||
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
|
||||
|
||||
|
|
@ -60,7 +59,7 @@ typedef enum {
|
|||
|
||||
using namespace std;
|
||||
|
||||
namespace rosbzz_node
|
||||
namespace rosbuzz_node
|
||||
{
|
||||
class roscontroller
|
||||
{
|
||||
|
|
@ -98,6 +97,7 @@ private:
|
|||
};
|
||||
typedef struct POSE ros_pose;
|
||||
|
||||
ros_pose target, home, cur_pos;
|
||||
|
||||
struct MsgData
|
||||
{
|
||||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,8 +229,8 @@ 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]);
|
||||
// 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);
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
@ -675,7 +675,7 @@ void update_neighbors(buzzvm_t vm)
|
|||
}
|
||||
}
|
||||
|
||||
// Clear neighbours pos
|
||||
// Clear neighbours pos
|
||||
void clear_neighbours_pos(){
|
||||
neighbors_map.clear();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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]);
|
||||
|
|
@ -873,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[])
|
||||
|
|
@ -888,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;
|
||||
}
|
||||
|
||||
|
|
@ -967,7 +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(
|
||||
|
|
@ -1099,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
|
||||
|
|
@ -1128,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
|
||||
|
|
@ -1262,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;
|
||||
|
|
@ -1442,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());
|
||||
|
|
@ -1454,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;
|
||||
}
|
||||
|
|
@ -1478,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);
|
||||
|
|
@ -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);
|
||||
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);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue