Skip to content

Commit

Permalink
pass logger to GoalHandler
Browse files Browse the repository at this point in the history
Signed-off-by: Harsh Deshpande <[email protected]>
  • Loading branch information
hsd-dev committed Mar 27, 2023
1 parent 5e3c841 commit 79a46de
Showing 1 changed file with 5 additions and 4 deletions.
9 changes: 5 additions & 4 deletions include/ros1_bridge/action_factory.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface

// create a new handler for the goal
std::shared_ptr<GoalHandler> handler;
handler = std::make_shared<GoalHandler>(gh1, client_);
handler = std::make_shared<GoalHandler>(gh1, client_, ros2_node_->get_logger());
std::lock_guard<std::mutex> lock(mutex_);
goals_.insert(std::make_pair(goal_id, handler));

Expand Down Expand Up @@ -138,7 +138,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface
translate_goal_1_to_2(*gh1_.getGoal(), goal2);

if (!client_->wait_for_action_server(std::chrono::seconds(1))) {
RCLCPP_INFO(ros2_node_->get_logger(), "Action server not available after waiting");
RCLCPP_INFO(this->logger_, "Action server not available after waiting");
gh1_.setRejected();
return;
}
Expand Down Expand Up @@ -190,13 +190,14 @@ class ActionFactory_1_2 : public ActionFactoryInterface
}
}

GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client)
: gh1_(gh1), gh2_(nullptr), client_(client), canceled_(false) {}
GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client, rclcpp::Logger logger)
: gh1_(gh1), gh2_(nullptr), client_(client), logger_(logger), canceled_(false) {}

private:
ROS1GoalHandle gh1_;
ROS2ClientGoalHandle gh2_;
ROS2ClientSharedPtr client_;
rclcpp::Logger logger_;
bool canceled_; // cancel was called
std::mutex mutex_gh_;
};
Expand Down

0 comments on commit 79a46de

Please sign in to comment.