Skip to content
GitLab
Commit 57474c0f authored by abhagwan's avatar abhagwan
Browse files

Fixed bugs respect client

parent e7082812
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 57474c0f
......@@ -129,6 +129,7 @@ add_library(iri_ana_nav_module_bt
target_link_libraries(iri_ana_nav_module ${catkin_LIBRARIES})
target_link_libraries(iri_ana_nav_module_bt ${catkin_LIBRARIES})
target_link_libraries(iri_ana_nav_module ${iriutils_LIBRARY})
target_link_libraries(iri_ana_nav_module_bt ${iriutils_LIBRARY})
##Link to other modules
##target_link_libraries(new_module <module path>/lib<module_name>.so)
......@@ -143,6 +144,7 @@ add_executable(iri_ana_nav_client src/iri_ana_nav_client_alg.cpp src/iri_ana_nav
target_link_libraries(iri_ana_nav_client ${catkin_LIBRARIES})
target_link_libraries(iri_ana_nav_client ${iriutils_LIBRARY})
target_link_libraries(iri_ana_nav_client iri_ana_nav_module)
target_link_libraries(iri_ana_nav_module_bt iri_ana_nav_module)
add_dependencies(iri_ana_nav_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
......
include/iri_ana_nav_module/iri_ana_nav_module_bt.h
View file @ 57474c0f
......@@ -17,10 +17,6 @@ class CIriAnaNavModuleBT
{
private:
// tree file in xml format needed for creation of the behavior tree
std::string path;
std::string tree_xml_file;
// tree object to save the behavior tree created and execute its tick()
BT::Tree tree;
......@@ -34,13 +30,10 @@ class CIriAnaNavModuleBT
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;
// object of iri_ana_nav_module
CIriAnaNavModule nav;
......@@ -51,7 +44,51 @@ class CIriAnaNavModuleBT
*/
CIriAnaNavModuleBT();
void init(IriBehaviorTreeFactory *factory);
// tree file in xml format needed for creation of the behavior tree
std::string path;
std::string tree_xml_file;
// variable to save behavior tree status
BT::NodeStatus status;
void init(IriBehaviorTreeFactory &factory);
void tick();
void stop();
/**
* \brief Restart behavior tree function
*
* This functions halts all nodes, so their status changes to IDLE. ATENTION:
* It does not reset the parameters values changed during the execution of the
* behavior tree and stop() should be called before restarting.
*
*/
void restart();
void create_tree(IriBehaviorTreeFactory &factory, std::string path, std::string tree_xml_file);
void enable_cout_logger();
void enable_minitrace_logger();
void enable_file_logger();
void enable_zmq_publisher();
void enable_print_tree();
void disable_cout_logger();
void disable_minitrace_logger();
void disable_file_logger();
void disable_zmq_publisher();
void disable_print_tree();
/**
* \brief Destructor
*
......@@ -515,42 +552,6 @@ class CIriAnaNavModuleBT
*/
BT::NodeStatus costmap_is_auto_clear_enabled();
void tick();
void stop();
/**
* \brief Restart behavior tree function
*
* This functions halts all nodes, so their status changes to IDLE. ATENTION:
* It does not reset the parameters values changed during the execution of the
* behavior tree and stop() should be called before restarting.
*
*/
void restart();
void create_tree(IriBehaviorTreeFactory *factory, std::string path, std::string tree_xml_file);
void enable_cout_logger();
void enable_minitrace_logger();
void enable_file_logger();
void enable_zmq_publisher();
void enable_print_tree();
void disable_cout_logger();
void disable_minitrace_logger();
void disable_file_logger();
void disable_zmq_publisher();
void disable_print_tree();
};
#endif
src/iri_ana_nav_module_bt.cpp
View file @ 57474c0f
......@@ -17,9 +17,11 @@ CIriAnaNavModuleBT::CIriAnaNavModuleBT() :
orientation_goal_sent = false;
position_goal_sent = false;
pose_goal_sent = false;
this->status=BT::NodeStatus::IDLE;
}
void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory)
void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
{
// definition of ports
BT::PortsList sync_orientation_port = { BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol") };
......@@ -33,39 +35,39 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory)
BT::PortsList costmap_port = { BT::InputPort<double>("rate_hz") };
// registry of conditions
factory->registerSimpleCondition("is_finished", std::bind(&CIriAnaNavModuleBT::is_finished, this));
factory->registerSimpleCondition("is_succeded", std::bind(&CIriAnaNavModuleBT::is_succeded, this));
factory->registerSimpleCondition("is_running", std::bind(&CIriAnaNavModuleBT::is_running, this));
factory->registerSimpleCondition("is_action_start_failed", std::bind(&CIriAnaNavModuleBT::is_action_start_failed, this));
factory->registerSimpleCondition("is_timeout", std::bind(&CIriAnaNavModuleBT::is_timeout, this));
factory->registerSimpleCondition("is_watchdog", std::bind(&CIriAnaNavModuleBT::is_watchdog, this));
factory->registerSimpleCondition("is_aborted", std::bind(&CIriAnaNavModuleBT::is_aborted, this));
factory->registerSimpleCondition("is_preempted", std::bind(&CIriAnaNavModuleBT::is_preempted, this));
factory->registerSimpleCondition("is_rejected", std::bind(&CIriAnaNavModuleBT::is_rejected, this));
factory->registerSimpleCondition("is_set_param_failed", std::bind(&CIriAnaNavModuleBT::is_set_param_failed, this));
factory->registerSimpleCondition("is_param_not_present", std::bind(&CIriAnaNavModuleBT::is_param_not_present, this));
factory->registerSimpleCondition("is_no_odom", std::bind(&CIriAnaNavModuleBT::is_no_odom, this));
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));
factory.registerSimpleCondition("is_finished", std::bind(&CIriAnaNavModuleBT::is_finished, this));
factory.registerSimpleCondition("is_succeded", std::bind(&CIriAnaNavModuleBT::is_succeded, this));
factory.registerSimpleCondition("is_running", std::bind(&CIriAnaNavModuleBT::is_running, this));
factory.registerSimpleCondition("is_action_start_failed", std::bind(&CIriAnaNavModuleBT::is_action_start_failed, this));
factory.registerSimpleCondition("is_timeout", std::bind(&CIriAnaNavModuleBT::is_timeout, this));
factory.registerSimpleCondition("is_watchdog", std::bind(&CIriAnaNavModuleBT::is_watchdog, this));
factory.registerSimpleCondition("is_aborted", std::bind(&CIriAnaNavModuleBT::is_aborted, this));
factory.registerSimpleCondition("is_preempted", std::bind(&CIriAnaNavModuleBT::is_preempted, this));
factory.registerSimpleCondition("is_rejected", std::bind(&CIriAnaNavModuleBT::is_rejected, this));
factory.registerSimpleCondition("is_set_param_failed", std::bind(&CIriAnaNavModuleBT::is_set_param_failed, this));
factory.registerSimpleCondition("is_param_not_present", std::bind(&CIriAnaNavModuleBT::is_param_not_present, this));
factory.registerSimpleCondition("is_no_odom", std::bind(&CIriAnaNavModuleBT::is_no_odom, this));
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), 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);
factory->registerSimpleAction("costmaps_clear", std::bind(&CIriAnaNavModuleBT::costmaps_clear, this));
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));
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);
factory.registerSimpleAction("costmaps_clear", std::bind(&CIriAnaNavModuleBT::costmaps_clear, this));
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), 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));
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
std::cout << "create tree" << std::endl;
this->tree = factory->createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml");
this->tree = factory.createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml");
}
CIriAnaNavModuleBT::~CIriAnaNavModuleBT(void)
......@@ -668,7 +670,7 @@ void CIriAnaNavModuleBT::restart()
}
}
void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory *factory, std::string path, std::string tree_xml_file)
void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory &factory, std::string path, std::string tree_xml_file)
{
if (this->path != path || this->tree_xml_file != tree_xml_file)
{
......@@ -685,7 +687,7 @@ void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory *factory, std::strin
this->tree_xml_file=tree_xml_file;
try
{
this->tree = factory->createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml");
this->tree = factory.createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml");
this->status = BT::NodeStatus::IDLE;
// if logger exists delete previous one and create a new one
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment