launching a unified behavior tree (+rbss)

rbs_bt_executor/CMakeLists.txt

@@ -83,8 +83,8 @@ list(APPEND plugin_libs rbs_skill_move_joint_state)
 # add_library(rbs_pose_estimation SHARED src/PoseEstimation.cpp)
 # list(APPEND plugin_libs rbs_pose_estimation)
 
-add_library(rbs_object_detection SHARED src/ObjectDetection.cpp)
+# add_library(rbs_object_detection SHARED src/ObjectDetection.cpp)
-list(APPEND plugin_libs rbs_object_detection)
+# list(APPEND plugin_libs rbs_object_detection)
 
 # add_library(rbs_env_manager_starter SHARED src/EnvManager.cpp)
 # list(APPEND plugin_libs rbs_env_manager_starter)
@@ -95,8 +95,11 @@ list(APPEND plugin_libs rbs_skill_move_topose_array_bt_action_client)
 add_library(rbs_get_workspace SHARED src/GetWorkspace.cpp)
 list(APPEND plugin_libs rbs_get_workspace)
 
-add_library(rbs_act SHARED src/rbsBTAction.cpp)
+add_library(rbss_act SHARED src/rbssAction.cpp)
-list(APPEND plugin_libs rbs_act)
+list(APPEND plugin_libs rbss_act)
+
+add_library(rbss_cond SHARED src/rbssCondition.cpp)
+list(APPEND plugin_libs rbss_cond)
 
 add_executable(rbs_bt_executor src/TreeRunner.cpp)
 ament_target_dependencies(rbs_bt_executor ${dependencies})

rbs_bt_executor/src/ObjectDetection.cpp

@@ -1,90 +0,0 @@
-#include "behaviortree_ros2/bt_service_node.hpp"
-#include "lifecycle_msgs/msg/transition.hpp"
-#include "lifecycle_msgs/srv/change_state.hpp"
-#include "rcl_interfaces/msg/parameter.hpp"
-#include "rcl_interfaces/srv/set_parameters.hpp"
-#include <behaviortree_cpp/tree_node.h>
-#include <behaviortree_ros2/plugins.hpp>
-#include <behaviortree_ros2/ros_node_params.hpp>
-#include <memory>
-#include <rclcpp/parameter_client.hpp>
-
-using namespace BT;
-using namespace std::chrono_literals;
-using ObjDetectionSrv = lifecycle_msgs::srv::ChangeState;
-
-class ObjectDetection : public RosServiceNode<ObjDetectionSrv> {
-public:
-  ObjectDetection(const std::string &name, const NodeConfig &conf,
-                  const RosNodeParams &params)
-      : RosServiceNode<ObjDetectionSrv>(name, conf, params) {
-    m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
-        node_.lock(), "/object_detection");
-    while (!m_params_client->wait_for_service(1s)) {
-      if (!rclcpp::ok()) {
-        RCLCPP_ERROR(logger(),
-                     "Interrupted while waiting for the service. Exiting.");
-        break;
-      }
-    }
-    getInput("SettingPath", m_settings_path);
-  }
-
-  static PortsList providedPorts() {
-    return providedBasicPorts({
-        InputPort<std::string>("ReqKind"),
-        InputPort<std::string>("SettingPath"),
-    });
-  }
-
-  bool setRequest(Request::SharedPtr &request) override {
-    getInput("ReqKind", m_req_type);
-    auto transition = transition_event(m_req_type);
-    request->set__transition(transition);
-    return true;
-  }
-
-  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
-    if (!response->success) {
-      return NodeStatus::FAILURE;
-    }
-    return NodeStatus::SUCCESS;
-  }
-
-private:
-  uint8_t transition_id_{};
-  std::string m_settings_path{};
-  //   std::string _model_path{};
-  std::string m_req_type{};
-  std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
-  std::vector<rcl_interfaces::msg::Parameter> _params;
-  rcl_interfaces::msg::Parameter m_param;
-
-  lifecycle_msgs::msg::Transition
-  transition_event(const std::string &req_type) {
-    lifecycle_msgs::msg::Transition translation{};
-    if (req_type == "configure") {
-      set_str_param();
-      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
-    } else if (req_type == "activate") {
-      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
-    } else if (req_type == "deactivate") {
-      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
-    } else if (req_type == "cleanup") {
-      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
-    }
-    translation.set__id(transition_id_);
-    return translation;
-  }
-
-  void set_str_param() {
-    RCLCPP_INFO_STREAM(logger(),
-                       "Set string parameter: <" + m_settings_path + ">");
-
-    std::vector<rclcpp::Parameter> t_parameters{
-        rclcpp::Parameter("setting_path", m_settings_path)};
-    m_params_client->set_parameters(t_parameters);
-  }
-};
-
-CreateRosNodePlugin(ObjectDetection, "ObjectDetection");

rbs_bt_executor/src/PoseEstimation.cpp

@@ -1,126 +0,0 @@
-#include "behaviortree_ros2/bt_service_node.hpp"
-
-#include "rcl_interfaces/msg/parameter.hpp"
-#include "rcl_interfaces/srv/set_parameters.hpp"
-
-#include "ament_index_cpp/get_package_share_directory.hpp"
-#include "lifecycle_msgs/msg/transition.hpp"
-#include "lifecycle_msgs/srv/change_state.hpp"
-#include <behaviortree_ros2/plugins.hpp>
-#include <memory>
-
-using namespace BT;
-using namespace std::chrono_literals;
-using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState;
-
-class PoseEstimation : public RosServiceNode<PoseEstimationSrv> {
-public:
-  PoseEstimation(const std::string &name, const NodeConfig &conf,
-                 const RosNodeParams &params)
-      : RosServiceNode<PoseEstimationSrv>(name, conf, params) {
-    RCLCPP_INFO_STREAM(logger(), "Start node.");
-
-    m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
-        node_.lock(), "/pose_estimation");
-
-    while (!m_params_client->wait_for_service(1s)) {
-      if (!rclcpp::ok()) {
-        RCLCPP_ERROR(logger(),
-                     "Interrupted while waiting for the service. Exiting.");
-        break;
-      }
-      RCLCPP_WARN(logger(), "service not available, waiting again...");
-    }
-
-    // Get model name paramter from BT ports
-    getInput("ObjectName", m_model_name);
-  }
-
-  bool setRequest(Request::SharedPtr &request) override {
-    getInput("ReqKind", m_req_type);
-    request->set__transition(transition_event(m_req_type));
-    return true;
-  }
-
-  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
-    if (!response->success) {
-      return NodeStatus::FAILURE;
-    }
-    return NodeStatus::SUCCESS;
-  }
-
-  static PortsList providedPorts() {
-    return providedBasicPorts({
-        InputPort<std::string>("ReqKind"),
-        InputPort<std::string>("ObjectName"),
-        InputPort<std::string>("ObjectPath"),
-    });
-  }
-
-private:
-  uint8_t m_transition_id{};
-  std::string m_model_name{};
-  std::string m_model_type{};
-  std::string m_req_type{};
-  std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
-  std::vector<rcl_interfaces::msg::Parameter> m_params;
-  rcl_interfaces::msg::Parameter m_param;
-
-  lifecycle_msgs::msg::Transition
-  transition_event(const std::string &req_type) {
-    lifecycle_msgs::msg::Transition translation{};
-    // ParamSetter param_setter(m_params_client);
-    if (req_type == "configure") {
-      set_mesh_param();
-      // auto str_mesh_param =
-      // std::make_shared<SetParamShareDirectoryStrategy>("model_name",
-      // m_model_name); param_setter.setStrategy(str_mesh_param);
-      // param_setter.setParam()
-
-      m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
-    } else if (req_type == "calibrate") {
-      set_str_param();
-      m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
-    } else if (req_type == "activate") {
-      m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
-    } else if (req_type == "deactivate") {
-      m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
-    } else if (req_type == "cleanup") {
-      m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
-    }
-    // calibrate
-    translation.set__id(m_transition_id);
-    return translation;
-  }
-
-  inline std::string build_model_path(const std::string &model_name,
-                                      const std::string &package_path) {
-    return package_path + "/config/" + model_name + ".ply";
-  }
-
-  inline std::string build_model_path(const std::string &model_path) {
-    return model_path;
-  }
-
-  void set_str_param() {
-    RCLCPP_INFO_STREAM(logger(),
-                       "Set string parameter: <" + m_model_name + ">");
-
-    std::vector<rclcpp::Parameter> params{
-        rclcpp::Parameter("model_name", m_model_name)};
-    m_params_client->set_parameters(params);
-  }
-
-  void set_mesh_param() {
-    auto t_package_name =
-        ament_index_cpp::get_package_share_directory("rbs_perception");
-    m_model_type = build_model_path(m_model_name, t_package_name);
-    RCLCPP_INFO_STREAM(logger(), m_model_type);
-
-    std::vector<rclcpp::Parameter> params{
-        rclcpp::Parameter("mesh_path", m_model_name)};
-    m_params_client->set_parameters(params);
-  }
-};
-
-CreateRosNodePlugin(PoseEstimation, "PoseEstimation");

