created a component to convert the thruster forces to body efforts

auv_control.orogen

@@ -366,6 +366,27 @@ task_context "ThrustersFeedback" do
     port_driven 'joint_samples_in'
 end
 
+# Task that converts thruster's feedback from rpm to force.
+task_context "ThrusterForce2BodyEffort" do
+    needs_configuration
+
+    # Matrix with size of 6 * n. n means the count of thrusters that are used.
+    # The rows 0 to 2 of the matrix are the linear axis. The lines 3 to 5 of the
+    # matrix are the angular axis.
+    property "thruster_configuration_matrix", "base::MatrixXd"
+
+    # Thruster individual forces
+    input_port "thruster_forces", "base::commands::Joints"
+
+    # Body efforts once the thruster configuration matrix is applied to the
+    # thruster forces
+    output_port "body_efforts", "base::LinearAngular6DCommand"
+
+    exception_states :UNSET_THRUSTER_INPUT, :UNEXPECTED_THRUSTER_INPUT
+
+    port_driven 'thruster_forces'
+end
+
 # Task that works as switch betwen two commands
 task_context "CommandInjection" do
     subclasses "Base"

tasks/ThrusterForce2BodyEffort.cpp

@@ -0,0 +1,111 @@
+/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */
+
+#include "ThrusterForce2BodyEffort.hpp"
+#include "base/Logging.hpp"
+
+using namespace auv_control;
+
+ThrusterForce2BodyEffort::ThrusterForce2BodyEffort(std::string const& name)
+    : ThrusterForce2BodyEffortBase(name)
+{
+}
+
+ThrusterForce2BodyEffort::ThrusterForce2BodyEffort(std::string const& name, RTT::ExecutionEngine* engine)
+    : ThrusterForce2BodyEffortBase(name, engine)
+{
+}
+
+ThrusterForce2BodyEffort::~ThrusterForce2BodyEffort()
+{
+}
+
+
+
+/// The following lines are template definitions for the various state machine
+// hooks defined by Orocos::RTT. See ThrusterForce2BodyEffort.hpp for more detailed
+// documentation about them.
+
+bool ThrusterForce2BodyEffort::configureHook()
+{
+    if (! ThrusterForce2BodyEffortBase::configureHook())
+        return false;
+
+    thrusterMatrix = _thruster_configuration_matrix.get();
+
+    return true;
+}
+bool ThrusterForce2BodyEffort::startHook()
+{
+    if (! ThrusterForce2BodyEffortBase::startHook())
+        return false;
+    return true;
+}
+void ThrusterForce2BodyEffort::updateHook()
+{
+    ThrusterForce2BodyEffortBase::updateHook();
+
+    base::samples::Joints thrusterForces;
+
+    if(_thruster_forces.read(thrusterForces) == RTT::NewData)
+    {
+        base::LinearAngular6DCommand bodyEffort;
+        base::VectorXd thrusterForcesVector;
+        base::Vector6d bodyEffortVector;
+        unsigned int numberOfThrusters = thrusterMatrix.cols();
+        thrusterForcesVector = base::VectorXd::Zero(numberOfThrusters);
+
+        for(int i = 0; i < numberOfThrusters; i++)
+        {
+            if(thrusterForces.elements.size() != numberOfThrusters)
+            {
+                LOG_ERROR("The input vector should have a size equal to %i, but actually it "
+                        "has size equal to %i. Check configuration. ",
+                        numberOfThrusters, thrusterForces.elements.size());
+                exception(UNEXPECTED_THRUSTER_INPUT);
+                return;
+            }
+
+            if(!thrusterForces.elements[i].hasEffort())
+            {
+                std::cout << "teste 4" << std::endl;
+                std::string textThruster;
+
+                // Check whether names were specified for the thrusters
+                if(thrusterForces.names.size() == numberOfThrusters)
+                    textThruster = thrusterForces.names[i];
+                else
+                {
+                    std::stringstream number;
+                    number << i;
+                    textThruster = number.str();
+                }
+                LOG_ERROR("The field effort of the thruster %s was not set.",
+                        textThruster.c_str());
+                exception(UNSET_THRUSTER_INPUT);
+                return;
+            }
+
+            thrusterForcesVector[i] = thrusterForces.elements[i].effort;
+        }
+
+        bodyEffortVector = thrusterMatrix * thrusterForcesVector;
+
+        bodyEffort.linear = bodyEffortVector.head(3);
+        bodyEffort.angular = bodyEffortVector.tail(3);
+        bodyEffort.time = thrusterForces.time;
+
+        _body_efforts.write(bodyEffort);
+    }
+}
+void ThrusterForce2BodyEffort::errorHook()
+{
+    ThrusterForce2BodyEffortBase::errorHook();
+}
+void ThrusterForce2BodyEffort::stopHook()
+{
+    ThrusterForce2BodyEffortBase::stopHook();
+}
+void ThrusterForce2BodyEffort::cleanupHook()
+{
+    ThrusterForce2BodyEffortBase::cleanupHook();
+}

