If this is for Fuerte then you will be using Gazebo 1.0. In your SDF file, add a sensor element which is a child of your link element. An example is below that includes two camera sensors in the link called base_link
<link name = "base_link" >
<visual name="base_link_visual">
<geometry>
<mesh filename="Base Assembly.stl" scale="1 1 1"/>
</geometry>
<origin pose="0 0 0 0 0 0"/>
<material script="Gazebo/Grey"/>
</visual>
<collision name="base_link_collision">
<geometry>
<box size="1.528 0.491 .825"/>
</geometry>
<origin pose="0 0 0 0 0 0"/>
</collision>
<inertial mass ="750.004">
<inertia ixx="67.5" ixy="0.022" iyy="67" ixz="-0.363" iyz="-0.0352" izz="25"/>
<origin pose=" 0.262 -0.005 0.053 0 0 0" />
</inertial>
<sensor name='cam_sensor2' type='camera' always_on='1' update_rate='15'>
<origin pose='-1.25 0.4 0.5 0 0.7 1.0'/>
<camera>
<horizontal_fov angle='1.57'/>
<image width='320' height='240' format='R8G8B8'/>
<clip near='0.5' far='5'/>
</camera>
<plugin name='CameraPublisher' filename='libCameraPublisher.so'/>
</sensor>
<sensor name='cam_sensor1' type='camera' always_on='1' update_rate='15'>
<origin pose='1.25 0.4 0.5 0 0.7 2.0'/>
<camera>
<horizontal_fov angle='1.57'/>
<image width='320' height='240' format='R8G8B8'/>
<clip near='0.5' far='5'/>
</camera>
<plugin name='CameraPublisher' filename='libCameraPublisher.so'/>
</sensor>
</link>
to position your cameras use the <origin> element of the sensor. this position is the difference compared to the link's origin.
The CameraPublisher is a plugin that I copied probably from the Gazebo website. You shouldn't need to change it as the configuring of the camera (size, framerate etc) occurs in the SDF file. The CameraPublisher plugin's code is here:
camerapublisher.cpp:
#include "gazebo.hh"
#include "plugins/CameraPlugin.hh"
#include "common/common.h"
#include "transport/transport.h"
#include <stdio.h>
#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#include "image_transport/image_transport.h"
namespace gazebo
{
class CameraPublisher : public CameraPlugin
{
public: CameraPublisher() : CameraPlugin(),saveCount(0)
{
std::string name = "CameraPublisher";
int argc = 0;
ros::init(argc, NULL, name);
}
public: ~CameraPublisher()
{
delete this->node;
}
public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
// Don't forget to load the camera plugin
CameraPlugin::Load(_parent,_sdf);
//this->model = _parent;
//ROS Nodehandle
//current_time = this->model->GetWorld()->GetSimTime();
this->node = new ros::NodeHandle("~");
//this->it = this->node;
ros::NodeHandle node_temp = *this->node;
image_transport::ImageTransport trans(node_temp);
char topicname[12];
char fullname[50];
std::cout<<this->parentSensor->GetCamera()->GetName().c_str()<<std::endl;
snprintf(fullname, 30, "%s", this->parentSensor->GetCamera()->GetName().c_str());
strncpy(&topicname[0], &fullname[9], 11);
topicname[11] ='\0';
std::cout<<topicname<<std::endl;
//ROS Publisher
//this->pub = this->node->advertise<sensor_msgs::Image>(topicname, 1000);
this->pub = trans.advertise(topicname, 1);
}
// Update the controller
public: void OnNewFrame(const unsigned char *_image,
unsigned int _width, unsigned int _height, unsigned int _depth,
const std::string &_format)
{
sensor_msgs::Image msg;
//msg.header.stamp = this->model->GetWorld()->GetSimTime();
msg.is_bigendian = 0;
if(this->pub.getNumSubscribers() >0)
{
//for (int i =0; i<(msg.height*msg ...
(more)