[simspark-cvs] simspark/contrib/agentspark .cvsignore, 1.1, 1.2 behavior.h, 1.1, 1.2 hoap2behavior.cpp, 1.1, 1.2 hoap2behavior.h, 1.1, 1.2 main.cpp, 1.1, 1.2 soccerbotbehavior.cpp, 1.1, 1.2 soccerbotbehavior.h, 1.1, 1.2

[Markus R. <rol...@us...>]

Update of /cvsroot/simspark/simspark/contrib/agentspark
In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv7693

Added Files:
	.cvsignore behavior.h hoap2behavior.cpp hoap2behavior.h 
	main.cpp soccerbotbehavior.cpp soccerbotbehavior.h 
Log Message:
- readded agentspark files that got lost in the WIN32 merge

--- NEW FILE: .cvsignore ---
.deps
.libs
Makefile
Makefile.in
agentspark

--- NEW FILE: main.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2003 Koblenz University
   $Id: main.cpp,v 1.2 2007/03/17 06:52:44 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include <string>
#include <iostream>
#include <boost/scoped_ptr.hpp>
#include <rcssnet/tcpsocket.hpp>
#include <rcssnet/exception.hpp>

#include "behavior.h"

#ifdef __linux__
#include <netinet/in.h>
#endif

#include "hoap2behavior.h"
#include "soccerbotbehavior.h"

using namespace rcss::net;
using namespace std;
using namespace boost;

TCPSocket gSocket;
string gHost = "127.0.0.1";
int gPort = 3100;

void PrintGreeting()
{
    cout << "agentspark, a spark demo agent\n"
         << "Copyright (C) 2004 Koblenz University.\n"
         << "2004 RoboCup Soccer Server 3D Maintenance Group.\n\n";
}

void PrintHelp()
{
    cout << "\nusage: agentspark [options]" << endl;
    cout << "\noptions:" << endl;
    cout << " --help      prints this message." << endl;
    cout << " --host=IP   IP of the server." << endl;
    cout << "\n";
}

void ReadOptions(int argc, char* argv[])
{
    for( int i = 0; i < argc; i++)
        {
            if ( strcmp( argv[i], "--help" ) == 0 )
                {
                    PrintHelp();
                    exit(0);
                }
            else if ( strncmp( argv[i], "--host", 6 ) == 0 )
            {
                string tmp=argv[i];
                
                if ( tmp.length() <= 7 ) // minimal sanity check
                {
                    PrintHelp();
                    exit(0);
                }
                gHost = tmp.substr(7);
            }
            
                
        }
}

bool Init()
{
    cout << "connecting to TCP " << gHost << ":" << gPort << endl;

    try
        {
            Addr local(INADDR_ANY,INADDR_ANY);
            gSocket.bind(local);
        }

    catch (BindErr error)
        {
            cerr << "failed to bind socket with '"
                 << error.what() << "'" << endl;

            gSocket.close();
            return false;
        }

    try
        {
            Addr server(gPort,gHost);
            gSocket.connect(server);
        }

    catch (ConnectErr error)
        {
            cerr << "connection failed with: '"
                 << error.what() << "'" << endl;
            gSocket.close();
            return false;
        }

    return true;
}

void Done()
{
    gSocket.close();
    cout << "closed connection to " << gHost << ":" << gPort << endl;
}

bool SelectInput()
{
    fd_set readfds;
    FD_ZERO(&readfds);
    FD_SET(gSocket.getFD(),&readfds);

#ifdef WIN32
    int maxFd = 0;
#else
    int maxFd = gSocket.getFD()+1;
#endif
    return select(maxFd,&readfds, 0, 0, 0) > 0;
}

void PutMessage(const string& msg)
{
    if (msg.empty())
        {
            return;
        }

    // prefix the message with it's payload length
    unsigned int len = static_cast<unsigned int>(htonl(msg.size()));
    gSocket.send((const char*)&len, sizeof(unsigned int));
    gSocket.send(msg.data(), msg.size());
}

