Home | Tutorials | Wiki | Issues
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Access Joint from Sensor Element to Detect Full LIDAR Rotation

I've been working on a sensor plugin for a LIDAR sensor simulation. We added a plugin element nested in a sensor element. However, we need to access a joint that is also in the model. We want to know when the LIDAR makes a full 360 degree rotation around it's rotating axis. Our sdf model is the following: <sdf version="1.5">

<model name="RPLidar-A2">
  <!-- Give the base link a unique name -->
  <link name="base">
    <!-- added to make sure model doesn't move, should be remomved later when added to full robot-->
    <gravity>0</gravity>
    <pose>0 0 0 0 0 0</pose>
    <inertial>
      <mass>1.2</mass>
      <inertia>
        <ixx>0.001087473</ixx>
        <iyy>0.001087473</iyy>
        <izz>0.001092437</izz>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyz>0</iyz>
      </inertia>
    </inertial>


    <!-- Offset the base by half the length of the cylinder -->
    <collision name="base_collision">
      <pose>0 0 0.0097 0 0 0</pose>
      <geometry>
        <cylinder>
          <!-- Radius and length provided by Velodyne -->
          <radius>.03785</radius>
          <length>.01940</length>
        </cylinder>
      </geometry>
    </collision>

    <visual name="base_visual">
      <geometry>
        <mesh>
          <uri>model://rplidar/meshes/rplidar_base.dae</uri>
        </mesh>
      </geometry>
    </visual>
  </link>

  <!-- Give the base link a unique name -->
  <link name="top">

    <!-- See abouve about gravity--> 
    <gravity>0</gravity>

    <!-- Add a ray sensor, and give it a name -->
    <sensor type="ray" name="sensor">
      <plugin name="rplidar_sensor" filename="libsensor_plugin.so"></plugin>

      <!-- Position the ray sensor based on the specification. Also rotate
           it by 90 degrees around the X-axis so that the <horizontal> rays
           become vertical -->
      <pose>0 0 0.0107 1.5707 0 0</pose>

      <!-- Enable visualization to see the rays in the GUI -->
      <visualize>true</visualize>

      <!-- Set the update rate of the sensor -->
      <update_rate>4000</update_rate>

      <ray>
        <!-- The scan element contains the horizontal and vertical beams.
             We are leaving out the vertical beams for this tutorial. -->
        <scan>
          <!-- The horizontal beams -->
          <horizontal>
            <!-- The RPLidar has 1 beam(sample) -->
            <samples>1</samples>

            <!-- Resolution is multiplied by samples to determine number of
                 simulated beams vs interpolated beams. See:
                 http://sdformat.org/spec?ver=1.6&elem=sensor#horizontal_resolution
                 -->
            <resolution>1</resolution>

            <!-- Minimum angle in radians -->
            <min_angle>0</min_angle>

            <!-- Maximum angle in radians -->
            <max_angle>0</max_angle>
          </horizontal>
        </scan>

        <!-- Range defines characteristics of an individual beam -->
        <range>

          <!-- Minimum distance of the beam -->
          <min>0.15</min>

          <!-- Maximum distance of the beam -->
          <max>6</max>

          <!-- Linear resolution of the beam -->
          <resolution>0.0001</resolution>
        </range>
      </ray>

    </sensor>

    <pose>0 0 0.01940 0 0 0</pose>
    <inertial>
      <mass>0.1</mass>
      <inertia>
        <ixx>0.000090623</ixx>
        <iyy>0.000090623</iyy>
        <izz>0.000091036</izz>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyz>0</iyz>
      </inertia>
    </inertial>
    <!-- Vertically offset the top cylinder by the length of the bottom
        cylinder and half the length of this cylinder. -->
    <collision name="top_collision">
      <pose>0 0 0.0107 0 0 0</pose>
      <geometry>
        <cylinder>
          <!-- Radius and length provided by Velodyne -->
          <radius>0.03625</radius>
          <length>0.02140</length>
        </cylinder>
      </geometry>
    </collision>

    <visual name="top_visual">
      <geometry>
        <mesh>
          <uri>model://rplidar/meshes/rplidar_top.dae</uri>
        </mesh>
      </geometry>
    </visual>
  </link>

  <!-- Each joint must have a unique name -->
  <joint type="revolute" name="joint">
    <pose>0 0 -0.036785 0 0 0</pose>
    <parent>base</parent>
    <child>top</child>
    <axis>
      <xyz>0 0 1</xyz> 
      <limit>
        <lower>-10000000000000000</lower>
        <upper>10000000000000000</upper>
      </limit>
    </axis>
  </joint>

</model>

</sdf>

First, we tried using the Pose from the sensor pointer, but that provided values that did not change. We did this through accessing the pose through sdf::SensorPtr ParentPointer, receiving the position vector from ParentPointer->Pose()->pos() and the rotation quaternion from ParentPointer->Pose()->rot(). However, these displayed constant values even with different rotation speeds, thus this did not help us to figure out when the RPLidar made a full rotation.

We also tried to use the parent of the given sdf::ElementPtr _sdf. We were able to return "sensor" when calling printf("%s\n", _sdf->GetParent()->GetName()). However, if we tried to look up one more level by writing _sdf->GetParent()->GetParent() we got a segmentation fault.

We want to find a way to access the joint to find its pose in order to tell when a full rotation is completed. What other ways can we approach this problem?

