search

ros-industrial / ros_canopen

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

ros_canopen uses wrong COB-id for PDOs #434

Closed PaulVerhoeckx closed 3 years ago

PaulVerhoeckx commented 3 years ago

Hi,

I am trying to control 2 drives using ros_canopen, see my ros_canopen configuration below and the PD4N.eds can be downloaded here.

canopen.yaml

bus:
  device: can0 # socketcan network
sync:
  interval_ms: 10 # set to 0 to disable sync
  overflow: 0 # overflow sync counter at value or do not set it (0, default)

nodes:
  wheel_left_joint:
    id: 1
  wheel_right_joint:
    id: 2

defaults: # optional, all defaults can be overwritten per node
  eds_pkg: snap_cart_driver
  eds_file: "cfg/PD4N.eds"
  motor_allocator: canopen::Motor402::Allocator
  motor_layer:
    switching_state: 2
  dcf_overlay: # "ObjectID": "ParameterValue" (both as strings)
    "1600sub0": "1"  # Number of mapped objects RPDO1
    "1600sub1": "0x60400010" # map Control Word to RPDO1

Trace

[ INFO] [1622540826.856333312]: Using fixed control period: 0.010000000  
[ INFO] [1622540828.959749648]: Initializing...  
[ INFO] [1622540828.960839256]: Current state: 1 device error: system:0 internal_error: 0 (OK)  
[ INFO] [1622540828.961597711]: Current state: 2 device error: system:0 internal_error: 0 (OK)    
[ERROR] [1622540829.253919240]: abort1400#1, reason: Invalid value for parameter (download only).  
[ERROR] [1622540829.254181954]: Could not process message  
[ INFO] [1622540829.260806585]: Current state: 2 device error: system:125 internal_error: 0 (OK)  
[ INFO] [1622540829.261699522]: Current state: 0 device error: system:125 internal_error: 0 (OK)  
[ INFO] [1622540829.262299067]: Current state: 0 device error: system:0 internal_error: 0 (OK)  
[ INFO] [1622540829.262879757]: Current state: 0 device error: system:0 internal_error: 0 (OK)  
[ERROR] [1622540829.263896447]: not operational; CAN not ready  
[ERROR] [1622540829.264252445]: Initializing failed: /home/snap/snap_cart_ws/src/ros_canopen/canopen_master/src/sdo.cpp(429): Throw in function void canopen::SDOClient::transmitAndWait(const canopen::ObjectDict::Entry&, const canopen::String&, canopen::String*)  
Dynamic exception type: boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<canopen::TimeoutException> >  
std::exception::what: SDO  
[canopen::tag_objectdict_key*] = 1400sub1

When looking at the candump regarding this object (1400h), I noticed (in the last two lines) that after the RPDO mapping for the drive with node id 2 is done, the wrong COB-id is tried to be written to the drive (201 instead of 202) and a SDO error is returned.

Candump

 (1622546939.737232)  can0  601   [8]  40 00 14 01 00 00 00 00
 (1622546939.737977)  can0  581   [8]  43 00 14 01 01 02 00 00
 (1622546939.738518)  can0  601   [8]  23 00 14 01 01 02 00 80
 (1622546939.739990)  can0  581   [8]  60 00 14 01 00 00 00 00
 (1622546939.740686)  can0  601   [8]  2F 00 16 00 00 00 00 00
 (1622546939.741979)  can0  581   [8]  60 00 16 00 00 00 00 00
 (1622546939.742785)  can0  601   [8]  23 00 16 01 10 00 40 60
 (1622546939.743980)  can0  581   [8]  60 00 16 01 00 00 00 00
 (1622546939.744874)  can0  601   [8]  2F 00 16 00 01 00 00 00
 (1622546939.745976)  can0  581   [8]  60 00 16 00 00 00 00 00
 (1622546939.746449)  can0  601   [8]  23 00 14 01 01 02 00 00
 (1622546939.747981)  can0  581   [8]  60 00 14 01 00 00 00 00
 (1622546939.885180)  can0  602   [8]  40 00 14 01 00 00 00 00
 (1622546939.885890)  can0  582   [8]  43 00 14 01 02 02 00 00
 (1622546939.887071)  can0  602   [8]  23 00 14 01 02 02 00 80
 (1622546939.887983)  can0  582   [8]  60 00 14 01 00 00 00 00
 (1622546939.888741)  can0  602   [8]  2F 00 16 00 00 00 00 00
 (1622546939.889833)  can0  582   [8]  60 00 16 00 00 00 00 00
 (1622546939.890892)  can0  602   [8]  23 00 16 01 10 00 40 60
 (1622546939.891857)  can0  582   [8]  60 00 16 01 00 00 00 00
 (1622546939.893035)  can0  602   [8]  2F 00 16 00 01 00 00 00
 (1622546939.893784)  can0  582   [8]  60 00 16 00 00 00 00 00  
 (1622546939.894444)  can0  602   [8]  23 00 14 01 01 02 00 00
 (1622546939.895778)  can0  582   [8]  80 00 14 01 30 00 09 06

When I remove the PDO mapping from my canopen.yaml, I obviously do not see this problem but I do notice that the control word for node 2 is send to the wrong COB-id (again to 201 instead of 202), the same holds for reading the status word. This practically let's ros_canopen control the CiA 402 Power State Machine twice for node id 1 and never for node id 2.

Is this a configuration issue or a bug? Any help would be appreciated!

System:

OS: Ubuntu 18.04 Kernel: 4.9.201-tegra ROS: melodic ros_canopen branch: melodic-devel Drives: Nanotec ST5918L3008-B Motor controller: Nanotec C5-E-1-09

mathias-luedtke commented 3 years ago

I think your EDS is wrong.

In line 1080, the COB-ID is hard-coded as 0x201:

DefaultValue=513

But it should depend on the NODEID:

DefaultValue=$NODEID+0x200

Please check it with CANeds.

PaulVerhoeckx commented 3 years ago

@ipa-mdl, thanks! Hard coded COB-ID's were indeed the problem.