Go to the documentation of this file.00001 #include <kdl/frames.hpp>
00002 #include <rtt/RTT.hpp>
00003 #include <rtt/Component.hpp>
00004
00005 using namespace RTT;
00006 using namespace KDL;
00007 using namespace std;
00008
00009 class KDLTypekitTestComponent : public TaskContext{
00010 private:
00011 OutputPort<Vector> port_vector_out;
00012 OutputPort<Rotation> port_rotation_out;
00013 OutputPort<Frame> port_frame_out;
00014 OutputPort<Wrench> port_wrench_out;
00015 OutputPort<Twist> port_twist_out;
00016
00017 InputPort<Vector> port_vector_in;
00018 InputPort<Rotation> port_rotation_in;
00019 InputPort<Frame> port_frame_in;
00020 InputPort<Wrench> port_wrench_in;
00021 InputPort<Twist> port_twist_in;
00022
00023 Vector prop_vector;
00024 Rotation prop_rotation;
00025 Frame prop_frame;
00026 Wrench prop_wrench;
00027 Twist prop_twist;
00028
00029 Vector vectorOperation(const Vector& vector_in)
00030 {
00031 Vector vector_tmp=prop_vector;
00032 prop_vector=vector_in;
00033 return vector_tmp;
00034 }
00035
00036 Rotation rotationOperation(const Rotation& rotation_in)
00037 {
00038 Rotation rotation_tmp=prop_rotation;
00039 prop_rotation=rotation_in;
00040 return rotation_tmp;
00041 }
00042
00043 Frame frameOperation(const Frame& frame_in)
00044 {
00045 Frame frame_tmp=prop_frame;
00046 prop_frame=frame_in;
00047 return frame_tmp;
00048 }
00049
00050 Wrench wrenchOperation(const Wrench& wrench_in)
00051 {
00052 Wrench wrench_tmp=prop_wrench;
00053 prop_wrench=wrench_in;
00054 return wrench_tmp;
00055 }
00056
00057 Twist twistOperation(const Twist& twist_in)
00058 {
00059 Twist twist_tmp=prop_twist;
00060 prop_twist=twist_in;
00061 return twist_tmp;
00062 }
00063
00064 public:
00065 KDLTypekitTestComponent(const string& name) : TaskContext(name)
00066 {
00067 this->addPort("VectorOut", port_vector_out);
00068 this->addPort("RotationOut", port_rotation_out);
00069 this->addPort("FrameOut", port_frame_out);
00070 this->addPort("WrenchOut", port_wrench_out);
00071 this->addPort("TwistOut", port_twist_out);
00072
00073 this->addEventPort("VectorIn", port_vector_in);
00074 this->addEventPort("RotationIn", port_rotation_in);
00075 this->addEventPort("FrameIn", port_frame_in);
00076 this->addEventPort("WrenchIn", port_wrench_in);
00077 this->addEventPort("TwistIn", port_twist_in);
00078
00079 this->addOperation("vectorOperation", &KDLTypekitTestComponent::vectorOperation, this);
00080 this->addOperation("rotationOperation", &KDLTypekitTestComponent::rotationOperation,this);
00081 this->addOperation("frameOperation", &KDLTypekitTestComponent::frameOperation, this);
00082 this->addOperation("wrenchOperation", &KDLTypekitTestComponent::wrenchOperation, this);
00083 this->addOperation("twistOperation", &KDLTypekitTestComponent::twistOperation, this);
00084
00085 this->addProperty("vectorProperty", prop_vector);
00086 this->addProperty("rotationProperty", prop_rotation);
00087 this->addProperty("frameProperty", prop_frame);
00088 this->addProperty("wrenchProperty", prop_wrench);
00089 this->addProperty("twistProperty", prop_twist);
00090 }
00091
00092 void updateHook()
00093 {
00094 KDL::Vector tmp_vector;
00095 if(port_vector_in.read(tmp_vector)==NewData){
00096 port_vector_out.write(prop_vector);
00097 prop_vector = tmp_vector;
00098 }
00099
00100 KDL::Rotation tmp_rotation;
00101 if(port_rotation_in.read(tmp_rotation)==NewData){
00102 port_rotation_out.write(prop_rotation);
00103 prop_rotation = tmp_rotation;
00104 }
00105
00106 KDL::Frame tmp_frame;
00107 if(port_frame_in.read(tmp_frame)==NewData){
00108 port_frame_out.write(prop_frame);
00109 prop_frame = tmp_frame;
00110 }
00111
00112 KDL::Wrench tmp_wrench;
00113 if(port_wrench_in.read(tmp_wrench)==NewData){
00114 port_wrench_out.write(prop_wrench);
00115 prop_wrench = tmp_wrench;
00116 }
00117
00118 KDL::Twist tmp_twist;
00119 if(port_twist_in.read(tmp_twist)==NewData){
00120 port_twist_out.write(prop_twist);
00121 prop_twist = tmp_twist;
00122 }
00123
00124 return;
00125 }
00126
00127 };
00128
00129 ORO_CREATE_COMPONENT(KDLTypekitTestComponent)