Skip to content

Commit

Permalink
test partial dead reckoning
Browse files Browse the repository at this point in the history
  • Loading branch information
@meliketanrikulu
meliketanrikulu committed Oct 18, 2023
1 parent fcc9756 commit 855b490
Show file tree
Hide file tree
Showing 11 changed files with 281 additions and 45 deletions.
33 changes: 29 additions & 4 deletions localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@
#include <queue>
#include <string>
#include <vector>

#include <std_msgs/msg/string.hpp>
class Simple1DFilter
{
public:
Expand Down Expand Up @@ -105,7 +105,7 @@ class EKFLocalizer : public rclcpp::Node

private:
const Warning warning_;

rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_rviz_string;
//!< @brief ekf estimated pose publisher
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_pose_;
//!< @brief estimated ekf pose with covariance publisher
Expand All @@ -124,12 +124,18 @@ class EKFLocalizer : public rclcpp::Node
rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_yaw_bias_;
//!< @brief ekf estimated yaw bias publisher
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_biased_pose_;
//!< @brief ekf estimated yaw bias publisher MT added here
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_biased_pose_on_ndt_closed_state_;
//!< @brief ekf estimated yaw bias publisher
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_biased_pose_cov_;
//!< @brief initial pose subscriber
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_initialpose_;
//!< @brief measurement pose with covariance subscriber
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_pose_with_cov_;
//!< @brief measurement pose with covariance subscriber MT added here
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_pose_with_cov_gnss_;
//MT added here
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_ndt_switching;
//!< @brief measurement twist with covariance subscriber
rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
sub_twist_with_cov_;
Expand All @@ -151,7 +157,10 @@ class EKFLocalizer : public rclcpp::Node
Simple1DFilter pitch_filter_;

const HyperParameters params_;

// MT added here
bool switch_ndt = true;
std_msgs::msg::String rviz_logger;
double close_twist_counter_ = 0.0;
double ekf_rate_;
double ekf_dt_;

Expand Down Expand Up @@ -192,7 +201,12 @@ class EKFLocalizer : public rclcpp::Node
*/
void callbackPoseWithCovariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);

/**
/**
* @brief set poseWithCovariance measurement MT added here
*/
void callbackPoseWithCovarianceGnss(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);

/**
* @brief set twistWithCovariance measurement
*/
void callbackTwistWithCovariance(geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
Expand Down Expand Up @@ -269,8 +283,19 @@ class EKFLocalizer : public rclcpp::Node
const std_srvs::srv::SetBool::Request::SharedPtr req,
std_srvs::srv::SetBool::Response::SharedPtr res);

// MT added here
void serviceNDTSwitch(
const std_srvs::srv::SetBool::Request::SharedPtr req,
std_srvs::srv::SetBool::Response::SharedPtr res);

tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;

friend class EKFLocalizerTestSuite; // for test code

///MT added here
std::chrono::system_clock::time_point exe_start_time;
std::chrono::system_clock::time_point exe_end_time;
float exe_time;
int counter = 0;
};
#endif // EKF_LOCALIZER__EKF_LOCALIZER_HPP_
6 changes: 5 additions & 1 deletion localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,9 @@ class HyperParameters
twist_smoothing_steps(node->declare_parameter("twist_smoothing_steps", 2)),
proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)),
proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)),
proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005))
proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)),
ekf_number_(node->declare_parameter("ekf_number", 0.0))
// close_ndt_counter_(node->declare_parameter("close_ndt", 0))
{
}

Expand All @@ -59,6 +61,8 @@ class HyperParameters
const double proc_stddev_vx_c; //!< @brief vx process noise
const double proc_stddev_wz_c; //!< @brief wz process noise
const double proc_stddev_yaw_c; //!< @brief yaw process noise
double ekf_number_;
// int close_ndt_counter_;
};

#endif // EKF_LOCALIZER__HYPER_PARAMETERS_HPP_
93 changes: 93 additions & 0 deletions localization/ekf_localizer/launch/ekf_localizer.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,5 +36,98 @@
<remap from="ekf_twist_with_covariance" to="$(var output_twist_with_covariance_name)"/>

<param from="$(var param_file)"/>
<param name="ekf_number" value="0.0" />
</node>

<!-- <node pkg="ekf_localizer" exec="ekf_localizer" name="ekf_localizer_with_gnss" output="screen">-->
<!-- <remap from="in_pose_with_covariance" to="$(var input_pose_with_cov_name)"/>-->

