Skip to content
Snippets Groups Projects
Commit a143f5ef authored by José Enrique Domínguez Vidal's avatar José Enrique Domínguez Vidal
Browse files

Added enable_timeout_nav_3d(), disable_timeout_nav_3d() and update_timeout_nav_3d() functions

parent 533ffd38
No related branches found
No related tags found
No related merge requests found
...@@ -531,6 +531,35 @@ class CIriAnaNavModuleBT ...@@ -531,6 +531,35 @@ class CIriAnaNavModuleBT
* *
*/ */
BT::NodeStatus costmap_is_auto_clear_enabled(); BT::NodeStatus costmap_is_auto_clear_enabled();
/**
* \brief Enable navigation timeout
*
* This function enables the navigation timeout using as input the duration
* in seconds of that timeout.
*
* \return a BT:NodeStatus::SUCCESS if the input is valid.
*
*/
BT::NodeStatus enable_timeout(BT::TreeNode& self);
/**
* \brief Disable navigation timeout
*
* This function disables the navigation timeout.
*
* \return a BT:NodeStatus::SUCCESS always.
*
*/
BT::NodeStatus disable_timeout();
/**
* \brief Update navigation timeout
*
* This function updates the navigation timeout using as input the duration
* in seconds of the new timeout.
*
* \return a BT:NodeStatus::SUCCESS if the input is valid.
*
*/
BT::NodeStatus update_timeout(BT::TreeNode& self);
}; };
......
...@@ -26,6 +26,8 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory) ...@@ -26,6 +26,8 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
BT::PortsList frame_port = { BT::InputPort<std::string>("frame_id") }; BT::PortsList frame_port = { BT::InputPort<std::string>("frame_id") };
BT::PortsList current_pose_port = { BT::OutputPort<double>("current_x_nav"), BT::OutputPort<double>("current_y_nav"), BT::OutputPort<double>("current_yaw_nav") }; BT::PortsList current_pose_port = { BT::OutputPort<double>("current_x_nav"), BT::OutputPort<double>("current_y_nav"), BT::OutputPort<double>("current_yaw_nav") };
BT::PortsList costmap_port = { BT::InputPort<double>("rate_hz") }; BT::PortsList costmap_port = { BT::InputPort<double>("rate_hz") };
BT::PortsList enable_timeout_port = { BT::InputPort<double>("timeout") };
BT::PortsList update_timeout_port = { BT::InputPort<double>("timeout") };
// detect huge variation in path // detect huge variation in path
BT::PortsList path_distance_port = { BT::OutputPort<double>("path_distance") }; BT::PortsList path_distance_port = { BT::OutputPort<double>("path_distance") };
...@@ -60,6 +62,10 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory) ...@@ -60,6 +62,10 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
factory.registerSimpleAction("costmaps_enable_auto_clear_nav_3d", std::bind(&CIriAnaNavModuleBT::costmaps_enable_auto_clear, this, std::placeholders::_1), costmap_port); factory.registerSimpleAction("costmaps_enable_auto_clear_nav_3d", std::bind(&CIriAnaNavModuleBT::costmaps_enable_auto_clear, this, std::placeholders::_1), costmap_port);
factory.registerSimpleAction("costmaps_disable_auto_clear_nav_3d", std::bind(&CIriAnaNavModuleBT::costmaps_disable_auto_clear, this)); factory.registerSimpleAction("costmaps_disable_auto_clear_nav_3d", std::bind(&CIriAnaNavModuleBT::costmaps_disable_auto_clear, this));
factory.registerSimpleAction("reset_goal_variables_nav_3d", std::bind(&CIriAnaNavModuleBT::reset_goal_variables, this)); factory.registerSimpleAction("reset_goal_variables_nav_3d", std::bind(&CIriAnaNavModuleBT::reset_goal_variables, this));
factory.registerSimpleAction("enable_timeout_nav_3d", std::bind(&CIriAnaNavModuleBT::enable_timeout, this, std::placeholders::_1), enable_timeout_port);
factory.registerSimpleAction("disable_timeout_nav_3d", std::bind(&CIriAnaNavModuleBT::disable_timeout, this));
factory.registerSimpleAction("update_timeout_nav_3d", std::bind(&CIriAnaNavModuleBT::update_timeout, this, std::placeholders::_1), update_timeout_port);
// detect huge variation in path // detect huge variation in path
factory.registerSimpleAction("get_current_path_distance_nav_3d", std::bind(&CIriAnaNavModuleBT::get_current_path_distance, this, std::placeholders::_1), path_distance_port); factory.registerSimpleAction("get_current_path_distance_nav_3d", std::bind(&CIriAnaNavModuleBT::get_current_path_distance, this, std::placeholders::_1), path_distance_port);
factory.registerSimpleAction("get_current_velocity_nav_3d", std::bind(&CIriAnaNavModuleBT::get_current_velocity, this, std::placeholders::_1), velocity_port); factory.registerSimpleAction("get_current_velocity_nav_3d", std::bind(&CIriAnaNavModuleBT::get_current_velocity, this, std::placeholders::_1), velocity_port);
...@@ -406,6 +412,44 @@ BT::NodeStatus CIriAnaNavModuleBT::reset_goal_variables() ...@@ -406,6 +412,44 @@ BT::NodeStatus CIriAnaNavModuleBT::reset_goal_variables()
return BT::NodeStatus::SUCCESS; return BT::NodeStatus::SUCCESS;
} }
BT::NodeStatus CIriAnaNavModuleBT::enable_timeout(BT::TreeNode& self)
{
BT::Optional<double> timeout_input = self.getInput<double>("timeout");
if (!timeout_input)
{
ROS_ERROR("CIriAnaNavModuleBT:enable_timeout Incorrect or missing required input [timeout]");
return BT::NodeStatus::FAILURE;
}
double timeout_desired = timeout_input.value();
this->nav.enable_timeout(timeout_desired);
return BT::NodeStatus::SUCCESS;
}
BT::NodeStatus CIriAnaNavModuleBT::disable_timeout()
{
this->nav.disable_timeout();
return BT::NodeStatus::SUCCESS;
}
BT::NodeStatus CIriAnaNavModuleBT::update_timeout(BT::TreeNode& self)
{
BT::Optional<double> timeout_input = self.getInput<double>("timeout");
if (!timeout_input)
{
ROS_ERROR("CIriAnaNavModuleBT:update_timeout Incorrect or missing required input [timeout]");
return BT::NodeStatus::FAILURE;
}
double timeout_new = timeout_input.value();
this->nav.update_timeout(timeout_new);
return BT::NodeStatus::SUCCESS;
}
BT::NodeStatus CIriAnaNavModuleBT::get_current_path_distance(BT::TreeNode& self) BT::NodeStatus CIriAnaNavModuleBT::get_current_path_distance(BT::TreeNode& self)
{ {
ROS_DEBUG("CIriAnaNavModuleBT:get_current_path_distance"); ROS_DEBUG("CIriAnaNavModuleBT:get_current_path_distance");
...@@ -704,7 +748,6 @@ BT::NodeStatus CIriAnaNavModuleBT::set_goal_frame(BT::TreeNode& self) ...@@ -704,7 +748,6 @@ BT::NodeStatus CIriAnaNavModuleBT::set_goal_frame(BT::TreeNode& self)
ROS_INFO("CIriAnaNavModuleBT:set_goal_frame"); ROS_INFO("CIriAnaNavModuleBT:set_goal_frame");
BT::Optional<std::string> frame_id_input = self.getInput<std::string>("frame_id"); BT::Optional<std::string> frame_id_input = self.getInput<std::string>("frame_id");
if (!frame_id_input) if (!frame_id_input)
{ {
ROS_ERROR("CIriAnaNavModuleBT:set_goal_frame Incorrect or missing required input [frame_id]"); ROS_ERROR("CIriAnaNavModuleBT:set_goal_frame Incorrect or missing required input [frame_id]");
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment