Skip to content

Commit

Permalink
ROS2 Linting: geo_pos_conv (autowarefoundation#165)
Browse files Browse the repository at this point in the history 
* Add linters and fix linter issues
 - convert Cmake to use ament_cmake_auto

* Address PR comments:
 - Remove unneeded cast
 - Throw error instead of setting default values to zero
  • Loading branch information
@jilaada
jilaada authored Dec 14, 2020
1 parent 92b19b0 commit ec406c3
Show file tree
Hide file tree
Showing 4 changed files with 53 additions and 70 deletions.
40 changes: 10 additions & 30 deletions sensing/preprocessor/gnss/geo_pos_conv/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,45 +4,25 @@ project(geo_pos_conv)
### Compile options
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wno-unused-parameter -Wall -Wextra -Wpedantic)
endif()

### Add dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

### Create library
add_library(geo_pos_conv
ament_auto_add_library(geo_pos_conv
src/geo_pos_conv.cpp
)

target_include_directories(geo_pos_conv
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)

ament_target_dependencies(geo_pos_conv rclcpp)

### Install library
ament_export_targets(geo_pos_conv HAS_LIBRARY_TARGET)
ament_export_dependencies(rclcpp)

install(
DIRECTORY include/
DESTINATION include
)

install(
TARGETS geo_pos_conv
EXPORT geo_pos_conv
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
INCLUDES DESTINATION include
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_package()
ament_auto_package()
6 changes: 3 additions & 3 deletions sensing/preprocessor/gnss/geo_pos_conv/include/geo_pos_conv/geo_pos_conv.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef __GEO_POS_CONV__
#define __GEO_POS_CONV__
#ifndef GEO_POS_CONV__GEO_POS_CONV_HPP_
#define GEO_POS_CONV__GEO_POS_CONV_HPP_

#include <math.h>

Expand Down Expand Up @@ -53,4 +53,4 @@ class geo_pos_conv
void conv_xyz2llh(void);
};

#endif
#endif // GEO_POS_CONV__GEO_POS_CONV_HPP_
10 changes: 4 additions & 6 deletions sensing/preprocessor/gnss/geo_pos_conv/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,16 +7,14 @@
<maintainer email="ryo.watanabe@tier4.jp">Ryo Watanabe</maintainer>
<maintainer email="tomo@axe.bz">Tomomi HORII</maintainer>
<!-- ROS2 -->
<maintainer email="jilada.eccleston@tier4.jp">Jilada ECCLESTON</maintainer>
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_auto</buildtool_depend>

<build_depend>rclcpp</build_depend>
<build_depend>rclcpp_components</build_depend>
<depend>rclcpp</depend>

<exec_depend>rclcpp</exec_depend>
<exec_depend>rclcpp_components</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
67 changes: 36 additions & 31 deletions sensing/preprocessor/gnss/geo_pos_conv/src/geo_pos_conv.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,11 +145,16 @@ void geo_pos_conv::set_plane(int num)
lon_min = 0;
lat_deg = 154;
lat_min = 0;
} else {
lon_deg = 0;
lon_min = 0;
lat_deg = 0;
lat_min = 0;
}

// swap longitude and latitude
m_PLo = M_PI * ((double)lat_deg + (double)lat_min / 60.0) / 180.0;
m_PLato = M_PI * ((double)lon_deg + (double)lon_min / 60.0) / 180;
m_PLo = M_PI * (lat_deg + lat_min / 60.0) / 180.0;
m_PLato = M_PI * (lon_deg + lon_min / 60.0) / 180;
}

void geo_pos_conv::set_xyz(double cx, double cy, double cz)
Expand Down Expand Up @@ -209,68 +214,68 @@ void geo_pos_conv::conv_llh2xyz(void)
AW = 6378137.0; // Semimajor Axis
FW = 1.0 / 298.257222101; // 298.257223563 //Geometrical flattening

Pe = (double)sqrt(2.0 * FW - pow(FW, 2));
Pet = (double)sqrt(pow(Pe, 2) / (1.0 - pow(Pe, 2)));
Pe = sqrt(2.0 * FW - pow(FW, 2));
Pet = sqrt(pow(Pe, 2) / (1.0 - pow(Pe, 2)));