bool GetMessage(string& msg)
{
    // try to read the first message segment
    if (! SelectInput())
    {
        return false;
    }

    static char buffer[16 * 1024];
    int bytesRead = gSocket.recv(buffer, sizeof(buffer));

    //cerr << "buffer = |" << string(buffer+1) << "|\n";
    //cerr << "bytesRead = |" << bytesRead << "|\n";
    //cerr << "Size of buffer = |" << sizeof(buffer) << "|\n";
    //cerr << "buffer = |" << buffer << "|\n";
    //cerr << "buffer[5] = |" << buffer[5] << "|\n";
    //printf ("xxx-%s\n", buffer+5);
    
    if (bytesRead < sizeof(unsigned int))
    {
        return false;
    }

    // msg is prefixed with it's total length
    unsigned int msgLen = ntohl(*(unsigned int*)buffer);
    //cerr << "GM 6 / " << msgLen << "\n";

    // read remaining message segments
    unsigned int msgRead = bytesRead - sizeof(unsigned int);

    //cerr << "msgRead = |" << msgRead << "|\n";

    char *offset = buffer + bytesRead;

    while (msgRead < msgLen)
    {
        if (! SelectInput())
        {
            return false;
        }
        msgRead += gSocket.recv(offset, sizeof(buffer) - msgRead);
        //cerr << "msgRead = |" << msgRead << "|\n";
        offset += msgRead;
    }

    // zero terminate received data
    (*offset) = 0;

    msg = string(buffer+sizeof(unsigned int));

    //cerr << msg << endl;

    return true;
}

void Run()
{    
    // scoped_ptr<Behavior> behavior(new Hoap2Behavior());
    scoped_ptr<Behavior> behavior(new SoccerbotBehavior());
    PutMessage(behavior->Init());

    string msg;
    while (GetMessage(msg))
        {
            PutMessage(behavior->Think(msg));
        }
}

int
main(int argc, char* argv[])
{
    PrintGreeting();
    ReadOptions(argc,argv);

    if (! Init())
        {
            return 1;
        }

    Run();
    Done();
}

--- NEW FILE: hoap2behavior.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
   this file is part of rcssserver3D
   Thu Nov 8 2005
   Copyright (C) 2005 Koblenz University

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "hoap2behavior.h"
#include <iostream>
#include <sstream>
#include <cmath>

using namespace oxygen;
using namespace zeitgeist;
using namespace std;
using namespace boost;
using namespace salt;

Hoap2Behavior::Hoap2Behavior() : mZG("." PACKAGE_NAME)
{
}

void Hoap2Behavior::SetupJointIDMap()
{
    mJointIDMap.clear();
    mJointIDMap["head_joint_2"]   = JID_HEAD_2;
    mJointIDMap["head_joint_1"]   = JID_HEAD_1;
    mJointIDMap["lleg_joint_1"]   = JID_LLEG_1;
    mJointIDMap["rleg_joint_1"]   = JID_RLEG_1;
    mJointIDMap["lleg_joint_2_3"] = JID_LLEG_2_3;
    mJointIDMap["rleg_joint_2_3"] = JID_RLEG_2_3;
    mJointIDMap["lleg_joint_4"]   = JID_LLEG_4;
    mJointIDMap["rleg_joint_4"]   = JID_RLEG_4;
    mJointIDMap["lleg_joint_5_6"] = JID_LLEG_5_6;
    mJointIDMap["rleg_joint_5_6"] = JID_RLEG_5_6;
    mJointIDMap["larm_joint_1_2"] = JID_LARM_1_2;
    mJointIDMap["rarm_joint_1_2"] = JID_RARM_1_2;
    mJointIDMap["larm_joint_3"]   = JID_LARM_3;
    mJointIDMap["rarm_joint_3"]   = JID_RARM_3;
    mJointIDMap["larm_joint_4"]   = JID_LARM_4;
    mJointIDMap["rarm_joint_4"]   = JID_RARM_4;
    mJointIDMap["larm_joint_5"]   = JID_LARM_5;
    mJointIDMap["rarm_joint_5"]   = JID_RARM_5;
}

