ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How can I use a service/request to capture a single callback from a subscription

asked 2012-09-03 06:51:33 -0500

benjaminabruzzo gravatar image

updated 2013-11-14 12:32:48 -0500

tfoote gravatar image

Greetings,

First, I apologize for not knowing how to format my code nicely, but it compiles and runs, so I know I don't have programmatic errors.

I have a turtlebot using binocular cameras. I am using two nodes to control the system, one has the locomotion/path planning the other deals with image processing.

I am using the client to request information from the image capture, which is supplied by the service. Inside the service I am using class methods to define my callbacks.

Using ros::spin(), once the service calls "bool grab", the subscription for images from the cameras spin indefinitely. I would like the ability to spin the service indefinitely, but force the camera images to be grabbed once and processed. then if i need new images, I will request them from the client and grab two more, without spinning indefinitely.

I think I can do this using separate queues, one for each camera stream, and a the master one for the service, but I don't think that will actually function the way I described.

Note* I have removed most of the guts from the camera callback to simplify what I have posted, because it would have been unnecessary.

class ImageGrab
{
private:
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Publisher image_pub_;
image_transport::Subscriber image_subr_;
image_transport::Subscriber image_subl_;
ros::ServiceServer grab_service_;

public:
long int p_x1r, p_x2r;

ImageGrab()
: it_(nh_)
{
grab_service_ = nh_.advertiseService("/obj_31/Image_Grab", &ImageGrab::grab, this);
}

~ImageGrab()
{
cv::destroyWindow(WINDOW);
}

bool grab(obj_31::Pixels::Request  &req, obj_31::Pixels::Response &res )
{
ROS_INFO("entered bool grab");
image_subr_ = it_.subscribe("/right/camera/image_raw", 1, &ImageGrab::imageCbr, this);
image_subl_ = it_.subscribe("/left/camera/image_raw", 1, &ImageGrab::imageCbl, this);
res.x1r = p_x1r;
res.x2r = p_x2r;
return true;
}

void imageCbr(const sensor_msgs::ImageConstPtr& msg)
{
p_x1r = 20;
}

void imageCbl(const sensor_msgs::ImageConstPtr& msg)
{
p_x2r = -56;
}

} // close ImageGrab class

int main(int argc, char **argv) 
{
ros::init(argc, argv, "test_server");
ImageGrab ig;
ros::spin();
return 0;
}
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2012-09-03 07:18:57 -0500

Lorenz gravatar image

I'm not sure if that's a good approach at all, but in your service callback you could use ros::topic::waitForMessage to get just one image. If you do that, make sure that you are actually running more than one spinner because otherwise waitForMessage in your service callback will block. You can use ros::AsyncSpinner for instance.

As I said, above solution is probably not the best one. I would instead have the subscriber callbacks running all the time and just store the newest image in a member variable of the class. Then, you can always use the latest image in your service callback.

Btw. please retag your question to help people to find your question easily.

edit flag offensive delete link more
0

answered 2013-09-04 05:22:30 -0500

benjaminabruzzo gravatar image

updated 2013-09-04 05:26:06 -0500

Ultimately, we implemented an external trigger (arduino based, since it was convenient) for the cameras. Though they spin continuously, the images are always within ~2ms of each other. I did take the advice to save it as a member variable (private).

Simplified:

Camera Service:

class ImageGrab
{
private:
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Subscriber image_subr_;
image_transport::Subscriber image_subl_;
ros::ServiceServer grab_service_;

cv::Mat left_image, right_image;
bool flag;

public:
ImageGrab()
: it_(nh_)
{
  grab_service_ = nh_.advertiseService("/hast/Image_Grab", &ImageGrab::grab, this); 
  image_subr_ = it_.subscribe("/right/camera/image_rect_color", 1, &ImageGrab::imageCbr, this);
  image_subl_ = it_.subscribe("/left/camera/image_rect_color", 1, &ImageGrab::imageCbl, this);
}//close constructor

void imageCbr(const sensor_msgs::ImageConstPtr& msg)
{
  cv_bridge::CvImagePtr cv_ptr;
  try
  {cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);}
  catch (cv_bridge::Exception& e)
  {ROS_ERROR("cv_bridge exception: %s", e.what());return;}
  right_image=cv_ptr->image;
} // end void CBr

void imageCbl(const sensor_msgs::ImageConstPtr& msg)
{
  cv_bridge::CvImagePtr cv_ptr;
  try
  {cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);}//end try
  catch (cv_bridge::Exception& e)
  {ROS_ERROR("cv_bridge exception: %s", e.what());return;}//end catch
  left_image=cv_ptr->image;
} // end void Cbl

bool grab(hast::targets::Request &req, hast::targets::Response &res)
{//begin grab
  res.rX = rX;
  res.rY = rY;
  res.lX = lX;
  res.lY = lY;
  res.flag = flag;
  return true;
}
}; //end ImageGrab class

int main(int argc, char **argv) 
{
  ros::init(argc, argv, "observe_srv");
  ImageGrab ig;
  ROS_INFO("Watching..");
  ros::spin();
  return 0;
}

Client:

#include "hast/targets.h"
hast::targets srv;
int main(int argc, char **argv)
{
  ros::init(argc, argv, "hast_cli");
  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<hast::targets>("/hast/Image_Grab");

  do
  {
    if (client.call(srv)) // Successful call() returns true, failed returns false
    {
    read_grab();
    } 
    else //Failed client call
    {
    ROS_INFO("Failed to call service 'Grab Images'");
    }//end else

    // -----Run Experiment here------

  } //end do block
  while (ros::ok());
  return 0;
} //end main
edit flag offensive delete link more

Question Tools

Stats

Asked: 2012-09-03 06:51:33 -0500

Seen: 1,862 times

Last updated: Sep 04 '13