Robotics StackExchange | Archived questions

Laser ray sensor not detecting object in plugin?

Hi all,

I’m new to Gazebo and I couldn’t find any examples that show the examination of a laser sensor data in a plugin.

I’m using a model plugin and I inserted a box model in the gui. The visual ray shows the box stopping the ray. But, when I displayed the min, max range and the fidtance from a deted object. The distance from the object is always infinite.

= 0.08 maxdistance = 10 currentdistance = inf

The model.sdf is:

<?xml version="1.0" ?>
<sdf version="1.5">
  <model name="ab_robot">
    <link name="chassis">
      <pose>0 0 0.16 0 0 0</pose>
      <inertial>
        <mass>5.67</mass>
        <!-- interias are tricky to compute -->
        <inertia>
          <ixx>0.07</ixx>
          <iyy>0.08</iyy>
          <izz>0.10</izz>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyz>0</iyz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>0.445 0.277 0.17</size>
          </box>
        </geometry>
      </collision>
      <collision name="castor_collision">
        <pose>-0.200 0 -0.12 0 0 0</pose>
        <geometry>
          <sphere>
            <radius>0.04</radius>
          </sphere>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>0</mu>
              <mu2>0</mu2>
              <slip1>1.0</slip1>
              <slip2>1.0</slip2>
            </ode>
          </friction>
        </surface>
      </collision>
      <visual name="visual">
        <pose>0 0 0.04 0 0 0</pose>
        <geometry>
          <mesh>
            <uri>model://pioneer2dx/meshes/chassis.dae</uri>
          </mesh>
        </geometry>
      </visual>
      <visual name="castor_visual">
        <pose>-0.200 0 -0.12 0 0 0</pose>
        <geometry>
          <sphere>
            <radius>0.04</radius>
          </sphere>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/FlatBlack</name>
          </script>
        </material>
      </visual>
    </link>
    <link name="right_wheel">
      <pose>0.1 -.17 0.11 0 1.5707 1.5707</pose>
      <!-- Note that the following is inertial not inertia! -->
      <inertial>
        <mass>1.5</mass>
        <!-- Note that the following is inertia not inertial! -->
        <inertia>
          <ixx>0.0051</ixx>
          <iyy>0.0051</iyy>
          <izz>0.0090</izz>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyz>0</iyz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <cylinder>
            <radius>0.11</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>100000.0</mu>
              <mu2>100000.0</mu2>
              <slip1>0.0</slip1>
              <slip2>0.0</slip2>
            </ode>
          </friction>
        </surface>
      </collision>
      <visual name="visual">
        <geometry>
          <cylinder>
            <radius>0.11</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/FlatBlack</name>
          </script>
        </material>
      </visual>
    </link>
    <link name="left_wheel">
      <pose>0.1 .17 0.11 0 1.5707 1.5707</pose>
      <!-- Note that the following is inertial not inertia! -->
      <inertial>
        <mass>1.5</mass>
        <!-- Note that the following is inertia not inertial! -->
        <inertia>
          <ixx>0.0051</ixx>
          <iyy>0.0051</iyy>
          <izz>0.0090</izz>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyz>0</iyz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <cylinder>
            <radius>0.11</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>100000.0</mu>
              <mu2>100000.0</mu2>
              <slip1>0.0</slip1>
              <slip2>0.0</slip2>
            </ode>
          </friction>
        </surface>
      </collision>
      <visual name="visual">
        <geometry>
          <cylinder>
            <radius>0.11</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/FlatBlack</name>
          </script>
        </material>
      </visual>
    </link>
    <joint type="revolute" name="left_wheel_hinge">
      <pose>0 0 -0.03 0 0 0</pose>
      <child>left_wheel</child>
      <parent>chassis</parent>
      <axis>
        <xyz>0 1 0</xyz>
        <use_parent_model_frame>true</use_parent_model_frame>
      </axis>
    </joint>
    <joint type="revolute" name="right_wheel_hinge">
      <pose>0 0 0.03 0 0 0</pose>
      <child>right_wheel</child>
      <parent>chassis</parent>
      <axis>
        <xyz>0 1 0</xyz>
        <use_parent_model_frame>true</use_parent_model_frame>
      </axis>
    </joint>

   <plugin name="model_manual_op" filename="build/libmodel_manual_op.so"/>

   <!-- Lasar Ray Sensor -->
    <include>
      <uri>model://hokuyo</uri>
      <pose>0.2 0 0.3 0 0 0</pose>

    <scan>
            <horizontal>
              <samples>5</samples>
              <resolution>1</resolution>
              <min_angle>-2.26889</min_angle>
              <max_angle>2.268899</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.08</min>
            <max>10</max>
            <resolution>0.01</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.01</stddev>
          </noise>


    </include>
    <joint name="hokuyo_joint" type="revolute">
      <child>hokuyo::link</child>
      <parent>chassis</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <upper>0</upper>
          <lower>0</lower>
        </limit>
      </axis>
    </joint>


  </model>
</sdf>

The RayPlugin.cc si

/*
 * Desc: Contact plugin
 * Author: Nate Koenig mod by John Hsu
 */

#include "gazebo/physics/physics.hh"
#include "RayPlugin.hh"
#include <iostream>


using namespace gazebo;

// Register this plugin with the simulator
GZ_REGISTER_SENSOR_PLUGIN(RayPlugin)

/////////////////////////////////////////////////
RayPlugin::RayPlugin()
{
}

/////////////////////////////////////////////////
RayPlugin::~RayPlugin()
{
  this->parentSensor->GetLaserShape()->DisconnectNewLaserScans(
      this->newLaserScansConnection);
  this->newLaserScansConnection.reset();

  this->parentSensor.reset();
  this->world.reset();
}

/////////////////////////////////////////////////
void RayPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr /*_sdf*/)
{
  std::cout << "RayPlugin loaded" << std::endl;


  // Get then name of the parent sensor
  this->parentSensor =
    boost::dynamic_pointer_cast<sensors::RaySensor>(_parent);

  if (!this->parentSensor)
    gzthrow("RayPlugin requires a Ray Sensor as its parent");

  this->world = physics::get_world(this->parentSensor->GetWorldName());

  this->newLaserScansConnection =
    this->parentSensor->GetLaserShape()->ConnectNewLaserScans(
      boost::bind(&RayPlugin::OnNewLaserScans, this));
}

/////////////////////////////////////////////////
void RayPlugin::OnNewLaserScans()
{
  //std::cout << "OnNewLaserScans entered!!" << std::endl;
/*
  lidar_message.set_time_msec(0);
  lidar_message.set_min_distance(parentSensor->GetRangeMin());
  lidar_message.set_max_distance(parentSensor->GetRangeMax());
  lidar_message.set_current_distance(parentSensor->GetRange(0));

  lidar_pub_->Publish(lidar_message);
  */

  std::cout << " = " << parentSensor->GetRangeMin() << 
  "  max_distance = " << parentSensor->GetRangeMax() << 
  "  current_distance = " << parentSensor->GetRange(0)
      <<  std::endl;

}

I’m using Gazebo 6.0 on the Mac. Any help is deeply appreciated. I eventually want to publish the detection. Question 2: is this the proper way to handle detection or would I use the gazebo/hokuyo/lonk/laser/scan topic topic messages for this?

Asked by arnoldb on 2016-02-22 11:41:22 UTC

Comments

Answers