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

addition of some additional buzz hooks

This commit is contained in:
David St-Onge 2016-11-18 14:05:31 -05:00
parent 66574a6653
commit 27bdabeda4
10 changed files with 250 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
include/buzzuav_closures.h
View File

@ -28,8 +28,13 @@ void set_goto(double pos[]);
void rc_call(int rc_cmd);
/* sets the battery state */
void set_battery(float voltage,float current,float remaining);
/* sets current position */
void set_currentpos(double latitude, double longitude, double altitude);
/*retuns the current go to position */
double* getgoto();
/* updates flight status*/
void flight_status_update(uint8_t state);
/*
* Commands the UAV to takeoff
*/
@ -47,6 +52,14 @@ int buzzuav_gohome(buzzvm_t vm);
* Updates battery information in Buzz
*/
int buzzuav_update_battery(buzzvm_t vm);
/*
* Updates current position in Buzz
*/
int buzzuav_update_currentpos(buzzvm_t vm);
/*
* Updates flight status in Buzz
*/
int buzzuav_update_flight_status(buzzvm_t vm);
/*
* Updates IR information in Buzz

26
include/roscontroller.h
View File

@ -4,6 +4,7 @@
#include "mavros_msgs/GlobalPositionTarget.h"
#include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/CommandInt.h"
#include "mavros_msgs/ExtendedState.h"
#include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/Mavlink.h"
@ -50,6 +51,7 @@ private:
ros::Subscriber current_position_sub;
ros::Subscriber battery_sub;
ros::Subscriber payload_sub;
ros::Subscriber flight_status_sub;
/*Create node Handler*/
ros::NodeHandle n_c;
/*Commands for flight controller*/
@ -61,36 +63,40 @@ private:
/*Obtain data from ros parameter server*/
void Rosparameters_get();
/*compiles buzz script from the specified .bzz file*/
void Compile_bzz();
/*Prepare messages and publish*/
inline void prepare_msg_and_publish();
void prepare_msg_and_publish();
/*Refresh neighbours Position for every ten step*/
inline void maintain_pos(int tim_step);
void maintain_pos(int tim_step);
/*Maintain neighbours position*/
inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
/*Set the current position of the robot callback*/
inline void set_cur_pos(double latitude,
void set_cur_pos(double latitude,
double longitude,
double altitude);
/*convert from catresian to spherical coordinate system callback */
inline double* cvt_spherical_coordinates(double neighbours_pos_payload []);
double* cvt_spherical_coordinates(double neighbours_pos_payload []);
/*battery status callback*/
inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
/*flight status callback*/
void flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
/*current position callback*/
inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
/*payload callback callback*/
inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
/* RC commands service */
inline bool rc_callback(mavros_msgs::CommandInt::Request &req,
bool rc_callback(mavros_msgs::CommandInt::Request &req,
mavros_msgs::CommandInt::Response &res);

100
include/roscontroller.h.save Normal file
View File

@ -0,0 +1,100 @@
#pragma once
#include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h"
#include "mavros_msgs/GlobalPositionTarget.h"
#include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/CommandInt.h"
#include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/Mavlink.h"
#include "sensor_msgs/NavSatStatus.h"
#include <sstream>
#include <buzz/buzzasm.h>
#include "buzzuav_closures.h"
#include "buzz_utility.h"
#include "uav_utility.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <signal.h>
#include <ostream>
#include <map>
using namespace std;
namespace rosbzz_node{
class roscontroller{
public:
roscontroller();
~roscontroller();
//void RosControllerInit();
void RosControllerRun();
private:
double cur_pos[3];
uint64_t payload;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
int timer_step=0;
int robot_id=0;
int oldcmdID=0;
int rc_cmd;
std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname; //, rcclient;
bool rcclient;
ros::ServiceClient mav_client;
ros::Publisher payload_pub;
ros::ServiceServer service;
ros::Subscriber current_position_sub;
ros::Subscriber battery_sub;
ros::Subscriber payload_sub;
/*Create node Handler*/
ros::NodeHandle n_c;
/*Commands for flight controller*/
mavros_msgs::CommandInt cmd_srv;
void Initialize_pub_sub();
/*Obtain data from ros parameter server*/
void Rosparameters_get();
void Compile_bzz();
/*Prepare messages and publish*/
inline void prepare_msg_and_publish();
/*Refresh neighbours Position for every ten step*/
inline void maintain_pos(int tim_step);
/*Maintain neighbours position*/
inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
/*Set the current position of the robot callback*/
inline void set_cur_pos(double latitude,
double longitude,
double altitude);
/*convert from catresian to spherical coordinate system callback */
inline double* cvt_spherical_coordinates(double neighbours_pos_payload []);
/*battery status callback*/
inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
/*current position callback*/
inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
/*payload callback callback*/
inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
/* RC commands service */
inline bool rc_callback(mavros_msgs::CommandInt::Request &req,
mavros_msgs::CommandInt::Response &res);
};
}

4
src/buzz_utility.cpp
View File

@ -301,7 +301,9 @@ void buzz_script_step() {
*/
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
/*
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
/*
* Call Buzz step() function
*/
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {

47
src/buzzuav_closures.cpp
View File

@ -10,6 +10,8 @@
namespace buzzuav_closures{
static double goto_pos[3];
static float batt[3];
static double cur_pos[3];
static uint8_t status;
static int cur_cmd;
static int rc_cmd=0;
/****************************************/
@ -148,6 +150,51 @@ int buzzuav_update_battery(buzzvm_t vm) {
buzzvm_gstore(vm);
return vm->state;
}
/****************************************/
/*current pos update*/
/***************************************/
void set_currentpos(double latitude, double longitude, double altitude){
cur_pos[0]=latitude;
cur_pos[1]=longitude;
cur_pos[2]=altitude;
}
/****************************************/
int buzzuav_update_currentpos(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
buzzvm_pushf(vm, cur_pos[0]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
buzzvm_pushf(vm, cur_pos[1]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
buzzvm_pushf(vm, cur_pos[2]);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
/****************************************/
/*flight status update*/
/***************************************/
void flight_status_update(uint8_t state){
status=state;
}
/****************************************/
int buzzuav_update_flight_status(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
buzzvm_pushf(vm, status);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
/****************************************/
/*To do !!!!!*/

82
src/out.basm
View File

@ -1,4 +1,4 @@
!11
!21
'init
'i
'uav_takeoff
@ -7,6 +7,16 @@
'bzz print: remaining:
'battery
'capacity
'latitude :
'position
'latitude
' longitude:
'longitude
' altitude :
'altitude
'flight status:
'flight
'status
'uav_land
'reset
'destroy
@ -17,10 +27,10 @@
pushs 3
pushcn @__label_2
gstore
pushs 9
pushs 19
pushcn @__label_5
gstore
pushs 10
pushs 20
pushcn @__label_6
gstore
nop
@ -50,26 +60,54 @@
tget |10,48,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 2 |10,49,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
callc |10,49,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 1 |11,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |11,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 20 |11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gt |11,7,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
jumpz @__label_3 |11,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 8 |11,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |11,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 0 |11,19,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
callc |11,19,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
@__label_3 |12,0,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 1 |12,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 1 |12,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |12,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 1 |12,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
add |12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gstore |12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
ret0 |14,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 4 |11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 8 |11,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 9 |11,28,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |11,28,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 10 |11,29,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
tget |11,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 11 |11,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 9 |11,61,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |11,61,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 12 |11,62,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
tget |11,71,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 13 |11,72,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 9 |11,96,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |11,96,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 14 |11,97,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
tget |11,105,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 6 |11,106,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
callc |11,106,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 4 |12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 15 |12,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 16 |12,30,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |12,30,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 17 |12,31,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
tget |12,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 2 |12,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
callc |12,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 1 |13,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |13,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 10 |13,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
eq |13,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
jumpz @__label_3 |13,9,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 18 |13,18,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |13,18,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 0 |13,20,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
callc |13,20,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
@__label_3 |14,0,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 1 |14,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushs 1 |14,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gload |14,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
pushi 1 |14,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
add |14,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
gstore |14,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
ret0 |16,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
@__label_5
ret0 |18,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
ret0 |20,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
@__label_6
ret0 |22,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
ret0 |24,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz

BIN
src/out.bdbg
View File

Binary file not shown.

BIN
src/out.bo
View File

Binary file not shown.

11
src/roscontroller.cpp
View File

@ -79,9 +79,10 @@ namespace rosbzz_node{
void roscontroller::Initialize_pub_sub(){
/*subscribers*/
current_position_sub = n_c.subscribe("/dji_sdk/global_position", 1000, &roscontroller::current_pos,this);
current_position_sub = n_c.subscribe("/mav/global_position", 1000, &roscontroller::current_pos,this);
battery_sub = n_c.subscribe("/mav/power_status", 1000, &roscontroller::battery,this);
payload_sub = n_c.subscribe("inMavlink", 1000, &roscontroller::payload_obt,this);
flight_status_sub =n_c.subscribe("/mav/flight_status",100, &roscontroller::flight_status_update,this);
/*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
/* Clients*/
@ -140,7 +141,7 @@ namespace rosbzz_node{
cout<<" [Debug:] sent message "<<*it<<endl;
i++;
}*/
/*publish prepared messages in respective topic*/
/*publish prepared messages in respective topic*/
payload_pub.publish(payload_out);
delete[] out;
delete[] payload_out_ptr;
@ -191,10 +192,14 @@ namespace rosbzz_node{
buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
//ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, msg->current, msg ->remaining);
}
/*flight status update*/
void roscontroller::flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){
buzzuav_closures::flight_status_update(msg->landed_state);
}
/*current position callback*/
void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude);
}
/*payload callback callback*/

4
src/test.bzz
View File

@ -8,7 +8,9 @@ uav_takeoff()
function step() {
print("bzz print: remaining: ", battery.capacity)
if(i>20){uav_land()}
print("latitude : ",position.latitude," longitude: ",position.longitude," altitude : ", position.altitude)
print("flight status: ",flight.status)
if(i==10){uav_land()}
i=i+1
}