diff --git a/air_moveit_config/include/detect_and_add.hpp b/air_moveit_config/include/detect_and_add.hpp index de03d79..f7056d8 100644 --- a/air_moveit_config/include/detect_and_add.hpp +++ b/air_moveit_config/include/detect_and_add.hpp @@ -3,8 +3,9 @@ #include #include +#include #include - +#include class air_object { public: @@ -13,13 +14,17 @@ class air_object void yolo_callback(const vision_msgs::Detection2DArrayConstPtr& d2d_arr_msg); void pcl_callback(const sensor_msgs::PointCloud2ConstPtr& msg); + void radius_estimator_callback(const std_msgs::Float32ConstPtr& radius); + void sphereCallback(const std_msgs::Float32MultiArray::ConstPtr& xyz); std_msgs::Float32MultiArray m_xyz; ros::NodeHandle m_NodeHandle; - ros::Publisher m_coordinate_pub; + ros::Publisher m_coordinate_pub; + ros::Subscriber m_coordinate_sub; ros::Subscriber m_yolo_sub; ros::Subscriber m_pcl_sub; ros::Subscriber m_sub; + ros::Subscriber m_estimator_sub; float m_x; float m_y; @@ -29,6 +34,7 @@ class air_object int m_center_y; int m_center_x_buffer; int m_center_y_buffer; + float m_radius; private: }; diff --git a/air_moveit_config/launch/air_bringup.launch b/air_moveit_config/launch/air_bringup.launch index 6403025..8ddebf7 100644 --- a/air_moveit_config/launch/air_bringup.launch +++ b/air_moveit_config/launch/air_bringup.launch @@ -19,8 +19,8 @@ - + - + diff --git a/air_moveit_config/src/add_sphere.cpp b/air_moveit_config/src/add_sphere.cpp index 9e4dc32..84796d3 100644 --- a/air_moveit_config/src/add_sphere.cpp +++ b/air_moveit_config/src/add_sphere.cpp @@ -1,19 +1,32 @@ -#include #include #include #include #include #include +#include "../include/detect_and_add.hpp" + + + +air_object::air_object(ros::NodeHandle& nh) + : m_NodeHandle(nh) +{ + ros::Subscriber sub; + m_estimator_sub = m_NodeHandle.subscribe("/radius_estimator", 1000, &air_object::radius_estimator_callback, this); + 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) +{ + m_radius = radius->data*0.01; +} -ros::Subscriber sub; // Callback function for the subscriber -void sphereCallback(const std_msgs::Float32MultiArray::ConstPtr xyz) +void air_object::sphereCallback(const std_msgs::Float32MultiArray::ConstPtr& xyz) { moveit::planning_interface::PlanningSceneInterface planning_scene_interface; // Get the sphere dimensions from the message - float radius = 0.0685; // Radius of sphere(ball) float x = xyz->data[0]; float y = xyz->data[1]; float z = xyz->data[2]; @@ -22,15 +35,15 @@ void sphereCallback(const std_msgs::Float32MultiArray::ConstPtr xyz) shape_msgs::SolidPrimitive primitive; primitive.type = primitive.SPHERE; primitive.dimensions.resize(1); - primitive.dimensions[0] = radius; + 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 + (((radius/2) / sin(((90-cam_degree) * M_PI)/180))*2); + pose.position.x = z + (((m_radius/2) / sin(((90-cam_degree) * M_PI)/180))*2); pose.position.y = -x; - pose.position.z = -y + radius; //hotfix for collision + pose.position.z = -y + (m_radius/4); //hotfix for collision // Orientation fix (to do: cam_degree) pose.orientation.w = 0.9659258; @@ -56,9 +69,9 @@ void sphereCallback(const std_msgs::Float32MultiArray::ConstPtr xyz) int main(int argc, char** argv) { ros::init(argc, argv, "detect_and_add_sphere_collision_object_demo"); - ros::NodeHandle node_handle; - // Subscribe to the sphere dimensions topic - sub = node_handle.subscribe("/coordinates_of_object", 10, sphereCallback); + ros::NodeHandle nh; + air_object object(nh); + ros::spin(); return 0; } \ No newline at end of file diff --git a/radius_estimation/scripts/estimate.py b/radius_estimation/scripts/estimate.py new file mode 100755 index 0000000..36ce39a --- /dev/null +++ b/radius_estimation/scripts/estimate.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 + +import rospy +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +from pyrealsense2 import pyrealsense2 as rs +from vision_msgs.msg import Detection2DArray +from std_msgs.msg import Float32 +import cv2 +import numpy as np +import os +# Modeli yükleme +import joblib + + + +class Estimator: + def __init__(self): + self.model = joblib.load("/home/cenk/regression_model2.joblib") #import pretrained model + self.bridge = CvBridge() + self.depth_scale = 0.001 + 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) + + + def image_callback(self,detection_array): + # Bbox to numpy + 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) + + + def depth_callback(self, depth_image): + self.depth_image = self.bridge.imgmsg_to_cv2(depth_image, desired_encoding="passthrough") + + + +if __name__ == '__main__': + estimator=Estimator() + rospy.spin() diff --git a/realsense-ros/realsense2_camera/launch/rs_camera.launch b/realsense-ros/realsense2_camera/launch/rs_camera.launch index d235e12..b04fb46 100644 --- a/realsense-ros/realsense2_camera/launch/rs_camera.launch +++ b/realsense-ros/realsense2_camera/launch/rs_camera.launch @@ -3,7 +3,7 @@ - + @@ -19,8 +19,8 @@ - - + + diff --git a/yolov7-ros/launch/yolov7.launch b/yolov7-ros/launch/yolov7.launch index dc0fe7a..855e3b8 100644 --- a/yolov7-ros/launch/yolov7.launch +++ b/yolov7-ros/launch/yolov7.launch @@ -3,7 +3,7 @@ ns="yolov7"> + value="$(find yolov7_ros)/src/ball-tiny.pt"/>