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

more coord. fixes

This commit is contained in:
David St-Onge 2017-01-26 19:23:33 -05:00
parent 91d68cd277
commit 1ac062ea3d
5 changed files with 39 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/roscontroller.h
View File

@ -86,10 +86,10 @@ private:
double longitude,
double altitude);
/*convert from cartesian to spherical coordinate system callback */
double* cvt_spherical_coordinates(double neighbours_pos_payload []);
void cvt_spherical_coordinates(double neighbours_pos_payload [], double out[]);
/*convert from spherical to cartesian coordinate system callback */
double* cvt_cartesian_coordinates(double neighbours_pos_payload []);
void cvt_cartesian_coordinates(double neighbours_pos_payload [], double out[]);
/*battery status callback*/
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);

2
launch/rosbuzzm100.launch
View File

@ -2,7 +2,7 @@
<launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="../djiros_ws/src/ROSBuzz/src/test1.bzz" />
<param name="bzzfile_name" value="../ROS_WS/src/ROSBuzz/src/test1.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<param name="fcclient_name" value="/dji_mavcmd" />

21
src/buzzuav_closures.cpp
View File

@ -63,16 +63,21 @@ int buzzros_print(buzzvm_t vm) {
/****************************************/
int buzzuav_goto(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* Altitude */
buzzvm_lload(vm, 2); /* Longitude */
buzzvm_lload(vm, 3); /* Latitude */
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
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);
goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value;
goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value;
goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
float dy = buzzvm_stack_at(vm, 1)->f.value;
float dx = buzzvm_stack_at(vm, 2)->f.value;
//goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
double dlat=dx/(6371000+cur_pos[2]);
double dlon=dy/cos(cur_pos[0]/180*3.1416)/(6371000+cur_pos[2]);
goto_pos[0]=cur_pos[0]+dlat*180/3.1416;
goto_pos[1]=cur_pos[1]+dlon*180/3.1416;
goto_pos[2]=cur_pos[2];
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
printf(" Buzz requested Go To, to Latitude: %15f , Longitude: %15f, Altitude: %15f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
buzz_cmd=2;

42
src/roscontroller.cpp
View File

@ -247,36 +247,33 @@ namespace rosbzz_node{
}
/*convert from spherical to cartesian coordinate system callback */
double* roscontroller::cvt_cartesian_coordinates(double neighbours_pos_payload []){
void roscontroller::cvt_cartesian_coordinates(double spherical_pos_payload [], double out[]){
double latitude, longitude, rho;
latitude=neighbours_pos_payload[0];
longitude = neighbours_pos_payload[1];
rho=neighbours_pos_payload[2]+6371000; //centered on Earth
latitude = spherical_pos_payload[0] / 180 * 3.1416;
longitude = spherical_pos_payload[1] / 180 * 3.1416;
rho = spherical_pos_payload[2]+6371000; //centered on Earth
try {
neighbours_pos_payload[0] = cos(latitude) * cos(longitude) * rho;
neighbours_pos_payload[1] = cos(latitude) * sin(longitude) * rho;
neighbours_pos_payload[2] = sin(latitude) * rho; // z is 'up'
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;
}
return neighbours_pos_payload;
}
/*convert from cartesian to spherical coordinate system callback */
double* roscontroller::cvt_spherical_coordinates(double neighbours_pos_payload []){
void roscontroller::cvt_spherical_coordinates(double cartesian_pos_payload [], double out[]){
double x, y, z;
x = neighbours_pos_payload[0];
y = neighbours_pos_payload[1];
z = neighbours_pos_payload[2];
x = cartesian_pos_payload[0];
y = cartesian_pos_payload[1];
z = cartesian_pos_payload[2];
try {
neighbours_pos_payload[0]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0));
neighbours_pos_payload[1]=atan(y/x);
neighbours_pos_payload[2]=atan((sqrt(pow(x,2.0)+pow(y,2.0)))/z);
out[0]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0));
out[1]=atan(y/x);
out[2]=atan((sqrt(pow(x,2.0)+pow(y,2.0)))/z);
} catch (std::overflow_error e) {
std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl;
}
return neighbours_pos_payload;
}
/*battery status callback*/
@ -320,12 +317,15 @@ namespace rosbzz_node{
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]);
//cout<<"obt lat ,long alt"<<neighbours_pos_payload[0]<<neighbours_pos_payload[1]<<neighbours_pos_payload[2];
/*Convert obtained position to relative CARTESIAN position*/
double* cartesian_neighbours_pos = cvt_cartesian_coordinates(neighbours_pos_payload);
double* cartesian_cur_pos = cvt_cartesian_coordinates(cur_pos);
double cartesian_neighbours_pos[3], cartesian_cur_pos[3];
cvt_cartesian_coordinates(neighbours_pos_payload, cartesian_neighbours_pos);
cvt_cartesian_coordinates(cur_pos, cartesian_cur_pos);
/*Compute the relative position*/
for(i=0;i<3;i++){
neighbours_pos_payload[i]=cartesian_neighbours_pos[i]-cartesian_cur_pos[i];
}
double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload);
double cvt_neighbours_pos_payload[3];
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));
/*pass neighbour position to local maintaner*/

2
src/test1.bzz
View File

@ -1,3 +1,5 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"