Updated with changes made in iri_ana_nav_bt node

include/iri_ana_nav_module/iri_ana_nav_module_bt.h

@@ -30,15 +30,17 @@ class CIriAnaNavModuleBT
     BT::FileLogger      *logger_file;
     BT::PublisherZMQ *publisher_zmq;
 
+    // goal sent variables
+    bool orientation_goal_sent;
+    bool position_goal_sent;
+    bool pose_goal_sent;
+    
     // print tree
     bool print_tree_enabled;
 
     // variable to save behavior tree status
     BT::NodeStatus status;
 
-    // variable to save iri_ana_nav_module status
-    iri_ana_nav_module_status_t nav_status;
-
     // object of iri_ana_nav_module
     CIriAnaNavModule nav;
 

src/iri_ana_nav_module_bt.cpp

@@ -7,15 +7,27 @@
 CIriAnaNavModuleBT::CIriAnaNavModuleBT() :
   nav("nav_module",ros::this_node::getName())
 {
+  // loggers
+  this->logger_cout = NULL;
+  this->logger_minitrace = NULL;
+  this->logger_file = NULL;
+  this->publisher_zmq = NULL;
 
+  // goal sent variables
+  orientation_goal_sent = false;
+  position_goal_sent = false;
+  pose_goal_sent = false;
 }
 
 void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory)
 {
   // definition of ports
-  BT::PortsList orientation_port = { BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol") };
-  BT::PortsList position_port = { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("x_y_pos_tol") };
-  BT::PortsList 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 sync_orientation_port = { BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol") };
+  BT::PortsList sync_position_port = { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("x_y_pos_tol") };
+  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::PortsList frame_port = { BT::InputPort<std::string>("frame_id") };
   BT::PortsList current_pose_port = { BT::OutputPort<double>("current_x"), BT::OutputPort<double>("current_y"), BT::OutputPort<double>("current_yaw") };
   BT::PortsList costmap_port = { BT::InputPort<double>("rate_hz") };
@@ -36,9 +48,9 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory)
   factory->registerSimpleCondition("is_no_transform",  std::bind(&CIriAnaNavModuleBT::is_no_transform, this));
   factory->registerSimpleCondition("costmap_is_auto_clear_enabled",  std::bind(&CIriAnaNavModuleBT::costmap_is_auto_clear_enabled, this));
   // registry of synchronous actions
-  factory->registerSimpleAction("sync_go_to_orientation",  std::bind(&CIriAnaNavModuleBT::sync_go_to_orientation, this, std::placeholders::_1), orientation_port);
-  factory->registerSimpleAction("sync_go_to_position",  std::bind(&CIriAnaNavModuleBT::sync_go_to_position, this, std::placeholders::_1), position_port);
-  factory->registerSimpleAction("sync_go_to_pose",  std::bind(&CIriAnaNavModuleBT::sync_go_to_pose, this, std::placeholders::_1), pose_port);
+  factory->registerSimpleAction("sync_go_to_orientation",  std::bind(&CIriAnaNavModuleBT::sync_go_to_orientation, this, std::placeholders::_1), sync_orientation_port);
+  factory->registerSimpleAction("sync_go_to_position",  std::bind(&CIriAnaNavModuleBT::sync_go_to_position, this, std::placeholders::_1), sync_position_port);
+  factory->registerSimpleAction("sync_go_to_pose",  std::bind(&CIriAnaNavModuleBT::sync_go_to_pose, this, std::placeholders::_1), sync_pose_port);
   factory->registerSimpleAction("sync_stop",  std::bind(&CIriAnaNavModuleBT::sync_stop, this));
   factory->registerSimpleAction("set_goal_frame",  std::bind(&CIriAnaNavModuleBT::set_goal_frame, this, std::placeholders::_1), frame_port);
   factory->registerSimpleAction("get_current_pose",  std::bind(&CIriAnaNavModuleBT::get_current_pose, this, std::placeholders::_1), current_pose_port);
@@ -46,9 +58,9 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory)
   factory->registerSimpleAction("costmaps_enable_auto_clear",  std::bind(&CIriAnaNavModuleBT::costmaps_enable_auto_clear, this, std::placeholders::_1), costmap_port);
   factory->registerSimpleAction("costmaps_disable_auto_clear",  std::bind(&CIriAnaNavModuleBT::costmaps_disable_auto_clear, this));
   // registry of asynchronous actions
-  factory->registerIriAsyncAction("async_go_to_orientation",  std::bind(&CIriAnaNavModuleBT::async_go_to_orientation, this, std::placeholders::_1), orientation_port);
-  factory->registerIriAsyncAction("async_go_to_position",  std::bind(&CIriAnaNavModuleBT::async_go_to_position, this, std::placeholders::_1), position_port);
-  factory->registerIriAsyncAction("async_go_to_pose",  std::bind(&CIriAnaNavModuleBT::async_go_to_pose, this, std::placeholders::_1), pose_port);
+  factory->registerIriAsyncAction("async_go_to_orientation",  std::bind(&CIriAnaNavModuleBT::async_go_to_orientation, this, std::placeholders::_1), async_orientation_port);
+  factory->registerIriAsyncAction("async_go_to_position",  std::bind(&CIriAnaNavModuleBT::async_go_to_position, this, std::placeholders::_1), async_position_port);
+  factory->registerIriAsyncAction("async_go_to_pose",  std::bind(&CIriAnaNavModuleBT::async_go_to_pose, this, std::placeholders::_1), async_pose_port);
   factory->registerIriAsyncAction("NOP",  std::bind(&CIriAnaNavModuleBT::NOP, this));
 
   // creation of tree from .xml
@@ -195,9 +207,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_orientation(BT::TreeNode& self)
 {
   ROS_INFO("async_go_to_orientation");
 
-  static bool orientation_goal_sent = false;
+  static bool update_goal_value;
 
-  if (!orientation_goal_sent)
+  BT::Optional<bool> update_goal_input = self.getInput<bool>("update_goal");
+
+  if (!update_goal_input)
+  {
+    update_goal_value = false;
+  }
+  else
+  {
+    update_goal_value = update_goal_input.value();
+  }
+  if (!orientation_goal_sent || update_goal_value)
   {
     BT::Optional<double> yaw_input = self.getInput<double>("yaw");
     BT::Optional<double> heading_tol_input = self.getInput<double>("heading_tol");
@@ -224,6 +246,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_orientation(BT::TreeNode& self)
         return BT::NodeStatus::FAILURE;
     }
     orientation_goal_sent = true;
+    self.setOutput("update_goal", false);
   }
   if (nav.is_finished())
   {
@@ -237,9 +260,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self)
 {
   ROS_INFO("async_go_to_position");
 
-  static bool position_goal_sent = false;
+  static bool update_goal_value;
 
-  if (!position_goal_sent)
+  BT::Optional<bool> update_goal_input = self.getInput<bool>("update_goal");
+
+  if (!update_goal_input)
+  {
+    update_goal_value = false;
+  }
+  else
+  {
+    update_goal_value = update_goal_input.value();
+  }
+  if (!position_goal_sent || update_goal_value)
   {
     BT::Optional<double> x_input = self.getInput<double>("x");
     BT::Optional<double> y_input = self.getInput<double>("y");
@@ -273,6 +306,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self)
       return BT::NodeStatus::FAILURE;
     }
     position_goal_sent = true;
+    self.setOutput("update_goal", false);
   }
   if (nav.is_finished())
   {
@@ -286,9 +320,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
 {
   ROS_INFO("async_go_to_pose");
 
-  static bool pose_goal_sent = false;
+  static bool update_goal_value;
+
+  BT::Optional<bool> update_goal_input = self.getInput<bool>("update_goal");
 
-  if (!pose_goal_sent)
+  if (!update_goal_input)
+  {
+    update_goal_value = false;
+  }
+  else
+  {
+    update_goal_value = update_goal_input.value();
+  }
+  if (!pose_goal_sent || update_goal_value)
   {
     BT::Optional<double> x_input = self.getInput<double>("x");
     BT::Optional<double> y_input = self.getInput<double>("y");
@@ -336,6 +380,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
       return BT::NodeStatus::FAILURE;
     }
     pose_goal_sent = true;
+    self.setOutput("update_goal", false);
   }
   if (nav.is_finished())
   {
@@ -347,9 +392,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
 
 BT::NodeStatus CIriAnaNavModuleBT::sync_stop()
 {
-  ROS_INFO("sync_stop");
-
-  nav.stop();
+  stop();
   return BT::NodeStatus::SUCCESS;
 }
 
@@ -367,7 +410,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_finished()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_succeded()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_SUCCESS)
   {
@@ -379,7 +422,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_succeded()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_running()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_RUNNING)
   {
@@ -391,7 +434,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_running()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_action_start_failed()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_ACTION_START_FAIL)
   {
@@ -403,7 +446,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_action_start_failed()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_timeout()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_TIMEOUT)
   {
@@ -415,7 +458,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_timeout()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_watchdog()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_FB_WATCHDOG)
   {
@@ -427,7 +470,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_watchdog()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_aborted()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_ABORTED)
   {
@@ -440,7 +483,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_aborted()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_preempted()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_PREEMPTED)
   {
@@ -452,7 +495,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_preempted()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_rejected()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_REJECTED)
   {
@@ -464,7 +507,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_rejected()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_set_param_failed()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_SET_PARAM_FAIL)
   {
@@ -476,7 +519,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_set_param_failed()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_param_not_present()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_PARAM_NOT_PRESENT)
   {
@@ -488,7 +531,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_param_not_present()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_no_odom()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_NO_ODOM)
   {
@@ -500,7 +543,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_no_odom()
 
 BT::NodeStatus CIriAnaNavModuleBT::is_no_transform()
 {
-  nav_status = nav.get_status();
+  iri_ana_nav_module_status_t nav_status = nav.get_status();
 
   if (nav_status == IRI_ANA_NAV_MODULE_NO_TRANSFORM)
   {
@@ -605,33 +648,41 @@ void CIriAnaNavModuleBT::stop()
 
 void CIriAnaNavModuleBT::restart()
 {
-  if (nav.is_finished())
+  ROS_INFO("-------RESTART-------");
+  if (!nav.is_finished())
   {
-    ROS_INFO("-------RESTART-------");
-    for(auto& node : this->tree.nodes)
-    {
-      node.get()->halt();
-    }
-    this->status = BT::NodeStatus::IDLE;
-    if (print_tree_enabled)
-    {
-      BT::printTreeRecursively(this->tree.root_node);
-    }
+    stop();
+    while (!nav.is_finished());
+    orientation_goal_sent = false;
+    position_goal_sent = false;
+    pose_goal_sent = false;
   }
-  else
+  for(auto& node : this->tree.nodes)
+  {
+    node.get()->halt();
+  }
+  this->status = BT::NodeStatus::IDLE;
+  if (print_tree_enabled)
   {
-    ROS_ERROR("Could not restart, stop movement first!");
+    BT::printTreeRecursively(this->tree.root_node);
   }
 }
 
 void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory *factory, std::string path, std::string tree_xml_file)
 {
-  this->path=path;
-  this->tree_xml_file=tree_xml_file;
-  if (nav.is_finished())
+  if (this->path != path || this->tree_xml_file != tree_xml_file)
   {
     ROS_INFO("-------CREATING NEW TREE-------");
-
+    if (!nav.is_finished())
+    {
+      stop();
+      while (!nav.is_finished());
+      orientation_goal_sent = false;
+      position_goal_sent = false;
+      pose_goal_sent = false;
+    }
+    this->path=path;
+    this->tree_xml_file=tree_xml_file;
     try
     {
       this->tree = factory->createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml");
@@ -669,36 +720,36 @@ void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory *factory, std::strin
     }
     catch (BT::RuntimeError &e)
     {
-      ROS_ERROR_STREAM("EXCEPTION " << e.what());
+      ROS_ERROR_STREAM("EXCEPTION: " << e.what());
     }
   }
   else
   {
-    ROS_ERROR("Could not create new tree, stop movement first!");
+      ROS_WARN("This tree has already been created, restart if you want to start again!");
   }
 }
 
 void CIriAnaNavModuleBT::enable_cout_logger()
 {
-  try
+  if (this->logger_cout == NULL)
   {
     this->logger_cout      = new BT::StdCoutLogger(this->tree);
   }
-  catch (BT::LogicError &e)
+  else
   {
-    ROS_ERROR("EXCEPTION: StdCoutLogger already enabled!");
+    ROS_WARN("StdCoutLogger already enabled!");
   }
 }
 
 void CIriAnaNavModuleBT::enable_minitrace_logger()
 {
-  try
+  if (this->logger_minitrace == NULL)
   {
     this->logger_minitrace = new BT::MinitraceLogger(this->tree, (this->path + "/logs/" + this->tree_xml_file + ".json").c_str());
   }
-  catch (BT::LogicError)
+  else
   {
-    ROS_ERROR("EXCEPTION: MinitraceLogger already enabled!");
+    ROS_WARN("MinitraceLogger already enabled!");
   }
 }
 
@@ -710,19 +761,19 @@ void CIriAnaNavModuleBT::enable_file_logger()
   }
   else
   {
-    ROS_ERROR("EXCEPTION: FileLogger already enabled!");
+    ROS_WARN("FileLogger already enabled!");
   }
 }
 
 void CIriAnaNavModuleBT::enable_zmq_publisher()
 {
-  try
+  if (this->publisher_zmq == NULL)
   {
     this->publisher_zmq    = new BT::PublisherZMQ(this->tree);
   }
-  catch (BT::LogicError &e)
+  else
   {
-    ROS_ERROR("EXCEPTION: PublisherZMQ already enabled!");
+    ROS_WARN("PublisherZMQ already enabled!");
   }
 }
 
@@ -735,30 +786,43 @@ void CIriAnaNavModuleBT::enable_print_tree()
       BT::printTreeRecursively(this->tree.root_node);
     }
     print_tree_enabled = true;
