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

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

@@ -23,7 +23,8 @@
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
float cam_x = 1.0;
float cam_y = 1.2;
@@ -38,13 +39,13 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
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) );
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<pcl::PCLPointCloud2> 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<pcl::PointXYZ> point_cloud;
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);
// 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;
@@ -87,18 +90,20 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_segmented(new pcl::PointCloud<pcl::PointXYZRGB>);
int j= 0;
int j = 0;
// 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) {
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) {
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;
@@ -155,7 +160,8 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
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) {
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;
@@ -165,11 +171,13 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
}
// identify surfaces of object cluster
if (obj_idx != -1) {
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) {
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;
@@ -187,7 +195,8 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
float ymax = 0;
float zmax = 0;
for (int i = 0; i < obj_cloud.points.size(); ++i) {
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;
@@ -200,7 +209,7 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
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 top area: " << area_top << std::endl;
std::cout << "object side area: " << area_side << std::endl;
geometry_msgs::Pose pose;
@@ -208,7 +217,8 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
geometry_msgs::Quaternion q;
// pick up from top
if (area_top > .002) {
if (area_top > .002)
{
std::cout << "picking up object from top" << std::endl;
p.x = (xmin + xmax) / 2;
p.y = (ymin + ymax) / 2;
@@ -220,7 +230,8 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
q.z = 0;
q.w = 1;
}
else {
else
{
// pick up from front
std::cout << "picking up object from front" << std::endl;
p.x = xmin;
@@ -256,7 +267,8 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
//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.
ros::init(argc, argv, "depth_camera_node");
@@ -265,11 +277,10 @@ int main(int argc, char ** argv) {
ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
pub = node.advertise<geometry_msgs::Pose> ("output", 1);
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
}