Skip to content

Commit b714a54

Browse files
a-maumauSakodaShintaro
authored andcommitted
refactor(pose_estimator_arbiter)!: prefix package and namespace with autoware (autowarefoundation#8386)
* add autoware_ prefix Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * add autoware_ prefix Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * fix link for landmark_based_localizer Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * remove Shadowing Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> --------- Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> Co-authored-by: SakodaShintaro <shintaro.sakoda@tier4.jp>
1 parent 8b23f9a commit b714a54

38 files changed

+205
-201
lines changed

launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -93,7 +93,7 @@
9393

9494
<!-- Pose Estimator Arbiter Launch -->
9595
<group if="$(var multi_localizer_mode)">
96-
<include file="$(find-pkg-share pose_estimator_arbiter)/launch/pose_estimator_arbiter.launch.xml">
96+
<include file="$(find-pkg-share autoware_pose_estimator_arbiter)/launch/pose_estimator_arbiter.launch.xml">
9797
<arg name="pose_sources" value="$(var pose_sources)"/>
9898
<arg name="input_pointcloud" value="$(var input_pointcloud)"/>
9999
</include>

launch/tier4_localization_launch/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -22,12 +22,12 @@
2222
<exec_depend>autoware_geo_pose_projector</exec_depend>
2323
<exec_depend>autoware_gyro_odometer</exec_depend>
2424
<exec_depend>autoware_pointcloud_preprocessor</exec_depend>
25+
<exec_depend>autoware_pose_estimator_arbiter</exec_depend>
2526
<exec_depend>eagleye_geo_pose_fusion</exec_depend>
2627
<exec_depend>eagleye_gnss_converter</exec_depend>
2728
<exec_depend>eagleye_rt</exec_depend>
2829
<exec_depend>ekf_localizer</exec_depend>
2930
<exec_depend>ndt_scan_matcher</exec_depend>
30-
<exec_depend>pose_estimator_arbiter</exec_depend>
3131
<exec_depend>pose_initializer</exec_depend>
3232
<exec_depend>pose_instability_detector</exec_depend>
3333
<exec_depend>topic_tools</exec_depend>

localization/pose_estimator_arbiter/CMakeLists.txt renamed to localization/autoware_pose_estimator_arbiter/CMakeLists.txt

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,22 +1,25 @@
11
cmake_minimum_required(VERSION 3.14)
2-
project(pose_estimator_arbiter)
2+
project(autoware_pose_estimator_arbiter)
33

44
find_package(autoware_cmake REQUIRED)
55
autoware_package()
66

77
find_package(PCL REQUIRED COMPONENTS common)
8-
include_directories(SYSTEM ${PCL_INCLUDE_DIRS})
8+
include_directories(
9+
SYSTEM ${PCL_INCLUDE_DIRS}
10+
src
11+
)
912

1013
# ==============================
1114
# pose estimator arbiter node
1215
ament_auto_add_library(${PROJECT_NAME} SHARED
13-
src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp
14-
src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp
16+
src/pose_estimator_arbiter_core.cpp
17+
src/switch_rule/enable_all_rule.cpp
1518
)
1619
target_include_directories(${PROJECT_NAME} PUBLIC src)
1720

1821
rclcpp_components_register_node(${PROJECT_NAME}
19-
PLUGIN "pose_estimator_arbiter::PoseEstimatorArbiter"
22+
PLUGIN "autoware::pose_estimator_arbiter::PoseEstimatorArbiter"
2023
EXECUTABLE ${PROJECT_NAME}_node
2124
EXECUTOR MultiThreadedExecutor
2225
)

localization/pose_estimator_arbiter/README.md renamed to localization/autoware_pose_estimator_arbiter/README.md

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
# pose_estimator_arbiter
1+
# autoware_pose_estimator_arbiter
22

33
Table of contents:
44

@@ -35,7 +35,7 @@ Also, even if both can be activated at the same time, the Kalman Filter may be a
3535
- [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher)
3636
- [eagleye](https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/launch-autoware/localization/eagleye/)
3737
- [yabloc](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc)
38-
- [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/landmark_based_localizer)
38+
- [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_landmark_based_localizer)
3939

4040
### Demonstration
4141

@@ -117,8 +117,8 @@ If it does not seems to work, users can get more information in the following wa
117117
> [!TIP]
118118
>
119119
> ```bash
120-
> ros2 service call /localization/pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \
121-
> '{logger_name: localization.pose_estimator_arbiter, level: debug}'
120+
> ros2 service call /localization/autoware_pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \
121+
> '{logger_name: localization.autoware_pose_estimator_arbiter, level: debug}'
122122
> ```
123123
124124
## Architecture
@@ -135,7 +135,7 @@ Following figure shows the node configuration when NDT, YabLoc Eagleye and AR-Ta
135135
136136
### Case of running multiple pose estimators
137137
138-
When running multiple pose_estimators, pose_estimator_arbiter is executed.
138+
When running multiple pose_estimators, autoware_pose_estimator_arbiter is executed.
139139
It comprises a **switching rule** and **stoppers** corresponding to each pose_estimator.
140140
141141
- Stoppers control the pose_estimator activity by relaying inputs or outputs, or by requesting a suspend service.
@@ -187,14 +187,14 @@ ros2 launch autoware_launch logging_simulator.launch.xml \
187187
Even if `pose_source` includes an unexpected string, it will be filtered appropriately.
188188
Please see the table below for details.
189189

190-
| given runtime argument | parsed pose_estimator_arbiter's param (pose_sources) |
191-
| ------------------------------------------- | ---------------------------------------------------- |
192-
| `pose_source:=ndt` | `["ndt"]` |
193-
| `pose_source:=nan` | `[]` |
194-
| `pose_source:=yabloc_ndt` | `["ndt","yabloc"]` |
195-
| `pose_source:=yabloc_ndt_ndt_ndt` | `["ndt","yabloc"]` |
196-
| `pose_source:=ndt_yabloc_eagleye` | `["ndt","yabloc","eagleye"]` |
197-
| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]` |
190+
| given runtime argument | parsed autoware_pose_estimator_arbiter's param (pose_sources) |
191+
| ------------------------------------------- | ------------------------------------------------------------- |
192+
| `pose_source:=ndt` | `["ndt"]` |
193+
| `pose_source:=nan` | `[]` |
194+
| `pose_source:=yabloc_ndt` | `["ndt","yabloc"]` |
195+
| `pose_source:=yabloc_ndt_ndt_ndt` | `["ndt","yabloc"]` |
196+
| `pose_source:=ndt_yabloc_eagleye` | `["ndt","yabloc","eagleye"]` |
197+
| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]` |
198198

