Skip to content
Snippets Groups Projects
Commit cb306d88 authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Add get_current_velocity and get_status_text functions. Add nav_with_gui_main_tree.xml

parent dac820dd
No related branches found
No related tags found
No related merge requests found
......@@ -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
*
......
......@@ -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.
......
......@@ -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())
......
......@@ -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())
......
<?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>
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