diff --git a/include/tiago_nav_module/tiago_nav_bt_module.h b/include/tiago_nav_module/tiago_nav_bt_module.h index 6aa21fdadc3c212fc831d23859f1188a5ed395e9..8f9914dc15d677c3e8d2374f6c7ceb899ae9b3f1 100644 --- a/include/tiago_nav_module/tiago_nav_bt_module.h +++ b/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. * diff --git a/src/tiago_nav_bt_module.cpp b/src/tiago_nav_bt_module.cpp index 06e1e18f7ab75787748692aaaee9ca37d2d2c688..30b2d06a77522119e27f66150ae4a7fd3cfcda2a 100644 --- a/src/tiago_nav_bt_module.cpp +++ b/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");