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

integrated enhancements from dev_kinetic

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

12
CMakeLists.txt
View File

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

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

@ -186,10 +186,6 @@ function lj_sum(rid, data, accum) {
} }
# Calculates and actuates the flocking interaction # Calculates and actuates the flocking interaction
<<<<<<< HEAD
old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes
=======
>>>>>>> 064760108611591426d86c679c7789b1a95cebee
function formation() { function formation() {
BVMSTATE="FORMATION" BVMSTATE="FORMATION"
# Calculate accumulator # Calculate accumulator

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

@ -389,7 +389,7 @@ function TransitionToJoined(){
#write statues #write statues
#v_tag.put(m_nLabel, m_lockstig) #v_tag.put(m_nLabel, m_lockstig)
barrier_create() barrier_create()
barrier_ready(940) barrier_ready(900+50)
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
@ -568,7 +568,7 @@ function set_rc_goto() {
goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x))) goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
print("Saving GPS goal: ",goal.latitude, goal.longitude) print("Saving GPS goal: ",goal.latitude, goal.longitude)
uav_storegoal(goal.latitude, goal.longitude, pose.position.altitude) storegoal(goal.latitude, goal.longitude, pose.position.altitude)
m_gotjoinedparent = 1 m_gotjoinedparent = 1
} }
} }

4
include/buzz_utility.h
View File

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

2
include/rosbuzz/mavrosCC.h
View File

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

14
include/roscontroller.h
View File