rbs_bt_executor/src/rbsBTAction.cpp

@@ -1,64 +0,0 @@
-// #include "behavior_tree/BtService.hpp"
-#include "behaviortree_ros2/bt_service_node.hpp"
-#include "rbs_skill_interfaces/srv/rbs_bt.hpp"
-#include <behaviortree_cpp/tree_node.h>
-#include <behaviortree_ros2/plugins.hpp>
-#include <behaviortree_ros2/ros_node_params.hpp>
-
-#define STR_ACTION  "do"
-#define STR_SID     "sid"
-#define STR_COMMAND "command"
-#define NODE_NAME   "rbs_interface"
-
-using namespace BT;
-using namespace std::chrono_literals;
-using RbsBtActionSrv = rbs_skill_interfaces::srv::RbsBt;
-
-class RbsBtAction : public RosServiceNode<RbsBtActionSrv> {
-public:
-  RbsBtAction(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
-      : RosServiceNode<RbsBtActionSrv>(name, conf, params) {
-
-    m_action_name = getInput<std::string>(STR_ACTION).value();
-    RCLCPP_INFO_STREAM(logger(), "Start node RbsBtAction: " + m_action_name);
-
-    m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
-
-    while (!m_params_client->wait_for_service(1s)) {
-      if (!rclcpp::ok()) {
-        RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
-        break;
-      }
-      RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
-    }
-  }
-
-  bool setRequest(Request::SharedPtr &request) override {
-    request->action = m_action_name; 
-    getInput(STR_SID, request->sid);
-    getInput(STR_COMMAND, request->command);
-    return true;
-  }
-
-  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
-    if (!response->ok) {
-      return NodeStatus::FAILURE;
-    }
-    return NodeStatus::SUCCESS;
-  }
-
-  static PortsList providedPorts() {
-    return providedBasicPorts({
-        InputPort<std::string>(STR_SID),
-        InputPort<std::string>(STR_ACTION),
-        InputPort<std::string>(STR_COMMAND)
-    });
-  }
-
-private:
-  std::string m_action_name{};
-  std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
-};
-
-// !!! теперь устаревшая версия !!!
-CreateRosNodePlugin(RbsBtAction, "RbsBtAction")

rbs_bt_executor/src/rbssAction.cpp

@@ -0,0 +1,84 @@
+// #include "behavior_tree/BtService.hpp"
+// #include "behaviortree_ros2/bt_service_node.hpp"
+#include "behaviortree_ros2/bt_action_node.hpp"
+// #include "rbs_skill_interfaces/srv/rbs_bt.hpp"
+#include "rbs_skill_interfaces/action/rbs_bt.hpp"
+#include <behaviortree_cpp/tree_node.h>
+#include <behaviortree_ros2/plugins.hpp>
+#include <behaviortree_ros2/ros_node_params.hpp>
+
+#define STR_ACTION  "do"
+#define STR_SID     "sid"
+#define STR_COMMAND "command"
+#define NODE_NAME   "rbs_interface"
+
+template<typename T> std::string to_string(const T &t)
+{
+    std::stringstream ss;
+    ss << t;
+    return ss.str();
+}
+
+using namespace BT;
+using namespace std::chrono_literals;
+using RbssActionSrv = rbs_skill_interfaces::action::RbsBt;
+
+class RbssAction : public RosActionNode<RbssActionSrv> {
+public:
+  RbssAction(const std::string &name, const NodeConfig &conf, const RosNodeParams &params)
+      : RosActionNode<RbssActionSrv>(name, conf, params) {
+
+    m_action = getInput<std::string>(STR_ACTION).value();
+    m_command = getInput<std::string>(STR_COMMAND).value();
+    RCLCPP_INFO_STREAM(logger(), "Start node RbssAction: " + m_action + "/" + m_command);
+
+    m_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
+
+    while (!m_client->wait_for_service(1s)) {
+      if (!rclcpp::ok()) {
+        RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
+        break;
+      }
+      RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
+    }
+
+  }
+
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts({
+      BT::InputPort<std::string>(STR_SID),
+      BT::InputPort<std::string>(STR_ACTION),
+      BT::InputPort<std::string>(STR_COMMAND)
+      });
+  }
+
+  bool setGoal(RosActionNode::Goal& goal) override {
+    getInput(STR_ACTION, goal.action);
+    getInput(STR_COMMAND, goal.command);
+    getInput(STR_SID, goal.sid);
+    m_action = goal.action;
+    m_command = goal.command;
+
+    m_i++;
+    RCLCPP_INFO_STREAM(logger(), "setGoal: " + to_string(m_i) + ") " + goal.action + "/" + goal.command + " goal.sid = " + to_string(goal.sid));
+    return true;
+  }
+
+  NodeStatus onResultReceived(const WrappedResult& wr) override {
+    m_i++;
+    RCLCPP_INFO_STREAM(logger(), "onResultReceived: " + to_string(m_i) + ") " + m_action + "/" + m_command + " wr.result->ok - " + to_string(wr.result->ok));
+
+    if (!wr.result->ok) {
+      return NodeStatus::FAILURE;
+    }
+    return NodeStatus::SUCCESS;
+  }
+
+private:
+  int m_i{0};
+  std::string m_action{};
+  std::string m_command{};
+  std::shared_ptr<rclcpp::AsyncParametersClient> m_client;
+};
+
+CreateRosNodePlugin(RbssAction, "RbssAction")

rbs_bt_executor/src/rbssCondition.cpp

