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

Untabification and Added get functions for Xbee_status

This commit is contained in:
pyhs 2017-06-23 10:39:14 -04:00
parent 1d24de28a5
commit 8e5acd264d
5 changed files with 1828 additions and 1607 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/buzzuav_closures.h
View File

@ -14,8 +14,8 @@
namespace buzzuav_closures{ namespace buzzuav_closures{
typedef enum { typedef enum {
COMMAND_NIL = 0, // Dummy command COMMAND_NIL = 0, // Dummy command
COMMAND_TAKEOFF, // Take off COMMAND_TAKEOFF, // Take off
COMMAND_LAND, COMMAND_LAND,
COMMAND_GOHOME, COMMAND_GOHOME,
COMMAND_ARM, COMMAND_ARM,
@ -64,7 +64,7 @@ void set_obstacle_dist(float dist[]);
*/ */
int buzzuav_takeoff(buzzvm_t vm); int buzzuav_takeoff(buzzvm_t vm);
/* /*
* Arm command from Buzz * Arm command from Buzz
*/ */
int buzzuav_arm(buzzvm_t vm); int buzzuav_arm(buzzvm_t vm);
/* /*
@ -89,7 +89,7 @@ int buzzuav_update_battery(buzzvm_t vm);
int buzzuav_update_currentpos(buzzvm_t vm); int buzzuav_update_currentpos(buzzvm_t vm);
int buzzuav_adduserRB(buzzvm_t vm); int buzzuav_adduserRB(buzzvm_t vm);
/* /*
* Updates flight status and rc command in Buzz, put it in a tabel to acess it * Updates flight status and rc command in Buzz, put it in a tabel to acess it
* use flight.status for flight status * use flight.status for flight status
* use flight.rc_cmd for current rc cmd * use flight.rc_cmd for current rc cmd
*/ */

309
include/roscontroller.h
View File

@ -42,197 +42,206 @@
using namespace std; using namespace std;
namespace rosbzz_node{ namespace rosbzz_node
{
class roscontroller{
class roscontroller
{
public: public:
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller(); ~roscontroller();
//void RosControllerInit(); //void RosControllerInit();
void RosControllerRun(); void RosControllerRun();
private: private:
struct num_robot_count struct num_robot_count
{ {
uint8_t history[10]; uint8_t history[10];
uint8_t index=0; uint8_t index = 0;
uint8_t current=0; uint8_t current = 0;
num_robot_count(){} num_robot_count(){}
}; typedef struct num_robot_count Num_robot_count ; // not useful in cpp
}; typedef struct num_robot_count Num_robot_count ;
struct gps struct gps
{ {
double longitude=0.0; double longitude=0.0;
double latitude=0.0; double latitude=0.0;
float altitude=0.0; float altitude=0.0;
}; typedef struct gps GPS ; }; typedef struct gps GPS ; // not useful in cpp
GPS target, home, cur_pos;
double cur_rel_altitude;
uint64_t payload; GPS target, home, cur_pos;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; double cur_rel_altitude;
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos; uint64_t payload;
int timer_step=0; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
int robot_id=0; std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
std::string robot_name = ""; //std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step=0;
int robot_id=0;
std::string robot_name = "";
//int oldcmdID=0; //int oldcmdID=0;
int rc_cmd; int rc_cmd;
float fcu_timeout; float fcu_timeout;
int armstate; int armstate;
int barrier; int barrier;
int message_number=0; int message_number=0;
uint8_t no_of_robots=0; uint8_t no_of_robots=0;
/*tmp to be corrected*/ /*tmp to be corrected*/
uint8_t no_cnt=0; uint8_t no_cnt=0;
uint8_t old_val=0; uint8_t old_val=0 ;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name; std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
std::string stream_client_name; std::string stream_client_name;
std::string relative_altitude_sub_name; std::string relative_altitude_sub_name;
std::string setpoint_nonraw; std::string setpoint_nonraw;
bool rcclient; bool rcclient;
bool xbeeplugged = false; bool xbeeplugged = false;
bool multi_msg; bool multi_msg;
Num_robot_count count_robots; Num_robot_count count_robots;
ros::ServiceClient mav_client; ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv; ros::ServiceClient xbeestatus_srv;
ros::Publisher payload_pub; ros::Publisher payload_pub;
ros::Publisher neigh_pos_pub; ros::Publisher neigh_pos_pub;
ros::Publisher localsetpoint_nonraw_pub; ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service; ros::ServiceServer service;
ros::Subscriber current_position_sub; ros::Subscriber current_position_sub;
ros::Subscriber users_sub; ros::Subscriber users_sub;
ros::Subscriber battery_sub; ros::Subscriber battery_sub;
ros::Subscriber payload_sub; ros::Subscriber payload_sub;
ros::Subscriber flight_status_sub; ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_sub; ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub; ros::Subscriber Robot_id_sub;
ros::Subscriber relative_altitude_sub; ros::Subscriber relative_altitude_sub;
std::string local_pos_sub_name; std::string local_pos_sub_name;
ros::Subscriber local_pos_sub; ros::Subscriber local_pos_sub;
double local_pos_new[3]; double local_pos_new[3];
ros::ServiceClient stream_client; ros::ServiceClient stream_client;
int setpoint_counter; int setpoint_counter;
double my_x = 0, my_y = 0; double my_x = 0, my_y = 0;
std::ofstream log;
/*Commands for flight controller*/ std::ofstream log;
//mavros_msgs::CommandInt cmd_srv;
mavros_msgs::CommandLong cmd_srv;
std::vector<std::string> m_sMySubscriptions;
std::map<std::string, std::string> m_smTopic_infos;
mavros_msgs::CommandBool m_cmdBool; /*Commands for flight controller*/
ros::ServiceClient arm_client; //mavros_msgs::CommandInt cmd_srv;
mavros_msgs::CommandLong cmd_srv;
std::vector<std::string> m_sMySubscriptions;
std::map<std::string, std::string> m_smTopic_infos;
mavros_msgs::SetMode m_cmdSetMode; mavros_msgs::CommandBool m_cmdBool;
ros::ServiceClient mode_client; ros::ServiceClient arm_client;
/*Initialize publisher and subscriber, done in the constructor*/
void Initialize_pub_sub(ros::NodeHandle& n_c);
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode mavros_msgs::SetMode m_cmdSetMode;
ros::ServiceClient mode_client;
/*Obtain data from ros parameter server*/ /*Initialize publisher and subscriber, done in the constructor*/
void Rosparameters_get(ros::NodeHandle& n_c_priv); void Initialize_pub_sub(ros::NodeHandle& n_c);
/*compiles buzz script from the specified .bzz file*/ std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
std::string Compile_bzz(std::string bzzfile_name);
/*Flight controller service call*/ /*Obtain data from ros parameter server*/
void flight_controller_service_call(); void Rosparameters_get(ros::NodeHandle& n_c_priv);
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
/*Prepare messages and publish*/ /*compiles buzz script from the specified .bzz file*/
void prepare_msg_and_publish(); std::string Compile_bzz(std::string bzzfile_name);
/*Flight controller service call*/
/*Refresh neighbours Position for every ten step*/ void flight_controller_service_call();
void maintain_pos(int tim_step);
/*Puts neighbours position inside neigbours_pos_map*/ /*Neighbours pos publisher*/
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); void neighbours_pos_publisher();
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/ /*Prepare messages and publish*/
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); void prepare_msg_and_publish();
/*Set the current position of the robot callback*/
void set_cur_pos(double latitude,
double longitude,
double altitude);
/*convert from spherical to cartesian coordinate system callback */
void gps_rb(GPS nei_pos, double out[]);
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
void gps_ned_home(float &ned_x, float &ned_y);
void gps_convert_ned(float &ned_x, float &ned_y,
double gps_t_lon, double gps_t_lat,
double gps_r_lon, double gps_r_lat);
/*battery status callback*/ /*Refresh neighbours Position for every ten step*/
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); void maintain_pos(int tim_step);
/*flight extended status callback*/
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
/*flight status callback*/ /*Puts neighbours position inside neigbours_pos_map*/
void flight_status_update(const mavros_msgs::State::ConstPtr& msg); void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
/*current position callback*/ /*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
/*Set the current position of the robot callback*/
void set_cur_pos(double latitude,
double longitude,
double altitude);
/*convert from spherical to cartesian coordinate system callback */
void gps_rb(GPS nei_pos, double out[]);
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
void gps_ned_home(float &ned_x, float &ned_y);
void gps_convert_ned(float &ned_x, float &ned_y,
double gps_t_lon, double gps_t_lat,
double gps_r_lon, double gps_r_lat);
/*battery status callback */
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
/*flight extended status callback*/
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
/*flight status callback*/
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
void users_pos(const rosbuzz::neigh_pos msg); void users_pos(const rosbuzz::neigh_pos msg);
/*current relative altitude callback*/ /*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
/*payload callback callback*/ /*payload callback callback*/
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
/* RC commands service */ /* RC commands service */
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res); bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
/*robot id sub callback*/ /*robot id sub callback*/
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg); void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
/*Obstacle distance table callback*/ /*Obstacle distance table callback*/
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg); void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
/*Get publisher and subscriber from YML file*/ /*Get publisher and subscriber from YML file*/
void GetSubscriptionParameters(ros::NodeHandle& node_handle); void GetSubscriptionParameters(ros::NodeHandle& node_handle);
/*Arm/disarm method that can be called from buzz*/ /*Arm/disarm method that can be called from buzz*/
void Arm(); void Arm();
/*set mode like guided for solo*/ /*set mode like guided for solo*/
void SetMode(std::string mode, int delay_miliseconds); void SetMode(std::string mode, int delay_miliseconds);
/*Robot independent subscribers*/ /*Robot independent subscribers*/
void Subscribe(ros::NodeHandle& n_c); void Subscribe(ros::NodeHandle& n_c);
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose); void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
//void WaypointMissionSetup(float lat, float lng, float alt); //void WaypointMissionSetup(float lat, float lng, float alt);
void fc_command_setup(); void fc_command_setup();
void SetLocalPosition(float x, float y, float z, float yaw); void SetLocalPosition(float x, float y, float z, float yaw);
void SetLocalPositionNonRaw(float x, float y, float z, float yaw); void SetLocalPositionNonRaw(float x, float y, float z, float yaw);
void SetStreamRate(int id, int rate, int on_off);
void get_number_of_robots();
void GetRobotId();
bool GetDequeFull(bool &result);
bool GetRssi(float &result);
bool TriggerAPIRssi(const uint8_t short_id);
bool GetAPIRssi(const uint8_t short_id, float &result);
bool GetRawPacketLoss(const uint8_t short_id, float &result);
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
void SetStreamRate(int id, int rate, int on_off);
void get_number_of_robots();
void GetRobotId();
}; };
} }

