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

39
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;
@@ -63,9 +64,11 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
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;
@@ -92,13 +95,15 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg)
// 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;
@@ -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");
@@ -267,9 +279,8 @@ int main(int argc, char ** argv) {
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
}