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

minor coord. fix

This commit is contained in:
Mario Alexis Labrana 2017-01-28 11:24:54 -05:00
parent 225d561606
commit a7140c5551
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
launch/rosbuzzm100.launch
View File

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

7
src/roscontroller.cpp
View File

@ -294,12 +294,13 @@ namespace rosbzz_node{
} }
#define EARTH_RADIUS 6371000.0
/*convert from spherical to cartesian coordinate system callback */ /*convert from spherical to cartesian coordinate system callback */
void roscontroller::cvt_cartesian_coordinates(double spherical_pos_payload [], double out[]){ void roscontroller::cvt_cartesian_coordinates(double spherical_pos_payload [], double out[]){
double latitude, longitude, rho; double latitude, longitude, rho;
latitude = spherical_pos_payload[0] / 180.0 * M_PI; latitude = spherical_pos_payload[0] / 180.0 * M_PI;
longitude = spherical_pos_payload[1] / 180.0 * M_PI; longitude = spherical_pos_payload[1] / 180.0 * M_PI;
rho = spherical_pos_payload[2]+6371000; //centered on Earth rho = spherical_pos_payload[2]+EARTH_RADIUS; //centered on Earth
try { try {
out[0] = cos(latitude) * cos(longitude) * rho; out[0] = cos(latitude) * cos(longitude) * rho;
out[1] = cos(latitude) * sin(longitude) * rho; out[1] = cos(latitude) * sin(longitude) * rho;
@ -412,8 +413,8 @@ namespace rosbzz_node{
z = cartesian_pos_payload[2]; z = cartesian_pos_payload[2];
try { try {
out[0]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0)); out[0]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0));
out[1]=atan(y/x); out[1]=atan2(y,x);
out[2]=atan((sqrt(pow(x,2.0)+pow(y,2.0)))/z); out[2]=atan2((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;
} }