Skip to content

Commit cd8d00f

Browse files
mktk1117mktk1117
authored
Merge pull request #2 from leggedrobotics/fix/formatting
Fix/formatting
2 parents 0b44278 + 1ff161a commit cd8d00f

File tree

5 files changed

+349
-376
lines changed

5 files changed

+349
-376
lines changed

‎elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp‎

Lines changed: 106 additions & 97 deletions
Original file line numberDiff line numberDiff line change
@@ -2,125 +2,134 @@
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-
#include <pybind11_catkin/pybind11/embed.h> // everything needed for embedding
5+
6+
#pragma once
7+
8+
// STL
69
#include <iostream>
10+
11+
// Boost
12+
#include <boost/thread/recursive_mutex.hpp>
13+
14+
// Eigen
715
#include <Eigen/Dense>
816

17+
// Pybind
18+
#include <pybind11_catkin/pybind11/embed.h> // everything needed for embedding
19+
20+
// ROS
21+
#include <geometry_msgs/PolygonStamped.h>
922
#include <ros/ros.h>
1023
#include <sensor_msgs/PointCloud2.h>
11-
#include <geometry_msgs/PolygonStamped.h>
1224
#include <std_srvs/Empty.h>
1325
#include <std_srvs/SetBool.h>
26+
#include <tf/transform_broadcaster.h>
1427
#include <tf/transform_listener.h>
1528
#include <visualization_msgs/Marker.h>
1629
#include <visualization_msgs/MarkerArray.h>
17-
#include <tf/transform_broadcaster.h>
30+
1831
// Grid Map
19-
#include <grid_map_ros/grid_map_ros.hpp>
20-
#include <grid_map_msgs/GridMap.h>
2132
#include <grid_map_msgs/GetGridMap.h>
22-
#include <elevation_map_msgs/CheckSafety.h>
23-
#include <elevation_map_msgs/Initialize.h>
33+
#include <grid_map_msgs/GridMap.h>
34+
#include <grid_map_ros/grid_map_ros.hpp>
35+
2436
// PCL
25-
#include <pcl/point_types.h>
2637
#include <pcl/PCLPointCloud2.h>
38+
#include <pcl/point_types.h>
2739
#include <pcl_conversions/pcl_conversions.h>
28-
#include <boost/thread/recursive_mutex.hpp>
40+
41+
#include <elevation_map_msgs/CheckSafety.h>
42+
#include <elevation_map_msgs/Initialize.h>
2943

3044
#include "elevation_mapping_cupy/elevation_mapping_wrapper.hpp"
3145

3246
namespace py = pybind11;
3347

48+
namespace elevation_mapping_cupy {
3449

35-
namespace elevation_mapping_cupy{
50+
class ElevationMappingNode {
51+
public:
52+
ElevationMappingNode(ros::NodeHandle& nh);
3653

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);
3774

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

126-
}
135+
} // namespace elevation_mapping_cupy

‎elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp‎

Lines changed: 50 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -2,61 +2,73 @@
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-
#include <pybind11_catkin/pybind11/embed.h> // everything needed for embedding
5+
6+
#pragma once
7+
8+
// STL
69
#include <iostream>
10+
11+
// Eigen
712
#include <Eigen/Dense>
813

14+
// Pybind
15+
#include <pybind11_catkin/pybind11/embed.h> // everything needed for embedding
16+
17+
// ROS
18+
#include <geometry_msgs/PoseWithCovarianceStamped.h>
919
#include <ros/ros.h>
1020
#include <sensor_msgs/PointCloud2.h>
11-
#include <geometry_msgs/PoseWithCovarianceStamped.h>
1221
#include <std_srvs/Empty.h>
1322
#include <tf/transform_listener.h>
23+
1424
// Grid Map
15-
#include <grid_map_ros/grid_map_ros.hpp>
16-
#include <grid_map_msgs/GridMap.h>
1725
#include <grid_map_msgs/GetGridMap.h>
26+
#include <grid_map_msgs/GridMap.h>
27+
#include <grid_map_ros/grid_map_ros.hpp>
28+
1829
// PCL
19-
#include <pcl/point_types.h>
2030
#include <pcl/PCLPointCloud2.h>
31+
#include <pcl/point_types.h>
2132
#include <pcl_conversions/pcl_conversions.h>
2233

2334
namespace py = pybind11;
2435

36+
namespace elevation_mapping_cupy {
2537

26-
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>;
2742

28-
using RowMatrixXd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
29-
using RowMatrixXf = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
43+
ElevationMappingWrapper();
3044

31-
class ElevationMappingWrapper {
32-
public:
33-
ElevationMappingWrapper();
34-
~ElevationMappingWrapper()=default;
35-
void initialize(ros::NodeHandle& nh);
36-
37-
void input(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, const RowMatrixXd& R, const Eigen::VectorXd& t,
38-
const double positionNoise, const double orientationNoise);
39-
void move_to(const Eigen::VectorXd& p);
40-
void clear();
41-
void update_variance();
42-
void update_time();
43-
bool exists_layer(const std::string& layerName);
44-
void get_layer_data(const std::string& layerName, RowMatrixXf& map);
45-
void get_grid_map(grid_map::GridMap& gridMap, const std::vector<std::string>& layerNames);
46-
void get_polygon_traversability(std::vector<Eigen::Vector2d>& polygon, Eigen::Vector3d& result, std::vector<Eigen::Vector2d> &untraversable_polygon);
47-
double get_additive_mean_error();
48-
void initializeWithPoints(std::vector<Eigen::Vector3d> &points, std::string method);
49-
void pointCloudToMatrix(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, RowMatrixXd& points);
50-
void addNormalColorLayer(grid_map::GridMap& map);
51-
private:
52-
void setParameters(ros::NodeHandle& nh);
53-
py::object map_;
54-
py::object param_;
55-
double resolution_;
56-
double map_length_;
57-
int map_n_;
58-
bool enable_normal_;
59-
bool enable_normal_color_;
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);
62+
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_;
6072
};
6173

62-
}
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)