force push from ros_drones_ws
This commit is contained in:
parent
0647601086
commit
8cdd5eb105
|
@ -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)
|
||||
|
|
|
@ -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
|
||||
# })
|
||||
# })
|
||||
}
|
||||
}
|
||||
})
|
||||
|
|
|
@ -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")
|
||||
}
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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()
|
||||
}
|
||||
|
|
|
@ -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();
|
||||
|
||||
|
|
|
@ -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
|
||||
*/
|
||||
|
|
|
@ -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;
|
|
@ -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();
|
||||
|
|
|
@ -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,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();
|
||||
}
|
||||
|
|
|
@ -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]);
|
||||
|
@ -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);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue