Robotics StackExchange | Archived questions

gazebo velocity controller

I tried to simulate my car model in the gazebo, I built the front and rear wheel of the car, I tried to use the velocity controller in the roscontrol to control my car front wheel and rear wheels respectively. had been loaded out of Wheel1velocitycontroller and Wheel2velocity_controller:

Test "rostopic pub-1/rrbot/wheel1velocitycontroller/command stdmsgs/float64 data:1.5" "Rostopic pub-1/rrbot/wheel1velocitycontroller/command stdmsgs/float64 data:1.0"

The car can be movement, theoretically should be driven in a straight line, and the result of the simulation is turning movement, why?

Supplementary questions: when i run

rostopic pub-1/rrbot/wheel1_velocity_controller/command std_msgs/float64  data:1.0
rostopic pub-1/rrbot/wheel1_velocity_controller/command std_msgs/float64  data:1.0

The car is going to swerve, isn't it supposed to be a straight motion?

this is my urdf file:

<?xml version="1.0"?>
<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="length_wheel" value="0.05" />
<xacro:property name="radius_wheel" value="0.05" />
<xacro:property name="camera_link" value="0.05" /> 
<xacro:macro name="default_inertial" params="mass">
  <inertial>
    <mass value="${mass}" />
    <inertia ixx="1.0" ixy="0.0" ixz="0.0"
      iyy="1.0" iyz="0.0"
      izz="1.0" />
    </inertial>
  </xacro:macro>

  <xacro:include filename="$(find rrbot_description)/urdf/mytest.gazebo" />
  <xacro:include filename="$(find rrbot_description)/urdf/materials.xacro" />

  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.2 .3 .1"/>
      </geometry>
      <origin rpy="0 0 1.54" xyz="0 0 0.05"/>
      <material name="orange">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.2 .3 0.1"/>
      </geometry>
    </collision>
    <xacro:default_inertial mass="10"/>
  </link>

  <link name="wheel_1">
    <visual>
      <geometry>
        <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
      </geometry>
      <!-- <origin rpy="0 1.5 0" xyz="0.1 0.1 0"/> -->
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
      </geometry>
    </collision>
    <xacro:default_inertial mass="1"/>
  </link>

  <link name="wheel_2">
    <visual>
      <geometry>
        <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
      </geometry>
      <!-- <origin rpy="0 1.5 0" xyz="-0.1 0.1 0"/> -->
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
      </geometry>
    </collision>
    <xacro:default_inertial mass="1"/>
  </link>

  <joint name="wheel1_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_1"/>
    <origin rpy="-1.5707 0 0" xyz="0.1 0 0"/>
    <axis xyz="0 0 1" />
  </joint>

  <joint name="wheel2_joint" type="continuous">
    <axis xyz="0 0 1" />
    <anchor xyz="0 0 0" />
    <limit effort="100" velocity="100" />
    <parent link="base_link"/>
    <child link="wheel_2"/>
    <origin rpy="-1.5707 0 0" xyz="-0.1 0 0"/>
  </joint>

  <joint name="camera_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="0.125 0 0.125" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="camera_link"/>
  </joint>

  <link name="camera_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
      <material name="red">
        <color rgba="1 0 0 1"/>
      </material>
    </visual>

    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>

  <joint name="camera_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="-1.5707 0 -1.5707"/>
    <parent link="camera_link"/>
    <child link="camera_link_optical"/>
  </joint>

  <link name="camera_link_optical">
  </link>

  <joint name="hokuyo_joint" type="fixed">
    <origin xyz="0.125 0.05 0.125" rpy="0 0 0"/>
    <parent link="base_link"/>
    <axis xyz="0 1 0" />
    <child link="hokuyo_link"/>
  </joint>

  <link name="hokuyo_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://rrbot_description/meshes/hokuyo.dae"/>
      </geometry>
    </visual>

    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>

  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel1_joint">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>10</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel2_joint">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>10</mechanicalReduction>
    </actuator>
  </transmission>

  <gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">

      <rosDebugLevel>Debug</rosDebugLevel>
      <publishWheelTF>false</publishWheelTF>
      <robotNamespace>/</robotNamespace>
      <publishTf>1</publishTf>
      <publishWheelJointState>false</publishWheelJointState>
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <leftJoint>wheel1_joint</leftJoint>
      <rightJoint>wheel2_joint</rightJoint>
      <wheelSeparation>0.2</wheelSeparation>
      <wheelDiameter>0.1</wheelDiameter>
      <broadcastTF>1</broadcastTF>
      <wheelTorque>30</wheelTorque>
      <wheelAcceleration>1.8</wheelAcceleration>
      <commandTopic>cmd_vel</commandTopic>
      <odometryFrame>odom</odometryFrame> 
      <odometryTopic>odom</odometryTopic> 
      <robotBaseFrame>base_link</robotBaseFrame>
    </plugin>
  </gazebo> 
</robot>

Asked by 623921767@qq.com on 2018-07-31 04:49:15 UTC

Comments

can you post the URDF for your car? What version of Gazebo are you using? At first look, it seems like you would need to set both wheel velocities.

Asked by KothariA on 2018-07-31 13:55:47 UTC

gazebo 2.2.6 My car model diagram and URDF as shown in the question

Asked by 623921767@qq.com on 2018-07-31 19:40:24 UTC

Can you please update your question with the exact commands that you're using (those will not work as is)? Also, there's no URDF file.

Asked by jayess on 2018-07-31 19:53:35 UTC

Supplementary questions: when i run "rostopic pub-1/rrbot/wheel1_velocity_controller/command std_msgs/float64 data:1.5" "rostopic pub-1/rrbot/wheel1_velocity_controller/command std_msgs/float64 data:1.0" The car is going to swerve, isn't it supposed to be a straight motion?

Asked by 623921767@qq.com on 2018-07-31 20:11:45 UTC

Answers