diff --git a/air_moveit_config/src/add_sphere.cpp b/air_moveit_config/src/add_sphere.cpp index e08e529..9e4dc32 100644 --- a/air_moveit_config/src/add_sphere.cpp +++ b/air_moveit_config/src/add_sphere.cpp @@ -13,7 +13,7 @@ void sphereCallback(const std_msgs::Float32MultiArray::ConstPtr xyz) { moveit::planning_interface::PlanningSceneInterface planning_scene_interface; // Get the sphere dimensions from the message - float radius = 0.04; // Radius of sphere + float radius = 0.0685; // Radius of sphere(ball) float x = xyz->data[0]; float y = xyz->data[1]; float z = xyz->data[2]; diff --git a/air_moveit_config/src/detect_and_add_ball.cpp b/air_moveit_config/src/detect_and_add_ball.cpp new file mode 100644 index 0000000..d0ba7b9 --- /dev/null +++ b/air_moveit_config/src/detect_and_add_ball.cpp @@ -0,0 +1,92 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include "../include/detect_and_add.hpp" + +air_object::air_object(ros::NodeHandle& nh) + : m_NodeHandle(nh) +{ + m_center_x = 0; + m_center_y = 0; + m_center_y_buffer = 0; + m_center_x_buffer = 0; + m_yolo_sub = m_NodeHandle.subscribe("/yolov7/yolov7", 1000, &air_object::yolo_callback, this); + m_pcl_sub = m_NodeHandle.subscribe("/camera/depth/color/points",1000, &air_object::pcl_callback, this); // Subscribe to the cylinder dimensions topic + m_coordinate_pub = nh.advertise("/coordinates_of_object", 1000); + ROS_INFO("INITIALIZING COMPLETED!!!!!!!!"); + } + +void air_object::yolo_callback(const vision_msgs::Detection2DArrayConstPtr& d2d_arr_msg) +{ + if(d2d_arr_msg->detections.empty()) + { + ROS_WARN("NO OBJECT DETECTED!!!"); + return; + } + vision_msgs::Detection2DArray d2dArr = *d2d_arr_msg; + for(auto detection : d2dArr.detections){ + if(detection.results[0].id == 32){ //id of sports_ball in coco + ROS_INFO("Orange Detected..."); + m_center_x = (int) detection.bbox.center.x; + m_center_y = (int) detection.bbox.center.y; + m_center_x_buffer = m_center_x; + m_center_y_buffer = m_center_y; + } + } + + ROS_INFO("The center of detected object is (%d, %d) !!", m_center_x, m_center_y); +} + + +void air_object::pcl_callback(const sensor_msgs::PointCloud2ConstPtr& msg) +{ + // Convert the PointCloud2 message to PCL data type + if(m_center_x == 0 && m_center_y == 0){ + ROS_ERROR("PCL ERROR"); + return; + } + pcl::PointCloud cloud; + pcl::fromROSMsg(*msg, cloud); + + if(cloud.empty()) + { + return; + } + // Get the XYZ data for a specific pixel + int r = m_center_y; // specify the row of the pixel you are interested in + int c = m_center_x; // specify the column of the pixel you are interested in + + pcl::PointXYZ p = cloud.at(c, r); + m_x = p.x; + m_y = p.y; + m_z = p.z; + + if(m_x == 0 && m_y == 0 && m_z == 0){ + ROS_ERROR("OBJECT DETECTED BUT CANNOT ACCESS DEPTH!!!!!!!!"); + return; + } + + ROS_INFO("The XYZ data for pixel at (%d, %d) is: (%f, %f, %f)", c, r, m_x, m_y, m_z); + + m_xyz.data.resize(3); + m_xyz.data[0] = m_x; + m_xyz.data[1] = m_y; + m_xyz.data[2] = m_z; + m_coordinate_pub.publish(m_xyz); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "detect_and_add_cylinder_collision_object_demo"); + ros::NodeHandle nh; + air_object obj(nh); + + ros::spin(); + + return 0; +}