diff --git a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp index 57259d9..8b5b470 100644 --- a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp @@ -23,28 +23,29 @@ ros::Publisher pub; -void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg) { - // define camera pose +void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg) +{ + // define camera pose float cam_x = 1.0; float cam_y = 1.2; float cam_z = 1.4; // rpy = 0 1.21 0 - // convert from PointCloud to PointCloud2 + // convert from PointCloud to PointCloud2 sensor_msgs::PointCloud2 cloud2; sensor_msgs::convertPointCloudToPointCloud2(*cloud_msg, cloud2); - // transform to world frame - sensor_msgs::PointCloud2 worldCloud; + // transform to world frame + sensor_msgs::PointCloud2 worldCloud; - tf::Transform xform; - xform.setOrigin( tf::Vector3(cam_x, cam_y, cam_z) ); - xform.setRotation( tf::Quaternion(0, 0.5697, 0, 0.82185) ); - - pcl_ros::transformPointCloud("world", xform, cloud2, worldCloud); + tf::Transform xform; + xform.setOrigin(tf::Vector3(cam_x, cam_y, cam_z)); + xform.setRotation(tf::Quaternion(0, 0.5697, 0, 0.82185)); + + pcl_ros::transformPointCloud("world", xform, cloud2, worldCloud); // Container for original & filtered data - pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; + pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2; pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); pcl::PCLPointCloud2 cloud_filtered; @@ -53,19 +54,21 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg) // Perform the actual filtering pcl::VoxelGrid sor; - sor.setInputCloud (cloudPtr); - sor.setLeafSize (0.001, 0.001, 0.001); - sor.filter (cloud_filtered); + sor.setInputCloud(cloudPtr); + sor.setLeafSize(0.001, 0.001, 0.001); + sor.filter(cloud_filtered); pcl::PointCloud point_cloud; pcl::PointCloud::Ptr point_cloudPtr(new pcl::PointCloud); - pcl::fromPCLPointCloud2( cloud_filtered, point_cloud); + pcl::fromPCLPointCloud2(cloud_filtered, point_cloud); pcl::copyPointCloud(point_cloud, *point_cloudPtr); // Filter out points on surface of belt - for (std::size_t i = 0; i < point_cloudPtr->points.size(); i++) { + for (std::size_t i = 0; i < point_cloudPtr->points.size(); i++) + { //std::cout << point_cloudPtr->points[i].x << ", " << point_cloudPtr->points[i].y << ", " << point_cloudPtr->points[i].z << std::endl; - if (point_cloudPtr->points[i].z < 0.93) { + if (point_cloudPtr->points[i].z < 0.93) + { point_cloudPtr->points[i].x = 0; point_cloudPtr->points[i].y = 0; point_cloudPtr->points[i].z = 0; @@ -79,172 +82,180 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg) std::vector cluster_indices; pcl::EuclideanClusterExtraction ec; ec.setClusterTolerance(0.05); // 5cm - ec.setMinClusterSize(10); //10 + ec.setMinClusterSize(10); //10 ec.setMaxClusterSize(99000000); ec.setSearchMethod(tree); ec.setInputCloud(point_cloudPtr); ec.extract(cluster_indices); - pcl::PointCloud::Ptr point_cloud_segmented(new pcl::PointCloud); + pcl::PointCloud::Ptr point_cloud_segmented(new pcl::PointCloud); - int j= 0; + int j = 0; - // store index of object cluster - int obj_idx = -1; - - for (std::vector::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { - // average all points - int s = 0; - float x = 0; - float y = 0; - float z = 0; - for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) { - // convert - pcl::PointXYZRGB point; - point.x = point_cloudPtr->points[*pit].x; - point.y = point_cloudPtr->points[*pit].y; - point.z = point_cloudPtr->points[*pit].z; + // store index of object cluster + int obj_idx = -1; - // add to running totals - s++; - x += point.x; - y += point.y; - z += point.z; + for (std::vector::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) + { + // average all points + int s = 0; + float x = 0; + float y = 0; + float z = 0; + for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) + { + // convert + pcl::PointXYZRGB point; + point.x = point_cloudPtr->points[*pit].x; + point.y = point_cloudPtr->points[*pit].y; + point.z = point_cloudPtr->points[*pit].z; - if (j == 0) //Red #FF0000 (255,0,0) - { - point.r = 0; - point.g = 0; - point.b = 255; - } - else if (j == 1) //Lime #00FF00 (0,255,0) - { - point.r = 0; - point.g = 255; - point.b = 0; - } - else if (j == 2) // Blue #0000FF (0,0,255) - { - point.r = 255; - point.g = 0; - point.b = 0; - } - else - { - if (j % 2 == 0) - { - point.r = 255 * j / (cluster_indices.size()); - point.g = 128; - point.b = 50; - } - else - { - point.r = 0; - point.g = 255 * j / (cluster_indices.size()); - point.b = 128; - } - } - point_cloud_segmented->push_back(point); - } - // calculate center of cluster - x /= s; - y /= s; - z /= s; + // add to running totals + s++; + x += point.x; + y += point.y; + z += point.z; - // print cluster center - std::cout << "cluster at: " << x << ", " << y << ", " << z << std::endl; + if (j == 0) //Red #FF0000 (255,0,0) + { + point.r = 0; + point.g = 0; + point.b = 255; + } + else if (j == 1) //Lime #00FF00 (0,255,0) + { + point.r = 0; + point.g = 255; + point.b = 0; + } + else if (j == 2) // Blue #0000FF (0,0,255) + { + point.r = 255; + point.g = 0; + point.b = 0; + } + else + { + if (j % 2 == 0) + { + point.r = 255 * j / (cluster_indices.size()); + point.g = 128; + point.b = 50; + } + else + { + point.r = 0; + point.g = 255 * j / (cluster_indices.size()); + point.b = 128; + } + } + point_cloud_segmented->push_back(point); + } + // calculate center of cluster + x /= s; + y /= s; + z /= s; - // check if center is within box - if (x < 1.25 && x > 1.15 && y < cam_y + .05 && y > cam_y - .05) { - // object detected - std::cout << "object in position" << std::endl; - obj_idx = j; - } + // print cluster center + std::cout << "cluster at: " << x << ", " << y << ", " << z << std::endl; - j++; - } + // check if center is within box + if (x < 1.25 && x > 1.15 && y < cam_y + .05 && y > cam_y - .05) + { + // object detected + std::cout << "object in position" << std::endl; + obj_idx = j; + } - // identify surfaces of object cluster - if (obj_idx != -1) { - // create new cloud containing only object cluster - pcl::PointCloud obj_cloud; - std::vector::const_iterator it = cluster_indices.begin() + obj_idx; - for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) { - pcl::PointXYZ point; - point.x = point_cloudPtr->points[*pit].x; - point.y = point_cloudPtr->points[*pit].y; - point.z = point_cloudPtr->points[*pit].z; - obj_cloud.push_back(point); - } + j++; + } - std::cout << "object cluster size: " << obj_cloud.points.size() << std::endl; + // identify surfaces of object cluster + if (obj_idx != -1) + { + // create new cloud containing only object cluster + pcl::PointCloud obj_cloud; + std::vector::const_iterator it = cluster_indices.begin() + obj_idx; + for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) + { + pcl::PointXYZ point; + point.x = point_cloudPtr->points[*pit].x; + point.y = point_cloudPtr->points[*pit].y; + point.z = point_cloudPtr->points[*pit].z; + obj_cloud.push_back(point); + } - // create 3D bounding box. - float xmin = 10.0; - float ymin = 10.0; - float zmin = 0.92; - float xmax = 0; - float ymax = 0; - float zmax = 0; + std::cout << "object cluster size: " << obj_cloud.points.size() << std::endl; - for (int i = 0; i < obj_cloud.points.size(); ++i) { - xmin = xmin < obj_cloud.points[i].x ? xmin : obj_cloud.points[i].x; - ymin = ymin < obj_cloud.points[i].y ? ymin : obj_cloud.points[i].y; - //zmin = zmin < obj_cloud.points[i].z ? zmin : obj_cloud.points[i].z; - xmax = xmax > obj_cloud.points[i].x ? xmax : obj_cloud.points[i].x; - ymax = ymax > obj_cloud.points[i].y ? ymax : obj_cloud.points[i].y; - zmax = zmax > obj_cloud.points[i].z ? zmax : obj_cloud.points[i].z; - } + // create 3D bounding box. + float xmin = 10.0; + float ymin = 10.0; + float zmin = 0.92; + float xmax = 0; + float ymax = 0; + float zmax = 0; - // determine target pose - float area_top = (ymax - ymin) * (xmax - xmin); - float area_side = (zmax - zmin) * (ymax - ymin); + for (int i = 0; i < obj_cloud.points.size(); ++i) + { + xmin = xmin < obj_cloud.points[i].x ? xmin : obj_cloud.points[i].x; + ymin = ymin < obj_cloud.points[i].y ? ymin : obj_cloud.points[i].y; + //zmin = zmin < obj_cloud.points[i].z ? zmin : obj_cloud.points[i].z; + xmax = xmax > obj_cloud.points[i].x ? xmax : obj_cloud.points[i].x; + ymax = ymax > obj_cloud.points[i].y ? ymax : obj_cloud.points[i].y; + zmax = zmax > obj_cloud.points[i].z ? zmax : obj_cloud.points[i].z; + } - std::cout << "object top area: " << area_top << std:: endl; - std::cout << "object side area: " << area_side << std::endl; + // determine target pose + float area_top = (ymax - ymin) * (xmax - xmin); + float area_side = (zmax - zmin) * (ymax - ymin); - geometry_msgs::Pose pose; - geometry_msgs::Point p; - geometry_msgs::Quaternion q; + std::cout << "object top area: " << area_top << std::endl; + std::cout << "object side area: " << area_side << std::endl; - // pick up from top - if (area_top > .002) { - std::cout << "picking up object from top" << std::endl; - p.x = (xmin + xmax) / 2; - p.y = (ymin + ymax) / 2; - p.z = zmax; + geometry_msgs::Pose pose; + geometry_msgs::Point p; + geometry_msgs::Quaternion q; - // rpy = 0 0 0 - q.x = 0; - q.y = 0; - q.z = 0; - q.w = 1; - } - else { - // pick up from front - std::cout << "picking up object from front" << std::endl; - p.x = xmin; - p.y = (ymin + ymax) / 2; - p.z = (zmax + zmin) / 2; + // pick up from top + if (area_top > .002) + { + std::cout << "picking up object from top" << std::endl; + p.x = (xmin + xmax) / 2; + p.y = (ymin + ymax) / 2; + p.z = zmax; - // rpy = -pi/2 0 pi/2 - q.x = -0.63; - q.y = 0; - q.z = 0.63; - q.w = 0.44; - } + // rpy = 0 0 0 + q.x = 0; + q.y = 0; + q.z = 0; + q.w = 1; + } + else + { + // pick up from front + std::cout << "picking up object from front" << std::endl; + p.x = xmin; + p.y = (ymin + ymax) / 2; + p.z = (zmax + zmin) / 2; - pose.position = p; - pose.orientation = q; + // rpy = -pi/2 0 pi/2 + q.x = -0.63; + q.y = 0; + q.z = 0.63; + q.w = 0.44; + } - // publish pose - pub.publish(pose); + pose.position = p; + pose.orientation = q; - // compute normals - } + // publish pose + pub.publish(pose); + + // compute normals + } + + //std::cerr<< "segmented: " << (int)point_cloud_segmented->size() << "\n"; - //std::cerr<< "segmented: " << (int)point_cloud_segmented->size() << "\n"; - // Convert to ROS data type //point_cloud_segmented->header.frame_id = point_cloudPtr->header.frame_id; //if(point_cloud_segmented->size()) pcl::toPCLPointCloud2(*point_cloud_segmented, cloud_filtered); @@ -256,20 +267,20 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg) //pub.publish (output); } -int main(int argc, char ** argv) { - // TODO: Subscribe to depth camera topic - // Last argument is the default name of the node. - ros::init(argc, argv, "depth_camera_node"); +int main(int argc, char **argv) +{ + // TODO: Subscribe to depth camera topic + // Last argument is the default name of the node. + ros::init(argc, argv, "depth_camera_node"); - ros::NodeHandle node; - - ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback); + ros::NodeHandle node; - pub = node.advertise ("output", 1); - - // TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item - + ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback); - // TODO: Publish object's current location to topic for RRRobot node to listen to - + pub = node.advertise("output", 1); + + ros::spin(); + // TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item + + // TODO: Publish object's current location to topic for RRRobot node to listen to } \ No newline at end of file