Skip to content

Commit 7a74217

Browse files
rubengrandiarubengrandia
committed
organize includes, run clang-format, fix simple clang-tidy complaints
1 parent e23d384 commit 7a74217

File tree

5 files changed

+342
-375
lines changed

5 files changed

+342
-375
lines changed

‎elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp‎

Lines changed: 104 additions & 98 deletions
Original file line numberDiff line numberDiff line change
@@ -5,125 +5,131 @@
55

66
#pragma once
77

8-
#include <pybind11_catkin/pybind11/embed.h> // everything needed for embedding
8+
// STL
99
#include <iostream>
10+
11+
// Boost
12+
#include <boost/thread/recursive_mutex.hpp>
13+
14+
// Eigen
1015
#include <Eigen/Dense>
1116

17+
// Pybind
18+
#include <pybind11_catkin/pybind11/embed.h> // everything needed for embedding
19+
20+
// ROS
21+
#include <geometry_msgs/PolygonStamped.h>
1222
#include <ros/ros.h>
1323
#include <sensor_msgs/PointCloud2.h>
14-
#include <geometry_msgs/PolygonStamped.h>
1524
#include <std_srvs/Empty.h>
1625
#include <std_srvs/SetBool.h>
26+
#include <tf/transform_broadcaster.h>
1727
#include <tf/transform_listener.h>
1828
#include <visualization_msgs/Marker.h>
1929
#include <visualization_msgs/MarkerArray.h>
20-
#include <tf/transform_broadcaster.h>
30+
2131
// Grid Map
22-
#include <grid_map_ros/grid_map_ros.hpp>
23-
#include <grid_map_msgs/GridMap.h>
2432
#include <grid_map_msgs/GetGridMap.h>
25-
#include <elevation_map_msgs/CheckSafety.h>
26-
#include <elevation_map_msgs/Initialize.h>
33+
#include <grid_map_msgs/GridMap.h>
34+
#include <grid_map_ros/grid_map_ros.hpp>
35+
2736
// PCL
28-
#include <pcl/point_types.h>
2937
#include <pcl/PCLPointCloud2.h>
38+
#include <pcl/point_types.h>
3039
#include <pcl_conversions/pcl_conversions.h>
31-
#include <boost/thread/recursive_mutex.hpp>
40+
41+
#include <elevation_map_msgs/CheckSafety.h>
42+
#include <elevation_map_msgs/Initialize.h>
3243

3344
#include "elevation_mapping_cupy/elevation_mapping_wrapper.hpp"
3445

3546
namespace py = pybind11;
3647

