Robotics StackExchange | Archived questions

Making a differential drive robot move

Hello all,

I am new to Gazebo in general and I require some assistance. I have the following .xacro file that describes links and joints of a differential drive robot. I need to move this robot around in ROS and I found the differentialdrivecontroller (libgazeborosdiffdrive.so) plugin. However, when I launch the simulation and although I publish a message to the /trs/cmdvel topic (rostopic pub -1 /trs/cmdvel geometrymsgs/Twist '{linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.0}}') the robot is not moving. Can someone help me on how to fix this?

<?xml version="1.0"?> <!-- Import Gazebo-specific properties --> <!-- Import helper macros for calculating inertia tensors -->

<!-- Helper xacro macro for calculating inertia tensors of cylindrical wheels --> /xacro:macro

<link name="${prefix}chassis_link">
  <inertial>
    <mass value="120.911"/>
    <origin rpy="0 0 0" xyz="-0.1832 0.0 0.15"/>
    <inertia ixx="3.50889411882" ixy="0.0" ixz="0.0" iyy="9.06900169571" iyz="0.0" izz="11.7718257055"/>
  </inertial>
  <collision name="base">
    <origin rpy="0.0 0.0 1.57" xyz="-0.2032 0.0 0.15"/>
    <geometry>
      <box size="0.3652 0.8374 0.15"/>
    </geometry>
  </collision>
  <collision name="front_sensor_bar_high">
    <origin xyz="0.1026 0.0 0.20"/>
    <geometry>
      <cylinder length=".075" radius=".2826"/>
    </geometry>
  </collision>
  <collision name="front_sensor_bar_low">
    <origin xyz="0.1026 0.0 0.075"/>
    <geometry>
      <cylinder length=".075" radius=".2826"/>
    </geometry>
  </collision>
  <collision name="rear_sensor_bar_high">
    <origin xyz="-0.498 0.0 0.20"/>
    <geometry>
      <cylinder length=".075" radius=".2826"/>
    </geometry>
  </collision>
  <collision name="rear_sensor_bar_low">
    <origin xyz="-0.498 0.0 0.075"/>
    <geometry>
      <cylinder length=".075" radius=".2826"/>
    </geometry>
  </collision>
  <!-- This model was changed to have a single central caster to remove a contact point and reduce the qcbot model jitter. \
    This change was based on successful, jitter-reducing changes to the RPR model, and is partially made of black magic. -->
  <collision name="center_caster_collision">
    <origin xyz="-0.37 0.0 0.1018"/>
    <geometry>
      <sphere radius=".1018"/>
    </geometry>
  </collision>
  <visual>
    <origin rpy="1.5708 0 1.5708" xyz="0 0 0.2159"/>
    <!-- uncomment to use a lower-resolution stl which includes the bottom of qcbot and its wheels -->
    <geometry>
      <mesh filename="package://trs/meshes/base.stl" scale="0.001 0.001 0.001"/>
    </geometry>
    <!-- uncomment to use a higher-resolution stl which does not include the bottom of qcbot or its wheels -->
    <material name="LightGrey">
      <color rgba="0.4 0.4 0.4 1.0"/>
    </material>
  </visual>


</link>

<link name="${prefix}left_wheel_link">
  <inertial>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <xacro:inertia_cylinder mass="2" radius="0.1018" length="0.04"/>
  </inertial>
  <collision name="collision">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <cylinder radius=".1018" length=".04"/>
    </geometry>
  </collision>
  <visual name="left_wheel_visual">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <cylinder radius=".1018" length=".04"/>
    </geometry>
  </visual>
</link>
<joint type="continuous" name="${prefix}left_wheel_joint">
  <origin rpy="-1.5707 0 0" xyz="0.0 0.23 0.1018"/>
  <axis xyz="0 0 1"/>
  <parent
    link="${prefix}chassis_link" />
  <child
    link="${prefix}left_wheel_link" />
  <limit effort="1000" velocity="1000000000" />
</joint>

<link name="${prefix}right_wheel_link">
  <inertial>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <xacro:inertia_cylinder mass="2" radius="0.1018" length="0.04"/>
  </inertial>
  <collision name="collision">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <cylinder radius=".1018" length=".04"/>
    </geometry>
  </collision>
  <visual name="right_wheel_visual">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <cylinder radius=".1018" length=".04"/>
    </geometry>
  </visual>
</link>
<joint type="continuous" name="${prefix}right_wheel_joint">
  <origin xyz="0.0 -0.23 0.1018" rpy="1.5707 0 0"/>
  <parent
    link="${prefix}chassis_link" />
  <child
    link="${prefix}right_wheel_link" />
  <axis
    xyz="0 0 -1" />
  <limit effort="1000" velocity="1000000000" />
</joint>

<!-- Currently, only data used from transmission tags is joint name. -->
<transmission name="tran_left_wheel_joint">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="${prefix}left_wheel_joint">
    <hardwareInterface>EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="motor_left_wheel_joints">
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>

<transmission name="tran_right_wheel_joint">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="${prefix}right_wheel_joint">
    <hardwareInterface>EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="motor_right_wheel_joints">
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>


<!-- Simulation dynamics parameters -->
<gazebo reference="${prefix}chassis_link">
  <mu1>0</mu1>
  <mu2>0</mu2>
  <kp value="1.0e+9"/>
  <kd value="5.0e+6"/>
</gazebo>

<gazebo reference="${prefix}right_wheel_link">
  <mu1 value="1.0e+6"/>
  <mu2 value="1.0e+6"/>
  <kp value="1.0e+9"/>
  <kd value="5.0e+6"/>
</gazebo>

<gazebo reference="${prefix}left_wheel_link">
  <mu1 value="1.0e+6"/>
  <mu2 value="1.0e+6"/>
  <kp value="1.0e+9"/>
  <kd value="5.0e+6"/>
</gazebo>

 <gazebo>
  <plugin name='differential_drive_controller'
          filename='libgazebo_ros_diff_drive.so'>
    <publishWheelTF>false</publishWheelTF>
    <robotNamespace>/trs</robotNamespace>
    <publishTf>1</publishTf>
    <publishWheelJointState>false</publishWheelJointState>
    <alwaysOn>true</alwaysOn>
    <updateRate>100</updateRate>
    <leftJoint>${prefix}left_wheel_joint</leftJoint>
    <rightJoint>${prefix}right_wheel_joint</rightJoint>
    <wheelSeparation>0.46</wheelSeparation>
    <wheelDiameter>0.2036</wheelDiameter>
    <broadcastTF>1</broadcastTF>
    <torque>5</torque>
    <commandTopic>cmd_vel</commandTopic>
    <odometryTopic>odom</odometryTopic>
    <odometryFrame>odom</odometryFrame>
    <robotBaseFrame>${prefix}chassis_link</robotBaseFrame>
  </plugin>
</gazebo>

Asked by zoid on 2018-11-10 13:50:21 UTC

Comments

Answers

It looks live you've made the URDF for this okay, but you'll need to start a few more components to be able to drive this with the /cmd_vel topic. At the moment you've just spawned a robot with controllable joints but without anything controlling them.

You'll need to create a yaml file which contains the configuration of the differential driver controller itself, and optionally a joint_state_controller so that ROS can know the angle of the wheel joints.

The launch file that starts the simulation will need to load the configuration from this yaml file and start the ros_control controllers.

The YAML file should look something like this:

# Publish all joint states -----------------------------------
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50  

# Differential Drive Controller
diff_drive_controller:
  type: "diff_drive_controller/DiffDriveController"
  left_wheel: ['left_wheel_hinge']
  right_wheel: ['right_wheel_hinge'']

  wheel_separation: 0.66
  wheel_radius: 0.1

  cmd_vel_timeout: 0.25
  enable_odom_tf: true

  pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]

You'll need to add the following lines to your launch file to start the controllers:

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find rorover_skid_steer)/config/rorover_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" args="joint1_position_controller diff_drive_controller joint_state_controller"/>

You will have to change the joint names to match your robot, but this should get this working for you.

Asked by PeteBlackerThe3rd on 2018-11-11 09:39:20 UTC

Comments

Hello and thank you for your answer. I have my controller YAML file as follows:

 trs:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

  # Position Controllers ---------------------------------------
  lift_position_controller:
    type: effort_controllers/JointPositionController
    joint: trs_frame_to_lift_joint
    pid: {p: 10000.0, i: 1.00, d: 1000.0}
  rotary_position_controller:
    type: effort_controllers/JointPositionController
    joint: trs_lift_to_rotary_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  forks_position_controller:
    type: effort_controllers/JointPositionController
    joint: trs_rotary_to_forks_joint
    pid: {p: 1000.0, i: 0.1, d: 100.0}
  carriage_position_controller:
    type: effort_controllers/JointPositionController
    joint: trs_rotary_to_carriage_joint
    pid: {p: 1000.0, i: 0.1, d: 100.0}

  # Trs base Controller-----------------------------------------
  diff_drive_controller:
    type: "diff_drive_controller/DiffDriveController"
    left_wheel: base_left_wheel_joint
    right_wheel: base_right_wheel_joint
    wheel_separation: 0.46
    wheel_radius: 0.1018
    cmd_vel_timeout: 0.25
    enable_odom_tf: true
    pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
    twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
    base_frame_id: base_chassis_link

and my launch file as follows:

<launch>
  <arg name="debug" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="paused" default="true"/>

  <!--//////////////////GAZEBO//////////////////-->
  <!-- Include empty world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find trs_gazebo)/worlds/trs.world"/>
    <arg name="debug" value="$(arg debug)"/>
    <arg name="gui" value="$(arg gui)"/>
    <arg name="paused" value="$(arg paused)"/>
  </include>

  <!--//////////////////URDF//////////////////-->
  <!-- Load URDF into the Ros Parameter Server-->
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find trs)/trs_with_base.urdf.xacro'"/>

  <!-- Run a python script to send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model trs -robot_namespace trs -J trs_frame_to_lift_joint 1.0" />


  <!--//////////////////JOINT STATE CONTROLLER//////////////////-->
  <!-- load configuration -->
  <rosparam command="load" file="$(find trs_gazebo)/launch/trs_control.yaml" />

  <!-- spawn controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/trs" args="lift_position_controller rotary_position_controller forks_position_controller carriage_position_controller joint_state_controller diff_drive_controller" /><!-- mobile_base_controller-->

  <!-- Convert joint states to TF transforms -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" ns="trs" output="screen"/>

<!--
  <node name="rviz" pkg="rviz" type="rviz" />
-->
</launch>

Also, the .xacro file as it is right now looks as follows:

<?xml version="1.0"?> <!-- Import Gazebo-specific properties --> <!-- Import helper macros for calculating inertia tensors -->

<!-- Helper xacro macro for calculating inertia tensors of cylindrical wheels --> /xacro:macro

<!--gazebo reference="robot">
  <material>Gazebo/Orange</material>
</gazebo-->
<!--gazebo reference="${prefix}chassis_link">
  <material>Gazebo/Orange</material>
</gazebo-->
<gazebo reference="${prefix}left_wheel_link">
  <material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="${prefix}right_wheel_link">
  <material>Gazebo/Orange</material>
</gazebo>

<link name="robot"/>
<joint name="${prefix}robot_to_chassis" type="fixed">
  <parent link="robot"/>
  <child link="${prefix}chassis_link"/>
  <origin rpy="0 0 0" xyz="0 0 0"/>
</joint>

