Rotate dvl velocity to body frame

tasks/VelocityAggregator.cpp

@@ -0,0 +1,104 @@
+/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */
+
+#include "VelocityAggregator.hpp"
+
+using namespace uwv_dynamic_model;
+
+VelocityAggregator::VelocityAggregator(std::string const& name)
+    : VelocityAggregatorBase(name)
+{
+}
+
+VelocityAggregator::VelocityAggregator(std::string const& name, RTT::ExecutionEngine* engine)
+    : VelocityAggregatorBase(name, engine)
+{
+    last_vel_state.sourceFrame = "body";
+    last_vel_state.targetFrame = "body";
+}
+
+VelocityAggregator::~VelocityAggregator()
+{
+    last_vel_state.sourceFrame = "body";
+    last_vel_state.targetFrame = "body";
+}
+
+bool VelocityAggregator::getTransformation(const transformer::Transformation &transformer, Eigen::Affine3d &transformationMatrix)
+{
+    if (!transformer.get(base::Time::now(), transformationMatrix))
+    {
+        if(state() != MISSING_TRANSFORMATION)
+            state(MISSING_TRANSFORMATION);
+        return false;
+    }
+    return true;
+}
+
+
+/// The following lines are template definitions for the various state machine
+// hooks defined by Orocos::RTT. See VelocityAggregator.hpp for more detailed
+// documentation about them.
+
+bool VelocityAggregator::configureHook()
+{
+    if (! VelocityAggregatorBase::configureHook())
+        return false;
+    return true;
+}
+bool VelocityAggregator::startHook()
+{
+    if (! VelocityAggregatorBase::startHook())
+        return false;
+    return true;
+}
+void VelocityAggregator::updateHook()
+{
+    VelocityAggregatorBase::updateHook();
+
+    base::samples::RigidBodyState dvl_sample;
+    if(_dvl_samples.read(dvl_sample) == RTT::NewData)
+    {
+        if(!dvl_sample.hasValidVelocity())
+            return;
+        Eigen::Affine3d dvl2body;
+        if(!getTransformation(_dvl2body, dvl2body))
+            return;
+        dvl_sample.velocity = dvl2body.rotation()*dvl_sample.velocity;
+        if(last_vel_state.hasValidAngularVelocity())
+            dvl_sample.velocity -= last_vel_state.angular_velocity.cross(dvl2body.translation());
+        last_vel_state.velocity = dvl_sample.velocity;
+        last_vel_state.time = dvl_sample.time;
+        _velocity_samples.write(last_vel_state);
+    }
+
+    base::samples::RigidBodyState ori_sample;
+    if(_orientation_samples.read(ori_sample) == RTT::NewData)
+    {
+        if(!ori_sample.hasValidOrientation())
+            return;
+        last_vel_state.orientation = ori_sample.orientation;
+    }
+
+    base::samples::IMUSensors imu_sample;
+    if(_imu_samples.read(imu_sample) == RTT::NewData)
+    {
+        Eigen::Affine3d imu2body;
+        if(imu_sample.gyro.hasNaN())
+            return;
+        if(!getTransformation(_imu2body, imu2body))
+            return;
+        last_vel_state.angular_velocity = imu2body.rotation()*imu_sample.gyro;
+    }
+
+}
+void VelocityAggregator::errorHook()
+{
+    VelocityAggregatorBase::errorHook();
+}
+void VelocityAggregator::stopHook()
+{
+    VelocityAggregatorBase::stopHook();
+}
+void VelocityAggregator::cleanupHook()
+{
+    VelocityAggregatorBase::cleanupHook();
+}

tasks/VelocityAggregator.hpp

@@ -0,0 +1,113 @@
+/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */
+
+#ifndef UWV_DYNAMIC_MODEL_VELOCITYAGGREGATOR_TASK_HPP
+#define UWV_DYNAMIC_MODEL_VELOCITYAGGREGATOR_TASK_HPP
+
+#include "uwv_dynamic_model/VelocityAggregatorBase.hpp"
+
+namespace uwv_dynamic_model{
+
+    /*! \class VelocityAggregator
+     * \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.
+     * The VelocityAggregator aims to aggregate the sensor's velocity in body-frame.
+It doesn't do any filter, just remove nan.
+     * \details
+     * The name of a TaskContext is primarily defined via:
+     \verbatim
+     deployment 'deployment_name'
+         task('custom_task_name','uwv_dynamic_model::VelocityAggregator')
+     end
+     \endverbatim
+     *  It can be dynamically adapted when the deployment is called with a prefix argument.
+     */
+    class VelocityAggregator : public VelocityAggregatorBase
+    {
+	friend class VelocityAggregatorBase;
+    protected:
+
+        bool getTransformation(const transformer::Transformation &transformer,
+            Eigen::Affine3d &transformationMatrix);
+
+        base::samples::RigidBodyState last_vel_state;
+    public:
+        /** TaskContext constructor for VelocityAggregator
+         * \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.
+         */
+        VelocityAggregator(std::string const& name = "uwv_dynamic_model::VelocityAggregator");
+
+        /** TaskContext constructor for VelocityAggregator
+         * \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.
+         * 
+         */
+        VelocityAggregator(std::string const& name, RTT::ExecutionEngine* engine);
+
+        /** Default deconstructor of VelocityAggregator
+         */
+	~VelocityAggregator();
+
+        /** 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
+

uwv_dynamic_model.orogen

@@ -126,3 +126,32 @@ task_context "VelocityEstimator" do
     runtime_states :MISSING_TRANSFORMATION
 
 end
+
+
+# The VelocityAggregator aims to aggregate the sensor's velocity in body-frame.
+# It doesn't do any filter, just remove nan.
+task_context "VelocityAggregator" do
+
+    # DVL's linear velocities samples
+    input_port("dvl_samples", "/base/samples/RigidBodyState")
+
+    # Orientation samples
+    input_port("orientation_samples", "/base/samples/RigidBodyState")
+
+    # IMU's angular velocities samples
+    input_port("imu_samples","/base/samples/IMUSensors")
+
+    output_port "velocity_samples", "/base/samples/RigidBodyState"
+    #*************************
+    #***  TRANSFORMATIONS  ***
+    #*************************
+
+    transformer do
+        transform "imu", "body"
+        transform "dvl", "body"
+        max_latency 0.05
+    end
+
+    runtime_states :MISSING_TRANSFORMATION
+    port_driven
+end