Skip to content

Ackermann Steering Controller failed to start on ros noetic #636

@Danendra10

Description

@Danendra10

Hi! it's my first time doing a robot with ackermann steering model. right now i got this error

[ INFO] [1744522733.554158501, 0.170000000]: Loaded gazebo_ros_control.
[INFO] [1744522733.790122, 0.398000]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1744522733.791888, 0.399000]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1744522733.793515, 0.401000]: Loading controller: joint_state_controller
[New Thread 0x7ffef0bf6700 (LWP 233443)]
[INFO] [1744522733.804974, 0.413000]: Loading controller: ackermann_steering_controller
[ INFO] [1744522733.825235380, 0.433000000]: Controller state will be published at 50Hz.
[ INFO] [1744522733.825650929, 0.434000000]: Wheel separation height will be multiplied by 1.
[ INFO] [1744522733.825941867, 0.434000000]: Wheel radius will be multiplied by 1.
[ INFO] [1744522733.826232527, 0.434000000]: Steer pos will be multiplied by 1.
[ INFO] [1744522733.826785626, 0.435000000]: Velocity rolling window size of 10.
[ INFO] [1744522733.827406981, 0.435000000]: Velocity commands will be considered old if they are older than 0.5s.
[ INFO] [1744522733.827704625, 0.436000000]: Allow mutiple cmd_vel publishers is enabled
[ INFO] [1744522733.828148312, 0.436000000]: Base frame_id set to base_footprint
[ INFO] [1744522733.828572746, 0.437000000]: Odometry frame_id set to odom
[ INFO] [1744522733.828874202, 0.437000000]: Publishing to tf is enabled
[ INFO] [1744522733.835806526, 0.444000000]: rear wheel to origin: 0,1.45, 0.42
[ INFO] [1744522733.835824893, 0.444000000]: front steer to origin: 0,-1.5, 0.42
[ INFO] [1744522733.835836750, 0.444000000]: Calculated wheel_separation_h: 0
[ INFO] [1744522733.835870557, 0.444000000]: Odometry params : wheel separation height 0, wheel radius 0.15
[ERROR] [1744522733.836263387, 0.444000000]: Exception thrown while initializing controller 'ackermann_steering_controller'
[ERROR] [1744522733.836309744, 0.444000000]: Initializing controller 'ackermann_steering_controller' failed
[ERROR] [1744522734.837597, 1.341000]: Failed to load ackermann_steering_controller

I noticed that it has some diffrences for the setup between the ros noetic and ros jazzy one. this is how my ackermann_controller.yaml looks like

controller_list:
  - name: joint_state_controller
    type: joint_state_controller/JointStateController
  
  - name: ackermann_steering_controller
    type: "ackermann_steering_controller/AckermannSteeringController"
    joints:
      - artificial_rear_joint
      - steer_front_joint

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

ackermann_steering_controller:
  type: "ackermann_steering_controller/AckermannSteeringController"
  rear_wheel: 'artificial_rear_joint'
  front_steer: 'steer_front_joint'
  wheelbase: 3.02972  # adjust to your robot
  track: 1.36431      # adjust to your robot
  wheel_radius: 0.15
  cmd_vel_timeout: 0.5
  publish_rate: 50
  velocity_rolling_window_size: 10

  base_frame_id: base_footprint #default: base_link

  # Odom frame_id
  odom_frame_id: odom

  linear:
    x:
      has_velocity_limits    : true
      max_velocity           : 1.0  # m/s
      min_velocity           : -0.5 # m/s
      has_acceleration_limits: true
      max_acceleration       : 0.8  # m/s^2
      min_acceleration       : -0.4 # m/s^2
      has_jerk_limits        : true
      max_jerk               : 5.0 # m/s^3

  angular:
    z:
      has_velocity_limits    : true
      max_velocity           : 1.7  # rad/s
      has_acceleration_limits: true
      max_acceleration       : 1.5  # rad/s^2
      has_jerk_limits        : true
      max_jerk               : 2.5 # rad/s^3

