Skip to content

Commit

Permalink
Cleaned up code, removed unused functions, added extra arguement to s…
Browse files Browse the repository at this point in the history 
…elect whether to run Max's version or Donal's version of ed_empty_sport_designator
  • Loading branch information
@Dyan367
Dyan367 committed Dec 26, 2023
1 parent 8b0e8fd commit 3a1e3d7
Show file tree
Hide file tree
Showing 5 changed files with 34 additions and 43 deletions.
6 changes: 3 additions & 3 deletions ed_sensor_integration/include/ed/kinect/place_area_finder.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,11 @@ class PlaceAreaFinder
* @param image image in which to find a place position
* @param sensor_pose pose of the sensor with respect to a horizontal plane 'base_link' recommended
* @param[out] place_pose expressed in the same frame as sensor pose. One of the possible poses where an object may be placed, currently returns the pose furthest on the table
* @param mask input mask of table obtained from yolov8 segmentation of the rgb image
* @param donal bool used to run either max or donals version of the solution to speed up comparisons
* @return whether or not a suitable place was found
*/
bool findArea(const rgbd::ImageConstPtr& image, geo::Pose3D sensor_pose, geo::Pose3D& place_pose,const cv::Mat &mask);
bool findArea(const rgbd::ImageConstPtr& image, geo::Pose3D sensor_pose, geo::Pose3D& place_pose,const cv::Mat &mask,bool donal);

