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

obstacle subscriber and buzz object addition

This commit is contained in:
vivek-shankar 2017-02-10 14:18:44 -05:00
parent b9b845802b
commit b5c512142d
7 changed files with 57 additions and 360 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
include/buzzuav_closures.h
View File

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

8
include/roscontroller.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h"
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#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 <sensor_msgs/LaserScan.h>
#include <rosbuzz/neigh_pos.h>
#include <sstream>
#include <buzz/buzzasm.h>
@ -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);
};

BIN
src/.buzzuav_closures.cpp.swp
View File

Binary file not shown.

1
src/buzz_utility.cpp
View File

@ -555,6 +555,7 @@ 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){

35
src/buzzuav_closures.cpp
View File

@ -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*/
/***************************************/

355
src/buzzuav_closures.cpp.save
View File

@ -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 "<<endl;
}
}
/*convert from cartesian to spherical coordinate system callback */
void spherical_coordinates(double cartesian_pos_payload [], double out[]){
double x, y, z;
x = cartesian_pos_payload[0];
y = cartesian_pos_payload[1];
z = cartesian_pos_payload[2];
try {
out[2]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0))-EARTH_RADIUS;
out[1]=atan2(y,x)*180.0/M_PI;
out[0]=atan2(z,(sqrt(pow(x,2.0)+pow(y,2.0))))*180.0/M_PI;
} catch (std::overflow_error e) {
// std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl;
}
}
void gps_from_rb(double range, double bearing, double out[3]) {
double lat = cur_pos[0]*M_PI/180.0;
double lon = cur_pos[1]*M_PI/180.0;
// bearing = bearing*M_PI/180.0;
out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
out[0] = out[0]*180.0/M_PI;
out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
out[1] = out[1]*180.0/M_PI;
out[2] = height; //constant height.
}
double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677};
int buzzuav_goto(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2);
buzzvm_lload(vm, 1); /* dx */
buzzvm_lload(vm, 2); /* dy */
//buzzvm_lload(vm, 3); /* Latitude */
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float dy = buzzvm_stack_at(vm, 1)->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);}
}

9
src/roscontroller.cpp
View File

@ -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<mavros_msgs::Mavlink>(out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/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*/
/*******************************************************************************************************/