199199
</details>
200200

localization/pose_estimator_arbiter/example_rule/CMakeLists.txt renamed to localization/autoware_pose_estimator_arbiter/example_rule/CMakeLists.txt

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,13 @@
11
# example switch rule library
22
ament_auto_add_library(example_rule
33
SHARED
4-
src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp
5-
src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp
6-
src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp
7-
src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp
4+
src/switch_rule/pcd_map_based_rule.cpp
5+
src/switch_rule/vector_map_based_rule.cpp
6+
src/rule_helper/pcd_occupancy.cpp
7+
src/rule_helper/pose_estimator_area.cpp
88
)
99
target_include_directories(example_rule PUBLIC src example_rule/src)
10-
target_link_libraries(example_rule pose_estimator_arbiter)
10+
target_link_libraries(example_rule autoware_pose_estimator_arbiter)
1111

1212
# ==============================
1313
# define test definition macro
@@ -16,7 +16,7 @@ function(add_testcase filepath)
1616
string(REGEX REPLACE ".cpp" "" test_name ${filename})
1717

1818
ament_add_gtest(${test_name} ${filepath})
19-
target_link_libraries("${test_name}" pose_estimator_arbiter example_rule)
19+
target_link_libraries("${test_name}" autoware_pose_estimator_arbiter example_rule)
2020
target_include_directories(${test_name} PUBLIC src)
2121
ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
2222
endfunction()
Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -12,15 +12,15 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_
16-
#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_
15+
#ifndef RULE_HELPER__GRID_KEY_HPP_
16+
#define RULE_HELPER__GRID_KEY_HPP_
1717
#include <boost/functional/hash.hpp>
1818