This is how i define the urdf on robot.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="car">

  <xacro:include filename="include/HDL-32E.urdf.xacro" />
  <xacro:include filename="include/macros.xacro" />
  <xacro:include filename="include/parameters.xacro" />

  <link name="base_footprint" />

  <link name="base_link">
    <xacro:inertial_box
      mass="${car_weight}"
      x="${car_length}"
      y="${car_width}"
      z="${car_height}">
      <origin xyz="0.0 0.0 ${car_height/2}" rpy="0 0 1.570796" />
    </xacro:inertial_box>
    <visual name="chassis">
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0.0" />
      <geometry>
        <mesh filename="package://car_description/meshes/car/body.dae" scale="1 1 1" />
      </geometry>
    </visual>
    <collision name="chassis_collision">
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0 0.0" />
      <geometry>
        <mesh filename="package://car_description/meshes/car/body.dae" scale="1 1 1" />
      </geometry>
    </collision>
  </link>

  <!-- JOINTS BASE -->
  <joint name="base_link_joint" type="fixed">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
    <parent link="base_footprint" />
    <child link="base_link" />
  </joint>

  <!-- STEERING -->
  <link name="steer_front">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="1.0" />
      <inertia ixx="0.037227" ixy="0" ixz="0" iyy="0.04" iyz="0" izz="0.06" />
    </inertial>
  </link>

  <link name="artificial_rear">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="1.0" />
      <inertia ixx="0.037227" ixy="0" ixz="0" iyy="0.04" iyz="0" izz="0.06" />
    </inertial>
  </link>

  <joint name="steer_front_joint" type="revolute">
    <origin xyz="0.0 -1.5 0.42" rpy="-1.57 0 0.0" />
    <parent link="base_link" />
    <child link="steer_front" />
    <axis xyz="0 1 0" />
    <limit lower="-0.6" upper="0.6" effort="10" velocity="1.0" />
  </joint>

  <joint name="artificial_rear_joint" type="continuous">
    <origin xyz="0.0 1.45 0.42" rpy="-1.57 0 0.0" />
    <parent link="base_link" />
    <child link="artificial_rear" />
    <axis xyz="1 0 0" />
  </joint>

  <!-- WHEELS -->

  <!-- Same for all wheels -->
  <link name="front_right_wheel">
    <xacro:inertial_cylinder
      mass="${tire_weight}"
      length="${tire_length}"
      radius="${tire_rad}">
      <origin xyz="0.0 0.0 0.0" rpy="1.570796 -1.570796 0.0" />
    </xacro:inertial_cylinder>
    <visual name="wheel">
      <origin xyz="0.0 0 0.0" rpy="1.570796 -1.570796 0.0" />
      <geometry>
        <mesh filename="package://car_description/meshes/car/wheel.dae" scale="1 1 1" />
      </geometry>
    </visual>
    <collision name="wheel_collision">
      <origin xyz="0.0 0.0 0.0" rpy="1.570796 -1.570796 0.0" />
      <geometry>
        <mesh filename="package://car_description/meshes/car/wheel.dae" scale="1 1 1" />
      </geometry>
    </collision>
  </link>

  <link name="front_left_wheel">
    <xacro:inertial_cylinder
      mass="${tire_weight}"
      length="${tire_length}"
      radius="${tire_rad}">
      <origin xyz="0.0 0.0 0.0" rpy="1.570796 1.570796 0.0" />
    </xacro:inertial_cylinder>
    <visual name="wheel">
      <origin xyz="0.0 0 0.0" rpy="1.570796 1.570796 0.0" />
      <geometry>
        <mesh filename="package://car_description/meshes/car/wheel.dae" scale="1 1 1" />
      </geometry>
    </visual>
    <collision name="wheel_collision">
      <origin xyz="0.0 0.0 0.0" rpy="1.570796 1.570796 0.0" />
      <geometry>
        <mesh filename="package://car_description/meshes/car/wheel.dae" scale="1 1 1" />
      </geometry>
    </collision>
  </link>

  <!-- REAR WHEELS -->
  <link name="rear_right_wheel">
    <xacro:inertial_cylinder
      mass="${tire_weight}"
      length="${tire_length}"
      radius="${tire_rad}">
      <origin xyz="0.0 0.0 0.0" rpy="1.570796 -1.570796 0" />
    </xacro:inertial_cylinder>
    <visual name="wheel">
      <origin xyz="0.0 0 0.0" rpy="1.570796 -1.570796 0" />
      <geometry>
        <mesh filename="package://car_description/meshes/car/wheel.dae" scale="1 1 1" />
      </geometry>
    </visual>
    <collision name="wheel_collision">
      <origin xyz="0.0 0.0 0.0" rpy="1.570796 -1.570796 0" />
      <geometry>
        <mesh filename="package://car_description/meshes/car/wheel.dae" scale="1 1 1" />
      </geometry>
    </collision>
  </link>

  <link name="rear_left_wheel">
    <xacro:inertial_cylinder
      mass="${tire_weight}"
      length="${tire_length}"
      radius="${tire_rad}">
      <origin xyz="0.0 0.0 0.0" rpy="1.570796 1.570796 0" />
    </xacro:inertial_cylinder>
    <visual name="wheel">
      <origin xyz="0.0 0 0.0" rpy="1.570796 1.570796 0" />
      <geometry>
        <mesh filename="package://car_description/meshes/car/wheel.dae" scale="1 1 1" />
      </geometry>
    </visual>
    <collision name="wheel_collision">
      <origin xyz="0.0 0.0 0.0" rpy="1.570796 1.570796 0" />
      <geometry>
        <mesh filename="package://car_description/meshes/car/wheel.dae" scale="1 1 1" />
      </geometry>
    </collision>
  </link>

  <!-- JOINTS WHEEL -->
  <joint name="front_left_wheel_joint" type="continuous">
    <origin xyz="0.79121 0 0" rpy="0.0 0 0.0" />
    <parent link="steer_front" />
    <child link="front_left_wheel" />
    <axis xyz="0 1 0" />
    <limit effort="0" velocity="22.0" />
  </joint>

  <joint name="front_right_wheel_joint" type="continuous">
    <origin xyz="-0.79121 0 0" rpy="0.0 0 0.0" />
    <parent link="steer_front" />
    <child link="front_right_wheel" />
    <axis xyz="0 1 0" />
    <limit effort="0" velocity="22.0" />
  </joint>

  <joint name="rear_right_wheel_joint" type="continuous">
    <origin xyz="-0.79121 0 0" rpy="0.0 0 0.0" />
    <parent link="artificial_rear" />
    <child link="rear_right_wheel" />
    <axis xyz="1 0 0" />
  </joint>

  <joint name="rear_left_wheel_joint" type="continuous">
    <origin xyz="0.79121 0 0" rpy="0.0 0 0.0" />
    <parent link="artificial_rear" />
    <child link="rear_left_wheel" />
    <axis xyz="-1 0 0" />
  </joint>

  <!-- Transmission -->
  <transmission name="steer_front_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="steer_front_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="steer_front_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="artificial_rear_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="artificial_rear_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="artificial_rear_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="wheel_rear_left_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rear_left_wheel_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rear_left_wheel_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="wheel_rear_right_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rear_right_wheel_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rear_right_wheel_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="wheel_front_left_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="front_left_wheel_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="front_left_wheel_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="wheel_front_right_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="front_right_wheel_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="front_right_wheel_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" />
  </gazebo>
