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

merge rosbuzz dev

This commit is contained in:
dave 2017-09-05 23:59:49 -04:00
parent f906be2d97 0dfe6b1497
commit b1514752b9
5 changed files with 48 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
buzz_scripts/graphformGPS.bzz
View File

@ -543,7 +543,6 @@ function DoJoining(){
set_rc_goto()
else
gotoWP(TransitionToJoined)
#pack the communication package
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel

3
buzz_scripts/include/barrier.bzz
View File

@ -30,7 +30,7 @@ function barrier_create() {
function barrier_set(threshold, transf, resumef, bdt) {
statef = function() {
barrier_wait(threshold, transf, resumef, bdt);
barrier_wait(threshold, transf, resumef);
}
UAVSTATE = "BARRIERWAIT"
barrier_create()
@ -40,6 +40,7 @@ function barrier_set(threshold, transf, resumef, bdt) {
# Make yourself ready
#
function barrier_ready() {
log("BARRIER READY -------")
barrier.put(id, 1)
barrier.put("d", 0)
}

7
buzz_scripts/include/uavstates.bzz
View File

@ -27,9 +27,15 @@ function idle() {
UAVSTATE = "IDLE"
}
firstpassT = 1
function takeoff() {
UAVSTATE = "TAKEOFF"
statef=takeoff
if(firstpassT) {
barrier_create(TAKEOFF_VSTIG)
firstpassT = 0
}
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, action, land, -1)
@ -41,6 +47,7 @@ function takeoff() {
}
}
firstpassL = 1
function land() {
UAVSTATE = "LAND"
statef=land

19
include/roscontroller.h
View File

@ -55,6 +55,8 @@ public:
//void RosControllerInit();
void RosControllerRun();
static const string CAPTURE_SRV_DEFAULT_NAME;
private:
struct num_robot_count
{
@ -70,7 +72,7 @@ private:
double latitude=0.0;
float altitude=0.0;
}; typedef struct gps GPS ; // not useful in cpp
GPS target, home, cur_pos;
double cur_rel_altitude;
@ -92,7 +94,19 @@ private:
/*tmp to be corrected*/
uint8_t no_cnt=0;
uint8_t old_val=0 ;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,obstacles_topic,stand_by,xbeesrv_name, setpoint_name;
std::string bzzfile_name;
std::string fcclient_name;
std::string armclient;
std::string modeclient;
std::string rcservice_name;
std::string bcfname,dbgfname;
std::string out_payload;
std::string in_payload;
std::string obstacles_topic;
std::string stand_by;
std::string xbeesrv_name;
std::string capture_srv_name;
std::string setpoint_name;
std::string stream_client_name;
std::string relative_altitude_sub_name;
std::string setpoint_nonraw;
@ -102,6 +116,7 @@ private:
Num_robot_count count_robots;
ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv;
ros::ServiceClient capture_srv;
ros::Publisher payload_pub;
ros::Publisher MPpayload_pub;
ros::Publisher neigh_pos_pub;

35
src/roscontroller.cpp
View File

@ -2,6 +2,8 @@
#include <thread>
namespace rosbzz_node {
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
/*---------------
/Constructor
---------------*/
@ -201,7 +203,7 @@ void roscontroller::RosControllerRun()
while (ros::ok() && !buzz_utility::buzz_script_done()) {
/*Update neighbors position inside Buzz*/
// buzz_closure::neighbour_pos_callback(neighbours_pos_map);
/*log ROS Time stamp, pos (lat,long,alt), Swarm size, no. of neigh,
neigh pos, RSSI val, Packet loss, filtered packet loss*/
log<<ros::Time::now()<<",";
@ -216,17 +218,17 @@ void roscontroller::RosControllerRun()
log << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z << ",";
}
const uint8_t shrt_id= 0xFF;
const uint8_t shrt_id= 0xFF;
float result;
if ( GetAPIRssi(shrt_id, result) )
if ( GetAPIRssi(shrt_id, result) )
log<<result<<",";
else
log<<"0,";
if ( GetRawPacketLoss(shrt_id, result) )
if ( GetRawPacketLoss(shrt_id, result) )
log<<result<<",";
else
log<<"0,";
if ( GetFilteredPacketLoss(shrt_id, result) )
if ( GetFilteredPacketLoss(shrt_id, result) )
log<<result<<",";
else
log<<"0,";
@ -265,15 +267,15 @@ void roscontroller::RosControllerRun()
// no_of_robots=get_number_of_robots();
get_number_of_robots();
buzz_utility::set_robot_var(no_of_robots);
/*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF
/*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF
SCRIPT IS NOT graphform.bzz*/
static buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
buzzvm_gload(VM);
buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
std::stringstream state_buff;
state_buff<< graph_state->s.value.str<<",";
std::stringstream state_buff;
//buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
// buzzvm_gload(VM);
// buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
// buzzvm_pop(VM);
// state_buff<< graph_state->s.value.str<<",";
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1));
buzzvm_gload(VM);
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
@ -367,6 +369,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
} else
n_c.getParam("xbee_status_srv", xbeesrv_name);
if(!n_c.getParam("capture_image_srv", capture_srv_name))
{
capture_srv_name = CAPTURE_SRV_DEFAULT_NAME;
}
GetSubscriptionParameters(n_c);
// initialize topics to null?
}
@ -475,6 +482,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
ROS_INFO("Ready to receive Mav Commands from RC client");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this);
@ -753,7 +761,8 @@ void roscontroller::flight_controller_service_call()
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
} else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/
ROS_INFO("TAKING A PICTURE HERE!! --------------");
ros::Duration(0.2).sleep();
mavros_msgs::CommandBool capture_command;
capture_srv.call(capture_command);
}
}
/*----------------------------------------------