Merge remote-tracking branch 'origin/main' into 90-real-robot-setup

rbs_bt_executor/bt_trees/test_tree.xml

@@ -3,7 +3,7 @@
     <BehaviorTree ID="PoseEstimation">
         <Sequence>
             <Action ID="PoseEstimation" 
-            ObjectName="!/rbs_ws/src/robossembler-ros2/rbs_perception/config/str_param.json"
+            ObjectName="!/home/shalenikol/robossembler_ws/src/robossembler-ros2/rbs_perception/config/str_param.json"
             ReqKind = "calibrate"
             server_name="/pose_estimation/change_state" 
             server_timeout="1000"/>
@@ -14,4 +14,3 @@
             server_timeout="1000"/>
         </Sequence>
     </BehaviorTree>
-</root>

rbs_bt_executor/src/PoseEstimation.cpp

@@ -101,6 +101,15 @@ private:
     _set_params_client->set_parameters(_parameters);
   }
 
+  void set_mesh_param() {
+    auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception");
+    _model_path = build_model_path(_model_name, _package_name);
+    RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
+
+    std::vector<rclcpp::Parameter> _parameters{rclcpp::Parameter("mesh_path", _model_name)};
+    _set_params_client->set_parameters(_parameters);
+  }
+
   void set_mesh_param() {
     auto _package_name =
         ament_index_cpp::get_package_share_directory("rbs_perception");

rbs_perception/scripts/pose_estimation_lifecycle.py

@@ -197,14 +197,6 @@ class PoseEstimator(Node):
         """
         gtpose = None
         str_param = self.get_parameter("mesh_path").get_parameter_value().string_value
-        # if str_param[0] == "{": # json string
-        #     y = json.loads(str_param)
-        #     if "mesh_path" not in y:
-        #         self.get_logger().info("JSon 'mesh_path' not set")
-        #         return TransitionCallbackReturn.FAILURE
-        #     mesh_path = y["mesh_path"]
-        #     if "gtpose" in y:
-        #       gtpose = y["gtpose"]
         if str_param[0] == "!": # filename json
             json_file = str_param[1:]
             if not os.path.isfile(json_file):
@@ -342,9 +334,14 @@ class PoseEstimator(Node):
             [k_[3], k_[4]*2.0, data.height / 2.0], # k_[5]], #
             [k_[6], k_[7], k_[8]] #self.mesh_scale]
         ]
+        # self._K = [
+        #     [k_[0]*2.0, k_[1], data.width / 2.0], # k_[2]], #
+        #     [k_[3], k_[4]*2.0, data.height / 2.0], # k_[5]], #
+        #     [k_[6], k_[7], k_[8]] #self.mesh_scale]
+        # ]
 
+        # *** for test
         tPath = self.objPath / "inputs"
-        #{"label": "fork", "bbox_modal": [329, 189, 430, 270]}
         output_fn = tPath / "object_data.json"
         output_json_dict = {
             "label": self.objName,
@@ -353,6 +350,7 @@ class PoseEstimator(Node):
         data = []
         data.append(output_json_dict)
         output_fn.write_text(json.dumps(data))
+        # ***
 
         #{"K": [[25.0, 0.0, 8.65], [0.0, 25.0, 6.5], [0.0, 0.0, 1.0]], "resolution": [480, 640]}
         output_fn = self.objPath / "camera_data.json"
@@ -371,8 +369,6 @@ class PoseEstimator(Node):
         """
         Depth image callback function.
         """
-        #self.get_logger().info("Receiving depth image")
-    
         # Convert ROS Image message to OpenCV image
         current_frame = self.br.imgmsg_to_cv2(data)
         
@@ -414,7 +410,6 @@ class PoseEstimator(Node):
             bbox = [2, 2, self._res[1]-4, self._res[0]-4]
 
         #tPath = path_to / "inputs"
-        #{"label": "fork", "bbox_modal": [329, 189, 430, 270]}
         output_fn = path_to / "inputs/object_data.json"
         output_json_dict = {
             "label": self.objName,
@@ -462,8 +457,6 @@ class PoseEstimator(Node):
             self.get_logger().info(f"darknet: begin {self._image_cnt}")
             result_json = str(self.objPath / "res.json")
 
-            #with open("log_darknet.txt", "w") as logf:
-            #self.get_logger().is_enabled_for(LoggingSeverity.WARN)
             subprocess.run([darknet_bin, "detector", "test",
                             "yolov4_objs2.data",
                             "yolov4_objs2.cfg",
@@ -475,11 +468,9 @@ class PoseEstimator(Node):
             detected = self.yolo2megapose(result_json, self.objPath)
             self.get_logger().info(f"darknet: end {self._image_cnt}")
 
-        #if detected and self.megapose_model:
+        if detected and self.megapose_model:
-        if self.megapose_model:
             # 6D pose estimation
             self.get_logger().info(f"megapose: begin {self._image_cnt} {self.objPath}")
-            #run_inference(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
             from megapose.scripts.run_inference_on_example import run_inference_rbs
             run_inference_rbs(self.megapose_model)
 
@@ -501,4 +492,3 @@ def main():
         lc_node.destroy_node()
 
 if __name__ == '__main__':
-    main()

rbs_simulation/launch/simulation.launch.py

@@ -80,6 +80,7 @@ def generate_launch_description():
             '/outer_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
         ],
         output='screen',
+        condition=IfCondition(sim_gazebo)
     )
     
     rgbd_bridge_in = Node(