Skip to content

Commit 38b9bdc

Browse files
authored
ROS2 Linting: raw_vehicle_cmd_converter (autowarefoundation#161)
* Add linters * Process linter corrections
1 parent 15205eb commit 38b9bdc

File tree

13 files changed

+84
-46
lines changed

13 files changed

+84
-46
lines changed

vehicle/raw_vehicle_cmd_converter/CMakeLists.txt

+12-2
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,11 @@ project(raw_vehicle_cmd_converter)
55
### Compile options
66
if(NOT CMAKE_CXX_STANDARD)
77
set(CMAKE_CXX_STANDARD 14)
8+
set(CMAKE_CXX_STANDARD_REQUIRED ON)
9+
set(CMAKE_CXX_EXTENSIONS OFF)
10+
endif()
11+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
12+
add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
813
endif()
914

1015
find_package(ament_cmake_auto REQUIRED)
@@ -15,17 +20,22 @@ ament_auto_add_library(accel_map_converter SHARED
1520
src/brake_map.cpp
1621
src/csv_loader.cpp
1722
src/interpolate.cpp
18-
)
23+
)
1924

2025
ament_auto_add_executable(raw_vehicle_cmd_converter_node
2126
src/node.cpp
2227
src/main.cpp
2328
)
29+
2430
add_dependencies(raw_vehicle_cmd_converter_node accel_map_converter)
2531
target_link_libraries(raw_vehicle_cmd_converter_node accel_map_converter)
2632

33+
if(BUILD_TESTING)
34+
find_package(ament_lint_auto REQUIRED)
35+
ament_lint_auto_find_test_dependencies()
36+
endif()
37+
2738
ament_auto_package(INSTALL_TO_SHARE
2839
launch
2940
data
3041
)
31-

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp

+9-9
Original file line numberDiff line numberDiff line change
@@ -12,23 +12,23 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef RAW_VEHICLE_CMD_CONVERTER_ACCEL_MAP_H
16-
#define RAW_VEHICLE_CMD_CONVERTER_ACCEL_MAP_H
17-
18-
#include "raw_vehicle_cmd_converter/csv_loader.hpp"
19-
#include "raw_vehicle_cmd_converter/interpolate.hpp"
20-
21-
#include "rclcpp/rclcpp.hpp"
15+
#ifndef RAW_VEHICLE_CMD_CONVERTER__ACCEL_MAP_HPP_
16+
#define RAW_VEHICLE_CMD_CONVERTER__ACCEL_MAP_HPP_
2217

2318
#include <algorithm>
2419
#include <iostream>
2520
#include <string>
2621
#include <vector>
2722

23+
#include "rclcpp/rclcpp.hpp"
24+
25+
#include "raw_vehicle_cmd_converter/csv_loader.hpp"
26+
#include "raw_vehicle_cmd_converter/interpolate.hpp"
27+
2828
class AccelMap
2929
{
3030
public:
31-
AccelMap(const rclcpp::Logger & logger);
31+
explicit AccelMap(const rclcpp::Logger & logger);
3232
~AccelMap();
3333

3434
bool readAccelMapFromCSV(std::string csv_path);
@@ -44,4 +44,4 @@ class AccelMap
4444
std::vector<std::vector<double>> accel_map_;
4545
};
4646

47-
#endif
47+
#endif // RAW_VEHICLE_CMD_CONVERTER__ACCEL_MAP_HPP_

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp

+9-9
Original file line numberDiff line numberDiff line change
@@ -12,23 +12,23 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef RAW_VEHICLE_CMD_CONVERTER_BRAKE_MAP_H
16-
#define RAW_VEHICLE_CMD_CONVERTER_BRAKE_MAP_H
17-
18-
#include "raw_vehicle_cmd_converter/csv_loader.hpp"
19-
#include "raw_vehicle_cmd_converter/interpolate.hpp"
20-
21-
#include "rclcpp/rclcpp.hpp"
15+
#ifndef RAW_VEHICLE_CMD_CONVERTER__BRAKE_MAP_HPP_
16+
#define RAW_VEHICLE_CMD_CONVERTER__BRAKE_MAP_HPP_
2217

2318
#include <algorithm>
2419
#include <iostream>
2520
#include <string>
2621
#include <vector>
2722

23+
#include "rclcpp/rclcpp.hpp"
24+
25+
#include "raw_vehicle_cmd_converter/csv_loader.hpp"
26+
#include "raw_vehicle_cmd_converter/interpolate.hpp"
27+
2828
class BrakeMap
2929
{
3030
public:
31-
BrakeMap(const rclcpp::Logger & logger);
31+
explicit BrakeMap(const rclcpp::Logger & logger);
3232
~BrakeMap();
3333

3434
bool readBrakeMapFromCSV(std::string csv_path);
@@ -45,4 +45,4 @@ class BrakeMap
4545
std::vector<std::vector<double>> brake_map_;
4646
};
4747