PA = (double)1.0 + 3.0 / 4.0 * pow(Pe, 2) + 45.0 / 64.0 * pow(Pe, 4) +
PA = 1.0 + 3.0 / 4.0 * pow(Pe, 2) + 45.0 / 64.0 * pow(Pe, 4) +
175.0 / 256.0 * pow(Pe, 6) + 11025.0 / 16384.0 * pow(Pe, 8) +
43659.0 / 65536.0 * pow(Pe, 10) + 693693.0 / 1048576.0 * pow(Pe, 12) +
19324305.0 / 29360128.0 * pow(Pe, 14) + 4927697775.0 / 7516192768.0 * pow(Pe, 16);

PB = (double)3.0 / 4.0 * pow(Pe, 2) + 15.0 / 16.0 * pow(Pe, 4) + 525.0 / 512.0 * pow(Pe, 6) +
PB = 3.0 / 4.0 * pow(Pe, 2) + 15.0 / 16.0 * pow(Pe, 4) + 525.0 / 512.0 * pow(Pe, 6) +
2205.0 / 2048.0 * pow(Pe, 8) + 72765.0 / 65536.0 * pow(Pe, 10) +
297297.0 / 262144.0 * pow(Pe, 12) + 135270135.0 / 117440512.0 * pow(Pe, 14) +
547521975.0 / 469762048.0 * pow(Pe, 16);

PC = (double)15.0 / 64.0 * pow(Pe, 4) + 105.0 / 256.0 * pow(Pe, 6) +
PC = 15.0 / 64.0 * pow(Pe, 4) + 105.0 / 256.0 * pow(Pe, 6) +
2205.0 / 4096.0 * pow(Pe, 8) + 10395.0 / 16384.0 * pow(Pe, 10) +
1486485.0 / 2097152.0 * pow(Pe, 12) + 45090045.0 / 58720256.0 * pow(Pe, 14) +
766530765.0 / 939524096.0 * pow(Pe, 16);

PD = (double)35.0 / 512.0 * pow(Pe, 6) + 315.0 / 2048.0 * pow(Pe, 8) +
PD = 35.0 / 512.0 * pow(Pe, 6) + 315.0 / 2048.0 * pow(Pe, 8) +
31185.0 / 131072.0 * pow(Pe, 10) + 165165.0 / 524288.0 * pow(Pe, 12) +
45090045.0 / 117440512.0 * pow(Pe, 14) + 209053845.0 / 469762048.0 * pow(Pe, 16);

PE = (double)315.0 / 16384.0 * pow(Pe, 8) + 3465.0 / 65536.0 * pow(Pe, 10) +
PE = 315.0 / 16384.0 * pow(Pe, 8) + 3465.0 / 65536.0 * pow(Pe, 10) +
99099.0 / 1048576.0 * pow(Pe, 12) + 4099095.0 / 29360128.0 * pow(Pe, 14) +
348423075.0 / 1879048192.0 * pow(Pe, 16);

PF = (double)693.0 / 131072.0 * pow(Pe, 10) + 9009.0 / 524288.0 * pow(Pe, 12) +
PF = 693.0 / 131072.0 * pow(Pe, 10) + 9009.0 / 524288.0 * pow(Pe, 12) +
4099095.0 / 117440512.0 * pow(Pe, 14) + 26801775.0 / 469762048.0 * pow(Pe, 16);

PG = (double)3003.0 / 2097152.0 * pow(Pe, 12) + 315315.0 / 58720256.0 * pow(Pe, 14) +
PG = 3003.0 / 2097152.0 * pow(Pe, 12) + 315315.0 / 58720256.0 * pow(Pe, 14) +
11486475.0 / 939524096.0 * pow(Pe, 16);

PH = (double)45045.0 / 117440512.0 * pow(Pe, 14) + 765765.0 / 469762048.0 * pow(Pe, 16);
PH = 45045.0 / 117440512.0 * pow(Pe, 14) + 765765.0 / 469762048.0 * pow(Pe, 16);

