Skip to content

Commit 6e821fd

Browse files
JesusSilvaUtreraJesusSilvaUtrera
committed
First draft, localization not working properly
Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> Finish adding comments to the code, other minimal changes, still same error Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> Code is compiling, still needs to be tested Add missing nodes to launch file, bond for lifecycle manager, solve minor errors Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> Added message filter for laser messages, refactor a bit of the code Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> Improved a bit the performance, first simulation tests Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com>
1 parent 1486d42 commit 6e821fd

18 files changed

Lines changed: 3139 additions & 0 deletions

File tree

‎docker/files/DOTaliases‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,3 +33,7 @@ alias nav2_integration_demo='cd ~/ws \
3333
alias amcl3_localization_demo='cd ~/ws \
3434
&& source install/setup.bash \
3535
&& ros2 launch beluga_demo_amcl3_localization demo_botanic_garden_amcl3_localization.launch.py'
36+
37+
alias mh_amcl_demo='cd ~/ws \
38+
&& source install/setup.bash \
39+
&& ros2 launch beluga_demo_mh_amcl bringup.launch.py'

‎localization/beluga_demo_mh_amcl/CMakeLists.txt‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
# Copyright 2025 Ekumen, Inc.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
cmake_minimum_required(VERSION 3.16)
16+
project(beluga_demo_mh_amcl)
17+
18+
# Dependencies
19+
find_package(ament_cmake REQUIRED)
20+
find_package(beluga REQUIRED)
21+
find_package(beluga_ros REQUIRED)
22+
find_package(bondcpp REQUIRED)
23+
find_package(Eigen3 REQUIRED)
24+
find_package(geometry_msgs REQUIRED)
25+
find_package(lifecycle_msgs REQUIRED)
26+
find_package(message_filters REQUIRED)
27+
find_package(rclcpp REQUIRED)
28+
find_package(rclcpp_components REQUIRED)
29+
find_package(rclcpp_lifecycle REQUIRED)
30+
find_package(tf2 REQUIRED)
31+
find_package(tf2_eigen REQUIRED)
32+
find_package(tf2_geometry_msgs REQUIRED)
33+
find_package(visualization_msgs REQUIRED)
34+
35+
# Add the include directory
36+
include_directories(include)
37+
38+
# Executable
39+
add_executable(beluga_mh_amcl_node
40+
src/beluga_mh_amcl_node.cpp
41+
src/mh_amcl/mh_amcl.cpp
42+
src/mh_amcl/map_matcher.cpp
43+
src/mh_amcl/particles_distribution.cpp
44+
src/mh_amcl/utils.cpp
45+
)
46+
ament_target_dependencies(beluga_mh_amcl_node
47+
beluga
48+
beluga_ros
49+
bondcpp
50+
Eigen3
51+
geometry_msgs
52+
lifecycle_msgs
53+
message_filters
54+
rclcpp
55+
rclcpp_components
56+
rclcpp_lifecycle
57+
tf2
58+
tf2_eigen
59+
tf2_geometry_msgs
60+
visualization_msgs
61+
)
62+
install(TARGETS beluga_mh_amcl_node
63+
DESTINATION lib/${PROJECT_NAME}
64+
)
65+
66+
# Install directories
67+
install(DIRECTORY
68+
config
69+
launch
70+
maps
71+
rviz
72+
DESTINATION share/${PROJECT_NAME})
73+
74+
ament_package()

‎localization/beluga_demo_mh_amcl/config/mh_amcl_params.yaml‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
mh_amcl:
2+
ros__parameters:
3+
use_sim_time: True
4+
5+
# Maximum number of hypotheses maintained
6+
max_hypotheses: 5
7+
8+
# Enable or disable multi-hypothesis AMCL
9+
multihypothesis: true
10+
11+
# Minimum weight for a candidate hypothesis to be considered
12+
min_candidate_weight: 0.5
13+
14+
# Minimum distance between candidate hypotheses (in meters)
15+
min_candidate_distance: 0.5
16+
17+
# Minimum angular difference between candidate hypotheses (in radians)
18+
min_candidate_angle: 1.5708 # Equivalent to PI/2
19+
20+
# Threshold for low-quality hypotheses
21+
low_q_hypo_threshold: 0.25
22+
23+
# Threshold for very low-quality hypotheses
24+
very_low_q_hypo_threshold: 0.1
25+
26+
# Distance threshold for merging similar hypotheses (in meters)
27+
hypo_merge_distance: 0.2
28+
29+
# Angular threshold for merging similar hypotheses (in radians)
30+
hypo_merge_angle: 0.3
31+
32+
# Threshold to consider a hypothesis as "good"
33+
good_hypo_threshold: 0.6
34+
35+
# Minimum difference in quality between the best hypothesis and others
36+
min_hypo_diff_winner: 0.2
37+
38+
# Bond timeout
39+
bond_timeout: 4.0

