Gazebo resources: Tutorials | Q&A | Documentation | Blog

Retrieving Lidar data from Gazebo (not using ROS)


#1

Greetings Gazebo community,

I am trying to get Lidar data from the simulation. In Gazebo, if I select the topic for the for the Lidar (hokuyo/link/laser/scan) I can plot and visualize the rays and the obstacles seen by the lidar. 

My problem is how to get this data out of Gazebo for later use? So far, I have implemented a short program which body is the following:

///////////////////// laser_scan///////////////////////////////////////////
// input: ConstlaserScanPtr //
// ouput: Void //
// Comments: Updated every time a message is //
// received //
// messages to the console //
////////////////////////////////////////////////////////////////////////////////
void lidar_info(ConstLaserScanPtr &_msg)
{
double dAngle_minimum;
dAngle_minimum = _msg->angle_min();
std::cout << dAngle_minimum<<std::endl;
}

But I am getting the following error :

[libprotobuf ERROR google/protobuf/wire_format.cc:1053] String field contains invalid UTF-8 data when parsing a protocol buffer. Use the ‘bytes’ type if you intend to send raw bytes.
[libprotobuf ERROR google/protobuf/message_lite.cc:123] Can’t parse message of type “gazebo.msgs.LaserScan” because it is missing required fields: angle_min, angle_max, angle_step, range_min, range_max, count, world_pose.position, world_pose.orientation
[libprotobuf ERROR google/protobuf/wire_format.cc:1053] String field contains invalid UTF-8 data when parsing a protocol buffer. Use the ‘bytes’ type if you intend to send raw bytes.

and the reported value for the minimum angle is 0

Thanks in advance for any suggestions or examples,
John Gonzalez


#2

If I had to hazard a guess, the version of the message received by your program is different from the version used during compilation.

Can you provide the following information:

  • What version of gazebo are your using?
  • Do you have multiple versions of Gazebo installed?
  • Can you include a complete working example so that we can reproduce the problem?

#3
--------------------------------------------------------------------------------------------
Hello,

I am using Gazebo 7.12.0 in ubuntu 14.04.5 this is the only version installed in my laptop.
here is the ,world file
--------------------------------------------------------------------------------------------