PI = (double)765765.0 / 7516192768.0 * pow(Pe, 16);
PI = 765765.0 / 7516192768.0 * pow(Pe, 16);

PB1 = (double)AW * (1.0 - pow(Pe, 2)) * PA;
PB2 = (double)AW * (1.0 - pow(Pe, 2)) * PB / -2.0;
PB3 = (double)AW * (1.0 - pow(Pe, 2)) * PC / 4.0;
PB4 = (double)AW * (1.0 - pow(Pe, 2)) * PD / -6.0;
PB5 = (double)AW * (1.0 - pow(Pe, 2)) * PE / 8.0;
PB6 = (double)AW * (1.0 - pow(Pe, 2)) * PF / -10.0;
PB7 = (double)AW * (1.0 - pow(Pe, 2)) * PG / 12.0;
PB8 = (double)AW * (1.0 - pow(Pe, 2)) * PH / -14.0;
PB9 = (double)AW * (1.0 - pow(Pe, 2)) * PI / 16.0;
PB1 = AW * (1.0 - pow(Pe, 2)) * PA;
PB2 = AW * (1.0 - pow(Pe, 2)) * PB / -2.0;
PB3 = AW * (1.0 - pow(Pe, 2)) * PC / 4.0;
PB4 = AW * (1.0 - pow(Pe, 2)) * PD / -6.0;
PB5 = AW * (1.0 - pow(Pe, 2)) * PE / 8.0;
PB6 = AW * (1.0 - pow(Pe, 2)) * PF / -10.0;
PB7 = AW * (1.0 - pow(Pe, 2)) * PG / 12.0;
PB8 = AW * (1.0 - pow(Pe, 2)) * PH / -14.0;
PB9 = AW * (1.0 - pow(Pe, 2)) * PI / 16.0;

PS = (double)PB1 * m_lat + PB2 * sin(2.0 * m_lat) + PB3 * sin(4.0 * m_lat) +
PS = PB1 * m_lat + PB2 * sin(2.0 * m_lat) + PB3 * sin(4.0 * m_lat) +
PB4 * sin(6.0 * m_lat) + PB5 * sin(8.0 * m_lat) + PB6 * sin(10.0 * m_lat) +
PB7 * sin(12.0 * m_lat) + PB8 * sin(14.0 * m_lat) + PB9 * sin(16.0 * m_lat);

PSo = (double)PB1 * m_PLato + PB2 * sin(2.0 * m_PLato) + PB3 * sin(4.0 * m_PLato) +
PSo = PB1 * m_PLato + PB2 * sin(2.0 * m_PLato) + PB3 * sin(4.0 * m_PLato) +
PB4 * sin(6.0 * m_PLato) + PB5 * sin(8.0 * m_PLato) + PB6 * sin(10.0 * m_PLato) +
PB7 * sin(12.0 * m_PLato) + PB8 * sin(14.0 * m_PLato) + PB9 * sin(16.0 * m_PLato);

PDL = (double)m_lon - m_PLo;
Pt = (double)tan(m_lat);
PW = (double)sqrt(1.0 - pow(Pe, 2) * pow(sin(m_lat), 2));
PN = (double)AW / PW;
Pnn = (double)sqrt(pow(Pet, 2) * pow(cos(m_lat), 2));
PDL = m_lon - m_PLo;
Pt = tan(m_lat);
PW = sqrt(1.0 - pow(Pe, 2) * pow(sin(m_lat), 2));
PN = AW / PW;
Pnn = sqrt(pow(Pet, 2) * pow(cos(m_lat), 2));

m_x =
(double)((PS - PSo) + (1.0 / 2.0) * PN *
((PS - PSo) + (1.0 / 2.0) * PN *
pow(
cos(m_lat),
2.0) * Pt *
Expand Down Expand Up @@ -321,7 +326,7 @@ void geo_pos_conv::conv_llh2xyz(void)
Pmo;

m_y =
(double)(PN * cos(m_lat) * PDL - 1.0 / 6.0 * PN *
(PN * cos(m_lat) * PDL - 1.0 / 6.0 * PN *
pow(
cos(m_lat),
3) *
Expand Down

0 comments on commit ec406c3

Please sign in to comment.