SampleController.cpp
Go to the documentation of this file.
00001 /*
00002  * SampleController.cpp
00003  *
00004  *  Created on: Mar 4, 2012
00005  *      Author: mriedel
00006  */
00007 
00008 #include "SampleController.hpp"
00009 
00010 // plugin stuff
00011 #include <pluginlib/class_list_macros.h>
00012 
00013 #include <ros/console.h>
00014 
00015 PLUGINLIB_DECLARE_CLASS(tk_haptics_base, SampleController, telekyb_haptic::SampleController, TELEKYB_NAMESPACE::HapticDeviceController);
00016 
00017 namespace telekyb_haptic {
00018 
00019 SampleController::SampleController()
00020 {
00021 
00022 }
00023 
00024 SampleController::~SampleController()
00025 {
00026 
00027 }
00028 
00029 // Identifier (e.g. for NodeHandle)
00030 void SampleController::setIdentifier(const std::string& identifier)
00031 {
00032         ROS_INFO("SampleController: Got identifier %s", identifier.c_str());
00033 }
00034 
00035 // Get's specific Axes mapping, Set if needed
00036 void SampleController::setAxesMapping(HapticAxesMapping& xAxis, HapticAxesMapping& yAxis, HapticAxesMapping& zAxis)
00037 {
00038         ROS_INFO("Recv Axes Mapping (X,Y,Z): (%s,%s,%s)", xAxis.str(), yAxis.str(), zAxis.str());
00039 }
00040 
00041 // Get the Range of each axes
00042 void SampleController::setAxesRange(const Position3D& minValues, const Position3D& maxValues) {
00043 
00044 }
00045 
00046 void SampleController::willEnterSpinLoop()
00047 {
00048         frequencyTimer.reset();
00049 }
00050 
00051 // has to be fast and should not slow down the loop
00052 void SampleController::loopCB(const HapticOuput& output, HapticInput& input) {
00053 
00054         if (frequencyTimer.frequency() < 1.0) {
00055                 ROS_INFO("Output every second");
00056 
00057                 ROS_INFO("Position: (%f,%f,%f)", output.position(0), output.position(1), output.position(2));
00058                 ROS_INFO("Velocity: (%f,%f,%f)", output.linVelocity(0), output.linVelocity(1), output.linVelocity(2));
00059                 Eigen::Vector3d angles = output.orientation.toRotationMatrix().eulerAngles(0,1,2);
00060                 ROS_INFO("Orientation: (%f,%f,%f)", angles(0), angles(1), angles(2));
00061                 ROS_INFO("Force: (%f,%f,%f)", output.force(0), output.force(1), output.force(2));
00062                 ROS_INFO("Loop Frequency: %d", (int)(output.frequency) );
00063                 ROS_INFO("Primary Button: %d", output.primaryButton);
00064 
00065 
00066                 frequencyTimer.reset();
00067         }
00068 
00069         // Center to 0/0/0
00070         double posGain = 100.0;
00071         double velGain = 20.0;
00072         //input.force = -posGain*output.position - velGain*output.linVelocity;
00073         input.force = Vector3D(0.0,0.0,0.0);
00074 }
00075 
00076 void SampleController::didLeaveSpinLoop()
00077 {
00078 
00079 }
00080 
00081 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


tk_haptics_base
Author(s): mriedel
autogenerated on Mon Nov 11 2013 11:13:55