tasks/ThrusterForce2BodyEffort.hpp

@@ -0,0 +1,110 @@
+/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */
+
+#ifndef AUV_CONTROL_THRUSTERFORCE2BODYEFFORT_TASK_HPP
+#define AUV_CONTROL_THRUSTERFORCE2BODYEFFORT_TASK_HPP
+
+#include "auv_control/ThrusterForce2BodyEffortBase.hpp"
+
+namespace auv_control{
+
+    /*! \class ThrusterForce2BodyEffort
+     * \brief The task context provides and requires services. It uses an ExecutionEngine to perform its functions.
+     * Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification.
+     * In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow.
+     * Task that converts thruster's feedback from rpm to force.
+     * \details
+     * The name of a TaskContext is primarily defined via:
+     \verbatim
+     deployment 'deployment_name'
+         task('custom_task_name','auv_control::ThrusterForce2BodyEffort')
+     end
+     \endverbatim
+     *  It can be dynamically adapted when the deployment is called with a prefix argument.
+     */
+    class ThrusterForce2BodyEffort : public ThrusterForce2BodyEffortBase
+    {
+	friend class ThrusterForce2BodyEffortBase;
+    protected:
+
+	    base::MatrixXd thrusterMatrix;
+
+    public:
+        /** TaskContext constructor for ThrusterForce2BodyEffort
+         * \param name Name of the task. This name needs to be unique to make it identifiable via nameservices.
+         * \param initial_state The initial TaskState of the TaskContext. Default is Stopped state.
+         */
+        ThrusterForce2BodyEffort(std::string const& name = "auv_control::ThrusterForce2BodyEffort");
+
+        /** TaskContext constructor for ThrusterForce2BodyEffort
+         * \param name Name of the task. This name needs to be unique to make it identifiable for nameservices.
+         * \param engine The RTT Execution engine to be used for this task, which serialises the execution of all commands, programs, state machines and incoming events for a task.
+         * 
+         */
+        ThrusterForce2BodyEffort(std::string const& name, RTT::ExecutionEngine* engine);
+
+        /** Default deconstructor of ThrusterForce2BodyEffort
+         */
+	~ThrusterForce2BodyEffort();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from PreOperational to Stopped. If it returns false, then the
+         * component will stay in PreOperational. Otherwise, it goes into
+         * Stopped.
+         *
+         * It is meaningful only if the #needs_configuration has been specified
+         * in the task context definition with (for example):
+         \verbatim
+         task_context "TaskName" do
+           needs_configuration
+           ...
+         end
+         \endverbatim
+         */
+        bool configureHook();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from Stopped to Running. If it returns false, then the component will
+         * stay in Stopped. Otherwise, it goes into Running and updateHook()
+         * will be called.
+         */
+        bool startHook();
+
+        /** This hook is called by Orocos when the component is in the Running
+         * state, at each activity step. Here, the activity gives the "ticks"
+         * when the hook should be called.
+         *
+         * The error(), exception() and fatal() calls, when called in this hook,
+         * allow to get into the associated RunTimeError, Exception and
+         * FatalError states.
+         *
+         * In the first case, updateHook() is still called, and recover() allows
+         * you to go back into the Running state.  In the second case, the
+         * errorHook() will be called instead of updateHook(). In Exception, the
+         * component is stopped and recover() needs to be called before starting
+         * it again. Finally, FatalError cannot be recovered.
+         */
+        void updateHook();
+
+        /** This hook is called by Orocos when the component is in the
+         * RunTimeError state, at each activity step. See the discussion in
+         * updateHook() about triggering options.
+         *
+         * Call recover() to go back in the Runtime state.
+         */
+        void errorHook();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from Running to Stopped after stop() has been called.
+         */
+        void stopHook();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from Stopped to PreOperational, requiring the call to configureHook()
+         * before calling start() again.
+         */
+        void cleanupHook();
+    };
+}
+
+#endif
+

