Robotics StackExchange | Archived questions

Infra Red Sensor in Gazebo

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 gazeborosIR similar to gazeboroslaser.

EDIT: I looked at gazebo plugins and on the base of gazeboros laser I created a new package with two files:
**ircontroller_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 ircontrollerfile.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);
  this->frameNameP = new ParamT<std::string>("frameName", "default_gazebo_ros_ir_frame", 0);
  Param::End();

  this->IrConnectCount = 0;
  this->deprecatedIrConnectCount = 0;
}

////////////////////////////////////////////////////////////////////////////////
// Destructor
GazeboRosIr::~GazeboRosIr()
{
  delete this->robotNamespaceP;
  //delete this->hokuyoMinIntensityP;
  delete this->gaussianNoiseP;
  delete this->topicNameP;
  delete this->deprecatedTopicNameP;
  delete this->frameNameP;
  delete this->rosnode_;
}

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosIr::LoadChild(XMLConfigNode *node)
{
  this->robotNamespaceP->Load(node);
  this->robotNamespace = this->robotNamespaceP->GetValue();

  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  }

  this->rosnode_ = new ros::NodeHandle(this->robotNamespace);

  //this->hokuyoMinIntensityP->Load(node);
  //this->hokuyoMinIntensity = this->hokuyoMinIntensityP->GetValue();
  //ROS_INFO("INFO: gazebo_ros_ir plugin artifically sets minimum intensity to %f due to cutoff in hokuyo filters." , this->hokuyoMinIntensity);

  this->topicNameP->Load(node);
  this->topicName = this->topicNameP->GetValue();
  this->deprecatedTopicNameP->Load(node);
  this->deprecatedTopicName = this->deprecatedTopicNameP->GetValue();
  this->frameNameP->Load(node);
  this->frameName = this->frameNameP->GetValue();
  this->gaussianNoiseP->Load(node);
  this->gaussianNoise = this->gaussianNoiseP->GetValue();

  if (this->topicName != "")
  {
#ifdef USE_CBQ
    ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::Range>(
      this->topicName,1,
      boost::bind( &GazeboRosIr::IrConnect,this),
      boost::bind( &GazeboRosIr::IrDisconnect,this), ros::VoidPtr(), &this->ir_queue_);
    this->pub_ = this->rosnode_->advertise(ao);
#else
    this->pub_ = this->rosnode_->advertise<sensor_msgs::Range>(this->topicName,1,
      boost::bind( &GazeboRosIr::IrConnect, this),
      boost::bind( &GazeboRosIr::IrDisconnect, this));
#endif
  }

  if (this->deprecatedTopicName != "")
  {
#ifdef USE_CBQ
    ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::Range>(
      this->deprecatedTopicName,1,
      boost::bind( &GazeboRosIr::DeprecatedIrConnect,this),
      boost::bind( &GazeboRosIr::DeprecatedIrDisconnect,this), ros::VoidPtr(), &this->ir_queue_);
    this->deprecated_pub_ = this->rosnode_->advertise(ao);
#else
    this->deprecated_pub_ = this->rosnode_->advertise<sensor_msgs::IrScan>(this->deprecatedTopicName,1,
      boost::bind( &GazeboRosIr::DeprecatedIrConnect, this),
      boost::bind( &GazeboRosIr::DeprecatedIrDisconnect, this));
#endif
  }
}

////////////////////////////////////////////////////////////////////////////////
// Initialize the controller
void GazeboRosIr::InitChild()
{
  // sensor generation off by default
  this->myParent->SetActive(false);
#ifdef USE_CBQ
  // start custom queue for Ir
  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosIr::IrQueueThread,this ) );
#endif
}

////////////////////////////////////////////////////////////////////////////////
// Increment count
void GazeboRosIr::IrConnect()
{
  this->IrConnectCount++;
}
////////////////////////////////////////////////////////////////////////////////
// Decrement count
void GazeboRosIr::IrDisconnect()
{
  this->IrConnectCount--;

  if (this->IrConnectCount == 0 && this->deprecatedIrConnectCount == 0)
    this->myParent->SetActive(false);
}

