tk_trajprocessor: XMModeCheck.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * XMModeCheck.cpp
00003  *
00004  *  Created on: Dec 14, 2011
00005  *      Author: mriedel
00006  */
00007 
00008 #include <trajmodules/XMModeCheck.hpp>
00009 
00010 #include <tk_trajprocessor/TrajectoryProcessorController.hpp>
00011 
00012 // Declare
00013 PLUGINLIB_DECLARE_CLASS(tk_trajprocessor, XMModeCheck, telekyb_traj::XMModeCheck, TELEKYB_NAMESPACE::TrajectoryModule);
00014 
00015 
00016 namespace telekyb_traj {
00017 
00018 XMModeCheckOptions::XMModeCheckOptions()
00019         : OptionContainer("XMModeCheck")
00020 {
00021         tAllowPositionMode = addOption("tAllowPositionMode", "Allow or disallow Position Mode",true,false,false);
00022         tAllowVelocityMode = addOption("tAllowVelocityMode", "Allow or disallow Velocity Mode",true,false,false);
00023         tAllowAccelerationMode = addOption("tAllowAccelerationMode", "Allow or disallow Acceleration Mode",false,false,false);
00024         tAllowMixedModes = addOption("tAllowMixedModes", "Allow or disallow Mixed Modes",false,false,false);
00025 }
00026 
00027 XMModeCheck::XMModeCheck()
00028         : TrajectoryModule("tk_trajprocessor/XMModeCheck", TrajModulePosType::All, -500)
00029 {
00030 
00031 }
00032 
00033 void XMModeCheck::initialize()
00034 {
00035 
00036 }
00037 
00038 void XMModeCheck::destroy()
00039 {
00040 
00041 }
00042 
00043 // set back to intial conditions
00044 void XMModeCheck::willTurnActive()
00045 {
00046 
00047 }
00048 
00049 // called after turning inactive
00050 void XMModeCheck::didTurnInactive()
00051 {
00052 
00053 }
00054 
00055 bool XMModeCheck::trajectoryStep(const TKState& currentState, TKTrajectory& trajInput)
00056 {
00057         //ROS_INFO("Called XMModeCheck");
00058         bool doNormalBrake = false;
00059         GlobalPosControlType inputPosCtrlType = trajInput.getGlobalPositionControlType();
00060         if (inputPosCtrlType == GlobalPosControlType::Position && !options.tAllowPositionMode->getValue()) {
00061                 ROS_ERROR_STREAM("Trajectory Module " << getName() << ": Disallowed PositionControlMode: Position!");
00062                 doNormalBrake = true;
00063         } else if (inputPosCtrlType == GlobalPosControlType::Velocity && !options.tAllowVelocityMode->getValue()) {
00064                 ROS_ERROR_STREAM("Trajectory Module " << getName() << ": Disallowed PositionControlMode: Velocity!");
00065                 doNormalBrake = true;
00066         } else if (inputPosCtrlType == GlobalPosControlType::Acceleration && !options.tAllowAccelerationMode->getValue()) {
00067                 ROS_ERROR_STREAM("Trajectory Module " << getName() << ": Disallowed PositionControlMode: Acceleration!");
00068                 doNormalBrake = true;
00069         } else if (inputPosCtrlType == GlobalPosControlType::Mixed && !options.tAllowMixedModes->getValue()) {
00070                 ROS_ERROR_STREAM("Trajectory Module " << getName() << ": Disallowed PositionControlMode: Mixed!");
00071                 doNormalBrake = true;
00072         }
00073 
00074         if (doNormalBrake) {
00075                 tpController.getBehaviorSwitcher()->switchToNormalBrake();
00076                 trajInput.setVelocity( Velocity3D::Zero() );
00077                 return false; // skip other
00078         }
00079 
00080 
00081         return true;
00082 }
00083 
00084 } /* namespace telekyb_traj */