<link name="${prefix}chassis_link">
  <inertial>
    <mass value="120.911"/>
    <origin rpy="0 0 0" xyz="-0.1832 0.0 0.15"/>
    <inertia ixx="3.50889411882" ixy="0.0" ixz="0.0" iyy="9.06900169571" iyz="0.0" izz="11.7718257055"/>
  </inertial>
  <collision name="base">
    <origin rpy="0.0 0.0 1.57" xyz="-0.2032 0.0 0.15"/>
    <geometry>
      <box size="0.3652 0.8374 0.15"/>
    </geometry>
  </collision>
  <collision name="front_sensor_bar_high">
    <origin xyz="0.1026 0.0 0.20"/>
    <geometry>
      <cylinder length=".075" radius=".2826"/>
    </geometry>
  </collision>
  <collision name="front_sensor_bar_low">
    <origin xyz="0.1026 0.0 0.075"/>
    <geometry>
      <cylinder length=".075" radius=".2826"/>
    </geometry>
  </collision>
  <collision name="rear_sensor_bar_high">
    <origin xyz="-0.498 0.0 0.20"/>
    <geometry>
      <cylinder length=".075" radius=".2826"/>
    </geometry>
  </collision>
  <collision name="rear_sensor_bar_low">
    <origin xyz="-0.498 0.0 0.075"/>
    <geometry>
      <cylinder length=".075" radius=".2826"/>
    </geometry>
  </collision>
  <!-- This model was changed to have a single central caster to remove a contact point and reduce the qcbot model jitter. \
    This change was based on successful, jitter-reducing changes to the RPR model, and is partially made of black magic. -->
  <collision name="center_caster_collision">
    <origin xyz="-0.37 0.0 0.1018"/>
    <geometry>
      <sphere radius=".1018"/>
    </geometry>
  </collision>
  <visual>
    <origin rpy="1.5708 0 1.5708" xyz="0 0 0.2159"/>
    <!-- uncomment to use a lower-resolution stl which includes the bottom of qcbot and its wheels -->
    <geometry>
      <mesh filename="package://trs/meshes/base.stl" scale="0.001 0.001 0.001"/>
    </geometry>
    <!-- uncomment to use a higher-resolution stl which does not include the bottom of qcbot or its wheels -->
    <material name="LightGrey">
      <color rgba="0.4 0.4 0.4 1.0"/>
    </material>
  </visual>


</link>

<link name="${prefix}left_wheel_link">
  <inertial>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <xacro:inertia_cylinder mass="2" radius="0.1018" length="0.04"/>
  </inertial>
  <collision name="collision">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <cylinder radius=".1018" length=".04"/>
    </geometry>
  </collision>
  <visual name="left_wheel_visual">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <cylinder radius=".1018" length=".04"/>
    </geometry>
  </visual>
</link>
<joint type="continuous" name="${prefix}left_wheel_joint">
  <origin rpy="-1.5707 0 0" xyz="0.0 0.23 0.1018"/>
  <axis xyz="0 0 1"/>
  <parent
    link="${prefix}chassis_link" />
  <child
    link="${prefix}left_wheel_link" />
  <limit effort="1000" velocity="1000000000" />
  <joint_properties damping="1.0" friction="1.0" />
</joint>

<link name="${prefix}right_wheel_link">
  <inertial>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <xacro:inertia_cylinder mass="2" radius="0.1018" length="0.04"/>
  </inertial>
  <collision name="collision">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <cylinder radius=".1018" length=".04"/>
    </geometry>
  </collision>
  <visual name="right_wheel_visual">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <cylinder radius=".1018" length=".04"/>
    </geometry>
  </visual>
</link>
<joint type="continuous" name="${prefix}right_wheel_joint">
  <origin xyz="0.0 -0.23 0.1018" rpy="1.5707 0 0"/>
  <parent
    link="${prefix}chassis_link" />
  <child
    link="${prefix}right_wheel_link" />
  <axis
    xyz="0 0 -1" />
  <limit effort="1000" velocity="1000000000" />
  <joint_properties damping="1.0" friction="1.0" />
</joint>

<!-- Currently, only data used from transmission tags is joint name. -->
<transmission name="tran_left_wheel_joint">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="${prefix}left_wheel_joint">
    <hardwareInterface>VelocityJointInterface</hardwareInterface>
  </joint>
  <actuator name="motor_left_wheel_joints">
    <hardwareInterface>VelocityJointInterface</hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>