string Hoap2Behavior::Init()
{
    mZG.GetCore()->ImportBundle("sexpparser");

    mParser = shared_static_cast<BaseParser>
        (mZG.GetCore()->New("SexpParser"));

    if (mParser.get() == 0)
    {
        cerr << "unable to create SexpParser instance." << endl;
    }

    SetupJointIDMap();
    // use the scene effector to build the agent and beam to a
    // position near the center of the playing field
    return
        "(scene rsg/agent/hoap2.rsg)";
}

void Hoap2Behavior::ParseHingeJointInfo(const oxygen::Predicate& predicate)
{
    //cout << "(Hoap2Behavior) parsing HJ info" << endl;

    // read the object name
    string name;
    Predicate::Iterator iter(predicate);

    if (! predicate.GetValue(iter, "name", name))
    {
        return;
    }

    // try to lookup the joint id
    TJointIDMap::iterator idIter = mJointIDMap.find(name);
    if (idIter == mJointIDMap.end())
    {
        cerr << "(Hoap2Behavior) unknown joint id!" << endl;
        return;
    }

    JointID jid = (*idIter).second;

    // read the angle value
    HingeJointSense sense;
    if (! predicate.GetValue(iter,"axis", sense.angle))
    {
        return;
    }

    // update the map
    mHingeJointSenseMap[jid] = sense;
}

void Hoap2Behavior::ParseUniversalJointInfo(const oxygen::Predicate& predicate)
{
    // read the object name
    string name;
    Predicate::Iterator iter(predicate);

    if (! predicate.GetValue(iter, "name", name))
    {
        return;
    }

    // try to lookup the joint id
    TJointIDMap::iterator idIter = mJointIDMap.find(name);
    if (idIter == mJointIDMap.end())
    {
        cerr << "(Hoap2Behavior) unknown joint id!" << endl;
        return;
    }

    JointID jid = (*idIter).second;

    // record the angle and rate values
    UniversalJointSense sense;

    // try to read axis1 angle
    if (! predicate.GetValue(iter,"ax1", sense.angle1))
    {
        cerr << "(Hoap2Behavior) could not parse universal joint angle1!" << endl;
        return;
    }

    // try to read axis2 angle
    if (! predicate.GetValue(iter,"ax2", sense.angle2))
    {
        cerr << "(Hoap2Behavior) could not parse universal joint rate2!" << endl;
        return;
    }

    //cout << "(ParseUniversalJointInfo) got angles " << sense.angle1 
    //     << " and " << sense.angle2 << endl;

    // update the map
    mUniversalJointSenseMap[jid] = sense;
}



string Hoap2Behavior::Think(const std::string& message)
{
    //sleep(1);

    static const float gain = 0.1f;
    static BehaviorState state = ARM_UP;

    // parse message and extract joint angles
    //cout << "(Hoap2Behavior) received message " << message << endl;

    shared_ptr<PredicateList> predList =
        mParser->Parse(message);

    if (predList.get() != 0)
    {
        PredicateList& list = *predList;

        for (
             PredicateList::TList::const_iterator iter = list.begin();
             iter != list.end();
             ++iter
             )
        {
            const Predicate& predicate = (*iter);

            // check for a joint percept
            switch(predicate.name[0])
            { 
            case 'H': // hinge joint (HJ)
                ParseHingeJointInfo(predicate);
                break;
            case 'U': // universal joint (UJ)
                ParseUniversalJointInfo(predicate);
                break;
            default:
                break;
            }
        }
    }

    float curAngle = 0;
    float newAngle = 0;
 
    // string stream for the server commands
    stringstream ss("");

//    curAngle = mUniversalJointSenseMap[JID_LLEG_5_6].angle1;

//    if (curAngle < 40.0)
//    {
//        newAngle = gain * (40.0 - curAngle);
//        ss << "(lleg_eff_5_6 0.0 " << newAngle << ")";
//    }

#if 1
    switch(state)
    {
        case ARM_UP:
            curAngle = mUniversalJointSenseMap[JID_RARM_1_2].angle2;
            if (curAngle < 90.0)
            {
                newAngle = gain * (90.0f - curAngle);
                ss << "(rarm_eff_1_2 0.0 " << newAngle << ")";
            }
            else
            {
                state = ARM_ROTATE;
            }
            break;
        case ARM_ROTATE:
            curAngle = mHingeJointSenseMap[JID_RARM_3].angle;
            if (curAngle > -90.0f)
            {
                newAngle = gain * (-90.0f - curAngle);
                ss << "(rarm_eff_3 " << newAngle << ")";
            }
            else    
            {
                state = ARM_WAVE_1;
            }
            break;
        case ARM_WAVE_1:
            curAngle = mHingeJointSenseMap[JID_RARM_4].angle;
            if (curAngle < 90.0)
            {
                newAngle = gain * (90.0f - curAngle);
                ss << "(rarm_eff_4 " << newAngle << ")"; 
            }
            else
            {
                state = ARM_WAVE_2;
            }
            break;
        case ARM_WAVE_2:
            curAngle = mHingeJointSenseMap[JID_RARM_4].angle;
            if (curAngle > 60.0 || curAngle <= 59.5)
            {
                newAngle = gain * (60.0f - curAngle);
                ss << "(rarm_eff_4 " << newAngle << ")";
            }
            else
            {
                state = ARM_WAVE_1;
            }
            break;
        default:   
            break;
    }

#endif

//    cout << "+++" << endl;
//    cout << "current angle: " << curAngle  << endl;
//    cout << "desired angle: " << newAngle  << endl;
//    cout << "(Behavior) sending string " << ss.str() << " to server" << endl;
//    cout << "State is " << state << endl;
//    cout << "---" << endl;

    return ss.str();
    //return string("");
}

--- NEW FILE: hoap2behavior.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
   this file is part of rcssserver3D
   Thu Mar 27 2006
   Copyright (C) 2006 RoboCup Simulation League Maintenance Committee

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef HOAP2BEHAVIOR_H
#define HOAP2BEHAVIOR_H

#include "behavior.h"
#include <oxygen/gamecontrolserver/baseparser.h>
#include <oxygen/gamecontrolserver/predicate.h>
#include <zeitgeist/zeitgeist.h>

class Hoap2Behavior : public Behavior
{
public:
    struct HingeJointSense
    {
        /** joint angle */
        float angle;

        HingeJointSense() : angle(0) {};
    };

    struct UniversalJointSense
    {
        /** joint angle axis 1*/
        float angle1;

        /** joint angle axis 2*/
        float angle2;

        UniversalJointSense() : angle1(0), angle2(0) {};
    };

    enum JointID
    {
        JID_HEAD_1   = 0,
        JID_HEAD_2   = 1,
        JID_LLEG_1   = 2,
        JID_RLEG_1   = 3,
        JID_LLEG_2_3 = 4,
        JID_RLEG_2_3 = 5,
        JID_LLEG_4   = 6,
        JID_RLEG_4   = 7,
        JID_LLEG_5_6 = 8,
        JID_RLEG_5_6 = 9,
        JID_LARM_1_2 = 10,
        JID_RARM_1_2 = 11,
        JID_LARM_3   = 12,
        JID_RARM_3   = 13,
        JID_LARM_4   = 14,
        JID_RARM_4   = 15,
        JID_LARM_5   = 16,
        JID_RARM_5   = 17
    };

    enum BehaviorState
    {
        ARM_UP = 0,
        ARM_ROTATE = 1,
        ARM_WAVE_1 = 2,
        ARM_WAVE_2 = 3
    };

public:
    Hoap2Behavior();

    virtual std::string Init();
    virtual std::string Think(const std::string& message);

protected:
    void SetupJointIDMap();
    void ParseHingeJointInfo(const oxygen::Predicate& predicate);
    void ParseUniversalJointInfo(const oxygen::Predicate& predicate);

protected:
    zeitgeist::Zeitgeist mZG;
    boost::shared_ptr<oxygen::BaseParser> mParser;

