From 14f1d48198d2a6ebf5deaca56798b908dda51658 Mon Sep 17 00:00:00 2001 From: Debora Date: Tue, 2 Apr 2024 14:36:22 -0300 Subject: [PATCH 1/3] feat: Add output port for the saturation signal --- manifest.xml | 2 ++ motors_weg_cvw300.orogen | 5 +++++ tasks/Task.cpp | 22 +++++++++++++++++++--- tasks/Task.hpp | 2 ++ 4 files changed, 28 insertions(+), 3 deletions(-) diff --git a/manifest.xml b/manifest.xml index b6eab70..a932be7 100644 --- a/manifest.xml +++ b/manifest.xml @@ -8,6 +8,8 @@ + + diff --git a/motors_weg_cvw300.orogen b/motors_weg_cvw300.orogen index 7c58a7f..28bdff8 100644 --- a/motors_weg_cvw300.orogen +++ b/motors_weg_cvw300.orogen @@ -13,6 +13,9 @@ import_types_from "motors_weg_cvw300/FaultState.hpp" import_types_from "motors_weg_cvw300Types.hpp" using_task_library "iodrivers_base" +using_library "usv_control" +import_types_from "usv_control" +import_types_from "usv_control/SaturationSignal.hpp" task_context "Task", subclasses: "iodrivers_base::Task" do needs_configuration @@ -83,6 +86,8 @@ task_context "Task", subclasses: "iodrivers_base::Task" do output_port "alarm_state", "/motors_weg_cvw300/AlarmState" output_port "fault_state", "/motors_weg_cvw300/FaultState" + output_port "saturation_signal", "/usv_control/SaturationSignal" + exception_states "INVALID_COMMAND_SIZE", "INVALID_COMMAND_PARAMETER", "CONTROLLER_FAULT", "CONTROLLER_UNDER_VOLTAGE" diff --git a/tasks/Task.cpp b/tasks/Task.cpp index e76d27a..501918c 100644 --- a/tasks/Task.cpp +++ b/tasks/Task.cpp @@ -7,6 +7,7 @@ using namespace std; using namespace base; using namespace motors_weg_cvw300; +using namespace usv_control; Task::Task(std::string const& name) : TaskBase(name) @@ -55,9 +56,9 @@ bool Task::configureHook() m_cmd_timeout = wd.timeout; driver->writeControlType(_control_type.get()); - auto limits = _limits.get(); - if (!limits.elements.empty()) { - driver->writeJointLimits(limits.elements.at(0)); + m_limits = _limits.get(); + if (!m_limits.elements.empty()) { + driver->writeJointLimits(m_limits.elements.at(0)); } driver->writeRampConfiguration(_ramps.get()); @@ -95,6 +96,16 @@ void Task::writeSpeedCommand(float cmd) m_cmd_deadline = base::Time::now() + m_cmd_timeout; } } +bool Task::checkSpeedSaturation(base::commands::Joints const& cmd) +{ + if (!m_limits.elements.empty()) { + if (cmd.elements[0].speed >= m_limits.elements[0].max.speed) { + return true; + } + } + return false; +} + void Task::updateHook() { while (_cmd_in.read(m_cmd_in) == RTT::NewData) { @@ -108,6 +119,11 @@ void Task::updateHook() } writeSpeedCommand(joint.speed); + + SaturationSignal saturation_signal; + saturation_signal.value = checkSpeedSaturation(m_cmd_in); + saturation_signal.time = m_cmd_in.time; + _saturation_signal.write(saturation_signal); } if (commandTimedOut()) { diff --git a/tasks/Task.hpp b/tasks/Task.hpp index d0e8a47..5fd83ad 100644 --- a/tasks/Task.hpp +++ b/tasks/Task.hpp @@ -33,6 +33,7 @@ namespace motors_weg_cvw300 { protected: base::commands::Joints m_cmd_in; base::samples::Joints m_sample; + base::JointLimits m_limits; base::Time m_last_temperature_update; base::Time m_cmd_timeout; base::Time m_cmd_deadline; @@ -44,6 +45,7 @@ namespace motors_weg_cvw300 { void writeSpeedCommand(float cmd); bool commandTimedOut() const; void publishFault(); + bool checkSpeedSaturation(base::commands::Joints const& cmd); public: /** TaskContext constructor for Task From 977529c45095357175201994eb26d66fa0e3204c Mon Sep 17 00:00:00 2001 From: Debora Date: Wed, 3 Apr 2024 15:57:40 -0300 Subject: [PATCH 2/3] feat: Unit tests for saturation signal port --- tasks/Task.cpp | 16 +++++++++------- test/task_test.rb | 48 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 57 insertions(+), 7 deletions(-) diff --git a/tasks/Task.cpp b/tasks/Task.cpp index 501918c..06e425a 100644 --- a/tasks/Task.cpp +++ b/tasks/Task.cpp @@ -57,9 +57,15 @@ bool Task::configureHook() driver->writeControlType(_control_type.get()); m_limits = _limits.get(); - if (!m_limits.elements.empty()) { + if (m_limits.elements.empty()) { + // Initialize limits as infinity + JointLimits infinity; + m_limits = infinity; + } + else { driver->writeJointLimits(m_limits.elements.at(0)); } + driver->writeRampConfiguration(_ramps.get()); m_cmd_in.elements.resize(1); @@ -98,12 +104,8 @@ void Task::writeSpeedCommand(float cmd) } bool Task::checkSpeedSaturation(base::commands::Joints const& cmd) { - if (!m_limits.elements.empty()) { - if (cmd.elements[0].speed >= m_limits.elements[0].max.speed) { - return true; - } - } - return false; + return cmd.elements[0].speed >= m_limits.elements[0].max.speed || + cmd.elements[0].speed <= m_limits.elements[0].min.speed; } void Task::updateHook() diff --git a/test/task_test.rb b/test/task_test.rb index 24fe235..37998ac 100644 --- a/test/task_test.rb +++ b/test/task_test.rb @@ -110,6 +110,54 @@ end end + describe "saturation signal reporting" do + before 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) + }] + ) + end + + it "does not output a saturation signal" do + modbus_configure_and_start + cmd = Types.base.samples.Joints.new( + elements: [Types.base.JointState.Speed(1000 * 2 * Math::PI / 60)] + ) + modbus_expect_execution(@writer, @reader).to do + have_no_new_sample(task.saturation_signal_port, at_least_during: 0.5) + end + end + + it "outputs a saturated signal" do + modbus_configure_and_start + cmd = Types.base.samples.Joints.new( + elements: [Types.base.JointState.Speed(1000 * 2 * Math::PI / 60)] + ) + signal = modbus_expect_execution(@writer, @reader) do + syskit_write task.cmd_in_port, cmd + end.to do + have_one_new_sample task.saturation_signal_port + end + assert_equal(1, signal.value) + end + + it "outputs an unsaturated signal" do + modbus_configure_and_start + cmd = Types.base.samples.Joints.new( + elements: [Types.base.JointState.Speed(10 * 2 * Math::PI / 60)] + ) + signal = modbus_expect_execution(@writer, @reader) do + syskit_write task.cmd_in_port, cmd + end.to do + have_one_new_sample task.saturation_signal_port + end + assert_equal(0, signal.value) + end + end + describe "fault and alarm reporting" do it "does not output on the fault_state port if there is no alarm or fault " do modbus_configure_and_start From 0e42a96e4c1d2db734642609553a5aa983184d78 Mon Sep 17 00:00:00 2001 From: Debora Date: Thu, 4 Apr 2024 17:36:06 -0300 Subject: [PATCH 3/3] fix: Change usv_control to control_base --- manifest.xml | 3 +-- motors_weg_cvw300.orogen | 7 +++---- tasks/Task.cpp | 7 +++++-- test/task_test.rb | 20 +++++++++++++++----- 4 files changed, 24 insertions(+), 13 deletions(-) diff --git a/manifest.xml b/manifest.xml index a932be7..bd076ec 100644 --- a/manifest.xml +++ b/manifest.xml @@ -8,8 +8,7 @@ - - + diff --git a/motors_weg_cvw300.orogen b/motors_weg_cvw300.orogen index 28bdff8..d6fb75c 100644 --- a/motors_weg_cvw300.orogen +++ b/motors_weg_cvw300.orogen @@ -13,9 +13,8 @@ import_types_from "motors_weg_cvw300/FaultState.hpp" import_types_from "motors_weg_cvw300Types.hpp" using_task_library "iodrivers_base" -using_library "usv_control" -import_types_from "usv_control" -import_types_from "usv_control/SaturationSignal.hpp" +using_library "control_base" +import_types_from "control_base" task_context "Task", subclasses: "iodrivers_base::Task" do needs_configuration @@ -86,7 +85,7 @@ task_context "Task", subclasses: "iodrivers_base::Task" do output_port "alarm_state", "/motors_weg_cvw300/AlarmState" output_port "fault_state", "/motors_weg_cvw300/FaultState" - output_port "saturation_signal", "/usv_control/SaturationSignal" + output_port "saturation_signal", "/control_base/SaturationSignal" exception_states "INVALID_COMMAND_SIZE", "INVALID_COMMAND_PARAMETER", "CONTROLLER_FAULT", "CONTROLLER_UNDER_VOLTAGE" diff --git a/tasks/Task.cpp b/tasks/Task.cpp index 06e425a..4615eaa 100644 --- a/tasks/Task.cpp +++ b/tasks/Task.cpp @@ -7,7 +7,7 @@ using namespace std; using namespace base; using namespace motors_weg_cvw300; -using namespace usv_control; +using namespace control_base; Task::Task(std::string const& name) : TaskBase(name) @@ -58,8 +58,11 @@ bool Task::configureHook() m_limits = _limits.get(); if (m_limits.elements.empty()) { - // Initialize limits as infinity + // Initialize speed limits as infinity JointLimits infinity; + JointLimitRange range; + range.Speed(-base::infinity(), base::infinity()); + infinity.elements.push_back(range); m_limits = infinity; } else { diff --git a/test/task_test.rb b/test/task_test.rb index 37998ac..fc53295 100644 --- a/test/task_test.rb +++ b/test/task_test.rb @@ -121,17 +121,14 @@ ) end - it "does not output a saturation signal" do + it "does not output a saturation signal if there is no new command" do modbus_configure_and_start - cmd = Types.base.samples.Joints.new( - elements: [Types.base.JointState.Speed(1000 * 2 * Math::PI / 60)] - ) modbus_expect_execution(@writer, @reader).to do have_no_new_sample(task.saturation_signal_port, at_least_during: 0.5) end end - it "outputs a saturated signal" do + it "outputs a saturated signal if the command is greater than the maximum limit" do modbus_configure_and_start cmd = Types.base.samples.Joints.new( elements: [Types.base.JointState.Speed(1000 * 2 * Math::PI / 60)] @@ -144,6 +141,19 @@ assert_equal(1, signal.value) end + it "outputs a saturated signal if the command is less than the minimum limit" do + modbus_configure_and_start + cmd = Types.base.samples.Joints.new( + elements: [Types.base.JointState.Speed(-1000 * 2 * Math::PI / 60)] + ) + signal = modbus_expect_execution(@writer, @reader) do + syskit_write task.cmd_in_port, cmd + end.to do + have_one_new_sample task.saturation_signal_port + end + assert_equal(1, signal.value) + end + it "outputs an unsaturated signal" do modbus_configure_and_start cmd = Types.base.samples.Joints.new(