Gazebo Subscriber for a Ray Sensor
Hello, I created a Lidar sensor in gazebo and i want to receive data from it using a gazebo subscriber. I am creating the gazebo subscriber inside the load function of the gazebo plugin so it gets automatically activated with the simulation, but when after inserting the model into the gazebo environment i can't run the simulation. I have noticed using gz topic -i that the Lidar sensor's topic has two subscribers and no publisher. Apparently gazebo creates a subscriber and a publisher for the Lidar sensor on its own and when i have created a subscriber on my own something went wrong. So is there anyway to control the Lidar sensor's data (not just displaying the data using gz topic echo)? Is there anything wrong in my approach? Here is the code for my plugin:
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo_client.hh>
#include <iostream>
namespace gazebo {
class VelodynePlugin:public ModelPlugin{
public:
physics::JointPtr joint;
// start
transport::NodePtr gazeboNode;
transport::SubscriberPtr gazeboSubscriber;
void gazebo_callBack_fun(ConstLaserScanStampedPtr &msg){
std::cout << "The minimum angle is:" << msg->scan().angle_min()<< std::endl;
}
//end
VelodynePlugin(){}
virtual void Load(physics::ModelPtr _model, sdf:: ElementPtr _sdf){
joint = _model->GetJoint("Lidar_Joint");
joint->SetVelocity(0, 3.14);
transport::init();
transport::run();
gazeboNode = transport::NodePtr(new transport::Node());
gazeboNode->Init();
gazeboSubscriber = gazeboNode->Subscribe("~/Velodyne_Lidar/Top_Part/Lidar_Sensor/scan", &VelodynePlugin::gazebo_callBack_fun, this);
}
};
GZ_REGISTER_MODEL_PLUGIN(VelodynePlugin)
}
Here is the CMakeLists.txt file:
cmake_minimum_required(VERSION 3.5)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
#find the Gazebo required packages
find_package(gazebo REQUIRED)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
#build
add_library(Velodyne_Plugin SHARED Velodyne_Plugin.cc)
target_link_libraries(Velodyne_Plugin ${GAZEBO_LIBRARIES})
And this is my model.sdf file:
<?xml version='1.0'?>
<sdf version='1.6'>
<model name='Velodyne_Lidar'>
<link name='Top_Part'>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>9.062e-05</ixx>
<iyy>9.062e-05</iyy>
<izz>9.104e-05</izz>
<ixy>0</ixy>
< ixz>0</ixz>
<iyz>0</iyz>
</inertia>
<pose frame=''>0 0 0 0 -0 0</pose>
</inertial>
<pose frame=''>0 0 0.058674 0 -0 0</pose>
<gravity>1</gravity>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<enable_wind>0</enable_wind>
<sensor name='Lidar_Sensor' type='ray'>
<update_rate>30</update_rate>
<visualize>1</visualize>
<pose frame=''>0 0 0.03 0 -0 -1.57</pose>
<ray>
<scan>
<horizontal>
<samples>32</samples>
<min_angle>-0.785398</min_angle>
<max_angle>0.785398</max_angle>
</horizontal>
<horizontal>
<samples>640</samples>
<resolution>1</resolution>
<min_angle>0</min_angle>
<max_angle>0</max_angle>
</horizontal>
</scan>
<range>
<min>0.1</min>
<max>70</max>
<resolution>0.02</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0</mean>
<stddev>0.1</stddev>
</noise>
</ray>
</sensor>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/abed/.gazebo/models/Velodyne_Lidar/meshes/Velodyne_Top.dae</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/abed/.gazebo/models/Velodyne_Lidar/meshes/Velodyne_Top.dae</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<link name='Bottom_Part'>
<inertial>
<mass>1.2</mass>
<inertia>
<ixx>0.00108747</ixx>
<iyy>0.00108747</iyy>
<izz>0.00109244</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
<pose frame=''>0 0 0 0 -0 0</pose>
</inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<gravity>1</gravity>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<enable_wind>0</enable_wind>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/abed/.gazebo/models/Velodyne_Lidar/meshes/Velodyne_Bottom.dae</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/abed/.gazebo/models/Velodyne_Lidar/meshes/Velodyne_Bottom.dae</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<joint name='Lidar_Joint' type='revolute'>
<parent>Bottom_Part</parent>
<child>Top_Part</child>
<pose frame=''>0 0 0 0 -0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<use_parent_model_frame>0</use_parent_model_frame>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
<effort>-1</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
<physics>
<ode>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
<suspension>
<cfm>0</cfm>
<erp>0.2</erp>
</suspension>
</ode>
</physics>
</joint>
<static>0</static>
<allow_auto_disable>1</allow_auto_disable>
<plugin name='Velodyne_Plugin' filename='libVelodyne_Plugin.so'>
</plugin>
</model>
</sdf>
Asked by abed on 2020-02-25 15:20:11 UTC
Comments
What i have meant by controlling the Lidar sensor's data, is for example being able to store this data's values in some variables for monitoring purposes
Asked by abed on 2020-02-25 16:00:35 UTC