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

Merge branch 'master' of https://github.com/MISTLab/ROSBuzz

This commit is contained in:
vivek-shankar 2017-01-31 01:12:28 -05:00
parent 6a4c579630 326c8e88ab
commit b472001974
4 changed files with 47 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/roscontroller.h
View File

@ -99,6 +99,9 @@ private:
/*convert from spherical to cartesian coordinate system callback */
void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
/*convert from spherical to cartesian coordinate system callback */
void cvt_rangebearingGB_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
/*convert from spherical to cartesian coordinate system callback */
void cvt_rangebearing2D_coordinates(double neighbours_pos_payload [], double out[], double pos[]);

35
src/buzzuav_closures.cpp
View File

@ -91,6 +91,18 @@ int buzzros_print(buzzvm_t vm) {
// 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.
}
int buzzuav_goto(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2);
buzzvm_lload(vm, 1); /* dx */
@ -99,20 +111,21 @@ int buzzuav_goto(buzzvm_t vm) {
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float dx = buzzvm_stack_at(vm, 1)->f.value;
float dy = buzzvm_stack_at(vm, 2)->f.value;
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
float d = buzzvm_stack_at(vm, 1)->f.value;
float b = buzzvm_stack_at(vm, 2)->f.value;
printf(" Vector for Goto: %.7f,%.7f\n",d,b);
//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);
//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
//goto_pos[2]=height; // force a constant altitude to avoid loop increases
gps_from_rb(d, b, goto_pos);
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;

23
src/roscontroller.cpp
View File

@ -13,7 +13,7 @@ namespace rosbzz_node{
/*Compile the .bzz file to .basm, .bo and .bdbg*/
Compile_bzz();
set_bzz_file(bzzfile_name.c_str());
/* double temp_gps[3]={45.564898,-73.563072,10.1};
double temp_gps[3]={45.564898,-73.563072,10.1};
double temp2_gps[3]={45.565246,-73.561951,10.1};
double temp_car[3], temp2_car[3];
double out[3], dif[3], out2[3];
@ -24,10 +24,10 @@ namespace rosbzz_node{
for(int i=0;i<3;i++)
dif[i]=temp2_car[i]-temp_car[i];
cvt_spherical_coordinates(dif,out);
cvt_rangebearing_coordinates(temp2_gps,out2,temp_gps);
cvt_rangebearingGB_coordinates(temp2_gps,out2,temp_gps);
// cvt_rangebearing2D_coordinates(temp2_gps,out2,temp_gps);
printf("TEST RESULT: %.7f,%.7f,%.7f\n",out[0],out[1],out[2]);
printf("TEST RESULT2: %.7f,%.7f,%.7f\n",out2[0],out2[1],out2[2]);*/
printf("TEST RESULT2: %.7f,%.7f,%.7f\n",out2[0],out2[1],out2[2]);
}
/***Destructor***/
@ -323,6 +323,19 @@ namespace rosbzz_node{
#define EARTH_RADIUS 6371000.0
#define LAT_AVERAGE 45.564898
/*rectangular projection range and bearing coordinate system callback */
void roscontroller::cvt_rangebearingGB_coordinates(double nei[], double out[], double cur[]){
double lat1 = cur[0]*M_PI/180.0;
double lon1 = cur[1]*M_PI/180.0;
double lat2 = nei[0]*M_PI/180.0;
double lon2 = nei[1]*M_PI/180.0;
out[0] = acos(sin(lat1) * sin(lat2) +cos(lat2) * cos(lat1)*cos(lon2-lon1))*EARTH_RADIUS;
double y = sin(lon1-lon2)*cos(lat1);
double x = cos(lat2)*sin(lat1) - sin(lat2)*cos(lat1)*cos(lon1-lon2);
out[1] = atan2(y,x)+M_PI;
out[2] = 0.0;
}
/*rectangular projection range and bearing coordinate system callback */
void roscontroller::cvt_rangebearing2D_coordinates(double spherical_pos_payload [], double out[], double cur[]){
double nei_lat = spherical_pos_payload[0] / 180.0 * M_PI;
@ -537,7 +550,7 @@ namespace rosbzz_node{
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]);
// cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl;
double cvt_neighbours_pos_test[3];
cvt_rangebearing_coordinates(neighbours_pos_payload, cvt_neighbours_pos_test, cur_pos);
cvt_rangebearingGB_coordinates(neighbours_pos_payload, cvt_neighbours_pos_test, cur_pos);
/*Convert obtained position to relative CARTESIAN position*/
double cartesian_neighbours_pos[3], cartesian_cur_pos[3];
cvt_cartesian_coordinates(neighbours_pos_payload, cartesian_neighbours_pos);
@ -550,7 +563,7 @@ namespace rosbzz_node{
// cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[2] << ", "<< cvt_neighbours_pos_payload[3] << endl;
cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;
// cout << "Rel Test Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_test[0] << ", "<< cvt_neighbours_pos_test[2] << ", "<< cvt_neighbours_pos_test[3] << endl;
/*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);

4
src/test1.bzz
View File

@ -13,11 +13,11 @@ function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 10.01
TARGET_ALTITUDE = 10.0
# Lennard-Jones parameters
TARGET = 50.0 #0.000001001
EPSILON = 80.0 #0.001
EPSILON = 200.0 #0.001
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {