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

tmp fix of spherical coord. bug

This commit is contained in:
David St-Onge 2017-01-26 17:42:30 -05:00
parent e9705ddb14
commit 91d68cd277
3 changed files with 34 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
include/roscontroller.h
View File

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

36
src/roscontroller.cpp
View File

@ -246,16 +246,32 @@ namespace rosbzz_node{
} }
/*convert from catresian to spherical coordinate system callback */ /*convert from spherical to cartesian coordinate system callback */
double* roscontroller::cvt_spherical_coordinates(double neighbours_pos_payload []){ double* roscontroller::cvt_cartesian_coordinates(double neighbours_pos_payload []){
double latitude,longitude,altitude; double latitude, longitude, rho;
latitude=neighbours_pos_payload[0]; latitude=neighbours_pos_payload[0];
longitude = neighbours_pos_payload[1]; longitude = neighbours_pos_payload[1];
altitude=neighbours_pos_payload[2]; rho=neighbours_pos_payload[2]+6371000; //centered on Earth
try { try {
neighbours_pos_payload[0]=sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0)); neighbours_pos_payload[0] = cos(latitude) * cos(longitude) * rho;
neighbours_pos_payload[1]=atan(longitude/latitude); neighbours_pos_payload[1] = cos(latitude) * sin(longitude) * rho;
neighbours_pos_payload[2]=atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude); neighbours_pos_payload[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 []){
double x, y, z;
x = neighbours_pos_payload[0];
y = neighbours_pos_payload[1];
z = neighbours_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);
} catch (std::overflow_error e) { } catch (std::overflow_error e) {
std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl; std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl;
} }
@ -303,9 +319,11 @@ namespace rosbzz_node{
memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t)); memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]); 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]; //cout<<"obt lat ,long alt"<<neighbours_pos_payload[0]<<neighbours_pos_payload[1]<<neighbours_pos_payload[2];
/*Convert obtained position to relative position*/ /*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);
for(i=0;i<3;i++){ for(i=0;i<3;i++){
neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[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 = cvt_spherical_coordinates(neighbours_pos_payload);
/*Extract robot id of the neighbour*/ /*Extract robot id of the neighbour*/

6
src/test1.bzz
View File

@ -2,11 +2,11 @@
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..." # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz" include "/home/ubuntu/buzz/src/include/vec2.bzz"
TARGET_ALTITUDE = 10.0 TARGET_ALTITUDE = 10.1
# Lennard-Jones parameters # Lennard-Jones parameters
TARGET = 0.000000100005 TARGET = 0.000001001
EPSILON = 0.1 EPSILON = 0.001
# Lennard-Jones interaction magnitude # Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) { function lj_magnitude(dist, target, epsilon) {