John
November 29, 2018, 11:32am
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///////////////////////////////////////////
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.
and the reported value for the minimum angle is 0
Thanks in advance for any suggestions or examples,
nate
November 29, 2018, 3:50pm
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?
John
January 11, 2019, 9:54pm
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
--------------------------------------------
Try to use ConstLaserScanStampedPtr instead of ConstLaserScanPtr