Multiple objects added to planning scene

air_moveit_config/src/add_sphere.cpp

@@ -15,57 +15,84 @@ air_object::air_object(ros::NodeHandle& nh)
   m_coordinate_sub = m_NodeHandle.subscribe("/coordinates_of_object", 1000, &air_object::sphereCallback, this);
 }
 
-void air_object::radius_estimator_callback(const std_msgs::Float32ConstPtr& radius)
+void air_object::radius_estimator_callback(const std_msgs::Float32MultiArrayConstPtr& radius)
 {
-  m_radius = radius->data*0.01;
+  m_radius = radius->data;
 }
 
 
 
 // Callback function for the subscriber
-void air_object::sphereCallback(const std_msgs::Float32MultiArray::ConstPtr& xyz)
+void air_object::sphereCallback(const vision_msgs::Detection3DArrayConstPtr& detections)
 { 
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-  // Get the sphere dimensions from the message
-  float x = xyz->data[0];
-  float y = xyz->data[1];
-  float z = xyz->data[2];
-  ROS_INFO("Object added to planning scene: X: %f  Y: %f  Z: %f",x,y,z);
-  // Create the sphere shape
-  shape_msgs::SolidPrimitive primitive;
-  primitive.type = primitive.SPHERE;
-  primitive.dimensions.resize(1);
-  primitive.dimensions[0] = m_radius;
-
-  // Specify the pose of the sphere
-  geometry_msgs::Pose pose;
-  // To fix data transformations -> x = z, y = -x, z = -y
-  float cam_degree = 30; // Angle between robot and camera
-  pose.position.x = z + (((m_radius/2) / sin(((90-cam_degree) * M_PI)/180))*2);
-  pose.position.y = -x; 
-  pose.position.z = -y + (m_radius/4); //hotfix for collision
-
-  // Orientation fix (to do: cam_degree)
-  pose.orientation.w = 0.9659258;
-  pose.orientation.y = -0.258819;
-
-  moveit_msgs::CollisionObject collision_object;
-  collision_object.header.frame_id = "camera_link";
-  collision_object.id = "sphere";
-
-  collision_object.primitives.push_back(primitive);
-  collision_object.primitive_poses.push_back(pose);
-  collision_object.operation = collision_object.ADD;
-
-  std::vector<moveit_msgs::CollisionObject> collision_objects;  
-  collision_objects.push_back(collision_object);  
-
-  // Add the sphere to the planning scene
-  planning_scene_interface.applyCollisionObjects(collision_objects);
 
-  // Unsubscribe after receiving the first message
+  std::vector<moveit_msgs::CollisionObject> collision_objects;
+  // int id_counter = 1;
+  int detection_counter = 1;
+  for (const auto& detection : detections->detections)
+  { 
+    for (const auto& result : detection.results)
+    {
+      float x = result.pose.pose.position.x;
+      float y = result.pose.pose.position.y;
+      float z = result.pose.pose.position.z;
+      std::string id = std::to_string(result.id);
+
+      ROS_INFO("Object added to planning scene: ID: %s  X: %f  Y: %f  Z: %f", id.c_str(), x, y, z);
+
+      shape_msgs::SolidPrimitive primitive;
+      primitive.type = primitive.SPHERE;
+      primitive.dimensions.resize(1);
+      float radius;
+      //if(m_radius.size() >= detection_counter -1)
+      radius = m_radius[detection_counter-1] * 0.01;
+      primitive.dimensions[0] = radius;
+      ROS_INFO("Radius: %f", radius);
+
+      geometry_msgs::Pose pose;
+      float cam_degree = 30; // Angle between robot and camera
+      pose.position.x = z + (((radius/2) / sin(((90-cam_degree) * M_PI)/180))*2);
+      pose.position.y = -x; 
+      pose.position.z = -y + (radius/4); //hotfix for collision
+
+      // Orientation fix (to do: cam_degree)
+      pose.orientation.w = 0.9659258;
+      pose.orientation.y = -0.258819;
+
+      moveit_msgs::CollisionObject collision_object;
+      collision_object.header.frame_id = "camera_link";
+
+      // if (detection_counter > 1)
+      // {
+      // id += "_" + std::to_string(detection_counter);
+      // }
+
+      detection_counter++;
+
+      collision_object.id = id;
+
+      collision_object.primitives.push_back(primitive);
+      collision_object.primitive_poses.push_back(pose);
+      collision_object.operation = collision_object.ADD;
+
+      collision_objects.push_back(collision_object);
+    }
+    // id_counter++;
+  }
+  if (!collision_objects.empty()){
+    planning_scene_interface.applyCollisionObjects(collision_objects);
+    ROS_INFO("COLLISION PUBLISHED");
+  }
+  else{
+    ROS_INFO("COLLISION EMPTY");
+    return;
+  }
 }
 