/**
* @brief Get an image of the analysed space, used for introspection
Expand Down Expand Up @@ -96,9 +98,7 @@ class PlaceAreaFinder

//--------------------------------------------------------------------------------------------------------------------------------------
cv::Point2d canvasToWorld2(double u, double v);
void drawContour(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, cv::Scalar color);
void drawContourAndTransformToWorld(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, cv::Scalar color,float height);
rgbd::ImageConstPtr createModifiedImage(const rgbd::ImageConstPtr& originalImagePtr, const cv::Mat& newRGBValues);
void extractMaskPoints(pcl::PointCloud<pcl::PointXYZRGB>::Ptr inputCloud);
//--------------------------------------------------------------------------------------------------------------------------------------

Expand Down
Binary file added ed_sensor_integration/scripts/yolov8n-seg.pt
View file
Binary file not shown.
3 changes: 2 additions & 1 deletion ed_sensor_integration/src/kinect/kinect_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -290,6 +290,7 @@ bool KinectPlugin::srvPlaceArea(__attribute__((unused)) ed_sensor_integration_ms
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Get new image
cv::Mat mask;
bool donal = true;
rgbd::ImageConstPtr image;
geo::Pose3D sensor_pose; // in base link frame

Expand All @@ -301,7 +302,7 @@ bool KinectPlugin::srvPlaceArea(__attribute__((unused)) ed_sensor_integration_ms

// Determine place area
geo::Pose3D place_pose; // w.r.t base link
if (!place_area_finder_.findArea(image, sensor_pose, place_pose,mask))
if (!place_area_finder_.findArea(image, sensor_pose, place_pose,mask,donal))
{
res.error_msg = "No valid place area found";
return true;
Expand Down
37 changes: 8 additions & 29 deletions ed_sensor_integration/src/kinect/place_area_finder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -390,7 +390,7 @@ PlaceAreaFinder::~PlaceAreaFinder()
{
}

bool PlaceAreaFinder::findArea(const rgbd::ImageConstPtr &image, geo::Pose3D sensor_pose, geo::Pose3D &place_pose,const cv::Mat &mask)
bool PlaceAreaFinder::findArea(const rgbd::ImageConstPtr &image, geo::Pose3D sensor_pose, geo::Pose3D &place_pose,const cv::Mat &mask,bool donal)
{
bool visualize = true;
// std::cout << "converting image to cloud" << std::endl;
Expand All @@ -413,8 +413,6 @@ bool PlaceAreaFinder::findArea(const rgbd::ImageConstPtr &image, geo::Pose3D sen
pcl::PointCloud<pcl::PointXYZRGB>::Ptr floorless_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
removeFloor(cloud, floorless_cloud, floorless_index);



// Segment the table plane and return a cloud with the plane and a cloud where the plane is removed
std::cout << "SegmentPlane" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr plane_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Expand Down Expand Up @@ -465,7 +463,7 @@ bool PlaceAreaFinder::findArea(const rgbd::ImageConstPtr &image, geo::Pose3D sen
float length = object_diameter + error_margin;
float placement_margin = 2 * 0.02 + length;

if (visualize && !mask.empty())
if ((visualize && !mask.empty())| !donal)
{
std::cout << "annotating image" << std::endl;
annotated_image = image->getRGBImage().clone();
Expand All @@ -486,22 +484,14 @@ bool PlaceAreaFinder::findArea(const rgbd::ImageConstPtr &image, geo::Pose3D sen
createCostmap(not_table_cloud, occupied_color);

// donal changes-----------------------------------------------------------------------------------------------------------------------------
if (donal){
cv::Scalar cyan(255, 255, 0);
cv::Scalar white(255, 255, 255);
cv::Scalar yellow(0, 255, 255);
// drawContour(plane_cloud, table_color);

drawContourAndTransformToWorld(plane_cloud,cyan,height);

//createCostmap(plane_cloud,white);

extractMaskPoints(plane_cloud);

createCostmap(plane_cloud,yellow);


// mapCanvasToWorldAndPlaceInAnnotatedImage(*image,plane_cloud, donal_colour);
// drawfilledContour(plane_cloud,table_color);
}
// donal changes--------------------------------------------------------------------------------------------------------------------------

// HERO preferred radius
Expand Down Expand Up @@ -536,7 +526,6 @@ cv::Point2d PlaceAreaFinder::worldToCanvas(double x, double y)

cv::Point2d PlaceAreaFinder::canvasToWorld2(double u, double v)
{
// Assuming canvas_center is defined in your class
double x = -(v - canvas_center.y) * resolution;
double y = -(u - canvas_center.x) * resolution;

Expand All @@ -557,7 +546,7 @@ void PlaceAreaFinder::drawContourAndTransformToWorld(pcl::PointCloud<pcl::PointX
// Create a binary mask
cv::Mat mask = cv::Mat::zeros(canvas.rows, canvas.cols, CV_8UC1);

std::vector<cv::Point> points; // Vector to store points for convex hull
std::vector<cv::Point> points;

for (uint nIndex = 0; nIndex < cloud->points.size(); nIndex++)
{
Expand All @@ -568,13 +557,8 @@ void PlaceAreaFinder::drawContourAndTransformToWorld(pcl::PointCloud<pcl::PointX

if (p.x >= 0 && p.y >= 0 && p.x < canvas.cols && p.y < canvas.rows)
{
// Update the canvas with the color
canvas.at<cv::Vec3b>(p) = cv::Vec3b(color[0], color[1], color[2]);

// Set corresponding pixel in the mask to 255 (white)
mask.at<uchar>(p) = 255;

// Store the point for convex hull
points.push_back(p);
}
}
Expand All @@ -585,8 +569,6 @@ void PlaceAreaFinder::drawContourAndTransformToWorld(pcl::PointCloud<pcl::PointX
// Find convex hull of the points
std::vector<cv::Point> hull;
cv::convexHull(points, hull);

// Fill the convex hull on the canvas
cv::fillConvexPoly(mask, hull, cv::Scalar(255, 255, 255));

// Transform the hull points to the world frame
Expand All @@ -609,13 +591,12 @@ void PlaceAreaFinder::drawContourAndTransformToWorld(pcl::PointCloud<pcl::PointX
}

// Draw the convex hull on the canvas
// std::vector<std::vector<cv::Point>> contours = { hull };
// cv::drawContours(canvas, contours, -1, color, cv::FILLED); // Use CV_FILLED to fill the contour
// cv::drawContours(canvas, contours, -1, cv::Scalar(255, 255, 255), 2);
std::vector<std::vector<cv::Point>> contours = { hull };
cv::drawContours(canvas, contours, -1, color, cv::FILLED); // Use CV_FILLED to fill the contour
cv::drawContours(canvas, contours, -1, cv::Scalar(255, 255, 255), 2);// Draws white border contour
}
else
{
// Handle the case when there are not enough points for convex hull
std::cerr << "Not enough points for convex hull." << std::endl;
}
}
Expand All @@ -639,8 +620,6 @@ void PlaceAreaFinder::extractMaskPoints(pcl::PointCloud<pcl::PointXYZRGB>::Ptr i
}




// donal changes----------------------------------------------------------------------------------------------------------------------------------
void PlaceAreaFinder::createCostmap(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, cv::Scalar color)
{
Expand Down
31 changes: 21 additions & 10 deletions ed_sensor_integration/tools/empty_spot_designator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,8 +153,9 @@ void imageCallback(const sensor_msgs::ImageConstPtr& msg)
*/
void usage()
{
std::cout << "Usage: ed_empty_spot_designator RGBD_TOPIC" << std::endl
<< "RGBD_TOPIC topic on which the rgbd image is published, example /hero/head_rgbd_sensor/rgbd" << std::endl;
std::cout << "Usage: ed_empty_spot_designator RGBD_TOPIC BOOL" << std::endl
<< "RGBD_TOPIC topic on which the rgbd image is published, example /hero/head_rgbd_sensor/rgbd" << std::endl
<< "Set second argument to donal or max to run that version of the code" << std::endl;
}

