fix(pose_initializer): add topic subscription to direct initialization #8941

meliketanrikulu · 2024-09-23T13:56:54Z

Description

You need to call service to initialize directly now. Documentation
However, if you trust the GNSS pose, you may want to initialize directly with the GNSS pose. For example, we think that the pose coming from GNSS for the simulation environment can be used for initialization directly. We think that the feature of using GNSS poses should be added to activate the direct initialization feature for such cases.

This PR allows you to directly initialize the position from GNSS poses with a parameter change.

How was this PR tested?

Test Steps:

1. Checkout autoware.universe

cd ~/autoware/src/universe/autoware.universe/
git remote add autoware.universe https://github.com/meliketanrikulu/autoware.universe.git
git remote update
git checkout fix/add_topic_subscription_to_direct_initialization

2. Compile updated packages:

cd ~/autoware
colcon build --symlink-install --packages-select autoware_pose_initializer autoware_default_adapi

3. Update Parameters

To enable the direct initialization feature using GNSS poses, the parameters must be adjusted accordingly.

Open the config/default_adapi.param.yaml file.

Set the initialization_method parameter to 1 to activate direct initialization.

4. Run autoware

source ~/autoware/install/setup.bash
ros2 launch autoware_launch logging_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-rosbag vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

4. Run bag file

source ~/autoware/install/setup.bash
ros2 bag play ~/autoware_map/sample-rosbag/ -r 0.2 -s sqlite3

Instead, you can test PR in any sensor model or simulation environment and observe it with the GNSS pose to see the results.

Notes for reviewers

What do we expect to see?
If we set initialization method as 1 (DIRECT Initialization method), we expect to see the vehicle initialize at exactly the same point and with the same orientation as the GNSS pose.

Interface changes

None.

Effects on system behavior

None.

github-actions · 2024-09-23T13:57:13Z

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

You've checked our contribution guidelines.
Your PR follows our pull request guidelines.
All required CI checks pass before marking the PR ready for review.

Signed-off-by: Melike Tanrıkulu <[email protected]>

meliketanrikulu · 2024-09-23T15:25:00Z

/review

github-actions · 2024-09-23T15:26:03Z

PR Reviewer Guide 🔍

⏱️ Estimated effort to review: 3 🔵🔵🔵⚪⚪
🧪 No relevant tests
🔒 No security concerns identified
⚡ Key issues to review Possible Bug The check for `gnss_` being null or uninitialized is not clear. Ensure `gnss_` is properly initialized before use to avoid potential null pointer dereference. Parameter Validation The `initialization_method_` parameter is directly used without validation. Consider adding validation to ensure it only accepts expected values (e.g., 0 or 1).

meliketanrikulu · 2024-09-23T15:43:24Z

/improve --extended

github-actions · 2024-09-23T16:15:13Z

localization/autoware_pose_initializer/src/pose_initializer_core.cpp

+      auto pose = req->pose_with_covariance.empty() ? get_gnss_pose().pose.pose
+                                                    : req->pose_with_covariance.front().pose.pose;


Suggestion: To improve readability and avoid potential confusion, consider renaming the variable pose to something more descriptive like initial_pose. [readability, importance: 6]

Suggested change

auto pose = req->pose_with_covariance.empty() ? get_gnss_pose().pose.pose

: req->pose_with_covariance.front().pose.pose;

auto initial_pose = req->pose_with_covariance.empty() ? get_gnss_pose().pose.pose

: req->pose_with_covariance.front().pose.pose;

github-actions · 2024-09-23T16:15:13Z

system/autoware_default_adapi/src/localization.cpp

@@ -27,13 +27,15 @@ LocalizationNode::LocalizationNode(const rclcpp::NodeOptions & options)
  adaptor.relay_message(pub_state_, sub_state_);
  adaptor.init_cli(cli_initialize_);
  adaptor.init_srv(srv_initialize_, this, &LocalizationNode::on_initialize, group_cli_);
+
+  initialization_method_ = declare_parameter<int>("initialization_method");


Suggestion: To ensure the parameter initialization_method is within the expected range, add a validation check after declaring the parameter. [validation, importance: 9]

Suggested change

initialization_method_ = declare_parameter<int>("initialization_method");

initialization_method_ = declare_parameter<int>("initialization_method");

if (initialization_method_ < 0 || initialization_method_ > 1) {

throw std::invalid_argument("initialization_method must be 0 (AUTO) or 1 (DIRECT)");

}

github-actions · 2024-09-23T16:15:13Z

system/autoware_default_adapi/src/utils/localization_conversion.cpp

+  if (initialization_method == 0)
+    internal->method = tier4_localization_msgs::srv::InitializeLocalization::Request::AUTO;
+  else if (initialization_method == 1)
+    internal->method = tier4_localization_msgs::srv::InitializeLocalization::Request::DIRECT;


Suggestion: To enhance readability and maintainability, use a switch statement instead of multiple if-else statements for setting the initialization_method. [readability, importance: 7]

