Skip to content

Commit

Permalink
remove second costmap
Browse files Browse the repository at this point in the history
Signed-off-by: Tony Najjar <[email protected]>
  • Loading branch information
tonynajjar committed Aug 6, 2024
1 parent e11923a commit 384bccb
Show file tree
Hide file tree
Showing 2 changed files with 9 additions and 41 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,6 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase
BT::InputPort<Goals>("input_goals", "Original goals to remove from"),
BT::InputPort<std::string>("costmap_cost_service",
"Service to get cost from costmap"),
BT::InputPort<std::string>("costmap_cost_service_2",
"Second service name to get cost from costmap. If empty, only first service is used"),
BT::InputPort<double>("cost_threshold",
"Cost threshold for considering a goal in collision"),
BT::InputPort<bool>("use_footprint",
Expand All @@ -63,9 +61,7 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase

rclcpp::Node::SharedPtr node_;
rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr get_cost_client_;
rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr get_cost_client_2_;
std::string costmap_cost_service_;
std::string costmap_cost_service_2_;
bool initialized_;
bool use_footprint_;
double cost_threshold_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,6 @@ RemoveInCollisionGoals::RemoveInCollisionGoals(
: BT::ActionNodeBase(name, conf),
initialized_(false),
costmap_cost_service_("/global_costmap/get_cost_global_costmap"),
costmap_cost_service_2_(""),
use_footprint_(true),
cost_threshold_(253)
{}
Expand All @@ -41,15 +40,10 @@ void RemoveInCollisionGoals::initialize()
server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");

getInput("costmap_cost_service", costmap_cost_service_);
getInput("costmap_cost_service_2", costmap_cost_service_2_);

get_cost_client_ = node_->create_client<nav2_msgs::srv::GetCost>(
costmap_cost_service_);

if (!costmap_cost_service_2_.empty()) {
get_cost_client_2_ = node_->create_client<nav2_msgs::srv::GetCost>(
costmap_cost_service_2_);
}
}

inline BT::NodeStatus RemoveInCollisionGoals::tick()
Expand Down Expand Up @@ -79,39 +73,17 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick()
request->theta = tf2::getYaw(goal.pose.orientation);
request->use_footprint = use_footprint_;

if (costmap_cost_service_2_.empty()) {
auto future = get_cost_client_->async_send_request(request);
auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_);
if (ret == rclcpp::FutureReturnCode::SUCCESS) {
if (future.get()->cost <= cost_threshold_) {
valid_goal_poses.push_back(goal);
}
} else {
RCLCPP_ERROR(
node_->get_logger(),
"RemoveInCollisionGoals BT node failed to call GetCost service of costmap");
return BT::NodeStatus::FAILURE;
auto future = get_cost_client_->async_send_request(request);
auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_);
if (ret == rclcpp::FutureReturnCode::SUCCESS) {
if (future.get()->cost <= cost_threshold_) {
valid_goal_poses.push_back(goal);
}
} else {
auto future = get_cost_client_->async_send_request(request);
auto future_2 = get_cost_client_2_->async_send_request(request);
auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_);
auto ret_2 = rclcpp::spin_until_future_complete(node_, future_2, server_timeout_);
if (ret == rclcpp::FutureReturnCode::SUCCESS &&
ret_2 == rclcpp::FutureReturnCode::SUCCESS)
{
if (future.get()->cost <= cost_threshold_ &&
future_2.get()->cost <= cost_threshold_)
{
valid_goal_poses.push_back(goal);
}
} else {
RCLCPP_ERROR(
node_->get_logger(),
"RemoveInCollisionGoals BT node failed to call GetCost "
"service of one or two costmaps");
return BT::NodeStatus::FAILURE;
}
RCLCPP_ERROR(
node_->get_logger(),
"RemoveInCollisionGoals BT node failed to call GetCost service of costmap");
return BT::NodeStatus::FAILURE;
}
}
setOutput("output_goals", valid_goal_poses);
Expand Down

0 comments on commit 384bccb

Please sign in to comment.