gps hector gazebo

edit

I have a lot of troubles to add one module of gps in the robot description.

I find the code in hector_gazebo_plugins/src/gazebo_ros_gps.cpp

I want to use it and add by the next code lines in my robot description:

<joint name="gps_joint" type="fixed">
  <origin xyz="0 0 0"/>
  <parent link="base_link"/>
  <child link="gps_link"/>
</joint>

<link name="gps_link">
  <inertial>
    <mass value="0.001" />
    <origin xyz="0 0 0" rpy="0 0 0" />
    <inertia ixx="0.000000017" ixy="0" ixz="0" iyy="0.000000017" iyz="0" izz="0.000000017" />
  </inertial>
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <geometry>
      <box size="0.01 0.01 0.01" />          
    </geometry>
  </visual>
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <geometry>
      <box size="0.01 0.01 0.01" />
    </geometry>
  </collision>
</link>


<gazebo>      
  <controller:hector_gazebo_ros_gps name="quadrotor_gps_sim" plugin="libhector_gazebo_ros_test.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>4.0</updateRate>
    <bodyName>gps_link</bodyName>
    <topicName>fix</topicName>
    <velocityTopicName>fix_vel</velocityTopicName>
    <drift>5.0 5.0 5.0</drift>
    <gaussianNoise>0.1 0.1 0.1</gaussianNoise>
    <velocityDrift>0 0 0</velocityDrift>
    <velocityGaussianNoise>0.1 0.1 0.1</velocityGaussianNoise>
<!--interface:position name="gps_position"/-->
  </controller:hector_gazebo_ros_gps>
</gazebo>

When I launch the robot, all works fine except this plugin. I am debugging this source and I notice the error in this line:

if (!_sdf->HasElement("bodyName"))
  {
    link = _model->GetLink();
    link_name_ = link->GetName();
  }
  else {
    link_name_ = _sdf->GetElement("bodyName")->GetValueString();
    link = boost::shared_dynamic_cast<physics::Link>(world->GetEntity(link_name_));
  }
  if (!link)
  {
    ROS_FATAL("GazeboRosTest plugin error: bodyName: %s does not exist\n", link_name_.c_str());
    return;
  }

The return of HasElement is 1 and go inside the else, then, link is empty and return the error, why the link is empty? I write 1 more lines to get

ROS_INFO("bodyName: %s" link_name_.c_str());

And the result is gps_link. In my robot description tf there is indeed gps_link I don't have any clues to continue.

Any ideas? Where Can I find some example to use in the simulator with quadrotor or ugv hector?

Thank you very much.