Merge pull request #323 from sthoduka/fix/perception-2023

Fix/perception 2023

mir_perception/mir_empty_space_detection/ros/config/params.yaml

@@ -37,7 +37,7 @@ 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.0 # setting it to 0 because we reduced the gripper height
+object_height_above_workspace: 0.0
 # parameters for finding empty spaces on a plane
 empty_space_point_count_percentage_threshold: 0.8
 empty_space_radius: 0.065

mir_perception/mir_object_recognition/ros/config/objects.xml

@@ -24,4 +24,9 @@
   <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><name>M30_H</name><shape>sphere</shape><color>black</color></object>
+  <object><name>M20_H</name><shape>sphere</shape><color>black</color></object>
+  <object><name>S40_40_V</name><shape>sphere</shape><color>black</color></object>
+  <object><name>F20_20_V</name><shape>sphere</shape><color>black</color></object>
+
 </object_info>

mir_perception/mir_object_recognition/ros/launch/multimodal_object_recognition.launch

@@ -11,7 +11,6 @@
   <arg name="scene_segmentation_config_file" default="$(find mir_object_recognition)/ros/config/scene_segmentation_constraints.yaml" />
   <arg name="object_info" default="$(find mir_object_recognition)/ros/config/objects.xml" />
 
-  <include file="$(find mir_object_recognition)/ros/launch/pc_object_recognition.launch" />
   <include file="$(find mir_object_recognition)/ros/launch/rgb_object_recognition.launch" />
 
   <group ns="mir_perception">
@@ -24,7 +23,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="/home/robocup/perception_debugs/" />
+      <param name="logdir" value="/tmp" />
       <param name="object_info" value="$(arg object_info)" />
     </node>
   </group>

mir_perception/mir_object_recognition/ros/scripts/pc_object_recognizer_node

@@ -158,9 +158,8 @@ if __name__ == '__main__':
     model_dir = rospy.get_param("~model_dir")
     dataset = rospy.get_param("~dataset")
 
-    # commented as we'are not using it
-    #object_recognizer = PointcloudObjectRecognizer(
-    #    model, model_id, dataset, model_dir)
-    #rospy.loginfo('\033[92m'+"PCL Recognizer is ready using %s , model: %s ",
-    #              model, model_id)
+    object_recognizer = PointcloudObjectRecognizer(
+        model, model_id, dataset, model_dir)
+    rospy.loginfo('\033[92m'+"PCL Recognizer is ready using %s , model: %s ",
+                  model, model_id)
     rospy.spin()

mir_perception/mir_object_recognition/ros/scripts/rgb_object_recognizer_node

@@ -34,15 +34,14 @@ class RGBObjectRecognizer():
             "input/images", ImageList, self.image_recognition_cb)
         self.net = net
         self.model_name = model_name
-        self.weights = weights
         self.confidence_threshold = confidence_threshold
         self.iou_threshold = iou_threshold
         self.device = "cuda" if torch.cuda.is_available() else device
 
         # Load model
         if self.model_name == 'yolov8':
-            self.model_atwork = YOLO(self.weights[0])
-            self.model_cavity = YOLO(self.weights[1])
+            self.model_atwork = YOLO(weights['atwork'])
+            self.model_cavity = YOLO(weights['cavity'])
 
     def image_recognition_cb(self, img_msg):
         if img_msg.images:
@@ -136,19 +135,16 @@ if __name__ == '__main__':
     classifier_name = rospy.get_param("~classifier")
     dataset = rospy.get_param("~dataset")
     model_dir = rospy.get_param("~model_dir")
-    model_category = rospy.get_param("~model_category")
-    weight_1 = os.path.join(roslib.packages.get_pkg_dir("mir_rgb_object_recognition_models"),
-                           'common', 'models', classifier_name, dataset, model_category[0]+".pt")
-    weight_2 = os.path.join(roslib.packages.get_pkg_dir("mir_rgb_object_recognition_models"),
-                            'common', 'models', classifier_name, dataset, model_category[1]+".pt")
-    # check if the model exists
-    if not os.path.isfile(weight_1):
-        rospy.logerr("[RGB Recognition] Model not found: %s", weight_1)
-        exit(-1)
-    if not os.path.isfile(weight_2):
-        rospy.logerr("[RGB Recognition] Model not found: %s", weight_2)
-        exit(-1)
-    weights = [weight_1, weight_2] # 0th index is the default model, eg. atwork
+    model_categories = rospy.get_param("~model_category")
+    weights = {}
+    for model_category in model_categories:
+        weight = os.path.join(roslib.packages.get_pkg_dir("mir_rgb_object_recognition_models"),
+                           'common', 'models', classifier_name, dataset, model_category +".pt")
+        if not os.path.isfile(weight):
+            rospy.logerr("[RGB Recognition] Model not found: %s", weight)
+            exit(-1)
+        weights[model_category] = weight
+
     object_recognizer = RGBObjectRecognizer(
         weights, 
         debug_mode=True)

mir_perception/mir_object_recognition/ros/src/multimodal_object_recognition_node.cpp

