From 993aa946decaf1245573b39d80bddbaa640b9c05 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Tue, 23 May 2023 16:59:21 +0200
Subject: [PATCH] Fixed missname in get/set_restart_dist
---
include/iri_nav_module/follow_lp_module.h | 4 +--
include/iri_nav_module/follow_lp_module_bt.h | 38 ++++++++++++++------
src/xml/bt_definitions.xml | 4 +--
3 files changed, 31 insertions(+), 15 deletions(-)
diff --git a/include/iri_nav_module/follow_lp_module.h b/include/iri_nav_module/follow_lp_module.h
index ec76a21..0468f07 100644
--- a/include/iri_nav_module/follow_lp_module.h
+++ b/include/iri_nav_module/follow_lp_module.h
@@ -143,14 +143,14 @@ class CFollowLPModule : public CNavPlannerModule<ModuleCfg>
}
template <class ModuleCfg>
- dyn_reconf_status_t CFollowLPModule<ModuleCfg>::get_follow_restart_dist(double &value)
+ dyn_reconf_status_t CFollowLPModule<ModuleCfg>::set_follow_restart_dist(double &value)
{
this->planner_reconf.set_parameter("follow_restart_dist",value);
return this->planner_reconf.get_status();
}
template <class ModuleCfg>
- dyn_reconf_status_t CFollowLPModule<ModuleCfg>::set_follow_restart_dist(double &value)
+ dyn_reconf_status_t CFollowLPModule<ModuleCfg>::get_follow_restart_dist(double &value)
{
if(this->planner_reconf.get_parameter("follow_restart_dist",value))
return DYN_RECONF_SUCCESSFULL;
diff --git a/include/iri_nav_module/follow_lp_module_bt.h b/include/iri_nav_module/follow_lp_module_bt.h
index cba089d..0db0baa 100644
--- a/include/iri_nav_module/follow_lp_module_bt.h
+++ b/include/iri_nav_module/follow_lp_module_bt.h
@@ -48,8 +48,8 @@ class CFollowLPModuleBT
BT::NodeStatus get_update_dist(BT::TreeNode& self);
BT::NodeStatus set_follow_dist(BT::TreeNode& self);
BT::NodeStatus get_follow_dist(BT::TreeNode& self);
- BT::NodeStatus get_follow_restart_dist(BT::TreeNode& self);
BT::NodeStatus set_follow_restart_dist(BT::TreeNode& self);
+ BT::NodeStatus get_follow_restart_dist(BT::TreeNode& self);
BT::NodeStatus set_spline_dist_samples(BT::TreeNode& self);
BT::NodeStatus get_spline_dist_samples(BT::TreeNode& self);
};
@@ -69,8 +69,8 @@ class CFollowLPModuleBT
BT::PortsList get_update_dist_ports = {BT::OutputPort<double>("update_dist")};
BT::PortsList set_follow_dist_ports = {BT::InputPort<double>("follow_dist")};
BT::PortsList get_follow_dist_ports = {BT::OutputPort<double>("follow_dist")};
- BT::PortsList get_follow_restart_dist_ports = {BT::InputPort<double>("follow_restart_dist")};
- BT::PortsList set_follow_restart_dist_ports = {BT::OutputPort<double>("follow_restart_dist")};
+ BT::PortsList set_follow_restart_dist_ports = {BT::InputPort<double>("follow_restart_dist")};
+ BT::PortsList get_follow_restart_dist_ports = {BT::OutputPort<double>("follow_restart_dist")};
BT::PortsList set_spline_dist_samples_ports = {BT::InputPort<int>("spline_dist_samples")};
BT::PortsList get_spline_dist_samples_ports = {BT::OutputPort<int>("spline_dist_samples")};
@@ -82,8 +82,8 @@ class CFollowLPModuleBT
factory.registerSimpleAction(this->name+"_follow_get_update_dist", std::bind(&CFollowLPModuleBT::get_update_dist, this, std::placeholders::_1), get_update_dist_ports);
factory.registerSimpleAction(this->name+"_follow_set_follow_dist", std::bind(&CFollowLPModuleBT::set_follow_dist, this, std::placeholders::_1), set_follow_dist_ports);
factory.registerSimpleAction(this->name+"_follow_get_follow_dist", std::bind(&CFollowLPModuleBT::get_follow_dist, this, std::placeholders::_1), get_follow_dist_ports);
- factory.registerSimpleAction(this->name+"_follow_get_follow_restart_dist", std::bind(&CFollowLPModuleBT::get_follow_restart_dist, this, std::placeholders::_1), get_follow_restart_dist_ports);
factory.registerSimpleAction(this->name+"_follow_set_follow_restart_dist", std::bind(&CFollowLPModuleBT::set_follow_restart_dist, this, std::placeholders::_1), set_follow_restart_dist_ports);
+ factory.registerSimpleAction(this->name+"_follow_get_follow_restart_dist", std::bind(&CFollowLPModuleBT::get_follow_restart_dist, this, std::placeholders::_1), get_follow_restart_dist_ports);
factory.registerSimpleAction(this->name+"_follow_set_spline_dist_samples", std::bind(&CFollowLPModuleBT::set_spline_dist_samples, this, std::placeholders::_1), set_spline_dist_samples_ports);
factory.registerSimpleAction(this->name+"_follow_get_spline_dist_samples", std::bind(&CFollowLPModuleBT::get_spline_dist_samples, this, std::placeholders::_1), get_spline_dist_samples_ports);
}
@@ -109,7 +109,10 @@ class CFollowLPModuleBT
if(this->module.set_update_dist(update_dist)==DYN_RECONF_SUCCESSFULL)
return BT::NodeStatus::SUCCESS;
else
+ {
+ ROS_ERROR("CFollowLPModuleBT::set_update_dist-> Error");
return BT::NodeStatus::FAILURE;
+ }
}
template <class ModuleCfg>
@@ -126,6 +129,7 @@ class CFollowLPModuleBT
else
{ //Just beacuse in case of error to put something on the output port
self.setOutput("update_dist", update_dist);
+ ROS_ERROR("CFollowLPModuleBT::get_update_dist-> Error");
return BT::NodeStatus::FAILURE;
}
}
@@ -145,7 +149,10 @@ class CFollowLPModuleBT
if(this->module.set_follow_dist(follow_dist)==DYN_RECONF_SUCCESSFULL)
return BT::NodeStatus::SUCCESS;
else
+ {
+ ROS_ERROR("CFollowLPModuleBT::set_follow_dist-> Error");
return BT::NodeStatus::FAILURE;
+ }
}
template <class ModuleCfg>
@@ -162,35 +169,39 @@ class CFollowLPModuleBT
else
{ //Just beacuse in case of error to put something on the output port
self.setOutput("follow_dist", follow_dist);
+ ROS_ERROR("CFollowLPModuleBT::get_follow_dist-> Error");
return BT::NodeStatus::FAILURE;
}
}
template <class ModuleCfg>
- BT::NodeStatus CFollowLPModuleBT<ModuleCfg>::get_follow_restart_dist(BT::TreeNode& self)
+ BT::NodeStatus CFollowLPModuleBT<ModuleCfg>::set_follow_restart_dist(BT::TreeNode& self)
{
- ROS_DEBUG("CFollowLPModuleBT::get_follow_restart_dist-> get_follow_restart_dist");
+ ROS_DEBUG("CFollowLPModuleBT::set_follow_restart_dist-> set_follow_restart_dist");
BT::Optional<double> bt_follow_restart_dist = self.getInput<double>("follow_restart_dist");
if (!bt_follow_restart_dist)
{
- ROS_ERROR("CFollowLPModuleBT::get_follow_restart_dist-> Incorrect or missing input. It needs the following input ports: follow_restart_dist(double)");
+ ROS_ERROR("CFollowLPModuleBT::set_follow_restart_dist-> Incorrect or missing input. It needs the following input ports: follow_restart_dist(double)");
return BT::NodeStatus::FAILURE;
}
double follow_restart_dist=bt_follow_restart_dist.value();
- if(this->module.get_follow_restart_dist(follow_restart_dist)==DYN_RECONF_SUCCESSFULL)
+ if(this->module.set_follow_restart_dist(follow_restart_dist)==DYN_RECONF_SUCCESSFULL)
return BT::NodeStatus::SUCCESS;
else
+ {
+ ROS_ERROR("CFollowLPModuleBT::set_follow_restart_dist-> Error");
return BT::NodeStatus::FAILURE;
+ }
}
template <class ModuleCfg>
- BT::NodeStatus CFollowLPModuleBT<ModuleCfg>::set_follow_restart_dist(BT::TreeNode& self)
+ BT::NodeStatus CFollowLPModuleBT<ModuleCfg>::get_follow_restart_dist(BT::TreeNode& self)
{
- ROS_DEBUG("CFollowLPModuleBT::set_follow_restart_dist-> set_follow_restart_dist");
+ ROS_DEBUG("CFollowLPModuleBT::get_follow_restart_dist-> get_follow_restart_dist");
double follow_restart_dist;
- if(this->module.set_follow_restart_dist(follow_restart_dist)==DYN_RECONF_SUCCESSFULL)
+ if(this->module.get_follow_restart_dist(follow_restart_dist)==DYN_RECONF_SUCCESSFULL)
{
self.setOutput("follow_restart_dist", follow_restart_dist);
return BT::NodeStatus::SUCCESS;
@@ -198,6 +209,7 @@ class CFollowLPModuleBT
else
{ //Just beacuse in case of error to put something on the output port
self.setOutput("follow_restart_dist", follow_restart_dist);
+ ROS_ERROR("CFollowLPModuleBT::get_follow_restart_dist-> Error");
return BT::NodeStatus::FAILURE;
}
}
@@ -217,7 +229,10 @@ class CFollowLPModuleBT
if(this->module.set_spline_dist_samples(dist_samples)==DYN_RECONF_SUCCESSFULL)
return BT::NodeStatus::SUCCESS;
else
+ {
+ ROS_ERROR("CFollowLPModuleBT::set_spline_dist_samples-> Error");
return BT::NodeStatus::FAILURE;
+ }
}
template <class ModuleCfg>
@@ -234,6 +249,7 @@ class CFollowLPModuleBT
else
{ //Just beacuse in case of error to put something on the output port
self.setOutput("dist_samples", dist_samples);
+ ROS_ERROR("CFollowLPModuleBT::get_spline_dist_samples-> Error");
return BT::NodeStatus::FAILURE;
}
}
diff --git a/src/xml/bt_definitions.xml b/src/xml/bt_definitions.xml
index 0c48e33..018ba5c 100644
--- a/src/xml/bt_definitions.xml
+++ b/src/xml/bt_definitions.xml
@@ -172,10 +172,10 @@
<Action ID="nav_module_follow_get_follow_dist">
<output_port name="follow_dist"> Local planner follow_dist parameter</output_port>
</Action>
- <Action ID="nav_module_follow_get_follow_restart_dist">
+ <Action ID="nav_module_follow_set_follow_restart_dist">
<input_port name="follow_restart_dist"> Local planner follow_restart_dist parameter</input_port>
</Action>
- <Action ID="nav_module_follow_set_follow_restart_dist">
+ <Action ID="nav_module_follow_get_follow_restart_dist">
<output_port name="follow_restart_dist"> Local planner follow_restart_dist parameter</output_port>
</Action>
<Action ID="nav_module_follow_set_spline_dist_samples">
--
GitLab