Add a GenericRawToTwist Task

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

tasks/GenericRawToTwist.cpp

@@ -0,0 +1,90 @@
+/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */
+
+#include "GenericRawToTwist.hpp"
+#include <base/Twist.hpp>
+#include <base/Angle.hpp>
+
+
+#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();
+}

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
+