Completed BT module layer

include/tiago_nav_module/tiago_nav_bt_module.h

@@ -380,6 +380,166 @@ class CTiagoNavModuleBT
       */
     BT::NodeStatus set_tiago_nav_goal_frame(BT::TreeNode& self);
 
+
+    /**
+      * \brief Synchronized map_change TIAGo nav function.
+      *
+      * This function calls map_change of tiago_nav_module.
+      *
+      * It has the following input ports:
+      *
+      *   map_name (std::string): name of the desired map.
+      *
+      * \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 tiago_nav_map_change(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized costmaps_clear TIAGo nav function.
+      *
+      * This function calls costmaps_clear of tiago_nav_module.
+      *
+      *
+      * \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 tiago_nav_costmaps_clear(void);
+
+    /**
+      * \brief Synchronized costmaps_enable_auto_clear TIAGo nav function.
+      *
+      * This function calls costmaps_enable_auto_clear of tiago_nav_module.
+      *
+      * It has the following input ports:
+      *
+      *   rate_hz (double): the desired clearing rate in Hz. This value should be less
+      *                than 1 Hz, normally in the range of 0.1 to 0.01 Hz.
+      *
+      * \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 tiago_nav_costmaps_enable_auto_clear(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized costmaps_disable_auto_clear TIAGo nav function.
+      *
+      * This function calls costmaps_disable_auto_clear of tiago_nav_module.
+      *
+      *
+      * \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 tiago_nav_costmaps_disable_auto_clear(void);
+
+    /**
+      * \brief Synchronized set_parameter TIAGo nav function.
+      *
+      * This function calls set_parameter of tiago_nav_module.
+      *
+      * It has the following input ports:
+      *
+      *   name_space (std::string): String with the parameter name space.
+      *
+      *   name (std::string): String with the name of the parameter to change. This must
+      *             coincide with a parameter name on the associated dynamic
+      *             reconfigure server.
+      *
+      *   value (bool): the new value for the parameter.
+      *
+      * \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 tiago_nav_set_bool_parameter(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized set_parameter TIAGo nav function.
+      *
+      * This function calls set_parameter of tiago_nav_module.
+      *
+      * It has the following input ports:
+      *
+      *   name_space (std::string): String with the parameter name space.
+      *
+      *   name (std::string): String with the name of the parameter to change. This must
+      *             coincide with a parameter name on the associated dynamic
+      *             reconfigure server.
+      *
+      *   value (int): the new value for the parameter.
+      *
+      * \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 tiago_nav_set_int_parameter(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized set_parameter TIAGo nav function.
+      *
+      * This function calls set_parameter of tiago_nav_module.
+      *
+      * It has the following input ports:
+      *
+      *   name_space (std::string): String with the parameter name space.
+      *
+      *   name (std::string): String with the name of the parameter to change. This must
+      *             coincide with a parameter name on the associated dynamic
+      *             reconfigure server.
+      *
+      *   value (std::string): the new value for the parameter.
+      *
+      * \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 tiago_nav_set_string_parameter(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized set_parameter TIAGo nav function.
+      *
+      * This function calls set_parameter of tiago_nav_module.
+      *
+      * It has the following input ports:
+      *
+      *   name_space (std::string): String with the parameter name space.
+      *
+      *   name (std::string): String with the name of the parameter to change. This must
+      *             coincide with a parameter name on the associated dynamic
+      *             reconfigure server.
+      *
+      *   value (double): the new value for the parameter.
+      *
+      * \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 tiago_nav_set_double_parameter(BT::TreeNode& self);
+
     /**
       * \brief Synchronized stop TIAGo nav function.
       *

src/tiago_nav_bt_module.cpp

@@ -24,9 +24,16 @@ void CTiagoNavModuleBT::init(IriBehaviorTreeFactory &factory)
   BT::PortsList async_go_to_position_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("x_y_pos_tol"), BT::BidirectionalPort<bool>("new_goal")};
   BT::PortsList sync_go_to_poi_ports = {BT::InputPort<std::string>("poi")};
   BT::PortsList async_go_to_poi_ports = {BT::InputPort<std::string>("poi"), BT::BidirectionalPort<bool>("new_goal")};
-
   BT::PortsList set_goal_frame_ports = {BT::InputPort<std::string>("frame_id")};
 
+  BT::PortsList change_map_ports = {BT::InputPort<std::string>("map_name")};
+  BT::PortsList auto_clear_ports = {BT::InputPort<double>("rate_hz")};
+  BT::PortsList set_bool_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<bool>("value")};
+  BT::PortsList set_int_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<int>("value")};
+  BT::PortsList set_string_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<std::string>("value")};
+  BT::PortsList set_double_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<double>("value")};
+
+
   //Registration of conditions
   factory.registerSimpleCondition("is_tiago_nav_finished",  std::bind(&CTiagoNavModuleBT::is_tiago_nav_finished, this));
 
@@ -53,12 +60,21 @@ void CTiagoNavModuleBT::init(IriBehaviorTreeFactory &factory)
   factory.registerIriAsyncAction("async_tiago_nav_go_to_position",  std::bind(&CTiagoNavModuleBT::async_tiago_nav_go_to_position, this, std::placeholders::_1), async_go_to_position_ports);
   factory.registerSimpleAction("sync_tiago_nav_go_to_poi",  std::bind(&CTiagoNavModuleBT::sync_tiago_nav_go_to_poi, this, std::placeholders::_1), sync_go_to_poi_ports);
   factory.registerIriAsyncAction("async_tiago_nav_go_to_poi",  std::bind(&CTiagoNavModuleBT::async_tiago_nav_go_to_poi, this, std::placeholders::_1), async_go_to_poi_ports);
-
   factory.registerSimpleAction("set_tiago_nav_goal_frame",  std::bind(&CTiagoNavModuleBT::set_tiago_nav_goal_frame, this, std::placeholders::_1), set_goal_frame_ports);
+
+  factory.registerSimpleAction("tiago_nav_map_change",  std::bind(&CTiagoNavModuleBT::tiago_nav_map_change, this, std::placeholders::_1), change_map_ports);
+  factory.registerSimpleAction("tiago_nav_costmaps_clear",  std::bind(&CTiagoNavModuleBT::tiago_nav_costmaps_clear, this));
+  factory.registerSimpleAction("tiago_nav_costmaps_enable_auto_clear",  std::bind(&CTiagoNavModuleBT::tiago_nav_costmaps_enable_auto_clear, this, std::placeholders::_1), auto_clear_ports);
+  factory.registerSimpleAction("tiago_nav_costmaps_disable_auto_clear",  std::bind(&CTiagoNavModuleBT::tiago_nav_costmaps_disable_auto_clear, this));
+  factory.registerSimpleAction("tiago_nav_set_bool_parameter",  std::bind(&CTiagoNavModuleBT::tiago_nav_set_bool_parameter, this, std::placeholders::_1), set_bool_parameter_ports);
+  factory.registerSimpleAction("tiago_nav_set_int_parameter",  std::bind(&CTiagoNavModuleBT::tiago_nav_set_int_parameter, this, std::placeholders::_1), set_int_parameter_ports);
+  factory.registerSimpleAction("tiago_nav_set_string_parameter",  std::bind(&CTiagoNavModuleBT::tiago_nav_set_string_parameter, this, std::placeholders::_1), set_string_parameter_ports);
+  factory.registerSimpleAction("tiago_nav_set_double_parameter",  std::bind(&CTiagoNavModuleBT::tiago_nav_set_double_parameter, this, std::placeholders::_1), set_double_parameter_ports);
+
   #ifdef HAVE_WAYPOINTS
   BT::PortsList sync_go_to_waypoints_ports = {BT::InputPort<std::string>("group"), BT::InputPort<unsigned int>("first_index"), BT::InputPort<unsigned int>("num"), BT::InputPort<bool>("continue_on_error")};
   BT::PortsList async_go_to_waypoints_ports = {BT::InputPort<std::string>("group"), BT::InputPort<unsigned int>("first_index"), BT::InputPort<unsigned int>("num"), BT::InputPort<bool>("continue_on_error"), BT::BidirectionalPort<bool>("new_goal")};
-  BT::PortsList get_current_waypoint_ports = {BT::InputPort<unsigned int>("waypoint")};
+  BT::PortsList get_current_waypoint_ports = {BT::OutputPort<unsigned int>("waypoint")};
   factory.registerSimpleAction("sync_tiago_nav_go_to_waypoints",  std::bind(&CTiagoNavModuleBT::sync_tiago_nav_go_to_waypoints, this, std::placeholders::_1), sync_go_to_waypoints_ports);
   factory.registerIriAsyncAction("async_tiago_nav_go_to_waypoints",  std::bind(&CTiagoNavModuleBT::async_tiago_nav_go_to_waypoints, this, std::placeholders::_1), async_go_to_waypoints_ports);
   factory.registerSimpleAction("get_tiago_nav_current_waypoint",  std::bind(&CTiagoNavModuleBT::get_tiago_nav_current_waypoint, this, std::placeholders::_1), get_current_waypoint_ports);
@@ -500,6 +516,154 @@ BT::NodeStatus CTiagoNavModuleBT::set_tiago_nav_goal_frame(BT::TreeNode& self)
   return BT::NodeStatus::SUCCESS;
 }
 
+BT::NodeStatus CTiagoNavModuleBT::tiago_nav_map_change(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::tiago_nav_map_change-> tiago_nav_map_change");
+  BT::Optional<std::string> map_name = self.getInput<std::string>("map_name");
+
+  std::string map_name_goal;
+  if (!map_name)
+  {
+    ROS_ERROR("CTiagoNavModuleBT::tiago_nav_map_change-> Incorrect or missing input. It needs the following input ports: map_name(std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  map_name_goal = map_name.value();
+
+  if (this->tiago_nav_module.map_change(map_name_goal))
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::tiago_nav_costmaps_clear(void)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::tiago_nav_costmaps_clear-> tiago_nav_costmaps_clear");
+  if (this->tiago_nav_module.costmaps_clear())
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::tiago_nav_costmaps_enable_auto_clear(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::tiago_nav_costmaps_enable_auto_clear-> tiago_nav_costmaps_enable_auto_clear");
+  BT::Optional<double> rate_hz = self.getInput<double>("rate_hz");
+
+  double rate_hz_goal;
+  if (!rate_hz)
+  {
+    ROS_ERROR("CTiagoNavModuleBT::tiago_nav_costmaps_enable_auto_clear-> Incorrect or missing input. It needs the following input ports: rate_hz(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  rate_hz_goal = rate_hz.value();
+
+  this->tiago_nav_module.costmaps_enable_auto_clear(rate_hz_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::tiago_nav_costmaps_disable_auto_clear(void)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::tiago_nav_costmaps_disable_auto_clear-> tiago_nav_costmaps_disable_auto_clear");
+  this->tiago_nav_module.costmaps_disable_auto_clear();
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::tiago_nav_set_bool_parameter(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::tiago_nav_set_bool_parameter-> tiago_nav_set_bool_parameter");
+  BT::Optional<std::string> name_space = self.getInput<std::string>("name_space");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+  BT::Optional<bool> value = self.getInput<bool>("value");
+
+  std::string name_space_goal, name_goal;
+  bool value_goal;
+  if (!name_space || !name || !value)
+  {
+    ROS_ERROR("CTiagoNavModuleBT::tiago_nav_set_bool_parameter-> Incorrect or missing input. It needs the following input ports: name_space(std::string), name(std::string) and value(bool)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_space_goal = name_space.value();
+  name_goal = name.value();
+  value_goal = value.value();
+
+  if (this->tiago_nav_module.set_parameter(name_space_goal, name_goal, value_goal))
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::tiago_nav_set_int_parameter(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::tiago_nav_set_int_parameter-> tiago_nav_set_int_parameter");
+  BT::Optional<std::string> name_space = self.getInput<std::string>("name_space");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+  BT::Optional<int> value = self.getInput<int>("value");
+
+  std::string name_space_goal, name_goal;
+  int value_goal;
+  if (!name_space || !name || !value)
+  {
+    ROS_ERROR("CTiagoNavModuleBT::tiago_nav_set_bool_parameter-> Incorrect or missing input. It needs the following input ports: name_space(std::string), name(std::string) and value(int)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_space_goal = name_space.value();
+  name_goal = name.value();
+  value_goal = value.value();
+
+  if (this->tiago_nav_module.set_parameter(name_space_goal, name_goal, value_goal))
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::tiago_nav_set_string_parameter(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::tiago_nav_set_string_parameter-> tiago_nav_set_string_parameter");
+  BT::Optional<std::string> name_space = self.getInput<std::string>("name_space");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+  BT::Optional<std::string> value = self.getInput<std::string>("value");
+
+  std::string name_space_goal, name_goal;
+  std::string value_goal;
+  if (!name_space || !name || !value)
+  {
+    ROS_ERROR("CTiagoNavModuleBT::tiago_nav_set_bool_parameter-> Incorrect or missing input. It needs the following input ports: name_space(std::string), name(std::string) and value(std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_space_goal = name_space.value();
+  name_goal = name.value();
+  value_goal = value.value();
+
+  if (this->tiago_nav_module.set_parameter(name_space_goal, name_goal, value_goal))
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoNavModuleBT::tiago_nav_set_double_parameter(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::tiago_nav_set_double_parameter-> tiago_nav_set_double_parameter");
+  BT::Optional<std::string> name_space = self.getInput<std::string>("name_space");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+  BT::Optional<double> value = self.getInput<double>("value");
+
+  std::string name_space_goal, name_goal;
+  double value_goal;
+  if (!name_space || !name || !value)
+  {
+    ROS_ERROR("CTiagoNavModuleBT::tiago_nav_set_bool_parameter-> Incorrect or missing input. It needs the following input ports: name_space(std::string), name(std::string) and value(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_space_goal = name_space.value();
+  name_goal = name.value();
+  value_goal = value.value();
+
+  if (this->tiago_nav_module.set_parameter(name_space_goal, name_goal, value_goal))
+    return BT::NodeStatus::SUCCESS;
+  return BT::NodeStatus::FAILURE;
+}
+
 BT::NodeStatus CTiagoNavModuleBT::sync_cancel_tiago_nav_action(void)
 {
   ROS_DEBUG("CTiagoNavModuleBT::sync_cancel_tiago_nav_action-> sync_cancel_tiago_nav_action");