camera_webots.cpp

Go to the documentation of this file.

 
#include <std_msgs/String.h>
#include "ros/ros.h"
#include <signal.h>
 
#include <image_transport/image_transport.h>
 
#include "rosinljubljana/rosinljubljana.h"
 
#define TIME_STEP 32
 
static int controllerCount;
static std::vector<std::string> controllerList;
 
// catch names of the controllers availables on ROS network
void controllerNameCallback(const std_msgs::String::ConstPtr &name) {
  controllerCount++;
  controllerList.push_back(name->data);
  ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}
 
void quit(int sig) {
  ROS_INFO("User stopped the 'robot_information_parser' node.");
  ros::shutdown();
  exit(0);
}
 
int main(int argc, char **argv) {
 
  std::string controllerName;
  std::vector<std::string> deviceList;
  int width, height;
 
  // create a node named 'robot_information_parser' on ROS network
  ros::init(argc, argv, "camera", ros::init_options::AnonymousName);
  ros::NodeHandle n;
 
  image_transport::ImageTransport it(n);
  signal(SIGINT, quit);
 
  // subscribe to the topic model_name to get the list of availables controllers
  ros::Subscriber nameSub = n.subscribe("model_name", 100, controllerNameCallback);
  while (controllerCount < 3 || controllerCount < nameSub.getNumPublishers()) {
    ros::spinOnce();
    ros::spinOnce();
    ros::spinOnce();
  }
  ros::spinOnce();
 
  for (int i = 0; i<controllerCount; i++){
    if (controllerList[i].find("CAMERA") != std::string::npos) {
      controllerName = controllerList[i];
    }
 
  }
 
  // leave topic once it is not necessary anymore
  nameSub.shutdown();
 
  // create camera object
  WebotsCamera kamera(n, it, controllerName);
  kamera.enable(n);
  kamera.setFrames();
  kamera.subscribe(n);
  kamera.setCamInfo();
  //tf::Quaternion q_rot;
  //q_rot.setRPY(0.0, -1.56, 0.0);
  //kamera.setPositionOrientation(tf::Vector3(0.0, 0.0, 1.5), q_rot);
 
  while(ros::ok()){
    // publish camera image
    kamera.publish();
    kamera.transmitFrames();
    ros::spinOnce();
  }
}