sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-09-01 03:43:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added ros::spin to depth camera node

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-25 23:50:36 -04:00
parent 2a85cdee8d
commit 4098662c5d
1 changed files with 181 additions and 170 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -23,28 +23,29 @@
ros::Publisher pub; ros::Publisher pub;
void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg) { void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
// define camera pose {
// define camera pose
float cam_x = 1.0; float cam_x = 1.0;
float cam_y = 1.2; float cam_y = 1.2;
float cam_z = 1.4; float cam_z = 1.4;
// rpy = 0 1.21 0 // rpy = 0 1.21 0
// convert from PointCloud to PointCloud2 // convert from PointCloud to PointCloud2
sensor_msgs::PointCloud2 cloud2; sensor_msgs::PointCloud2 cloud2;
sensor_msgs::convertPointCloudToPointCloud2(*cloud_msg, cloud2); sensor_msgs::convertPointCloudToPointCloud2(*cloud_msg, cloud2);
// transform to world frame // transform to world frame
sensor_msgs::PointCloud2 worldCloud; sensor_msgs::PointCloud2 worldCloud;
tf::Transform xform; tf::Transform xform;
xform.setOrigin( tf::Vector3(cam_x, cam_y, cam_z) ); xform.setOrigin(tf::Vector3(cam_x, cam_y, cam_z));
xform.setRotation( tf::Quaternion(0, 0.5697, 0, 0.82185) ); xform.setRotation(tf::Quaternion(0, 0.5697, 0, 0.82185));
pcl_ros::transformPointCloud("world", xform, cloud2, worldCloud); pcl_ros::transformPointCloud("world", xform, cloud2, worldCloud);
// Container for original & filtered data // Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered; pcl::PCLPointCloud2 cloud_filtered;
@@ -53,19 +54,21 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
// Perform the actual filtering // Perform the actual filtering
pcl::VoxelGrid<pcl::PCLPointCloud2> sor; pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr); sor.setInputCloud(cloudPtr);
sor.setLeafSize (0.001, 0.001, 0.001); sor.setLeafSize(0.001, 0.001, 0.001);
sor.filter (cloud_filtered); sor.filter(cloud_filtered);
pcl::PointCloud<pcl::PointXYZ> point_cloud; pcl::PointCloud<pcl::PointXYZ> point_cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloudPtr(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloudPtr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2( cloud_filtered, point_cloud); pcl::fromPCLPointCloud2(cloud_filtered, point_cloud);
pcl::copyPointCloud(point_cloud, *point_cloudPtr); pcl::copyPointCloud(point_cloud, *point_cloudPtr);
// Filter out points on surface of belt // 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; //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].x = 0;
point_cloudPtr->points[i].y = 0; point_cloudPtr->points[i].y = 0;
point_cloudPtr->points[i].z = 0; point_cloudPtr->points[i].z = 0;
@@ -79,172 +82,180 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
std::vector<pcl::PointIndices> cluster_indices; std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.05); // 5cm ec.setClusterTolerance(0.05); // 5cm
ec.setMinClusterSize(10); //10 ec.setMinClusterSize(10); //10
ec.setMaxClusterSize(99000000); ec.setMaxClusterSize(99000000);
ec.setSearchMethod(tree); ec.setSearchMethod(tree);
ec.setInputCloud(point_cloudPtr); ec.setInputCloud(point_cloudPtr);
ec.extract(cluster_indices); ec.extract(cluster_indices);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_segmented(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_segmented(new pcl::PointCloud<pcl::PointXYZRGB>);
int j= 0; int j = 0;
// store index of object cluster // store index of object cluster
int obj_idx = -1; 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 for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
s++; {
x += point.x; // average all points
y += point.y; int s = 0;
z += point.z; 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;
if (j == 0) //Red #FF0000 (255,0,0) // add to running totals
{ s++;
point.r = 0; x += point.x;
point.g = 0; y += point.y;
point.b = 255; z += point.z;
}
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;
// print cluster center if (j == 0) //Red #FF0000 (255,0,0)
std::cout << "cluster at: " << x << ", " << y << ", " << z << std::endl; {
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 // print cluster center
if (x < 1.25 && x > 1.15 && y < cam_y + .05 && y > cam_y - .05) { std::cout << "cluster at: " << x << ", " << y << ", " << z << std::endl;
// object detected
std::cout << "object in position" << std::endl;
obj_idx = j;
}
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 j++;
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; // 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);
}
// create 3D bounding box. std::cout << "object cluster size: " << obj_cloud.points.size() << std::endl;
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) { // create 3D bounding box.
xmin = xmin < obj_cloud.points[i].x ? xmin : obj_cloud.points[i].x; float xmin = 10.0;
ymin = ymin < obj_cloud.points[i].y ? ymin : obj_cloud.points[i].y; float ymin = 10.0;
//zmin = zmin < obj_cloud.points[i].z ? zmin : obj_cloud.points[i].z; float zmin = 0.92;
xmax = xmax > obj_cloud.points[i].x ? xmax : obj_cloud.points[i].x; float xmax = 0;
ymax = ymax > obj_cloud.points[i].y ? ymax : obj_cloud.points[i].y; float ymax = 0;
zmax = zmax > obj_cloud.points[i].z ? zmax : obj_cloud.points[i].z; float zmax = 0;
}
// determine target pose for (int i = 0; i < obj_cloud.points.size(); ++i)
float area_top = (ymax - ymin) * (xmax - xmin); {
float area_side = (zmax - zmin) * (ymax - ymin); 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; // determine target pose
std::cout << "object side area: " << area_side << std::endl; float area_top = (ymax - ymin) * (xmax - xmin);
float area_side = (zmax - zmin) * (ymax - ymin);
geometry_msgs::Pose pose; std::cout << "object top area: " << area_top << std::endl;
geometry_msgs::Point p; std::cout << "object side area: " << area_side << std::endl;
geometry_msgs::Quaternion q;
// pick up from top geometry_msgs::Pose pose;
if (area_top > .002) { geometry_msgs::Point p;
std::cout << "picking up object from top" << std::endl; geometry_msgs::Quaternion q;
p.x = (xmin + xmax) / 2;
p.y = (ymin + ymax) / 2;
p.z = zmax;
// rpy = 0 0 0 // pick up from top
q.x = 0; if (area_top > .002)
q.y = 0; {
q.z = 0; std::cout << "picking up object from top" << std::endl;
q.w = 1; p.x = (xmin + xmax) / 2;
} p.y = (ymin + ymax) / 2;
else { p.z = zmax;
// 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 // rpy = 0 0 0
q.x = -0.63; q.x = 0;
q.y = 0; q.y = 0;
q.z = 0.63; q.z = 0;
q.w = 0.44; 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; // rpy = -pi/2 0 pi/2
pose.orientation = q; q.x = -0.63;
q.y = 0;
q.z = 0.63;
q.w = 0.44;
}
// publish pose pose.position = p;
pub.publish(pose); 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 // Convert to ROS data type
//point_cloud_segmented->header.frame_id = point_cloudPtr->header.frame_id; //point_cloud_segmented->header.frame_id = point_cloudPtr->header.frame_id;
//if(point_cloud_segmented->size()) pcl::toPCLPointCloud2(*point_cloud_segmented, cloud_filtered); //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); //pub.publish (output);
} }
int main(int argc, char ** argv) { int main(int argc, char **argv)
// TODO: Subscribe to depth camera topic {
// Last argument is the default name of the node. // TODO: Subscribe to depth camera topic
ros::init(argc, argv, "depth_camera_node"); // Last argument is the default name of the node.
ros::init(argc, argv, "depth_camera_node");
ros::NodeHandle node; ros::NodeHandle node;
ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
pub = node.advertise<geometry_msgs::Pose> ("output", 1); ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
// 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 pub = node.advertise<geometry_msgs::Pose>("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
} }