write unit tests for the main component features

CMakeLists.txt

@@ -5,6 +5,8 @@ set(CMAKE_CXX_STANDARD 11)
 SET (CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/.orogen/config")
 INCLUDE(motors_weg_cvw300Base)
 
-# FIND_PACKAGE(KDL)
-# FIND_PACKAGE(OCL)
-
+if (ROCK_TEST_ENABLED)
+    enable_testing()
+    find_package(Syskit REQUIRED)
+    syskit_orogen_tests(test)
+endif()

manifest.xml

@@ -8,4 +8,6 @@
   <depend package="drivers/motors_weg_cvw300" />
   <depend package="base/orogen/types" />
   <depend package="drivers/orogen/iodrivers_base" />
+
+  <test_depend package="tools/syskit" />
 </package>

motors_weg_cvw300.orogen

@@ -1,3 +1,5 @@
+# frozen_string_literal: true
+
 name 'motors_weg_cvw300'
 
 import_types_from 'std'

tasks/Task.cpp

@@ -50,7 +50,9 @@ bool Task::configureHook()
     driver->writeControlType(_control_type.get());
 
     auto limits = _limits.get();
-    driver->writeJointLimits(limits.elements.at(0));
+    if (!limits.elements.empty()) {
+        driver->writeJointLimits(limits.elements.at(0));
+    }
     driver->writeRampConfiguration(_ramps.get());
 
     m_cmd_in.elements.resize(1);
@@ -67,9 +69,9 @@ bool Task::startHook()
         return false;
     }
 
+    writeSpeedCommand(0);
     m_driver->enable();
     m_last_temperature_update = Time();
