From b2fa9582b09c7a3e32336678b1cc24374e6bd0c3 Mon Sep 17 00:00:00 2001 From: joaobrittoneto Date: Tue, 16 May 2017 15:13:36 -0300 Subject: [PATCH] Rotate dvl velocity to body frame Aggregate angular velocity. --- tasks/VelocityAggregator.cpp | 104 ++++++++++++++++++++++++++++++++ tasks/VelocityAggregator.hpp | 113 +++++++++++++++++++++++++++++++++++ uwv_dynamic_model.orogen | 29 +++++++++ 3 files changed, 246 insertions(+) create mode 100644 tasks/VelocityAggregator.cpp create mode 100644 tasks/VelocityAggregator.hpp diff --git a/tasks/VelocityAggregator.cpp b/tasks/VelocityAggregator.cpp new file mode 100644 index 0000000..9c3090a --- /dev/null +++ b/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(); +} diff --git a/tasks/VelocityAggregator.hpp b/tasks/VelocityAggregator.hpp new file mode 100644 index 0000000..7e4d050 --- /dev/null +++ b/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 + diff --git a/uwv_dynamic_model.orogen b/uwv_dynamic_model.orogen index 067772e..3599b1f 100644 --- a/uwv_dynamic_model.orogen +++ b/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