@@ -0,0 +1,81 @@
+// #include "behavior_tree/BtService.hpp"
+#include "behaviortree_ros2/bt_service_node.hpp"
+// #include "behaviortree_ros2/bt_action_node.hpp"
+#include "rbs_skill_interfaces/srv/rbs_bt.hpp"
+// #include "rbs_skill_interfaces/action/rbs_bt.hpp"
+#include <behaviortree_cpp/tree_node.h>
+#include <behaviortree_ros2/plugins.hpp>
+#include <behaviortree_ros2/ros_node_params.hpp>
+
+#define STR_ACTION  "do"
+#define STR_SID     "sid"
+#define STR_COMMAND "command"
+#define NODE_NAME   "rbs_interface"
+
+template<typename T> std::string to_string(const T &t)
+{
+    std::stringstream ss;
+    ss << t;
+    return ss.str();
+}
+
+using namespace BT;
+using namespace std::chrono_literals;
+using RbssConditionSrv = rbs_skill_interfaces::srv::RbsBt;
+
+class RbssCondition : public RosServiceNode<RbssConditionSrv> {
+  public:
+    RbssCondition(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
+        : RosServiceNode<RbssConditionSrv>(name, conf, params) {
+  
+      m_action_name = getInput<std::string>(STR_ACTION).value();
+      m_command = getInput<std::string>(STR_COMMAND).value();
+      RCLCPP_INFO_STREAM(logger(), "Start node RbssCondition: " + m_action_name + "/" + m_command);
+  
+      m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
+  
+      while (!m_params_client->wait_for_service(1s)) {
+        if (!rclcpp::ok()) {
+          RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
+          break;
+        }
+        RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
+      }
+    }
+  
+    bool setRequest(Request::SharedPtr &request) override {
+      request->action = m_action_name; 
+      request->command = m_command;
+      getInput(STR_SID, request->sid);
+  
+      m_i++;
+      RCLCPP_INFO_STREAM(logger(), "setRequest: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " request->sid = " + to_string(request->sid));
+      return true;
+    }
+  
+    NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+      m_i++;
+      RCLCPP_INFO_STREAM(logger(), "onResponseReceived: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " response->ok - " + to_string(response->ok));
+  
+      if (!response->ok) {
+        return NodeStatus::FAILURE;
+      }
+      return NodeStatus::SUCCESS;
+    }
+  
+    static PortsList providedPorts() {
+      return providedBasicPorts({
+          InputPort<std::string>(STR_SID),
+          InputPort<std::string>(STR_ACTION),
+          InputPort<std::string>(STR_COMMAND)
+      });
+    }
+  
+  private:
+    int m_i{0};
+    std::string m_action_name{};
+    std::string m_command{};
+    std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
+};
+
+CreateRosNodePlugin(RbssCondition, "RbssCondition")

rbss_objectdetection/scripts/od_yolo_lc.py

@@ -41,9 +41,11 @@ class ObjectDetection(Node):
         """Construct the node."""
         self._count: int = 0
         self._pub: Optional[Publisher] = None
+        self._pubI: Optional[Publisher] = None
         self._timer: Optional[Timer] = None
         self._image_cnt: int = 0
         self._sub = None
+        self._threshold = 0.0
         self._is_image_mode = False
         self.yolov8_weights_file = ""
         self.model = None
@@ -75,14 +77,15 @@ class ObjectDetection(Node):
 
     def _Settings(self):
         # Initialization service settings
-        # TODO
+        for prop in self.skill_cfg["Settings"]["output"]["params"]:
-        # for prop in self.skill_cfg["Settings"]:
+            nam = prop["name"]
-        #     nam = prop["name"]
+            val = prop["value"]
-        #     val = prop["value"]
+            if nam == "threshold":
-        #     if nam == "publishDelay":
+                self._threshold = float(val)
-        #         self.publishDelay = val
+            elif nam == "publishDelay":
-        #     elif nam == "is_image_mode":
+                self.publishDelay = float(val)
-        #         self._is_image_mode = val
+            elif nam == "is_image_mode":
+                self._is_image_mode = (val == "True")
 
         for prop in self.skill_cfg["topicsOut"]:
             if prop["type"] == OUT_TOPIC_TYPE:
@@ -133,10 +136,9 @@ class ObjectDetection(Node):
             return TransitionCallbackReturn.FAILURE
 
         # Create the publisher.
+        self._pub = self.create_lifecycle_publisher(BoundBox, self.topicSrv, 10)
         if self._is_image_mode:
-            self._pub = self.create_lifecycle_publisher(Image, self.topicDetectImage, 3)
+            self._pubI = self.create_lifecycle_publisher(Image, self.topicDetectImage, 1)
-        else:
-            self._pub = self.create_lifecycle_publisher(BoundBox, self.topicSrv, 10)
 
         self._timer = self.create_timer(self.publishDelay, self.publish)
 
@@ -149,7 +151,7 @@ class ObjectDetection(Node):
     def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn:
         self.get_logger().info('on_activate() is called.')
         # Create the main subscriber.
-        self._sub = self.create_subscription(Image, self.topicImage, self.listener_callback, 3)
+        self._sub = self.create_subscription(Image, self.topicImage, self.listener_callback, 1)
         return super().on_activate(state)
 
     def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn:
@@ -176,6 +178,7 @@ class ObjectDetection(Node):
 
         self.destroy_timer(self._timer)
         self.destroy_publisher(self._pub)
+        self.destroy_publisher(self._pubI)
         self.destroy_subscription(self._sub)
 
         self.get_logger().info('on_cleanup() is called.')
@@ -187,6 +190,7 @@ class ObjectDetection(Node):
         """
         self.destroy_timer(self._timer)
         self.destroy_publisher(self._pub)
+        self.destroy_publisher(self._pubI)
         self.destroy_subscription(self._sub)
 
         self.get_logger().info('on_shutdown() is called.')
@@ -198,36 +202,38 @@ class ObjectDetection(Node):
 
         if self._pub is not None and self._pub.is_activated:
             # Publish result
+            if not self.bbox_res:
+                return
+            msg = BoundBox()
+            msg.is_detection = False
+            #from ultralytics.engine.results
+            cconf = self._threshold # threshold
+            bb = None
+            for c in self.bbox_res.boxes:
+                idx = int(c.cls)
+                if self.bbox_res.names[idx] == self.objName:
+                    conf = float(c.conf)
+                    if cconf < conf:
+                        cconf = conf
+                        bb = c
+            if bb:
+                msg.is_detection = True
+                msg.name = self.objName
+                msg.x = float(bb.xywhn[0,0])
+                msg.y = float(bb.xywhn[0,1])
+                msg.w = float(bb.xywhn[0,2])
+                msg.h = float(bb.xywhn[0,3])
+                msg.conf = float(bb.conf)
+             # Publish the message.
+            self._pub.publish(msg)
+
             if self._is_image_mode:
                 if len(self.image_det) == 0:
                     return
                 # The 'cv2_to_imgmsg' method converts an OpenCV image to a ROS 2 image message
                 msg = self.br.cv2_to_imgmsg(self.image_det, encoding="bgr8")
-            else:
+                # Publish the message.
-                if not self.bbox_res:
+                self._pubI.publish(msg)
-                    return
-                msg = BoundBox()
-                msg.is_detection = False
-                #from ultralytics.engine.results
-                cconf = 0.0 # threshold
-                bb = None
-                for c in self.bbox_res.boxes:
-                    idx = int(c.cls)
-                    if self.bbox_res.names[idx] == self.objName:
-                        conf = float(c.conf)
-                        if cconf < conf:
-                            cconf = conf
-                            bb = c
-                if bb:
-                    msg.is_detection = True
-                    msg.name = self.objName
-                    msg.x = float(bb.xywhn[0,0])
-                    msg.y = float(bb.xywhn[0,1])
-                    msg.w = float(bb.xywhn[0,2])
-                    msg.h = float(bb.xywhn[0,3])
-                    msg.conf = float(bb.conf)
-             # Publish the message.
-            self._pub.publish(msg)
 
     def listener_callback(self, data):
         """
@@ -235,7 +241,7 @@ class ObjectDetection(Node):
         """
         if self.model:
             self._image_cnt += 1
-            if self._image_cnt % 100 == 1:
+            if self._image_cnt % 200 == 1:
               self.get_logger().info(f"detection: {self._image_cnt}")
 
             # Convert ROS Image message to OpenCV image
@@ -243,12 +249,12 @@ class ObjectDetection(Node):
             # run inference
             results = self.model(current_frame)
 
+            for r in results:
+                self.bbox_res = r
+
             if self._is_image_mode:
                 for r in results:
                     self.image_det = r.plot()  # plot a BGR numpy array of predictions
-            else:
-                for r in results:
-                    self.bbox_res = r
 
             # self.get_logger().info(f"detection: end {self._image_cnt}")