-  }}
+  }
+}
 
 void CIriAnaNavModuleBT::disable_cout_logger()
 {
-  delete this->logger_cout;
-  this->logger_cout = NULL;
+  if (this->logger_cout != NULL)
+  {
+    delete this->logger_cout;
+    this->logger_cout = NULL;
+  }
 }
 
 void CIriAnaNavModuleBT::disable_minitrace_logger()
 {
-  delete this->logger_minitrace;
-  this->logger_minitrace = NULL;
+  if (this->logger_minitrace != NULL)
+  {
+    delete this->logger_minitrace;
+    this->logger_minitrace = NULL;
+  }
 }
 
 void CIriAnaNavModuleBT::disable_file_logger()
 {
-  delete this->logger_file;
-  this->logger_file = NULL;
+  if (this->logger_file != NULL)
+  {
+    delete this->logger_file;
+    this->logger_file = NULL;
+  }
 }
 
 void CIriAnaNavModuleBT::disable_zmq_publisher()
 {
-  delete this->publisher_zmq;
-  this->publisher_zmq = NULL;
+  if (this->publisher_zmq != NULL)
+  {
+    delete this->publisher_zmq;
+    this->publisher_zmq = NULL;
+  }
 }
 void CIriAnaNavModuleBT::disable_print_tree()
 {