search

qboticslabs / mastering_ros

This repository contains exercise files of the book "Mastering ROS for Robotics Programming"
https://www.packtpub.com/hardware-and-creative/mastering-ros-robotics-programming
468 stars 282 forks source link

ROS joint_state_controller fail to start #42

Open tzoutzoukos opened 4 years ago

tzoutzoukos commented 4 years ago

Hello all, I am having a problem where although I have managed to load joint_state_controller and velocity controller for the four joints of my robot, while spawning them i get:

[INFO] [1583148364.122978, 0.000000]: Loading controller: joint_state_controller
[INFO] [1583148364.137343, 27.471000]: Loading controller: FRJ_velocity_controller
[INFO] [1583148364.224540, 27.544000]: Loading controller: FLJ_velocity_controller
[INFO] [1583148364.282939, 27.590000]: Loading controller: BLJ_velocity_controller
[INFO] [1583148364.347087, 27.639000]: Loading controller: BRJ_velocity_controller
[INFO] [1583148364.423555, 27.701000]: Controller Spawner: Loaded controllers: joint_state_controller, FRJ_velocity_controller, FLJ_velocity_controller, BLJ_velocity_controller, BRJ_velocity_controller
[ERROR] [1583148364.426532, 27.703000]: Failed to start controllers: joint_state_controller, FRJ_velocity_controller, FLJ_velocity_controller, BLJ_velocity_controller, BRJ_velocity_controller

My robot_control launch:

<launch>

   <param name="robot_description" textfile="$(find robot)/urdf/robot.urdf" />
   <arg name="gui" default="False" />
   <param name="use_gui" value="$(arg gui)"/>
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

<!-- Show in Rviz-->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find robot)/urdf.rviz"/>

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

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen" ns="/robot">
    <remap from="/joint_states" to="/robot/joint_states" />
  </node>

<!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/robot" args="joint_state_controller FRJ_velocity_controller FLJ_velocity_controller BLJ_velocity_controller BRJ_velocity_controller"/>

</launch>

My robot_control.yaml :

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

  # # Position Controllers ---------------------------------------
  # FLJ_position_controller:
  #   type: effort_controllers/JointPositionController
  #   joint: FLJ
  #   pid: {p: 1.0, i: 1.0, d: 0.0}
  # FRJ_position_controller:
  #   type: effort_controllers/JointPositionController
  #   joint: FRJ
  #   pid: {p: 1.0, i: 1.0, d: 0.0}

  # BLJ_position_controller:
  #   type: effort_controllers/JointPositionController
  #   joint: BLJ
  #   pid: {p: 1.0, i: 1.0, d: 0.0}

  # BRJ_position_controller:
  #   type: effort_controllers/JointPositionController
  #   joint: BRJ
  #   pid: {p: 1.0, i: 1.0, d: 0.0}

   #Velocity Controllers ---------------------------------------
  FRJ_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: FRJ
    pid: {p: 1.0, i: 1.0, d: 0.0}

  FLJ_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: FRJ
    pid: {p: 1.0, i: 1.0, d: 0.0}

  BRJ_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: BRJ
    pid: {p: 1.0, i: 1.0, d: 0.0}

  BLJ_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: BLJ
    pid: {p: 1.0, i: 1.0, d: 0.0}

URDF Model:


