From 5bd23b65f6ddea1b4fe3810f9ad3113d140ea6ff Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan <shernand@iri.upc.edu> Date: Thu, 2 Jun 2022 10:08:19 +0200 Subject: [PATCH] Used the TF instead of the odom topic to find out the current position of the robot. Added the robot and global frames and functions to handle them. --- include/iri_nav_module/nav_module.h | 217 +++++++++++++++++++------ include/iri_nav_module/nav_module_bt.h | 47 +++++- src/iri_nav_module/dyn_params.py | 4 +- 3 files changed, 213 insertions(+), 55 deletions(-) diff --git a/include/iri_nav_module/nav_module.h b/include/iri_nav_module/nav_module.h index 6fe4a69..a15258e 100644 --- a/include/iri_nav_module/nav_module.h +++ b/include/iri_nav_module/nav_module.h @@ -31,6 +31,16 @@ class CNavModule : public CModule<ModuleCfg> { private: // basic navigation action + /** + * \brief + * + */ + std::string robot_frame_id; + /** + * \brief + * + */ + std::string global_frame_id; /** * \brief * @@ -88,7 +98,6 @@ class CNavModule : public CModule<ModuleCfg> * */ geometry_msgs::Twist twist_msg; - /** * \brief * @@ -99,24 +108,16 @@ class CNavModule : public CModule<ModuleCfg> * */ void path_callback(const nav_msgs::Path::ConstPtr& msg); - /** * \brief * */ nav_msgs::Path path_msg; - /** * \brief * */ bool path_available; - /** - * \brief - * - */ - geometry_msgs::PoseStamped current_pose; - // tf listener /** * \brief @@ -189,7 +190,12 @@ class CNavModule : public CModule<ModuleCfg> * \brief * */ - std::string get_pose_frame_id(void); + std::string get_robot_frame_id(void); + /** + * \brief + * + */ + std::string get_global_frame_id(void); /** * \brief * @@ -209,7 +215,7 @@ class CNavModule : public CModule<ModuleCfg> * \brief * */ - bool transform_current_pose(void); + bool transform_goal(double &x,double &y,double &yaw); public: /** @@ -228,11 +234,33 @@ class CNavModule : public CModule<ModuleCfg> */ bool make_plan(double start_x,double start_y,double start_yaw,double end_x,double end_y,double end_yaw,std::string &frame_id,double tolerance,nav_msgs::Path &plan); // common functions + /** + * \brief Set the robot reference frame + * + * This functions sets the main reference from for the robot. By default + * this frame is set to base_link + * + * \param frame_id name of the new reference frame of the robot. + * This frame can be any that exist inside the TF tree + * of the robot. + */ + void set_robot_frame_id(std::string &frame_id); + /** + * \brief Set the global reference frame + * + * This functions sets the global reference frame of the TF. By default + * this frame is set to map + * + * \param frame_id name of the global reference frame of the TF. + * This frame can be any that exist inside the TF tree + * of the robot. + */ + void set_global_frame_id(std::string &frame_id); /** * \brief Set the goal reference frame * * This functions sets the reference with used for all the position, - * orientation and pose goals. By default this frame is set to /map. + * orientation and pose goals. By default this frame is set to map. * * \param frame_id name of the new reference frame for the future goals. * This frame can be any that exist inside the TF tree @@ -419,12 +447,22 @@ class CNavModule : public CModule<ModuleCfg> this->auto_clear_rate_hz=10.0; this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/this->auto_clear_rate_hz),&CNavModule::clear_costmaps_call,this); this->clear_costmaps_timer.stop(); + + this->robot_frame_id="base_link"; + this->global_frame_id="map"; + this->goal_frame_id="map"; } template <class ModuleCfg> - std::string CNavModule<ModuleCfg>::get_pose_frame_id(void) + std::string CNavModule<ModuleCfg>::get_robot_frame_id(void) { - return this->current_pose.header.frame_id; + return this->robot_frame_id; + } + + template <class ModuleCfg> + std::string CNavModule<ModuleCfg>::get_global_frame_id(void) + { + return this->global_frame_id; } template <class ModuleCfg> @@ -440,7 +478,7 @@ class CNavModule : public CModule<ModuleCfg> 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.header.frame_id=this->global_frame_id; this->move_base_goal.target_pose.pose.position.x=x; this->move_base_goal.target_pose.pose.position.y=y; this->move_base_goal.target_pose.pose.position.z=0.0; @@ -482,10 +520,6 @@ class CNavModule : public CModule<ModuleCfg> this->lock(); // reset the odometry callback this->odom_watchdog.reset(ros::Duration(this->odom_watchdog_time_s)); - // 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(); } @@ -502,38 +536,38 @@ class CNavModule : public CModule<ModuleCfg> } template <class ModuleCfg> - bool CNavModule<ModuleCfg>::transform_current_pose(void) + bool CNavModule<ModuleCfg>::transform_goal(double &x,double &y,double &yaw) { geometry_msgs::TransformStamped transform; - geometry_msgs::PoseStamped pose; + geometry_msgs::PoseStamped pose,current_pose; + ros::Time current_time=ros::Time::now(); + tf2::Quaternion quat; + double roll, pitch; bool tf2_exists; try{ - if(!this->odom_watchdog.is_active()) + tf2_exists=this->tf2_buffer.canTransform(this->global_frame_id,this->goal_frame_id,current_time,ros::Duration(this->tf_timeout_time_s)); + if(tf2_exists) { - tf2_exists=this->tf2_buffer.canTransform(this->goal_frame_id,this->current_pose.header.frame_id,this->current_pose.header.stamp,ros::Duration(this->tf_timeout_time_s)); - transform = this->tf2_buffer.lookupTransform(this->goal_frame_id,this->current_pose.header.frame_id,this->current_pose.header.stamp); - if(tf2_exists) - { - tf2::doTransform(this->current_pose,pose,transform); - if(!this->odom_watchdog.is_active()) - { - this->move_base_goal.target_pose.pose.position.x=pose.pose.position.x; - this->move_base_goal.target_pose.pose.position.y=pose.pose.position.y; - this->move_base_goal.target_pose.pose.position.z=0.0; - this->move_base_goal.target_pose.pose.orientation=pose.pose.orientation; - } - return true; - } - else - { - ROS_WARN_STREAM("CNavModule: No transform found from " << this->current_pose.header.frame_id << " to " << this->goal_frame_id); - return false; - } + transform = this->tf2_buffer.lookupTransform(this->global_frame_id,this->goal_frame_id,current_time); + current_pose.header.frame_id=this->global_frame_id; + current_pose.header.stamp=current_time; + current_pose.pose.position.x=x; + current_pose.pose.position.y=y; + current_pose.pose.position.z=0.0; + quat.setRPY(0.0,0.0,yaw); + current_pose.pose.orientation=tf2::toMsg(quat); + tf2::doTransform(current_pose,pose,transform); + x=pose.pose.position.x; + y=pose.pose.position.y; + tf2::Quaternion q(pose.pose.orientation.x,pose.pose.orientation.y,pose.pose.orientation.z,pose.pose.orientation.w); + tf2::Matrix3x3 m(q); + m.getRPY(roll, pitch, yaw); + return true; } else { - ROS_WARN_STREAM("CNavModule: Not receiving odometry messages"); + ROS_WARN_STREAM("CNavModule: No transform found from " << this->goal_frame_id << " to " << this->global_frame_id); return false; } }catch(tf2::TransformException &ex){ @@ -563,6 +597,22 @@ class CNavModule : public CModule<ModuleCfg> } // common functions + template <class ModuleCfg> + void CNavModule<ModuleCfg>::set_robot_frame_id(std::string &frame_id) + { + this->lock(); + this->robot_frame_id=frame_id; + this->unlock(); + } + + template <class ModuleCfg> + void CNavModule<ModuleCfg>::set_global_frame_id(std::string &frame_id) + { + this->lock(); + this->global_frame_id=frame_id; + this->unlock(); + } + template <class ModuleCfg> void CNavModule<ModuleCfg>::set_goal_frame_id(std::string &frame_id) { @@ -651,7 +701,38 @@ class CNavModule : public CModule<ModuleCfg> template <class ModuleCfg> geometry_msgs::PoseStamped CNavModule<ModuleCfg>::get_current_pose(void) { - return this->current_pose; + geometry_msgs::TransformStamped transform; + geometry_msgs::PoseStamped pose,current_pose; + ros::Time current_time=ros::Time::now(); + bool tf2_exists; + + try{ + tf2_exists=this->tf2_buffer.canTransform(this->global_frame_id,this->robot_frame_id,current_time,ros::Duration(this->tf_timeout_time_s)); + if(tf2_exists) + { + transform = this->tf2_buffer.lookupTransform(this->global_frame_id,this->robot_frame_id,current_time); + current_pose.header.frame_id=this->robot_frame_id; + current_pose.header.stamp=current_time; + current_pose.pose.position.x=0.0; + current_pose.pose.position.y=0.0; + current_pose.pose.position.z=0.0; + current_pose.pose.orientation.x=0.0; + current_pose.pose.orientation.y=0.0; + current_pose.pose.orientation.z=0.0; + current_pose.pose.orientation.w=1.0; + tf2::doTransform(current_pose,pose,transform); + } + else + { + ROS_WARN_STREAM("CNavModule: No transform found from " << this->robot_frame_id << " to " << this->global_frame_id); + return pose; + } + }catch(tf2::TransformException &ex){ + ROS_ERROR_STREAM("CNavModule: TF2 Exception: " << ex.what()); + return pose; + } + + return pose; } template <class ModuleCfg> @@ -659,7 +740,8 @@ class CNavModule : public CModule<ModuleCfg> { double min_d=std::numeric_limits<double>::max(); geometry_msgs::TransformStamped transform; - geometry_msgs::PoseStamped pose; + geometry_msgs::PoseStamped pose,current_pose; + ros::Time current_time=ros::Time::now(); unsigned int min_index=0; bool tf2_exists; @@ -677,13 +759,32 @@ class CNavModule : public CModule<ModuleCfg> this->unlock(); return std::numeric_limits<double>::quiet_NaN(); } - tf2_exists=this->tf2_buffer.canTransform(this->goal_frame_id,this->current_pose.header.frame_id,this->current_pose.header.stamp,ros::Duration(this->tf_timeout_time_s)); - transform = this->tf2_buffer.lookupTransform(this->goal_frame_id,this->current_pose.header.frame_id,this->current_pose.header.stamp); - if(tf2_exists) - tf2::doTransform(this->current_pose,pose,transform); - else - { - ROS_WARN("CNavModule: Goal distance not calculed. Impossible to transform to goal frame."); + try{ + this->unlock(); + tf2_exists=this->tf2_buffer.canTransform(this->path_msg.header.frame_id,this->robot_frame_id,current_time,ros::Duration(this->tf_timeout_time_s)); + this->lock(); + if(tf2_exists) + { + transform = this->tf2_buffer.lookupTransform(this->path_msg.header.frame_id,this->robot_frame_id,current_time); + current_pose.header.frame_id=this->robot_frame_id; + current_pose.header.stamp=current_time; + current_pose.pose.position.x=0.0; + current_pose.pose.position.y=0.0; + current_pose.pose.position.z=0.0; + current_pose.pose.orientation.x=0.0; + current_pose.pose.orientation.y=0.0; + current_pose.pose.orientation.z=0.0; + current_pose.pose.orientation.w=1.0; + tf2::doTransform(current_pose,pose,transform); + } + else + { + ROS_WARN_STREAM("CNavModule: No transform found from " << this->robot_frame_id << " to " << this->global_frame_id); + this->unlock(); + return std::numeric_limits<double>::quiet_NaN(); + } + }catch(tf2::TransformException &ex){ + ROS_ERROR_STREAM("CNavModule: TF2 Exception: " << ex.what()); this->unlock(); return std::numeric_limits<double>::quiet_NaN(); } @@ -753,12 +854,24 @@ class CNavModule : public CModule<ModuleCfg> for(typename std::vector<typename ModuleCfg::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++) { - if((*param)->name==(name+"_move_base_frame_id")) + if((*param)->name==(name+"_move_base_goal_frame_id")) { (*param)->getValue(config,value); if(value.type()==typeid(std::string)) this->goal_frame_id=boost::any_cast<std::string &>(value); } + else if((*param)->name==(name+"_move_base_global_frame_id")) + { + (*param)->getValue(config,value); + if(value.type()==typeid(std::string)) + this->global_frame_id=boost::any_cast<std::string &>(value); + } + else if((*param)->name==(name+"_move_base_robot_frame_id")) + { + (*param)->getValue(config,value); + if(value.type()==typeid(std::string)) + this->robot_frame_id=boost::any_cast<std::string &>(value); + } else if((*param)->name==(name+"_move_base_cancel_prev")) { (*param)->getValue(config,value); diff --git a/include/iri_nav_module/nav_module_bt.h b/include/iri_nav_module/nav_module_bt.h index ecc2118..d3bf24d 100644 --- a/include/iri_nav_module/nav_module_bt.h +++ b/include/iri_nav_module/nav_module_bt.h @@ -62,6 +62,9 @@ class CNavModuleBT */ BT::NodeStatus set_goal_frame(BT::TreeNode& self); + BT::NodeStatus set_global_frame(BT::TreeNode& self); + + BT::NodeStatus set_robot_frame(BT::TreeNode& self); /** * \brief Synchronized costmaps_clear add_sbpl nav function. * @@ -287,7 +290,7 @@ class CNavModuleBT std::string name; //Definition of ports - BT::PortsList set_goal_frame_ports = {BT::InputPort<std::string>("frame_id")}; + BT::PortsList set_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")}; @@ -314,7 +317,9 @@ class CNavModuleBT factory.registerSimpleCondition(name+"_is_costmap_autoclear_enabled", std::bind(&CNavModuleBT::is_costmap_autoclear_enabled, this)); //Registration of actions - factory.registerSimpleAction(name+"_set_goal_frame", std::bind(&CNavModuleBT::set_goal_frame, this, std::placeholders::_1), set_goal_frame_ports); + factory.registerSimpleAction(name+"_set_goal_frame", std::bind(&CNavModuleBT::set_goal_frame, this, std::placeholders::_1), set_frame_ports); + factory.registerSimpleAction(name+"_set_global_frame", std::bind(&CNavModuleBT::set_global_frame, this, std::placeholders::_1), set_frame_ports); + factory.registerSimpleAction(name+"_set_robot_frame", std::bind(&CNavModuleBT::set_robot_frame, this, std::placeholders::_1), set_frame_ports); 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)); @@ -361,6 +366,44 @@ class CNavModuleBT return BT::NodeStatus::SUCCESS; } + template <class ModuleCfg> + BT::NodeStatus CNavModuleBT<ModuleCfg>::set_global_frame(BT::TreeNode& self) + { + ROS_DEBUG("CNavModuleBT::set_global_frame-> set_global_frame"); + BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id"); + + std::string frame_id_goal; + if (!frame_id) + { + ROS_ERROR("CNavModuleBT::set_global_frame-> Incorrect or missing input. It needs the following input ports: frame_id(std::string)"); + return BT::NodeStatus::FAILURE; + } + + frame_id_goal = frame_id.value(); + + this->nav_module.set_global_frame_id(frame_id_goal); + return BT::NodeStatus::SUCCESS; + } + + template <class ModuleCfg> + BT::NodeStatus CNavModuleBT<ModuleCfg>::set_robot_frame(BT::TreeNode& self) + { + ROS_DEBUG("CNavModuleBT::set_robot_frame-> set_robot_frame"); + BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id"); + + std::string frame_id_goal; + if (!frame_id) + { + ROS_ERROR("CNavModuleBT::set_robot_frame-> Incorrect or missing input. It needs the following input ports: frame_id(std::string)"); + return BT::NodeStatus::FAILURE; + } + + frame_id_goal = frame_id.value(); + + this->nav_module.set_robot_frame_id(frame_id_goal); + return BT::NodeStatus::SUCCESS; + } + template <class ModuleCfg> BT::NodeStatus CNavModuleBT<ModuleCfg>::costmaps_clear(void) { diff --git a/src/iri_nav_module/dyn_params.py b/src/iri_nav_module/dyn_params.py index 4de336e..9deeb34 100644 --- a/src/iri_nav_module/dyn_params.py +++ b/src/iri_nav_module/dyn_params.py @@ -12,7 +12,9 @@ def add_nav_module_params(gen,name): add_module_params(new_group,name+"_module") move_base_action=add_module_action_params(new_group,name+"_move_base") - move_base_action.add(name+"_move_base_frame_id",str_t, 0, "Reference frame of the position goals","map") + move_base_action.add(name+"_move_base_goal_frame_id",str_t, 0, "Reference frame of the position goals","map") + move_base_action.add(name+"_move_base_global_frame_id",str_t, 0, "Global reference frame of the TF","map") + move_base_action.add(name+"_move_base_robot_frame_id",str_t, 0, "Main reference frame of the robot","map") move_base_action.add(name+"_move_base_cancel_prev", bool_t, 0, "Cancel previous action", True) odom.add(name+"_odom_watchdog_time_s", double_t, 0, "Maximum time between odom messages",1, 0.01, 50) -- GitLab