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

feat: add pose_initializer params #279

Merged
merged 7 commits into from
Jan 21, 2022
+95 −39
Merged

feat: add pose_initializer params #279

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
6c6af15
add pose_initializer param
YamatoAndo Jan 20, 2022
0721408
rename yaml file
YamatoAndo Jan 21, 2022
7a89cd8
ci(pre-commit): autofix
pre-commit-ci[bot] Jan 21, 2022
742cf2c
Revert "ci(pre-commit): autofix"
YamatoAndo Jan 21, 2022
e8fa831
ci(pre-commit): autofix
pre-commit-ci[bot] Jan 21, 2022
2ff2ea5
refactor
YamatoAndo Jan 21, 2022
c4cce89
refactor
YamatoAndo Jan 21, 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
1 change: 1 addition & 0 deletions localization/pose_initializer/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,4 +29,5 @@ endif()

ament_auto_package(INSTALL_TO_SHARE
launch
config
)
46 changes: 46 additions & 0 deletions localization/pose_initializer/config/pose_initializer.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
/**:
ros__parameters:

# from initialpose (Rviz's 2DPoseEstimate)
initialpose_particle_covariance:
[
4.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 4.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
]

# from gnss
gnss_particle_covariance:
[
1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 10.0,
]

# from service
service_particle_covariance:
[
1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 10.0,
]

# output
output_pose_covariance:
[
1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.2,
]
15 changes: 9 additions & 6 deletions localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,16 +36,15 @@ class PoseInitializer : public rclcpp::Node
{
public:
PoseInitializer();
~PoseInitializer();

private:
void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr);
void serviceInitializePose(
const std::shared_ptr<tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request> req,
std::shared_ptr<tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response> res);
const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res);
void serviceInitializePoseAuto(
const std::shared_ptr<tier4_external_api_msgs::srv::InitializePoseAuto::Request> req,
std::shared_ptr<tier4_external_api_msgs::srv::InitializePoseAuto::Response> res);
const tier4_external_api_msgs::srv::InitializePoseAuto::Request::SharedPtr req,
tier4_external_api_msgs::srv::InitializePoseAuto::Response::SharedPtr res);
void callbackInitialPose(
geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr);
void callbackGNSSPoseCov(
Expand Down Expand Up @@ -91,6 +90,10 @@ class PoseInitializer : public rclcpp::Node
uint32_t response_id_ = 0;

bool enable_gnss_callback_;
std::array<double, 36> initialpose_particle_covariance_;
std::array<double, 36> gnss_particle_covariance_;
std::array<double, 36> service_particle_covariance_;
std::array<double, 36> output_pose_covariance_;
};

#endif // POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
5 changes: 5 additions & 0 deletions localization/pose_initializer/launch/pose_initializer.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,7 @@
<launch>

<arg name="config_file" default="$(find-pkg-share pose_initializer)/config/pose_initializer.param.yaml"/>

<node pkg="pose_initializer" exec="pose_initializer" name="pose_initializer" output="log">
<remap from="initialpose" to="/initialpose" />
<remap from="initialpose3d" to="/initialpose3d" />
Expand All @@ -7,6 +10,8 @@
<remap from="ndt_align_srv" to="/localization/pose_estimator/ndt_align_srv" />
<remap from="service/initialize_pose" to="/localization/util/initialize_pose" />
<remap from="service/initialize_pose_auto" to="/localization/util/initialize_pose_auto" />

<param from="$(var config_file)"/>
<param name="enable_gnss_callback" value="true" />
</node>
</launch>
67 changes: 34 additions & 33 deletions localization/pose_initializer/src/pose_initializer_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,30 @@ PoseInitializer::PoseInitializer()
{
enable_gnss_callback_ = this->declare_parameter("enable_gnss_callback", true);

std::vector<double> initialpose_particle_covariance =
this->declare_parameter<std::vector<double>>("initialpose_particle_covariance");
for (std::size_t i = 0; i < initialpose_particle_covariance.size(); ++i) {
initialpose_particle_covariance_[i] = initialpose_particle_covariance[i];
}

std::vector<double> gnss_particle_covariance =
this->declare_parameter<std::vector<double>>("gnss_particle_covariance");
for (std::size_t i = 0; i < gnss_particle_covariance.size(); ++i) {
gnss_particle_covariance_[i] = gnss_particle_covariance[i];
}

std::vector<double> service_particle_covariance =
this->declare_parameter<std::vector<double>>("service_particle_covariance");
for (std::size_t i = 0; i < service_particle_covariance.size(); ++i) {
service_particle_covariance_[i] = service_particle_covariance[i];
}

std::vector<double> output_pose_covariance =
this->declare_parameter<std::vector<double>>("output_pose_covariance");
for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
output_pose_covariance_[i] = output_pose_covariance[i];
}

// We can't use _1 because pcl leaks an alias to boost::placeholders::_1, so it would be ambiguous
initial_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"initialpose", 10,
Expand Down Expand Up @@ -83,8 +107,6 @@ PoseInitializer::PoseInitializer()
std::placeholders::_1, std::placeholders::_2));
}

PoseInitializer::~PoseInitializer() {}

void PoseInitializer::callbackMapPoints(
sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr)
{
Expand All @@ -94,21 +116,15 @@ void PoseInitializer::callbackMapPoints(
}

void PoseInitializer::serviceInitializePose(
const std::shared_ptr<tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request> req,
std::shared_ptr<tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response> res)
const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res)
{
enable_gnss_callback_ = false; // get only first topic

auto add_height_pose_msg_ptr = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
getHeight(req->pose_with_covariance, add_height_pose_msg_ptr);

// TODO(YamatoAndo)
add_height_pose_msg_ptr->pose.covariance[0] = 1.0;
add_height_pose_msg_ptr->pose.covariance[1 * 6 + 1] = 1.0;
add_height_pose_msg_ptr->pose.covariance[2 * 6 + 2] = 0.01;
add_height_pose_msg_ptr->pose.covariance[3 * 6 + 3] = 0.01;
add_height_pose_msg_ptr->pose.covariance[4 * 6 + 4] = 0.01;
add_height_pose_msg_ptr->pose.covariance[5 * 6 + 5] = 1.0;
add_height_pose_msg_ptr->pose.covariance = service_particle_covariance_;

res->success = callAlignServiceAndPublishResult(add_height_pose_msg_ptr);
}
Expand All @@ -121,13 +137,7 @@ void PoseInitializer::callbackInitialPose(
auto add_height_pose_msg_ptr = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
getHeight(*pose_cov_msg_ptr, add_height_pose_msg_ptr);

// TODO(YamatoAndo)
add_height_pose_msg_ptr->pose.covariance[0] = 2.0;
add_height_pose_msg_ptr->pose.covariance[1 * 6 + 1] = 2.0;
add_height_pose_msg_ptr->pose.covariance[2 * 6 + 2] = 0.01;
add_height_pose_msg_ptr->pose.covariance[3 * 6 + 3] = 0.01;
add_height_pose_msg_ptr->pose.covariance[4 * 6 + 4] = 0.01;
add_height_pose_msg_ptr->pose.covariance[5 * 6 + 5] = 0.3;
add_height_pose_msg_ptr->pose.covariance = initialpose_particle_covariance_;

callAlignServiceAndPublishResult(add_height_pose_msg_ptr);
}
Expand All @@ -145,21 +155,17 @@ void PoseInitializer::callbackGNSSPoseCov(
auto add_height_pose_msg_ptr = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
getHeight(*pose_cov_msg_ptr, add_height_pose_msg_ptr);

// TODO(YamatoAndo)
add_height_pose_msg_ptr->pose.covariance[0] = 1.0;
add_height_pose_msg_ptr->pose.covariance[1 * 6 + 1] = 1.0;
add_height_pose_msg_ptr->pose.covariance[2 * 6 + 2] = 0.01;
add_height_pose_msg_ptr->pose.covariance[3 * 6 + 3] = 0.01;
add_height_pose_msg_ptr->pose.covariance[4 * 6 + 4] = 0.01;
add_height_pose_msg_ptr->pose.covariance[5 * 6 + 5] = 3.14;
add_height_pose_msg_ptr->pose.covariance = gnss_particle_covariance_;

callAlignServiceAndPublishResult(add_height_pose_msg_ptr);
}

void PoseInitializer::serviceInitializePoseAuto(
const std::shared_ptr<tier4_external_api_msgs::srv::InitializePoseAuto::Request> req,
std::shared_ptr<tier4_external_api_msgs::srv::InitializePoseAuto::Response> res)
const tier4_external_api_msgs::srv::InitializePoseAuto::Request::SharedPtr req,
tier4_external_api_msgs::srv::InitializePoseAuto::Response::SharedPtr res)
{
(void)req;

RCLCPP_INFO(this->get_logger(), "Called Pose Initialize Service");
enable_gnss_callback_ = true;
res->status = tier4_api_utils::response_success();
Expand Down Expand Up @@ -226,12 +232,7 @@ bool PoseInitializer::callAlignServiceAndPublishResult(
// NOTE temporary cov
geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_covariance =
result.get()->pose_with_covariance;
pose_with_covariance.pose.covariance[0] = 1.0;
pose_with_covariance.pose.covariance[1 * 6 + 1] = 1.0;
pose_with_covariance.pose.covariance[2 * 6 + 2] = 0.01;
pose_with_covariance.pose.covariance[3 * 6 + 3] = 0.01;
pose_with_covariance.pose.covariance[4 * 6 + 4] = 0.01;
pose_with_covariance.pose.covariance[5 * 6 + 5] = 0.2;
pose_with_covariance.pose.covariance = output_pose_covariance_;
initial_pose_pub_->publish(pose_with_covariance);
enable_gnss_callback_ = false;
} else {
Expand Down