test/auv_control::ThrusterForce2BodyEffort.yml

@@ -0,0 +1,44 @@
+--- name:default
+# Matrix with size of 6 * n. n means the count of thrusters that are used.
+# The rows 0 to 2 of the matrix are the linear axis. The lines 3 to 5 of the
+# matrix are the angular axis.
+thruster_configuration_matrix:
+  rows: 6
+  cols: 6
+  data:
+  - 1.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - -0.42
+  - 1.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.42
+  - 0.0
+  - -1.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - -0.5735
+  - 0.0
+  - -1.0
+  - 0.0
+  - -0.0
+  - 0.0
+  - 0.936
+  - 0.0
+  - 0.0
+  - -1.0
+  - 0.0
+  - 0.4235
+  - 0.0
+  - 0.0
+  - 0.0
+  - -1.0
+  - -0.0
+  - -0.556
+  - 0.0

test/test_thruster_force_2_body_effort.rb

@@ -0,0 +1,82 @@
+require 'minitest/spec'
+require 'orocos/test/component'
+require 'minitest/autorun'
+
+
+describe 'auv_control::ThrusterForce2BodyEffort configuration' do
+  include Orocos::Test::Component
+  start 'thruster_force_2_body_effort', 'auv_control::ThrusterForce2BodyEffort' => 'thruster_force_2_body_effort'
+  reader 'thruster_force_2_body_effort', 'body_efforts', :attr_name => 'body_efforts'
+  writer 'thruster_force_2_body_effort', 'thruster_forces', :attr_name => 'thruster_forces'
+
+
+  it 'testing thruster surge and yaw force values' do
+
+    thruster_force_2_body_effort.apply_conf_file("auv_control::ThrusterForce2BodyEffort.yml")
+
+    thruster_force_2_body_effort.configure  
+    thruster_force_2_body_effort.start  
+
+    sample = thruster_force_2_body_effort.thruster_forces.new_sample
+
+    thruster1 = Types::Base::JointState.new
+    thruster2 = Types::Base::JointState.new
+    thruster3 = Types::Base::JointState.new
+    thruster4 = Types::Base::JointState.new
+    thruster5 = Types::Base::JointState.new
+    thruster6 = Types::Base::JointState.new
+    thruster1.effort = 30
+    thruster2.effort = 30
+    thruster3.effort = 0
+    thruster4.effort = 0
+    thruster5.effort = 50
+    thruster6.effort = 50
+
+    sample.elements = [thruster1, thruster2, thruster3, thruster4, thruster5, thruster6]
+
+    thruster_forces.write sample 
+
+    data = assert_has_one_new_sample body_efforts, 1
+
+    assert (data.linear[0] - 60).abs < 0.001, "wrong expected surge value"
+    assert (data.linear[1] - 0.0).abs < 0.001, "wrong expected sway value"
+    assert (data.linear[2] + 100).abs < 0.001, "wrong expected heave value"
+    assert (data.angular[2] - 0.0).abs < 0.001, "wrong expected yaw value"
+  end
+
+  it 'testing thruster sway force values' do
+
+    thruster_force_2_body_effort.apply_conf_file("auv_control::ThrusterForce2BodyEffort.yml")
+
+    thruster_force_2_body_effort.configure  
+    thruster_force_2_body_effort.start  
+
+    sample = thruster_force_2_body_effort.thruster_forces.new_sample
+
+    thruster1 = Types::Base::JointState.new
+    thruster2 = Types::Base::JointState.new
+    thruster3 = Types::Base::JointState.new
+    thruster4 = Types::Base::JointState.new
+    thruster5 = Types::Base::JointState.new
+    thruster6 = Types::Base::JointState.new
+    thruster1.effort = 0
+    thruster2.effort = 0
+    thruster3.effort = 25
+    thruster4.effort = 32
+    thruster5.effort = 0
+    thruster6.effort = 0
+
+    sample.elements = [thruster1, thruster2, thruster3, thruster4, thruster5, thruster6]
+
+    thruster_forces.write sample 
+
+    data = assert_has_one_new_sample body_efforts, 1
+
+    assert (data.linear[0] - 0.0).abs < 0.001, "wrong expected surge value"
+    assert (data.linear[1] + 57).abs < 0.001, "wrong expected sway value"
+    assert (data.linear[2] + 0.0).abs < 0.001, "wrong expected heave value"
+    assert (data.angular[2] - 15.615).abs < 0.001, "wrong expected yaw value"
+  end
+
+end
+