Skip to content

Commit

Permalink
service working
Browse files Browse the repository at this point in the history
  • Loading branch information
@meliketanrikulu
meliketanrikulu committed Mar 1, 2024
1 parent ddc866e commit 194442c
Show file tree
Hide file tree
Showing 5 changed files with 45 additions and 64 deletions.
83 changes: 34 additions & 49 deletions localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,72 +31,57 @@ AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode() : Node("AWPoseCovar

debug_pose_with_cov_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("/aw_pose_covariance_modifier/debug/pose_with_cov_stamped",10);

client_ = this->create_client<rcl_interfaces::srv::SetParameters>("/localization/pose_estimator/ndt_scan_matcher/set_parameters");
client_ = this->create_client<rcl_interfaces::srv::SetParameters>("/localization/pose_estimator/ndt_scan_matcher/set_parameters_");

while (!client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
RCLCPP_DEBUG_THROTTLE(
this->get_logger(), *get_clock(), 5000,
"Waiting for pcd map loader service. Check if the enable_selected_load in "
"pointcloud_map_loader is set `true`.");
}

startNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier();

while(startNDTCovModifier != true && rclcpp::ok()){
startNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier();
}
if (startNDTCovModifier == 1) {
RCLCPP_INFO(get_logger(), "NDT pose covariance modifier activated ...");
}
else{
startNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier();
RCLCPP_INFO(get_logger(), "Failed to enable NDT pose covariance modifier ...");
}
}
std::string futureStatusToString(std::future_status status) {
switch (status) {
case std::future_status::ready:
return "ready";
case std::future_status::timeout:
return "timeout";
case std::future_status::deferred:
return "deferred";
default:
return "unknown";
}
}
bool AWPoseCovarianceModifierNode::callNDTCovarianceModifier() {
while (!client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(get_logger(), "Interrupted while waiting for the service. Exiting.");
return false;
}
RCLCPP_INFO(get_logger(), "Service not available, waiting again...");
}

auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();

rcl_interfaces::msg::Parameter parameter;
parameter.name = "aw_pose_covariance_modifier.enable";
parameter.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
parameter.value.bool_value = true;
bool AWPoseCovarianceModifierNode::callNDTCovarianceModifier() {

request->parameters.push_back(parameter);
auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();

auto future_result = client_->async_send_request(request);
rcl_interfaces::msg::Parameter parameter;
parameter.name = "aw_pose_covariance_modifier.enable";
parameter.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
parameter.value.bool_value = true;

auto result_status = future_result.wait_for(std::chrono::seconds(10));
std::string status_str = futureStatusToString(result_status);
RCLCPP_INFO(this->get_logger(), "Result Status: %s", status_str.c_str());
request->parameters.push_back(parameter);

if (result_status == std::future_status::ready) {
client_->async_send_request(
request, [this](rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedFuture result_) {
auto result_status = result_.wait_for(std::chrono::seconds(1));

auto response = future_result.get();
if (result_status == std::future_status::ready) {
auto response = result_.get();

if (response && response->results.data()) {
RCLCPP_INFO(this->get_logger(), "Parametreler başarıyla ayarlandı.");
return true;
} else {
RCLCPP_ERROR(this->get_logger(), "Parametre ayarlanırken bir hata oluştu.");
return false;
}
} else {
RCLCPP_ERROR(this->get_logger(), "İstek belirtilen süre içinde tamamlanmadı.");
return false;
}
if (response && response->results.data()) {
RCLCPP_INFO(this->get_logger(), "Parameters set successfully");
return true;
} else {
RCLCPP_ERROR(this->get_logger(), "An error occurred while setting the parameter.");
return false;
}
} else {
RCLCPP_ERROR(this->get_logger(), "The request was not completed within the specified time.");
return false;
}

});
return false;
}

void AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback(
Expand Down
1 change: 1 addition & 0 deletions localization/ndt_scan_matcher/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.14)
project(ndt_scan_matcher)

find_package(autoware_cmake REQUIRED)
find_package(rcl_interfaces REQUIRED)
autoware_package()

# Compile flags for SIMD instructions
Expand Down
4 changes: 3 additions & 1 deletion localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ struct HyperParameters
double lidar_radius;
} dynamic_map_loading;


bool enable_aw_pose_covariance_modifier;

public:
explicit HyperParameters(rclcpp::Node * node)
Expand Down Expand Up @@ -170,6 +170,8 @@ struct HyperParameters
dynamic_map_loading.lidar_radius =
node->declare_parameter<double>("dynamic_map_loading.lidar_radius");

enable_aw_pose_covariance_modifier = node->declare_parameter<bool>("aw_pose_covariance_modifier.enable", false);

}
};

Expand Down
1 change: 1 addition & 0 deletions localization/ndt_scan_matcher/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
<depend>tier4_localization_msgs</depend>
<depend>tree_structured_parzen_estimator</depend>
<depend>visualization_msgs</depend>
<depend>rcl_interfaces</depend>

<test_depend>ament_cmake_cppcheck</test_depend>
<test_depend>ament_lint_auto</test_depend>
Expand Down
20 changes: 6 additions & 14 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,7 +190,7 @@ NDTScanMatcher::NDTScanMatcher()
rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group);

set_parameters_service_ = this->create_service<rcl_interfaces::srv::SetParameters>(
"ndt_scan_matcher/set_parameters",
"ndt_scan_matcher/set_parameters_",
std::bind(
&NDTScanMatcher::setParametersCallback, this, std::placeholders::_1, std::placeholders::_2),
rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group);
Expand All @@ -213,29 +213,21 @@ void NDTScanMatcher::setParametersCallback(
{
std::lock_guard<std::mutex> lock(*mutex_mt_);
std::cout<<"xdxdxddxddxd"<<std::endl;

for (const auto &param : request->parameters) {

if(param.name == "aw_pose_covariance_modifier.enable"){

activate_pose_covariance_modifier_ = param.value.bool_value;
RCLCPP_INFO(this->get_logger(), "aw_pose_covariance_modifier.enable set to : %d", activate_pose_covariance_modifier_);

// rcl_interfaces::msg::SetParametersResult result;
// result.successful = true;
// result.reason = "Parameter successfully set.";
//
// response->results.push_back(result);
// std::cout<<"ppppppppp: "<<response->results.data()->successful<<std::endl;
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
result.reason = "Parameter successfully set.";

response->results.push_back(result);
}
}

rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
result.reason = "Parameter successfully set.";

response->results.push_back(result);
std::cout<<"ppppppppp: "<<response->results.data()->successful<<std::endl;
}
void NDTScanMatcher::publish_diagnostic()
{
Expand Down

0 comments on commit 194442c

Please sign in to comment.