<transmission name="tran_right_wheel_joint">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="${prefix}right_wheel_joint">
    <hardwareInterface>VelocityJointInterface</hardwareInterface>
  </joint>
  <actuator name="motor_right_wheel_joints">
    <hardwareInterface>VelocityJointInterface</hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>


<!-- Simulation dynamics parameters -->
<gazebo reference="${prefix}chassis_link">
  <mu1>0</mu1>
  <mu2>0</mu2>
  <kp value="1.0e+9"/>
  <kd value="5.0e+6"/>
</gazebo>

<gazebo reference="${prefix}right_wheel_link">
  <mu1 value="1.0e+6"/>
  <mu2 value="1.0e+6"/>
  <kp value="1.0e+9"/>
  <kd value="5.0e+6"/>
</gazebo>

<gazebo reference="${prefix}left_wheel_link">
  <mu1 value="1.0e+6"/>
  <mu2 value="1.0e+6"/>
  <kp value="1.0e+9"/>
  <kd value="5.0e+6"/>
</gazebo>

<joint name="${prefix}chassis_to_hokuyo_front" type="fixed">
  <parent link="${prefix}chassis_link"/>
  <child link="${prefix}hokuyo_front"/>
  <origin rpy="0 0 0" xyz="0.3231 0.0 0.1219"/>
</joint>
<link name="${prefix}hokuyo_front"/>
<!-- LIDAR and sonar sensors -->
<gazebo reference="${prefix}hokuyo_front">
  <sensor name="${prefix}hokuyo_front" type="ray" >
    <always_on>true</always_on>
    <update_rate>20</update_rate>
    <pose>0 0 0 0 0 0</pose>
    <visualize>true</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>640</samples>
          <resolution>1</resolution>
          <min_angle>-2.034</min_angle>
          <max_angle>2.094</max_angle>
        </horizontal>
      </scan>
      <range>
        <min>0.005</min>
        <max>40.0</max>
        <resolution>0.36</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_hokuyo_front_controller">
      <gaussianNoise>0.005</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>20</updateRate>
      <topicName>hokuyo_front/scan_raw</topicName>
      <frameName>${prefix}hokuyo_front</frameName>
      <hokuyoMinIntensity>101</hokuyoMinIntensity>
    </plugin>
  </sensor>
</gazebo>

<joint name="${prefix}chassis_to_hokuyo_back" type="fixed">
  <parent link="${prefix}chassis_link"/>
  <child link="${prefix}hokuyo_back"/>
  <origin rpy="0 0 3.14159" xyz="-0.7295 0.0 0.1219"/>
</joint>
<link name="${prefix}hokuyo_back"/>
<gazebo reference="${prefix}hokuyo_back">
  <sensor name="${prefix}hokuyo_back" type="ray" >
    <!-- Actual z position is 0.1619, but I increased it to avoid hitting the ground and other parts of the robot model-->
    <always_on>true</always_on>
    <update_rate>10</update_rate>
    <pose>0 0 0 0 0 0</pose>
    <visualize>true</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>640</samples>
          <resolution>1</resolution>
          <min_angle>-2.034</min_angle>
          <max_angle>2.094</max_angle>
        </horizontal>
      </scan>
      <range>
        <min>0.005</min>
        <max>5.6</max>
        <resolution>0.36</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_hokuyo_back_controller">
      <gaussianNoise>0.005</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>hokuyo_back/scan_raw</topicName>
      <frameName>${prefix}hokuyo_back</frameName>
      <hokuyoMinIntensity>101</hokuyoMinIntensity>
    </plugin>
  </sensor>
</gazebo>