<!-- <remap from="in_twist_with_covariance" to="$(var input_twist_with_cov_name)"/>-->

<!-- <remap from="initialpose" to="$(var input_initial_pose_name)"/>-->
<!-- <remap from="trigger_node_srv" to="$(var input_trigger_node_service_name)"/>-->

<!-- <param name="pose_frame_id" value="map"/>-->

<!-- <remap from="ekf_odom" to="$(var output_odom_name)_with_gnss"/>-->
<!-- <remap from="ekf_pose" to="$(var output_pose_name)_with_gnss"/>-->
<!-- <remap from="ekf_pose_with_covariance" to="$(var output_pose_with_covariance_name)_with_gnss"/>-->
<!-- <remap from="ekf_biased_pose" to="$(var output_biased_pose_name)_with_gnss"/>-->
<!-- <remap from="ekf_biased_pose_with_covariance" to="$(var output_biased_pose_with_covariance_name)_with_gnss"/>-->
<!-- <remap from="ekf_twist" to="$(var output_twist_name)_with_gnss"/>-->
<!-- <remap from="ekf_twist_with_covariance" to="$(var output_twist_with_covariance_name)_with_gnss"/>-->

<!-- <param from="$(var param_file)"/>-->
<!-- <param name="ekf_number" value="1.0" />-->
<!-- </node>-->


<node pkg="ekf_localizer" exec="ekf_localizer" name="ekf_localizer_only_velocity" output="screen">
<remap from="in_pose_with_covariance" to="$(var input_pose_with_cov_name)"/>

<remap from="in_twist_with_covariance" to="$(var input_twist_with_cov_name)"/>

<remap from="initialpose" to="$(var input_initial_pose_name)"/>
<remap from="trigger_node_srv" to="$(var input_trigger_node_service_name)"/>

<param name="pose_frame_id" value="map"/>

<remap from="ekf_odom" to="$(var output_odom_name)_only_velocity"/>
<remap from="ekf_pose" to="$(var output_pose_name)_only_velocity"/>
<remap from="ekf_pose_with_covariance" to="$(var output_pose_with_covariance_name)_only_velocity"/>
<remap from="ekf_biased_pose" to="$(var output_biased_pose_name)_only_velocity"/>
<remap from="ekf_biased_pose_with_covariance" to="$(var output_biased_pose_with_covariance_name)_only_velocity"/>
<remap from="ekf_twist" to="$(var output_twist_name)_only_velocity"/>
<remap from="ekf_twist_with_covariance" to="$(var output_twist_with_covariance_name)_only_velocity"/>

<param from="$(var param_file)"/>
<param name="ekf_number" value="2.0" />
<!-- <param name="close_ndt" value="0.0" />-->
</node>

<!-- <node pkg="ekf_localizer" exec="ekf_localizer" name="ekf_localizer_only_ndt" output="screen">-->
<!-- <remap from="in_pose_with_covariance" to="$(var input_pose_with_cov_name)"/>-->

<!-- <remap from="in_twist_with_covariance" to="$(var input_twist_with_cov_name)"/>-->

<!-- <remap from="initialpose" to="$(var input_initial_pose_name)"/>-->
<!-- <remap from="trigger_node_srv" to="$(var input_trigger_node_service_name)"/>-->

<!-- <param name="pose_frame_id" value="map"/>-->

<!-- <remap from="ekf_odom" to="$(var output_odom_name)_only_ndt"/>-->
<!-- <remap from="ekf_pose" to="$(var output_pose_name)_only_ndt"/>-->
<!-- <remap from="ekf_pose_with_covariance" to="$(var output_pose_with_covariance_name)_only_ndt"/>-->
<!-- <remap from="ekf_biased_pose" to="$(var output_biased_pose_name)_only_ndt"/>-->
<!-- <remap from="ekf_biased_pose_with_covariance" to="$(var output_biased_pose_with_covariance_name)_only_ndt"/>-->
<!-- <remap from="ekf_twist" to="$(var output_twist_name)_only_ndt"/>-->
<!-- <remap from="ekf_twist_with_covariance" to="$(var output_twist_with_covariance_name)_only_ndt"/>-->

<!-- <param from="$(var param_file)"/>-->
<!-- <param name="ekf_number" value="3.0" />-->
<!-- &lt;!&ndash; <param name="close_ndt" value="0.0" />&ndash;&gt;-->
<!-- </node>-->