1919
#include <pcl/point_types.h>
2020

2121
#include <functional>
2222

23-
namespace pose_estimator_arbiter::rule_helper
23+
namespace autoware::pose_estimator_arbiter::rule_helper
2424
{
2525
struct GridKey
2626
{
@@ -46,16 +46,16 @@ struct GridKey
4646
friend bool operator!=(const GridKey & one, const GridKey & other) { return !(one == other); }
4747
};
4848

49-
} // namespace pose_estimator_arbiter::rule_helper
49+
} // namespace autoware::pose_estimator_arbiter::rule_helper
5050

5151
// This is for unordered_map and unordered_set
5252
namespace std
5353
{
5454
template <>
55-
struct hash<pose_estimator_arbiter::rule_helper::GridKey>
55+
struct hash<autoware::pose_estimator_arbiter::rule_helper::GridKey>
5656
{
5757
public:
58-
size_t operator()(const pose_estimator_arbiter::rule_helper::GridKey & grid) const
58+
size_t operator()(const autoware::pose_estimator_arbiter::rule_helper::GridKey & grid) const
5959
{
6060
std::size_t seed = 0;
6161
boost::hash_combine(seed, grid.x);
@@ -65,4 +65,4 @@ struct hash<pose_estimator_arbiter::rule_helper::GridKey>
6565
};
6666
} // namespace std
6767

68-
#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_
68+
#endif // RULE_HELPER__GRID_KEY_HPP_
Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,9 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp"
15+
#include "rule_helper/pcd_occupancy.hpp"
1616

17-
#include "pose_estimator_arbiter/rule_helper/grid_key.hpp"
17+
#include "rule_helper/grid_key.hpp"
1818

1919
#include <boost/functional/hash.hpp>
2020

@@ -25,7 +25,7 @@
2525
#include <unordered_map>
2626
#include <vector>
2727

28-
namespace pose_estimator_arbiter::rule_helper
28+
namespace autoware::pose_estimator_arbiter::rule_helper
2929
{
3030
PcdOccupancy::PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold)
3131
: pcd_density_upper_threshold_(pcd_density_upper_threshold),
@@ -114,4 +114,4 @@ void PcdOccupancy::init(PointCloud2::ConstSharedPtr msg)
114114
kdtree_->setInputCloud(occupied_areas_);
115115
}
116116

117-
} // namespace pose_estimator_arbiter::rule_helper
117+
} // namespace autoware::pose_estimator_arbiter::rule_helper
Lines changed: 5 additions & 5 deletions
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 POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_
16-
#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_
15+
#ifndef RULE_HELPER__PCD_OCCUPANCY_HPP_
16+
#define RULE_HELPER__PCD_OCCUPANCY_HPP_
1717

1818
#include <rclcpp/node.hpp>
1919

@@ -26,7 +26,7 @@
2626

2727
#include <string>
2828

29-
namespace pose_estimator_arbiter::rule_helper
29+
namespace autoware::pose_estimator_arbiter::rule_helper
3030
{
3131
class PcdOccupancy
3232
{
@@ -49,6 +49,6 @@ class PcdOccupancy
4949
pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree_{nullptr};
5050
};
5151

52-
} // namespace pose_estimator_arbiter::rule_helper
52+
} // namespace autoware::pose_estimator_arbiter::rule_helper
5353

54-
#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_
54+
#endif // RULE_HELPER__PCD_OCCUPANCY_HPP_
Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp"
15+
#include "rule_helper/pose_estimator_area.hpp"
1616

1717
#include <autoware_lanelet2_extension/utility/message_conversion.hpp>
1818
#include <autoware_lanelet2_extension/utility/utilities.hpp>
@@ -26,7 +26,7 @@
2626
#include <unordered_set>
2727
#include <vector>
2828

