service waiting for a subscriber msg
I would like to have a Service server that upon request takes the next available camera image from the topic /raw_image
and returns the position of a red blob. I would like to do this with a detector class, but I'm not sure how to do the conditoning: when client request received than process the next available video frame and send the response
My code so far:
class SwitchState_Detector
{
public:
explicit SwitchState_Detector(ros::NodeHandle *nh);
~SwitchState_Detector();
void image_callback(const sensor_msgs::ImageConstPtr &original_image);
bool find_redBlob(cv::Mat input_image, cv::Mat output_image);
private:
image_transport::Subscriber sub;
cv::Mat I; //OpeCV image
std::string filename; //calibration file path
cv::Mat *intrinsics; //camera intrinsics
cv::Mat *distortion_coeff; //camera distortion coeffs
cv::Size *image_size;
}
SwitchState_Detector::SwitchState_Detector(ros::NodeHandle *nh)
{
image_transport::ImageTransport it(nh);
sub = it.subscribe("/image_raw", 1, &SwitchState_Detector::image_callback, this);
}
SwitchState_Detector::~SwitchState_Detector()
{
delete intrinsics;
delete distortion_coeff;
delete image_size;
}
void
SwitchState_Detector::image_callback(const sensor_msgs::ImageConstPtr &original_image)
{
//ROS Image to Mat structure
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("red_ball_detection::cv_bridge_exception %s", e.what());
return;
}
I = cv_ptr->image;
imshow("RGB", I);
cv::waitKey(10);
}
double find_redBlob(cv::Mat input_image, cv::Mat output_image)
{
// do the processing and calculations on Mat I received after the request was issued from a client
// This Part I don't know how to condition that a fresh image was received.
return blob_size;
}
main(int argc, char *argv[])
{
ros::init(argc, argv, "switch_state_detection");
ros::NodeHandle nh;
SwitchState_Detector detector(&nh);
ros::ServiceServer service = nh.advertiseService("detect_switch_state", &SwitchState_Detector::find_redBlob, &detector);
ros::spin();
return(0);
}