+
+
+
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "detect_and_add_sphere_collision_object_demo");

air_moveit_config/src/detect_and_add_objects.cpp

@@ -43,7 +43,7 @@ void air_object::yolo_callback(const vision_msgs::Detection2DArrayConstPtr& d2d_
         const auto resVal = *res;
         
       } */
-      if(detection.results[0].id == 32 || detection.results[0].id == 40){ //id of sports_ball in coco
+      if(detection.results[0].id == 47 || detection.results[0].id == 49 || detection.results[0].id == 80 ){ //id of sports_ball in coco
         ROS_INFO("Object Detected...");
         int center_x = (int) detection.bbox.center.x;
         int center_y = (int) detection.bbox.center.y;

radius_estimation/scripts/estimate.py

@@ -5,7 +5,7 @@
 from cv_bridge import CvBridge
 from pyrealsense2 import pyrealsense2 as rs
 from vision_msgs.msg import Detection2DArray
-from std_msgs.msg import Float32
+from std_msgs.msg import Float32MultiArray
 import cv2
 import numpy as np
 import os
@@ -22,20 +22,26 @@ def __init__(self):
         rospy.init_node('regression_node')
         rospy.Subscriber('/yolov7/yolov7', Detection2DArray, self.image_callback)
         rospy.Subscriber('/camera/aligned_depth_to_color/image_raw', Image, self.depth_callback)
-        self.pub = rospy.Publisher("/radius_estimator", Float32, queue_size=10)
+        self.pub = rospy.Publisher("/radius_estimator", Float32MultiArray, queue_size=10)
 
 
-    def image_callback(self,detection_array):
+    def image_callback(self, detection_array):
         # Bbox to numpy
+        predictions=[]
         for detection in detection_array.detections:
-            if detection.results[0].id == 32: # nesne ID'si 32 ise
-                bbox = detection.bbox
-                depth = (self.depth_image[int(bbox.center.y), int(bbox.center.x)]) * self.depth_scale
-                features = np.array([depth, bbox.size_x, bbox.size_y])
-                features = features.reshape(1, -1) # add an extra dimension
-                predict=self.model.predict(features)
-                self.pub.publish(predict)   #check
-                print("Radius of ball predicted as %d cm" % predict)
+            for result in detection.results:
+                if result.id == 47 or result.id == 49 or result.id == 80:  # Nesne ID'si 32 ise
+                    bbox = detection.bbox
+                    depth = (self.depth_image[int(bbox.center.y), int(bbox.center.x)]) * self.depth_scale
+                    features = np.array([depth, bbox.size_x, bbox.size_y])
+                    features = features.reshape(1, -1)  # Ekstra bir boyut ekleniyor
+                    predict = self.model.predict(features)
+                    predictions.append(predict)
+                    print("Radius of ball predicted as %f cm" % predict)
+        msg = Float32MultiArray()
+        msg.data = np.array(predictions).flatten().tolist()
+        self.pub.publish(msg)  # Kontrol et
+
 
 
     def depth_callback(self, depth_image):

yolov7-ros/launch/yolov7.launch

@@ -3,7 +3,7 @@
     ns="yolov7">
         <!-- Download the official weights from the original repo -->
         <param name="weights_path" type="str"
-        value="$(find yolov7_ros)/src/ball-tiny.pt"/>
+        value="$(find yolov7_ros)/src/best_may.pt"/>
         <!-- Path to a class_labels.txt file containing your desired class labels. The i-th entry corresponds to the i-th class id. For example, in coco class label 0 corresponds to 'person'. Files for the coco and berkeley deep drive datasets are provided in the 'class_labels/' directory. If you leave it empty then no class labels are visualized.-->
         <param name="classes_path" type="str" value="$(find yolov7_ros)/class_labels/coco.txt" />
         <!-- topic name to subscribe to -->