From cb306d88f41d0f1c31c653ea8bc47c6c308da44e Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Thu, 27 Feb 2020 16:50:28 +0100
Subject: [PATCH] Add get_current_velocity and get_status_text functions. Add
nav_with_gui_main_tree.xml
---
.../iri_ana_nav_module/iri_ana_nav_module.h | 22 ++++-
.../iri_ana_nav_module_bt.h | 4 +
src/iri_ana_nav_module.cpp | 51 ++++++++++
src/iri_ana_nav_module_bt.cpp | 25 ++++-
src/xml/nav_with_gui_main_tree.xml | 98 +++++++++++++++++++
5 files changed, 198 insertions(+), 2 deletions(-)
create mode 100644 src/xml/nav_with_gui_main_tree.xml
diff --git a/include/iri_ana_nav_module/iri_ana_nav_module.h b/include/iri_ana_nav_module/iri_ana_nav_module.h
index bcaa776..871e263 100644
--- a/include/iri_ana_nav_module/iri_ana_nav_module.h
+++ b/include/iri_ana_nav_module/iri_ana_nav_module.h
@@ -164,6 +164,11 @@ class CIriAnaNavModule : public CModule<iri_ana_nav_module::IriAnaNavModuleConfi
*
*/
geometry_msgs::PoseStamped current_pose;
+ /**
+ * \brief
+ *
+ */
+ geometry_msgs::Twist current_velocity;
/**
* \brief
*
@@ -535,7 +540,22 @@ class CIriAnaNavModule : public CModule<iri_ana_nav_module::IriAnaNavModuleConfi
* path.
*/
int get_path_length(void);
-
+ /**
+ * \brief Get the current velocity (twist) of the robot
+ *
+ * This function returns a Twist message with the current robot velocity received in the Odometry messages
+ *
+ * \return A Twist message with the robot velocities
+ */
+ geometry_msgs::Twist get_current_velocity(void);
+ /**
+ * \brief Get the current navigation status as a text string
+ *
+ * This function returns a text string with the current navigation status information
+ *
+ * \return A std::string text with the status
+ */
+ std::string get_status_text(void);
/**
* \brief Destructor
*
diff --git a/include/iri_ana_nav_module/iri_ana_nav_module_bt.h b/include/iri_ana_nav_module/iri_ana_nav_module_bt.h
index 7708fc5..0c38ee1 100644
--- a/include/iri_ana_nav_module/iri_ana_nav_module_bt.h
+++ b/include/iri_ana_nav_module/iri_ana_nav_module_bt.h
@@ -259,6 +259,10 @@ class CIriAnaNavModuleBT
BT::NodeStatus get_current_path_distance(BT::TreeNode& self);
+ BT::NodeStatus get_current_velocity(BT::TreeNode& self);
+
+ BT::NodeStatus get_status_text(BT::TreeNode& self);
+
// Conditions to check if the last goal has finished or not and how
/**
* \brief Checks if the last goal has finished or not.
diff --git a/src/iri_ana_nav_module.cpp b/src/iri_ana_nav_module.cpp
index 12110f6..048fed9 100644
--- a/src/iri_ana_nav_module.cpp
+++ b/src/iri_ana_nav_module.cpp
@@ -207,6 +207,7 @@ void CIriAnaNavModule::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
// save the current position of the robot
this->current_pose.pose=msg->pose.pose;
this->current_pose.header=msg->header;
+ this->current_velocity = msg->twist.twist;
this->unlock();
}
@@ -648,6 +649,56 @@ int CIriAnaNavModule::get_path_length(void){
}
}
+geometry_msgs::Twist CIriAnaNavModule::get_current_velocity(void)
+{
+ return this->current_velocity;
+}
+
+std::string CIriAnaNavModule::get_status_text(void)
+{
+ std::string status_text;
+ switch(this->status)
+ {
+ case IRI_ANA_NAV_MODULE_RUNNING:
+ status_text = "Running";
+ break;
+ case IRI_ANA_NAV_MODULE_SUCCESS:
+ status_text = "Succeeded";
+ break;
+ case IRI_ANA_NAV_MODULE_ACTION_START_FAIL:
+ status_text = "Failed at start";
+ break;
+ case IRI_ANA_NAV_MODULE_TIMEOUT:
+ status_text = "Failed by timeout";
+ break;
+ case IRI_ANA_NAV_MODULE_FB_WATCHDOG:
+ status_text = "Failed by watchdog";
+ break;
+ case IRI_ANA_NAV_MODULE_ABORTED:
+ status_text = "Aborted";
+ break;
+ case IRI_ANA_NAV_MODULE_PREEMPTED:
+ status_text = "Preempted";
+ break;
+ case IRI_ANA_NAV_MODULE_REJECTED:
+ status_text = "Rejected";
+ break;
+ case IRI_ANA_NAV_MODULE_SET_PARAM_FAIL:
+ status_text = "Failed by param set";
+ break;
+ case IRI_ANA_NAV_MODULE_NO_ODOM:
+ status_text = "Failed by no odom received";
+ break;
+ case IRI_ANA_NAV_MODULE_NO_TRANSFORM:
+ status_text = "Failed by no transform received";
+ break;
+ default:
+ status_text = "Unknown status";
+ break;
+ }
+ return status_text;
+}
+
CIriAnaNavModule::~CIriAnaNavModule()
{
if(!this->move_base_action.is_finished())
diff --git a/src/iri_ana_nav_module_bt.cpp b/src/iri_ana_nav_module_bt.cpp
index ea2077d..6f1aeea 100644
--- a/src/iri_ana_nav_module_bt.cpp
+++ b/src/iri_ana_nav_module_bt.cpp
@@ -20,7 +20,7 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
BT::PortsList sync_pose_port = { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::InputPort<double>("x_y_pos_tol") };
BT::PortsList async_orientation_port = { BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::BidirectionalPort<bool>("update_goal") };
BT::PortsList async_position_port = { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("x_y_pos_tol"), BT::BidirectionalPort<bool>("update_goal")};
- BT::PortsList async_pose_port = { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::InputPort<double>("x_y_pos_tol"), BT::BidirectionalPort<bool>("update_goal"), BT::BidirectionalPort<bool>("salida_estado") };
+ BT::PortsList async_pose_port = { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::InputPort<double>("x_y_pos_tol"), BT::BidirectionalPort<bool>("update_goal") };
BT::PortsList status_code_port = { BT::OutputPort<iri_ana_nav_module_status_t>("status_code")};
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") };
@@ -28,6 +28,9 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
// detect huge variation in path
BT::PortsList path_distance_port = { BT::OutputPort<double>("path_distance") };
+
+ BT::PortsList velocity_port = { BT::OutputPort<double>("velocity") };
+ BT::PortsList status_text_port = { BT::OutputPort<std::string>("nav_status") };
// registry of conditions
factory.registerSimpleCondition("is_finished_nav", std::bind(&CIriAnaNavModuleBT::is_finished_nav, this));
@@ -58,6 +61,8 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
factory.registerSimpleAction("reset_goal_variables", std::bind(&CIriAnaNavModuleBT::reset_goal_variables, this));
// detect huge variation in path
factory.registerSimpleAction("get_current_path_distance", std::bind(&CIriAnaNavModuleBT::get_current_path_distance, this, std::placeholders::_1), path_distance_port);
+ factory.registerSimpleAction("get_current_velocity", std::bind(&CIriAnaNavModuleBT::get_current_velocity, this, std::placeholders::_1), velocity_port);
+ factory.registerSimpleAction("get_status_text", std::bind(&CIriAnaNavModuleBT::get_status_text, this, std::placeholders::_1), status_text_port);
// registry of asynchronous actions
factory.registerIriAsyncAction("async_go_to_orientation", std::bind(&CIriAnaNavModuleBT::async_go_to_orientation, this, std::placeholders::_1), async_orientation_port);
@@ -416,6 +421,24 @@ BT::NodeStatus CIriAnaNavModuleBT::get_current_path_distance(BT::TreeNode& self)
return BT::NodeStatus::SUCCESS;
}
+BT::NodeStatus CIriAnaNavModuleBT::get_current_velocity(BT::TreeNode& self)
+{
+ ROS_DEBUG("get_current_velocity");
+
+ geometry_msgs::Twist twist = this->nav.get_current_velocity();
+ self.setOutput("velocity", twist.linear.x);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::get_status_text(BT::TreeNode& self)
+{
+ ROS_DEBUG("get_status_text");
+
+ std::string status_text = this->nav.get_status_text();
+ self.setOutput("nav_status", status_text);
+ return BT::NodeStatus::SUCCESS;
+}
+
BT::NodeStatus CIriAnaNavModuleBT::is_finished_nav()
{
if (nav.is_finished())
diff --git a/src/xml/nav_with_gui_main_tree.xml b/src/xml/nav_with_gui_main_tree.xml
new file mode 100644
index 0000000..fadb784
--- /dev/null
+++ b/src/xml/nav_with_gui_main_tree.xml
@@ -0,0 +1,98 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="NAV_MAIN">
+ <!-- ////////// -->
+ <BehaviorTree ID="NAV_MAIN">
+ <Sequence>
+ <Action ID="sync_start_nav_gui"/>
+ <Fallback>
+ <ReactiveSequence>
+ <Action ID="get_status_text" nav_status="{status_text}"/>
+ <Action ID="set_nav_status_nav_gui" nav_status="{status_text}"/>
+ <Action ID="get_current_velocity" velocity="{velocity}"/>
+ <Action ID="set_current_velocity_nav_gui" velocity="{velocity}"/>
+ <Action ID="get_current_path_distance" path_distance="{path_distance}"/>
+ <Action ID="set_distance_to_goal_nav_gui" distance_to_goal="{path_distance}"/>
+ <Fallback>
+ <Sequence>
+ <SubTree ID="go_to_goal" f_id="f_id" heading_tol="heading_tol" output_go_to_goal="NAV_BT_status" update_goal="update_goal" x="x" x_y_pos_tol="x_y_pos_tol" y="y" yaw="yaw"/>
+ <SetBlackboard output_key="output_nav" value="{NAV_BT_status}"/>
+ </Sequence>
+ <ForceFailure>
+ <Sequence>
+ <Action ID="get_current_status_nav" status_code="{NAV_BT_status}"/>
+ <SetBlackboard output_key="output_nav" value="{NAV_BT_status}"/>
+ </Sequence>
+ </ForceFailure>
+ </Fallback>
+ </ReactiveSequence>
+ <ForceFailure>
+ <Action ID="sync_stop_nav"/>
+ </ForceFailure>
+ </Fallback>
+ </Sequence>
+ </BehaviorTree>
+ <!-- ////////// -->
+ <BehaviorTree ID="go_to_goal">
+ <Sequence>
+ <Action ID="set_goal_frame" frame_id="{f_id}"/>
+ <Timeout msec="600000">
+ <RetryUntilSuccesful num_attempts="3">
+ <Action ID="async_go_to_pose" heading_tol="{heading_tol}" update_goal="{update_goal}" x="{x}" x_y_pos_tol="{x_y_pos_tol}" y="{y}" yaw="{yaw}"/>
+ </RetryUntilSuccesful>
+ </Timeout>
+ <Condition ID="is_succeded_nav"/>
+ <Action ID="get_current_status_nav" status_code="{NAV_BT_status}"/>
+ <SetBlackboard output_key="output_go_to_goal" value="{NAV_BT_status}"/>
+ </Sequence>
+ </BehaviorTree>
+ <!-- ////////// -->
+ <TreeNodesModel>
+ <Action ID="async_go_to_pose">
+ <inout_port name="heading_tol"/>
+ <inout_port name="update_goal"/>
+ <inout_port name="x"/>
+ <inout_port name="x_y_pos_tol"/>
+ <inout_port name="y"/>
+ <inout_port name="yaw"/>
+ </Action>
+ <Action ID="get_current_path_distance">
+ <inout_port name="path_distance"/>
+ </Action>
+ <Action ID="get_current_status_nav">
+ <inout_port name="status_code"/>
+ </Action>
+ <Action ID="get_current_velocity">
+ <inout_port name="velocity"/>
+ </Action>
+ <Action ID="get_status_text">
+ <inout_port name="nav_status"/>
+ </Action>
+ <SubTree ID="go_to_goal">
+ <inout_port name="f_id"/>
+ <inout_port name="heading_tol"/>
+ <inout_port name="output_go_to_goal"/>
+ <inout_port name="update_goal"/>
+ <inout_port name="x"/>
+ <inout_port name="x_y_pos_tol"/>
+ <inout_port name="y"/>
+ <inout_port name="yaw"/>
+ </SubTree>
+ <Condition ID="is_succeded_nav"/>
+ <Action ID="set_current_velocity_nav_gui">
+ <inout_port name="velocity"/>
+ </Action>
+ <Action ID="set_distance_to_goal_nav_gui">
+ <inout_port name="distance_to_goal"/>
+ </Action>
+ <Action ID="set_goal_frame">
+ <inout_port name="frame_id"/>
+ </Action>
+ <Action ID="set_nav_status_nav_gui">
+ <inout_port name="nav_status"/>
+ </Action>
+ <Action ID="sync_start_nav_gui"/>
+ <Action ID="sync_stop_nav"/>
+ </TreeNodesModel>
+ <!-- ////////// -->
+</root>
+
--
GitLab