Skip to content

Commit 984bef0

Browse files
RollingOatRollingOat
committed
updated mapper package to ros2 jazzy
1 parent 91f0a17 commit 984bef0

File tree

8 files changed

+102
-115
lines changed

8 files changed

+102
-115
lines changed

‎autonomy_core/map_plan/mapper/CMakeLists.txt‎

Lines changed: 33 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,7 @@ find_package(kr_planning_msgs REQUIRED)
7878
find_package(kr_planning_rviz_plugins REQUIRED)
7979
find_package(depth_image_proc REQUIRED)
8080
find_package(sensor_msgs REQUIRED)
81-
find_package(eigen_conversions REQUIRED)
81+
# find_package(eigen_conversions REQUIRED) # Eigen conversions for ROS 2 is not supported yet
8282
find_package(tf2_eigen REQUIRED)
8383
find_package(motion_primitive_library REQUIRED)
8484
find_package(Boost REQUIRED COMPONENTS timer)
@@ -98,42 +98,49 @@ ament_target_dependencies(${PROJECT_NAME}
9898
kr_planning_rviz_plugins
9999
depth_image_proc
100100
sensor_msgs
101-
eigen_conversions
101+
# eigen_conversions
102102
tf2_eigen
103103
motion_primitive_library
104104
)
105105
# Link the Boost::timer target
106-
target_link_libraries(${PROJECT_NAME} PUBLIC Boost::timer)
106+
target_link_libraries(${PROJECT_NAME} Boost::timer)
107+
target_include_directories(${PROJECT_NAME} PUBLIC
108+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
109+
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
110+
)
107111

108112
# -----------------------------------------------------------------------------
109113
# 3) Build your node
110114
add_executable(local_global_mapper src/local_global_mapper.cpp)
111-
ament_target_dependencies(local_global_mapper rclcpp)
112115
target_link_libraries(local_global_mapper PUBLIC ${PROJECT_NAME} Boost::timer)
116+
target_include_directories(local_global_mapper PUBLIC
117+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
118+
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
119+
)
113120

114121
# -----------------------------------------------------------------------------
115122
# 4) Testing with ament_cmake_gtest
116-
if(BUILD_TESTING)
117-
find_package(ament_lint_auto REQUIRED)
118-
ament_lint_auto_find_test_dependencies()
119-
120-
ament_add_gtest(test_voxel_mapper test/test_voxel_mapper.cpp)
121-
if(TARGET test_voxel_mapper)
122-
ament_target_dependencies(test_voxel_mapper
123-
rclcpp
124-
pcl_ros
125-
image_geometry
126-
kr_planning_msgs
127-
kr_planning_rviz_plugins
128-
depth_image_proc
129-
sensor_msgs
130-
eigen_conversions
131-
tf2_eigen
132-
motion_primitive_library
133-
)
134-
target_link_libraries(test_voxel_mapper PUBLIC ${PROJECT_NAME} Boost::timer)
135-
endif()
136-
endif()
123+
# if(BUILD_TESTING)
124+
# find_package(ament_lint_auto REQUIRED)
125+
# ament_lint_auto_find_test_dependencies()
126+
127+
# ament_add_gtest(test_voxel_mapper test/test_voxel_mapper.cpp)
128+
# if(TARGET test_voxel_mapper)
129+
# ament_target_dependencies(test_voxel_mapper
130+
# rclcpp
131+
# pcl_ros
132+
# image_geometry
133+
# kr_planning_msgs
134+
# kr_planning_rviz_plugins
135+
# depth_image_proc
136+
# sensor_msgs
137+
# # eigen_conversions
138+
# tf2_eigen
139+
# motion_primitive_library
140+
# )
141+
# target_link_libraries(test_voxel_mapper PUBLIC ${PROJECT_NAME} Boost::timer)
142+
# endif()
143+
# endif()
137144

138145
# -----------------------------------------------------------------------------
139146
# 5) Install rules
@@ -153,7 +160,6 @@ install(
153160

154161
# -----------------------------------------------------------------------------
155162
# 6) Export & finalize
156-
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
157163
ament_export_dependencies(
158164
rclcpp
159165
pcl_ros
@@ -162,7 +168,7 @@ ament_export_dependencies(
162168
kr_planning_rviz_plugins
163169
depth_image_proc
164170
sensor_msgs
165-
eigen_conversions
171+
# eigen_conversions
166172
tf2_eigen
167173
motion_primitive_library
168174
Boost

‎autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h‎

Lines changed: 15 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
#include <eigen_conversions/eigen_msg.h>
1+
// #include <eigen_conversions/eigen_msg.h>
22
// #include <nav_msgs/Odometry.h>
33
#include <nav_msgs/msg/odometry.hpp>
44
#include <kr_planning_rviz_plugins/data_ros_utils.h>
@@ -11,6 +11,8 @@
1111
#include <sensor_msgs/msg/point_cloud2.hpp>
1212
#include <sensor_msgs/msg/point_cloud.hpp>
1313
#include <std_msgs/msg/bool.hpp>
14+
#include <std_msgs/msg/header.hpp>
15+
#include <geometry_msgs/msg/pose.hpp>
1416

1517
#include <boost/timer/timer.hpp>
1618
#include <memory>
@@ -27,7 +29,7 @@ class LocalGlobalMapperNode : public rclcpp::Node {
2729
public:
2830
/**
2931
* @brief Local Global Mapper Constructor
30-
* @param nh ROS Node handler
32+
* @param options Node options for rclcpp::Node
3133
*/
3234
// explicit LocalGlobalMapperNode(const ros::NodeHandle& nh);
3335
explicit LocalGlobalMapperNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
@@ -56,9 +58,9 @@ class LocalGlobalMapperNode : public rclcpp::Node {
5658
* @param pose_odom_lidar_ptr Output tf from lidar to odom
5759
*/
5860
// void getLidarPoses(const std_msgs::Header& cloud_header,
59-
// geometry_msgs::Pose* pose_map_lidar_ptr,
60-
// geometry_msgs::Pose* pose_odom_lidar_ptr);
61-
void getLidarPoses(const std_msgs:msg::Header& cloud_header,
61+
// geometry_msgs::msg::Pose* pose_map_lidar_ptr,
62+
// geometry_msgs::msg::Pose* pose_odom_lidar_ptr);
63+
void getLidarPoses(const std_msgs::msg::Header& cloud_header,
6264
geometry_msgs::msg::Pose* pose_map_lidar_ptr,
6365
geometry_msgs::msg::Pose* pose_odom_lidar_ptr);
6466

@@ -104,22 +106,22 @@ class LocalGlobalMapperNode : public rclcpp::Node {
104106
std::unique_ptr<mapper::VoxelMapper> storage_voxel_mapper_; // mapper
105107
// std::unique_ptr<VoxelMapper> local_voxel_mapper_; // mapper
106108

107-
kr_planning_msgs::VoxelMap global_map_info_;
108-
kr_planning_msgs::VoxelMap storage_map_info_;
109-
kr_planning_msgs::VoxelMap local_map_info_;
109+
kr_planning_msgs::msg::VoxelMap global_map_info_;
110+
kr_planning_msgs::msg::VoxelMap storage_map_info_;
111+
kr_planning_msgs::msg::VoxelMap local_map_info_;
110112

111113
// ros::NodeHandle nh_;
112114
// ros::Subscriber cloud_sub;
113115
// ros::Publisher global_map_pub;
114116
// ros::Publisher storage_map_pub;
115117
// ros::Publisher local_map_pub;
116118
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub;
117-
rclcpp::Publisher<kr_planning_msgs::VoxelMap>::SharedPtr global_map_pub;
118-
rclcpp::Publisher<kr_planning_msgs::VoxelMap>::SharedPtr storage_map_pub;
119-
rclcpp::Publisher<kr_planning_msgs::VoxelMap>::SharedPtr local_map_pub;
119+
rclcpp::Publisher<kr_planning_msgs::msg::VoxelMap>::SharedPtr global_map_pub;
120+
rclcpp::Publisher<kr_planning_msgs::msg::VoxelMap>::SharedPtr storage_map_pub;
121+
rclcpp::Publisher<kr_planning_msgs::msg::VoxelMap>::SharedPtr local_map_pub;
120122

121123
// ros::Publisher global_occ_map_pub;
122-
ros::Publisher local_voxel_map_pub;
124+
// ros::Publisher local_voxel_map_pub;
123125
// ros::Publisher local_cloud_pub;
124126

125127
bool real_robot_; // define it's real-robot experiment or not
@@ -149,8 +151,7 @@ class LocalGlobalMapperNode : public rclcpp::Node {
149151
double local_max_raycast_, global_max_raycast_; // maximum raycasting range
150152
double occ_map_height_;
151153
Eigen::Vector3d local_ori_offset_;
152-
bool pub_storage_map_ =
153-
false; // don't set this as true unless you're debugging, it's very slow
154+
bool pub_storage_map_ = false; // don't set this as true unless you're debugging, it's very slow
154155

155156
int update_interval_;
156157
int counter_ = 0;

‎autonomy_core/map_plan/mapper/include/mapper/tf_listener.h‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -9,14 +9,17 @@
99
#include <tf2_ros/transform_listener.h>
1010

1111
#include <optional>
12+
#include <memory>
1213

1314
namespace mapper {
1415

1516
class TFListener {
1617
public:
17-
TFListener(rclcpp::Node::SharedPtr node) : buffer_(node->get_clock()), listener_(buffer_) {}
18+
TFListener(rclcpp::Node::SharedPtr node) : node_(node),
19+
buffer_(std::make_shared<tf2_ros::Buffer>(node_->get_clock())),
20+
listener_(std::make_shared<tf2_ros::TransformListener>(*buffer_)) {}
1821

19-
// std::optional<geometry_msgs::Pose> LookupTransform(const std::string& target,
22+
// std::optional<geometry_msgs::msg::Pose> LookupTransform(const std::string& target,
2023
// const std::string& source,
2124
// const ros::Time& time);
2225
std::optional<geometry_msgs::msg::Pose> LookupTransform(
@@ -25,8 +28,9 @@ class TFListener {
2528

2629

2730
private:
28-
tf2_ros::Buffer buffer_;
29-
tf2_ros::TransformListener listener_;
31+
std::shared_ptr<tf2_ros::Buffer> buffer_;
32+
std::shared_ptr<tf2_ros::TransformListener> listener_;
33+
rclcpp::Node::SharedPtr node_;
3034
};
3135

3236
} // namespace mapper

‎autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h‎

Lines changed: 15 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,13 @@
11
#pragma once
22

3-
#include <kr_planning_msgs/VoxelMap.h>
3+
#include <kr_planning_msgs/msg/voxel_map.h>
44

55
#include <Eigen/Geometry>
66
#include <boost/multi_array.hpp>
77
#include <gtest/gtest_prod.h>
88
#include <mpl_collision/map_util.h>
99
#include <vector>
10+
#include <rclcpp/rclcpp.hpp>
1011

1112
namespace mapper {
1213

@@ -51,6 +52,7 @@ class VoxelMapper {
5152
VoxelMapper(const Eigen::Vector3d& origin,
5253
const Eigen::Vector3d& dim,
5354
double res,
55+
const rclcpp::Logger& logger,
5456
int8_t default_val = 0,
5557
int decay_times_to_empty = 0);
5658

@@ -86,22 +88,22 @@ class VoxelMapper {
8688

8789
/**
8890
* @brief Get the Map object
89-
* @return kr_planning_msgs::VoxelMap
91+
* @return kr_planning_msgs::msg::VoxelMap
9092
*/
91-
kr_planning_msgs::VoxelMap getMap();
93+
kr_planning_msgs::msg::VoxelMap getMap();
9294

9395
/**
9496
* @brief Get the Inflated Map object
95-
* @return kr_planning_msgs::VoxelMap
97+
* @return kr_planning_msgs::msg::VoxelMap
9698
*/
97-
kr_planning_msgs::VoxelMap getInflatedMap();
99+
kr_planning_msgs::msg::VoxelMap getInflatedMap();
98100

99101
/**
100102
* @brief Crop a local voxel map from the global inflated voxel map
101103
* @param ori The origin of the local voxel map (most negative corner)
102104
* @param dim the range of the local voxel map in world units
103105
*/
104-
kr_planning_msgs::VoxelMap getInflatedLocalMap(const Eigen::Vector3d& ori,
106+
kr_planning_msgs::msg::VoxelMap getInflatedLocalMap(const Eigen::Vector3d& ori,
105107
const Eigen::Vector3d& dim);
106108

107109
/**
@@ -111,7 +113,7 @@ class VoxelMapper {
111113
* refers to how much space (in either direction) along the z-axis is going to
112114
* be considered for the 2D slice.
113115
*/
114-
kr_planning_msgs::VoxelMap getInflatedOccMap(double h, double hh = 0);
116+
kr_planning_msgs::msg::VoxelMap getInflatedOccMap(double h, double hh = 0);
115117

116118
/**
117119
* @brief Add point cloud to map and inflated map
@@ -204,16 +206,16 @@ class VoxelMapper {
204206
double res_; // Resolution used for both maps
205207

206208
// Possible voxel values taken from VoxelMap.msg
207-
int8_t val_free_ = kr_planning_msgs::VoxelMap::val_free;
208-
int8_t val_occ_ = kr_planning_msgs::VoxelMap::val_occ;
209-
int8_t val_unknown_ = kr_planning_msgs::VoxelMap::val_unknown;
210-
int8_t val_even_ = kr_planning_msgs::VoxelMap::val_even;
211-
int8_t val_default_ = kr_planning_msgs::VoxelMap::val_default;
209+
int8_t val_free_ = kr_planning_msgs::msg::VoxelMap::VAL_FREE;
210+
int8_t val_occ_ = kr_planning_msgs::msg::VoxelMap::VAL_OCC;
211+
int8_t val_unknown_ = kr_planning_msgs::msg::VoxelMap::VAL_UNKNOWN;
212+
int8_t val_even_ = kr_planning_msgs::msg::VoxelMap::VAL_EVEN;
213+
int8_t val_default_ = kr_planning_msgs::msg::VoxelMap::VAL_DEFAULT;
212214

213215
// Be careful of overflow (should always be within -128 and 128 range)
214216
// Add val_add to the voxel whenever a point lies in it. Should always be less
215217
// than 27 to avoid overflow (should always be within -128 and 128 range)
216-
int8_t val_add_ = kr_planning_msgs::VoxelMap::val_add;
218+
int8_t val_add_ = kr_planning_msgs::msg::VoxelMap::VAL_ADD;
217219

218220
// Value decay (voxels will disappear if unobserved for
219221
// ((val_occ - val_even) / val_decay times)

‎autonomy_core/map_plan/mapper/package.xml‎

Lines changed: 1 addition & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -1,29 +1,3 @@
1-
<!-- <?xml version="1.0"?>
2-
<package format="2">
3-
<name>mapper</name>
4-
<version>0.0.1</version>
5-
<description>The mapper package</description>
6-
<maintainer email="sikang@seas.upenn.edu">sikang</maintainer>
7-
<maintainer email="liuxu@seas.upenn.edu">Xu Liu</maintainer>
8-
<maintainer email="guilhermenardari@gmail.com">Guilherme Nardari</maintainer>
9-
<maintainer email="quchao@seas.upenn.edu">Chao Qu</maintainer>
10-
11-
<license>Penn Software License</license>
12-
13-
<buildtool_depend>catkin</buildtool_depend>
14-
15-
<depend>roscpp</depend>
16-
<depend>pcl_ros</depend>
17-
<depend>image_geometry</depend>
18-
<depend>kr_planning_msgs</depend>
19-
<depend>kr_planning_rviz_plugins</depend>
20-
<depend>sensor_msgs</depend>
21-
<depend>depth_image_proc</depend>
22-
<depend>tf_conversions</depend>
23-
<depend>eigen_conversions</depend>
24-
25-
</package> -->
26-
271
<?xml version="1.0"?>
282
<package format="2">
293
<name>mapper</name>
@@ -50,7 +24,7 @@
5024
<depend>sensor_msgs</depend>
5125
<depend>depth_image_proc</depend>
5226
<depend>tf2_eigen</depend>
53-
<depend>eigen_conversions</depend>
27+
<!-- <depend>eigen_conversions</depend> -->
5428
<depend>motion_primitive_library</depend>
5529

5630
<export>

0 commit comments

Comments
 (0)