From 6572b3658a0010f4ead05782c59dcb4328c05134 Mon Sep 17 00:00:00 2001
From: nrodriguez <nicolas.adrian.rodriguez@upc.edu>
Date: Wed, 18 May 2022 12:21:10 +0200
Subject: [PATCH] Change return type of get_current_pose to PoseStamped
---
include/iri_nav_module/nav_module.h | 20 ++++++++++----------
include/iri_nav_module/nav_module_bt.h | 16 ++++++++--------
2 files changed, 18 insertions(+), 18 deletions(-)
diff --git a/include/iri_nav_module/nav_module.h b/include/iri_nav_module/nav_module.h
index 921edd8..cc0b080 100644
--- a/include/iri_nav_module/nav_module.h
+++ b/include/iri_nav_module/nav_module.h
@@ -281,19 +281,19 @@ class CNavModule : public CModule<ModuleCfg>
bool costamp_is_auto_clear_enabled(void);
/**
* \brief Function to get the current pose of the robot.
- *
+ *
* \return The robot pose with respect to the map frame.
*/
- geometry_msgs::Pose get_current_pose(void);
+ geometry_msgs::PoseStamped get_current_pose(void);
/**
* \brief Function to get the euclidian distance to the goal.
- *
+ *
* \return The Euclidian distance to the goal. -1.0 when not calculated.
*/
double get_goal_distance(void);
/**
* \brief Function to get the euclidian distance to the path.
- *
+ *
* \return The Euclidian distance to the path. -1.0 when not calculated.
*/
double get_path_length(void);
@@ -367,7 +367,7 @@ class CNavModule : public CModule<ModuleCfg>
* \brief
*
*/
- const CNavCostmapModule<ModuleCfg> &get_global_costmap(void) const;
+ const CNavCostmapModule<ModuleCfg> &get_global_costmap(void) const;
/**
* \brief Destructor
*
@@ -421,7 +421,7 @@ class CNavModule : public CModule<ModuleCfg>
{
tf2::Quaternion quat;
act_srv_status status;
-
+
this->move_base_goal.target_pose.header.stamp=ros::Time::now();
this->move_base_goal.target_pose.header.frame_id=this->goal_frame_id;
this->move_base_goal.target_pose.pose.position.x=x;
@@ -456,7 +456,7 @@ class CNavModule : public CModule<ModuleCfg>
return status;
}
-
+
template <class ModuleCfg>
void CNavModule<ModuleCfg>::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
@@ -618,9 +618,9 @@ class CNavModule : public CModule<ModuleCfg>
}
template <class ModuleCfg>
- geometry_msgs::Pose CNavModule<ModuleCfg>::get_current_pose(void)
+ geometry_msgs::PoseStamped CNavModule<ModuleCfg>::get_current_pose(void)
{
- return this->current_pose.pose;
+ return this->current_pose;
}
template <class ModuleCfg>
@@ -631,7 +631,7 @@ class CNavModule : public CModule<ModuleCfg>
geometry_msgs::PoseStamped pose;
unsigned int min_index=0;
bool tf2_exists;
-
+
this->lock();
if (!this->path_available)
{
diff --git a/include/iri_nav_module/nav_module_bt.h b/include/iri_nav_module/nav_module_bt.h
index 878fd31..dbabfd8 100644
--- a/include/iri_nav_module/nav_module_bt.h
+++ b/include/iri_nav_module/nav_module_bt.h
@@ -233,7 +233,7 @@ class CNavModuleBT
*
*/
BT::NodeStatus are_costmaps_shutdown(void);
-
+
/**
* \brief
*
@@ -255,7 +255,7 @@ 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_pose_ports = {BT::OutputPort<geometry_msgs::Pose>("pose")};
+ 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")};
BT::PortsList costmaps_shutdown_ports = {BT::InputPort<bool>("shutdown")};
@@ -273,7 +273,7 @@ class CNavModuleBT
std::size_t found = this->nav_module.get_name().find_last_of("/\\");
name=this->nav_module.get_name().substr(found+1);
- // Registration of conditions
+ // Registration of conditions
factory.registerSimpleCondition(name+"_are_costmaps_shutdown", std::bind(&CNavModuleBT::are_costmaps_shutdown, this));
factory.registerSimpleCondition(name+"_is_costmap_autoclear_enabled", std::bind(&CNavModuleBT::is_costmap_autoclear_enabled, this));
@@ -306,7 +306,7 @@ class CNavModuleBT
template <class ModuleCfg>
BT::NodeStatus CNavModuleBT<ModuleCfg>::set_goal_frame(BT::TreeNode& self)
- {
+ {
ROS_DEBUG("CNavModuleBT::set_goal_frame-> set_goal_frame");
BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id");
@@ -318,7 +318,7 @@ class CNavModuleBT
}
frame_id_goal = frame_id.value();
-
+
this->nav_module.set_goal_frame_id(frame_id_goal);
return BT::NodeStatus::SUCCESS;
}
@@ -361,10 +361,10 @@ class CNavModuleBT
template <class ModuleCfg>
BT::NodeStatus CNavModuleBT<ModuleCfg>::get_current_pose(BT::TreeNode& self)
- {
+ {
ROS_DEBUG("CNavModuleBT::get_current_pose-> get_current_pose");
- geometry_msgs::Pose pose_out = this->nav_module.get_current_pose();
+ geometry_msgs::PoseStamped pose_out = this->nav_module.get_current_pose();
self.setOutput("pose", pose_out);
return BT::NodeStatus::SUCCESS;
@@ -399,7 +399,7 @@ class CNavModuleBT
else
return BT::NodeStatus::FAILURE;
}
-
+
template <class ModuleCfg>
BT::NodeStatus CNavModuleBT<ModuleCfg>::costmaps_shutdown(BT::TreeNode& self)
{
--
GitLab