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.

Sign up for GitHub

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

Jump to bottom

Feature/debugging dml multithreading #4

Merged
merged 2 commits into from
Sep 6, 2022
Merged

Feature/debugging dml multithreading #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
25764a9
debugged callback_group problem (map_callback_group is now a class me…
kminoda Sep 6, 2022
92f7451
DEBUGGING COMMITgit add perception/! force elevation_map_loader to lo…
kminoda Sep 6, 2022
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
3 changes: 2 additions & 1 deletion localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -294,7 +294,8 @@ class NDTScanMatcher : public rclcpp::Node

bool ndt_service_align_in_progress_{false};


rclcpp::CallbackGroup::SharedPtr map_callback_group_;

};

#endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_
11 changes: 6 additions & 5 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -181,6 +181,7 @@ NDTScanMatcher::NDTScanMatcher()
ndt_ptr_->setRegularizationScaleFactor(regularization_scale_factor_);

copyNDT(ndt_ptr_, backup_ndt_ptr_, ndt_implement_type_);


RCLCPP_INFO(
get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d",
Expand Down Expand Up @@ -226,8 +227,8 @@ NDTScanMatcher::NDTScanMatcher()
rclcpp::CallbackGroup::SharedPtr main_callback_group;
main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

rclcpp::CallbackGroup::SharedPtr map_callback_group;
map_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
// rclcpp::CallbackGroup::SharedPtr map_callback_group_;
map_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

auto initial_pose_sub_opt = rclcpp::SubscriptionOptions();
initial_pose_sub_opt.callback_group = initial_pose_callback_group;
Expand All @@ -236,7 +237,7 @@ NDTScanMatcher::NDTScanMatcher()
main_sub_opt.callback_group = main_callback_group;

auto map_sub_opt = rclcpp::SubscriptionOptions();
map_sub_opt.callback_group = map_callback_group;
map_sub_opt.callback_group = map_callback_group_;

initial_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"ekf_pose_with_covariance", 100,
Expand Down Expand Up @@ -299,7 +300,7 @@ NDTScanMatcher::NDTScanMatcher()
service_ = this->create_service<tier4_localization_msgs::srv::PoseWithCovarianceStamped>(
"ndt_align_srv",
std::bind(&NDTScanMatcher::serviceNDTAlign, this, std::placeholders::_1, std::placeholders::_2),
rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group);
rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group_);
// rclcpp::ServicesQoS().get_rmw_qos_profile());

pcd_loader_client_ = this->create_client<autoware_map_msgs::srv::LoadPCDMapsGeneral>(
Expand All @@ -316,7 +317,7 @@ NDTScanMatcher::NDTScanMatcher()
auto period_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(map_update_dt));
map_update_timer_ = rclcpp::create_timer(
this, get_clock(), period_ns, std::bind(&NDTScanMatcher::mapUpdateTimerCallback, this), map_callback_group);
this, get_clock(), period_ns, std::bind(&NDTScanMatcher::mapUpdateTimerCallback, this), map_callback_group_);

diagnostic_thread_ = std::thread(&NDTScanMatcher::timerDiagnostic, this);
diagnostic_thread_.detach();
Expand Down
21 changes: 13 additions & 8 deletions perception/elevation_map_loader/src/elevation_map_loader_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,21 +90,26 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio
std::bind(&ElevationMapLoaderNode::onPointcloudMap, this, _1));
sub_vector_map_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
"input/vector_map", durable_qos, std::bind(&ElevationMapLoaderNode::onVectorMap, this, _1));
// publish();

}

void ElevationMapLoaderNode::publish()
{
struct stat info;
if (stat(data_manager_.elevation_map_path_->c_str(), &info) != 0) {
RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map ");
createElevationMap();
} else if (info.st_mode & S_IFDIR) {
// struct stat info;
// if (stat(data_manager_.elevation_map_path_->c_str(), &info) != 0) {
// RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map ");
// createElevationMap();
// } else if (info.st_mode & S_IFDIR) {
data_manager_.elevation_map_path_ = std::make_unique<std::filesystem::path>(
std::filesystem::path(elevation_map_directory_) / "elevation_map_eneos/elevation_map_eneos_0.db3"
);
RCLCPP_INFO(
this->get_logger(), "Load elevation map from: %s",
data_manager_.elevation_map_path_->c_str());
grid_map::GridMapRosConverter::loadFromBag(
*data_manager_.elevation_map_path_, "elevation_map", elevation_map_);
}
// }

elevation_map_.setFrameId(map_frame_);
auto msg = grid_map::GridMapRosConverter::toMessage(elevation_map_);
Expand All @@ -126,9 +131,9 @@ void ElevationMapLoaderNode::onMapHash(
const auto elevation_map_hash = map_hash->pcd;
data_manager_.elevation_map_path_ = std::make_unique<std::filesystem::path>(
std::filesystem::path(elevation_map_directory_) / elevation_map_hash);
if (data_manager_.isInitialized()) {
// if (data_manager_.isInitialized()) {
publish();
}
// }
}

void ElevationMapLoaderNode::onPointcloudMap(
Expand Down