SHARE
TWEET

Untitled

a guest Apr 7th, 2011 339 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <ros/ros.h>
  2. #include <image_transport/image_transport.h>
  3.  
  4. #include <alproxies/alvideodeviceproxy.h>
  5. #include <alvision/alvideo.h>
  6.  
  7. int main(int argc, char** argv)
  8. {
  9.         ros::init(argc, argv, "camera");
  10.         ros::NodeHandle nh("~");
  11.         image_transport::ImageTransport it(nh);
  12.         image_transport::Publisher pub = it.advertise("image", 1);
  13.  
  14.         // Connecting to video module
  15.         AL::ALPtr<AL::ALVideoDeviceProxy> camera(new AL::ALVideoDeviceProxy("127.0.0.1", 9559));
  16.  
  17.         // Registering a generic video module
  18.         std::string gvmName = camera->subscribe(
  19.                 "naoqi_wrapper",
  20.                 // kVGA   ( 640 * 480 ),
  21.                 // kQVGA  ( 320 * 240 ),
  22.                 // kQQVGA ( 160 * 120 ).
  23.                 AL::kQQVGA,
  24.                 // kYuvColorSpace,
  25.                 // kYUVColorSpace,
  26.                 // kYUV422InterlacedColorSpace,
  27.                 // kRGBColorSpace,
  28.                 AL::kYuvColorSpace,
  29.                 // vision module required among : 5, 10, 15, and 30 fps.
  30.                 // (AL note: this field has no effect right now but will be implemented
  31.                 // in a future version)
  32.                 5
  33.         );
  34.  
  35.         ros::Rate loop_rate(5);
  36.  
  37.         while (nh.ok()) {
  38.  
  39.                 AL::ALValue alimage = camera->getImageRemote(gvmName);
  40.                 // TODO release needed with remote?
  41.                 camera->releaseImage(gvmName);
  42.  
  43.                 sensor_msgs::Image msg;
  44.                 // TODO use time given by AL
  45.                 msg.header.stamp = ros::Time::now();
  46.                 msg.header.frame_id = "/base_link";
  47.  
  48.                 msg.width  = (int) alimage[0];
  49.                 msg.height = (int) alimage[1];
  50.                 //msg.encoding = "rgb8"; // ok for kRGBColorSpace
  51.                 msg.encoding = "mono8"; // ok for AL::kYuvColorSpace
  52.                 msg.step = msg.width * ((int) alimage[2]);
  53.                 msg.data = alimage[6];
  54.  
  55.                 pub.publish(msg);
  56.                 ros::spinOnce();
  57.                 loop_rate.sleep();
  58.         }
  59.  
  60.         camera->unsubscribe(gvmName);
  61. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top