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
+