Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(pose_estimator_arbiter)!: prefix package and namespace with autoware #8386

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@

<!-- Pose Estimator Arbiter Launch -->
<group if="$(var multi_localizer_mode)">
<include file="$(find-pkg-share pose_estimator_arbiter)/launch/pose_estimator_arbiter.launch.xml">
<include file="$(find-pkg-share autoware_pose_estimator_arbiter)/launch/pose_estimator_arbiter.launch.xml">
<arg name="pose_sources" value="$(var pose_sources)"/>
<arg name="input_pointcloud" value="$(var input_pointcloud)"/>
</include>
Expand Down
2 changes: 1 addition & 1 deletion launch/tier4_localization_launch/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -22,12 +22,12 @@
<exec_depend>autoware_geo_pose_projector</exec_depend>
<exec_depend>autoware_gyro_odometer</exec_depend>
<exec_depend>autoware_pointcloud_preprocessor</exec_depend>
<exec_depend>autoware_pose_estimator_arbiter</exec_depend>
<exec_depend>eagleye_geo_pose_fusion</exec_depend>
<exec_depend>eagleye_gnss_converter</exec_depend>
<exec_depend>eagleye_rt</exec_depend>
<exec_depend>ekf_localizer</exec_depend>
<exec_depend>ndt_scan_matcher</exec_depend>
<exec_depend>pose_estimator_arbiter</exec_depend>
<exec_depend>pose_initializer</exec_depend>
<exec_depend>pose_instability_detector</exec_depend>
<exec_depend>topic_tools</exec_depend>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,22 +1,25 @@
cmake_minimum_required(VERSION 3.14)
project(pose_estimator_arbiter)
project(autoware_pose_estimator_arbiter)

find_package(autoware_cmake REQUIRED)
autoware_package()

find_package(PCL REQUIRED COMPONENTS common)
include_directories(SYSTEM ${PCL_INCLUDE_DIRS})
include_directories(
SYSTEM ${PCL_INCLUDE_DIRS}
src
)

# ==============================
# pose estimator arbiter node
ament_auto_add_library(${PROJECT_NAME} SHARED
src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp
src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp
src/pose_estimator_arbiter_core.cpp
src/switch_rule/enable_all_rule.cpp
)
target_include_directories(${PROJECT_NAME} PUBLIC src)

rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "pose_estimator_arbiter::PoseEstimatorArbiter"
PLUGIN "autoware::pose_estimator_arbiter::PoseEstimatorArbiter"
EXECUTABLE ${PROJECT_NAME}_node
EXECUTOR MultiThreadedExecutor
)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
# pose_estimator_arbiter
# autoware_pose_estimator_arbiter

Table of contents:

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

### Demonstration

Expand Down Expand Up @@ -117,8 +117,8 @@ If it does not seems to work, users can get more information in the following wa
> [!TIP]
>
> ```bash
> ros2 service call /localization/pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \
> '{logger_name: localization.pose_estimator_arbiter, level: debug}'
> ros2 service call /localization/autoware_pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \
> '{logger_name: localization.autoware_pose_estimator_arbiter, level: debug}'
> ```

## Architecture
Expand All @@ -135,7 +135,7 @@ Following figure shows the node configuration when NDT, YabLoc Eagleye and AR-Ta

### Case of running multiple pose estimators

When running multiple pose_estimators, pose_estimator_arbiter is executed.
When running multiple pose_estimators, autoware_pose_estimator_arbiter is executed.
It comprises a **switching rule** and **stoppers** corresponding to each pose_estimator.

- Stoppers control the pose_estimator activity by relaying inputs or outputs, or by requesting a suspend service.
Expand Down Expand Up @@ -187,14 +187,14 @@ ros2 launch autoware_launch logging_simulator.launch.xml \
Even if `pose_source` includes an unexpected string, it will be filtered appropriately.
Please see the table below for details.

