sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-28 02:12:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

depth camera node

Browse Source
This commit is contained in:
Euxitheos
2020-04-24 15:45:05 -04:00
parent 24ae633ad2
commit 6ae4e18100
1 changed files with 203 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

177
src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp
View File

@@ -5,38 +5,55 @@
#include <string>
#include <ros/ros.h>
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "message_filters/subscriber.h"
#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <std_srvs/Trigger.h>
ros::Publisher pub;
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;
// convert from PointCloud to PointCloud2
sensor_msgs::PointCloud2 cloud2;
sensor_msgs::convertPointCloudToPointCloud2(*cloud_msg, cloud2);
// 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);
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
// Convert to PCL data type
pcl_conversions::toPCL(cloud2, *cloud);
pcl_conversions::toPCL(worldCloud, *cloud);
// Perform the actual filtering
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize (0.02, 0.02, 0.02);
sor.setLeafSize (0.001, 0.001, 0.001);
sor.filter (cloud_filtered);
pcl::PointCloud<pcl::PointXYZ> point_cloud;
@@ -44,14 +61,24 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
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++) {
//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) {
point_cloudPtr->points[i].x = 0;
point_cloudPtr->points[i].y = 0;
point_cloudPtr->points[i].z = 0;
}
}
// Create the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(point_cloudPtr);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.1); // 2cm
ec.setMinClusterSize(10); //100
ec.setClusterTolerance(0.05); // 5cm
ec.setMinClusterSize(10); //10
ec.setMaxClusterSize(99000000);
ec.setSearchMethod(tree);
ec.setInputCloud(point_cloudPtr);
@@ -61,15 +88,28 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
int j= 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
{
// store index of object cluster
int obj_idx = -1;
for (std::vector<pcl::PointIndices>::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<int>::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;
// add to running totals
s++;
x += point.x;
y += point.y;
z += point.z;
if (j == 0) //Red #FF0000 (255,0,0)
{
point.r = 0;
@@ -104,20 +144,115 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
}
}
point_cloud_segmented->push_back(point);
}
// calculate center of cluster
x /= s;
y /= s;
z /= s;
// print cluster center
std::cout << "cluster at: " << x << ", " << y << ", " << z << std::endl;
// 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;
}
j++;
}
// identify surfaces of object cluster
if (obj_idx != -1) {
// create new cloud containing only object cluster
pcl::PointCloud<pcl::PointXYZ> obj_cloud;
std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin() + obj_idx;
for (std::vector<int>::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);
}
std::cout << "object cluster size: " << obj_cloud.points.size() << std::endl;
// 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;
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;
}
// determine target pose
float area_top = (ymax - ymin) * (xmax - xmin);
float area_side = (zmax - zmin) * (ymax - ymin);
std::cout << "object top area: " << area_top << std:: endl;
std::cout << "object side area: " << area_side << std::endl;
geometry_msgs::Pose pose;
geometry_msgs::Point p;
geometry_msgs::Quaternion q;
// 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 = 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;
// rpy = -pi/2 0 pi/2
q.x = -0.63;
q.y = 0;
q.z = 0.63;
q.w = 0.44;
}
pose.position = p;
pose.orientation = q;
// publish pose
pub.publish(pose);
// compute normals
}
//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);
else pcl::toPCLPointCloud2(*point_cloudPtr, cloud_filtered);
sensor_msgs::PointCloud2 output;
pcl_conversions::fromPCL(cloud_filtered, output);
//point_cloud_segmented->header.frame_id = point_cloudPtr->header.frame_id;
//if(point_cloud_segmented->size()) pcl::toPCLPointCloud2(*point_cloud_segmented, cloud_filtered);
//else pcl::toPCLPointCloud2(*point_cloudPtr, cloud_filtered);
//sensor_msgs::PointCloud2 output;
//pcl_conversions::fromPCL(cloud_filtered, output);
// Publish the data
pub.publish (output);
//pub.publish (output);
}
int main(int argc, char ** argv) {
@@ -129,7 +264,7 @@ int main(int argc, char ** argv) {
ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
pub = nh.advertise<geometry_msgs::Pose> ("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