diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index b334d53..b25032e 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -39,6 +39,9 @@ double* getgoto(); /* updates flight status*/ void flight_status_update(uint8_t state); +/*Flight status*/ +void set_obstacle_dist(float dist[]); + /* * Commands the UAV to takeoff */ @@ -60,6 +63,10 @@ int buzzuav_update_battery(buzzvm_t vm); * Updates current position in Buzz */ int buzzuav_update_currentpos(buzzvm_t vm); + +/*update obstacles in Buzz*/ +int buzzuav_update_obstacle(buzzvm_t vm); + /* * Updates flight status and rc command in Buzz, put it in a tabel to acess it * use flight.status for flight status diff --git a/include/roscontroller.h b/include/roscontroller.h index b2db0d8..0568521 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -1,6 +1,6 @@ #pragma once -#include "ros/ros.h" -#include "sensor_msgs/NavSatFix.h" +#include +#include #include "mavros_msgs/GlobalPositionTarget.h" #include "mavros_msgs/CommandCode.h" #include "mavros_msgs/CommandInt.h" @@ -9,6 +9,7 @@ #include "mavros_msgs/BatteryStatus.h" #include "mavros_msgs/Mavlink.h" #include "sensor_msgs/NavSatStatus.h" +#include #include #include #include @@ -61,6 +62,7 @@ private: ros::Subscriber battery_sub; ros::Subscriber payload_sub; ros::Subscriber flight_status_sub; + ros::Subscriber obstacle_sub; /*Commands for flight controller*/ mavros_msgs::CommandInt cmd_srv; @@ -121,7 +123,7 @@ private: bool rc_callback(mavros_msgs::CommandInt::Request &req, mavros_msgs::CommandInt::Response &res); - + void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg); }; diff --git a/src/.buzzuav_closures.cpp.swp b/src/.buzzuav_closures.cpp.swp deleted file mode 100644 index 7d1f48f..0000000 Binary files a/src/.buzzuav_closures.cpp.swp and /dev/null differ diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 931c1eb..1222866 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -555,7 +555,8 @@ int update_step_test(){ buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::buzzuav_update_flight_status(VM); - + buzzuav_closures::buzzuav_update_obstacle(VM); + int a = buzzvm_function_call(VM, "step", 0); if(a != BUZZVM_STATE_READY){ fprintf(stdout, "step test VM state %i\n",a); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 613f050..67509876 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -12,6 +12,7 @@ namespace buzzuav_closures{ static double goto_pos[3]; static double rc_goto_pos[3]; static float batt[3]; +static float obst[5]; static double cur_pos[3]; static uint8_t status; static int cur_cmd = 0; @@ -201,6 +202,12 @@ void rc_call(int rc_cmd_in){ rc_cmd=rc_cmd_in; } +void set_obstacle_dist(float dist[]){ + for(int i=0; i<5;i++) + dist[i]=obst[i]; +} + + /****************************************/ /****************************************/ @@ -284,6 +291,34 @@ int buzzuav_update_currentpos(buzzvm_t vm) { buzzvm_gstore(vm); return vm->state; } +/*obstacle*/ + +int buzzuav_update_obstacle(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "obstacle", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1)); + buzzvm_pushf(vm, obst[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1)); + buzzvm_pushf(vm, obst[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1)); + buzzvm_pushf(vm, obst[2]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1)); + buzzvm_pushf(vm, obst[3]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1)); + buzzvm_pushf(vm, obst[4]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} /****************************************/ /*flight status update*/ /***************************************/ diff --git a/src/buzzuav_closures.cpp.save b/src/buzzuav_closures.cpp.save deleted file mode 100644 index 9fe53eb..0000000 --- a/src/buzzuav_closures.cpp.save +++ /dev/null @@ -1,355 +0,0 @@ -/** @file buzzuav_closures.cpp - * @version 1.0 - * @date 27.09.2016 - * @brief Buzz Implementation as a node in ROS for Dji M100 Drone. - * @author Vivek Shankar Varadharajan - * @copyright 2016 MistLab. All rights reserved. - */ -//#define _GNU_SOURCE -#include "buzzuav_closures.h" -#include "buzz_utility.h" -namespace buzzuav_closures{ -static double goto_pos[3]; -static double rc_goto_pos[3]; -static float batt[3]; -static double cur_pos[3]; -static uint8_t status; -static int cur_cmd = 0; -static int rc_cmd=0; -static int buzz_cmd=0; -static float height=0; -/****************************************/ -/****************************************/ - -int buzzros_print(buzzvm_t vm) { - int i; - for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { - buzzvm_lload(vm, i); - buzzobj_t o = buzzvm_stack_at(vm, 1); - buzzvm_pop(vm); - switch(o->o.type) { - case BUZZTYPE_NIL: - ROS_INFO("BUZZ - [nil]"); - break; - case BUZZTYPE_INT: - ROS_INFO("%d", o->i.value); - //fprintf(stdout, "%d", o->i.value); - break; - case BUZZTYPE_FLOAT: - ROS_INFO("%f", o->f.value); - break; - case BUZZTYPE_TABLE: - ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value))); - break; - case BUZZTYPE_CLOSURE: - if(o->c.value.isnative) - ROS_INFO("[n-closure @%d]", o->c.value.ref); - else - ROS_INFO("[c-closure @%d]", o->c.value.ref); - break; - case BUZZTYPE_STRING: - ROS_INFO("%s", o->s.value.str); - break; - case BUZZTYPE_USERDATA: - ROS_INFO("[userdata @%p]", o->u.value); - break; - default: - break; - } - } - //fprintf(stdout, "\n"); -45.564489-73.56253745.56472945.564140-73.56233645.564362 return buzzvm_ret0(vm); -} - -/****************************************/ -/****************************************/ -#define EARTH_RADIUS 6371000.0 -/*convert from spherical to cartesian coordinate system callback */ - void cartesian_coordinates(double spherical_pos_payload [], double out[]){ - double latitude, longitude, rho; - latitude = spherical_pos_payload[0] / 180.0 * M_PI; - longitude = spherical_pos_payload[1] / 180.0 * M_PI; - rho = spherical_pos_payload[2]+EARTH_RADIUS; //centered on Earth - try { - out[0] = cos(latitude) * cos(longitude) * rho; - out[1] = cos(latitude) * sin(longitude) * rho; - out[2] = sin(latitude) * rho; // z is 'up' - } catch (std::overflow_error e) { -// std::cout << e.what() << " Error in convertion to cartesian coordinate system "<f.value; - float dx = buzzvm_stack_at(vm, 2)->f.value; -// float d = sqrt(dx*dx+dy*dy); -// float b = atan2(dy,dx); - printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); - //goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value; - //double cur_pos_cartesian[3]; - //cartesian_coordinates(cur_pos,cur_pos_cartesian); - //double goto_cartesian[3]; - //goto_cartesian[0] = dx + cur_pos_cartesian[0]; - //goto_cartesian[1] = dy + cur_pos_cartesian[1]; - //goto_cartesian[2] = cur_pos_cartesian[2]; - //spherical_coordinates(goto_cartesian, goto_pos); -// goto_pos[0]=dx; -// goto_pos[1]=dy; - //goto_pos[2]=height; // force a constant altitude to avoid loop increases -// gps_from_rb(d, b, goto_pos); - if(dx == 1.0){ - if((int)buzz_utility::get_robotid()==1){ - goto_pos[0]=hcpos1[0];goto_pos[1]=hcpos1[1];goto_pos[2]=height; - } - if((int)buzz_utility::get_robotid()==2){ - goto_pos[0]=hcpos2[0];goto_pos[1]=hcpos2[1];goto_pos[2]=height; - } - if((int)buzz_utility::get_robotid()==3){ - goto_pos[0]=hcpos3[0];goto_pos[1]=hcpos3[1];goto_pos[2]=height; - } - } - if(dx == 2.0){ - if((int)buzz_utility::get_robotid()==1){ - goto_pos[0]=hcpos1[2];goto_pos[1]=hcpos1[3];goto_pos[2]=height; - } - if((int)buzz_utility::get_robotid()==2){ - goto_pos[0]=hcpos2[2];goto_pos[1]=hcpos2[3];goto_pos[2]=height; - } - if((int)buzz_utility::get_robotid()==3){ - goto_pos[0]=hcpos3[2];goto_pos[1]=hcpos3[3];goto_pos[2]=height; - } - } - 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]); - buzz_cmd=2; - return buzzvm_ret0(vm); -} - -/******************************/ - -double* getgoto(){ -return goto_pos; -} -/******************************/ -int getcmd(){ - return cur_cmd; -} - -void set_goto(double pos[]){ -goto_pos[0]=pos[0]; -goto_pos[1]=pos[1]; -goto_pos[2]=pos[2]; - -} - -int bzz_cmd(){ -int cmd = buzz_cmd; -buzz_cmd=0; -return cmd; -} - -void rc_set_goto(double pos[]){ -rc_goto_pos[0]=pos[0]; -rc_goto_pos[1]=pos[1]; -rc_goto_pos[2]=pos[2]; - -} -void rc_call(int rc_cmd_in){ -rc_cmd=rc_cmd_in; -} - -/****************************************/ -/****************************************/ - -int buzzuav_takeoff(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 1); - buzzvm_lload(vm, 1); /* Altitude */ - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value; - height = goto_pos[2]; - cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; - printf(" Buzz requested Take off !!! \n"); - buzz_cmd=1; - return buzzvm_ret0(vm); -} - -int buzzuav_land(buzzvm_t vm) { - cur_cmd=mavros_msgs::CommandCode::NAV_LAND; - printf(" Buzz requested Land !!! \n"); - buzz_cmd=1; - return buzzvm_ret0(vm); -} - -int buzzuav_gohome(buzzvm_t vm) { - cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; - printf(" Buzz requested gohome !!! \n"); - buzz_cmd=1; - return buzzvm_ret0(vm); -} - -/****************************************/ -void set_battery(float voltage,float current,float remaining){ - batt[0]=voltage; - batt[1]=current; - batt[2]=remaining; -} -/****************************************/ - -int buzzuav_update_battery(buzzvm_t vm) { - //static char BATTERY_BUF[256]; - buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1)); - buzzvm_pushf(vm, batt[0]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1)); - buzzvm_pushf(vm, batt[1]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); - buzzvm_pushf(vm, batt[2]); - buzzvm_tput(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, "rc_cmd", 1)); - buzzvm_pushi(vm, rc_cmd); - buzzvm_tput(vm); - buzzvm_dup(vm); - rc_cmd=0; - buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1)); - buzzvm_pushi(vm, status); - buzzvm_tput(vm); - buzzvm_gstore(vm); - //also set rc_controllers goto - buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[0]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[1]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[2]); - buzzvm_tput(vm); - buzzvm_gstore(vm); - return vm->state; -} - - - -/****************************************/ -/*To do !!!!!*/ -/****************************************/ - -int buzzuav_update_prox(buzzvm_t vm) { - /* static char PROXIMITY_BUF[256]; - int i; - //kh4_proximity_ir(PROXIMITY_BUF, DSPIC); - buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity_ir", 1)); - buzzvm_pusht(vm); - for(i = 0; i < 8; i++) { - buzzvm_dup(vm); - buzzvm_pushi(vm, i+1); - buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8)); - buzzvm_tput(vm); - } - buzzvm_gstore(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "ground_ir", 1)); - buzzvm_pusht(vm); - for(i = 8; i < 12; i++) { - buzzvm_dup(vm); - buzzvm_pushi(vm, i-7); - buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8)); - buzzvm_tput(vm); - } - buzzvm_gstore(vm);*/ - return vm->state; -} - -/****************************************/ -/****************************************/ - -int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);} - -} - diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 16387be..aaab1c6 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -148,6 +148,7 @@ namespace rosbzz_node{ battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this); payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this); flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_status_update,this); + obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this); /*publishers*/ payload_pub = n_c.advertise(out_payload, 1000); neigh_pos_pub = n_c.advertise("/neighbours_pos",1000); @@ -493,7 +494,13 @@ namespace rosbzz_node{ set_cur_pos(msg->latitude,msg->longitude,msg->altitude); buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude); } - + /*Obstacle distance call back*/ + void roscontroller::obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg){ + float obst[5]; + for(int i=0;i<5;i++) + obst[i]=msg->ranges[i]; + buzzuav_closures::set_obstacle_dist(obst); + } /*payload callback callback*/ /*******************************************************************************************************/