diff --git a/controldev.orogen b/controldev.orogen index c69c29a..bfa3a4c 100644 --- a/controldev.orogen +++ b/controldev.orogen @@ -74,6 +74,42 @@ task_context "GenericRawToMotion2D" do port_driven :raw_command end +# This task converts controldev/RawCommand to base/Twist +# with configurable axes, ranges and deadzones +task_context "GenericRawToTwist" do + property("linear_x_axis", "int").doc("Axis index of RawCommand to set x component of the linear part, -1 means ignore, will be set to 0") + property("linear_y_axis", "int").doc("Axis index of RawCommand to set y component of the linear part, -1 means ignore, will be set to 0") + property("linear_z_axis", "int").doc("Axis index of RawCommand to set z component of the linear part, -1 means ignore, will be set to 0") + property("angular_x_axis", "int").doc("Axis index of RawCommand to set x component of the angular part, -1 means ignore, will be set to 0") + property("angular_y_axis", "int").doc("Axis index of RawCommand to set y component of the angular part, -1 means ignore, will be set to 0") + property("angular_z_axis", "int").doc("Axis index of RawCommand to set z component of the angular part, -1 means ignore, will be set to 0") + + property("invert_linear_x_axis", "bool", 0).doc("Invert Axis, default false") + property("invert_linear_y_axis", "bool", 0).doc("Invert Axis, default false") + property("invert_linear_z_axis", "bool", 0).doc("Invert Axis, default false") + property("invert_angular_x_axis", "bool", 0).doc("Invert Axis, default false") + property("invert_angular_y_axis", "bool", 0).doc("Invert Axis, default false") + property("invert_angular_z_axis", "bool", 0).doc("Invert Axis, default false") + + property("max_linear_x_axis", "double", 1.0).doc("Maximum speed in m/s") + property("max_linear_y_axis", "double", 1.0).doc("Maximum speed in m/s") + property("max_linear_z_axis", "double", 1.0).doc("Maximum speed in m/s") + property("max_angular_x_axis", "double", 1.0).doc("Maximum speed in m/s") + property("max_angular_y_axis", "double", 1.0).doc("Maximum speed in m/s") + property("max_angular_z_axis", "double", 1.0).doc("Maximum speed in m/s") + + property("linear_x_axis_deadzone", "double", 0.02).doc("linear axis X deadzone (range 0-1)") + property("linear_y_axis_deadzone", "double", 0.02).doc("linear axis Y deadzone (range 0-1)") + property("linear_z_axis_deadzone", "double", 0.02).doc("linear axis Z deadzone (range 0-1)") + property("angular_x_axis_deadzone", "double", 0.02).doc("angular axis X deadzone (range 0-1)") + property("angular_y_axis_deadzone", "double", 0.02).doc("angular axis Y deadzone (range 0-1)") + property("angular_z_axis_deadzone", "double", 0.02).doc("angular axis Z deadzone (range 0-1)") + + input_port "raw_command", "controldev/RawCommand" + output_port "motion_command", "base/Twist" + + port_driven :raw_command +end # A Task that receives CAN messages and converts them into raw-commands task_context "Remote", subclasses: 'GenericTask' do diff --git a/tasks/GenericRawToTwist.cpp b/tasks/GenericRawToTwist.cpp new file mode 100644 index 0000000..637efd8 --- /dev/null +++ b/tasks/GenericRawToTwist.cpp @@ -0,0 +1,90 @@ +/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */ + +#include "GenericRawToTwist.hpp" +#include +#include + + +#define WRITEAXIS(AXISNAME, TARGET) {\ + int axis = _##AXISNAME.get(); \ + if (axis >= 0) { \ + double axisvalue = rcmd.axisValue[axis]; \ + axisvalue = fabs(axisvalue) < _##AXISNAME##_deadzone ? 0 : axisvalue; \ + axisvalue *= _invert_##AXISNAME ? -1 : 1; \ + TARGET = axisvalue * _max_##AXISNAME; \ + } else { \ + TARGET = 0; \ + } \ + }; + + +using namespace controldev; + +GenericRawToTwist::GenericRawToTwist(std::string const& name, TaskCore::TaskState initial_state) + : GenericRawToTwistBase(name, initial_state) +{ +} + +GenericRawToTwist::~GenericRawToTwist() +{ +} + + + +/// The following lines are template definitions for the various state machine +// hooks defined by Orocos::RTT. See GenericRawToTwist.hpp for more detailed +// documentation about them. + +bool GenericRawToTwist::configureHook() +{ + if (! GenericRawToTwistBase::configureHook()) + return false; + return true; +} +bool GenericRawToTwist::startHook() +{ + if (! GenericRawToTwistBase::startHook()) + return false; + return true; +} + + +void GenericRawToTwist::updateHook() +{ + GenericRawToTwistBase::updateHook(); + + RawCommand rcmd; + base::Twist mcmd; + if (_raw_command.read(rcmd) == RTT::NewData) { + // example code result by the WRITEAXIS macro + // int axis = _linear_x_axis.get(); + // if (axis >= 0) { + // double axisvalue = rcmd.axisValue[axis]; + // axisvalue = fabs(rot_raw) < _rotation_axis_deadzone ? 0 : rot_raw; + // axisvalue *= _invert_linear_x_axis ? -1 : 1; + // mcmd.linear.x() = axisvalue * _max_linear_x_axis; + // } else { + // mcmd.linear.x() = 0; + // } + + WRITEAXIS(linear_x_axis, mcmd.linear.x()) + WRITEAXIS(linear_y_axis, mcmd.linear.y()) + WRITEAXIS(linear_z_axis, mcmd.linear.z()) + WRITEAXIS(angular_x_axis, mcmd.angular.x()) + WRITEAXIS(angular_y_axis, mcmd.angular.y()) + WRITEAXIS(angular_z_axis, mcmd.angular.z()) + _motion_command.write(mcmd); + } +} +void GenericRawToTwist::errorHook() +{ + GenericRawToTwistBase::errorHook(); +} +void GenericRawToTwist::stopHook() +{ + GenericRawToTwistBase::stopHook(); +} +void GenericRawToTwist::cleanupHook() +{ + GenericRawToTwistBase::cleanupHook(); +} diff --git a/tasks/GenericRawToTwist.hpp b/tasks/GenericRawToTwist.hpp new file mode 100644 index 0000000..1db3c62 --- /dev/null +++ b/tasks/GenericRawToTwist.hpp @@ -0,0 +1,104 @@ +/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */ + +#ifndef CONTROLDEV_GENERICRAWTOTWIST_TASK_HPP +#define CONTROLDEV_GENERICRAWTOTWIST_TASK_HPP + +#include "controldev/GenericRawToTwistBase.hpp" + +namespace controldev{ + + /*! \class GenericRawToTwist + * \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. + * This task converts controldev/RawCommand to base/MotionCommand2D +with configurable axes, ranges and deadzones + * \details + * The name of a TaskContext is primarily defined via: + \verbatim + deployment 'deployment_name' + task('custom_task_name','controldev::GenericRawToTwist') + end + \endverbatim + * It can be dynamically adapted when the deployment is called with a prefix argument. + */ + class GenericRawToTwist : public GenericRawToTwistBase + { + friend class GenericRawToTwistBase; + protected: + + + + public: + /** TaskContext constructor for GenericRawToTwist + * \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. + */ + GenericRawToTwist(std::string const& name = "controldev::GenericRawToTwist", TaskCore::TaskState initial_state = Stopped); + + /** Default deconstructor of GenericRawToTwist + */ + ~GenericRawToTwist(); + + /** 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 +