‎localization/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/map_matcher.hpp‎

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
// Copyright 2025 Ekumen, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef MH_AMCL__MAPMATCHER_HPP_
16+
#define MH_AMCL__MAPMATCHER_HPP_
17+
18+
#include <list>
19+
#include <memory>
20+
#include <vector>
21+
22+
#include <beluga_ros/occupancy_grid.hpp>
23+
24+
#include <nav_msgs/msg/occupancy_grid.hpp>
25+
#include <rclcpp/rclcpp.hpp>
26+
#include <sensor_msgs/msg/laser_scan.hpp>
27+
#include <tf2/LinearMath/Transform.h>
28+
#include <tf2/transform_datatypes.h>
29+
30+
namespace mh_amcl {
31+
32+
/**
33+
* @brief Struct containing a transform and its associated weight
34+
*/
35+
typedef struct {
36+
float weight;
37+
tf2::Transform transform;
38+
} TransformWeighted;
39+
40+
/**
41+
* @brief Overload of the operator '<' to be able to compare TransformWeighted
42+
* structs
43+
*/
44+
bool operator<(const TransformWeighted &tw1, const TransformWeighted &tw2);
45+
46+
/**
47+
* MapMatcher class
48+
*
49+
* Performs the multi-resolution map matching process. It receives an occupancy
50+
* grid map and laser scan data to find transformations (poses) that align the
51+
* scan data with the map. It weights all potential transformations to select
52+
* the higher quality candidates.
53+
*/
54+
class MapMatcher {
55+
public:
56+
explicit MapMatcher(std::shared_ptr<const nav_msgs::msg::OccupancyGrid> map);
57+
58+
/**
59+
* @brief Get the candidate transformations from the laser information and the
60+
* map
61+
*/
62+
std::list<TransformWeighted>
63+
get_matches(const sensor_msgs::msg::LaserScan &scan);
64+
65+
protected:
66+
static const int NUM_LEVEL_SCALE_COSTMAP = 4;
67+
68+
/**
69+
* @brief Reduce the resolution of the map to half to perform the
70+
* multi-resolution map-matching
71+
*/
72+
std::shared_ptr<beluga_ros::OccupancyGrid>
73+
half_scale(std::shared_ptr<beluga_ros::OccupancyGrid> costmap_in);
74+
75+
/**
76+
* @brief Transform the LaserScan message in a vector of 3D points
77+
*/
78+
std::vector<tf2::Vector3>
79+
laser2points(const sensor_msgs::msg::LaserScan &scan);
80+
81+
// Auxiliary methods to get the matches between the laser data and the map
82+
std::list<TransformWeighted>
83+
get_matches(int scale, const std::vector<tf2::Vector3> &scan, float min_x,
84+
float min_y, float max_y, float max_x);
85+
float match(int scale, std::shared_ptr<beluga_ros::OccupancyGrid> costmap,
86+
const std::vector<tf2::Vector3> &scan, tf2::Transform &transform);
87+
88+
// Current costmaps
89+
std::vector<std::shared_ptr<beluga_ros::OccupancyGrid>> costmaps_;
90+
};
91+
92+
/**
93+
* @brief wrapper to transform the costmap to ROS Occupancy grid message
94+
*/
95+
nav_msgs::msg::OccupancyGrid
96+
toMsg(std::shared_ptr<beluga_ros::OccupancyGrid> costmap);
97+
98+
} // namespace mh_amcl
99+
100+
#endif // MH_AMCL__MAPMATCHER_HPP_

‎localization/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/mh_amcl.hpp‎