    // mapping from joint id to joint hinge sense object
    typedef std::map<JointID, HingeJointSense> THingeJointSenseMap;
    THingeJointSenseMap mHingeJointSenseMap;

    // mapping from joint id to joint hinge sense object
    typedef std::map<JointID, UniversalJointSense> TUniversalJointSenseMap;
    TUniversalJointSenseMap mUniversalJointSenseMap;

    // mapping from object name to joint id
    typedef std::map<std::string, JointID> TJointIDMap;
    TJointIDMap mJointIDMap;
};

#endif // HOAP2BEHAVIOR_H

--- NEW FILE: soccerbotbehavior.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
   this file is part of rcssserver3D
   Thu Nov 8 2005
   Copyright (C) 2005 Koblenz University

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "soccerbotbehavior.h"
#include <iostream>
#include <sstream>
#include <cmath>

using namespace oxygen;
using namespace zeitgeist;
using namespace std;
using namespace boost;
using namespace salt;

SoccerbotBehavior::SoccerbotBehavior() : mZG("." PACKAGE_NAME)
{
}

void SoccerbotBehavior::SetupJointIDMap()
{
    mJointIDMap.clear();
    mJointIDMap["head_joint_2"]   = JID_HEAD_2;
    mJointIDMap["head_joint_1"]   = JID_HEAD_1;
    mJointIDMap["lleg_joint_1"]   = JID_LLEG_1;
    mJointIDMap["rleg_joint_1"]   = JID_RLEG_1;
    mJointIDMap["lleg_joint_2_3"] = JID_LLEG_2_3;
    mJointIDMap["rleg_joint_2_3"] = JID_RLEG_2_3;
    mJointIDMap["lleg_joint_4"]   = JID_LLEG_4;
    mJointIDMap["rleg_joint_4"]   = JID_RLEG_4;
    mJointIDMap["lleg_joint_5_6"] = JID_LLEG_5_6;
    mJointIDMap["rleg_joint_5_6"] = JID_RLEG_5_6;
    mJointIDMap["larm_joint_1_2"] = JID_LARM_1_2;
    mJointIDMap["rarm_joint_1_2"] = JID_RARM_1_2;
    mJointIDMap["larm_joint_3"]   = JID_LARM_3;
    mJointIDMap["rarm_joint_3"]   = JID_RARM_3;
    mJointIDMap["larm_joint_4"]   = JID_LARM_4;
    mJointIDMap["rarm_joint_4"]   = JID_RARM_4;
    mJointIDMap["larm_joint_5"]   = JID_LARM_5;
    mJointIDMap["rarm_joint_5"]   = JID_RARM_5;
}

string SoccerbotBehavior::Init()
{
    mZG.GetCore()->ImportBundle("sexpparser");

    mParser = shared_static_cast<BaseParser>
        (mZG.GetCore()->New("SexpParser"));

    if (mParser.get() == 0)
    {
        cerr << "unable to create SexpParser instance." << endl;
    }

    SetupJointIDMap();

    // use the scene effector to build the agent and beam to a
    // position near the center of the playing field
    return
        "(scene rsg/agent/soccerbot.rsg)";
}

void SoccerbotBehavior::ParseHingeJointInfo(const oxygen::Predicate& predicate)
{
    //cout << "(SoccerbotBehavior) parsing HJ info" << endl;

    // read the object name
    string name;
    Predicate::Iterator iter(predicate);

    if (! predicate.GetValue(iter, "name", name))
    {
        return;
    }

    // try to lookup the joint id
    TJointIDMap::iterator idIter = mJointIDMap.find(name);
    if (idIter == mJointIDMap.end())
    {
        cerr << "(SoccerbotBehavior) unknown joint id!" << endl;
        return;
    }

    JointID jid = (*idIter).second;

    // read the angle value
    HingeJointSense sense;
    if (! predicate.GetValue(iter,"ax", sense.angle))
    {
        return;
    }

    // update the map
    mHingeJointSenseMap[jid] = sense;
}

