opencv_perception_node.cpp File Reference

#include <image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <std_msgs/msg/int32.hpp>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>

Include dependency graph for opencv_perception_node.cpp:

Go to the source code of this file.

Functions
	RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE (image_tools::ROSCvMatContainer, sensor_msgs::msg::Image)
void	segmentColor (const cv::Mat &inputRGB, int hueMean, int hueWindow, cv::Mat &out)
int	testImage (cv::Mat &input, cv::Mat &debugImage, std::string colorName, int hueMean, int hueWindow)
int	testRed (cv::Mat &input, cv::Mat &debugImage)
int	testBlue (cv::Mat &input, cv::Mat &debugImage)
int	testGreen (cv::Mat &input, cv::Mat &debugImage)
int	testRed (std::string path, cv::Mat &debugImage)
int	testBlue (std::string path, cv::Mat &debugImage)
int	testGreen (std::string path, cv::Mat &debugImage)
void	update ()
void	callback (const image_tools::ROSCvMatContainer &img)
int	main (int argc, char **argv)

Variables
rclcpp::Publisher< std_msgs::msg::Int32 >::SharedPtr	detectionPub
rclcpp::Publisher< image_tools::ROSCvMatContainer >::SharedPtr	debugImagePub
rclcpp::Subscription< image_tools::ROSCvMatContainer >::SharedPtr	imageSub

Function Documentation

callback()

void callback

(

const image_tools::ROSCvMatContainer &

img

)

Definition at line 133 of file opencv_perception_node.cpp.

{
  cv::Mat image = img.cv_mat();
 
  image_tools::ROSCvMatContainer outimg;
  auto outmat = outimg.cv_mat();
 
  int detectedColor = 0;
  if (testRed(image, outmat) > 0)
  {
    detectedColor = 1;
  }
 
  if (testGreen(image, outmat) > 0)
  {
    detectedColor = 2;
  }
 
  if (testBlue(image, outmat) > 0)
  {
    detectedColor = 3;
  }
 
  std_msgs::msg::Int32 detectedColorMsg;
  detectedColorMsg.data = detectedColor;
  detectionPub->publish(detectedColorMsg);
 
 
  outimg.header() = img.header();
  debugImagePub->publish(outimg);
}

References image_tools::ROSCvMatContainer::cv_mat(), debugImagePub, detectionPub, image_tools::ROSCvMatContainer::header(), testBlue(), testGreen(), and testRed().

Referenced by smacc2::utils::Bind< 1 >::bindaux(), smacc2::utils::Bind< 2 >::bindaux(), smacc2::utils::Bind< 3 >::bindaux(), smacc2::utils::Bind< 4 >::bindaux(), smacc2::introspection::EventGeneratorHandler::configureEventGenerator(), smacc2::introspection::StateReactorHandler::configureStateReactor(), smacc2::ISmaccClient::connectSignal(), smacc2::StateReactor::createEventCallback(), smacc2::ISmaccStateMachine::createSignalConnection(), main(), smacc2::client_bases::SmaccActionClientBase< ActionType >::onAborted(), smacc2::client_bases::SmaccActionClientBase< ActionType >::onCancelled(), smacc2::components::CpTopicSubscriber< MessageType >::onFirstMessageReceived(), smacc2::client_bases::SmaccSubscriberClient< MessageType >::onFirstMessageReceived(), cl_keyboard::ClKeyboard::OnKeyPress(), smacc2::components::CpTopicSubscriber< MessageType >::onMessageReceived(), smacc2::client_bases::SmaccSubscriberClient< MessageType >::onMessageReceived(), cl_multirole_sensor::ClMultiroleSensor< MessageType >::onMessageTimeout(), smacc2::client_bases::SmaccActionClientBase< ActionType >::onSucceeded(), and smacc2::state_reactors::SrConditional< TEv >::SrConditional().

Here is the call graph for this function:

Here is the caller graph for this function:

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 165 of file opencv_perception_node.cpp.

{
  rclcpp::init(argc, argv);
  rclcpp::Node::SharedPtr nh;
 
  detectionPub = nh->create_publisher<std_msgs::msg::Int32>("detected_color", 1);
  imageSub = nh->create_subscription<image_tools::ROSCvMatContainer>("/image_raw", 1, callback);
  debugImagePub = nh->create_publisher<image_tools::ROSCvMatContainer>("/opencv_debug_image", 1);
 
  rclcpp::Rate r(10);
 
  while (!rclcpp::shutdown())
  {
    update();
    rclcpp::spin_some(nh);
    r.sleep();
  }
 
  /*
  testRed("../../red1.png");
  testRed("../../green1.png");
  testRed("../../blue1.png");
  testRed("../../red2.png");
 
  testGreen("../../red1.png");
  testGreen("../../green1.png");
  testGreen("../../blue1.png");
  testGreen("../../red2.png");
 
  testBlue("../../red1.png");
  testBlue("../../green1.png");
  testBlue("../../blue1.png");
  testBlue("../../red2.png");
  */
}

References callback(), debugImagePub, detectionPub, imageSub, and update().

Here is the call graph for this function:

RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE()

RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE	(	image_tools::ROSCvMatContainer	,
		sensor_msgs::msg::Image
	)

segmentColor()

void segmentColor	(	const cv::Mat &	inputRGB,
		int	hueMean,
		int	hueWindow,
		cv::Mat &	out
	)

Definition at line 42 of file opencv_perception_node.cpp.

{
  cv::Mat hsvInput;
  cv::cvtColor(inputRGB, hsvInput, cv::COLOR_BGR2HSV);
  cv::inRange(hsvInput, cv::Scalar(hueMean - hueWindow, 20, 20), cv::Scalar(hueMean + hueWindow, 255, 255), out);
  cv::threshold(out, out, 100, 255, cv::THRESH_BINARY);
}

Referenced by testImage().

Here is the caller graph for this function:

testBlue() [1/2]

int testBlue	(	cv::Mat &	input,
		cv::Mat &	debugImage
	)

Definition at line 101 of file opencv_perception_node.cpp.

{
  return testImage(input, debugImage, "blue", 10, 10);
}

References testImage().

Referenced by callback().

Here is the call graph for this function:

Here is the caller graph for this function:

testBlue() [2/2]

int testBlue	(	std::string	path,
		cv::Mat &	debugImage
	)

Definition at line 117 of file opencv_perception_node.cpp.

{
  cv::Mat input = cv::imread(path);
  return testImage(input, debugImage, "blue", 10, 10);
}

References testImage().

Here is the call graph for this function:

testGreen() [1/2]

int testGreen	(	cv::Mat &	input,
		cv::Mat &	debugImage
	)

Definition at line 106 of file opencv_perception_node.cpp.

{
  return testImage(input, debugImage, "green", 50, 10);
}

References testImage().

Referenced by callback().

Here is the call graph for this function:

Here is the caller graph for this function:

testGreen() [2/2]

int testGreen	(	std::string	path,
		cv::Mat &	debugImage
	)

Definition at line 123 of file opencv_perception_node.cpp.

{
  cv::Mat input = cv::imread(path);
  return testImage(input, debugImage, "green", 50, 10);
}

References testImage().

Here is the call graph for this function:

testImage()

int testImage	(	cv::Mat &	input,
		cv::Mat &	debugImage,
		std::string	colorName,
		int	hueMean,
		int	hueWindow
	)

Definition at line 50 of file opencv_perception_node.cpp.

{
  cv::Mat segmented;
 
  segmentColor(input, hueMean, hueWindow, segmented);
 
  cv::SimpleBlobDetector::Params params;
  // params.filterByArea= true;
  // params.minArea = 10;
  params.filterByArea = true;
  params.minArea = 100;
  params.maxArea = 100000;
  params.filterByColor = true;
  params.blobColor = 255;
 
  auto blobDetector = cv::SimpleBlobDetector::create(params);
  std::vector<cv::KeyPoint> blobs;
  blobDetector->detect(segmented, blobs);
 
  std::cout << "-------" << std::endl;
  for (auto& b : blobs)
  {
    std::cout << "blob " << colorName << " detected: " << b.size << std::endl;
  }
 
  if(!blobs.empty())
  {
    for(auto& b: blobs)
    {
      cv::Rect r;
      float diameter = b.size;
      auto radius = diameter*0.5;
      r.x = b.pt.x - radius;
      r.y = b.pt.y - radius;
      r.width = diameter;
      r.height = diameter;
      cv::rectangle(debugImage,r, cv::Scalar(255,0,0),1 );
    }
  }
 
  // cv::imshow(colorName + " filter - "+ path, segmented);
  // cv::waitKey();
 
  return blobs.size();
}

References segmentColor().

Referenced by testBlue(), testGreen(), and testRed().

Here is the call graph for this function:

Here is the caller graph for this function:

testRed() [1/2]

int testRed	(	cv::Mat &	input,
		cv::Mat &	debugImage
	)

Definition at line 96 of file opencv_perception_node.cpp.

{
  return testImage(input, debugImage, "red", 130, 20);
}

References testImage().

Referenced by callback().

Here is the call graph for this function:

Here is the caller graph for this function:

testRed() [2/2]

int testRed	(	std::string	path,
		cv::Mat &	debugImage
	)

Definition at line 111 of file opencv_perception_node.cpp.

{
  cv::Mat input = cv::imread(path);
  return testImage(input, debugImage, "red", 130, 20);
}

References testImage().

Here is the call graph for this function:

update()

void update

(

)

Definition at line 129 of file opencv_perception_node.cpp.

{
}

Referenced by main().

Here is the caller graph for this function:

Variable Documentation

debugImagePub

rclcpp::Publisher<image_tools::ROSCvMatContainer>::SharedPtr debugImagePub

Definition at line 39 of file opencv_perception_node.cpp.

Referenced by callback(), and main().

detectionPub

rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr detectionPub

Definition at line 38 of file opencv_perception_node.cpp.

Referenced by callback(), and main().

imageSub

rclcpp::Subscription<image_tools::ROSCvMatContainer>::SharedPtr imageSub

Definition at line 40 of file opencv_perception_node.cpp.

Referenced by main().