00001 /* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*- 00002 00003 this file is part of rcssserver3D 00004 Fri May 9 2003 00005 Copyright (C) 2002,2003 Koblenz University 00006 Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group 00007 $Id: hinge2perceptor.cpp,v 1.1 2004/05/07 12:14:51 rollmark Exp $ 00008 00009 This program is free software; you can redistribute it and/or modify 00010 it under the terms of the GNU General Public License as published by 00011 the Free Software Foundation; version 2 of the License. 00012 00013 This program is distributed in the hope that it will be useful, 00014 but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00016 GNU General Public License for more details. 00017 00018 You should have received a copy of the GNU General Public License 00019 along with this program; if not, write to the Free Software 00020 Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 00021 */ 00022 #include "hinge2perceptor.h" 00023 #include <zeitgeist/logserver/logserver.h> 00024 00025 using namespace oxygen; 00026 using namespace zeitgeist; 00027 using namespace boost; 00028 using namespace std; 00029 00030 Hinge2Perceptor::Hinge2Perceptor() : Perceptor() 00031 { 00032 } 00033 00034 Hinge2Perceptor::~Hinge2Perceptor() 00035 { 00036 } 00037 00038 void Hinge2Perceptor::OnLink() 00039 { 00040 mJoint = make_shared(FindParentSupportingClass<Hinge2Joint>()); 00041 00042 if (mJoint.get() == 0) 00043 { 00044 GetLog()->Error() 00045 << "(Hinge2Perceptor) ERROR: found no Hinge2Joint parent\n"; 00046 } 00047 00048 } 00049 00050 void Hinge2Perceptor::OnUnlink() 00051 { 00052 mJoint.reset(); 00053 } 00054 00055 void Hinge2Perceptor::InsertAxisAngle(Predicate& predicate, Joint::EAxisIndex idx) 00056 { 00057 ParameterList& axisElement = predicate.parameter.AddList(); 00058 axisElement.AddValue(string("axis")); 00059 axisElement.AddValue(static_cast<int>(idx)); 00060 axisElement.AddValue(mJoint->GetAngle(idx)); 00061 } 00062 00063 void Hinge2Perceptor::InsertAxisRate(Predicate& predicate, Joint::EAxisIndex idx) 00064 { 00065 ParameterList& axisElement = predicate.parameter.AddList(); 00066 axisElement.AddValue(string("rate")); 00067 axisElement.AddValue(static_cast<int>(idx)); 00068 axisElement.AddValue(mJoint->GetAngleRate(idx)); 00069 } 00070 00071 bool Hinge2Perceptor::Percept(boost::shared_ptr<oxygen::PredicateList> predList) 00072 { 00073 if (mJoint.get() == 0) 00074 { 00075 return false; 00076 } 00077 00078 Predicate& predicate = predList->AddPredicate(); 00079 predicate.name = "hinge2"; 00080 predicate.parameter.Clear(); 00081 00082 ParameterList& nameElement = predicate.parameter.AddList(); 00083 nameElement.AddValue(string("name")); 00084 nameElement.AddValue(GetName()); 00085 00086 InsertAxisAngle(predicate,Joint::AI_FIRST); 00087 InsertAxisRate(predicate,Joint::AI_SECOND); 00088 00089 return true; 00090 } 00091 00092