void SoccerbotBehavior::ParseUniversalJointInfo(const oxygen::Predicate& predicate)
{
    // read the object name
    string name;
    Predicate::Iterator iter(predicate);

    if (! predicate.GetValue(iter, "name", name))
    {
        return;
    }

    // try to lookup the joint id
    TJointIDMap::iterator idIter = mJointIDMap.find(name);
    if (idIter == mJointIDMap.end())
    {
        cerr << "(SoccerbotBehavior) unknown joint id!" << endl;
        return;
    }

    JointID jid = (*idIter).second;

    // record the angle and rate values
    UniversalJointSense sense;

    // try to read axis1 angle
    if (! predicate.GetValue(iter,"ax1", sense.angle1))
    {
        cerr << "(SoccerbotBehavior) could not parse universal joint angle1!" << endl;
        return;
    }

    // try to read axis2 angle
    if (! predicate.GetValue(iter,"ax2", sense.angle2))
    {
        cerr << "(SoccerbotBehavior) could not parse universal joint angle2!" << endl;
        return;
    }

    // update the map
    mUniversalJointSenseMap[jid] = sense;
}

static const float gain = 0.1f;
static SoccerbotBehavior::BehaviorState state = SoccerbotBehavior::ARM_UP;

float SoccerbotBehavior::ClampAngle(float angle)
{
    if (angle >= 0.0)
    {
        if (angle < gain)
        {
            return gain;
        }
    } else
    {
        if (angle > - gain)
        {
            return -gain;
        }
    }

    return angle;
}

string SoccerbotBehavior::Think(const std::string& message)
{
    // parse message and extract joint angles
    //cout << "(SoccerbotBehavior) received message " << message << endl;

    shared_ptr<PredicateList> predList =
        mParser->Parse(message);

    if (predList.get() != 0)
    {
        PredicateList& list = *predList;

        for (
             PredicateList::TList::const_iterator iter = list.begin();
             iter != list.end();
             ++iter
             )
        {
            const Predicate& predicate = (*iter);

            // check for a joint percept
            switch(predicate.name[0])
            {
            case 'H': // hinge joint (HJ)
                ParseHingeJointInfo(predicate);
                break;
            case 'U': // universal joint (UJ)
                ParseUniversalJointInfo(predicate);
                break;
            default:
                break;
            }
        }
    }

    float curAngle = 0;
    float newAngle = 0;

    // string stream for the server commands
    stringstream ss("");

    switch(state)
    {
        case ARM_UP:
            curAngle = mUniversalJointSenseMap[JID_RARM_1_2].angle2;
            if (curAngle < 90.0)
            {
                newAngle = ClampAngle(gain * (90.0f - curAngle));
                ss << "(rarm_eff_1_2 0.0 " << newAngle << ")";
            }
            else
            {
                state = ARM_ROTATE;
            }
            break;

        case ARM_ROTATE:
            curAngle = mHingeJointSenseMap[JID_RARM_3].angle;
            if (curAngle > -90.0f)
            {
                newAngle = ClampAngle(gain * (-90.0f - curAngle));
                ss << "(rarm_eff_3 " << newAngle << ")";
            }
            else
            {
                state = ARM_WAVE_1;
            }
            break;

        case ARM_WAVE_1:
            curAngle = mHingeJointSenseMap[JID_RARM_4].angle;
            if (curAngle < 90.0)
            {
                newAngle = ClampAngle(gain * (90.0f - curAngle));
                ss << "(rarm_eff_4 " << newAngle << ")";
            }
            else
            {
                state = ARM_WAVE_2;
            }
            break;

        case ARM_WAVE_2:
            curAngle = mHingeJointSenseMap[JID_RARM_4].angle;
            if (curAngle > 60.0 || curAngle <= 59.5)
            {
                newAngle = ClampAngle(gain * (60.0f - curAngle));
                ss << "(rarm_eff_4 " << newAngle << ")";
            }
            else
            {
                state = ARM_DOWN;
            }
            break;

        case ARM_DOWN:
           curAngle = mUniversalJointSenseMap[JID_RARM_1_2].angle2;
            if (curAngle > 0)
            {
                newAngle = ClampAngle(-gain * 20.0f);
                ss << "(rarm_eff_1_2 0.0 " << newAngle << ")";
            }
            else
            {
                state = ARM_UP;
            }
            break;

        default:
            break;
    }

    //    cout << "+++" << endl;
    //    cout << "current angle: " << curAngle  << endl;
    //    cout << "desired angle: " << newAngle  << endl;
    //    cout << "(Behavior) sending string " << ss.str() << " to server" << endl;
    //    cout << "State is " << state << endl;
    //    cout << "---" << endl;

    return ss.str();
}