void drawMaskContour(cv::Mat& image, const cv::Mat& mask)
Expand All @@ -180,13 +181,8 @@ void drawMaskContour(cv::Mat& image, const cv::Mat& mask)
}

rgbd::ImageConstPtr createModifiedImage(const rgbd::ImageConstPtr& originalImagePtr, const cv::Mat& newRGBValues) {
// Create a copy of the original image
rgbd::Image modifiedImage = originalImagePtr->clone();

// Set the new RGB values
modifiedImage.setRGBImage(newRGBValues);

// Create a shared pointer to the modified image
rgbd::ImageConstPtr modifiedImagePtr = std::make_shared<const rgbd::Image>(modifiedImage);

return modifiedImagePtr;
Expand All @@ -199,7 +195,7 @@ rgbd::ImageConstPtr createModifiedImage(const rgbd::ImageConstPtr& originalImage
*/
int main (int argc, char **argv)
{
if (argc != 2)
if (argc != 3)
{
usage();
return 0;
Expand All @@ -210,6 +206,15 @@ int main (int argc, char **argv)
std::string topic = argv[1];
std::cout << "Using topic: " << topic << std::endl;

std::string arg = argv[2];
bool donal = (arg == "donal");
if (donal){
std::cout <<"Running Donal's version!"<<std::endl;
}
else{
std::cout<<"Running Max's version!"<<std::endl;
}

rgbd::ImageBuffer image_buffer;
image_buffer.initialize(topic, "base_link");

Expand Down Expand Up @@ -237,22 +242,28 @@ int main (int argc, char **argv)
}

std::cout << "Trying to replace RGB with mask" << std::endl;
if(!mask.empty())
if(!mask.empty() && donal)
{
if(mutex.try_lock()){
rgbd::ImageConstPtr new_image_ptr = createModifiedImage(image, mask.clone());
cv::Mat rgbcanvas = new_image_ptr->getRGBImage();
cv::imshow("RGB remapped to mask", rgbcanvas);
std::cout << "Replaced RGB with mask!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;

if (!place_area_finder.findArea(new_image_ptr, sensor_pose, place_pose,mask))
if (!place_area_finder.findArea(new_image_ptr, sensor_pose, place_pose,mask,donal))
{
std::cout << "no place area found" << std::endl;
}
mutex.unlock();
}

}
else{
if (!place_area_finder.findArea(image, sensor_pose, place_pose,mask,donal))
{
std::cout << "no place area found" << std::endl;
}
}


std::cout << place_pose << std::endl;
Expand Down

0 comments on commit 3a1e3d7

Please sign in to comment.