Hector sonar implementation on a Pioneer 3DX [closed]

edit

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 catkin_ws/src : https://github.com/tu-darmstadt-ros-p...

I used https://github.com/tu-darmstadt-ros-p... from hector_models package to make my pioneer3dx_sonar.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 ... (more)