<!-- <node pkg="ekf_localizer" exec="ekf_localizer" name="ekf_localizer_only_gnss" output="screen">-->
<!-- <remap from="in_pose_with_covariance" to="$(var input_pose_with_cov_name)"/>-->

<!-- <remap from="in_twist_with_covariance" to="$(var input_twist_with_cov_name)"/>-->

<!-- <remap from="initialpose" to="$(var input_initial_pose_name)"/>-->
<!-- <remap from="trigger_node_srv" to="$(var input_trigger_node_service_name)"/>-->

<!-- <param name="pose_frame_id" value="map"/>-->

<!-- <remap from="ekf_odom" to="$(var output_odom_name)_only_gnss"/>-->
<!-- <remap from="ekf_pose" to="$(var output_pose_name)_only_gnss"/>-->
<!-- <remap from="ekf_pose_with_covariance" to="$(var output_pose_with_covariance_name)_only_gnss"/>-->
<!-- <remap from="ekf_biased_pose" to="$(var output_biased_pose_name)_only_gnss"/>-->
<!-- <remap from="ekf_biased_pose_with_covariance" to="$(var output_biased_pose_with_covariance_name)_only_gnss"/>-->
<!-- <remap from="ekf_twist" to="$(var output_twist_name)_only_gnss"/>-->
<!-- <remap from="ekf_twist_with_covariance" to="$(var output_twist_with_covariance_name)_only_gnss"/>-->

<!-- <param from="$(var param_file)"/>-->
<!-- <param name="ekf_number" value="4.0" />-->
<!-- <param name="close_ndt" value="0.0" />-->
<!-- </node>-->
</launch>
103 changes: 99 additions & 4 deletions localization/ekf_localizer/src/ekf_localizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@
#include <queue>
#include <string>
#include <utility>

#include "std_msgs/msg/string.hpp"
// clang-format off
#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl
#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}}
Expand Down Expand Up @@ -76,6 +76,7 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(),
std::bind(&EKFLocalizer::timerTFCallback, this));

pub_rviz_string = create_publisher<std_msgs::msg::String>("/ndt_status",1);
pub_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_pose", 1);
pub_pose_cov_ =
create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("ekf_pose_with_covariance", 1);
Expand All @@ -85,14 +86,21 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
"ekf_twist_with_covariance", 1);
pub_yaw_bias_ = create_publisher<tier4_debug_msgs::msg::Float64Stamped>("estimated_yaw_bias", 1);
pub_biased_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_biased_pose", 1);
pub_biased_pose_on_ndt_closed_state_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_poses_on_ndt_closed_state",1);
pub_biased_pose_cov_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"ekf_biased_pose_with_covariance", 1);
sub_initialpose_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1));
sub_pose_with_cov_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"in_pose_with_covariance", 1, std::bind(&EKFLocalizer::callbackPoseWithCovariance, this, _1));
sub_pose_with_cov_gnss_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"/sensing/gnss/pose_with_covariance",1,std::bind(&EKFLocalizer::callbackPoseWithCovarianceGnss, this, _1));
sub_twist_with_cov_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
"in_twist_with_covariance", 1, std::bind(&EKFLocalizer::callbackTwistWithCovariance, this, _1));

