ros2_control PDO commands

.pre-commit-config.yaml

@@ -47,7 +47,7 @@ repos:
         entry: codespell
         args:
           [
-            "--exclude-file=panther_hardware_interfaces/config/roboteq_motor_controllers_v80_21.eds",
+            "--exclude-file=panther_hardware_interfaces/config/roboteq_motor_controllers_v80_21a.eds",
           ]
         language: python
         types: [text]

panther_controller/config/WH01_controller.yaml

@@ -2,7 +2,7 @@
   controller_manager:
     ros__parameters:
       use_sim_time: false
-      update_rate: 50
+      update_rate: 100
 
       joint_state_broadcaster:
         type: joint_state_broadcaster/JointStateBroadcaster
@@ -31,7 +31,7 @@
       left_wheel_radius_multiplier: 1.0
       right_wheel_radius_multiplier: 1.0
 
-      publish_rate: 50.0
+      publish_rate: 100.0
       odom_frame_id: odom
       base_frame_id: base_link
       pose_covariance_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]

panther_controller/config/WH02_controller.yaml

@@ -2,7 +2,7 @@
   controller_manager:
     ros__parameters:
       use_sim_time: false
-      update_rate: 50
+      update_rate: 100
 
       joint_state_broadcaster:
         type: joint_state_broadcaster/JointStateBroadcaster

panther_controller/config/WH04_controller.yaml

@@ -2,7 +2,7 @@
   controller_manager:
     ros__parameters:
       use_sim_time: false
-      update_rate: 50
+      update_rate: 100
 
       joint_state_broadcaster:
         type: joint_state_broadcaster/JointStateBroadcaster

panther_description/urdf/panther_macro.urdf.xacro

@@ -97,19 +97,25 @@
           <param name="master_can_id">3</param>
           <param name="front_driver_can_id">1</param>
           <param name="rear_driver_can_id">2</param>
-          <!-- Shouldn't but can depend on frequencies of publishing PDO (set on Roboteq) and
-            frequency of controller (frequency of publishing commands) -->
-          <param name="sdo_operation_timeout_ms">3</param>
-          <!-- Depends on frequency of the controller -->
-          <param name="pdo_feedback_timeout_ms">30</param>
+          <param name="sdo_operation_timeout_ms">100</param>
+
+          <!-- Depends on frequency of the controller, more critical motors state are sent with
+           higher frequency, other parameters are sent with lower, here allowed time is set
+           to be expected period +50% margin -->
+          <param name="pdo_motor_states_timeout_ms">15</param>
+          <param name="pdo_driver_state_timeout_ms">75</param>
+
+          <!-- It will be rounded to the closest value taking into account current controller frequency -->
+          <param name="driver_states_update_frequency">20.0</param>
 
           <param name="max_roboteq_initialization_attempts">5</param>
           <param name="max_roboteq_activation_attempts">5</param>
           <param name="max_safety_stop_attempts">20</param>
 
-          <param name="max_write_sdo_errors_count">2</param>
-          <param name="max_read_sdo_errors_count">2</param>
-          <param name="max_read_pdo_errors_count">2</param>
+          <!-- TODO after all tests update parameters, these ones are quite high for the worst case scenario -->
+          <param name="max_write_pdo_cmds_errors_count">4</param>
+          <param name="max_read_pdo_motor_states_errors_count">4</param>
+          <param name="max_read_pdo_driver_state_errors_count">20</param>
         </xacro:unless>
       </hardware>
 

panther_hardware_interfaces/CMakeLists.txt

@@ -80,6 +80,9 @@ if(BUILD_TESTING)
   target_include_directories(${PROJECT_NAME}_test_roboteq_error_filter
                              PRIVATE include)
 
+  ament_add_gtest(${PROJECT_NAME}_test_utils test/test_utils.cpp src/utils.cpp)
+  target_include_directories(${PROJECT_NAME}_test_utils PRIVATE include)
+
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_data_converters
     test/test_roboteq_data_converters.cpp src/roboteq_data_converters.cpp
@@ -90,13 +93,10 @@ if(BUILD_TESTING)
   ament_target_dependencies(${PROJECT_NAME}_test_roboteq_data_converters
                             panther_msgs)
 