<sdf version='1.6'>
  <world name='default'>
    <light name='sun' type='directional'>
      <cast_shadows>1</cast_shadows>
      <pose frame=''>0 0 10 0 -0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>
    <model name='ground_plane'>
      <static>1</static>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>50</mu2>
              </ode>
              <torsional>
                <ode/>
              </torsional>
            </friction>
            <contact>
              <ode/>
            </contact>
            <bounce/>
          </surface>
          <max_contacts>10</max_contacts>
        </collision>
        <visual name='visual'>
          <cast_shadows>0</cast_shadows>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
          </material>
        </visual>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
      </link>
    </model>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type='adiabatic'/>
    <physics name='default_physics' default='0' type='ode'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>1</shadows>
    </scene>
    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <latitude_deg>0</latitude_deg>
      <longitude_deg>0</longitude_deg>
      <elevation>0</elevation>
      <heading_deg>0</heading_deg>
    </spherical_coordinates>
    <model name='hokuyo'>
      <pose frame=''>0.371413 -2.43883 0.035 0 -0 0</pose>
      <link name='link'>
        <inertial>
          <mass>0.1</mass>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
        </inertial>
        <visual name='visual'>
          <geometry>
            <mesh>
              <uri>model://hokuyo/meshes/hokuyo.dae</uri>
            </mesh>
          </geometry>
        </visual>
        <collision name='collision-base'>
          <pose frame=''>0 0 -0.0145 0 -0 0</pose>
          <geometry>
            <box>
              <size>0.05 0.05 0.041</size>
            </box>
          </geometry>
          <max_contacts>10</max_contacts>
          <surface>
            <contact>
              <ode/>
            </contact>
            <bounce/>
            <friction>
              <torsional>
                <ode/>
              </torsional>
              <ode/>
            </friction>
          </surface>
        </collision>
        <collision name='collision-top'>
          <pose frame=''>0 0 0.0205 0 -0 0</pose>
          <geometry>
            <cylinder>
              <radius>0.021</radius>
              <length>0.029</length>
            </cylinder>
          </geometry>
          <max_contacts>10</max_contacts>
          <surface>
            <contact>
              <ode/>
            </contact>
            <bounce/>
            <friction>
              <torsional>
                <ode/>
              </torsional>
              <ode/>
            </friction>
          </surface>
        </collision>
        <sensor name='laser' type='ray'>
          <pose frame=''>0.01 0 0.0175 0 -0 0</pose>
          <ray>
            <scan>
              <horizontal>
                <samples>640</samples>
                <resolution>1</resolution>
                <min_angle>-2.26889</min_angle>
                <max_angle>2.2689</max_angle>
              </horizontal>
              <vertical>
                <samples>1</samples>
                <min_angle>0</min_angle>
                <max_angle>0</max_angle>
              </vertical>
            </scan>
            <range>
              <min>0.08</min>
              <max>10</max>
              <resolution>0.01</resolution>
            </range>
          </ray>
          <plugin name='laser' filename='libRayPlugin.so'/>
          <always_on>1</always_on>
          <update_rate>30</update_rate>
          <visualize>1</visualize>
        </sensor>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
      </link>
    </model>
    <model name='unit_cylinder'>
      <pose frame=''>2.27756 -1.73568 0.5 0 -0 0</pose>
      <link name='link'>
        <inertial>
          <mass>1</mass>
          <inertia>
            <ixx>0.145833</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.145833</iyy>
            <iyz>0</iyz>
            <izz>0.125</izz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <cylinder>
              <radius>0.5</radius>
              <length>1</length>
            </cylinder>
          </geometry>
          <max_contacts>10</max_contacts>
          <surface>
            <contact>
              <ode/>
            </contact>
            <bounce/>
            <friction>
              <torsional>
                <ode/>
              </torsional>
              <ode/>
            </friction>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <cylinder>
              <radius>0.5</radius>
              <length>1</length>
            </cylinder>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Grey</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
      </link>
    </model>
    <state world_name='default'>
      <sim_time>118 798000000</sim_time>
      <real_time>119 248700107</real_time>
      <wall_time>1543509603 559096777</wall_time>
      <iterations>118798</iterations>
      <model name='ground_plane'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <scale>1 1 1</scale>
        <link name='link'>
          <pose frame=''>0 0 0 0 -0 0</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
      </model>
      <model name='hokuyo'>
        <pose frame=''>0.371414 -2.43883 0.035 -1e-06 1.5e-05 0</pose>
        <scale>1 1 1</scale>
        <link name='link'>
          <pose frame=''>0.371414 -2.43883 0.035 -1e-06 1.5e-05 0</pose>
          <velocity>1e-05 -5e-06 4e-06 0.000135 0.000289 0</velocity>
          <acceleration>-0.000857 -0.000857 -0.001225 0.024494 -0.024493 0</acceleration>
          <wrench>-8.6e-05 -8.6e-05 -0.000122 0 -0 0</wrench>
        </link>
      </model>
      <model name='unit_cylinder'>
        <pose frame=''>2.27756 -1.73568 0.499997 3e-06 4e-06 -0</pose>
        <scale>1 1 1</scale>
        <link name='link'>
          <pose frame=''>2.27756 -1.73568 0.499997 3e-06 4e-06 -0</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>0 0 -9.8 0 -0 0</acceleration>
          <wrench>0 0 -9.8 0 -0 0</wrench>
        </link>
      </model>
      <light name='sun'>
        <pose frame=''>0 0 10 0 -0 0</pose>
      </light>
    </state>
    <gui fullscreen='0'>
      <camera name='user_camera'>
        <pose frame=''>5 -5 2 0 0.275643 2.35619</pose>
        <view_controller>orbit</view_controller>
        <projection_type>perspective</projection_type>
      </camera>
    </gui>
  </world>
</sdf>
--------------------------------------------------------------------------------------------
and here is the program I am using which is based on the listener program found in the tutorials:
---------------------------------------------------------------------------------------------
#include <boost/bind.hpp>
#include <stdio.h>
#include <iostream>
#include <stdlib.h>

#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/math/gzmath.hh>
#include <gazebo/gazebo_client.hh>
#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>
#include <gazebo/plugins/RayPlugin.hh>

#include <dds/core/ddscore.hpp>
#include <dds/pub/ddspub.hpp>
#include <dds/sub/ddssub.hpp>

#include "IDATypes.hpp"