////////////////////////////////////////////////////////////////////////////////
// Increment count
void GazeboRosIr::DeprecatedIrConnect()
{
  ROS_WARN("you are subscribing to a deprecated ROS topic %s, please change your code/launch script to use new ROS topic %s",
           this->deprecatedTopicName.c_str(), this->topicName.c_str());
  this->deprecatedIrConnectCount++;
}
////////////////////////////////////////////////////////////////////////////////
// Decrement count
void GazeboRosIr::DeprecatedIrDisconnect()
{
  this->deprecatedIrConnectCount--;

  if (this->IrConnectCount == 0 && this->deprecatedIrConnectCount == 0)
    this->myParent->SetActive(false);
}

////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosIr::UpdateChild()
{
  // as long as ros is connected, parent is active
  //ROS_ERROR("debug ir count %d",this->irConnectCount);
  if (!this->myParent->IsActive())
  {
    // do this first so there's chance for sensor to run 1 frame after activate
    if ((this->IrConnectCount > 0 && this->topicName != "") ||
        (this->deprecatedIrConnectCount > 0 && this->deprecatedTopicName != ""))
      this->myParent->SetActive(true);
  }
  else
  {
    this->PutIrData();
  }
}

////////////////////////////////////////////////////////////////////////////////
// Finalize the controller
void GazeboRosIr::FiniChild()
{
  this->ir_queue_.clear();
  this->ir_queue_.disable();
  this->rosnode_->shutdown();
  sleep(1);
#ifdef USE_CBQ
  this->callback_queue_thread_.join();
#endif
}

////////////////////////////////////////////////////////////////////////////////
// Put Ir data to the interface
void GazeboRosIr::PutIrData()
{
 // int i, ja, jb;
  //double ra, rb, r, b;
  //double intensity;

  //Angle maxAngle = this->myParent->GetMaxAngle();
  //Angle minAngle = this->myParent->GetMinAngle();

  //double maxRange = this->myParent->GetMaxRange();
  //double minRange = this->myParent->GetMinRange();
  int irCount = this->myParent->GetIRCount();
  //int rangeCount = this->myParent->GetRangeCount();

  /***************************************************************/
  /*                                                             */
  /*  point scan from Ir                                      */
  /*                                                             */
  /***************************************************************/
  this->lock.lock();
  // Add Frame Name
  this->irMsg.header.frame_id = this->frameName;
  this->irMsg.header.stamp.sec = (Simulator::Instance()->GetSimTime()).sec;
  this->irMsg.header.stamp.nsec = (Simulator::Instance()->GetSimTime()).nsec;


  //double tmp_res_angle = (maxAngle.GetAsRadian() - minAngle.GetAsRadian())/((double)(rangeCount -1)); // for computing yaw
  this->irMsg.min_range = 0;
  this->irMsg.max_range = 250;
  //this->irMsg.angle_increment = tmp_res_angle;
  //this->irMsg.time_increment  = 0; // instantaneous simulator scan
  //this->irMsg.scan_time       = 0; // FIXME: what's this?
  //this->irMsg.range_min = minRange;
  //this->irMsg.range_max = maxRange;
  //this->irMsg.ranges.clear();
  //this->irMsg.intensities.clear();
/*
  // Interpolate the range readings from the rays
  for (i = 0; i<rangeCount; i++)
  {
    b = (double) i * (rayCount - 1) / (rangeCount - 1);
    ja = (int) floor(b);
    jb = std::min(ja + 1, rayCount - 1);
    b = b - floor(b);

    assert(ja >= 0 && ja < rayCount);
    assert(jb >= 0 && jb < rayCount);

    ra = std::min(this->myParent->GetRange(ja) , maxRange-minRange); // length of ray
    rb = std::min(this->myParent->GetRange(jb) , maxRange-minRange); // length of ray

    // Range is linear interpolation if values are close,
    // and min if they are very different
    //if (fabs(ra - rb) < 0.10)
      r = (1 - b) * ra + b * rb;
    //else r = std::min(ra, rb);

    // Intensity is averaged
    intensity = 0.5*( this->myParent->GetRetro(ja) + (int) this->myParent->GetRetro(jb));
*/
    /***************************************************************/
    /*                                                             */
    /*  point scan from Ir                                     */
    /*                                                             */
    /***************************************************************/
    //this->irMsg.ranges.push_back(std::min(r + minRange + this->GaussianKernel(0,this->gaussianNoise), maxRange));
    //this->irMsg.intensities.push_back(std::max(this->hokuyoMinIntensity,intensity + this->GaussianKernel(0,this->gaussianNoise)));
 // }
  this->irMsg.range=this->myParent->GetRange(0);
  this->irMsg.field_of_view = 0;
  this->irMsg.radiation_type=1; 
  // send data out via ros message
  if (this->IrConnectCount > 0 && this->topicName != "")
      this->pub_.publish(this->irMsg);

  if (this->deprecatedIrConnectCount > 0 && this->deprecatedTopicName != "")
      this->deprecated_pub_.publish(this->irMsg);

  this->lock.unlock();

}

