Skip to content

Commit 4166265

Browse files
JesusSilvaUtreraJesusSilvaUtrera
committed
First commit, code is still not compiling
1 parent dbc6e32 commit 4166265

17 files changed

Lines changed: 2459 additions & 0 deletions

File tree

‎docker/files/DOTaliases‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,3 +29,7 @@ alias light_beacons_localization_demo='cd ~/ws \
2929
alias nav2_integration_demo='cd ~/ws \
3030
&& source install/setup.bash \
3131
&& ros2 launch beluga_demo_nav2_integration demo_office_navigation.launch.py'
32+
33+
alias mh_amcl_demo='cd ~/ws \
34+
&& source install/setup.bash \
35+
&& ros2 launch beluga_demo_mh_amcl bringup.launch.py'

‎localization/beluga_demo_mh_amcl/CMakeLists.txt‎

Lines changed: 65 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
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(Eigen3 REQUIRED)
23+
find_package(geometry_msgs REQUIRED)
24+
find_package(lifecycle_msgs REQUIRED)
25+
find_package(message_filters REQUIRED)
26+
find_package(rclcpp REQUIRED)
27+
find_package(rclcpp_components REQUIRED)
28+
find_package(rclcpp_lifecycle REQUIRED)
29+
find_package(tf2 REQUIRED)
30+
find_package(tf2_eigen REQUIRED)
31+
find_package(tf2_geometry_msgs REQUIRED)
32+
find_package(visualization_msgs REQUIRED)
33+
34+
# Add the include directory
35+
include_directories(include)
36+
37+
# Executable
38+
add_executable(beluga_mh_amcl_node
39+
src/beluga_mh_amcl_node.cpp
40+
src/mh_amcl/mh_amcl.cpp
41+
)
42+
ament_target_dependencies(beluga_mh_amcl_node
43+
beluga
44+
beluga_ros
45+
Eigen3
46+
geometry_msgs
47+
lifecycle_msgs
48+
message_filters
49+
rclcpp
50+
rclcpp_components
51+
rclcpp_lifecycle
52+
tf2
53+
tf2_eigen
54+
tf2_geometry_msgs
55+
visualization_msgs
56+
)
57+
install(TARGETS beluga_mh_amcl_node
58+
DESTINATION lib/${PROJECT_NAME}
59+
)
60+
61+
# Install directories
62+
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
63+
install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
64+
65+
ament_package()

‎localization/beluga_demo_mh_amcl/config/mh_amcl_params.yaml‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
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

‎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(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: 153 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,153 @@
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 <geometry_msgs/msg/pose_array.hpp>
31+
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
32+
#include <nav_msgs/msg/occupancy_grid.hpp>
33+
#include <rclcpp/rclcpp.hpp>
34+
#include <rclcpp_lifecycle/lifecycle_node.hpp>
35+
#include <sensor_msgs/msg/laser_scan.hpp>
36+
#include <tf2/LinearMath/Transform.h>
37+
#include <tf2/transform_datatypes.h>
38+
#include <tf2_ros/buffer.h>
39+
#include <tf2_ros/transform_broadcaster.h>
40+
#include <tf2_ros/transform_listener.h>
41+
42+
#include "beluga_demo_mh_amcl/map_matcher.hpp"
43+
#include "beluga_demo_mh_amcl/particles_distribution.hpp"
44+
45+
namespace mh_amcl {
46+
47+
using CallbackReturnT =
48+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
49+
50+
/**
51+
* MH_AMCL_Node class
52+
*
53+
* Serves as the main lifecycle node, integrating all the components.
54+
* Instantiates a list of 'ParticlesDistribution', each element representing a
55+
* set of particles (hypothesis), and the map matcher.
56+
*/
57+
class MH_AMCL_Node : public rclcpp_lifecycle::LifecycleNode {
58+
public:
59+
explicit MH_AMCL_Node(
60+
const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
61+
62+
// Initialization
63+
void init();
64+
65+
// Lifecycle node methods
66+
CallbackReturnT on_configure(const rclcpp_lifecycle::State &state) override;
67+
CallbackReturnT on_activate(const rclcpp_lifecycle::State &state) override;
68+
CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &state) override;
69+
CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &state) override;
70+
CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &state) override;
71+
CallbackReturnT on_error(const rclcpp_lifecycle::State &state) override;
72+
73+
protected:
74+
// Steps of the MH-AMCL algorithm
75+
void predict();
76+
void correct();
77+
void reseed();
78+
void manage_hypotesis();
79+
80+
// Methods to publish the results of the algorithm (for localization and for
81+
// visualization)
82+
void publish_particles();
83+
void publish_position();
84+
85+
// Auxiliar methods
86+
void get_distances(const geometry_msgs::msg::Pose &pose1,
87+
const geometry_msgs::msg::Pose &pose2, double &dist_xy,
88+
double &dist_theta);
89+
unsigned char get_cost(const geometry_msgs::msg::Pose &pose);
90+
geometry_msgs::msg::Pose toMsg(const tf2::Transform &tf);
91+
92+
private:
93+
// Publishers and subscribers
94+
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
95+
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_;
96+
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
97+
init_pose_sub_;
98+
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
99+
pose_pub_;
100+
rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr particles_pub_;
101+
102+
// Timers
103+
rclcpp::TimerBase::SharedPtr predict_timer_;
104+
rclcpp::TimerBase::SharedPtr correct_timer_;
105+
rclcpp::TimerBase::SharedPtr reseed_timer_;
106+
rclcpp::TimerBase::SharedPtr hypotesys_timer_;
107+
rclcpp::TimerBase::SharedPtr publish_particles_timer_;
108+
rclcpp::TimerBase::SharedPtr publish_position_timer_;
109+
110+
// Configurable params
111+
int max_hypotheses_;
112+
bool multihypothesis_;
113+
float min_candidate_weight_;
114+
double min_candidate_distance_;
115+
double min_candidate_angle_;
116+
float low_q_hypo_threshold_;
117+
float very_low_q_hypo_threshold_;
118+
double hypo_merge_distance_;
119+
double hypo_merge_angle_;
120+
float good_hypo_threshold_;
121+
float min_hypo_diff_winner_;
122+
123+
// Time used as the timestamp for the published results
124+
rclcpp::Time last_time_;
125+
126+
// Particles
127+
std::list<std::shared_ptr<ParticlesDistribution>> particles_population_;
128+
std::shared_ptr<ParticlesDistribution> current_hypothesis_;
129+
float current_hypothesis_q_;
130+
131+
// Transformations
132+
tf2::BufferCore tf_buffer_;
133+
tf2_ros::TransformListener tf_listener_;
134+
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
135+
tf2::Stamped<tf2::Transform> odom2prevbf_;
136+
bool valid_prev_odom2bf_{false};
137+
138+
// Map-matching
139+
std::shared_ptr<beluga_ros::OccupancyGrid> costmap_;
140+
sensor_msgs::msg::LaserScan::UniquePtr last_laser_;
141+
std::shared_ptr<mh_amcl::MapMatcher> map_matcher_;
142+
143+
// Callbacks
144+
void map_callback(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr &msg);
145+
void laser_callback(sensor_msgs::msg::LaserScan::UniquePtr lsr_msg);
146+
void initpose_callback(
147+
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr
148+
&pose_msg);
149+
};
150+
151+
} // namespace mh_amcl
152+
153+
#endif // MH_AMCL__MH_AMCL_HPP_

0 commit comments

Comments
 (0)