Merge pull request #304 from kvnptl/robocup2023-perception

[RoboCup2023] perception module

mir_manipulation/mir_pregrasp_planning/ros/src/mir_pregrasp_planning_ros/orientation_independent_ik.py

@@ -58,7 +58,13 @@ def get_reachable_pose_and_joint_msg_from_point(self, x, y, z, frame_id):
         :returns: (geometry_msgs.msg.PoseStamped, brics_actuator.JointPositions) or None
 
         """
-        pitch_ranges = [(0.0, 0.0), (-30.0, 0.0), (-60.0, -30.0), (-90.0, -60.0), (0.0, 10.0)]
+        # check is_picking_from_shelf from parameter server
+        self.is_picking_from_shelf = rospy.get_param("/pick_from_shelf_server/pick_statemachine_says_shelf", False)
+        rospy.loginfo(f"[orientation_independent_ik] shelf picking is set to: {self.is_picking_from_shelf}")
+        if self.is_picking_from_shelf=='True':
+            pitch_ranges = [(-20.0, -15.0)] # limiting the pitch range to avoid collisions with shelf and table
+        else:
+            pitch_ranges = [(0.0, 0.0), (-30.0, 0.0), (-60.0, -30.0), (-90.0, -60.0), (0.0, 10.0)]
         return self._get_reachable_pose_and_joint_msg_from_point_and_pitch_ranges(
                 x, y, z, frame_id, pitch_ranges)
 

mir_perception/mir_barrier_tape_detection/CMakeLists.txt

@@ -18,9 +18,7 @@ find_package(OpenCV REQUIRED)
 find_package(PCL 1.10 REQUIRED)
 
 set (CMAKE_CXX_STANDARD 14)
-add_compile_options(
-    -O3
-)
+set (CMAKE_BUILD_TYPE Release)
 
 generate_dynamic_reconfigure_options(
   ros/config/BarrierTapeDetection.cfg

mir_perception/mir_barrier_tape_detection/ros/launch/barrier_tape_detection.launch

@@ -16,11 +16,11 @@
           <param name="loop_rate" type="int" value="30" />
           <param name="target_frame" value="map"/>
           <remap from="~event_in" to="/mir_perception/barrier_tape_detection/event_in"/>
-          <remap from="~output/yellow_barrier_tape_pointcloud" to="/mir_perception/barrier_tape_detection/output/yellow_barrier_tape_pointcloud"/>
+          <remap from="~output/yellow_barrier_tape_pointcloud" to="/mir_perception/front_camera/barrier_tape_detection/output/yellow_barrier_tape_pointcloud"/>
       </node>
       <!-- Point cloud to laser scan conversion-->
       <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
-          <remap from="cloud_in" to="/mir_perception/barrier_tape_detection/output/yellow_barrier_tape_pointcloud"/>
+          <remap from="cloud_in" to="/mir_perception/front_camera/barrier_tape_detection/output/yellow_barrier_tape_pointcloud"/>
           <remap from="scan" to="/barrier_tape/scan_front" />
           <rosparam>
             target_frame: base_laser_front_link # Leave disabled to output scan in pointcloud frame
@@ -51,15 +51,15 @@
           <remap from="~input_rgb_image" to="/$(arg back_camera)/rgb/image_raw"/>
           <remap from="~input_pointcloud" to="/$(arg back_camera)/depth_registered/points"/>
           <remap from="~camera_info" to="/$(arg back_camera)/rgb/camera_info"/>
-          <param name="loop_rate" type="int" value="1" />
+          <param name="loop_rate" type="int" value="30" />
           <param name="target_frame" value="map"/>
           <remap from="~event_in" to="/mir_perception/barrier_tape_detection/event_in"/>
-          <remap from="~output/yellow_barrier_tape_pointcloud" to="/mir_perception/barrier_tape_detection/output/yellow_barrier_tape_pointcloud"/>
+          <remap from="~output/yellow_barrier_tape_pointcloud" to="/mir_perception/back_camera/barrier_tape_detection/output/yellow_barrier_tape_pointcloud"/>
       </node>
 
       <!-- Point cloud to laser scan conversion-->
       <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
-          <remap from="cloud_in" to="/mir_perception/barrier_tape_detection/output/yellow_barrier_tape_pointcloud"/>
+          <remap from="cloud_in" to="/mir_perception/back_camera/barrier_tape_detection/output/yellow_barrier_tape_pointcloud"/>
           <remap from="scan" to="/barrier_tape/scan_rear" />
           <rosparam>
             target_frame: base_laser_rear_link # Leave disabled to output scan in pointcloud frame

mir_perception/mir_empty_space_detection/ros/config/params.yaml

@@ -9,10 +9,10 @@ passthrough_filter_limit_min: 0.45 # all the min distance starts from the baseli
 passthrough_filter_limit_max: 0.7
 enable_cropbox_filter: True
 cropbox_filter_min_x: 0.45
-cropbox_filter_max_x: 0.8
-cropbox_filter_min_y: -0.4
-cropbox_filter_max_y: 0.4
-cropbox_filter_min_z: 0.0
+cropbox_filter_max_x: 0.55
+cropbox_filter_min_y: -0.25
+cropbox_filter_max_y: 0.25
+cropbox_filter_min_z: -0.1
 cropbox_filter_max_z: 0.6
 normal_radius_search: 0.03
 use_omp: true
@@ -37,10 +37,10 @@ cluster_max_height: 0.09
 cluster_max_length: 0.25
 cluster_min_distance_to_polygon: 0.04
 octree_resolution: 0.01
-object_height_above_workspace: 0.035
+object_height_above_workspace: 0.0 # setting it to 0 because we reduced the gripper height
 # parameters for finding empty spaces on a plane
 empty_space_point_count_percentage_threshold: 0.8
-empty_space_radius: 0.045
-num_of_empty_spaces_required: 4
+empty_space_radius: 0.065
+num_of_empty_spaces_required: 2
 trial_duration: 3.0
-enable_debug_pc: False
+enable_debug_pc: false

mir_perception/mir_object_recognition/ros/config/SceneSegmentation.cfg

@@ -58,7 +58,9 @@ pc_os_cluster.add ("pad_cluster", bool_t,  0, "Pad cluster so that it has the sa
 pc_os_cluster.add ("padded_cluster_size", int_t, 0, "The size of the padded cluster", 2048, 128, 4096)
 
 object_pose = gen.add_group("Object pose")
-object_pose.add ("object_height_above_workspace", double_t, 0, "The height of the object above the workspace", 0.038, 0, 2.0)
+object_pose.add ("use_fixed_heights", bool_t,  0, "Used fixed heights of objects by assuming platforms are 0, 5, 10 or 15 cm exactly", False)
+object_pose.add ("height_of_floor", double_t, 0, "The height of floor (or 0cm platform) based on scene segmentation", -0.08, -1.0, 2.0)
+object_pose.add ("object_height_above_workspace", double_t, 0, "The height of the object above the workspace", 0.03, -1.0, 2.0)
 object_pose.add ("container_height", double_t, 0, "The height of the container pose", 0.0335, 0, 2.0)
 
 rgb_bbox_proposal = gen.add_group("RGB bbox proposal")
@@ -78,6 +80,7 @@ roi.add ("roi_min_bbox_z", double_t, 0, "Min height of objects", 0.03, 0, 1)
 object_recognizer = gen.add_group("Object recognizer")
 object_recognizer.add ("enable_rgb_recognizer", bool_t,  0, "Enable rgb object detection and recognition", True)
 object_recognizer.add ("enable_pc_recognizer", bool_t,  0, "Enable pointcloud object detection and recognition", True)
+object_recognizer.add ("obj_category", str_t, 0, "Object category name (eg. atwork, cavity, container)", "atwork")
 
 exit (gen.generate (PACKAGE, "mir_object_recognition", "SceneSegmentation"))
 

mir_perception/mir_object_recognition/ros/config/objects.xml

@@ -15,9 +15,13 @@
   <object><name>CONTAINER_BOX_RED</name><shape>box</shape><color>red</color></object>
   <object><name>BEARING_BOX</name><shape>box</shape><color>gray</color></object>
   <object><name>BEARING</name><shape>sphere</shape><color>gray</color></object>
+  <object><name>BEARING2</name><shape>sphere</shape><color>gray</color></object>
   <object><name>AXIS</name><shape>cylinder</shape><color>gray</color></object>
+  <object><name>ALLENKEY</name><shape>flat</shape><color>gray</color></object>
+  <object><name>WRENCH</name><shape>flat</shape><color>gray</color></object>
+  <object><name>DRILL</name><shape>flat</shape><color>gray</color></object>
 
   <object><name>BLUE_CONTAINER</name><shape>box</shape><color>blue</color></object>
   <object><name>RED_CONTAINER</name><shape>box</shape><color>red</color></object>
 
-</object_info>
+</object_info>

mir_perception/mir_object_recognition/ros/config/scene_segmentation_constraints.yaml

@@ -13,7 +13,7 @@ multimodal_object_recognition:
   cropbox_filter_max_x: 0.8
   cropbox_filter_min_y: -0.4
   cropbox_filter_max_y: 0.4
-  cropbox_filter_min_z: 0.0
+  cropbox_filter_min_z: -0.1
   cropbox_filter_max_z: 0.6
   normal_radius_search: 0.03
   use_omp: False
@@ -41,14 +41,17 @@ multimodal_object_recognition:
   pad_cluster: True
   padded_cluster_size: 2048
   octree_resolution: 0.0025
-  object_height_above_workspace: 0.012
+  height_of_floor: -0.083
+  use_fixed_heights: False
+  object_height_above_workspace: -0.008
   container_height: 0.07
   enable_rgb_recognizer: true
   enable_pc_recognizer: false
+  obj_category: 'atwork' # atwork, cavity
   rgb_roi_adjustment: 2
   rgb_bbox_min_diag: 21
-  rgb_bbox_max_diag: 175
-  rgb_cluster_filter_limit_min: 0.02 # for tower camera configuration (youbot 2)
+  rgb_bbox_max_diag: 200
+  rgb_cluster_filter_limit_min: 0.005 # for tower camera configuration (youbot 2)
   rgb_cluster_filter_limit_max: 0.35
   rgb_cluster_remove_outliers: True
   enable_roi: True

mir_perception/mir_object_recognition/ros/include/mir_object_recognition/multimodal_object_recognition_node.h

@@ -111,7 +111,6 @@ class MultimodalObjectRecognitionROS
     ros::Publisher pub_rgb_object_pose_array_;
     // Publisher debug
     ros::Publisher pub_debug_cloud_plane_;
-    ros::Publisher pub_filtered_rgb_cloud_plane_;
     std::string horizontal_object_list[9];
 
     // Synchronize callback for image and pointcloud
@@ -155,11 +154,13 @@ class MultimodalObjectRecognitionROS
     // Enable recognizer
     bool enable_rgb_recognizer_;
     bool enable_pc_recognizer_ ;
+    std::string obj_category_;
 
     // Visualization
     BoundingBoxVisualizer bounding_box_visualizer_pc_;
     ClusteredPointCloudVisualizer cluster_visualizer_rgb_;
     ClusteredPointCloudVisualizer cluster_visualizer_pc_;
+    ClusteredPointCloudVisualizer cluster_visualizer_filtered_rgb_;
     LabelVisualizer label_visualizer_rgb_;
     LabelVisualizer label_visualizer_pc_;
 
@@ -168,11 +169,14 @@ class MultimodalObjectRecognitionROS
     std::string target_frame_id_;
     std::string pointcloud_source_frame_id_;
     std::set<std::string> round_objects_;
+    std::set<std::string> flat_objects_;
     ObjectInfo object_info_;
     std::string object_info_path_;
 
     // Dynamic parameter
     double object_height_above_workspace_;
+    double height_of_floor_;
+    bool use_fixed_heights_;
     double container_height_;
     int rgb_roi_adjustment_;
     int rgb_bbox_min_diag_;
@@ -238,7 +242,8 @@ class MultimodalObjectRecognitionROS
      **/
     void publishDebug(mas_perception_msgs::ObjectList &combined_object_list,
               std::vector<PointCloud::Ptr> &clusters_3d,
-              std::vector<PointCloud::Ptr> &clusters_2d);
+              std::vector<PointCloud::Ptr> &clusters_2d,
+              std::vector<PointCloud::Ptr> &filtered_clusters_2d);
 
     /** \brief Load qualitative object info
      * \param[in] Path to the xml object file

mir_perception/mir_object_recognition/ros/launch/multimodal_object_recognition.launch

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
 
-  <arg name="camera_name" default="tower_cam3d" />
+  <arg name="camera_name" default="tower_cam3d_front" />
   <arg name="input_pointcloud_topic"  default="/$(arg camera_name)/depth_registered/points" />
   <arg name="input_image_topic" default="/$(arg camera_name)/rgb/image_raw" />
   <arg name="target_frame" default="base_link" />
@@ -24,7 +24,7 @@
       <param name="pointcloud_source_frame_id" value="$(arg pointcloud_source_frame_id)" type="str" />
       <param name="debug_mode" value="$(arg debug_mode)" type="bool" />
       <param name="dataset_collection" value="true" />
-      <param name="logdir" value="/tmp/" />
+      <param name="logdir" value="/home/robocup/perception_debugs/" />
       <param name="object_info" value="$(arg object_info)" />
     </node>
   </group>

mir_perception/mir_object_recognition/ros/launch/rgb_object_recognition.launch

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="net" default="detection" />
-  <arg name="classifier" default="yolov7" />
-  <arg name="dataset" default="ss22_local_competition" />
+  <arg name="classifier" default="yolov8" />
+  <arg name="dataset" default="robocup_2023_dataset" />
   <arg name="model_dir" default="$(find mir_rgb_object_recognition_models)/common/models/$(arg classifier)/$(arg dataset)" />
 
   <group ns="mir_perception">
@@ -14,6 +14,10 @@
       <param name="model_dir" value="$(arg model_dir)" type="str" />
       <remap from="~input/images" to="/mir_perception/multimodal_object_recognition/recognizer/rgb/input/images" />
       <remap from="~output/object_list" to="/mir_perception/multimodal_object_recognition/recognizer/rgb/output/object_list"/>
+      <rosparam param="model_category">
+        - atwork
+        - cavity
+      </rosparam>
     </node>
   </group>
 </launch>