44
src/buzz_utility.cpp
View File

@ -1,7 +1,7 @@
/** @file buzz_utility.cpp /** @file buzz_utility.cpp
* @version 1.0 * @version 1.0
* @date 27.09.2016 * @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone. * @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan * @author Vivek Shankar Varadharajan
* @copyright 2016 MistLab. All rights reserved. * @copyright 2016 MistLab. All rights reserved.
*/ */
@ -18,11 +18,11 @@ namespace buzz_utility{
static uint8_t* BO_BUF = 0; static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0; static buzzdebug_t DBG_INFO = 0;
static uint32_t MSG_SIZE = 600;//250; // Only 100 bytes of Buzz messages every step static uint32_t MSG_SIZE = 600;//250; // Only 100 bytes of Buzz messages every step
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0; static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG; static std::vector<uint8_t*> IN_MSG;
std::map< int, Pos_struct> users_map; std::map< int, Pos_struct> users_map;
/****************************************/ /****************************************/
void add_user(int id, double latitude, double longitude, float altitude) void add_user(int id, double latitude, double longitude, float altitude)
@ -181,12 +181,12 @@ namespace buzz_utility{
/* Copy packet into temporary buffer */ /* Copy packet into temporary buffer */
memcpy(pl, payload ,size); memcpy(pl, payload ,size);
IN_MSG.push_back(pl); IN_MSG.push_back(pl);
} }
void in_message_process(){ void in_message_process(){
while(!IN_MSG.empty()){ while(!IN_MSG.empty()){
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front(); uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
/* Go through messages and append them to the FIFO */ /* Go through messages and append them to the FIFO */
uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]); uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]);
/*Size is at first 2 bytes*/ /*Size is at first 2 bytes*/
@ -220,7 +220,7 @@ namespace buzz_utility{
/***************************************************/ /***************************************************/
uint64_t* obt_out_msg(){ uint64_t* obt_out_msg(){
/* Process out messages */ /* Process out messages */
buzzvm_process_outmsgs(VM); buzzvm_process_outmsgs(VM);
uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE); uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
memset(buff_send, 0, MAX_MSG_SIZE); memset(buff_send, 0, MAX_MSG_SIZE);
/*Taking into consideration the sizes included at the end*/ /*Taking into consideration the sizes included at the end*/
@ -361,7 +361,7 @@ namespace buzz_utility{
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_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);
@ -414,7 +414,7 @@ int create_stig_tables() {
//buzzvm_gstore(VM); //buzzvm_gstore(VM);
//buzzvm_dump(VM); //buzzvm_dump(VM);
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM); buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM); buzzvm_tget(VM);
@ -435,7 +435,7 @@ int create_stig_tables() {
buzzobj_t data = buzzvm_stack_at(VM, 1); buzzobj_t data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM); buzzvm_tput(VM);
buzzvm_push(VM, data); buzzvm_push(VM, data);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM); buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1));
@ -498,7 +498,7 @@ int create_stig_tables() {
ROS_ERROR("[%i] Error registering hooks", Robot_id); ROS_ERROR("[%i] Error registering hooks", Robot_id);
return 0; return 0;
} }
/* Create vstig tables /* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) { if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -507,10 +507,10 @@ int create_stig_tables() {
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0; return 0;
}*/ }*/
/* Save bytecode file name */ /* Save bytecode file name */
BO_FNAME = strdup(bo_filename); BO_FNAME = strdup(bo_filename);
// Execute the global part of the script // Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state); ROS_ERROR("Error executing global part, VM state : %i",VM->state);
@ -527,7 +527,7 @@ int create_stig_tables() {
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size); return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
} }
/****************************************/ /****************************************/
/*Sets a new update */ /*Sets a new update */
/****************************************/ /****************************************/
@ -560,7 +560,7 @@ int create_stig_tables() {
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0; return 0;
} }
/* Create vstig tables /* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) { if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -569,7 +569,7 @@ int create_stig_tables() {
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0; return 0;
}*/ }*/
// Execute the global part of the script // Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state); ROS_ERROR("Error executing global part, VM state : %i",VM->state);
@ -616,7 +616,7 @@ int create_stig_tables() {
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0; return 0;
} }
/* Create vstig tables /* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) { if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -708,12 +708,12 @@ int create_stig_tables() {
buzz_error_info()); buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
} }
/*Print swarm*/ /*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1; //int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */ /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
//int status = 1; //int status = 1;
@ -771,7 +771,7 @@ int create_stig_tables() {
buzz_error_info()); buzz_error_info());
fprintf(stdout, "step test VM state %i\n",a); fprintf(stdout, "step test VM state %i\n",a);
} }
return a == BUZZVM_STATE_READY; return a == BUZZVM_STATE_READY;
} }
@ -785,5 +785,3 @@ int create_stig_tables() {
buzzvm_gstore(VM); buzzvm_gstore(VM);
} }
} }