//////////////////////////////////////////////////////////////////////////////
// Utility for adding noise
double GazeboRosIr::GaussianKernel(double mu,double sigma)
{
  // using Box-Muller transform to generate two independent standard normally disbributed normal variables
  // see wikipedia
  double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
  double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
  double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
  //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
  // we'll just use X
  // scale to our mu and sigma
  X = sigma * X + mu;
  return X;
}

#ifdef USE_CBQ
////////////////////////////////////////////////////////////////////////////////
// Put ir data to the interface
void GazeboRosIr::IrQueueThread()
{
  static const double timeout = 0.01;

  while (this->rosnode_->ok())
  {
    this->ir_queue_.callAvailable(ros::WallDuration(timeout));
  }
}
#endif

}

I added rosbuild_add_library(ir_controller_lib src/ir_controller_file.cpp) to my CMakeLists.txt
Then I defined an IR element

<?xml version="1.0"?>

<robot
    xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
    xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
    xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">

    <macro name="erratic_ir" params="parent *origin">
    <joint name="my_ir_joint" type="fixed">
        <insert_block name="origin" />
            <parent link="${parent}" />
            <child link="my_ir_link" /> 
    </joint>

    <link name="my_ir_link">
            <inertial>
                <mass value="0.01"/>
                <origin xyz="0 0 0" />
                <inertia ixx="0.001" ixy="0.0" ixz="0.0"
                         iyy="0.001" iyz="0.0" izx="0.0"
                         izz="0.001" />
            </inertial>

            <visual>
               <origin xyz="0 0 0" rpy="0 0 0"/>
               <geometry>
                  <box size="0.01 0.01 0.01" />
               </geometry>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.001 0.001 0.001"/>
                </geometry>
            </collision>
        </link>

    <gazebo reference="my_ir_link">
     <sensor:ir name="my_ir_sensor">
     <minRange>0.06</minRange>
         <maxRange>4.0</maxRange>
     <updateRate>20.0</updateRate>
        <controller:ir_controller_file name="my_ir_controller" plugin="libir_controller_lib.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>20.0</updateRate>
      <topicName>my_ir/ir</topicName>
          <frameName>my_ir_link</frameName>
          <interface:irarray name="my_ir_iface" />
        </controller:ir_controller_file>
      </sensor:ir>
    <!--  <turnGravityOff>true</turnGravityOff>  -->
      <material>Erratic/White</material>
    </gazebo>

    </macro>
</robot>

and tried to add it to a pioneer p3dx model simulated in gazebo.
But I receive the error:

Failed to load libircontrollerlib.so: libircontrollerlib.so: cannot open shared object file: No such file or directory

How can I let the simulation know where the library is?
Many thanks!

Asked by camilla on 2013-02-26 00:27:44 UTC

Comments

Please ask gazebo questions on http://answers.gazebosim.org/questions/

Asked by tfoote on 2015-06-24 13:40:35 UTC

Answers