diff --git a/src/iri_ana_nav_module_bt.cpp b/src/iri_ana_nav_module_bt.cpp
index 6f1aeeae5cb05928993023c2d6e326bc8d31ef78..e719a1141dce159e71dedea4207eeebe6474adb1 100644
--- a/src/iri_ana_nav_module_bt.cpp
+++ b/src/iri_ana_nav_module_bt.cpp
@@ -28,7 +28,7 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
// detect huge variation in path
BT::PortsList path_distance_port = { BT::OutputPort<double>("path_distance") };
-
+
BT::PortsList velocity_port = { BT::OutputPort<double>("velocity") };
BT::PortsList status_text_port = { BT::OutputPort<std::string>("nav_status") };
@@ -78,28 +78,28 @@ CIriAnaNavModuleBT::~CIriAnaNavModuleBT(void)
BT::NodeStatus CIriAnaNavModuleBT::sync_go_to_orientation(BT::TreeNode& self)
{
- ROS_INFO("sync_go_to_orientation");
+ ROS_INFO("CIriAnaNavModuleBT:sync_go_to_orientation");
BT::Optional<double> yaw_input = self.getInput<double>("yaw");
BT::Optional<double> heading_tol_input = self.getInput<double>("heading_tol");
if (!yaw_input)
{
- ROS_ERROR("Incorrect or missing required input [yaw]");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_orientation Incorrect or missing required input [yaw]");
return BT::NodeStatus::FAILURE;
}
if (!heading_tol_input)
{
- ROS_ERROR("Incorrect or missing required input [heading_tol]");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_orientation Incorrect or missing required input [heading_tol]");
return BT::NodeStatus::FAILURE;
}
double yaw_goal = yaw_input.value();
double heading_tol_goal = heading_tol_input.value();
- ROS_INFO("The goal is yaw: [%f] with heading_tol: [%f]", yaw_goal, heading_tol_goal);
+ ROS_INFO("CIriAnaNavModuleBT:sync_go_to_orientation The goal is yaw: [%f] with heading_tol: [%f]", yaw_goal, heading_tol_goal);
if(!this->nav.go_to_orientation(yaw_goal, heading_tol_goal))
{
- ROS_ERROR("Impossible to start goal");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_orientation Impossible to start goal");
return BT::NodeStatus::FAILURE;
}
else
@@ -110,35 +110,35 @@ BT::NodeStatus CIriAnaNavModuleBT::sync_go_to_orientation(BT::TreeNode& self)
BT::NodeStatus CIriAnaNavModuleBT::sync_go_to_position(BT::TreeNode& self)
{
- ROS_INFO("sync_go_to_position");
+ ROS_INFO("CIriAnaNavModuleBT:sync_go_to_position");
BT::Optional<double> x_input = self.getInput<double>("x");
BT::Optional<double> y_input = self.getInput<double>("y");
BT::Optional<double> x_y_pos_tol_input = self.getInput<double>("x_y_pos_tol");
if (!x_input)
{
- ROS_ERROR("Incorrect or missing required input [x]");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_position Incorrect or missing required input [x]");
return BT::NodeStatus::FAILURE;
}
if (!y_input)
{
- ROS_ERROR("Incorrect or missing required input [y]");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_position Incorrect or missing required input [y]");
return BT::NodeStatus::FAILURE;
}
if (!x_y_pos_tol_input)
{
- ROS_ERROR("Incorrect or missing required input [x_y_pos_tol]");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_position Incorrect or missing required input [x_y_pos_tol]");
return BT::NodeStatus::FAILURE;
}
double x_goal = x_input.value();
double y_goal = y_input.value();
double x_y_pos_tol_goal = x_y_pos_tol_input.value();
- ROS_INFO("The goal is x: [%f], y: [%f] with x_y_pos_tol: [%f]", x_goal, y_goal, x_y_pos_tol_goal);
+ ROS_INFO("CIriAnaNavModuleBT:sync_go_to_position The goal is x: [%f], y: [%f] with x_y_pos_tol: [%f]", x_goal, y_goal, x_y_pos_tol_goal);
if(!this->nav.go_to_position(x_goal, y_goal, x_y_pos_tol_goal))
{
- ROS_ERROR("Impossible to start goal");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_position Impossible to start goal");
return BT::NodeStatus::FAILURE;
}
else
@@ -150,7 +150,7 @@ BT::NodeStatus CIriAnaNavModuleBT::sync_go_to_position(BT::TreeNode& self)
BT::NodeStatus CIriAnaNavModuleBT::sync_go_to_pose(BT::TreeNode& self)
{
- ROS_INFO("sync_go_to_pose");
+ ROS_INFO("CIriAnaNavModuleBT:sync_go_to_pose");
BT::Optional<double> x_input = self.getInput<double>("x");
BT::Optional<double> y_input = self.getInput<double>("y");
BT::Optional<double> yaw_input = self.getInput<double>("yaw");
@@ -159,29 +159,29 @@ BT::NodeStatus CIriAnaNavModuleBT::sync_go_to_pose(BT::TreeNode& self)
if (!x_input)
{
- ROS_ERROR("Incorrect or missing required input [x]");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_pose Incorrect or missing required input [x]");
return BT::NodeStatus::FAILURE;
}
if (!y_input)
{
- ROS_ERROR("Incorrect or missing required input [y]");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_pose Incorrect or missing required input [y]");
return BT::NodeStatus::FAILURE;
}
if (!yaw_input)
{
- ROS_ERROR("Incorrect or missing required input [yaw]");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_pose Incorrect or missing required input [yaw]");
return BT::NodeStatus::FAILURE;
}
if (!heading_tol_input)
{
- ROS_ERROR("Incorrect or missing required input [heading_tol]");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_pose Incorrect or missing required input [heading_tol]");
return BT::NodeStatus::FAILURE;
}
if (!x_y_pos_tol_input)
{
- ROS_ERROR("Incorrect or missing required input [x_y_pos_tol]");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_pose Incorrect or missing required input [x_y_pos_tol]");
return BT::NodeStatus::FAILURE;
}
@@ -190,10 +190,10 @@ BT::NodeStatus CIriAnaNavModuleBT::sync_go_to_pose(BT::TreeNode& self)
double yaw_goal = yaw_input.value();
double heading_tol_goal = heading_tol_input.value();
double x_y_pos_tol_goal = x_y_pos_tol_input.value();
- ROS_INFO("The goal is x: [%f], y: [%f], yaw: [%f] with heading_tol: [%f] and x_y_pos_tol: [%f]", x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal);
+ ROS_INFO("CIriAnaNavModuleBT:sync_go_to_pose The goal is x: [%f], y: [%f], yaw: [%f] with heading_tol: [%f] and x_y_pos_tol: [%f]", x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal);
if(!this->nav.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal))
{
- ROS_ERROR("Impossible to start goal");
+ ROS_ERROR("CIriAnaNavModuleBT:sync_go_to_pose Impossible to start goal");
return BT::NodeStatus::FAILURE;
}
else
@@ -204,7 +204,7 @@ BT::NodeStatus CIriAnaNavModuleBT::sync_go_to_pose(BT::TreeNode& self)
BT::NodeStatus CIriAnaNavModuleBT::async_go_to_orientation(BT::TreeNode& self)
{
- ROS_INFO("async_go_to_orientation");
+ ROS_INFO("CIriAnaNavModuleBT:async_go_to_orientation");
static bool update_goal_value;
@@ -225,23 +225,23 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_orientation(BT::TreeNode& self)
if (!yaw_input)
{
- ROS_ERROR("Incorrect or missing required input [yaw]");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_orientation Incorrect or missing required input [yaw]");
return BT::NodeStatus::FAILURE;
}
if (!heading_tol_input)
{
- ROS_ERROR("Incorrect or missing required input [heading_tol]");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_orientation Incorrect or missing required input [heading_tol]");
return BT::NodeStatus::FAILURE;
}
double yaw_goal = yaw_input.value();
double heading_tol_goal = heading_tol_input.value();
- ROS_INFO("The goal is yaw: [%f] with heading_tol: [%f]", yaw_goal, heading_tol_goal);
+ ROS_INFO("CIriAnaNavModuleBT:async_go_to_orientation The goal is yaw: [%f] with heading_tol: [%f]", yaw_goal, heading_tol_goal);
if(!this->nav.go_to_orientation(yaw_goal, heading_tol_goal))
{
- ROS_ERROR("Impossible to start goal");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_orientation Impossible to start goal");
return BT::NodeStatus::FAILURE;
}
orientation_goal_sent = true;
@@ -257,7 +257,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_orientation(BT::TreeNode& self)
BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self)
{
- ROS_INFO("async_go_to_position");
+ ROS_INFO("CIriAnaNavModuleBT:async_go_to_position");
static bool update_goal_value;
@@ -279,29 +279,29 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self)
if (!x_input)
{
- ROS_ERROR("Incorrect or missing required input [x]");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_position Incorrect or missing required input [x]");
return BT::NodeStatus::FAILURE;
}
if (!y_input)
{
- ROS_ERROR("Incorrect or missing required input [y]");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_position Incorrect or missing required input [y]");
return BT::NodeStatus::FAILURE;
}
if (!x_y_pos_tol_input)
{
- ROS_ERROR("Incorrect or missing required input [x_y_pos_tol]");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_position Incorrect or missing required input [x_y_pos_tol]");
return BT::NodeStatus::FAILURE;
}
double x_goal = x_input.value();
double y_goal = y_input.value();
double x_y_pos_tol_goal = x_y_pos_tol_input.value();
- ROS_INFO("The goal is x: [%f], y: [%f] with x_y_pos_tol: [%f]", x_goal, y_goal, x_y_pos_tol_goal);
+ ROS_INFO("CIriAnaNavModuleBT:async_go_to_position The goal is x: [%f], y: [%f] with x_y_pos_tol: [%f]", x_goal, y_goal, x_y_pos_tol_goal);
if(!this->nav.go_to_position(x_goal, y_goal, x_y_pos_tol_goal))
{
- ROS_ERROR("Impossible to start goal");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_position Impossible to start goal");
return BT::NodeStatus::FAILURE;
}
position_goal_sent = true;
@@ -317,7 +317,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self)
BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
{
- ROS_INFO("async_go_to_pose");
+ ROS_INFO("CIriAnaNavModuleBT:async_go_to_pose");
static bool update_goal_value;
@@ -341,29 +341,29 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
if (!x_input)
{
- ROS_ERROR("Incorrect or missing required input [x]");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_pose Incorrect or missing required input [x]");
return BT::NodeStatus::FAILURE;
}
if (!y_input)
{
- ROS_ERROR("Incorrect or missing required input [y]");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_pose Incorrect or missing required input [y]");
return BT::NodeStatus::FAILURE;
}
if (!yaw_input)
{
- ROS_ERROR("Incorrect or missing required input [yaw]");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_pose Incorrect or missing required input [yaw]");
return BT::NodeStatus::FAILURE;
}
if (!heading_tol_input)
{
- ROS_ERROR("Incorrect or missing required input [heading_tol]");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_pose Incorrect or missing required input [heading_tol]");
return BT::NodeStatus::FAILURE;
}
if (!x_y_pos_tol_input)
{
- ROS_ERROR("Incorrect or missing required input [x_y_pos_tol]");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_pose Incorrect or missing required input [x_y_pos_tol]");
return BT::NodeStatus::FAILURE;
}
@@ -372,10 +372,10 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
double yaw_goal = yaw_input.value();
double heading_tol_goal = heading_tol_input.value();
double x_y_pos_tol_goal = x_y_pos_tol_input.value();
- ROS_INFO("The goal is x: [%f], y: [%f], yaw: [%f] with heading_tol: [%f] and x_y_pos_tol: [%f]", x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal);
+ ROS_INFO("CIriAnaNavModuleBT:async_go_to_pose The goal is x: [%f], y: [%f], yaw: [%f] with heading_tol: [%f] and x_y_pos_tol: [%f]", x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal);
if(!this->nav.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal))
{
- ROS_ERROR("Impossible to start goal");
+ ROS_ERROR("CIriAnaNavModuleBT:async_go_to_pose Impossible to start goal");
return BT::NodeStatus::FAILURE;
}
pose_goal_sent = true;
@@ -391,7 +391,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
BT::NodeStatus CIriAnaNavModuleBT::sync_stop_nav()
{
- ROS_INFO("-------STOP-------");
+ ROS_INFO("CIriAnaNavModuleBT:sync_stop_nav -------STOP-------");
nav.stop();
return BT::NodeStatus::SUCCESS;
}
@@ -407,15 +407,15 @@ BT::NodeStatus CIriAnaNavModuleBT::reset_goal_variables()
BT::NodeStatus CIriAnaNavModuleBT::get_current_path_distance(BT::TreeNode& self)
{
- ROS_DEBUG("get_current_path_distance");
+ ROS_DEBUG("CIriAnaNavModuleBT:get_current_path_distance");
int path_length = this->nav.get_path_length();
if (path_length>0) {
double path_distance = (path_length-1)*0.1;
self.setOutput("path_distance", path_distance);
- ROS_DEBUG("path_distance: %f", path_distance);
+ ROS_DEBUG("CIriAnaNavModuleBT:get_current_path_distance path_distance: %f", path_distance);
}
else {
- ROS_WARN("Path with length <= 0");
+ ROS_WARN("CIriAnaNavModuleBT:get_current_path_distance Path with length <= 0");
self.setOutput("path_distance", 0.0);
}
return BT::NodeStatus::SUCCESS;
@@ -423,8 +423,8 @@ BT::NodeStatus CIriAnaNavModuleBT::get_current_path_distance(BT::TreeNode& self)
BT::NodeStatus CIriAnaNavModuleBT::get_current_velocity(BT::TreeNode& self)
{
- ROS_DEBUG("get_current_velocity");
-
+ ROS_DEBUG("CIriAnaNavModuleBT:get_current_velocity");
+
geometry_msgs::Twist twist = this->nav.get_current_velocity();
self.setOutput("velocity", twist.linear.x);
return BT::NodeStatus::SUCCESS;
@@ -432,8 +432,8 @@ BT::NodeStatus CIriAnaNavModuleBT::get_current_velocity(BT::TreeNode& self)
BT::NodeStatus CIriAnaNavModuleBT::get_status_text(BT::TreeNode& self)
{
- ROS_DEBUG("get_status_text");
-
+ ROS_DEBUG("CIriAnaNavModuleBT:get_status_text");
+
std::string status_text = this->nav.get_status_text();
self.setOutput("nav_status", status_text);
return BT::NodeStatus::SUCCESS;
@@ -601,41 +601,41 @@ BT::NodeStatus CIriAnaNavModuleBT::get_current_status_nav(BT::TreeNode& self)
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_PREEMPTED){
- ROS_INFO("------- NAV_MODULE: PREEMPTED -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: PREEMPTED -------");
}
if (nav_status == IRI_ANA_NAV_MODULE_SUCCESS){
- ROS_INFO("------- NAV_MODULE: SUCCEEDED -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: SUCCEEDED -------");
}
if (nav_status == IRI_ANA_NAV_MODULE_ABORTED){
- ROS_INFO("------- NAV_MODULE: ABORTED -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: ABORTED -------");
}
if (nav_status == IRI_ANA_NAV_MODULE_REJECTED){
- ROS_INFO("------- NAV_MODULE: REJECTED -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: REJECTED -------");
}
if (nav_status == IRI_ANA_NAV_MODULE_RUNNING){
- ROS_INFO("------- NAV_MODULE: RUNNING -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: RUNNING -------");
}
if (nav_status == IRI_ANA_NAV_MODULE_ACTION_START_FAIL){
- ROS_INFO("------- NAV_MODULE: ACTION_START_FAIL -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: ACTION_START_FAIL -------");
}
if (nav_status == IRI_ANA_NAV_MODULE_TIMEOUT){
- ROS_INFO("------- NAV_MODULE: TIMEOUT -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: TIMEOUT -------");
}
if (nav_status == IRI_ANA_NAV_MODULE_FB_WATCHDOG){
- ROS_INFO("------- NAV_MODULE: FB_WATCHDOG -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: FB_WATCHDOG -------");
}
if (nav_status == IRI_ANA_NAV_MODULE_SET_PARAM_FAIL){
- ROS_INFO("------- NAV_MODULE: SET_PARAM_FAIL -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: SET_PARAM_FAIL -------");
}
if (nav_status == IRI_ANA_NAV_MODULE_PARAM_NOT_PRESENT){
- ROS_INFO("------- NAV_MODULE: PARAM_NOT_PRESENT -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: PARAM_NOT_PRESENT -------");
}
if (nav_status == IRI_ANA_NAV_MODULE_NO_ODOM){
- ROS_INFO("------- NAV_MODULE: NO_ODOM -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: NO_ODOM -------");
}
if (nav_status == IRI_ANA_NAV_MODULE_NO_TRANSFORM){
- ROS_INFO("------- NAV_MODULE: NO_TRANSFORM -------");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_status_nav ------- NAV_MODULE: NO_TRANSFORM -------");
}
self.setOutput("status_code", nav_status);
@@ -645,31 +645,31 @@ BT::NodeStatus CIriAnaNavModuleBT::get_current_status_nav(BT::TreeNode& self)
BT::NodeStatus CIriAnaNavModuleBT::set_goal_frame(BT::TreeNode& self)
{
- ROS_INFO("set_goal_frame");
+ ROS_INFO("CIriAnaNavModuleBT:set_goal_frame");
BT::Optional<std::string> frame_id_input = self.getInput<std::string>("frame_id");
if (!frame_id_input)
{
- ROS_ERROR("Incorrect or missing required input [frame_id]");
+ ROS_ERROR("CIriAnaNavModuleBT:set_goal_frame Incorrect or missing required input [frame_id]");
return BT::NodeStatus::FAILURE;
}
std::string frame_id_goal = frame_id_input.value();
- ROS_INFO_STREAM("The new frame is: " << frame_id_goal);
+ ROS_INFO_STREAM("CIriAnaNavModuleBT:set_goal_frame The new frame is: " << frame_id_goal);
nav.set_goal_frame(frame_id_goal);
return BT::NodeStatus::SUCCESS;
}
BT::NodeStatus CIriAnaNavModuleBT::get_current_pose_nav(BT::TreeNode& self)
{
- ROS_INFO("get_current_pose_nav");
+ ROS_INFO("CIriAnaNavModuleBT:get_current_pose_nav");
geometry_msgs::Pose current_pose = nav.getCurrentPose();
self.setOutput("current_x_nav", current_pose.position.x);
self.setOutput("current_y_nav", current_pose.position.y);
self.setOutput("current_yaw_nav", tf::getYaw(current_pose.orientation));
- ROS_INFO("The current pose is: x-> [%f], y-> [%f], yaw-> [%f]", current_pose.position.x, current_pose.position.y, tf::getYaw(current_pose.orientation));
+ ROS_INFO("CIriAnaNavModuleBT:get_current_pose_nav The current pose is: x-> [%f], y-> [%f], yaw-> [%f]", current_pose.position.x, current_pose.position.y, tf::getYaw(current_pose.orientation));
return BT::NodeStatus::SUCCESS;
}
@@ -680,7 +680,7 @@ BT::NodeStatus CIriAnaNavModuleBT::NOP()
BT::NodeStatus CIriAnaNavModuleBT::costmaps_clear()
{
- ROS_INFO("costmaps_clear");
+ ROS_INFO("CIriAnaNavModuleBT:costmaps_clear");
if (!this->nav.costmaps_clear())
{
return BT::NodeStatus::FAILURE;
@@ -690,18 +690,18 @@ BT::NodeStatus CIriAnaNavModuleBT::costmaps_clear()
BT::NodeStatus CIriAnaNavModuleBT::costmaps_enable_auto_clear(BT::TreeNode& self)
{
- ROS_INFO("costmaps_enable_auto_clear");
+ ROS_INFO("CIriAnaNavModuleBT:costmaps_enable_auto_clear");
BT::Optional<double> rate_hz_input = self.getInput<double>("rate_hz");
if (!rate_hz_input)
{
- ROS_ERROR("Incorrect or missing required input [rate_hz]");
+ ROS_ERROR("CIriAnaNavModuleBT:costmaps_enable_auto_clear Incorrect or missing required input [rate_hz]");
return BT::NodeStatus::FAILURE;
}
double rate_hz_goal = rate_hz_input.value();
- ROS_INFO_STREAM("The rate in [hz] is: " << rate_hz_goal);
+ ROS_INFO_STREAM("CIriAnaNavModuleBT:costmaps_enable_auto_clear The rate in [hz] is: " << rate_hz_goal);
nav.costmaps_enable_auto_clear(rate_hz_goal);
return BT::NodeStatus::SUCCESS;
@@ -709,14 +709,14 @@ BT::NodeStatus CIriAnaNavModuleBT::costmaps_enable_auto_clear(BT::TreeNode& self
BT::NodeStatus CIriAnaNavModuleBT::costmaps_disable_auto_clear()
{
- ROS_INFO("costmaps_disable_auto_clear");
+ ROS_INFO("CIriAnaNavModuleBT:costmaps_disable_auto_clear");
nav.costmaps_disable_auto_clear();
return BT::NodeStatus::SUCCESS;
}
BT::NodeStatus CIriAnaNavModuleBT::costmap_is_auto_clear_enabled_nav()
{
- ROS_INFO("costmap_is_auto_clear_enabled_nav");
+ ROS_INFO("CIriAnaNavModuleBT:costmap_is_auto_clear_enabled_nav");
if (!this->nav.costmap_is_auto_clear_enabled())
{
return BT::NodeStatus::FAILURE;