ros_canopen fails at init: transition timout

asked 2021-08-23 15:00:10 -0600

Shaevita gravatar image

The hardware that I am using is a Nanotec C5-1-09 stepper controller. When attempting to initialize the device I am met with a transition timeout error that further indicates that the motor I am using could not be initialized. I am given the warning that the controller spawner couldn't find the expected controller interface. I believe that I have the necessary PDO's mapped in my .eds file (located on github) and I have attempted switching the transmission type (as I had seen some other posts where this was the problem) from the default asynchronous (0xFF) to synchronous (0x01), however this did not solve the problem. Below is my launch file:

<?xml version="1.0"?>
<launch>

  <!-- send urdf to param server -->
  <param name="robot_description" command="cat '$(find nanotec_ros)/urdf/urdf.xml'" />

  <!-- robot state publisher -->
  <node ns="nanotec_stepper" pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" >
    <!-- < param name="publish_frequency" type="double" value="50.0" />
    < param name="tf_prefix" type="string" value="" /> --> 
    <remap from="/joint_states" to="/nanotec_stepper/joint_states" />
  </node>

  <node ns="nanotec_stepper" name="driver" pkg="canopen_motor_node" type="canopen_motor_node" output="screen" clear_params="true" launch-prefix="">
    <rosparam command="load" file="$(find nanotec_ros)/config/nanotec_node.yaml" />
    <remap from="joint_states" to="/nanotec_stepper/joint_states"/>
  </node>

  <!-- Load controller configurations -->
  <rosparam command="load" file="$(find nanotec_ros)/config/nanotec_stepper_controller.yaml" />

  <!-- start controllers -->
  <!-- <node ns="nanotec_stepper" name="nanotec_contoller_spawner" pkg="controller_manager" type="spawner" 
        respawn="false" output="screen" args="nanotec_stepper_position_contoller"  /> -->
  <node ns="nanotec_stepper" name="nanotec_contoller_spawner" pkg="controller_manager" type="spawner" 
          respawn="false" output="screen" args="nanotec_stepper_position_contoller"  />

</launch>

Here is my config file:

bus:
  device: can0 #socketcan network  
  master_allocator: canopen::SimpleMaster::Allocator

sync:
  interval_ms: 10
  #  update_ms: 10  
  overlfow: 0

nodes:
  nanotec_stepper:
    id: 1
    eds_pkg: "nanotec_ros"
    eds_file: "config/C5-E-1-09.eds"


defaults:
  ### 402
  motor_allocator: canopen::Motor402::Allocator
  pos_to_device: "rint(rad2deg(pos)/0.18)"
  pos_from_device: "deg2rad(obj6064)*0.18"
  vel_to_device: "rint(rad2deg(vel)/0.18)"
  vel_from_device: "deg2rad(obj606C)*0.18"
  eff_to_device: "rint(eff)"
  eff_from_device: "0"

When I run the roslaunch command and perform the driver service call I get these results (in the respective terminals for each command):

roslaunch --screen nanotec_ros main.launch 
... logging to /home/usr/.ros/log/86117b06-0445-11ec-ac6d-c8b29bf77957/roslaunch-usr-ThinkPad-X1-Extreme-2nd-3206.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://usr-ThinkPad-X1-Extreme-2nd:41361/

SUMMARY
========

CLEAR PARAMETERS
 * /nanotec_stepper/driver/

PARAMETERS
 * /nanotec_stepper/driver/bus/device: can0
 * /nanotec_stepper/driver/bus/master_allocator: canopen::SimpleMa...
 * /nanotec_stepper/driver/defaults/eff_from_device: 0
 * /nanotec_stepper/driver/defaults/eff_to_device: rint(eff)
 * /nanotec_stepper/driver/defaults/motor_allocator: canopen::Motor402...
 * /nanotec_stepper/driver/defaults/pos_from_device: deg2rad(obj6064)*...
 * /nanotec_stepper/driver/defaults/pos_to_device: rint(rad2deg(pos)...
 * /nanotec_stepper/driver/defaults/vel_from_device: deg2rad(obj606C)*...
 * /nanotec_stepper/driver/defaults/vel_to_device: rint(rad2deg(vel)...
 * /nanotec_stepper/driver/nodes/nanotec_stepper/eds_file: config/C5-E-1-09.eds
 * /nanotec_stepper/driver/nodes/nanotec_stepper/eds_pkg: nanotec_ros
 * /nanotec_stepper/driver/nodes/nanotec_stepper/id: 1
 * /nanotec_stepper/driver/sync/interval_ms: 10
 * /nanotec_stepper/driver/sync/overlfow: 0
 * /nanotec_stepper_joint_position_controller/joint: nanotec_stepper
 * /nanotec_stepper_joint_position_controller/required_drive_mode: 1
 * /nanotec_stepper_joint_position_controller/type: position_controll ...
(more)
edit retag flag offensive close merge delete