37-
38-
namespace elevation_mapping_cupy{
39-
48+
namespace elevation_mapping_cupy {
4049

4150
class ElevationMappingNode {
42-
public:
43-
ElevationMappingNode(ros::NodeHandle& nh);
44-
~ElevationMappingNode() = default;
45-
46-
private:
47-
void readParameters();
48-
void setupMapPublishers();
49-
void pointcloudCallback(const sensor_msgs::PointCloud2& cloud);
50-
void publishAsPointCloud();
51-
bool getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response);
52-
bool checkSafety(elevation_map_msgs::CheckSafety::Request& request,
53-
elevation_map_msgs::CheckSafety::Response& response);
54-
bool initializeMap(elevation_map_msgs::Initialize::Request& request,
55-
elevation_map_msgs::Initialize::Response& response);
56-
bool clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
57-
bool clearMapWithInitializer(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
58-
bool setPublishPoint(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response);
59-
void updatePose(const ros::TimerEvent&);
60-
void updateVariance(const ros::TimerEvent&);
61-
void updateTime(const ros::TimerEvent&);
62-
void updateGridMap(const ros::TimerEvent&);
63-
void publishNormalAsArrow(const grid_map::GridMap& map);
64-
void initializeWithTF();
65-
void publishMapToOdom(double error);
66-
void publishStatistics(const ros::TimerEvent&);
67-
void publishMapOfIndex(int index);
68-
69-
visualization_msgs::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id);
70-
ros::NodeHandle nh_;
71-
std::vector<ros::Subscriber> pointcloudSubs_;
72-
std::vector<ros::Publisher> mapPubs_;
73-
ros::Publisher alivePub_;
74-
ros::Publisher pointPub_;
75-
ros::Publisher normalPub_;
76-
ros::Publisher statisticsPub_;
77-
ros::ServiceServer rawSubmapService_;
78-
ros::ServiceServer clearMapService_;
79-
ros::ServiceServer clearMapWithInitializerService_;
80-
ros::ServiceServer initializeMapService_;
81-
ros::ServiceServer setPublishPointService_;
82-
ros::ServiceServer checkSafetyService_;
83-
ros::Timer updateVarianceTimer_;
84-
ros::Timer updateTimeTimer_;
85-
ros::Timer updatePoseTimer_;
86-
ros::Timer updateGridMapTimer_;
87-
ros::Timer publishStatisticsTimer_;
88-
ros::Time lastStatisticsPublishedTime_;
89-
tf::TransformListener transformListener_;
90-
ElevationMappingWrapper map_;
91-
std::string mapFrameId_;
92-
std::string correctedMapFrameId_;
93-
std::string baseFrameId_;
94-
grid_map::GridMap gridMap_;
95-
96-
// map topics info
97-
std::vector<std::vector<std::string>> map_topics_;
98-
std::vector<std::vector<std::string>> map_layers_;
99-
std::vector<std::vector<std::string>> map_basic_layers_;
100-
std::set<std::string> map_layers_all_;
101-
std::set<std::string> map_layers_sync_;
102-
std::vector<double> map_fps_;
103-
std::set<double> map_fps_unique_;
104-
std::vector<ros::Timer> mapTimers_;
105-
106-
std::vector<std::string> initialize_frame_id_;
107-
std::vector<double> initialize_tf_offset_;
108-
std::string initializeMethod_;
109-
110-
Eigen::Vector3d lowpassPosition_;
111-
Eigen::Vector4d lowpassOrientation_;
112-
113-
boost::recursive_mutex mapMutex_;
114-
115-
double positionError_;
116-
double orientationError_;
117-
double positionAlpha_;
118-
double orientationAlpha_;
119-
double recordableFps_;
120-
bool enablePointCloudPublishing_;
121-
bool enableNormalArrowPublishing_;
122-
bool enableDriftCorrectedTFPublishing_;
123-
bool useInitializerAtStart_;
124-
bool isGridmapUpdated_;
125-
double initializeTfGridSize_;
126-
int pointCloudProcessCounter_;
51+
public:
52+
ElevationMappingNode(ros::NodeHandle& nh);
53+
54+
private:
55+
void readParameters();
56+
void setupMapPublishers();
57+
void pointcloudCallback(const sensor_msgs::PointCloud2& cloud);
58+
void publishAsPointCloud();
59+
bool getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response);
60+
bool checkSafety(elevation_map_msgs::CheckSafety::Request& request, elevation_map_msgs::CheckSafety::Response& response);
61+
bool initializeMap(elevation_map_msgs::Initialize::Request& request, elevation_map_msgs::Initialize::Response& response);
62+
bool clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
63+
bool clearMapWithInitializer(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
64+
bool setPublishPoint(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response);
65+
void updatePose(const ros::TimerEvent&);
66+
void updateVariance(const ros::TimerEvent&);
67+
void updateTime(const ros::TimerEvent&);
68+
void updateGridMap(const ros::TimerEvent&);
69+
void publishNormalAsArrow(const grid_map::GridMap& map);
70+
void initializeWithTF();
71+
void publishMapToOdom(double error);
72+
void publishStatistics(const ros::TimerEvent&);
73+
void publishMapOfIndex(int index);
74+
75+
visualization_msgs::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id);
76+
ros::NodeHandle nh_;
77+
std::vector<ros::Subscriber> pointcloudSubs_;
78+
std::vector<ros::Publisher> mapPubs_;
79+
ros::Publisher alivePub_;
80+
ros::Publisher pointPub_;
81+
ros::Publisher normalPub_;
82+
ros::Publisher statisticsPub_;
83+
ros::ServiceServer rawSubmapService_;
84+
ros::ServiceServer clearMapService_;
85+
ros::ServiceServer clearMapWithInitializerService_;
86+
ros::ServiceServer initializeMapService_;
87+
ros::ServiceServer setPublishPointService_;
88+
ros::ServiceServer checkSafetyService_;
89+
ros::Timer updateVarianceTimer_;
90+
ros::Timer updateTimeTimer_;
91+
ros::Timer updatePoseTimer_;
92+
ros::Timer updateGridMapTimer_;
93+
ros::Timer publishStatisticsTimer_;
94+
ros::Time lastStatisticsPublishedTime_;
95+
tf::TransformListener transformListener_;
96+
ElevationMappingWrapper map_;
97+
std::string mapFrameId_;
98+
std::string correctedMapFrameId_;
99+
std::string baseFrameId_;
100+
grid_map::GridMap gridMap_;
101+
102+
// map topics info
103+
std::vector<std::vector<std::string>> map_topics_;
104+
std::vector<std::vector<std::string>> map_layers_;
105+
std::vector<std::vector<std::string>> map_basic_layers_;
106+
std::set<std::string> map_layers_all_;
107+
std::set<std::string> map_layers_sync_;
108+
std::vector<double> map_fps_;
109+
std::set<double> map_fps_unique_;
110+
std::vector<ros::Timer> mapTimers_;
111+
112+
std::vector<std::string> initialize_frame_id_;
113+
std::vector<double> initialize_tf_offset_;
114+
std::string initializeMethod_;
115+
116+
Eigen::Vector3d lowpassPosition_;
117+
Eigen::Vector4d lowpassOrientation_;
118+
119+
boost::recursive_mutex mapMutex_;
120+
121+
double positionError_;
122+
double orientationError_;
123+
double positionAlpha_;
124+
double orientationAlpha_;
125+
double recordableFps_;
126+
bool enablePointCloudPublishing_;
127+
bool enableNormalArrowPublishing_;
128+
bool enableDriftCorrectedTFPublishing_;
129+
bool useInitializerAtStart_;
130+
bool isGridmapUpdated_;
131+
double initializeTfGridSize_;
132+
int pointCloudProcessCounter_;
127133
};
128134