Lines changed: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
1+
// Copyright 2025 Ekumen, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef MH_AMCL__MH_AMCL_HPP_
16+
#define MH_AMCL__MH_AMCL_HPP_
17+
18+
#include <list>
19+
#include <memory>
20+
#include <mutex>
21+
#include <vector>
22+
23+
#include <Eigen/Dense>
24+
#include <Eigen/LU>
25+
26+
// The particle traits need to be included before the particle_cloud
27+
#include "beluga_demo_mh_amcl/mh_amcl_particle_traits.hpp"
28+
#include <beluga_ros/particle_cloud.hpp>
29+
30+
#include <bondcpp/bond.hpp>
31+
#include <geometry_msgs/msg/pose_array.hpp>
32+
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
33+
#include <message_filters/subscriber.h>
34+
#include <nav_msgs/msg/occupancy_grid.hpp>
35+
#include <rclcpp/rclcpp.hpp>
36+
#include <rclcpp_lifecycle/lifecycle_node.hpp>
37+
#include <sensor_msgs/msg/laser_scan.hpp>
38+
#include <tf2/LinearMath/Transform.h>
39+
#include <tf2/transform_datatypes.h>
40+
#include <tf2_ros/buffer.h>
41+
#include <tf2_ros/message_filter.h>
42+
#include <tf2_ros/transform_broadcaster.h>
43+
#include <tf2_ros/transform_listener.h>
44+
45+
#include "beluga_demo_mh_amcl/map_matcher.hpp"
46+
#include "beluga_demo_mh_amcl/particles_distribution.hpp"
47+
48+
namespace mh_amcl {
49+
50+
// Alias for complex types
51+
using CallbackReturnT =
52+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
53+
using LaserScanSubscriber =
54+
message_filters::Subscriber<sensor_msgs::msg::LaserScan, rclcpp_lifecycle::LifecycleNode>;
55+
using LaserScanFilter = tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>;
56+
57+
/**
58+
* MH_AMCL_Node class
59+
*
60+
* Serves as the main lifecycle node, integrating all the components.
61+
* Instantiates a list of 'ParticlesDistribution', each element representing a
62+
* set of particles (hypothesis), and the map matcher.
63+
*/
64+
class MH_AMCL_Node : public rclcpp_lifecycle::LifecycleNode {
65+
public:
66+
explicit MH_AMCL_Node(
67+
const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
68+
69+
// Initialization
70+
void init();
71+
72+
// Lifecycle node methods
73+
CallbackReturnT on_configure(const rclcpp_lifecycle::State &state) override;
74+
CallbackReturnT on_activate(const rclcpp_lifecycle::State &state) override;
75+
CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &state) override;
76+
CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &state) override;
77+
CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &state) override;
78+
CallbackReturnT on_error(const rclcpp_lifecycle::State &state) override;
79+
80+
protected:
81+
// Steps of the MH-AMCL algorithm
82+
void predict();
83+
void correct();
84+
void reseed();
85+
void manage_hypotheses();
86+
87+
// Methods to publish the results of the algorithm (for localization and for
88+
// visualization)
89+
void publish_particles();
90+
void publish_position();
91+
92+
// Auxiliar methods
93+
void get_distances(const geometry_msgs::msg::Pose &pose1,
94+
const geometry_msgs::msg::Pose &pose2, double &dist_xy,
95+
double &dist_theta);
96+
signed char get_cost(const geometry_msgs::msg::Pose &pose);
97+
geometry_msgs::msg::Pose toMsg(const tf2::Transform &tf);
98+
99+
private:
100+
// Publishers and subscribers
101+
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
102+
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
103+
init_pose_sub_;
104+
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
105+
pose_pub_;
106+
rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr particles_pub_;
107+
108+
// Message filter for the laser subscription
109+
std::unique_ptr<LaserScanSubscriber> laser_sub_;
110+
std::unique_ptr<LaserScanFilter> laser_scan_filter_;
111+
message_filters::Connection laser_scan_connection_;
112+
113+
// Bond object to use with the lifecycle manager
114+
std::unique_ptr<bond::Bond> bond_;
115+
116+
// Timers
117+
rclcpp::TimerBase::SharedPtr predict_timer_;
118+
rclcpp::TimerBase::SharedPtr correct_timer_;
119+
rclcpp::TimerBase::SharedPtr reseed_timer_;
120+
rclcpp::TimerBase::SharedPtr hypothesis_timer_;
121+
rclcpp::TimerBase::SharedPtr publish_particles_timer_;
122+
rclcpp::TimerBase::SharedPtr publish_position_timer_;
123+
124+
// Configurable params
125+
int max_hypotheses_;
126+
bool multihypothesis_;
127+
float min_candidate_weight_;
128+
double min_candidate_distance_;
129+
double min_candidate_angle_;
130+
float low_q_hypo_threshold_;
131+
float very_low_q_hypo_threshold_;
132+
double hypo_merge_distance_;
133+
double hypo_merge_angle_;
134+
float good_hypo_threshold_;
135+
float min_hypo_diff_winner_;
136+
double bond_timeout_;
137+
138+
// Time used as the timestamp for the published results
139+
rclcpp::Time last_time_;
140+
141+
// Hypotheses
142+
std::list<std::shared_ptr<ParticlesDistribution>> particles_population_;
143+
std::shared_ptr<ParticlesDistribution> current_hypothesis_;
144+
float current_hypothesis_q_;
145+
146+
// Transformations
147+
tf2_ros::Buffer tf_buffer_;
148+
tf2_ros::TransformListener tf_listener_;
149+
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
150+
tf2::Stamped<tf2::Transform> odom2prevbf_;
151+
bool valid_prev_odom2bf_{false};
152+
153+
// Map-matching
154+
std::shared_ptr<beluga_ros::OccupancyGrid> costmap_;
155+
sensor_msgs::msg::LaserScan::ConstSharedPtr last_laser_;
156+
std::shared_ptr<mh_amcl::MapMatcher> map_matcher_;
157+
158+
// Callbacks
159+
void map_callback(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr &msg);
160+
void laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg);
161+
void initpose_callback(
162+
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr
163+
&pose_msg);
164+
};
165+
166+
} // namespace mh_amcl
167+
168+
#endif // MH_AMCL__MH_AMCL_HPP_

0 commit comments

Comments
 (0)