--- NEW FILE: soccerbotbehavior.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
   this file is part of rcssserver3D
   Thu Mar 27 2006
   Copyright (C) 2006 RoboCup Simulation League Maintenance Committee

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef SOCCERBOTBEHAVIOR_H
#define SOCCERBOTBEHAVIOR_H

#include "behavior.h"
#include <oxygen/gamecontrolserver/baseparser.h>
#include <oxygen/gamecontrolserver/predicate.h>
#include <zeitgeist/zeitgeist.h>

class SoccerbotBehavior : public Behavior
{
public:
    struct HingeJointSense
    {
        /** joint angle */
        float angle;

        HingeJointSense() : angle(0) {};
    };

    struct UniversalJointSense
    {
        /** joint angle axis 1*/
        float angle1;

        /** joint angle axis 2*/
        float angle2;

        UniversalJointSense() : angle1(0), angle2(0) {};
    };

    enum JointID
    {
        JID_HEAD_1   = 0,
        JID_HEAD_2   = 1,
        JID_LLEG_1   = 2,
        JID_RLEG_1   = 3,
        JID_LLEG_2_3 = 4,
        JID_RLEG_2_3 = 5,
        JID_LLEG_4   = 6,
        JID_RLEG_4   = 7,
        JID_LLEG_5_6 = 8,
        JID_RLEG_5_6 = 9,
        JID_LARM_1_2 = 10,
        JID_RARM_1_2 = 11,
        JID_LARM_3   = 12,
        JID_RARM_3   = 13,
        JID_LARM_4   = 14,
        JID_RARM_4   = 15,
        JID_LARM_5   = 16,
        JID_RARM_5   = 17
    };

    enum BehaviorState
    {
        ARM_UP = 0,
        ARM_ROTATE = 1,
        ARM_WAVE_1 = 2,
        ARM_WAVE_2 = 3,
        ARM_DOWN = 4
    };

public:
    SoccerbotBehavior();

    virtual std::string Init();
    virtual std::string Think(const std::string& message);

protected:
    void SetupJointIDMap();
    void ParseHingeJointInfo(const oxygen::Predicate& predicate);
    void ParseUniversalJointInfo(const oxygen::Predicate& predicate);

    float ClampAngle(float angle);

protected:
    zeitgeist::Zeitgeist mZG;
    boost::shared_ptr<oxygen::BaseParser> mParser;

    // mapping from joint id to joint hinge sense object
    typedef std::map<JointID, HingeJointSense> THingeJointSenseMap;
    THingeJointSenseMap mHingeJointSenseMap;

    // mapping from joint id to joint hinge sense object
    typedef std::map<JointID, UniversalJointSense> TUniversalJointSenseMap;
    TUniversalJointSenseMap mUniversalJointSenseMap;

    // mapping from object name to joint id
    typedef std::map<std::string, JointID> TJointIDMap;
    TJointIDMap mJointIDMap;
};

#endif // SOCCERBOTBEHAVIOR_H

--- NEW FILE: behavior.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2003 Koblenz University
   $Id: behavior.h,v 1.2 2007/03/17 06:52:44 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef BEHAVIOR_H
#define BEHAVIOR_H

#include <string>

class Behavior
{
public:
    /** called once when the initially connected to the server */
    virtual std::string Init() = 0;

    /** called for every message received from the server; should
        return an action string
     */
    virtual std::string Think(const std::string& message) = 0;
};

#endif // BEHAVIOR_H