Robotics StackExchange | Archived questions

Hector sonar implementation on a Pioneer 3DX

Hello,

I'm working on a simulation of two Pioneer-3DX robots on Gazebo. (Debian Stretch / Gazebo 7 / ROS Kinetic)

I'm trying to implement sonar readings with Hector_sonar (to make one robot follow the other) but it seems like it doesn't work. The problem I face is that I don't get any range/sonar display on rviz and the range I get from the sonar is always the same, even when the p3dx is alone with nothing around it (range moves between 0.04 and 0.05 which is not really what I would expect).

Here's how I did :

I cloned in my catkinws/src : https://github.com/tu-darmstadt-ros-pkg/hectormodels

I used https://github.com/tu-darmstadt-ros-pkg/hector_models/blob/groovy-devel/hector_sensors_description/urdf/sonar_sensor.urdf.xacro from hectormodels package to make my pioneer3dxsonar.xacro :

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <property name="M_PI" value="3.1415926535897931" />

  <xacro:macro name="pioneer_sonar" params="name parent xyz rpy ros_topic update_rate min_range max_range field_of_view ray_count meshes">

  <joint name="{name}_joint" type="fixed">
    <origin xyz="${xyz}" rpy="${rpy}"/>
    <parent link="${parent}"/>
    <child link="${name}_link"/>
  </joint>
  <link name="${name}_link">
    <inertial>
      <mass value="0.0001"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
    </inertial>
    <visual name="front_sonar_vis">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry name="pioneer_geom">
        <mesh filename="${meshes}/back_sonar.stl"/>
      </geometry>
      <material name="SonarYellow">
        <color rgba="0.715 0.583 0.210 1.0"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>
  </link>

   <gazebo reference="${name}_link">
      <sensor type="ray" name="${name}">
        <always_on>true</always_on>
        <update_rate>${update_rate}</update_rate>
        <pose>0 0 0 0 0 0</pose>
        <visualize>false</visualize>
        <ray>
          <scan>
            <horizontal>
              <samples>${ray_count}</samples>
              <resolution>1</resolution>
              <min_angle>-${field_of_view/2}</min_angle>
              <max_angle> ${field_of_view/2}</max_angle>
            </horizontal>
            <vertical>
              <samples>${ray_count}</samples>
              <resolution>1</resolution>
              <min_angle>-${field_of_view/2}</min_angle>
              <max_angle> ${field_of_view/2}</max_angle>
            </vertical>
          </scan>
          <range>
            <min>${min_range}</min>
            <max>${max_range}</max>
            <resolution>0.01</resolution>
          </range>
        </ray>

        <plugin name="gazebo_ros_${name}_controller" filename="libhector_gazebo_ros_sonar.so">
          <gaussianNoise>0.005</gaussianNoise>
          <topicName>${ros_topic}</topicName>
          <frameId>${name}_link</frameId>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>
</robot>

Ten I added into my pioneer3dx.xacro a line to call the previous file :

<xacro:include filename="$(find p3dx_robot)/xacro/p3dx/pioneer3dx_sonar.xacro">
<xacro:pioneer_sonar 
  name="sonar" 
  parent="base_link"
  xyz="0.109 0 0.209"
  rpy="0 0 0"
  ros_topic="sonar_front" 
  update_rate="10" 
  min_range="0.01" 
  max_range="5.0" 
  field_of_view="0.01745329251" 
  ray_count="1"
  meshes="package://p3dx_robot/meshes/p3dx"/>

And I obviously deleted how the sonar was defined before. (There was only a model with joint and link and nothing to actually make use of the sonar)

If someone could figure out how to make this work with these informations, I'd be really thankful. If you need to know more and these aren't enough, just let me know.

Hdifsttar

Edit 1 : I changed the min value of the sonar range(=0.7) and the range changed to oscillate between the min range(=0.7) and the min range +0.04 (=0.74). Seems like the data I get from the sonar is only the gaussian noise produced in the plugin. So, I think I get 0 from the sonar and the rest is up to the noise.

Edit 2: I changed the min value and i still got 0.7 from the sonar. The value I got from the sonar (0.74) and the value min I set (0.7) was just a coincidence. The sonar is now working just because i changed the min value from 0.01 to 0.08. Don't know how to explain that. I still can't see any sonar on Rviz though.

Edit 3: I fixed Rviz, it was just a mistake of topic selected, my bad. I'm closing the topic since I got my answers, feel free to pm me if you ever encounter any problems like that, or want to do the same thing.

Asked by Hdifsttar on 2017-06-01 05:54:21 UTC

Comments

Answers