diff --git a/canopen_core/src/device_container.cpp b/canopen_core/src/device_container.cpp index 361e3c28..3c0faf01 100644 --- a/canopen_core/src/device_container.cpp +++ b/canopen_core/src/device_container.cpp @@ -49,6 +49,14 @@ bool DeviceContainer::load_component( opts.use_global_arguments(false); opts.use_intra_process_comms(true); std::vector remap_rules; + + std::string can_ns = this->get_namespace(); + if (!can_ns.empty()) + { + remap_rules.push_back("--ros-args"); + remap_rules.push_back("-r"); + remap_rules.push_back("__ns:=" + can_ns); + } remap_rules.push_back("--ros-args"); // remap_rules.push_back("--log-level"); // remap_rules.push_back("debug"); diff --git a/canopen_tests/CMakeLists.txt b/canopen_tests/CMakeLists.txt index ff995ec5..f2f7c052 100644 --- a/canopen_tests/CMakeLists.txt +++ b/canopen_tests/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(lely_core_libraries REQUIRED) generate_dcf(simple) generate_dcf(canopen_system) cogen_dcf(cia402_system) +cogen_dcf(cia402_namespaced_system) cogen_dcf(cia402) generate_dcf(cia402_lifecycle) cogen_dcf(cia402_diagnostics) diff --git a/canopen_tests/ROS_NAMESPACES.md b/canopen_tests/ROS_NAMESPACES.md new file mode 100644 index 00000000..e46c10aa --- /dev/null +++ b/canopen_tests/ROS_NAMESPACES.md @@ -0,0 +1,11 @@ +The `cia402_namespaced_system.launch.py` example demonstrates how to run a +CIA402 system in a namespace to allow multiple CANOpen-based robots in the +same ROS domain. This example is necessary because the controller names +are picked up from `ros2_controllers.yaml` and we must define their names +in that file with a `__namespace__/controller_name:` key, then use +ReplaceString from `nav2_common` to dynamically add the namespace to the +controller definition. + +For a proxy system, no example is included; it's sufficient to include, e.g. +the `proxy_setup.launch.py` launch description in another launch file and +push a namespace onto it with PushROSNamespace in the ordinary way. diff --git a/canopen_tests/config/cia402_namespaced_system/bus.yml b/canopen_tests/config/cia402_namespaced_system/bus.yml new file mode 100644 index 00000000..7b6f174c --- /dev/null +++ b/canopen_tests/config/cia402_namespaced_system/bus.yml @@ -0,0 +1,56 @@ +options: + dcf_path: "@BUS_CONFIG_PATH@" + +master: + node_id: 1 + driver: "ros2_canopen::MasterDriver" + package: "canopen_master_driver" + sync_period: 10000 + +defaults: + dcf: "cia402_slave.eds" + driver: "ros2_canopen::Cia402Driver" + package: "canopen_402_driver" + period: 10 + revision_number: 0 + sdo: + - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms + - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s + - {index: 0x6081, sub_index: 0, value: 1000} + - {index: 0x6083, sub_index: 0, value: 2000} + tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation + 1: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6041, sub_index: 0} # status word + - {index: 0x6061, sub_index: 0} # mode of operation display + 2: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6064, sub_index: 0} # position actual value + - {index: 0x606c, sub_index: 0} # velocity actual position + 3: + enabled: false + 4: + enabled: false + rpdo: # RPDO needed controlword, target position, target velocity, mode of operation + 1: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x6040, sub_index: 0} # controlword + - {index: 0x6060, sub_index: 0} # mode of operation + 2: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x607A, sub_index: 0} # target position + - {index: 0x60FF, sub_index: 0} # target velocity + +nodes: + cia402_device_1: + node_id: 2 diff --git a/canopen_tests/config/cia402_namespaced_system/cia402_slave.eds b/canopen_tests/config/cia402_namespaced_system/cia402_slave.eds new file mode 100644 index 00000000..24be3815 --- /dev/null +++ b/canopen_tests/config/cia402_namespaced_system/cia402_slave.eds @@ -0,0 +1,1441 @@ +[DeviceInfo] +VendorName=ROS-Industrial +VendorNumber=0x555 +ProductName=CIA402VTD +BaudRate_10=0 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +LSS_Supported=0 +Granularity=8 +SimpleBootUpSlave=1 +SimpleBootUpMaster=0 +NrOfRXPDO=4 +NrOfTXPDO=4 + +[Comments] +Lines=0 + +[DummyUsage] +Dummy0001=0 +Dummy0002=0 +Dummy0003=0 +Dummy0004=0 +Dummy0005=0 +Dummy0006=0 +Dummy0007=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[ManufacturerObjects] +SupportedObjects=0 + +[OptionalObjects] +SupportedObjects=67 +1=0x1005 +2=0x1008 +3=0x1009 +4=0x100A +5=0x100C +6=0x100D +7=0x1010 +8=0x1011 +9=0x1014 +10=0x1015 +11=0x1016 +12=0x1017 +13=0x1023 +14=0x1029 +15=0x1400 +16=0x1401 +17=0x1402 +18=0x1403 +19=0x1600 +20=0x1601 +21=0x1602 +22=0x1603 +23=0x1800 +24=0x1801 +25=0x1802 +26=0x1803 +27=0x1A00 +28=0x1A01 +29=0x1A02 +30=0x1A03 +31=0x6040 +32=0x6041 +33=0x605A +34=0x605B +35=0x605C +36=0x605D +37=0x605E +38=0x6060 +39=0x6061 +40=0x6062 +41=0x6063 +42=0x6064 +43=0x6065 +44=0x6067 +45=0x6068 +46=0x606A +47=0x606C +48=0x607A +49=0x607C +50=0x607D +51=0x6081 +52=0x6082 +53=0x6083 +54=0x6084 +55=0x6085 +56=0x608F +57=0x6098 +58=0x6099 +59=0x609A +60=0x60B0 +61=0x60B1 +62=0x60C2 +63=0x60F2 +64=0x60FD +65=0x60FF +66=0x6502 +67=0x67FF + +[1000] +ParameterName=Device Type +ObjectType=0x07 +DataType=0x0007 +AccessType=const +DefaultValue=0xFFFF0192 +PDOMapping=0 + +[1001] +ParameterName=Error Register +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1005] +ParameterName=COB-ID SYNC +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 +PDOMapping=0 + +[1008] +ParameterName=Manufacturer Device Name +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[1009] +ParameterName=Manufacturer Hardware Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100A] +ParameterName=Manufacturer Software Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100C] +ParameterName=Guard Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[100D] +ParameterName=Life Time Factor +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1010] +SubNumber=8 +ParameterName=Store Parameter Field +ObjectType=0x08 + +[1010sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x7 +PDOMapping=0 + +[1010sub1] +ParameterName=Save all Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub2] +ParameterName=Save Communication Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub3] +ParameterName=Save Device Profile Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub4] +ParameterName=Save Axis 0 Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub5] +ParameterName=Save Axis 1 Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub6] +ParameterName=Save Axis 2 Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub7] +ParameterName=Save Device Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1011] +SubNumber=8 +ParameterName=Restore Default Parameters +ObjectType=0x08 + +[1011sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x7 +PDOMapping=0 + +[1011sub1] +ParameterName=Restore all Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub2] +ParameterName=Restore Communication Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub3] +ParameterName=Restore Device Profile Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub4] +ParameterName=Restore Axis 0 Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub5] +ParameterName=Restore Axis 1 Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub6] +ParameterName=Restore Axis 2 Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1011sub7] +ParameterName=Restore Device Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1014] +ParameterName=COB-ID EMCY +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 +PDOMapping=0 + +[1015] +ParameterName=Inhibit Time Emergency +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1016] +SubNumber=2 +ParameterName=Heartbeat Consumer Entries +ObjectType=0x08 + +[1016sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x1 +PDOMapping=0 + +[1016sub1] +ParameterName=Consumer Heartbeat Time 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=number of entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x4 +PDOMapping=0 + +[1018sub1] +ParameterName=Vendor Id +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x555 +PDOMapping=0 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x0 +PDOMapping=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x0 +PDOMapping=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1023] +SubNumber=4 +ParameterName=OS Command +ObjectType=0x09 + +[1023sub0] +ParameterName=NumOfEntries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x3 +PDOMapping=0 + +[1023sub1] +ParameterName=Command +ObjectType=0x07 +DataType=0x000A +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1023sub2] +ParameterName=Status +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1023sub3] +ParameterName=Reply +ObjectType=0x07 +DataType=0x000A +AccessType=ro +PDOMapping=0 + +[1029] +SubNumber=3 +ParameterName=Error Behaviour +ObjectType=0x08 + +[1029sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x2 +PDOMapping=0 + +[1029sub1] +ParameterName=Communication Error +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1029sub2] +ParameterName=Application Error +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x1 +PDOMapping=0 + +[1400] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 1 +ObjectType=0x09 + +[1400sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1400sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000200 +PDOMapping=0 + +[1400sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 + +[1401] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 2 +ObjectType=0x09 + +[1401sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1401sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000300 +PDOMapping=0 + +[1401sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 + +[1402] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 3 +ObjectType=0x09 + +[1402sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1402sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000400 +PDOMapping=0 + +[1402sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 + +[1403] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 4 +ObjectType=0x09 + +[1403sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1403sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000500 +PDOMapping=0 + +[1403sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFE +PDOMapping=0 + +[1600] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 1 +ObjectType=0x09 + +[1600sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1600sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1600sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1600sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 2 +ObjectType=0x09 + +[1601sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1601sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1601sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60600008 +PDOMapping=0 + +[1601sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 3 +ObjectType=0x09 + +[1602sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1602sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1602sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x607A0020 +PDOMapping=0 + +[1602sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 4 +ObjectType=0x09 + +[1603sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1603sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1603sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60FF0020 +PDOMapping=0 + +[1603sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1800] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 1 +ObjectType=0x09 + +[1800sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1800sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000180 +PDOMapping=0 + +[1800sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1800sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1800sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1800sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1801] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 2 +ObjectType=0x09 + +[1801sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1801sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000280 +PDOMapping=0 + +[1801sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1801sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1801sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1801sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1802] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 3 +ObjectType=0x09 + +[1802sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1802sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000380 +PDOMapping=0 + +[1802sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x1 +PDOMapping=0 + +[1802sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1802sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1802sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1803] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 4 +ObjectType=0x09 + +[1803sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1803sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000480 +PDOMapping=0 + +[1803sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFE +PDOMapping=0 + +[1803sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1803sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1803sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1A00] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 1 +ObjectType=0x09 + +[1A00sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1A00sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A00sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 2 +ObjectType=0x09 + +[1A01sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1A01sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A01sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60610008 +PDOMapping=0 + +[1A01sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 3 +ObjectType=0x09 + +[1A02sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1A02sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A02sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60640020 +PDOMapping=0 + +[1A02sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 4 +ObjectType=0x09 + +[1A03sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1A03sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A03sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x606C0020 +PDOMapping=0 + +[1A03sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[6040] +ParameterName=Controlword 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[6041] +ParameterName=Statusword 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=ro +PDOMapping=1 + +[605A] +ParameterName=Quick Stop Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=rw +DefaultValue=0x0002 +PDOMapping=0 + +[605B] +ParameterName=Shutdown Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +DefaultValue=0x0000 +PDOMapping=0 + +[605C] +ParameterName=Disable Operation Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +DefaultValue=0x0001 +PDOMapping=0 + +[605D] +ParameterName=Halt Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=rw +DefaultValue=0x0001 +PDOMapping=0 + +[605E] +ParameterName=Fault Reaction Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +DefaultValue=0x0002 +PDOMapping=0 + +[6060] +ParameterName=Modes of Operation 1 +ObjectType=0x07 +DataType=0x0002 +AccessType=rww +DefaultValue=0x00 +PDOMapping=1 + +[6061] +ParameterName=Modes of Operation Display 1 +ObjectType=0x07 +DataType=0x0002 +AccessType=ro +PDOMapping=1 + +[6062] +ParameterName=Position Demand Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6063] +ParameterName=Position Actual Internal Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6064] +ParameterName=Position Actual Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6065] +ParameterName=Following Error Window 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6067] +ParameterName=Position Window 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0xFFFFFFFF +PDOMapping=0 + +[6068] +ParameterName=Position Window Time 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[606A] +ParameterName=Sensor Selection Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=rw +PDOMapping=0 + +[606C] +ParameterName=Velocity Actual Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[607A] +ParameterName=Target Position 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rww +DefaultValue=0x0 +PDOMapping=1 + +[607C] +ParameterName=Home Offset 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[607D] +SubNumber=3 +ParameterName=Software Position Limit 1 +ObjectType=0x08 + +[607Dsub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=5 +AccessType=const +DefaultValue=0x2 +PDOMapping=0 + +[607Dsub1] +ParameterName=Min Software Position Limit +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=-2147483648 +PDOMapping=0 + +[607Dsub2] +ParameterName=Max Software Position Limit +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=2147483647 +PDOMapping=0 + +[6081] +ParameterName=Profile Velocity in pp-mode 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6082] +ParameterName=End velocity 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[6083] +ParameterName=Profile Acceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6084] +ParameterName=Profile Deceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6085] +ParameterName=Quick Stop Deceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[608F] +SubNumber=3 +ParameterName=Position Encoder Resolution 1 +ObjectType=0x08 + +[608Fsub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=5 +AccessType=const +DefaultValue=0x00000002 +PDOMapping=0 + +[608Fsub1] +ParameterName=Encoder Increments +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[608Fsub2] +ParameterName=Motor Revolutions +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000001 +PDOMapping=0 + +[6098] +ParameterName=Homing Method 1 +ObjectType=0x07 +DataType=0x0002 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[6099] +SubNumber=3 +ParameterName=Homing Speeds 1 +ObjectType=0x08 + +[6099sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=5 +AccessType=const +DefaultValue=0x2 +PDOMapping=0 + +[6099sub1] +ParameterName=Fast Homing Speed +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6099sub2] +ParameterName=Slow Homing Speed +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[609A] +ParameterName=Homing Acceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[60B0] +ParameterName=Position Offset 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[60B1] +ParameterName=Velocity Offset 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[60C2] +SubNumber=3 +ParameterName=Interpolation Time Period 1 +ObjectType=0x09 + +[60C2sub0] +ParameterName=NumOfEntries +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x02 +PDOMapping=0 + +[60C2sub1] +ParameterName=timeUnits +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x1 +PDOMapping=0 + +[60C2sub2] +ParameterName=timeIndex +ObjectType=0x07 +DataType=0x0002 +AccessType=rw +DefaultValue=-2 +PDOMapping=0 + +[60F2] +ParameterName=Positioning Option Code 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[60FD] +ParameterName=Digital Inputs 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=1 + +[60FF] +ParameterName=Target Velocity 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x0 +PDOMapping=1 + +[6502] +ParameterName=Supported Drive Modes 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x000000A5 +PDOMapping=0 + +[67FF] +ParameterName=Single Device Type 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00040192 +PDOMapping=0 diff --git a/canopen_tests/config/cia402_namespaced_system/ros2_controllers.yaml b/canopen_tests/config/cia402_namespaced_system/ros2_controllers.yaml new file mode 100644 index 00000000..5ccc0e3a --- /dev/null +++ b/canopen_tests/config/cia402_namespaced_system/ros2_controllers.yaml @@ -0,0 +1,42 @@ +__namespace__/controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + cia402_device_1_controller: + type: canopen_ros2_controllers/Cia402DeviceController + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + +__namespace__/cia402_device_1_controller: + ros__parameters: + joint: node_1 + +__namespace__/forward_position_controller: + ros__parameters: + joints: + - node_1 + interface_name: position + +__namespace__/joint_trajectory_controller: + ros__parameters: + joints: + - node_1 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + node_1: { trajectory: 0.2, goal: 0.1 } diff --git a/canopen_tests/launch/cia402_namespaced_system.launch.py b/canopen_tests/launch/cia402_namespaced_system.launch.py new file mode 100644 index 00000000..e2f7c5e9 --- /dev/null +++ b/canopen_tests/launch/cia402_namespaced_system.launch.py @@ -0,0 +1,295 @@ +# Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# +# Author: Lovro Ivanov lovro.ivanov@gmail.com + +from launch import LaunchDescription +import launch.actions +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from nav2_common.launch import ReplaceString +from launch.actions import GroupAction +from launch_ros.actions import PushROSNamespace + + +def launch_setup(context, *args, **kwargs): + + name = LaunchConfiguration("name") + prefix = LaunchConfiguration("prefix") + + bot_ns = LaunchConfiguration("bot_ns") + + # bus configuration + bus_config_package = LaunchConfiguration("bus_config_package") + bus_config_directory = LaunchConfiguration("bus_config_directory") + bus_config_file = LaunchConfiguration("bus_config_file") + # bus configuration file full path + bus_config = PathJoinSubstitution( + [FindPackageShare(bus_config_package), bus_config_directory, bus_config_file] + ) + + # master configuration + master_config_package = LaunchConfiguration("master_config_package") + master_config_directory = LaunchConfiguration("master_config_directory") + master_config_file = LaunchConfiguration("master_config_file") + # master configuration file full path + master_config = PathJoinSubstitution( + [FindPackageShare(master_config_package), master_config_directory, master_config_file] + ) + + # can interface name + can_interface_name = LaunchConfiguration("can_interface_name") + + # robot description stuff + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", "cia402_system", description_file] + ), + " ", + "name:=", + name, + " ", + "prefix:=", + prefix, + " ", + "bus_config:=", + bus_config, + " ", + "master_config:=", + master_config, + " ", + "can_interface_name:=", + can_interface_name, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content} + + # ros2 control configuration + ros2_control_config_package = LaunchConfiguration("ros2_control_config_package") + ros2_control_config_directory = LaunchConfiguration("ros2_control_config_directory") + ros2_control_config_file = LaunchConfiguration("ros2_control_config_file") + # ros2 control configuration file full path + ros2_control_config = PathJoinSubstitution( + [ + FindPackageShare(ros2_control_config_package), + ros2_control_config_directory, + ros2_control_config_file, + ] + ) + + # Our controllers get their names from the control config, so we have to + # include their namespace here to get them to start up within it. + ros2_control_config = ReplaceString( + source_file=ros2_control_config, replacements={"__namespace__/": (bot_ns, "/")} + ) + + # nodes to start are listed below + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_control_config], + output="screen", + ) + + # load one controller just to make sure it can connect to controller_manager + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ) + + cia402_device_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "cia402_device_1_controller", + ], + ) + + forward_position_controller = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "forward_position_controller", + ], + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # hardcoded slave configuration form test package + slave_config = PathJoinSubstitution( + [FindPackageShare("canopen_tests"), "config/cia402", "cia402_slave.eds"] + ) + + slave_launch = PathJoinSubstitution( + [FindPackageShare("canopen_fake_slaves"), "launch", "cia402_slave.launch.py"] + ) + slave_node_1 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slave_launch), + launch_arguments={ + "node_id": "2", + "node_name": "cia402_node_1", + "slave_config": slave_config, + }.items(), + ) + + namespaced_nodes = [ + GroupAction( + actions=[ + PushROSNamespace(bot_ns), + control_node, + robot_state_publisher_node, + joint_state_broadcaster_spawner, + slave_node_1, + cia402_device_controller_spawner, + forward_position_controller, + ] + ) + ] + return namespaced_nodes + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument("bot_ns", description="Namespace for these nodes", default_value="") + ) + declared_arguments.append( + DeclareLaunchArgument( + "name", description="robot name", default_value="canopen_test_system" + ) + ) + declared_arguments.append( + DeclareLaunchArgument("prefix", description="Prefix.", default_value="") + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + description="Package where urdf file is stored.", + default_value="canopen_tests", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + description="Name of the urdf file.", + default_value="cia402_system.urdf.xacro", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ros2_control_config_package", + default_value="canopen_tests", + description="Path to ros2_control configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ros2_control_config_directory", + default_value="config/cia402_namespaced_system", + description="Path to ros2_control configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ros2_control_config_file", + default_value="ros2_controllers.yaml", + description="Path to ros2_control configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "bus_config_package", + default_value="canopen_tests", + description="Path to bus configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "bus_config_directory", + default_value="config/cia402_system", + description="Path to bus configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "bus_config_file", + default_value="bus.yml", + description="Path to bus configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "master_config_package", + default_value="canopen_tests", + description="Path to master configuration file (*.dcf)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "master_config_directory", + default_value="config/cia402_system", + description="Path to master configuration file (*.dcf)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "master_config_file", + default_value="master.dcf", + description="Path to master configuration file (*.dcf)", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "can_interface_name", + default_value="vcan0", + description="Interface name for can", + ) + ) + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])