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")
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

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
<<<<<<< HEAD
old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes
=======
>>>>>>> 064760108611591426d86c679c7789b1a95cebee
function formation() {
BVMSTATE="FORMATION"
# Calculate accumulator

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

@ -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
@ -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
}
}

4
include/buzz_utility.h
View File

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

2
include/rosbuzz/mavrosCC.h
View File

@ -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;

14
include/roscontroller.h
View File

@ -35,7 +35,6 @@
#include <signal.h>
#include <ostream>
#include <map>
#include <cmath>
#include "buzzuav_closures.h"
#include "rosbuzz/mavrosCC.h"
@ -45,7 +44,7 @@
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;
@ -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
{
@ -112,8 +112,6 @@ private:
};
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;
@ -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();

55
src/buzz_utility.cpp
View File

@ -204,25 +204,25 @@ static int buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
buzzvm_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

14
src/buzzuav_closures.cpp
View File

@ -105,10 +105,10 @@ float constrainAngle(float x)
/ Wrap the angle between -pi, pi
----------------------------------------------------------- */
{
x = fmod(x, 2 * M_PI);
x = fmod(x + M_PI, 2 * M_PI);
if (x < 0.0)
x += 2 * M_PI;
return x;
return x - M_PI;
}
void rb_from_gps(double nei[], double out[], double cur[])
@ -229,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);
}

2
src/rosbuzz.cpp
View File

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

62
src/roscontroller.cpp
View File

@ -8,10 +8,9 @@
#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):
logical_clock(ros::Time()), previous_step_time(ros::Time())
@ -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()
@ -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;
}
@ -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;