From 68be8c68a25602a296e6b737878243289125bf84 Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura <82020865+ToshikiNakamura0412@users.noreply.github.com> Date: Sun, 26 May 2024 16:37:49 +0900 Subject: [PATCH] Don't use std::optional to map (#16) --- include/emcl/emcl.h | 2 +- src/emcl.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/emcl/emcl.h b/include/emcl/emcl.h index eb1f766..f5df604 100644 --- a/include/emcl/emcl.h +++ b/include/emcl/emcl.h @@ -216,7 +216,7 @@ class EMCL ros::Subscriber laser_scan_sub_; ros::Subscriber odom_sub_; - std::optional map_; + nav_msgs::OccupancyGrid map_; std::optional prev_odom_; std::optional last_odom_; }; diff --git a/src/emcl.cpp b/src/emcl.cpp index f5f3547..067431d 100644 --- a/src/emcl.cpp +++ b/src/emcl.cpp @@ -46,7 +46,7 @@ void EMCL::get_map(void) void EMCL::laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg) { - if (!map_.has_value() || !prev_odom_.has_value()) + if (!prev_odom_.has_value()) return; // Update the observation model @@ -131,7 +131,7 @@ void EMCL::broadcast_odom_state(void) odom_state.header.stamp = ros::Time::now(); - odom_state.header.frame_id = map_.value().header.frame_id; + odom_state.header.frame_id = map_.header.frame_id; odom_state.child_frame_id = odom_frame_id_; odom_state.transform.translation.x = isnan(map_to_odom_x) ? 0.0 : map_to_odom_x; @@ -171,7 +171,7 @@ float EMCL::calc_average_likelihood(const sensor_msgs::LaserScan &laser_scan) for (auto &p : particles_) { const float likelihood = - p.likelihood(map_.value(), laser_scan, emcl_param_.sensor_noise_ratio, emcl_param_.laser_step); + p.likelihood(map_, laser_scan, emcl_param_.sensor_noise_ratio, emcl_param_.laser_step); p.set_weight(p.weight() * likelihood); } @@ -263,7 +263,7 @@ void EMCL::publish_estimated_pose(void) q.setRPY(0, 0, emcl_pose_.yaw()); tf2::convert(q, emcl_pose_msg.pose.pose.orientation); - emcl_pose_msg.header.frame_id = map_.value().header.frame_id; + emcl_pose_msg.header.frame_id = map_.header.frame_id; emcl_pose_msg.header.stamp = ros::Time::now(); emcl_pose_pub_.publish(emcl_pose_msg); } @@ -286,7 +286,7 @@ void EMCL::publish_particles(void) particle_cloud_msg.poses.emplace_back(pose); } - particle_cloud_msg.header.frame_id = map_.value().header.frame_id; + particle_cloud_msg.header.frame_id = map_.header.frame_id; particle_cloud_msg.header.stamp = ros::Time::now(); particle_cloud_pub_.publish(particle_cloud_msg); }