double CalculatedVelocity_x;
double AngularVelocity;

struct initial_position
       {
	     double x;
	     double y;
		 double z;
       };

struct final_position
       {
	     double x;
	     double y;
		 double z;
       };

struct calculated_velocity
       {
	     double x;
	     double y;
		 double z;
       };

initial_position Pos_ini{0,0,0};


dds::domain::DomainParticipant participant(1);
dds::pub::Publisher publisher(participant);

//dds::pub::DataWriter<ida::Velocity> VelocityFromGazebo_writer(publisher, dds::topic::Topic<ida::Velocity>(participant, "Velocity_act"));
//ida::Velocity VelocityFromGazebo_msg;

dds::pub::DataWriter<ida::BoschPose> PositionFromGazebo_writer(publisher, dds::topic::Topic<ida::BoschPose>(participant, "truck_pose"));
ida::BoschPose PositionFromGazebo_msg;

/////////////////// joint//////////////////////////////
// input: ConstPosesStampedPtr			 			//
// ouput: Void					 					//
// Comments: This String is updated every time a 	//
//           message is received		 			//
//////////////////////////////////////////////////////
void joint(ConstJointPtr &_msg)
{
  // Dump the message contents to stdout.
  //std::cout << _msg->DebugString();
}

/////////////// model_info////////////////////////////
// input: ConstPosesStampedPtr			 			//
// ouput: Void					 					//
// Comments: This String is updated every time a 	//
//           message is received		 			//
//////////////////////////////////////////////////////
void model_info(ConstModelPtr &_msg)
{
  // Dump the message contents to stdout.
  //std::cout << _msg->DebugString();
}

////////// model_modify///////////////////////////////
// input: ConstPosesStampedPtr			 			//
// ouput: Void					 					//
// Comments: This String is updated every time a 	//
//           message is received		 			//
//////////////////////////////////////////////////////
void model_modify(ConstModelPtr &_msg)
{
  // Dump the message contents to stdout.
  //std::cout << _msg->DebugString();
}

////////////////// physics////////////////////////////
// input: ConstPosesStampedPtr			 			//
// ouput: Void					 					//
// Comments: This String is updated every time a 	//
//           message is received		 			//
//////////////////////////////////////////////////////
void physics(ConstPhysicsPtr &_msg)
{
  // Dump the message contents to stdout.
  //std::cout << _msg->DebugString();
}

////////// pose_local_info////////////////////////////
// input: ConstPosesStampedPtr			 			//
// ouput: Void					 					//
// Comments: This String is updated every time a 	//
//           message is received		 			//
//////////////////////////////////////////////////////
void pose_local_info(ConstPosesStampedPtr &_msg)
{
  // Dump the message contents to stdout.
  //std::cout << _msg->DebugString();
}

//////////publishDDSPosition /////////////////////////
// input: const ::gazebo::msgs::Vector3d &position	//
// ouput: Void					 					//
// Comments: Take the position message and publish	//
//			  as a DDS  message			 			//
//////////////////////////////////////////////////////
void publishDDSPosition(const ::gazebo::msgs::Vector3d &position, double &yaw)
{

   double x = position.x();
   double y = position.y();


		PositionFromGazebo_msg.pose().x(x);
		PositionFromGazebo_msg.pose().y(y);
		PositionFromGazebo_msg.pose().theta()=yaw;

		PositionFromGazebo_writer.write(PositionFromGazebo_msg);

}