service_ndt_switching = create_service<std_srvs::srv::SetBool>("switch_ndt", std::bind(
&EKFLocalizer::serviceNDTSwitch, this, std::placeholders::_1, std::placeholders::_2),
rclcpp::ServicesQoS().get_rmw_qos_profile());
service_trigger_node_ = create_service<std_srvs::srv::SetBool>(
"trigger_node_srv",
std::bind(
Expand Down Expand Up @@ -261,7 +269,10 @@ void EKFLocalizer::timerTFCallback()
geometry_msgs::msg::TransformStamped transform_stamped;
transform_stamped = tier4_autoware_utils::pose2transform(current_ekf_pose_, "base_link");
transform_stamped.header.stamp = this->now();
tf_br_->sendTransform(transform_stamped);

if(params_.ekf_number_ == 0.0) {
tf_br_->sendTransform(transform_stamped);
}
}

/*
Expand Down Expand Up @@ -334,17 +345,80 @@ void EKFLocalizer::callbackInitialPose(
initSimple1DFilters(*initialpose);
}


/*
* callbackPoseWithCovariance MT added here
*/
void EKFLocalizer::callbackPoseWithCovarianceGnss(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
if (!is_activated_) {
return;
}

if(params_.ekf_number_ == 1.0 || params_.ekf_number_ == 4.0) {
pose_queue_.push(msg);
// std::cout<<"GNSS ADDED -------"<<std::endl;
}
// else{
// std::cout<<"GNSS NOT ADDED -------"<<std::endl;
// }

}

/*
* callbackPoseWithCovariance
*/
void EKFLocalizer::callbackPoseWithCovariance(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{

if (!is_activated_) {
return;
}

pose_queue_.push(msg);
if(params_.ekf_number_ != 2.0 && (params_.ekf_number_ != 4.0 ) ) {
pose_queue_.push(msg);
}
else if(params_.ekf_number_ == 2.0 && exe_time >= 1 && exe_time <= 5) {
// else if(params_.ekf_number_ == 2.0 && switch_ndt == false ) {

exe_end_time = std::chrono::system_clock::now();
const auto duration_micro_sec =
std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
exe_time = static_cast<float>(duration_micro_sec) / 1000000.0f;

geometry_msgs::msg::PoseStamped pose_on_ndt_closed_state;
pose_on_ndt_closed_state.pose = msg->pose.pose;
pose_on_ndt_closed_state.header = msg->header;
pub_biased_pose_on_ndt_closed_state_->publish(pose_on_ndt_closed_state);
std::cout<<"NDT CLOSED--------------------"<<std::endl;
rviz_logger.data = "NDT Closed";
// }
counter = 0;
}
else {
pose_queue_.push(msg);
if(counter == 0){
exe_start_time = std::chrono::system_clock::now();
}
counter++;
exe_end_time = std::chrono::system_clock::now();
const auto duration_micro_sec =
std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
exe_time = static_cast<float>(duration_micro_sec) / 1000000.0f;
std::cout<<"NDT OPEN --------------------"<<std::endl;
rviz_logger.data = "NDT Active";

}
// std::cout<<"EXE_TIME : "<<exe_time<<std::endl;
if(rviz_logger.data != "") {
pub_rviz_string->publish(rviz_logger);
}
// else{
// rviz_logger.data = "NDT Active";
// pub_rviz_string->publish(rviz_logger);
// }
}

/*
Expand All @@ -353,7 +427,7 @@ void EKFLocalizer::callbackPoseWithCovariance(
void EKFLocalizer::callbackTwistWithCovariance(
geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg)
{
twist_queue_.push(msg);
twist_queue_.push(msg);
}

/*
Expand Down Expand Up @@ -658,3 +732,24 @@ void EKFLocalizer::serviceTriggerNode(
res->success = true;
return;
}

// MT added here
void EKFLocalizer::serviceNDTSwitch(
const std_srvs::srv::SetBool::Request::SharedPtr req,
std_srvs::srv::SetBool::Response::SharedPtr res){
if (req->data) {
pose_queue_.clear();
twist_queue_.clear();
switch_ndt = true;
// std::cout<<"--------- NDT Active --------"<<std::endl;
// rviz_logger.data = "NDT Active";
} else {
switch_ndt = false;
// rviz_logger.data = "NDT Closed";
// std::cout<<"--------- NDT Closed --------"<<std::endl;
}
res->success = true;
return;


}
2 changes: 2 additions & 0 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,8 @@ class NDTScanMatcher : public rclcpp::Node
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr ndt_pose_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
ndt_pose_with_covariance_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
ndt_pose_as_gnss_pose_name_with_covariance_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
initial_pose_with_covariance_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr exe_time_pub_;
Expand Down
3 changes: 1 addition & 2 deletions localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,13 +21,12 @@

<remap from="ekf_pose_with_covariance" to="$(var input_initial_pose_topic)"/>
<remap from="pointcloud_map" to="$(var input_map_points_topic)"/>

<remap from="ndt_pose" to="$(var output_pose_topic)"/>
<remap from="ndt_pose_with_covariance" to="$(var output_pose_with_covariance_topic)"/>
<remap from="regularization_pose_with_covariance" to="$(var input_regularization_pose_topic)"/>
<remap from="trigger_node_srv" to="$(var input_service_trigger_node)"/>
<remap from="pcd_loader_service" to="$(var client_map_loader)"/>
<remap from="ekf_odom" to="$(var input_ekf_odom)"/>
<remap from="ekf_odom" to="$(var input_ekf_odom)_with_gnss"/>

<param from="$(var param_file)"/>
</node>
Expand Down
Loading

0 comments on commit 855b490

Please sign in to comment.