diff --git a/include/iri_nav_module/nav_module.h b/include/iri_nav_module/nav_module.h
index 6fe4a69cacad694fffc912d1bba4d264cb20e048..a15258ecf78ab4d142dc47fe18bb847eee472384 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 ecc21182553e22fc2a5987ec0c89dc5413e387f5..d3bf24d9852ab565e1b7077422ec81948268e74b 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 4de336e8f2c67eacadb0f3787f81a68a1fd3adf5..9deeb3497178d31d1ae2a26c6e527c98f872f5ed 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)