@ -35,7 +35,6 @@
#include <signal.h> #include <signal.h>
#include <ostream> #include <ostream>
#include <map> #include <map>
#include <cmath>
#include "buzzuav_closures.h" #include "buzzuav_closures.h"
#include "rosbuzz/mavrosCC.h" #include "rosbuzz/mavrosCC.h"
@ -45,7 +44,7 @@
typedef enum { typedef enum {
ROS_BUZZ_MSG_NIL = 0, // dummy msg ROS_BUZZ_MSG_NIL = 0, // dummy msg
UPDATER_MESSAGE, // Update msg UPDATER_MESSAGE, // Update msg
BUZZ_MESSAGE_WTO_TIME, // Broadcast message wihout time info BUZZ_MESSAGE_NO_TIME, // Broadcast message wihout time info
BUZZ_MESSAGE_TIME, // Broadcast message with time info BUZZ_MESSAGE_TIME, // Broadcast message with time info
} rosbuzz_msgtype; } rosbuzz_msgtype;
@ -60,7 +59,7 @@ typedef enum {
using namespace std; using namespace std;
namespace rosbzz_node namespace rosbuzz_node
{ {
class roscontroller class roscontroller
{ {
@ -98,6 +97,7 @@ private:
}; };
typedef struct POSE ros_pose; typedef struct POSE ros_pose;
ros_pose target, home, cur_pos;
struct MsgData struct MsgData
{ {
@ -112,8 +112,6 @@ private:
}; };
typedef struct MsgData msg_data; typedef struct MsgData msg_data;
ros_pose target, home, cur_pos;
uint64_t payload; uint64_t payload;
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map; std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map; std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
@ -133,7 +131,6 @@ private:
int armstate; int armstate;
int barrier; int barrier;
int update; int update;
int statepub_active;
int message_number = 0; int message_number = 0;
uint8_t no_of_robots = 0; uint8_t no_of_robots = 0;
bool rcclient; bool rcclient;
@ -141,6 +138,7 @@ private:
bool multi_msg; bool multi_msg;
uint8_t no_cnt = 0; uint8_t no_cnt = 0;
uint8_t old_val = 0; uint8_t old_val = 0;
bool debug = false;
std::string bzzfile_name; std::string bzzfile_name;
std::string fcclient_name; std::string fcclient_name;
std::string armclient; std::string armclient;
@ -164,7 +162,7 @@ private:
ros::Publisher payload_pub; ros::Publisher payload_pub;
ros::Publisher MPpayload_pub; ros::Publisher MPpayload_pub;
ros::Publisher neigh_pos_pub; ros::Publisher neigh_pos_pub;
ros::Publisher uavstate_pub; ros::Publisher bvmstate_pub;
ros::Publisher grid_pub; ros::Publisher grid_pub;
ros::Publisher localsetpoint_nonraw_pub; ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service; ros::ServiceServer service;
@ -212,7 +210,7 @@ private:
void neighbours_pos_publisher(); void neighbours_pos_publisher();
/*UAVState publisher*/ /*UAVState publisher*/
void uavstate_publisher(); void state_publisher();
/*Grid publisher*/ /*Grid publisher*/
void grid_publisher(); void grid_publisher();

53
src/buzz_utility.cpp
View File

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

12
src/buzzuav_closures.cpp
View File

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

2
src/rosbuzz.cpp
View File

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

62
src/roscontroller.cpp
View File

@ -8,10 +8,9 @@
#include "roscontroller.h" #include "roscontroller.h"
#include <thread> #include <thread>
namespace rosbzz_node namespace rosbuzz_node
{ {
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
const bool debug = true;
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv): roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv):
logical_clock(ros::Time()), previous_step_time(ros::Time()) logical_clock(ros::Time()), previous_step_time(ros::Time())
@ -132,23 +131,15 @@ void roscontroller::RosControllerRun()
// set ROS loop rate // set ROS loop rate
ros::Rate loop_rate(BUZZRATE); ros::Rate loop_rate(BUZZRATE);
// check for BVMSTATE variable // check for BVMSTATE variable
buzzvm_t VM = buzz_utility::get_vm(); if(buzz_utility::get_bvmstate()=="Not Available")
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE.");
buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1);
if(obj->o.type == BUZZTYPE_STRING) statepub_active = 1;
else
{
statepub_active = 0;
ROS_ERROR("BVMSTATE undeclared in .bzz file, BVMSTATE pusblisher disabled.");
}
// DEBUG // DEBUG
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
while (ros::ok() && !buzz_utility::buzz_script_done()) while (ros::ok() && !buzz_utility::buzz_script_done())
{ {
// Publish topics // Publish topics
neighbours_pos_publisher(); neighbours_pos_publisher();
if(statepub_active) uavstate_publisher(); state_publisher();
grid_publisher(); grid_publisher();
send_MPpayload(); send_MPpayload();
// Check updater state and step code // Check updater state and step code
@ -184,10 +175,6 @@ void roscontroller::RosControllerRun()
<< cur_pos.altitude * 100000 << ","; << cur_pos.altitude * 100000 << ",";
log << (int)no_of_robots<<","; log << (int)no_of_robots<<",";
log << neighbours_pos_map.size()<< ","; log << neighbours_pos_map.size()<< ",";
log<<(int)inmsgdata.size()<<",";
log<< message_number<<",";
log<< out_msg_time<<",";
log <<buzz_utility::getuavstate();
// if(neighbours_pos_map.size() > 0)log<<","; // if(neighbours_pos_map.size() > 0)log<<",";
map<int, buzz_utility::Pos_struct>::iterator it = map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin(); neighbours_pos_map.begin();
@ -202,7 +189,9 @@ void roscontroller::RosControllerRun()
<<","<<it->received_time; <<","<<it->received_time;
} }
inmsgdata.clear(); inmsgdata.clear();
log<<std::endl; log<<(int)inmsgdata.size()<<","<< message_number<<",";
log<< out_msg_time<<",";
log <<buzz_utility::get_bvmstate()<<std::endl;
// time_sync_step(); // time_sync_step();
// if(debug) // if(debug)
@ -227,7 +216,7 @@ void roscontroller::RosControllerRun()
loop_rate.cycleTime().toSec()); loop_rate.cycleTime().toSec());
// Safety land if the data coming from the flight controller are too old // Safety land if the data coming from the flight controller are too old
if (fcu_timeout <= 0) if (fcu_timeout <= 0)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); buzzuav_closures::rc_call(NAV_LAND);
else else
fcu_timeout -= 1 / BUZZRATE; fcu_timeout -= 1 / BUZZRATE;
timer_step += 1; timer_step += 1;
@ -252,6 +241,14 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
ROS_ERROR("Provide a .bzz file to run in Launch file"); ROS_ERROR("Provide a .bzz file to run in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
// Obtain debug mode from launch file parameter
if (n_c.getParam("debug", debug))
;
else
{
ROS_ERROR("Provide a debug mode in Launch file");
system("rosnode kill rosbuzz_node");
}
// Obtain rc service option from parameter server // Obtain rc service option from parameter server
if (n_c.getParam("rcclient", rcclient)) if (n_c.getParam("rcclient", rcclient))
{ {
@ -335,7 +332,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/LaserScan")); m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/LaserScan"));
node_handle.getParam("topics/battery", topic); node_handle.getParam("topics/battery", topic);
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/BatteryStatus")); m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/BatteryState"));
node_handle.getParam("topics/status", topic); node_handle.getParam("topics/status", topic);
m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/State")); m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/State"));
@ -398,7 +395,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5); payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5);
MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5); MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", MAX_NUMBER_OF_ROBOTS); neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", MAX_NUMBER_OF_ROBOTS);
uavstate_pub = n_c.advertise<std_msgs::String>("uavstate", 5); bvmstate_pub = n_c.advertise<std_msgs::String>("bvmstate", 5);
grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5); grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5);
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5); localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
@ -431,7 +428,7 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c)
{ {
flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this);
} }
else if (it->second == "mavros_msgs/BatteryStatus") else if (it->second == "sensor_msgs/BatteryState")
{ {
battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this);
} }
@ -506,14 +503,14 @@ void roscontroller::neighbours_pos_publisher()
neigh_pos_pub.publish(neigh_pos_array); neigh_pos_pub.publish(neigh_pos_array);
} }
void roscontroller::uavstate_publisher() void roscontroller::state_publisher()
/* /*
/ Publish current UAVState from Buzz script / Publish current UAVState from Buzz script
/----------------------------------------------------*/ /----------------------------------------------------*/
{ {
std_msgs::String uavstate_msg; std_msgs::String state_msg;
uavstate_msg.data = buzz_utility::getuavstate(); state_msg.data = buzz_utility::get_bvmstate();
uavstate_pub.publish(uavstate_msg); bvmstate_pub.publish(state_msg);
} }
void roscontroller::grid_publisher() void roscontroller::grid_publisher()
@ -639,7 +636,7 @@ with size......... | /
} }
else{ else{
// prepare rosbuzz msg header // prepare rosbuzz msg header
tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME; tmphead[0] = (uint8_t)BUZZ_MESSAGE_NO_TIME;
memcpy(rheader, tmphead, sizeof(uint64_t)); memcpy(rheader, tmphead, sizeof(uint64_t));
// push header into the buffer // push header into the buffer
payload_out.payload64.push_back(rheader[0]); payload_out.payload64.push_back(rheader[0]);
@ -873,10 +870,10 @@ float roscontroller::constrainAngle(float x)
/ Wrap the angle between -pi, pi / Wrap the angle between -pi, pi
----------------------------------------------------------- */ ----------------------------------------------------------- */
{ {
x = fmod(x, 2 * M_PI); x = fmod(x + M_PI, 2 * M_PI);
if (x < 0.0) if (x < 0.0)
x += 2 * M_PI; x += 2 * M_PI;
return x; return x - M_PI;
} }
void roscontroller::gps_rb(POSE nei_pos, double out[]) void roscontroller::gps_rb(POSE nei_pos, double out[])
@ -888,10 +885,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[])
float ned_x = 0.0, ned_y = 0.0; float ned_x = 0.0, ned_y = 0.0;
gps_ned_cur(ned_x, ned_y, nei_pos); gps_ned_cur(ned_x, ned_y, nei_pos);
out[0] = sqrt(ned_x * ned_x + ned_y * ned_y); out[0] = sqrt(ned_x * ned_x + ned_y * ned_y);
// out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned_y, ned_x); out[1] = atan2(ned_y, ned_x);
out[1] = constrainAngle(atan2(ned_y, ned_x));
// out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0; out[2] = 0.0;
} }
@ -1128,7 +1122,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
} }
// BVM FIFO message // BVM FIFO message
else if ((int)mtype == (int)BUZZ_MESSAGE_TIME || else if ((int)mtype == (int)BUZZ_MESSAGE_TIME ||
(int)mtype == (int)BUZZ_MESSAGE_WTO_TIME) (int)mtype == (int)BUZZ_MESSAGE_NO_TIME)
{ {
uint64_t message_obt[msg->payload64.size() - 1]; uint64_t message_obt[msg->payload64.size() - 1];
// Go throught the obtained payload // Go throught the obtained payload
@ -1262,7 +1256,7 @@ void roscontroller::get_number_of_robots()
/ Garbage collector for the number of robots in the swarm / Garbage collector for the number of robots in the swarm
--------------------------------------------------------------------------*/ --------------------------------------------------------------------------*/
{ {
int cur_robots = (int)buzzdict_size(buzz_utility::get_vm()->swarmmembers) + 1; int cur_robots = buzz_utility::get_swarmsize();
if (no_of_robots == 0) if (no_of_robots == 0)
{ {
no_of_robots = cur_robots; no_of_robots = cur_robots;