From 5d5020d01efa92f6dc9b0e93a4cbd42510319622 Mon Sep 17 00:00:00 2001
From: nrodriguez <nicolas.adrian.rodriguez@upc.edu>
Date: Wed, 18 May 2022 19:28:42 +0200
Subject: [PATCH] Added function to get the path and the current speed of the
robot
---
include/iri_nav_module/nav_module.h | 31 ++++++++++++
include/iri_nav_module/nav_module_bt.h | 65 ++++++++++++++++++++++++--
2 files changed, 92 insertions(+), 4 deletions(-)
diff --git a/include/iri_nav_module/nav_module.h b/include/iri_nav_module/nav_module.h
index cc0b080..6fe4a69 100644
--- a/include/iri_nav_module/nav_module.h
+++ b/include/iri_nav_module/nav_module.h
@@ -83,6 +83,11 @@ class CNavModule : public CModule<ModuleCfg>
*
*/
void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
+ /**
+ * \brief
+ *
+ */
+ geometry_msgs::Twist twist_msg;
/**
* \brief
@@ -279,6 +284,18 @@ class CNavModule : public CModule<ModuleCfg>
* enabled (true) or not (false).
*/
bool costamp_is_auto_clear_enabled(void);
+ /**
+ * \brief Function to get the current speed of the robot.
+ *
+ * \return The robot speed.
+ */
+ geometry_msgs::Twist get_current_speed(void);
+ /**
+ * \brief Function to get the current path.
+ *
+ * \return The current path.
+ */
+ nav_msgs::Path get_current_path(void);
/**
* \brief Function to get the current pose of the robot.
*
@@ -468,6 +485,8 @@ class CNavModule : public CModule<ModuleCfg>
// save the current position of the robot
this->current_pose.pose=msg->pose.pose;
this->current_pose.header=msg->header;
+
+ this->twist_msg = msg->twist.twist;
this->unlock();
}
@@ -617,6 +636,18 @@ class CNavModule : public CModule<ModuleCfg>
return this->enable_auto_clear;
}
+ template <class ModuleCfg>
+ geometry_msgs::Twist CNavModule<ModuleCfg>::get_current_speed(void)
+ {
+ return this->twist_msg;
+ }
+
+ template <class ModuleCfg>
+ nav_msgs::Path CNavModule<ModuleCfg>::get_current_path(void)
+ {
+ return this->path_msg;
+ }
+
template <class ModuleCfg>
geometry_msgs::PoseStamped CNavModule<ModuleCfg>::get_current_pose(void)
{
diff --git a/include/iri_nav_module/nav_module_bt.h b/include/iri_nav_module/nav_module_bt.h
index dbabfd8..6b80bb5 100644
--- a/include/iri_nav_module/nav_module_bt.h
+++ b/include/iri_nav_module/nav_module_bt.h
@@ -106,15 +106,14 @@ class CNavModuleBT
*
*/
BT::NodeStatus costmaps_disable_auto_clear(void);
-
/**
- * \brief Synchronized getCurrentPose add_sbpl nav function.
+ * \brief Synchronized getCurrentSpeed add_sbpl nav function.
*
- * This function calls getCurrentPose of nav_module.
+ * This function calls getCurrentSpeed of nav_module.
*
* It has the following output ports:
*
- * pose (geometry_msgs::Pose): current pose.
+ * twist (geometry_msgs::Twist): current speed.
*
* \param self Self node with the required ports:
*
@@ -123,6 +122,41 @@ class CNavModuleBT
* If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
*
*/
+ BT::NodeStatus get_current_speed(BT::TreeNode& self);
+ /**
+ * \brief Synchronized getCurrentPath add_sbpl nav function.
+ *
+ * This function calls getCurrentPath of nav_module.
+ *
+ * It has the following output ports:
+ *
+ * path (nav_msgs::Path): current path.
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus get_current_path(BT::TreeNode& self);
+ /**
+ * \brief Synchronized getCurrentPose add_sbpl nav function.
+ *
+ * This function calls getCurrentPose of nav_module.
+ *
+ * It has the following output ports:
+ *
+ * pose (geometry_msgs::Pose): current pose.
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+
BT::NodeStatus get_current_pose(BT::TreeNode& self);
/**
@@ -255,6 +289,8 @@ class CNavModuleBT
//Definition of ports
BT::PortsList set_goal_frame_ports = {BT::InputPort<std::string>("frame_id")};
BT::PortsList auto_clear_ports = {BT::InputPort<double>("rate_hz")};
+ BT::PortsList current_speed_ports = {BT::OutputPort<geometry_msgs::Twist>("speed")};
+ BT::PortsList current_path_ports = {BT::OutputPort<nav_msgs::Path>("path")};
BT::PortsList current_pose_ports = {BT::OutputPort<geometry_msgs::PoseStamped>("pose")};
BT::PortsList goal_distance_ports = {BT::OutputPort<double>("distance")};
BT::PortsList path_length_ports = {BT::OutputPort<double>("length")};
@@ -282,6 +318,8 @@ class CNavModuleBT
factory.registerSimpleAction(name+"_costmaps_clear", std::bind(&CNavModuleBT::costmaps_clear, this));
factory.registerSimpleAction(name+"_costmaps_enable_auto_clear", std::bind(&CNavModuleBT::costmaps_enable_auto_clear, this, std::placeholders::_1), auto_clear_ports);
factory.registerSimpleAction(name+"_costmaps_disable_auto_clear", std::bind(&CNavModuleBT::costmaps_disable_auto_clear, this));
+ factory.registerSimpleAction(name+"_get_current_pose", std::bind(&CNavModuleBT::get_current_speed, this, std::placeholders::_1), current_speed_ports);
+ factory.registerSimpleAction(name+"_get_current_pose", std::bind(&CNavModuleBT::get_current_path, this, std::placeholders::_1), current_path_ports);
factory.registerSimpleAction(name+"_get_current_pose", std::bind(&CNavModuleBT::get_current_pose, this, std::placeholders::_1), current_pose_ports);
factory.registerSimpleAction(name+"_get_goal_distance", std::bind(&CNavModuleBT::get_goal_distance, this, std::placeholders::_1), goal_distance_ports);
factory.registerSimpleAction(name+"_get_path_length", std::bind(&CNavModuleBT::get_path_length, this, std::placeholders::_1), path_length_ports);
@@ -358,7 +396,26 @@ class CNavModuleBT
this->nav_module.costmaps_disable_auto_clear();
return BT::NodeStatus::SUCCESS;
}
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::get_current_speed(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::get_current_speed-> get_current_speed");
+ geometry_msgs::Twist speed_out = this->nav_module.get_current_speed();
+
+ self.setOutput("speed", speed_out);
+ return BT::NodeStatus::SUCCESS;
+ }
+ template <class ModuleCfg>
+ BT::NodeStatus CNavModuleBT<ModuleCfg>::get_current_path(BT::TreeNode& self)
+ {
+ ROS_DEBUG("CNavModuleBT::get_current_path-> get_current_path");
+
+ nav_msgs::Path path_out = this->nav_module.get_current_path();
+
+ self.setOutput("path", path_out);
+ return BT::NodeStatus::SUCCESS;
+ }
template <class ModuleCfg>
BT::NodeStatus CNavModuleBT<ModuleCfg>::get_current_pose(BT::TreeNode& self)
{
--
GitLab