</robot>

As for the included xacro, it's just simply some macros or parameters. And last file, this is my simulator.launch file.

<launch>

    <arg name="ros_control_file" default="$(find car_description)/config/ackermann_controller.yaml" />

    <rosparam file="$(arg ros_control_file)" command="load" />
    <param name="robot_description"
        command="$(find xacro)/xacro --inorder
                       '$(find car_description)/urdf/robot.urdf.xacro'" />

    <node name="controller_spawner" pkg="controller_manager" type="spawner"
        respawn="false" output="screen"
        args="
            joint_state_controller ackermann_steering_controller">
    </node>

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
        output="screen">
        <param name="use_tf_static" value="true" />
        <param name="publish_frequency" value="200" />
        <param name="ignore_timestamp" value="false" />
        <remap from="/joint_states" to="/joint_states" />
    </node>

    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="paused" value="false" />
        <arg name="use_sim_time" value="true" />
        <arg name="gui" value="true" />
        <arg name="debug" value="true" />
        <arg name="verbose" value="true" />
    </include>

    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
        args="-urdf -model car -param robot_description -x 0 -y 0 -z 0.0 -R 0.0 -P 0.0 -Y 0.0" />

    <!-- rvic -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find car_description)/display/viz.rviz" />

</launch>

This is the screenshot from the rviz (the tf)
rviz
and visual from the gz
gz

And also the car on the gazebo is shaking up and down. I don't get it why did i'm getting an error like that, i also tried to see what controllers is active, and it's only the joint_state_controller

rosservice call /controller_manager/list_controllers "{}" 
controller: 
  - 
    name: "joint_state_controller"
    state: "running"
    type: "joint_state_controller/JointStateController"
    claimed_resources: 
      - 
        hardware_interface: "hardware_interface::JointStateInterface"
        resources: []

Can anyone help me with this issue?

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type
    No fields configured for issues without a type.

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions