diff --git a/air_moveit_config/src/add_sphere.cpp b/air_moveit_config/src/add_sphere.cpp index 84796d3..a4b8883 100644 --- a/air_moveit_config/src/add_sphere.cpp +++ b/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 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 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"); diff --git a/air_moveit_config/src/detect_and_add_objects.cpp b/air_moveit_config/src/detect_and_add_objects.cpp index c3eddb5..31590ce 100644 --- a/air_moveit_config/src/detect_and_add_objects.cpp +++ b/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; diff --git a/radius_estimation/scripts/estimate.py b/radius_estimation/scripts/estimate.py index 36ce39a..3f63071 100755 --- a/radius_estimation/scripts/estimate.py +++ b/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): diff --git a/yolov7-ros/launch/yolov7.launch b/yolov7-ros/launch/yolov7.launch index 855e3b8..fe03751 100644 --- a/yolov7-ros/launch/yolov7.launch +++ b/yolov7-ros/launch/yolov7.launch @@ -3,7 +3,7 @@ ns="yolov7"> + value="$(find yolov7_ros)/src/best_may.pt"/>