search

ros-industrial / ros_canopen

CANopen driver framework for ROS (http://wiki.ros.org/ros_canopen)
GNU Lesser General Public License v3.0
340 stars 272 forks source link

Could not switch to mode 1, reason: Mode switch timed out. #473

Open zouzhe1 opened 1 year ago

zouzhe1 commented 1 year ago

1，My error info：

roslaunch lichuan_ros_canopen lichuan_canopen_motor_1dof_ppm_hm.launch
rosservice call /lichuan/driver/init
[ INFO] [1665817526.729889272]: Initializing successful
Loaded '/lichuan/canopen_motor/joint_state_controller'
Loaded '/lichuan/canopen_motor/base_link1_joint_position_controller'
Started ['/lichuan/canopen_motor/joint_state_controller'] successfully
[ERROR] [1665817531.866585648]: Could not switch to mode 1, reason: Mode switch timed out.
[ERROR] [1665817531.866793471]: base_link1_jointcould not enter mode 1
[ERROR] [1665817531.866974888]: Could not switch one joint for /lichuan/canopen_motor/base_link1_joint_position_controller, will stop all related joints and the controller.
[ERROR] [1665817531.873886624]: Could not switch one joint for /lichuan/canopen_motor/base_link1_joint_position_controller, will stop all related joints and the controller.
Started ['/lichuan/canopen_motor/base_link1_joint_position_controller'] successfully
[lichuan/controller_spawner-3] process has finished cleanly

2，note：

https://github.com/Roboprotos/maxon_epos4_ros1/tree/master/maxon_epos4_ros_canopen when i use maxon ros canopen link code change to my motor，it happen so error.

3，my info:

raspberry4B +ubuntu server 20.04 +USBCAN motor driver support：PP PV HM

3.1my yaml1

bus:
  device: can0 # socketcan network
#  loopback: false # socket should loop back messages
#  driver_plugin: can::SocketCANInterface
  master_allocator: canopen::SimpleMaster::Allocator 
sync:
  interval_ms: 10 # 10 ms is recommended for non real-time systems, set to 0 to disable sync
#  update_ms: <interval_ms> #update interval of control loop, must be set explecitly if sync is disabled
  overflow: 0 # overflow sync counter at value or do not set it (0, default)
heartbeat: # simple heartbeat producer, optional!
  rate: 20 # heartbeat rate
  msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started

3.2my yaml2

joint_names: [base_link1_joint]

joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

# Settings of the controller and assossiated drive modes
# drive mode : see http://wiki.ros.org/canopen_402

# position controllers
# example using Profiled Position (1) drive mode

joint_group_position_controller:
  type: position_controllers/JointGroupPositionController
  joints:
    - base_link1_joint 
  required_drive_mode: 1

base_link1_joint_position_controller:
  type: position_controllers/JointPositionController
  joint: base_link1_joint 
  required_drive_mode: 1

3.3my yaml3

# struct syntax
nodes:
  node1:
    id: 1
    name: base_link1_joint 
    eds_pkg: lichuan_ros_canopen # optionals package  name for relative path
    eds_file: "config/LCok10141839.eds" #hc.eds"# path to EDS/DCF file
    #encoder_resolution: 4000
    #vel_to_device: "vel" # rad/s -> mdeg/s
    #vel_from_device: "obj606C" # actual velocity [mdeg/s] -> rad/s

defaults: # optional, all defaults can be overwritten per node
#  eds_pkg: my_config_package # optional package  name for relative path
#  eds_file: "my_config.dcf" # path to EDS/DCF file
  dcf_overlay: # "ObjectID": "ParameterValue" (both as strings)
# Homing mode settings
     "6098": "0" # Homing method (set to 0 for "No homing operation required") #lbadd
# #    "6065": "2000" # Following error window 
# #    "30B1": "1000" # Home offset move distance 
# #    "607F": "1300" # Max profile velocity 
# #    "6085": "10000" # Quick stop deceleration 
#     "6099sub1": "100" # Speed for switch search #ok
#     "6099sub2": "10" # Speed for zero search    #ok
#     "609A": "1000" # Homing acceleration        #ok
# #    "30B0": "0" # Home position 
# #    "30B2": "1500" # Current threshold for homing mode
# #    "1016sub1" : "0x7F0064" # heartbeat timeout of 100 ms for master at 127
# #    "1017": "100" # heartbeat producer
     "6040": "0"   #init SM
     #"200E": "5"   #set init speed rad/min
     "6083": "100" #set + speed
     "6084": "100" #set - speed
     "6098": "0"

# canopen_chain_node settings ..
  motor_allocator: canopen::Motor402::Allocator # select allocator for motor layer
#   motor_layer: settings passed to motor layer (plugin-specific)
  switching_state: 15 # (Operation_Enable), state for mode switching. Drive mode of operation from canopen_402 wiki
#   pos_to_device: "rint(rad2deg(pos)*1000)" # rad -> mdeg
#   pos_from_device: "deg2rad(obj6064)/1000" # actual position [mdeg] -> rad
  pos_to_device: "pos" # inc
  pos_from_device: "obj6064" # inc 
  vel_to_device: "vel" # rpm
  vel_from_device: "obj606C" # rpm
  eff_to_device: "rint(eff)" # just round to integer
  eff_from_device: "0" # unset

3.4 my launch

<?xml version="1.0"?>
<launch>
    <group ns="/lichuan">
        <!-- Load the URDF into ROS parameter server -->
        <param name="/lichuan/robot_description" command="$(find xacro)/xacro '$(find lichuan_ros_canopen)/urdf/lichuan_1dof_ppm_lb.xacro'"/>

        <node name="canopen_motor" pkg="canopen_motor_node" type="canopen_motor_node" output="screen" clear_params="true" > 
            <rosparam command="load" file="$(find lichuan_ros_canopen)/config/canopen_bus_layer_lb.yaml" />
            <rosparam command="load" file="$(find lichuan_ros_canopen)/config/controller_1dof_ppm_lb.yaml" />
            <rosparam command="load" file="$(find lichuan_ros_canopen)/config/node_layer_1dof_ppm_hm_lb.yaml" />
        </node>

        <!-- load the controllers -->
        <node name="controller_spawner" pkg="controller_manager" type="controller_manager" respawn="false"
            output="screen" args="spawn 
            /lichuan/canopen_motor/joint_state_controller
            /lichuan/canopen_motor/base_link1_joint_position_controller
            "/>
    </group>
</launch>

3.5my urdf xacro

<?xml version="1.0"?>
<!-- maxon EPOS4 example with 1 DOF -->
<robot name="maxon_epos4" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Base Link -->
  <link name="base_link">
  </link>

  <!-- Link1 Link -->
  <link name="link1_link">
  </link>

  <!-- Joint between Base Link and Link1 Link -->
  <joint name="base_link1_joint" type="revolute">
    <parent link="base_link"/>
    <child link="link1_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <!--dynamics damping="0.7"/-->
    <limit effort="100.0" velocity="100000" lower="-100000.0" upper="100000.0"/>
  </joint>

  <transmission name="transmission_base_link1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="base_link1_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="link1_maxon_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

</robot>

3.5 my EDS file

here ： Motor_EDSfile.zip

4, my candump

b@raspberrypi:~$ candump can0
  can0  000   [2]  82 01
  can0  701   [1]  00
  can0  601   [8]  2B 17 10 00 00 00 00 00
  can0  581   [8]  60 17 10 00 00 00 00 00
  can0  000   [2]  01 01
  can0  181   [7]  50 80 78 00 00 00 00
  can0  201   [7]  06 01 78 00 00 00 01
  can0  181   [7]  31 80 78 00 00 00 00
  can0  080   [0]
  can0  201   [7]  07 01 78 00 00 00 01
  can0  181   [7]  33 80 78 00 00 00 00
  can0  080   [0]
  can0  201   [7]  0F 01 78 00 00 00 01

...
lots of   can0  080   [0]
...

  can0  181   [7]  37 80 78 00 00 00 00
  can0  080   [0]
  can0  080   [0]
  can0  601   [8]  40 01 10 00 00 00 00 00
  can0  581   [8]  4F 01 10 00 00 00 00 00
  can0  080   [0]
  can0  601   [8]  2F 03 10 00 00 00 00 00
  can0  581   [8]  60 03 10 00 00 00 00 00
  can0  080   [0]

  can0  77F   [1]  05
  can0  080   [0]

  can0  77F   [1]  05
  can0  080   [0]

  can0  201   [7]  0F 01 78 00 00 00 01
  can0  080   [0]
  can0  77F   [1]  05
  can0  080   [0]

  can0  77F   [1]  05
  can0  080   [0]

  can0  77F   [1]  05
 can0  77F   [1]  05
  can0  601   [8]  40 01 10 00 00 00 00 00
  can0  581   [8]  4F 01 10 00 00 00 00 00
  can0  080   [0]

  can0  77F   [1]  05
  can0  080   [0]

  can0  77F   [1]  05
  can0  080   [0]

  can0  77F   [1]  05
  can0  080   [0]

  can0  201   [7]  0B 01 78 00 00 00 00
  can0  181   [7]  17 80 78 00 00 00 00
  can0  080   [0]
  can0  77F   [1]  05
  can0  080   [0]
  can0  181   [7]  50 80 78 00 00 00 00
  can0  77F   [1]  05
  can0  080   [0]
  can0  000   [2]  02 01
zouzhe1 commented 1 year ago

I don't know what I should do?

I only have one motor. I set the motor drive ID to 1. Why are all hexes 70xx? I think it should be 60xx.

I'm a novice in canopen and I'm also a novice in C++.

I hope you can help to designate it.

thank you.