Guest User

Untitled

a guest
Apr 7th, 2011
411
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