Example of rosinljubljana package for constructing a camera for webots simulation environment:
/*! \file camera_webots.cpp
* Example of rosinljubljana package for constructing a camera for webots simulation environment:
* \verbinclude camera_webots.cpp
*/
#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();
}
}
Definition in file camera_webots.cpp.