Suggested change

if (initialization_method == 0)

internal->method = tier4_localization_msgs::srv::InitializeLocalization::Request::AUTO;

else if (initialization_method == 1)

internal->method = tier4_localization_msgs::srv::InitializeLocalization::Request::DIRECT;

switch (initialization_method) {

case 0:

internal->method = tier4_localization_msgs::srv::InitializeLocalization::Request::AUTO;

break;

case 1:

internal->method = tier4_localization_msgs::srv::InitializeLocalization::Request::DIRECT;

break;

default:

throw std::invalid_argument("Invalid initialization_method");

}

github-actions · 2024-09-23T16:15:13Z

system/autoware_default_adapi/src/utils/localization_conversion.hpp

 {
-  return convert_response(client->call(convert_request(req))->status);
+  return convert_response(client->call(convert_request(req, initialization_method))->status);


Suggestion: To ensure the convert_call function is robust, add a check to handle the case where the client call fails and returns a null response. [robustness, importance: 10]

Suggested change

return convert_response(client->call(convert_request(req, initialization_method))->status);

auto response = client->call(convert_request(req, initialization_method));

if (!response) {

throw std::runtime_error("Client call failed");

}

return convert_response(response->status);

Signed-off-by: Melike Tanrıkulu <[email protected]>

YamatoAndo

This makes the ndt_enable and yabloc_enabled parameters meaningless. Please revise the implementation.

meliketanrikulu · 2024-09-24T13:19:16Z

This PR parameter change is based on the addition of DIRECT initialization. However, instead of this, it seems more logical to decide whether the pose will be DIRECT initialized by checking if the covariance of the gnss poses is 0.For this reason I want to close this and opened a new PR. Here is the mentioned PR --> #8948

However, the covariance values of the GNSS poses that come in the autoware_pose_initializer package are automatically changed . This parameter decide the covariance values of GNSS poses -->

autoware.universe/localization/autoware_pose_initializer/config/pose_initializer.param.yaml

Line 16 in d25e9a1

gnss_particle_covariance:

Here is the GNSS poses covariance replaced here --> https://github.com/autowarefoundation/autoware.universe/blame/d25e9a1bbedc176d4e29c70b3ed9586808bdf813/localization/autoware_pose_initializer/src/pose_initializer_core.cpp#L221
What is the reason for this? Why are default values assigned instead of using the covariance values that come with the poses.
@isamu-takagi @YamatoAndo

YamatoAndo · 2024-09-25T09:20:09Z

What is the reason for this? Why are default values assigned instead of using the covariance values that come with the poses.

It's not that we want the covariance of the GNSS, but rather, we want to determine the kind of distribution to use when spreading the particles.

YamatoAndo · 2024-09-25T09:21:53Z

it seems more logical to decide whether the pose will be DIRECT initialized by checking if the covariance of the gnss poses is 0

I don't think this method is a good approach.

xmfcx · 2024-09-25T09:25:24Z

it seems more logical to decide whether the pose will be DIRECT initialized by checking if the covariance of the gnss poses is 0

I don't think this method is a good approach.

@YamatoAndo -san, Why do you think so?

If all the covariances are 0, or lower than some threshold we define, it means, the pose is very reliable and we can use it for direct initialization.

Overriding the original covariances should be an optional feature, not the default.

The GNSS already provides this information.

xmfcx · 2024-09-25T09:28:13Z

It's not that we want the covariance of the GNSS, but rather, we want to determine the kind of distribution to use when spreading the particles.

Why shouldn't you distribute it along the covariance ellipse?

YamatoAndo · 2024-09-25T10:27:22Z

Why shouldn't you distribute it along the covariance ellipse?

In the first place, the position output by the GNSS receiver often does not match the correct position on the point cloud map. While GNSS positioning errors are one factor, it is difficult to perfectly align the point cloud map with GNSS positioning. Despite this, relying on the dispersion output by the GNSS receiver to determine the particle distribution is not a good idea.
Therefore, considering various errors, I believe it would be more robust to distribute particles with a wider spread than the dispersion output by the GNSS receiver.

xmfcx · 2024-09-27T15:36:37Z

Let's discuss here:
Proposal for Simplifying Pose Initialization in Autoware #5285

meliketanrikulu requested review from isamu-takagi, mitsudome-r, yukkysaito, anhnv3991, KYabuuchi, Motsu-san, RyuYamamoto, SakodaShintaro, TaikiYamada4 and YamatoAndo as code owners September 23, 2024 13:56

github-actions bot added component:localization Vehicle's position determination in its environment. (auto-assigned) component:system System design and integration. (auto-assigned) labels Sep 23, 2024

meliketanrikulu requested review from Aysenayilmaz and mozhoku September 23, 2024 13:57

meliketanrikulu marked this pull request as draft September 23, 2024 13:57

meliketanrikulu force-pushed the fix/add_topic_subscription_to_direct_initialization branch from 101508c to daf7949 Compare September 23, 2024 13:59

fix(pose_initializer): Add topic subscription to direct initialization

3db8ce1

Signed-off-by: Melike Tanrıkulu <[email protected]>

meliketanrikulu force-pushed the fix/add_topic_subscription_to_direct_initialization branch from f4d9220 to 3db8ce1 Compare September 23, 2024 14:17

github-actions bot added the type:documentation Creating or refining documentation. (auto-assigned) label Sep 23, 2024

meliketanrikulu changed the title ~~fix(pose_initializer): Add topic subscription to direct initialization~~ fix(pose_initializer): add topic subscription to direct initialization Sep 23, 2024

meliketanrikulu self-assigned this Sep 23, 2024

meliketanrikulu force-pushed the fix/add_topic_subscription_to_direct_initialization branch from de8073d to e7d83c9 Compare September 23, 2024 15:28

github-actions bot reviewed Sep 23, 2024

View reviewed changes

meliketanrikulu added 2 commits September 23, 2024 19:57

update README.md

d7b969a

Signed-off-by: Melike Tanrıkulu <[email protected]>

add throw check

b2235ec

Signed-off-by: Melike Tanrıkulu <[email protected]>

meliketanrikulu force-pushed the fix/add_topic_subscription_to_direct_initialization branch from e7d83c9 to b2235ec Compare September 23, 2024 16:58

pre-commit-ci bot and others added 3 commits September 23, 2024 17:00

style(pre-commit): autofix

da31884

Update README.md

63b7939

Signed-off-by: Melike Tanrıkulu <[email protected]>

style(pre-commit): autofix

1f161fa

meliketanrikulu requested a review from xmfcx September 23, 2024 17:22

meliketanrikulu marked this pull request as ready for review September 23, 2024 17:34

YamatoAndo requested changes Sep 24, 2024

View reviewed changes

meliketanrikulu marked this pull request as draft September 24, 2024 14:06

xmfcx closed this Sep 27, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(pose_initializer): add topic subscription to direct initialization #8941

fix(pose_initializer): add topic subscription to direct initialization #8941

meliketanrikulu commented Sep 23, 2024 •

edited

Loading

github-actions bot commented Sep 23, 2024 •

edited

Loading

meliketanrikulu commented Sep 23, 2024

github-actions bot commented Sep 23, 2024

meliketanrikulu commented Sep 23, 2024 •

edited

Loading

github-actions bot Sep 23, 2024

github-actions bot Sep 23, 2024

github-actions bot Sep 23, 2024

github-actions bot Sep 23, 2024

YamatoAndo left a comment

meliketanrikulu commented Sep 24, 2024 •

edited

Loading

YamatoAndo commented Sep 25, 2024

YamatoAndo commented Sep 25, 2024

xmfcx commented Sep 25, 2024

xmfcx commented Sep 25, 2024

YamatoAndo commented Sep 25, 2024

xmfcx commented Sep 27, 2024

		auto pose = req->pose_with_covariance.empty() ? get_gnss_pose().pose.pose
		: req->pose_with_covariance.front().pose.pose;

-  return convert_response(client->call(convert_request(req, initialization_method))->status);
+  auto response = client->call(convert_request(req, initialization_method));
+  if (!response) {
+    throw std::runtime_error("Client call failed");
+  }
+  return convert_response(response->status);

fix(pose_initializer): add topic subscription to direct initialization #8941

fix(pose_initializer): add topic subscription to direct initialization #8941

Conversation

meliketanrikulu commented Sep 23, 2024 • edited Loading

Description

Related links

How was this PR tested?

1. Checkout autoware.universe

2. Compile updated packages:

3. Update Parameters

4. Run autoware

4. Run bag file

Notes for reviewers

Interface changes

Effects on system behavior

github-actions bot commented Sep 23, 2024 • edited Loading

meliketanrikulu commented Sep 23, 2024

github-actions bot commented Sep 23, 2024

PR Reviewer Guide 🔍

meliketanrikulu commented Sep 23, 2024 • edited Loading

github-actions bot Sep 23, 2024

Choose a reason for hiding this comment

github-actions bot Sep 23, 2024

Choose a reason for hiding this comment

github-actions bot Sep 23, 2024

Choose a reason for hiding this comment

github-actions bot Sep 23, 2024

Choose a reason for hiding this comment

YamatoAndo left a comment

Choose a reason for hiding this comment

meliketanrikulu commented Sep 24, 2024 • edited Loading

YamatoAndo commented Sep 25, 2024

YamatoAndo commented Sep 25, 2024

xmfcx commented Sep 25, 2024

xmfcx commented Sep 25, 2024

YamatoAndo commented Sep 25, 2024

xmfcx commented Sep 27, 2024

meliketanrikulu commented Sep 23, 2024 •

edited

Loading

github-actions bot commented Sep 23, 2024 •

edited

Loading

meliketanrikulu commented Sep 23, 2024 •

edited

Loading

meliketanrikulu commented Sep 24, 2024 •

edited

Loading