Infra Red Sensor in Gazebo [closed]

edit

Hi
I'm using ROS electric and I want to simulate an IRSensor in Gazebo. I've found a tutorial about creating new plugins but it works only for Fuerte. Anyone can help me to understand how to create a new controller gazebo_ros_IR similar to gazebo_ros_laser.

EDIT: I looked at gazebo plugins and on the base of gazebo_ros laser I created a new package with two files:
ir_controller_file.h,

#ifndef GAZEBO_ROS_IR_HH
#define GAZEBO_ROS_IR_HH

#define USE_CBQ
#ifdef USE_CBQ
#include <ros/callback_queue.h>
#include <ros/advertise_options.h>
#endif

#include <gazebo/Controller.hh>
#include <gazebo/Param.hh>

#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
#include <sensor_msgs/Range.h>

namespace gazebo
{
  class IRSensor;

class GazeboRosIr : public Controller
{
  /// \brief Constructor
  /// \param parent The parent entity, must be a Model or a Sensor
  public: GazeboRosIr(Entity *parent);

  /// \brief Destructor
  public: virtual ~GazeboRosIr();

  /// \brief Load the controller
  /// \param node XML config node
  protected: virtual void LoadChild(XMLConfigNode *node);

  /// \brief Init the controller
  protected: virtual void InitChild();

  /// \brief Update the controller
  protected: virtual void UpdateChild();

  /// \brief Finalize the controller
  protected: virtual void FiniChild();

  /// \brief Put laser data to the ROS topic
  private: void PutIrData();

  /// \brief Keep track of number of connctions
  private: int IrConnectCount;
  private: void IrConnect();
  private: void IrDisconnect();
  private: int deprecatedIrConnectCount;
  private: void DeprecatedIrConnect();
  private: void DeprecatedIrDisconnect();

  /// \brief The parent sensor
  private: IRSensor *myParent;

  /// \brief pointer to ros node
  private: ros::NodeHandle* rosnode_;
  private: ros::Publisher pub_;
  private: ros::Publisher deprecated_pub_;

  /// \brief ros message
  private: sensor_msgs::Range irMsg;

  /// \brief topic name
  private: ParamT<std::string> *topicNameP;
  private: std::string topicName;
  private: ParamT<std::string> *deprecatedTopicNameP;
  private: std::string deprecatedTopicName;

  /// \brief frame transform name, should match link name
  private: ParamT<std::string> *frameNameP;
  private: std::string frameName;

  /// \brief Gaussian noise
  private: ParamT<double> *gaussianNoiseP;
  private: double gaussianNoise;

  /// \brief Gaussian noise generator
  private: double GaussianKernel(double mu,double sigma);

  /// \brief A mutex to lock access to fields that are used in message callbacks
  private: boost::mutex lock;

  /// \brief hack to mimic hokuyo intensity cutoff of 100
  private: ParamT<double> *hokuyoMinIntensityP;
  private: double hokuyoMinIntensity;

  /// \brief for setting ROS name space
  private: ParamT<std::string> *robotNamespaceP;
  private: std::string robotNamespace;

#ifdef USE_CBQ
  private: ros::CallbackQueue ir_queue_;
  private: void IrQueueThread();
  private: boost::thread callback_queue_thread_;
#endif

};

/** \} */
/// @}

}

#endif

and ir_controller_file.cpp

#include <algorithm>
#include <assert.h>


#include <ir_controller_package/ir_controller_file.h>

#include <gazebo/Sensor.hh>
#include <gazebo/Global.hh>
#include <gazebo/XMLConfig.hh>
#include <gazebo/HingeJoint.hh>
#include <gazebo/World.hh>
#include <gazebo/Simulator.hh>
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <gazebo/ControllerFactory.hh>
#include <gazebo/IRSensor.hh>
#include <pluginlib/class_list_macros.h>

namespace gazebo
{

GZ_REGISTER_DYNAMIC_CONTROLLER("ir_controller_package", GazeboRosIr);

////////////////////////////////////////////////////////////////////////////////
// Constructor
GazeboRosIr::GazeboRosIr(Entity *parent)
    : Controller(parent)
{
  this->myParent = dynamic_cast<IRSensor*>(this->parent);

  if (!this->myParent)
    gzthrow("GazeboRosIr controller requires a IR Sensor as its parent");

  Param::Begin(&this->parameters);
  this->robotNamespaceP = new ParamT<std::string>("robotNamespace", "/", 0);
  //this->hokuyoMinIntensityP = new ParamT<double>("hokuyoMinIntensity", 101.0, 0);
  this->gaussianNoiseP = new ParamT<double>("gaussianNoise", 0.0, 0);
  this->topicNameP = new ParamT<std::string>("topicName", "", 1);
  this->deprecatedTopicNameP = new ParamT<std::string>("deprecatedTopicName", "", 0 ...