<gazebo reference="${prefix}chassis_link">
  <sensor name="${prefix}sonar_0" type="ray">
    <pose>0.214 0.263 0.2241 0 -0.38 2.21</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_0</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>

  <sensor name="${prefix}sonar_1" type="ray">
    <pose>0.2718 0.2475 0.2274 0 -0.38 0.93</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_1</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>

  <sensor name="${prefix}sonar_2" type="ray">
    <pose>0.3669 0.0318 0.2277 0 -0.38 0.36</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_2</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>

  <sensor name="${prefix}sonar_3" type="ray">
    <pose>0.3669 -0.0318 0.2277 0 -0.38 -0.36</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_3</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>

  <sensor name="${prefix}sonar_4" type="ray">
    <pose>0.2713 -0.247 0.2271 0 -0.38 -0.93</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_4</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>

  <sensor name="${prefix}sonar_5" type="ray">
    <pose>0.2137 -0.2627 0.2241 0 -0.38 -2.21</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_5</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>

  <sensor name="${prefix}sonar_6" type="ray">
    <pose>-0.6201 0.2627 0.2242 0 -0.38 0.93</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_6</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>

  <sensor name="${prefix}sonar_7" type="ray">
    <pose>-0.6782 0.247 0.2271 0 -0.38 2.42</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_7</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>

  <sensor name="${prefix}sonar_8" type="ray">
    <pose>-0.7733 0.0318 0.2277 0 -0.38 2.78</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_8</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>

  <sensor name="${prefix}sonar_9" type="ray">
    <pose>-0.7733 -0.0318 0.2277 0 -0.38 -2.78</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_9</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>

  <sensor name="${prefix}sonar_10" type="ray">
    <pose>-0.6782 -0.2475 0.2274 0 -0.38 -2.42</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_10</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>

  <sensor name="${prefix}sonar_11" type="ray">
    <pose>-0.6201 -0.2627 0.2242 0 -0.38 -0.93</pose>
    <update_rate>10</update_rate>
    <visualize>false</visualize>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1</resolution>
          <min_angle>-0.48</min_angle>
          <max_angle>0.48</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.025</min>
        <max>3</max>
        <resolution>1</resolution>
      </range>
    </ray>
    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
      <robotNamespace>/</robotNamespace>
      <gaussianNoise>0.05</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <topicName>/sonar_gazebo</topicName>
      <frameName>/sonar_11</frameName>
      <minRange>0.025</minRange>
      <maxRange>3</maxRange>
      <fov>0.959914982319</fov>
      <radiation>ultrasound</radiation>
    </plugin>
  </sensor>
</gazebo>

<!-- Robot teleporter -->
<!--gazebo>
  <plugin name="teleporter" filename="libTeleporter.so"/>
</gazebo-->

<gazebo>
  <plugin name='differential_drive_controller'
          filename='libgazebo_ros_diff_drive.so'>
    <publishWheelTF>true</publishWheelTF>
    <robotNamespace>/trs/diff_drive_controller</robotNamespace>
    <publishTf>1</publishTf>
<legacyMode>false</legacyMode>
    <publishWheelJointState>false</publishWheelJointState>
    <alwaysOn>true</alwaysOn>
    <updateRate>100</updateRate>
    <leftJoint>base_left_wheel_joint</leftJoint>
    <rightJoint>base_right_wheel_joint</rightJoint>
    <wheelSeparation>0.46</wheelSeparation>
    <wheelDiameter>0.2036</wheelDiameter>
    <broadcastTF>1</broadcastTF>
    <torque>20</torque>
    <!--commandTopic>/trs/diff_drive_controller/cmd_vel</commandTopic-->
    <!--odometryTopic>/trs/diff_drive_controller/odom</odometryTopic-->
    <odometryFrame>odom</odometryFrame>
    <robotBaseFrame>base_chassis_link</robotBaseFrame>
    <rosDebugLevel>na</rosDebugLevel>
  </plugin>
</gazebo>

/xacro:macro

I can now dynamically reconfigure my differential controller from the rqt_gui but still the robot remains stationary :( ...

Any ideas?

Thank you in advance...

Asked by zoid on 2018-11-11 14:02:32 UTC

Comments

The names of the wheel joints in your URDF do not match the names you've given in the controller yaml. The urdf has ${prefix}left_wheel_joint while the controller has base_left_wheel_joint There will probably be a warning/error message about this somewhere in the console when you launch this.

Asked by PeteBlackerThe3rd on 2018-11-12 07:48:55 UTC