RIVeR-Lab / epos_hardware

Other
12 stars 39 forks source link

Velocity Controller Crashing #4

Open DaveyGravey opened 8 years ago

DaveyGravey commented 8 years ago

Having some issues with running velocity controllers from an Odroid. Below is my setup, my config files, and the error I'm getting. Running the exact same setup from my computer (Ubuntu 14.04 - ROS Indigo) and it works fine, so suspect there's an issue coming from the libraries compiled for ARM inside the epos_libraries. Though this is a huge assumption based on the fact that this is the main difference between my computer and odroid setup.

Odroid setup: Ubuntu 14.04 ROS Indigo Compiled epos_hardware from source, selecting arm/hf and changed the library to libftd2xx.so.1.2.8 in epos_library CMakeLists. Compiles fine and will run the position controller (profile position mode) perfectly fine.

Note to anyone seeing the hex serial number issue, I found the easiest solution to this is getting rid of the hex conversion inside epos.cpp and use the int version of the numbers. With my setup there was a weird issue where the hex value was being converted wrong everytime.

Files in question ~ removed unnecessary parts. yaml file:

motorFL:
  actuator_name: 'FL_act'
  serial_number: '105692551797093' #changed the hextonumber function so this is fine
  operation_mode: 'profile_velocity'
  clear_faults: true

  motor:
    type: 1
    dc_motor:
      nominal_current: 2
      max_output_current: 4
      thermal_time_constant: 24.1

  sensor:
    type: 1
    incremental_encoder:
      resolution: 500
      inverted_polarity: false

  safety:
    max_following_error: 20000
    max_profile_velocity: 12000
    max_acceleration: 15000

  position_profile:
    velocity: 10000
    acceleration: 8000
    deceleration: 9000

  velocity_profile:
    acceleration: 8000
    deceleration: 9000

launch file:

  <param name="robot_description" textfile="$(find epos_hardware)/launch/inspection_robot.urdf" />

  <node name="epos_hardware" pkg="epos_hardware" type="epos_hardware_node" output="screen" 
    args="motorFL motorFR motorBL motorBR" >
    <rosparam command="load" file="$(find epos_hardware)/launch/motor_config.yaml" />
  </node>

  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" 
    args="FL_controller FL_state FR_controller FR_state BL_controller BL_state BR_controller BR_state">
  </node>

  <!-- FL motor state and velocity controllers -->
  <param name="FL_controller/type" value="velocity_controllers/JointVelocityController" />
  <param name="FL_controller/joint" value="FL_joint" />
  <param name="FL_state/type" value="joint_state_controller/JointStateController" />
  <param name="FL_state/publish_rate" value="50" />

urdf file:

<?xml version="1.0"?>
<robot name="inspection_robot">
    <link name="robot_base" />
    <link name="FL_wheel" />

    <joint name="FL_joint" type="continuous" >
        <parent link="robot_base" />
        <child link="FL_wheel" />
        <axes xyz="0 1 0" />
        <origin xyz="-0.15 0.2 0" rpy="0 0 0.7854" />
    </joint>

  <transmission name="FL_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="FL_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="FL_act">
      <mechanicalReduction>231</mechanicalReduction>
    </actuator>
  </transmission>

Traceback of the error. Controllers shut down immediately after this.

Traceback (most recent call last):
  File "/home/odroid/catkin_ws/src/ros_control/controller_manager/scripts/spawner", line 212, in <module>
    if __name__ == '__main__': main()
  File "/home/odroid/catkin_ws/src/ros_control/controller_manager/scripts/spawner", line 190, in main
    resp = load_controller(name)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 525, in call
    raise ServiceException("transport error completing service call: %s"%(str(e)))
rospy.service.ServiceException: transport error completing service call: unable to receive data from sender, check sender's logs for details

Closest I've found to this error is this: controller_manager dies, which suggests there is conflicting issues with binaries, but this doesn't seem possible with my setup since I only have the EPOS_hardware on my system, not to mention the position controller works fine.

Anyone have any thoughts on this or ideas on how to debug? Log files aren't giving me any useful information (at least nothing that I understand).