@@ -96,7 +96,7 @@ MultimodalObjectRecognitionROS::MultimodalObjectRecognitionROS(ros::NodeHandle n
   ROS_WARN_STREAM("[multimodal_object_recognition] target frame: " <<target_frame_id_);
   nh_.param<std::string>("pointcloud_source_frame_id", pointcloud_source_frame_id_, "fixed_camera_link");
 
-  nh_.param<std::string>("logdir", logdir_, "/home/robocup/perception_debug_data/");
+  nh_.param<std::string>("logdir", logdir_, "/tmp");
   nh_.param<std::string>("object_info", object_info_path_, "None");
   loadObjectInfo(object_info_path_);
 
@@ -109,7 +109,7 @@ MultimodalObjectRecognitionROS::~MultimodalObjectRecognitionROS()
 void MultimodalObjectRecognitionROS::synchronizeCallback(const sensor_msgs::ImageConstPtr &image,
                       const sensor_msgs::PointCloud2ConstPtr &cloud)
 {
-  ROS_INFO("[multimodal_object_recognition_ros] Received enough messages");
+  ROS_INFO("[multimodal_object_recognition_ros] Received synchronized pointcloud and image");
   if (pointcloud_msg_received_count_ < 1)
   {
     pointcloud_msg_ = cloud;
@@ -743,16 +743,9 @@ void MultimodalObjectRecognitionROS::adjustObjectPose(mas_perception_msgs::Objec
     double change_in_pitch = 0.0;
     if (round_objects_.count(object_list.objects[i].name))
     {
+      ROS_INFO_STREAM("Setting yaw to zero for " << object_list.objects[i].name);
       yaw = 0.0;
     }
-    if (object_list.objects[i].name == "M30_H" 
-        or object_list.objects[i].name == "M20_H"
-        or object_list.objects[i].name == "S40_40_V"
-        or object_list.objects[i].name == "F20_20_V")
-    {
-        ROS_WARN("Setting yaw for M30/M20 to zero");
-        yaw = 0.0;
-    }
 
     // Update container pose
     if (object_list.objects[i].name == "CONTAINER_BOX_RED" ||
@@ -904,8 +897,6 @@ void MultimodalObjectRecognitionROS::loadObjectInfo(const std::string &filename)
 void MultimodalObjectRecognitionROS::eventCallback(const std_msgs::String::ConstPtr &msg)
 {
   std_msgs::String event_out;
-  ROS_INFO("Event call back");
-
   if (msg->data == "e_start")
   {
     // Synchronize callback

mir_perception/mir_perception_utils/ros/src/object_utils_ros.cpp

@@ -62,7 +62,7 @@ PointCloud object::estimatePose(const PointCloud::Ptr &xyz_input_cloud, geometry
   // circular/spherical object
   // to find its orientation
   PointCloud filtered_cloud;
-  if (shape == "sphere" or shape == "flat") {
+  if (obj_category == "atwork" and (shape == "sphere" or shape == "flat")) {
     filtered_cloud = *xyz_input_cloud;
   } else {
     pcl::PassThrough<PointT> pass_through;
@@ -74,9 +74,6 @@ PointCloud object::estimatePose(const PointCloud::Ptr &xyz_input_cloud, geometry
     double limit_max = max_pt.z + passthrough_lim_max_offset;
     if (obj_category == "cavity")
     {
-      // print
-      // std::cout << "[CAVITYYYYYYYYYYYY] min_pt.z: " << min_pt.z << std::endl;
-      // std::cout << "[CAVITYYYYYYYYYYYY] max_pt.z: " << max_pt.z << std::endl;
       limit_max = max_pt.z - 0.015; // TODO: make 0.01 as a dynamic configurable parameter
       if (object.name == "M20_H")
       {

mir_planning/mir_actions/mir_perceive_location/ros/scripts/perceive_location_server.py

@@ -44,7 +44,7 @@ def get_robot_pose(self):
         return None
 
     def execute(self, userdata):
-        print("[percieve] userdata goal: ", str(userdata.goal.parameters))
+        rospy.loginfo("[perceive] userdata goal: %s" % str(userdata.goal.parameters))
         target_location = Utils.get_value_of(userdata.goal.parameters, 'location')
         if target_location is not None:
             target_pose = Utils.get_pose_from_param_server(target_location)
@@ -211,15 +211,12 @@ def __init__(self):
 
     def execute(self, userdata):
         obj_category = Utils.get_value_of(userdata.goal.parameters, "obj_category")
-
-        rospy.loginfo("=============[PERCEIVE_LOCATION] obj_category: %s", obj_category)
+        rospy.loginfo("[perceive] obj_category: %s", obj_category)
 
         if obj_category:
-            rospy.loginfo("=============[PERCEIVE_LOCATION] [IFFFF] obj_category: %s", obj_category)
             self.set_named_config.execute(userdata, obj_category)
             return "success"
         else:
-            rospy.loginfo("=============[PERCEIVE_LOCATION] [ELSEEEE] obj_category: %s", obj_category)
             self.set_named_config.execute(userdata)
             return "success"
 

mir_scenarios/mir_states/ros/src/mir_states/common/action_states.py

@@ -48,7 +48,7 @@ def execute(self, userdata):
 
 
 class perceive_location(smach.State):
-    def __init__(self, obj_category="multimodal_object_recognition_atwork"):
+    def __init__(self, obj_category="atwork"):
         smach.State.__init__(self, outcomes=["success", "failed"])
         self.client = SimpleActionClient(
             "perceive_location_server", GenericExecuteAction