////////// pose_info//////////////////////////////////
// input: ConstPosesStampedPtr			 			//
// ouput: Void					 					//
// Comments: This is a callback function printing	//
//			 messages to the console			  	//
//////////////////////////////////////////////////////
void pose_info(ConstPosesStampedPtr &_msg)
{
	//std::cout << _msg->DebugString(); // Dump model information in the console

	::google::protobuf::int32 sec =  _msg->time().sec();
	::google::protobuf::int32 nsec = _msg->time().nsec();
	// std::cout << "Read time: sec: " << sec << " nsec: " << nsec << std::endl;

	 for (int i =0; i < _msg->pose_size(); ++i)
	 {
		 const ::gazebo::msgs::Pose &pose = _msg->pose(i);
		 std::string name = pose.name();

		 if (name == std::string("LT_improved_model_v11"))
		 {
		   const ::gazebo::msgs::Vector3d &position = pose.position();

		   double x = position.x();
		   double y = position.y();
		   double z = position.z();

		   std::cout << "Read position: x: " << x << " y: " << y << " z: " << z << std::endl;

		   if (name == std::string("LT_improved_model_v11"))
			{
				 ::gazebo::math::Quaternion pOrientation = ::gazebo::math::Quaternion(pose.orientation().w(),pose.orientation().x(), pose.orientation().y(), pose.orientation().z());

				 //double roll  = pOrientation.GetRoll();
				 //double pitch = pOrientation.GetPitch();
				 double yaw   = pOrientation.GetYaw();

				 //std::cout << "Read orientation: roll: " << roll << " pitch: " << pitch << " yaw: " << yaw << std::endl;
				 //std::cout << "Read orientation: yaw: " << yaw << std::endl;
				 sleep(0.5);

				 publishDDSPosition(position,yaw);
			}

		 }

	 }

}

////////// laser_scan//////////////////////////////////
// input: ConstlaserScanPtr			 	 			//
// ouput: Void					 					//
// Comments: Updated every time a message is 		//
//			 received								//
//			 messages to the console			  	//
//////////////////////////////////////////////////////
void lidar_info(ConstLaserScanPtr &_msg)
{
//	std::cout << _msg->DebugString();
//	std::cout<<_msg->LaserScan::DebugString();

//	double dMinimum_range;
//	double dMaximum_range;

	double dAngle_minimum;
//	double dAngle_maximum;

	dAngle_minimum = _msg->angle_min();
	std::cout << dAngle_minimum<<std::endl;
//	std::cout << _msg->has_angle_min() <<std::endl;
//
//	dAngle_maximum = _msg->angle_max();
//	std::cout << dAngle_maximum<<std::endl;
//
//
//	dMinimum_range =_msg->range_min();
//	std::cout << dMinimum_range<<std::endl;
//
//	dMaximum_range = _msg->LaserScan::angle_max();
//	std::cout << _msg->ranges().size()<<std::endl;

}

////////////// World_stats////////////////////////////
// input: ConstPosesStampedPtr			 			//
// ouput: Void					 					//
// Comments: This String is updated every time a 	//
//           message is received		 			//
//////////////////////////////////////////////////////
void world_stats(ConstWorldStatisticsPtr &_msg)
{
  // Dump the message contents to stdout.
  //std::cout << _msg->DebugString();

}


int main(int _argc, char **_argv)
{


	// Load gazebo
	gazebo::client::setup(_argc, _argv);

	// Create our node for communication
	gazebo::transport::NodePtr node(new gazebo::transport::Node());
	node->Init();

  	// listen to Gazebo joint topic
  	//gazebo::transport::SubscriberPtr sub1 = node->Subscribe("~/joint", joint);

  	// Listen to Gazebo model topic
  	//gazebo::transport::SubscriberPtr sub2 = node->Subscribe("~/model/info", model_info);
  	//gazebo::transport::SubscriberPtr sub3 = node->Subscribe("~/model/modify", model_modify);

  	// Listen to Gazebo physics topic
  	//gazebo::transport::SubscriberPtr sub4 = node->Subscribe("~/physics", physics);

  	// Listen to Gazebo Pose topic
  	//gazebo::transport::SubscriberPtr sub5 = node->Subscribe("~/pose/local/info", pose_local_info);

  	// Listen to Gazebo Pose topic
  	gazebo::transport::SubscriberPtr sub6 = node->Subscribe("~/pose/info", pose_info);


  	// Listen to Gazebo world_stats topic
  	//gazebo::transport::SubscriberPtr sub7 = node->Subscribe("~/world_stats", world_stats);

  	// Listen to Gazebo Pose topic
  	//gazebo::transport::SubscriberPtr sub8 = node->Subscribe("~/LiftTruckWithSensor_v2/link_1/laser/scan", lidar_info);
  	gazebo::transport::SubscriberPtr sub8 = node->Subscribe("~/hokuyo/link/laser/scan", lidar_info);

  	// Busy wait loop done in order to listen to gazebo messages recursively
   	while (true)
  	{
  		gazebo::common::Time::MSleep(1000);
  	}

  // Make sure to shut everything down.
  gazebo::client::shutdown();
}

--------------------------------------------

Best regards,
John Gonzalez

--------------------------------------------