| given runtime argument | parsed pose_estimator_arbiter's param (pose_sources) |
| ------------------------------------------- | ---------------------------------------------------- |
| `pose_source:=ndt` | `["ndt"]` |
| `pose_source:=nan` | `[]` |
| `pose_source:=yabloc_ndt` | `["ndt","yabloc"]` |
| `pose_source:=yabloc_ndt_ndt_ndt` | `["ndt","yabloc"]` |
| `pose_source:=ndt_yabloc_eagleye` | `["ndt","yabloc","eagleye"]` |
| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]` |
| given runtime argument | parsed autoware_pose_estimator_arbiter's param (pose_sources) |
| ------------------------------------------- | ------------------------------------------------------------- |
| `pose_source:=ndt` | `["ndt"]` |
| `pose_source:=nan` | `[]` |
| `pose_source:=yabloc_ndt` | `["ndt","yabloc"]` |
| `pose_source:=yabloc_ndt_ndt_ndt` | `["ndt","yabloc"]` |
| `pose_source:=ndt_yabloc_eagleye` | `["ndt","yabloc","eagleye"]` |
| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]` |

</details>

Expand Down
Original file line number Diff line number Diff line change
@@ -1,13 +1,13 @@
# example switch rule library
ament_auto_add_library(example_rule
SHARED
src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp
src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp
src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp
src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp
src/switch_rule/pcd_map_based_rule.cpp
src/switch_rule/vector_map_based_rule.cpp
src/rule_helper/pcd_occupancy.cpp
src/rule_helper/pose_estimator_area.cpp
)
target_include_directories(example_rule PUBLIC src example_rule/src)
target_link_libraries(example_rule pose_estimator_arbiter)
target_link_libraries(example_rule autoware_pose_estimator_arbiter)

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

ament_add_gtest(${test_name} ${filepath})
target_link_libraries("${test_name}" pose_estimator_arbiter example_rule)
target_link_libraries("${test_name}" autoware_pose_estimator_arbiter example_rule)
target_include_directories(${test_name} PUBLIC src)
ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
endfunction()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_
#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_
#ifndef RULE_HELPER__GRID_KEY_HPP_
#define RULE_HELPER__GRID_KEY_HPP_
#include <boost/functional/hash.hpp>

#include <pcl/point_types.h>

#include <functional>

namespace pose_estimator_arbiter::rule_helper
namespace autoware::pose_estimator_arbiter::rule_helper
{
struct GridKey
{
Expand All @@ -46,16 +46,16 @@ struct GridKey
friend bool operator!=(const GridKey & one, const GridKey & other) { return !(one == other); }
};

} // namespace pose_estimator_arbiter::rule_helper
} // namespace autoware::pose_estimator_arbiter::rule_helper

// This is for unordered_map and unordered_set
namespace std
{
template <>
struct hash<pose_estimator_arbiter::rule_helper::GridKey>
struct hash<autoware::pose_estimator_arbiter::rule_helper::GridKey>
{
public:
size_t operator()(const pose_estimator_arbiter::rule_helper::GridKey & grid) const
size_t operator()(const autoware::pose_estimator_arbiter::rule_helper::GridKey & grid) const
{
std::size_t seed = 0;
boost::hash_combine(seed, grid.x);
Expand All @@ -65,4 +65,4 @@ struct hash<pose_estimator_arbiter::rule_helper::GridKey>
};
} // namespace std

#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_
#endif // RULE_HELPER__GRID_KEY_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp"
#include "rule_helper/pcd_occupancy.hpp"

#include "pose_estimator_arbiter/rule_helper/grid_key.hpp"
#include "rule_helper/grid_key.hpp"

#include <boost/functional/hash.hpp>

Expand All @@ -25,7 +25,7 @@
#include <unordered_map>
#include <vector>

namespace pose_estimator_arbiter::rule_helper
namespace autoware::pose_estimator_arbiter::rule_helper
{
PcdOccupancy::PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold)
: pcd_density_upper_threshold_(pcd_density_upper_threshold),
Expand Down Expand Up @@ -114,4 +114,4 @@ void PcdOccupancy::init(PointCloud2::ConstSharedPtr msg)
kdtree_->setInputCloud(occupied_areas_);
}

} // namespace pose_estimator_arbiter::rule_helper
} // namespace autoware::pose_estimator_arbiter::rule_helper
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_
#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_
#ifndef RULE_HELPER__PCD_OCCUPANCY_HPP_
#define RULE_HELPER__PCD_OCCUPANCY_HPP_

#include <rclcpp/node.hpp>

Expand All @@ -26,7 +26,7 @@

#include <string>