<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from robot.xacro                    | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!--This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
     Commit Version: 1.5.1-0-g916b5db  Build Version: 1.5.7152.31018
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
  <link name="dummy">
  </link>
  <link name="base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00390777224516517 -0.032446267219719 0.184169550820421"/>
      <mass value="4.20121630268732"/>
      <inertia ixx="0.0149000386129946" ixy="-4.66831001352174E-09" ixz="5.23920338795194E-08" iyy="0.0234359493013497" iyz="0.000771538751024883" izz="0.0286744535302635"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/base_link.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.529411764705882 0.549019607843137 0.549019607843137 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/base_link.STL"/>
      </geometry>
    </collision>
  </link>
  <link name="FL">
    <inertial>
      <origin rpy="0 0 0" xyz="-5.55111512312578E-17 9.10729824887824E-18 -6.93889390390723E-18"/>
      <mass value="0.0615219544751675"/>
      <inertia ixx="5.54433425808419E-05" ixy="-1.45453466603006E-20" ixz="-1.00225538664655E-21" iyy="3.00921775305435E-05" iyz="2.309188417276E-21" izz="3.00921775305435E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/FL.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.298039215686275 0.298039215686275 0.298039215686275 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/FL.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="dummy_joint" type="fixed">
    <parent link="dummy"/>
    <child link="base_link"/>
  </joint>
  <joint name="FLJ" type="continuous">
    <origin rpy="1.5707963267949 0 0" xyz="-0.162088045105054 0.0440194465480433 -0.0035605397876617"/>
    <parent link="base_link"/>
    <child link="FL"/>
    <axis xyz="-1 0 0"/>
  </joint>
  <link name="FR">
    <inertial>
      <origin rpy="0 0 0" xyz="2.77555756156289E-17 -5.3776427755281E-17 3.46944695195361E-17"/>
      <mass value="0.0615219544751675"/>
      <inertia ixx="5.54433425808419E-05" ixy="-1.73616162087809E-20" ixz="-8.97406720914896E-21" iyy="3.00921775305435E-05" iyz="-2.24902265994014E-21" izz="3.00921775305435E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/FR.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.298039215686275 0.298039215686275 0.298039215686275 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/FR.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="FRJ" type="continuous">
    <origin rpy="1.5707963267949 0 0" xyz="0.169947769597777 0.0440194465480434 -0.00356053978766185"/>
    <parent link="base_link"/>
    <child link="FR"/>
    <axis xyz="-1 0 0"/>
  </joint>
  <link name="BL">
    <inertial>
      <origin rpy="0 0 0" xyz="-5.55111512312578E-17 7.85396053748499E-16 -1.38777878078145E-17"/>
      <mass value="0.0615219544751675"/>
      <inertia ixx="5.54433425808419E-05" ixy="-5.54431358512462E-21" ixz="2.27097066282367E-22" iyy="3.00921775305435E-05" iyz="2.73146067339844E-21" izz="3.00921775305435E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/BL.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.298039215686275 0.298039215686275 0.298039215686275 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/BL.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="BLJ" type="continuous">
    <origin rpy="1.5707963267949 0 0" xyz="-0.162088045105054 -0.0759805534519569 -0.00356053978766179"/>
    <parent link="base_link"/>
    <child link="BL"/>
    <axis xyz="-1 0 0"/>
  </joint>
  <link name="BR">
    <inertial>
      <origin rpy="0 0 0" xyz="-8.32667268468867E-17 7.03430369508595E-16 2.77555756156289E-17"/>
      <mass value="0.0615219544751675"/>
      <inertia ixx="5.54433425808419E-05" ixy="-7.26178700863739E-21" ixz="-6.24920580783796E-21" iyy="3.00921775305435E-05" iyz="-3.51228039755728E-21" izz="3.00921775305435E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/BR.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.298039215686275 0.298039215686275 0.298039215686275 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/BR.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="BRJ" type="continuous">
    <origin rpy="1.5708 0 0" xyz="0.16991 -0.075981 -0.0035605"/>
    <parent link="base_link"/>
    <child link="BR"/>
    <axis xyz="-1 0 0"/>
  </joint>
  <link name="TB">
    <inertial>
      <origin rpy="0 0 0" xyz="-7.8063E-18 1.3878E-17 0.001"/>
      <mass value="0.36807"/>
      <inertia ixx="0.00019523" ixy="7.0972E-20" ixz="-4.0656E-22" iyy="0.0026688" iyz="4.5169E-23" izz="0.0028638"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/TB.STL"/>
      </geometry>
      <material name="">
        <color rgba="0.52941 0.54902 0.54902 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot/meshes/TB.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="TBJ" type="fixed">
    <origin rpy="0 0 0" xyz="0.003912 -0.086602 0.46844"/>
    <parent link="base_link"/>
    <child link="TB"/>
    <axis xyz="0 0 0"/>
  </joint>
  <!--             LIDAR                     -->
  <gazebo reference="hokuyo_link">
    <sensor name="head_hokuyo_sensor" type="ray">
      <pose> 0 0 0 0 0 0 </pose>
      <visualize>true</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-1.57079</min_angle>
            <max_angle>1.57079</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.1</min>
          <max>30</max>
          <resolution>0.1</resolution>
        </range>
      </ray>
      <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_head_hokuyo_controller">
        <topicName>/scan</topicName>
        <frameName>hokuyo_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="TB_Lidar" type="fixed">
    <axis xyz="0 1 0"/>
    <parent link="TB"/>
    <child link="hokuyo_link"/>
    <origin rpy="0 0 1.57079633" xyz="0 0 0.005"/>
  </joint>
  <!-- Hokuyo Laser -->
  <link name="hokuyo_link">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
    </collision>
  </link>
  <!--                 Gazebo Ros Control Plugin                   -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <robotNamespace>/robot</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>
  <!--                 Gazebo Transmission                        -->
  <transmission name="FLT">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor1">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="FLJ">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="FRT">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor2">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="FRJ">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="BLT">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor3">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="BLJ">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="BRT">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor4">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="BRJ">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <!--                Differential Drive           
  <gazebo>
    <plugin name="differential_drive_controller" filename = "libgazebo_ros_diff_drive.so">
      <leftJoint>FLJ</leftJoint>
      <legacyMode>false</legacyMode>
      <rightJoint>FRJ</rightJoint>
      <robotBaseFrame>base_link</robotBaseFrame>
      <wheelSeperation>0.25</wheelSeperation>
      <wheelDiameter>0.07</wheelDiameter>
      <publishWheelJointState>true</publishWheelJointState>
    </plugin>
   </gazebo>
  -->
</robot>

Note that if I replace velocity with position controllers, I am able to load joint_state_controller as well as postion controllers for my 4 joints after first loading Gazebo simulation.

Any help appreciated.