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

print tests

This commit is contained in:
David St-Onge 2017-01-26 20:11:22 -05:00
parent 641e07fe81
commit 07c22ebcd2
2 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/roscontroller.cpp
View File

@ -249,8 +249,8 @@ namespace rosbzz_node{
/*convert from spherical to cartesian coordinate system callback */
void roscontroller::cvt_cartesian_coordinates(double spherical_pos_payload [], double out[]){
double latitude, longitude, rho;
latitude = spherical_pos_payload[0] / 180 * 3.1416;
longitude = spherical_pos_payload[1] / 180 * 3.1416;
latitude = spherical_pos_payload[0] / 180.0 * 3.1416;
longitude = spherical_pos_payload[1] / 180.0 * 3.1416;
rho = spherical_pos_payload[2]+6371000; //centered on Earth
try {
out[0] = cos(latitude) * cos(longitude) * rho;
@ -315,7 +315,7 @@ namespace rosbzz_node{
double neighbours_pos_payload[3];
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]);
//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 CARTESIAN position*/
double cartesian_neighbours_pos[3], cartesian_cur_pos[3];
cvt_cartesian_coordinates(neighbours_pos_payload, cartesian_neighbours_pos);
@ -328,7 +328,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 " << out << ": " << 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[2] << ", "<< cvt_neighbours_pos_payload[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]);
/*Put RID and pos*/

1
src/test1.bzz
View File

@ -1,5 +1,6 @@
# 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"