129-
}
135+
} // namespace elevation_mapping_cupy

‎elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp‎

Lines changed: 46 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -5,61 +5,70 @@
55

66
#pragma once
77

8-
#include <pybind11_catkin/pybind11/embed.h> // everything needed for embedding
8+
// STL
99
#include <iostream>
10+
11+
// Eigen
1012
#include <Eigen/Dense>
1113

14+
// Pybind
15+
#include <pybind11_catkin/pybind11/embed.h> // everything needed for embedding
16+
17+
// ROS
18+
#include <geometry_msgs/PoseWithCovarianceStamped.h>
1219
#include <ros/ros.h>
1320
#include <sensor_msgs/PointCloud2.h>
14-
#include <geometry_msgs/PoseWithCovarianceStamped.h>
1521
#include <std_srvs/Empty.h>
1622
#include <tf/transform_listener.h>
23+
1724
// Grid Map
18-
#include <grid_map_ros/grid_map_ros.hpp>
19-
#include <grid_map_msgs/GridMap.h>
2025
#include <grid_map_msgs/GetGridMap.h>
26+
#include <grid_map_msgs/GridMap.h>
27+
#include <grid_map_ros/grid_map_ros.hpp>
28+
2129
// PCL
22-
#include <pcl/point_types.h>
2330
#include <pcl/PCLPointCloud2.h>
31+
#include <pcl/point_types.h>
2432
#include <pcl_conversions/pcl_conversions.h>
2533

2634
namespace py = pybind11;
2735