namespace pose_estimator_arbiter::rule_helper
namespace autoware::pose_estimator_arbiter::rule_helper
{
class PcdOccupancy
{
Expand All @@ -49,6 +49,6 @@ class PcdOccupancy
pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree_{nullptr};
};

} // namespace pose_estimator_arbiter::rule_helper
} // namespace autoware::pose_estimator_arbiter::rule_helper

#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_
#endif // RULE_HELPER__PCD_OCCUPANCY_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp"
#include "rule_helper/pose_estimator_area.hpp"

#include <autoware_lanelet2_extension/utility/message_conversion.hpp>
#include <autoware_lanelet2_extension/utility/utilities.hpp>
Expand All @@ -26,7 +26,7 @@
#include <unordered_set>
#include <vector>

namespace pose_estimator_arbiter::rule_helper
namespace autoware::pose_estimator_arbiter::rule_helper
{
using BoostPoint = boost::geometry::model::d2::point_xy<double>;
using BoostPolygon = boost::geometry::model::polygon<BoostPoint>;
Expand Down Expand Up @@ -141,4 +141,4 @@ bool PoseEstimatorArea::Impl::within(
}
return false;
}
} // namespace pose_estimator_arbiter::rule_helper
} // namespace autoware::pose_estimator_arbiter::rule_helper
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_
#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_
#ifndef RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_
#define RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_

#include <rclcpp/logger.hpp>
#include <rclcpp/node.hpp>
Expand All @@ -25,7 +25,7 @@
#include <memory>
#include <string>

namespace pose_estimator_arbiter::rule_helper
namespace autoware::pose_estimator_arbiter::rule_helper
{
class PoseEstimatorArea
{
Expand All @@ -51,6 +51,6 @@ class PoseEstimatorArea
rclcpp::Logger logger_;
};

} // namespace pose_estimator_arbiter::rule_helper
} // namespace autoware::pose_estimator_arbiter::rule_helper

#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_
#endif // RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,13 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp"
#include "switch_rule/pcd_map_based_rule.hpp"

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

#include <utility>

namespace pose_estimator_arbiter::switch_rule
namespace autoware::pose_estimator_arbiter::switch_rule
{
PcdMapBasedRule::PcdMapBasedRule(
rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
Expand Down Expand Up @@ -77,4 +77,4 @@ std::unordered_map<PoseEstimatorType, bool> PcdMapBasedRule::update()
{PoseEstimatorType::artag, false}};
}

} // namespace pose_estimator_arbiter::switch_rule
} // namespace autoware::pose_estimator_arbiter::switch_rule
Original file line number Diff line number Diff line change
Expand Up @@ -12,20 +12,20 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_
#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_
#ifndef SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_
#define SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_

#include "pose_estimator_arbiter/pose_estimator_type.hpp"
#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp"
#include "pose_estimator_arbiter/shared_data.hpp"
#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp"
#include "pose_estimator_type.hpp"
#include "rule_helper/pcd_occupancy.hpp"
#include "shared_data.hpp"
#include "switch_rule/base_switch_rule.hpp"

#include <memory>
#include <string>
#include <unordered_map>
#include <unordered_set>

namespace pose_estimator_arbiter::switch_rule
namespace autoware::pose_estimator_arbiter::switch_rule
{
class PcdMapBasedRule : public BaseSwitchRule
{
Expand All @@ -49,6 +49,6 @@ class PcdMapBasedRule : public BaseSwitchRule
// Store the reason why which pose estimator is enabled
mutable std::string debug_string_;
};
} // namespace pose_estimator_arbiter::switch_rule
} // namespace autoware::pose_estimator_arbiter::switch_rule

#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_
#endif // SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,13 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp"
#include "switch_rule/vector_map_based_rule.hpp"

#include <magic_enum.hpp>

#include <utility>

namespace pose_estimator_arbiter::switch_rule
namespace autoware::pose_estimator_arbiter::switch_rule
{
VectorMapBasedRule::VectorMapBasedRule(
rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
Expand Down Expand Up @@ -76,4 +76,4 @@ std::unordered_map<PoseEstimatorType, bool> VectorMapBasedRule::update()
return enable_list;
}

} // namespace pose_estimator_arbiter::switch_rule
} // namespace autoware::pose_estimator_arbiter::switch_rule
Loading
Loading