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