Access Joint from Sensor Element to Detect Full LIDAR Rotation

I've been working on a sensor plugin for a LIDAR sensor simulation. We added a plugin element nested in a sensor element. However, we need to access a joint that is also in the model. We want to know when the LIDAR makes a full 360 degree rotation around it's rotating axis. Our sdf model is the following: <sdf version="1.5">

<sdf version="1.5">

<model name="RPLidar-A2">
  <!-- Give the base link a unique name -->
  <link name="base">
    <!-- added to make sure model doesn't move, should be remomved later when added to full robot-->
    <gravity>0</gravity>
    <pose>0 0 0 0 0 0</pose>
    <inertial>
      <mass>1.2</mass>
      <inertia>
        <ixx>0.001087473</ixx>
        <iyy>0.001087473</iyy>
        <izz>0.001092437</izz>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyz>0</iyz>
      </inertia>
    </inertial>


    <!-- Offset the base by half the length of the cylinder -->
    <collision name="base_collision">
      <pose>0 0 0.0097 0 0 0</pose>
      <geometry>
        <cylinder>
          <!-- Radius and length provided by Velodyne -->
          <radius>.03785</radius>
          <length>.01940</length>
        </cylinder>
      </geometry>
    </collision>

    <visual name="base_visual">
      <geometry>
        <mesh>
          <uri>model://rplidar/meshes/rplidar_base.dae</uri>
        </mesh>
      </geometry>
    </visual>
  </link>

  <!-- Give the base link a unique name -->
  <link name="top">

    <!-- See abouve about gravity--> 
    <gravity>0</gravity>

    <!-- Add a ray sensor, and give it a name -->
    <sensor type="ray" name="sensor">
      <plugin name="rplidar_sensor" filename="libsensor_plugin.so"></plugin>

      <!-- Position the ray sensor based on the specification. Also rotate
           it by 90 degrees around the X-axis so that the <horizontal> rays
           become vertical -->
      <pose>0 0 0.0107 1.5707 0 0</pose>

      <!-- Enable visualization to see the rays in the GUI -->
      <visualize>true</visualize>

      <!-- Set the update rate of the sensor -->
      <update_rate>4000</update_rate>

      <ray>
        <!-- The scan element contains the horizontal and vertical beams.
             We are leaving out the vertical beams for this tutorial. -->
        <scan>
          <!-- The horizontal beams -->
          <horizontal>
            <!-- The RPLidar has 1 beam(sample) -->
            <samples>1</samples>

            <!-- Resolution is multiplied by samples to determine number of
                 simulated beams vs interpolated beams. See:
                 http://sdformat.org/spec?ver=1.6&elem=sensor#horizontal_resolution
                 -->
            <resolution>1</resolution>

            <!-- Minimum angle in radians -->
            <min_angle>0</min_angle>

            <!-- Maximum angle in radians -->
            <max_angle>0</max_angle>
          </horizontal>
        </scan>

        <!-- Range defines characteristics of an individual beam -->
        <range>

          <!-- Minimum distance of the beam -->
          <min>0.15</min>

          <!-- Maximum distance of the beam -->
          <max>6</max>

          <!-- Linear resolution of the beam -->
          <resolution>0.0001</resolution>
        </range>
      </ray>

    </sensor>

    <pose>0 0 0.01940 0 0 0</pose>
    <inertial>
      <mass>0.1</mass>
      <inertia>
        <ixx>0.000090623</ixx>
        <iyy>0.000090623</iyy>
        <izz>0.000091036</izz>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyz>0</iyz>
      </inertia>
    </inertial>
    <!-- Vertically offset the top cylinder by the length of the bottom
        cylinder and half the length of this cylinder. -->
    <collision name="top_collision">
      <pose>0 0 0.0107 0 0 0</pose>
      <geometry>
        <cylinder>
          <!-- Radius and length provided by Velodyne -->
          <radius>0.03625</radius>
          <length>0.02140</length>
        </cylinder>
      </geometry>
    </collision>

    <visual name="top_visual">
      <geometry>
        <mesh>
          <uri>model://rplidar/meshes/rplidar_top.dae</uri>
        </mesh>
      </geometry>
    </visual>
  </link>

  <!-- Each joint must have a unique name -->
  <joint type="revolute" name="joint">
    <pose>0 0 -0.036785 0 0 0</pose>
    <parent>base</parent>
    <child>top</child>
    <axis>
      <xyz>0 0 1</xyz> 
      <limit>
        <lower>-10000000000000000</lower>
        <upper>10000000000000000</upper>
      </limit>
    </axis>
  </joint>

</model>

</sdf>

</sdf>

First, we tried using the Pose from the sensor pointer, but that provided values that did not change. We did this through accessing the pose through sdf::SensorPtr ParentPointer, receiving the position vector from ParentPointer->Pose()->pos() and the rotation quaternion from ParentPointer->Pose()->rot(). However, these displayed constant values even with different rotation speeds, thus this did not help us to figure out when the RPLidar made a full rotation.

We also tried to use the parent of the given sdf::ElementPtr _sdf. We were able to return "sensor" when calling printf("%s\n", _sdf->GetParent()->GetName()). However, if we tried to look up one more level by writing _sdf->GetParent()->GetParent() we got a segmentation fault.

We want to find a way to access the joint to find its pose in order to tell when a full rotation is completed. What other ways can we approach this problem?