-  ament_add_gtest(${PROJECT_NAME}_test_utils test/test_utils.cpp src/utils.cpp)
-  target_include_directories(${PROJECT_NAME}_test_utils PRIVATE include)
-
   ament_add_gtest(
     ${PROJECT_NAME}_test_canopen_controller test/test_canopen_controller.cpp
-    test/src/roboteq_mock.cpp src/canopen_controller.cpp src/roboteq_driver.cpp
-    src/utils.cpp)
+    test/src/roboteqs_mock.cpp src/canopen_controller.cpp
+    src/roboteq_driver.cpp src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_canopen_controller
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test/include> include)
@@ -107,8 +107,8 @@ if(BUILD_TESTING)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_roboteq_driver test/test_roboteq_driver.cpp
-    test/src/roboteq_mock.cpp src/canopen_controller.cpp src/roboteq_driver.cpp
-    src/utils.cpp)
+    test/src/roboteqs_mock.cpp src/canopen_controller.cpp
+    src/roboteq_driver.cpp src/utils.cpp)
   target_include_directories(
     ${PROJECT_NAME}_test_roboteq_driver
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/test/include> include)
@@ -120,7 +120,7 @@ if(BUILD_TESTING)
   ament_add_gtest(
     ${PROJECT_NAME}_test_motors_controller
     test/test_motors_controller.cpp
-    test/src/roboteq_mock.cpp
+    test/src/roboteqs_mock.cpp
     src/canopen_controller.cpp
     src/roboteq_driver.cpp
     src/roboteq_data_converters.cpp
@@ -149,7 +149,7 @@ if(BUILD_TESTING)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_panther_system test/test_panther_system.cpp
-    test/src/roboteq_mock.cpp test/src/panther_system_test_utils.cpp)
+    test/src/roboteqs_mock.cpp test/src/panther_system_test_utils.cpp)
   set_tests_properties(${PROJECT_NAME}_test_panther_system PROPERTIES TIMEOUT
                                                                       120)
   target_include_directories(

panther_hardware_interfaces/CODE_STRUCTURE.md

@@ -4,11 +4,14 @@ A brief introduction to the code structure of the Panther system.
 
 ## RoboteqDriver
 
-Low-level CANopen driver implementing FiberDriver from [Lely](https://opensource.lely.com/canopen/) ([here](https://en.wikipedia.org/wiki/Fiber_%28computer_science%29) you can read more about fibers). It takes care of translating CANopen indexes into meaningful data. It handles PDO and SDO communication and provides methods for sending commands and reading all the useful parameters from the Roboteq drivers. It saves the timestamp of the last RPDO, which can be later used to detect timeout errors.
+Low-level CANopen driver implementing LoopDriver from [Lely](https://opensource.lely.com/canopen/).
+It takes care of translating CANopen indexes into meaningful data.
+Provided methods can be used for sending commands and reading all the useful parameters from the Roboteq drivers (they abstract low level SDO and PDO communication).
+Timestamp of all received PDO data is also saved, which can be later used for detecting timeout errors.
 
 ## CANopenController
 
-Takes care of CANopen communication - creates and initializes master controller and two Roboteq drivers (front and rear). For handling CANopen communication separate thread is created.
+Takes care of CANopen communication - creates and initializes master controller and two Roboteq drivers (front and rear). For handling CANopen communication separate thread is created with configurable RT priority (additionally two threads for each driver is also created).
 
 ## MotorsController
 
@@ -35,7 +38,7 @@ Feedback converters are combined in the `RoboteqData` class to provide the full
 A class that keeps track of different types of errors. In some rare cases, Roboteq controllers can miss for example the SDO response, or PDO can be received a bit later, which results in a timeout.
 As they usually are rare and singular occurrences, it is better to filter some of these errors and escalate only when a certain number of errors happen.
 
-## GpioDriver
+## GPIODriver
 
 WIP - it will handle reading/writing pins of the RPi GPIO.
 
@@ -46,5 +49,5 @@ A class that takes care of additional ROS interface of panther system, such as p
 ## PantherSystem
 
 The main class that implements SystemInterface from ros2_control (for details refer to the [ros2_control documentation](https://control.ros.org/master/index.html)).
-
-<!-- todo: when an exception is thrown it is not RT safe (situation may change when we switch to PDO - on hold) -->
+Handles transitions (initialization, activation, shutdown, error, etc.), provides interfaces for feedback (position, velocity, effort) and commands (velocity).
+In the main loop controller should call `read` and `write` functions to communicate with motor drivers.

panther_hardware_interfaces/README.md

@@ -32,15 +32,15 @@ That said apart from the usual interface provided by the ros2_control, this plug
 
 [//]: # (ROS_API_NODE_PUBLISHERS_START)
 
-- `/panther_system_node/driver/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers' state and error flags
+- `/panther_system_node/driver/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers' state and error flags.
 
 [//]: # (ROS_API_NODE_PUBLISHERS_END)
 
 #### Service Servers
 
 [//]: # (ROS_API_NODE_SERVICE_SERVERS_START)
 
-- `/panther_system_node/clear_errors` [*std_srvs/Trigger*]: clear current errors
+- `/panther_system_node/clear_errors` [*std_srvs/Trigger*]: clear current errors.
 
 [//]: # (ROS_API_NODE_SERVICE_SERVERS_END)
 
@@ -51,28 +51,32 @@ That said apart from the usual interface provided by the ros2_control, this plug
 Parameters that are required, are defined when including interface in URDF (you can check out [panther_macro.urdf.xacro](../panther_description/urdf/panther_macro.urdf.xacro)).
 
 Physical properties
- - `encoder_resolution` [*int*, default: 1600]: property of the encoder used, shouldn't be changed
- - `gear_ratio` [*float*, default: 30.08]: property of the gearbox used, shouldn't be changed
- - `motor_torque_constant` [*float*, default: 0.11]: same as set in the Roboteq driver (TNM parameter), also shouldn't be changed, as it is measured property of the motor
- - `max_rpm_motor_speed` [*float*, default: 3600.0]: max RPM speed set in the Roboteq driver (MXRPM parameter)
- - `gearbox_efficiency` [*float*, default: 0.75]: measured efficiency, used for converting read current to torque, can vary depending on different factors such as temperature and wear
+ - `encoder_resolution` [*int*, default: **1600**]: property of the encoder used, shouldn't be changed.
+ - `gear_ratio` [*float*, default: **30.08**]: property of the gearbox used, shouldn't be changed.
+ - `motor_torque_constant` [*float*, default: **0.11**]: same as set in the Roboteq driver (TNM parameter), also shouldn't be changed, as it is measured property of the motor.
+ - `max_rpm_motor_speed` [*float*, default: **3600.0**]: max RPM speed set in the Roboteq driver (MXRPM parameter).
+ - `gearbox_efficiency` [*float*, default: **0.75**]: measured efficiency, used for converting read current to torque, can vary depending on different factors such as temperature and wear.
 
 CAN settings
- - `can_interface_name` [*string*, default: panther_can]: name of the CAN interface
- - `master_can_id` [*int*, default: 3]: CAN ID of the master device (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml))
- - `front_driver_can_id` [*int*, default: 1]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml))
- - `rear_driver_can_id` [*int*, default: 2]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml))
- - `sdo_operation_timeout_ms` [*int*, default: 3 [ms]]: it is set so that the full controller loop takes up to the required time. Each controller loop contains five SDO operations (four writes and one read). For example, in a 100Hz loop, there is up to 10ms for all operations. This timeout should be set so that in the worst case everything takes 10ms.
- - `pdo_feedback_timeout_ms` [*int*, default: 30 [ms]]  - depends on the frequency at which Roboteq is configured to send PDO data. At 100Hz there should be 10ms between received data if it takes more than `pdo_feedback_timeout_ms`, a PDO read error is triggered
- - `max_roboteq_initialization_attempts` [*int*, default: 5]: in some cases, an SDO error can happen during initialization, it is possible to configure more attempts, before escalating to general error
- - `max_roboteq_activation_attempts` [*int*, default: 5]: similar to initialization, it is possible to allow some SDO errors before escalating to error
- - `max_safety_stop_attempts` [*int*, default: 20]: how many attempts to activate safety stop will be taken before failing
- - `max_write_sdo_errors_count` [*int*, default: 2]: how many consecutive errors can happen before escalating to general error
- - `max_read_sdo_errors_count` [*int*, default: 2]: how many consecutive errors can happen before escalating to general error
- - `max_read_pdo_errors_count` [*int*, default: 2]: how many consecutive errors can happen before escalating to general error
+ - `can_interface_name` [*string*, default: **panther_can**]: name of the CAN interface.
+ - `master_can_id` [*int*, default: **3**]: CAN ID of the master device (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
+ - `front_driver_can_id` [*int*, default: **1**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
+ - `rear_driver_can_id` [*int*, default: **2**]: CAN ID defined in the properties of Roboteq (set as in [canopen_configuration.yaml](./config/canopen_configuration.yaml)).
+ - `sdo_operation_timeout_ms` [*int*, default: **100**]: timeout of the SDO operations, currently no SDO operation is required in RT operation, so this timeout can be set to a higher value.
+ - `pdo_motor_states_timeout_ms` [*int*, default: **15**]: depends on the frequency at which Roboteq is configured to send motor states (PDO 1 and 2) data. By default, there should be 10 **[ms]** between received data, if it takes more than `pdo_motor_states_timeout_ms`, a motor states read error is triggered. The default value is set to be expected period +50% margin.
+ - `pdo_driver_state_timeout_ms` [*int*, default: **75**]: depends on the frequency at which Roboteq is configured to send driver state (PDO 3 and 4) data. By default, there should be 50 **[ms]** between received data, if it takes more than `pdo_driver_state_timeout_ms`, a driver state read error is triggered. The default value is set to be expected period +50% margin.
+ - `driver_states_update_frequency` [*float*, default: **20.0**]: as by default, the driver state is published with lower frequency, it also shouldn't be updated with every controller loop iteration. The exact frequency at which driver state is published won't match this value - it will also depend on the frequency of the controller (the exact value of the period can be calculated with the following formula `controller_frequency / ceil(controller_frequency / driver_states_update_frequency)`).
+ - `max_roboteq_initialization_attempts` [*int*, default: **5**]: in some cases, an SDO error can happen during initialization, it is possible to configure more attempts, before escalating to a general error.
+ - `max_roboteq_activation_attempts` [*int*, default: **5**]: similar to initialization, it is possible to allow some SDO errors before escalating to error.
+ <!-- TODO: with GPIODriver it should no longer be needed -->
+ - `max_safety_stop_attempts` [*int*, default: **20**]: how many attempts to activate safety stop will be taken before failing.
+ - `max_write_pdo_cmds_errors_count` [*int*, default: **4**]: how many consecutive errors can happen before escalating to general error.
+ - `max_read_pdo_motor_states_errors_count` [*int*, default: **4**]: how many consecutive errors can happen before escalating to general error.
+ - `max_read_pdo_driver_state_errors_count` [*int*, default: **20**]: how many consecutive errors can happen before escalating to general error.
+
 
 > [!CAUTION]
-> `max_write_sdo_errors_count`, `max_read_sdo_errors_count`, `max_read_pdo_errors_count`, `max_safety_stop_attempts`, `sdo_operation_timeout_ms` and `pdo_feedback_timeout_ms` are safety-critical parameters, they should be changed only in very specific cases, be sure that you know how they work and be really cautious when changing them.
+> `max_write_pdo_cmds_errors_count`, `max_read_pdo_motor_states_errors_count`, `max_read_pdo_driver_state_errors_count`, `max_safety_stop_attempts`, `sdo_operation_timeout`, `pdo_motor_states_timeout_ms` and `pdo_driver_state_timeout_ms` are safety-critical parameters, they should be changed only in very specific cases, be sure that you know how they work and be really cautious when changing them.
 
 [//]: # (ROS_API_NODE_PARAMETERS_END)
 [//]: # (ROS_API_NODE_END)
@@ -136,4 +140,4 @@ Copy eds file to config and run
 ```bash
 dcfgen canopen_configuration.yaml -r
 ```
-Remove master.dcf
+Remove `master.dcf`

panther_hardware_interfaces/config/canopen_configuration.yaml

@@ -3,9 +3,9 @@ master:
   sync_period: 1000000 # us
 
 slave_1:
-  dcf: "roboteq_motor_controllers_v80_21.eds"
+  dcf: "roboteq_motor_controllers_v80_21a.eds"
   node_id: 1
 
 slave_2:
-  dcf: "roboteq_motor_controllers_v80_21.eds"
+  dcf: "roboteq_motor_controllers_v80_21a.eds"
   node_id: 2