48-
#endif
48+
#endif // RAW_VEHICLE_CMD_CONVERTER__BRAKE_MAP_HPP_

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/csv_loader.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef RAW_VEHICLE_CMD_CONVERTER_CSV_LOADER_H
16-
#define RAW_VEHICLE_CMD_CONVERTER_CSV_LOADER_H
15+
#ifndef RAW_VEHICLE_CMD_CONVERTER__CSV_LOADER_HPP_
16+
#define RAW_VEHICLE_CMD_CONVERTER__CSV_LOADER_HPP_
1717

1818
#include <fstream>
1919
#include <iostream>
@@ -24,7 +24,7 @@
2424
class CSVLoader
2525
{
2626
public:
27-
CSVLoader(std::string csv_path);
27+
explicit CSVLoader(std::string csv_path);
2828
~CSVLoader();
2929

3030
bool readCSV(std::vector<std::vector<std::string>> & result, const char delim = ',');
@@ -33,4 +33,4 @@ class CSVLoader
3333
std::string csv_path_;
3434
};
3535

36-
#endif
36+
#endif // RAW_VEHICLE_CMD_CONVERTER__CSV_LOADER_HPP_

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/interpolate.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef RAW_VEHICLE_CMD_CONVERTER_INTERPOLATE_H
16-
#define RAW_VEHICLE_CMD_CONVERTER_INTERPOLATE_H
15+
#ifndef RAW_VEHICLE_CMD_CONVERTER__INTERPOLATE_HPP_
16+
#define RAW_VEHICLE_CMD_CONVERTER__INTERPOLATE_HPP_
1717

1818
#include <cmath>
1919
#include <iostream>
@@ -29,4 +29,4 @@ class LinearInterpolate
2929
const double & return_index, double & return_value);
3030
};
3131

32-
#endif
32+
#endif // RAW_VEHICLE_CMD_CONVERTER__INTERPOLATE_HPP_

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp

+18-11
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,11 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef VEHICLE_RAW_VEHICLE_CMD_CONVERTER_INCLUDE_RAW_VEHICLE_CMD_CONVERTER_NODE_HPP_
16-
#define VEHICLE_RAW_VEHICLE_CMD_CONVERTER_INCLUDE_RAW_VEHICLE_CMD_CONVERTER_NODE_HPP_
15+
#ifndef RAW_VEHICLE_CMD_CONVERTER__NODE_HPP_
16+
#define RAW_VEHICLE_CMD_CONVERTER__NODE_HPP_
17+
18+
#include <memory>
19+
#include <string>
1720

1821
#include "raw_vehicle_cmd_converter/accel_map.hpp"
1922
#include "raw_vehicle_cmd_converter/brake_map.hpp"
@@ -25,8 +28,6 @@
2528
#include "autoware_vehicle_msgs/msg/shift.hpp"
2629
#include "autoware_vehicle_msgs/msg/vehicle_command.hpp"
2730

28-
#include <memory>
29-
#include <string>
3031

3132
class AccelMapConverter : public rclcpp::Node
3233
{
@@ -35,17 +36,23 @@ class AccelMapConverter : public rclcpp::Node
3536
~AccelMapConverter() = default;
3637

3738
private:
38-
rclcpp::Publisher<autoware_vehicle_msgs::msg::RawVehicleCommand>::SharedPtr pub_cmd_; //!< @brief topic publisher for low-level vehicle command
39-
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_velocity_; //!< @brief subscriber for current velocity
40-
rclcpp::Subscription<autoware_vehicle_msgs::msg::VehicleCommand>::SharedPtr sub_cmd_; //!< @brief subscriber for vehicle command
39+
rclcpp::Publisher<autoware_vehicle_msgs::msg::RawVehicleCommand>::SharedPtr
40+
pub_cmd_; //!< @brief topic publisher for low-level vehicle command
41+
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr
42+
sub_velocity_; //!< @brief subscriber for current velocity
43+
rclcpp::Subscription<autoware_vehicle_msgs::msg::VehicleCommand>::SharedPtr
44+
sub_cmd_; //!< @brief subscriber for vehicle command
4145

4246
std::shared_ptr<double> current_velocity_ptr_; // [m/s]
4347

4448
AccelMap accel_map_;
4549
BrakeMap brake_map_;
46-
bool acc_map_initialized_; //!< @brief flag to manage validity of imported accel map files
47-
double max_throttle_; //!< @brief maximum throttle that can be passed to low level controller. In general [0.0, 1.0]
48-
double max_brake_; //!< @brief maximum brake value that can be passed to low level controller. In general [0.0, 1.0]
50+
//!< @brief flag to manage validity of imported accel map files
51+
bool acc_map_initialized_;
52+
//!< @brief maximum throttle that can be passed to low level controller. In general [0.0, 1.0]
53+
double max_throttle_;
54+
//!< @brief maximum brake value that can be passed to low level controller. In general [0.0, 1.0]
55+
double max_brake_;
4956

5057
void callbackVehicleCmd(
5158
const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr vehicle_cmd_ptr);
@@ -55,4 +62,4 @@ class AccelMapConverter : public rclcpp::Node
5562
double * desired_brake);
5663
};
5764