895
src/buzzuav_closures.cpp
View File

@ -1,7 +1,7 @@
/** @file buzzuav_closures.cpp /** @file buzzuav_closures.cpp
* @version 1.0 * @version 1.0
* @date 27.09.2016 * @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone. * @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan * @author Vivek Shankar Varadharajan
* @copyright 2016 MistLab. All rights reserved. * @copyright 2016 MistLab. All rights reserved.
*/ */
@ -11,427 +11,432 @@
namespace buzzuav_closures{ namespace buzzuav_closures{
// TODO: Minimize the required global variables and put them in the header // TODO: Minimize the required global variables and put them in the header
//static const rosbzz_node::roscontroller* roscontroller_ptr; //static const rosbzz_node::roscontroller* roscontroller_ptr;
/*forward declaration of ros controller ptr storing function*/ /*forward declaration of ros controller ptr storing function*/
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin); //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
static double goto_pos[3]; static double goto_pos[3];
static double rc_goto_pos[3]; static double rc_goto_pos[3];
static float batt[3]; static float batt[3];
static float obst[5]={0,0,0,0,0}; static float obst[5] = {0,0,0,0,0};
static double cur_pos[3]; static double cur_pos[3];
static uint8_t status; static uint8_t status;
static int cur_cmd = 0; static int cur_cmd = 0;
static int rc_cmd=0; static int rc_cmd = 0;
static int buzz_cmd=0; static int buzz_cmd = 0;
static float height=0; static float height = 0;
std::map< int, buzz_utility::Pos_struct> neighbors_map; std::map< int, buzz_utility::Pos_struct> neighbors_map;
/****************************************/ /****************************************/
/****************************************/ /****************************************/
int buzzros_print(buzzvm_t vm) { int buzzros_print(buzzvm_t vm)
int i; {
char buffer [50] = ""; int i;
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid()); char buffer [50] = "";
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
buzzvm_lload(vm, i); for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i)
buzzobj_t o = buzzvm_stack_at(vm, 1); {
buzzvm_pop(vm); buzzvm_lload(vm, i);
switch(o->o.type) { buzzobj_t o = buzzvm_stack_at(vm, 1);
case BUZZTYPE_NIL: buzzvm_pop(vm);
sprintf(buffer,"%s BUZZ - [nil]", buffer); switch(o->o.type)
break; {
case BUZZTYPE_INT: case BUZZTYPE_NIL:
sprintf(buffer,"%s %d", buffer, o->i.value); sprintf(buffer,"%s BUZZ - [nil]", buffer);
//fprintf(stdout, "%d", o->i.value); break;
break; case BUZZTYPE_INT:
case BUZZTYPE_FLOAT: sprintf(buffer,"%s %d", buffer, o->i.value);
sprintf(buffer,"%s %f", buffer, o->f.value); //fprintf(stdout, "%d", o->i.value);
break; break;
case BUZZTYPE_TABLE: case BUZZTYPE_FLOAT:
sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value))); sprintf(buffer,"%s %f", buffer, o->f.value);
break; break;
case BUZZTYPE_CLOSURE: case BUZZTYPE_TABLE:
if(o->c.value.isnative) sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value)));
sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref); break;
else case BUZZTYPE_CLOSURE:
sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref); if(o->c.value.isnative)
break; sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref);
case BUZZTYPE_STRING: else
sprintf(buffer,"%s %s", buffer, o->s.value.str); sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref);
break; break;
case BUZZTYPE_USERDATA: case BUZZTYPE_STRING:
sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value); sprintf(buffer,"%s %s", buffer, o->s.value.str);
break; break;
default: case BUZZTYPE_USERDATA:
break; sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value);
} break;
} default:
ROS_INFO(buffer); break;
//fprintf(stdout, "\n"); }
return buzzvm_ret0(vm); }
} ROS_INFO("%s",buffer);
//fprintf(stdout, "\n");
return buzzvm_ret0(vm);
}
/*----------------------------------------/ /*----------------------------------------/
/ Compute GPS destination from current position and desired Range and Bearing / Compute GPS destination from current position and desired Range and Bearing
/----------------------------------------*/ /----------------------------------------*/
void gps_from_rb(double range, double bearing, double out[3]) { void gps_from_rb(double range, double bearing, double out[3]) {
double lat = cur_pos[0]*M_PI/180.0; double lat = cur_pos[0]*M_PI/180.0;
double lon = cur_pos[1]*M_PI/180.0; double lon = cur_pos[1]*M_PI/180.0;
out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing)); out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0])); out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
out[0] = out[0]*180.0/M_PI; out[0] = out[0]*180.0/M_PI;
out[1] = out[1]*180.0/M_PI; out[1] = out[1]*180.0/M_PI;
out[2] = height; //constant height. out[2] = height; //constant height.
} }
void rb_from_gps(double nei[], double out[], double cur[]){ void rb_from_gps(double nei[], double out[], double cur[]){
double d_lon = nei[1] - cur[1]; double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0]; double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
out[1] = atan2(ned_y,ned_x); out[1] = atan2(ned_y,ned_x);
out[2] = 0.0; out[2] = 0.0;
} }
// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests // Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336}; double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958}; double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677}; double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677};
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to move following a 2D vector / Buzz closure to move following a 2D vector
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_moveto(buzzvm_t vm) { int buzzuav_moveto(buzzvm_t vm)
buzzvm_lnum_assert(vm, 2); {
buzzvm_lload(vm, 1); /* dx */ buzzvm_lnum_assert(vm, 2);
buzzvm_lload(vm, 2); /* dy */ buzzvm_lload(vm, 1); /* dx */
//buzzvm_lload(vm, 3); /* Latitude */ buzzvm_lload(vm, 2); /* dy */
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); //buzzvm_lload(vm, 3); /* Latitude */
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); //buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
float dy = buzzvm_stack_at(vm, 1)->f.value; buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float dx = buzzvm_stack_at(vm, 2)->f.value; float dy = buzzvm_stack_at(vm, 1)->f.value;
double d = sqrt(dx*dx+dy*dy); //range float dx = buzzvm_stack_at(vm, 2)->f.value;
goto_pos[0]=dx; // The compiler warnings are useful!
goto_pos[1]=dy; // double d = sqrt(dx*dx+dy*dy); //range
goto_pos[2]=height; goto_pos[0]=dx;
/*double b = atan2(dy,dx); //bearing goto_pos[1]=dy;
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); goto_pos[2]=height;
gps_from_rb(d, b, goto_pos); /*double b = atan2(dy,dx); //bearing
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
//printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
//printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]); //printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]);
buzz_cmd= COMMAND_MOVETO; // TO DO what should we use buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) { int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) {
if(vm->state != BUZZVM_STATE_READY) return vm->state; if(vm->state != BUZZVM_STATE_READY) return vm->state;
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
buzzvm_gload(vm); buzzvm_gload(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
buzzobj_t nbr = buzzvm_stack_at(vm, 1); buzzobj_t nbr = buzzvm_stack_at(vm, 1);
/* Get "data" field */ /* Get "data" field */
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
buzzvm_tget(vm); buzzvm_tget(vm);
if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) { if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) {
//ROS_INFO("Empty data, create a new table"); //ROS_INFO("Empty data, create a new table");
buzzvm_pop(vm); buzzvm_pop(vm);
buzzvm_push(vm, nbr); buzzvm_push(vm, nbr);
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzobj_t data = buzzvm_stack_at(vm, 1); buzzobj_t data = buzzvm_stack_at(vm, 1);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_push(vm, data); buzzvm_push(vm, data);
} }
/* When we get here, the "data" table is on top of the stack */ /* When we get here, the "data" table is on top of the stack */
/* Push user id */ /* Push user id */
buzzvm_pushi(vm, id); buzzvm_pushi(vm, id);
/* Create entry table */ /* Create entry table */
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
/* Insert range */ /* Insert range */
buzzvm_push(vm, entry); buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1));
buzzvm_pushf(vm, range); buzzvm_pushf(vm, range);
buzzvm_tput(vm); buzzvm_tput(vm);
/* Insert longitude */ /* Insert longitude */
buzzvm_push(vm, entry); buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1));
buzzvm_pushf(vm, bearing); buzzvm_pushf(vm, bearing);
buzzvm_tput(vm); buzzvm_tput(vm);
/* Save entry into data table */ /* Save entry into data table */
buzzvm_push(vm, entry); buzzvm_push(vm, entry);
buzzvm_tput(vm); buzzvm_tput(vm);
//printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing); //printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing);
return vm->state; return vm->state;
} }
int buzzuav_adduserRB(buzzvm_t vm) { int buzzuav_adduserRB(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 3); buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* longitude */ buzzvm_lload(vm, 1); /* longitude */
buzzvm_lload(vm, 2); /* latitude */ buzzvm_lload(vm, 2); /* latitude */
buzzvm_lload(vm, 3); /* id */ buzzvm_lload(vm, 3); /* id */
buzzvm_type_assert(vm, 3, BUZZTYPE_INT); buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
double tmp[3]; double tmp[3];
tmp[0] = buzzvm_stack_at(vm, 2)->f.value; tmp[0] = buzzvm_stack_at(vm, 2)->f.value;
tmp[1] = buzzvm_stack_at(vm, 1)->f.value; tmp[1] = buzzvm_stack_at(vm, 1)->f.value;
tmp[2] = 0.0; tmp[2] = 0.0;
int uid = buzzvm_stack_at(vm, 3)->i.value; int uid = buzzvm_stack_at(vm, 3)->i.value;
double rb[3]; double rb[3];
rb_from_gps(tmp, rb, cur_pos); rb_from_gps(tmp, rb, cur_pos);
if(fabs(rb[0])<100.0) { if(fabs(rb[0])<100.0) {
//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]); //printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
return users_add2localtable(vm, uid, rb[0], rb[1]); return users_add2localtable(vm, uid, rb[0], rb[1]);
} else } else
printf(" ---------- User too far %f\n",rb[0]); printf(" ---------- User too far %f\n",rb[0]);
return 0; return 0;
} }
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to go directly to a GPS destination from the Mission Planner / Buzz closure to go directly to a GPS destination from the Mission Planner
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_goto(buzzvm_t vm) { int buzzuav_goto(buzzvm_t vm) {
rc_goto_pos[2]=height; rc_goto_pos[2]=height;
set_goto(rc_goto_pos); set_goto(rc_goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]); printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
buzz_cmd=COMMAND_GOTO; buzz_cmd=COMMAND_GOTO;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running / Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_arm(buzzvm_t vm) { int buzzuav_arm(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n"); printf(" Buzz requested Arm \n");
buzz_cmd=COMMAND_ARM; buzz_cmd=COMMAND_ARM;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_disarm(buzzvm_t vm) { int buzzuav_disarm(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n"); printf(" Buzz requested Disarm \n");
buzz_cmd=COMMAND_DISARM; buzz_cmd=COMMAND_DISARM;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/*---------------------------------------/ /*---------------------------------------/
/ Buss closure for basic UAV commands / Buss closure for basic UAV commands
/---------------------------------------*/ /---------------------------------------*/
int buzzuav_takeoff(buzzvm_t vm) { int buzzuav_takeoff(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 1); buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1); /* Altitude */ buzzvm_lload(vm, 1); /* Altitude */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value; goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value;
height = goto_pos[2]; height = goto_pos[2];
cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
printf(" Buzz requested Take off !!! \n"); printf(" Buzz requested Take off !!! \n");
buzz_cmd = COMMAND_TAKEOFF; buzz_cmd = COMMAND_TAKEOFF;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_land(buzzvm_t vm) { int buzzuav_land(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::NAV_LAND; cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
printf(" Buzz requested Land !!! \n"); printf(" Buzz requested Land !!! \n");
buzz_cmd = COMMAND_LAND; buzz_cmd = COMMAND_LAND;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_gohome(buzzvm_t vm) { int buzzuav_gohome(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n"); printf(" Buzz requested gohome !!! \n");
buzz_cmd = COMMAND_GOHOME; buzz_cmd = COMMAND_GOHOME;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/*------------------------------- /*-------------------------------
/ Get/Set to transfer variable from Roscontroller to buzzuav / Get/Set to transfer variable from Roscontroller to buzzuav
/------------------------------------*/ /------------------------------------*/
double* getgoto() { double* getgoto() {
return goto_pos; return goto_pos;
} }
int getcmd() { int getcmd() {
return cur_cmd; return cur_cmd;
} }
void set_goto(double pos[]) { void set_goto(double pos[]) {
goto_pos[0] = pos[0]; goto_pos[0] = pos[0];
goto_pos[1] = pos[1]; goto_pos[1] = pos[1];
goto_pos[2] = pos[2]; goto_pos[2] = pos[2];
} }
int bzz_cmd() { int bzz_cmd() {
int cmd = buzz_cmd; int cmd = buzz_cmd;
buzz_cmd = 0; buzz_cmd = 0;
return cmd; return cmd;
} }
void rc_set_goto(double pos[]) { void rc_set_goto(double pos[]) {
rc_goto_pos[0] = pos[0]; rc_goto_pos[0] = pos[0];
rc_goto_pos[1] = pos[1]; rc_goto_pos[1] = pos[1];
rc_goto_pos[2] = pos[2]; rc_goto_pos[2] = pos[2];
} }
void rc_call(int rc_cmd_in) { void rc_call(int rc_cmd_in) {
rc_cmd = rc_cmd_in; rc_cmd = rc_cmd_in;
} }
void set_obstacle_dist(float dist[]) { void set_obstacle_dist(float dist[]) {
for (int i = 0; i < 5; i++) for (int i = 0; i < 5; i++)
obst[i] = dist[i]; obst[i] = dist[i];
} }
void set_battery(float voltage,float current,float remaining){ void set_battery(float voltage,float current,float remaining){
batt[0]=voltage; batt[0]=voltage;
batt[1]=current; batt[1]=current;
batt[2]=remaining; batt[2]=remaining;
} }
int buzzuav_update_battery(buzzvm_t vm) { int buzzuav_update_battery(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1));
buzzvm_pushf(vm, batt[0]); buzzvm_pushf(vm, batt[0]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1));
buzzvm_pushf(vm, batt[1]); buzzvm_pushf(vm, batt[1]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1));
buzzvm_pushf(vm, batt[2]); buzzvm_pushf(vm, batt[2]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_gstore(vm); buzzvm_gstore(vm);
return vm->state; return vm->state;
} }
/****************************************/ /****************************************/
/*current pos update*/ /*current pos update*/
/***************************************/ /***************************************/
void set_currentpos(double latitude, double longitude, double altitude){ void set_currentpos(double latitude, double longitude, double altitude){
cur_pos[0]=latitude; cur_pos[0]=latitude;
cur_pos[1]=longitude; cur_pos[1]=longitude;
cur_pos[2]=altitude; cur_pos[2]=altitude;
} }
/*adds neighbours position*/ /*adds neighbours position*/
void neighbour_pos_callback(int id, float range, float bearing, float elevation){ void neighbour_pos_callback(int id, float range, float bearing, float elevation){
buzz_utility::Pos_struct pos_arr; buzz_utility::Pos_struct pos_arr;
pos_arr.x=range; pos_arr.x=range;
pos_arr.y=bearing; pos_arr.y=bearing;
pos_arr.z=elevation; pos_arr.z=elevation;
map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id); map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id);
if(it!=neighbors_map.end()) if(it!=neighbors_map.end())
neighbors_map.erase(it); neighbors_map.erase(it);
neighbors_map.insert(make_pair(id, pos_arr)); neighbors_map.insert(make_pair(id, pos_arr));
} }
/* update at each step the VM table */ /* update at each step the VM table */
void update_neighbors(buzzvm_t vm){ void update_neighbors(buzzvm_t vm){
/* Reset neighbor information */ /* Reset neighbor information */
buzzneighbors_reset(vm); buzzneighbors_reset(vm);
/* Get robot id and update neighbor information */ /* Get robot id and update neighbor information */
map< int, buzz_utility::Pos_struct >::iterator it; map< int, buzz_utility::Pos_struct >::iterator it;
for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){ for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){
buzzneighbors_add(vm, buzzneighbors_add(vm,
it->first, it->first,
(it->second).x, (it->second).x,
(it->second).y, (it->second).y,
(it->second).z); (it->second).z);
} }
} }
/****************************************/ /****************************************/
int buzzuav_update_currentpos(buzzvm_t vm) { int buzzuav_update_currentpos(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
buzzvm_pushf(vm, cur_pos[0]); buzzvm_pushf(vm, cur_pos[0]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
buzzvm_pushf(vm, cur_pos[1]); buzzvm_pushf(vm, cur_pos[1]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
buzzvm_pushf(vm, cur_pos[2]); buzzvm_pushf(vm, cur_pos[2]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_gstore(vm); buzzvm_gstore(vm);
return vm->state; return vm->state;
} }
void flight_status_update(uint8_t state){ void flight_status_update(uint8_t state){
status=state; status=state;
} }
/*---------------------------------------------------- /*----------------------------------------------------
/ Create the generic robot table with status, remote controller current comand and destination / Create the generic robot table with status, remote controller current comand and destination
/ and current position of the robot / and current position of the robot
/ TODO: change global name for robot / TODO: change global name for robot
/------------------------------------------------------*/ /------------------------------------------------------*/
int buzzuav_update_flight_status(buzzvm_t vm) { int buzzuav_update_flight_status(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1));
buzzvm_pushi(vm, rc_cmd); buzzvm_pushi(vm, rc_cmd);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
rc_cmd=0; rc_cmd=0;
buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
buzzvm_pushi(vm, status); buzzvm_pushi(vm, status);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_gstore(vm); buzzvm_gstore(vm);
//also set rc_controllers goto //also set rc_controllers goto
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
buzzvm_pushf(vm, rc_goto_pos[0]); buzzvm_pushf(vm, rc_goto_pos[0]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
buzzvm_pushf(vm, rc_goto_pos[1]); buzzvm_pushf(vm, rc_goto_pos[1]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
buzzvm_pushf(vm, rc_goto_pos[2]); buzzvm_pushf(vm, rc_goto_pos[2]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_gstore(vm); buzzvm_gstore(vm);
return vm->state; return vm->state;
} }
/******************************************************/ /******************************************************/
/*Create an obstacle Buzz table from proximity sensors*/ /*Create an obstacle Buzz table from proximity sensors*/
/* Acessing proximity in buzz script /* Acessing proximity in buzz script
proximity[0].angle and proximity[0].value - front proximity[0].angle and proximity[0].value - front
"" "" "" - right and back "" "" "" - right and back
proximity[3].angle and proximity[3].value - left proximity[3].angle and proximity[3].value - left
proximity[4].angle = -1 and proximity[4].value -bottom */ proximity[4].angle = -1 and proximity[4].value -bottom */
/******************************************************/ /******************************************************/
int buzzuav_update_prox(buzzvm_t vm) { int buzzuav_update_prox(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1); buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
buzzvm_gstore(vm); buzzvm_gstore(vm);
/* Fill into the proximity table */ /* Fill into the proximity table */
buzzobj_t tProxRead; buzzobj_t tProxRead;
float angle =0; float angle =0;
@ -441,80 +446,80 @@ namespace buzzuav_closures{
tProxRead = buzzvm_stack_at(vm, 1); tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm); buzzvm_pop(vm);
/* Fill in the read */ /* Fill in the read */
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[i+1]); buzzvm_pushf(vm, obst[i+1]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle); buzzvm_pushf(vm, angle);
buzzvm_tput(vm); buzzvm_tput(vm);
/* Store read table in the proximity table */ /* Store read table in the proximity table */
buzzvm_push(vm, tProxTable); buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, i); buzzvm_pushi(vm, i);
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_tput(vm); buzzvm_tput(vm);
angle+=1.5708; angle+=1.5708;
} }
/* Create table for bottom read */ /* Create table for bottom read */
angle =-1; angle =-1;
buzzvm_pusht(vm); buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1); tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm); buzzvm_pop(vm);
/* Fill in the read */ /* Fill in the read */
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[0]); buzzvm_pushf(vm, obst[0]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle); buzzvm_pushf(vm, angle);
buzzvm_tput(vm); buzzvm_tput(vm);
/*Store read table in the proximity table*/ /*Store read table in the proximity table*/
buzzvm_push(vm, tProxTable); buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, 4); buzzvm_pushi(vm, 4);
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_tput(vm); buzzvm_tput(vm);
/* /*
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1));
buzzvm_pushf(vm, obst[0]); buzzvm_pushf(vm, obst[0]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1));
buzzvm_pushf(vm, obst[1]); buzzvm_pushf(vm, obst[1]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1));
buzzvm_pushf(vm, obst[2]); buzzvm_pushf(vm, obst[2]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1));
buzzvm_pushf(vm, obst[3]); buzzvm_pushf(vm, obst[3]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
buzzvm_pushf(vm, obst[4]); buzzvm_pushf(vm, obst[4]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_gstore(vm);*/ buzzvm_gstore(vm);*/
return vm->state; return vm->state;
} }
/**********************************************/ /**********************************************/
/*Dummy closure for use during update testing */ /*Dummy closure for use during update testing */
/**********************************************/ /**********************************************/
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);} int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
/***********************************************/ /***********************************************/
/* Store Ros controller object pointer */ /* Store Ros controller object pointer */
/***********************************************/ /***********************************************/
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){ //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
//roscontroller_ptr = roscontroller_ptrin; //roscontroller_ptr = roscontroller_ptrin;
//} //}
} }

2179
src/roscontroller.cpp
View File

File diff suppressed because it is too large Load Diff