29-
namespace pose_estimator_arbiter::rule_helper
29+
namespace autoware::pose_estimator_arbiter::rule_helper
3030
{
3131
using BoostPoint = boost::geometry::model::d2::point_xy<double>;
3232
using BoostPolygon = boost::geometry::model::polygon<BoostPoint>;
@@ -141,4 +141,4 @@ bool PoseEstimatorArea::Impl::within(
141141
}
142142
return false;
143143
}
144-
} // namespace pose_estimator_arbiter::rule_helper
144+
} // namespace autoware::pose_estimator_arbiter::rule_helper
Lines changed: 5 additions & 5 deletions
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 POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_
16-
#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_
15+
#ifndef RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_
16+
#define RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_
1717

1818
#include <rclcpp/logger.hpp>
1919
#include <rclcpp/node.hpp>
@@ -25,7 +25,7 @@
2525
#include <memory>
2626
#include <string>
2727

28-
namespace pose_estimator_arbiter::rule_helper
28+
namespace autoware::pose_estimator_arbiter::rule_helper
2929
{
3030
class PoseEstimatorArea
3131
{
@@ -51,6 +51,6 @@ class PoseEstimatorArea
5151
rclcpp::Logger logger_;
5252
};
5353

54-
} // namespace pose_estimator_arbiter::rule_helper
54+
} // namespace autoware::pose_estimator_arbiter::rule_helper
5555

56-
#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_
56+
#endif // RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_
Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,13 +12,13 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp"
15+
#include "switch_rule/pcd_map_based_rule.hpp"
1616

1717
#include <autoware/universe_utils/ros/parameter.hpp>
1818

1919
#include <utility>
2020

21-
namespace pose_estimator_arbiter::switch_rule
21+
namespace autoware::pose_estimator_arbiter::switch_rule
2222
{
2323
PcdMapBasedRule::PcdMapBasedRule(
2424
rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
@@ -77,4 +77,4 @@ std::unordered_map<PoseEstimatorType, bool> PcdMapBasedRule::update()
7777
{PoseEstimatorType::artag, false}};
7878
}
7979

80-
} // namespace pose_estimator_arbiter::switch_rule
80+
} // namespace autoware::pose_estimator_arbiter::switch_rule
Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -12,20 +12,20 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_
16-
#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_
15+
#ifndef SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_
16+
#define SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_
1717

18-
#include "pose_estimator_arbiter/pose_estimator_type.hpp"
19-
#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp"
20-
#include "pose_estimator_arbiter/shared_data.hpp"
21-
#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp"
18+
#include "pose_estimator_type.hpp"
19+
#include "rule_helper/pcd_occupancy.hpp"
20+
#include "shared_data.hpp"
21+
#include "switch_rule/base_switch_rule.hpp"
2222

2323
#include <memory>
2424
#include <string>
2525
#include <unordered_map>
2626
#include <unordered_set>
2727

28-
namespace pose_estimator_arbiter::switch_rule
28+
namespace autoware::pose_estimator_arbiter::switch_rule
2929
{
3030
class PcdMapBasedRule : public BaseSwitchRule
3131
{
@@ -49,6 +49,6 @@ class PcdMapBasedRule : public BaseSwitchRule
4949
// Store the reason why which pose estimator is enabled
5050
mutable std::string debug_string_;
5151
};
52-
} // namespace pose_estimator_arbiter::switch_rule
52+
} // namespace autoware::pose_estimator_arbiter::switch_rule
5353

54-
#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_
54+
#endif // SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_
Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,13 +12,13 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp"
15+
#include "switch_rule/vector_map_based_rule.hpp"
1616

1717
#include <magic_enum.hpp>
1818

1919
#include <utility>
2020

21-
namespace pose_estimator_arbiter::switch_rule
21+
namespace autoware::pose_estimator_arbiter::switch_rule
2222
{
2323
VectorMapBasedRule::VectorMapBasedRule(
2424
rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
@@ -76,4 +76,4 @@ std::unordered_map<PoseEstimatorType, bool> VectorMapBasedRule::update()
7676
return enable_list;
7777
}
7878

79-
} // namespace pose_estimator_arbiter::switch_rule
79+
} // namespace autoware::pose_estimator_arbiter::switch_rule

0 commit comments

Comments
 (0)