tk_obstacle: SurroundingBox.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * SurroundingBox.cpp
00003  *
00004  *  Created on: Dec 14, 2011
00005  *      Author: mriedel
00006  */
00007 
00008 #include <obs_providers/SurroundingBox.hpp>
00009 
00010 // Declare
00011 PLUGINLIB_DECLARE_CLASS(tk_obstacle, SurroundingBox, telekyb_obs::SurroundingBox, TELEKYB_NAMESPACE::ObstacleProvider);
00012 
00013 namespace telekyb_obs {
00014 
00015 SurroundingBoxOptions::SurroundingBoxOptions()
00016         : OptionContainer("SurroundingBox")
00017 {
00018         tMinSurrObsBox = addOption<Position3D>("tMinSurrObsBox",
00019                         "Boundary of the surrounding obstacle box detector. Min Values",
00020                         Position3D(-2.5,-3.7,-3.0),false,false);
00021         tMaxSurrObsBox = addOption<Position3D>("tMaxSurrObsBox",
00022                         "Boundary of the surrounding obstacle box detector. Max Values",
00023                         Position3D(2.8,3.1,0.2),false,false);
00024 }
00025 
00026 SurroundingBox::SurroundingBox()
00027         : ObstacleProvider("tk_obstacle/SurroundingBox")
00028 {
00029 
00030 }
00031 
00032 SurroundingBox::~SurroundingBox()
00033 {
00034 
00035 }
00036 
00037 // called directly after Creation
00038 void SurroundingBox::initialize()
00039 {
00040 
00041 }
00042 
00043 // called right before destruction
00044 void SurroundingBox::destroy()
00045 {
00046 
00047 }
00048 
00049 
00050 void SurroundingBox::getObstaclePoints(const TKState& lastState, std::vector<Position3D>& obstaclePoints) const
00051 {
00052         // copy because of efficiency;
00053         Position3D minSurrObsBox = options.tMinSurrObsBox->getValue();
00054         Position3D maxSurrObsBox = options.tMaxSurrObsBox->getValue();
00055 
00056         Position3D currentPos = lastState.position;
00057 
00058         if(     currentPos(0) > maxSurrObsBox(0) ||
00059                         currentPos(1) > maxSurrObsBox(1) ||
00060                         currentPos(2) > maxSurrObsBox(2) ||
00061                         currentPos(0) < minSurrObsBox(0) ||
00062                         currentPos(1) < minSurrObsBox(1) ||
00063                         currentPos(2) < minSurrObsBox(2)){
00064                 ROS_WARN("Current Position outside the obstacle box.");
00065         }else{
00066                 obstaclePoints.push_back(Position3D(minSurrObsBox(0),currentPos(1),currentPos(2)));
00067                 obstaclePoints.push_back(Position3D(maxSurrObsBox(0),currentPos(1),currentPos(2)));
00068                 obstaclePoints.push_back(Position3D(currentPos(0),minSurrObsBox(1),currentPos(2)));
00069                 obstaclePoints.push_back(Position3D(currentPos(0),maxSurrObsBox(1),currentPos(2)));
00070                 obstaclePoints.push_back(Position3D(currentPos(0),currentPos(1),minSurrObsBox(2)));
00071                 obstaclePoints.push_back(Position3D(currentPos(0),currentPos(1),maxSurrObsBox(2)));
00072         }
00073 }
00074 
00075 } /* namespace telekyb_obs */