36+
namespace elevation_mapping_cupy {
2837

29-
namespace elevation_mapping_cupy{
38+
class ElevationMappingWrapper {
39+
public:
40+
using RowMatrixXd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
41+
using RowMatrixXf = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
3042

31-
using RowMatrixXd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
32-
using RowMatrixXf = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
43+
ElevationMappingWrapper();
3344

34-
class ElevationMappingWrapper {
35-
public:
36-
ElevationMappingWrapper();
37-
~ElevationMappingWrapper()=default;
38-
void initialize(ros::NodeHandle& nh);
45+
void initialize(ros::NodeHandle& nh);
46+
47+
void input(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, const RowMatrixXd& R, const Eigen::VectorXd& t,
48+
const double positionNoise, const double orientationNoise);
49+
void move_to(const Eigen::VectorXd& p);
50+
void clear();
51+
void update_variance();
52+
void update_time();
53+
bool exists_layer(const std::string& layerName);
54+
void get_layer_data(const std::string& layerName, RowMatrixXf& map);
55+
void get_grid_map(grid_map::GridMap& gridMap, const std::vector<std::string>& layerNames);
56+
void get_polygon_traversability(std::vector<Eigen::Vector2d>& polygon, Eigen::Vector3d& result,
57+
std::vector<Eigen::Vector2d>& untraversable_polygon);
58+
double get_additive_mean_error();
59+
void initializeWithPoints(std::vector<Eigen::Vector3d>& points, std::string method);
60+
void pointCloudToMatrix(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, RowMatrixXd& points);
61+
void addNormalColorLayer(grid_map::GridMap& map);
3962

40-
void input(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, const RowMatrixXd& R, const Eigen::VectorXd& t,
41-
const double positionNoise, const double orientationNoise);
42-
void move_to(const Eigen::VectorXd& p);
43-
void clear();
44-
void update_variance();
45-
void update_time();
46-
bool exists_layer(const std::string& layerName);
47-
void get_layer_data(const std::string& layerName, RowMatrixXf& map);
48-
void get_grid_map(grid_map::GridMap& gridMap, const std::vector<std::string>& layerNames);
49-
void get_polygon_traversability(std::vector<Eigen::Vector2d>& polygon, Eigen::Vector3d& result, std::vector<Eigen::Vector2d> &untraversable_polygon);
50-
double get_additive_mean_error();
51-
void initializeWithPoints(std::vector<Eigen::Vector3d> &points, std::string method);
52-
void pointCloudToMatrix(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, RowMatrixXd& points);
53-
void addNormalColorLayer(grid_map::GridMap& map);
54-
private:
55-
void setParameters(ros::NodeHandle& nh);
56-
py::object map_;
57-
py::object param_;
58-
double resolution_;
59-
double map_length_;
60-
int map_n_;
61-
bool enable_normal_;
62-
bool enable_normal_color_;
63+
private:
64+
void setParameters(ros::NodeHandle& nh);
65+
py::object map_;
66+
py::object param_;
67+
double resolution_;
68+
double map_length_;
69+
int map_n_;
70+
bool enable_normal_;
71+
bool enable_normal_color_;
6372
};
6473

65-
}
74+
} // namespace elevation_mapping_cupy

‎elevation_mapping_cupy/src/elevation_mapping_node.cpp‎

Lines changed: 9 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -2,32 +2,25 @@
22
// Copyright (c) 2022, Takahiro Miki. All rights reserved.
33
// Licensed under the MIT license. See LICENSE file in the project root for details.
44
//
5+
6+
// Pybind
7+
#include <pybind11_catkin/pybind11/embed.h> // everything needed for embedding
8+
9+
// ROS
510
#include <ros/ros.h>
6-
#include <sensor_msgs/PointCloud2.h>
7-
// Grid Map
8-
#include <grid_map_ros/grid_map_ros.hpp>
9-
#include <grid_map_msgs/GridMap.h>
10-
// PCL
11-
#include <pcl/point_cloud.h>
12-
#include <pcl/point_types.h>
13-
#include <pcl/PCLPointCloud2.h>
14-
#include <pcl_conversions/pcl_conversions.h>
1511

1612
#include "elevation_mapping_cupy/elevation_mapping_ros.hpp"
1713

18-
using namespace elevation_mapping_cupy;
19-
20-
int main(int argc, char** argv)
21-
{
14+
int main(int argc, char** argv) {
2215
ros::init(argc, argv, "elevation_mapping");
2316
ros::NodeHandle nh("~");
2417

25-
py::scoped_interpreter guard{}; // start the interpreter and keep it alive
26-
ElevationMappingNode mapNode(nh);
18+
py::scoped_interpreter guard{}; // start the interpreter and keep it alive
19+
elevation_mapping_cupy::ElevationMappingNode mapNode(nh);
2720
py::gil_scoped_release release;
2821

2922
// Spin
30-
ros::AsyncSpinner spinner(1); // Use n threads
23+
ros::AsyncSpinner spinner(1); // Use n threads
3124
spinner.start();
3225
ros::waitForShutdown();
3326
return 0;

0 commit comments

Comments
 (0)