-    writeSpeedCommand(0);
     return true;
 }
 bool Task::commandTimedOut() const {

test/modbus_helpers.rb

@@ -0,0 +1,120 @@
+# frozen_string_literal: true
+
+module ModbusHelpers
+    def setup
+        super
+
+        @modbus_registers = {}
+    end
+
+    def iodrivers_base_prepare(model)
+        task = syskit_deploy(model)
+        syskit_start_execution_agents(task)
+        reader = syskit_create_reader(task.io_raw_out_port, type: :buffer, size: 10)
+        writer = syskit_create_writer(task.io_raw_in_port)
+
+        [task, reader, writer]
+    end
+
+    def modbus_stop_task
+        expect_execution { task.stop! }
+            .join_all_waiting_work(false)
+            .poll do
+                while (sample = @reader.read_new)
+                    @writer.write(modbus_reply(sample))
+                end
+            end
+            .to { emit task.stop_event }
+    end
+
+    def modbus_expect_during_configuration_and_start
+        expect_execution
+            .scheduler(true)
+            .join_all_waiting_work(false)
+            .poll do
+                while (sample = @reader.read_new)
+                    @writer.write(modbus_reply(sample))
+                end
+            end
+    end
+
+    def modbus_configure_and_start
+        modbus_expect_during_configuration_and_start.to { emit task.start_event }
+    end
+
+    def modbus_set(register, value)
+        @modbus_registers[register] = value
+    end
+
+    def modbus_get(register)
+        @modbus_registers.fetch(register)
+    rescue KeyError
+        raise ArgumentError, "register #{register} not set"
+    end
+
+    def modbus_reply_until(writer, reader)
+        sample = nil
+        expect_execution
+            .join_all_waiting_work(false)
+            .poll do
+                while !sample && (s = reader.read_new)
+                    writer.write(modbus_reply(s))
+                    sample = s if yield(s)
+                end
+            end
+            .to { achieve { sample } }
+    end
+
+    def modbus_write?(sample, register: nil, value: nil)
+        return unless sample.data[1] == 6
+
+        actual_register = sample.data[2] << 8 | sample.data[3]
+        actual_value = sample.data[4] << 8 | sample.data[5]
+
+        (!register || register == actual_register) &&
+            (!value || value == actual_value)
+    end
+
+    def modbus_reply(sample)
+        reply = [sample.data[0], sample.data[1]]
+
+        function = sample.data[1]
+        if function == 6 # write
+            address = sample.data[2] << 8 | sample.data[3]
+            value = sample.data[4] << 8 | sample.data[5]
+            @modbus_registers[address] = value
+        elsif [3, 4].include?(function) # Read registers
+            start_address  = sample.data[2] << 8 | sample.data[3]
+            register_count = sample.data[4] << 8 | sample.data[5]
+
+            reply << register_count * 2
+            register_count.times do |i|
+                register_value = modbus_get(start_address + i)
+                reply += [register_value >> 8, register_value & 0xFF]
+            end
+        end
+        reply += modbus_crc(reply)
+        Types.iodrivers_base.RawPacket.new(time: Time.now, data: reply)
+    end
+
+    def modbus_reply_to_write(id)
+        reply = [id, 6]
+        reply += modbus_crc(reply)
+        Types.iodrivers_base.RawPacket.new(time: Time.now, data: reply)
+    end
+
+    def modbus_crc(data)
+        crc = 0xFFFF
+        data.each do |byte|
+            crc ^= byte
+
+            8.times do
+                odd = crc.odd?
+                crc >>= 1
+                crc ^= 0xA001 if odd
+            end
+        end
+
+        [crc & 0xFF, (crc & 0xFF00) >> 8]
+    end
+end

test/task_test.rb

@@ -0,0 +1,143 @@
+# frozen_string_literal: true
+
+require_relative 'modbus_helpers'
+
+using_task_library 'motors_weg_cvw300'
+
+describe OroGen.motors_weg_cvw300.Task do
+    run_live
+
+    attr_reader :task
+    attr_reader :reader
+
+    include ModbusHelpers
+
+    before do
+        @task, @reader, @writer = iodrivers_base_prepare(
+            OroGen.motors_weg_cvw300.Task
+                  .deployed_as('motors_weg_cvw300_test')
+        )
+
+        @task.properties.io_read_timeout = Time.at(2)
+        @task.properties.modbus_interframe_delay = Time.at(0.01)
+        @task.properties.watchdog do |sw|
+            sw.timeout = Time.at(0.5)
+            sw
+        end
+
+        modbus_set(405, 512) # number encoder pulses
+        modbus_set(401, 10) # nominal current
+        modbus_set(402, 2000) # nominal speed (rpm)
+        modbus_set(404, 2) # 12 kW
+
+        modbus_set(2, 0) # actual rotation (rpm)
+        modbus_set(3, 0) # actual current (A)
+        modbus_set(4, 0) # battery voltage (0.1 V)
+        modbus_set(5, 0) # output frequency (0.1 Hz)
+        modbus_set(6, 1) # inverter state (1 == Run)
+        modbus_set(7, 0) # output voltage (0.1V)
+        modbus_set(8, 0) # not used - vehicle speed
+        modbus_set(9, 0) # motor torque (0.1 % torque nominal)
+
+        modbus_set(30, 0) # mosfet temperature (0.1 C)
+        modbus_set(34, 0) # air temperature (0.1 C)
+    end
+
+    after do
+        modbus_stop_task if task.running?
+    end
+
+    describe 'configuration' do
+        it 'sets the control type' do
+            task.properties.control_type = :CONTROL_SENSORLESS
+            modbus_configure_and_start
+            assert_equal 1, modbus_get(202)
+        end
+
+        it 'sets the ramps' do
+            task.properties.ramps = {
+                acceleration_time: Time.at(5.1),
+                deceleration_time: Time.at(7.2),
+                type: 'RAMP_S_CURVE'
+            }
+            modbus_configure_and_start
+            assert_equal 5, modbus_get(100)
+            assert_equal 7, modbus_get(101)
+            assert_equal 1, modbus_get(104)
+        end
+
+        it 'sets the speed limits' do
+            rpm100 = 100 * 2 * Math::PI / 60
+            task.properties.limits = Types.base.JointLimits.new(
+                elements: [{
+                    min: Types.base.JointState.Speed(-rpm100),
+                    max: Types.base.JointState.Speed(rpm100)
+                }]
+            )
+
+            modbus_configure_and_start
+
+            assert_equal 100, modbus_get(134)
+        end
+    end
+
+    describe 'command watchdog' do
+        it 'periodically sends zero velocity commands' do
+            @task.properties.watchdog do |sw|
+                sw.timeout = Time.at(0.01)
+                sw
+            end
+            modbus_configure_and_start
+            10.times do
+                modbus_reply_until(@writer, @reader) do |sample|
+                    modbus_write?(sample, register: 683, value: 0)
+                end
+            end
+        end
+
+        it 'waits watchdog.timeout after a new command before it sends a new velocity command again' do
+            modbus_configure_and_start
+
+            cmd = Types.base.samples.Joints.new(
+                elements: [Types.base.JointState.Speed(1000 * 2 * Math::PI / 60)]
+            )
+            syskit_write task.cmd_in_port, cmd
+            modbus_reply_until(@writer, @reader) do |sample|
+                # Note: register 683 is expressed in ratio of nominal speed,
+                # with 0x2000 (8192) == 100%
+                modbus_write?(sample, register: 683, value: 0x1000)
+            end
+            tic = Time.now
+            modbus_reply_until(@writer, @reader) do |sample|
+                modbus_write?(sample, register: 683, value: 0)
+            end
+            assert((Time.now - tic) > 0.4)
+        end
+
+        it 'does not send zero velocity commnds if new commands are regularly sent' do
+            @task.properties.watchdog do |sw|
+                sw.timeout = Time.at(0.1)
+                sw
+            end
+            modbus_configure_and_start
+
+            cmd = Types.base.samples.Joints.new(
+                elements: [Types.base.JointState.Speed(1000 * 2 * Math::PI / 60)]
+            )
+
+            10.times do
+                syskit_write task.cmd_in_port, cmd
+                modbus_reply_until(@writer, @reader) do |sample|
+                    # Note: register 683 is expressed in ratio of nominal speed,
+                    # with 0x2000 (8192) == 100%
+                    if modbus_write?(sample, register: 683, value: 0)
+                        flunk('received zero velocity command')
+                    else
+                        modbus_write?(sample, register: 683, value: 0x1000)
+                        sleep 0.02
+                    end
+                end
+            end
+        end
+    end
+end