wip: test logging

mojin-robotics · Jul 12, 2023 · 4cf0f70 · 4cf0f70
1 parent 790f6ba
commit 4cf0f70
Showing 1 changed file with 12 additions and 6 deletions.
diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp
@@ -106,7 +106,8 @@ class ActionFactory_1_2 : public ActionFactoryInterface
     std::lock_guard<std::mutex> lock(mutex_);
     goals_.insert(std::make_pair(goal_id, handler));
 
-    RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal");
+    ROS_INFO("ROS_INFO ActionFactory_1_2 Sending goal");
+    RCLCPP_INFO(ros2_node_->get_logger(), "RCLCPP_INFO ActionFactory_1_2 Sending goal");
     std::thread(
       [handler, goal_id, this]() mutable {
         // execute the goal remotely
@@ -297,7 +298,8 @@ class ActionFactory_2_1 : public ActionFactoryInterface
     std::lock_guard<std::mutex> lock(mutex_);
     goals_.insert(std::make_pair(goal_id, handler));
 
-    RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal");
+    ROS_INFO("ROS_INFO ActionFactory_2_1 Sending goal");
+    RCLCPP_INFO(ros2_node_->get_logger(), "RCLCPP_INFO ActionFactory_2_1 Sending goal");
     std::thread(
       [handler, goal_id, this]() mutable {
         // execute the goal remotely
@@ -342,7 +344,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface
         [this, &result_ready,
         &cond_result](ROS1ClientGoalHandle goal_handle) mutable           // transition_cb
         {
-          ROS_INFO("Goal [%s]", goal_handle.getCommState().toString().c_str());
+          ROS_INFO("ActionFactory_2_1 Goal [%s]", goal_handle.getCommState().toString().c_str());
           if (goal_handle.getCommState() == actionlib::CommState::RECALLING) {
             // cancelled before being processed
             auto result2 = std::make_shared<ROS2Result>();
@@ -355,7 +357,8 @@ class ActionFactory_2_1 : public ActionFactoryInterface
             auto result2 = std::make_shared<ROS2Result>();
             auto result1 = goal_handle.getResult();
             translate_result_1_to_2(*result2, *result1);
-            ROS_INFO("Goal [%s]", goal_handle.getTerminalState().toString().c_str());
+            ROS_INFO("ActionFactory_2_1 Goal [%s]",
+                     goal_handle.getTerminalState().toString().c_str());
             if (goal_handle.getTerminalState() == actionlib::TerminalState::SUCCEEDED) {
               gh2_->succeed(result2);
             } else {
@@ -379,13 +382,16 @@ class ActionFactory_2_1 : public ActionFactoryInterface
       cond_result.wait(lck, [&result_ready] {return result_ready.load();});
     }
 
-    GoalHandler(std::shared_ptr<ROS2ServerGoalHandle> & gh2, std::shared_ptr<ROS1Client> & client)
-    : gh2_(gh2), client_(client), canceled_(false) {}
+    GoalHandler(std::shared_ptr<ROS2ServerGoalHandle> & gh2,
+                std::shared_ptr<ROS1Client> & client,
+                rclcpp::Logger logger)
+    : gh2_(gh2), client_(client), logger_(logger), canceled_(false) {}
 
 private:
     std::shared_ptr<ROS1ClientGoalHandle> gh1_;
     std::shared_ptr<ROS2ServerGoalHandle> gh2_;
     std::shared_ptr<ROS1Client> client_;
+    rclcpp::Logger logger_;
     bool canceled_;      // cancel was called
     std::mutex mutex_;
   };