58-
#endif // VEHICLE_RAW_VEHICLE_CMD_CONVERTER_INCLUDE_RAW_VEHICLE_CMD_CONVERTER_NODE_HPP_
65+
#endif // RAW_VEHICLE_CMD_CONVERTER__NODE_HPP_

vehicle/raw_vehicle_cmd_converter/package.xml

+3
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,9 @@
1515
<depend>autoware_vehicle_msgs</depend>
1616
<depend>geometry_msgs</depend>
1717

18+
<test_depend>ament_lint_auto</test_depend>
19+
<test_depend>ament_lint_common</test_depend>
20+
1821
<export>
1922
<build_type>ament_cmake</build_type>
2023
</export>

vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp

+6-1
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,13 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "raw_vehicle_cmd_converter/accel_map.hpp"
15+
#include <algorithm>
1616
#include <chrono>
17+
#include <string>
18+
#include <vector>
19+
20+
#include "raw_vehicle_cmd_converter/accel_map.hpp"
21+
1722
using namespace std::chrono_literals;
1823

1924
AccelMap::AccelMap(const rclcpp::Logger & logger)

vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,10 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include <algorithm>
16+
#include <string>
17+
#include <vector>
18+
1519
#include "raw_vehicle_cmd_converter/brake_map.hpp"
1620

1721
BrakeMap::BrakeMap(const rclcpp::Logger & logger)

vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp

+3
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,9 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include <string>
16+
#include <vector>
17+
1518
#include "raw_vehicle_cmd_converter/csv_loader.hpp"
1619

1720
CSVLoader::CSVLoader(std::string csv_path) {csv_path_ = csv_path;}

vehicle/raw_vehicle_cmd_converter/src/interpolate.cpp

+7-5
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include <vector>
16+
1517
#include "raw_vehicle_cmd_converter/interpolate.hpp"
1618

1719
/*
@@ -23,7 +25,7 @@ bool LinearInterpolate::interpolate(
2325
const double & return_index, double & return_value)
2426
{
2527
auto isIncrease = [](const std::vector<double> & x) {
26-
for (int i = 0; i < (int)x.size() - 1; ++i) {
28+
for (size_t i = 0; i < x.size() - 1; ++i) {
2729
if (x[i] > x[i + 1]) {return false;}
2830
}
2931
return true;
@@ -51,12 +53,12 @@ bool LinearInterpolate::interpolate(
5153
printf(
5254
"base_index.size() = %lu, base_value.size() = %lu\n", base_index.size(), base_value.size());
5355
printf("base_index: [");
54-
for (int i = 0; i < base_index.size(); ++i) {
56+
for (size_t i = 0; i < base_index.size(); ++i) {
5557
printf("%f, ", base_index.at(i));
5658
}
5759
printf("]\n");
5860
printf("base_value: [");
59-
for (int i = 0; i < base_value.size(); ++i) {
61+
for (size_t i = 0; i < base_value.size(); ++i) {
6062
printf("%f, ", base_value.at(i));
6163
}
6264
printf("]\n");
@@ -65,15 +67,15 @@ bool LinearInterpolate::interpolate(
6567
}
6668

6769
// calculate linear interpolation
68-
int i = 0;
70+
size_t i = 0;
6971
if (base_index[i] == return_index) {
7072
return_value = base_value[i];
7173
return true;
7274
}
7375
while (base_index[i] < return_index) {
7476
++i;
7577
}
76-
if (i <= 0 || (int)base_index.size() - 1 < i) {
78+
if (i <= 0 || base_index.size() - 1 < i) {
7779
std::cerr << "? something wrong. skip this return_index." << std::endl;
7880
return false;
7981
}

vehicle/raw_vehicle_cmd_converter/src/main.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include <memory>
1516

1617
#include "raw_vehicle_cmd_converter/node.hpp"
1718

vehicle/raw_vehicle_cmd_converter/src/node.cpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -12,12 +12,15 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include <algorithm>
16+
#include <functional>
17+
#include <memory>
18+
#include <string>
19+
1520
#include "raw_vehicle_cmd_converter/node.hpp"
1621

1722
#include "rclcpp/logging.hpp"
1823

19-
#include <functional>
20-
2124
using std::placeholders::_1;
2225

2326
AccelMapConverter::AccelMapConverter()

0 commit comments

Comments
 (0)