From: <he...@us...> - 2013-06-09 18:35:19
|
Revision: 356 http://sourceforge.net/p/simspark/svn/356 Author: hedayat Date: 2013-06-09 18:35:13 +0000 (Sun, 09 Jun 2013) Log Message: ----------- Merge (except renderserver.cpp) with trunk Modified Paths: -------------- branches/gui-bullet/rcssserver3d/CMakeLists.txt branches/gui-bullet/rcssserver3d/ChangeLog branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/hingejoint.rsg branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoarm.rsg branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoleg.rsg branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/universaljoint.rsg branches/gui-bullet/rcssserver3d/plugin/soccer/CMakeLists.txt branches/gui-bullet/rcssserver3d/plugin/soccer/agentcollisionhandler/agentcollisionhandler.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate.h branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate_c.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/agentstateperceptor/agentstateperceptor.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/ball/ball.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/ballstateaspect/ballstateaspect.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/beameffector/beameffector.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/catcheffector/catcheffector.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/createeffector/createeffector.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/driveeffector/driveeffector.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/export.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/gamestateaspect/gamestateaspect.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/gamestateaspect/gamestateaspect.h branches/gui-bullet/rcssserver3d/plugin/soccer/gametimeperceptor/gametimeperceptor.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/hmdp_effector/hmdpeffector.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/hmdp_effector/hmdpperceptor.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/initeffector/initeffector.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/initeffector/singlematiniteffector.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/initeffector/staticmeshiniteffector.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/internalsoccermonitor/internalsoccerinput.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/internalsoccermonitor/internalsoccerrender.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/kickeffector/kickeffector.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/pantilteffector/pantilteffector.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/rcs3dmonitor/rcs3dmonitor.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.h branches/gui-bullet/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor_c.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/sayeffector/sayeffector.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/sexpmonitor/sexpmonitor.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/soccerbase/soccerbase.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/soccercontrolaspect/soccercontrolaspect.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/soccernode/soccernode.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/soccerruleaspect/soccerruleaspect.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/soccerruleaspect/soccerruleaspect.h branches/gui-bullet/rcssserver3d/plugin/soccer/trainercommandparser/trainercommandparser.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/visionperceptor/visionperceptor.cpp branches/gui-bullet/rcssserver3d/plugin/soccermonitor/soccerinput.cpp branches/gui-bullet/rcssserver3d/plugin/soccermonitor/soccerinputlogplayer.cpp branches/gui-bullet/rcssserver3d/plugin/soccermonitor/soccerrender.cpp branches/gui-bullet/rcssserver3d/rcssagent3d/hoap2behavior.cpp branches/gui-bullet/rcssserver3d/rcssagent3d/naobehavior.cpp branches/gui-bullet/rcssserver3d/rcssagent3d/soccerbehavior.cpp branches/gui-bullet/rcssserver3d/rcssagent3d/soccerbotbehavior.cpp branches/gui-bullet/rcssserver3d/rcssmonitor3d/CMakeLists.txt branches/gui-bullet/rcssserver3d/rcssserver3d/CMakeLists.txt branches/gui-bullet/rcssserver3d/rcssserver3d/naosoccersim.rb branches/gui-bullet/spark/CMakeLists.txt branches/gui-bullet/spark/ChangeLog branches/gui-bullet/spark/lib/kerosin/fontserver/glyph.cpp branches/gui-bullet/spark/lib/kerosin/imageserver/imageserver.cpp branches/gui-bullet/spark/lib/kerosin/inputserver/inputcontrol.cpp branches/gui-bullet/spark/lib/kerosin/inputserver/inputserver.cpp branches/gui-bullet/spark/lib/kerosin/materialserver/material2dtexture.cpp branches/gui-bullet/spark/lib/kerosin/materialserver/materialserver.cpp branches/gui-bullet/spark/lib/kerosin/openglserver/openglserver.cpp branches/gui-bullet/spark/lib/kerosin/renderserver/rendercontrol.cpp branches/gui-bullet/spark/lib/kerosin/renderserver/renderserver.cpp branches/gui-bullet/spark/lib/kerosin/sceneserver/singlematnode.cpp branches/gui-bullet/spark/lib/kerosin/sceneserver/staticmesh.cpp branches/gui-bullet/spark/lib/kerosin/soundserver/soundserver.cpp branches/gui-bullet/spark/lib/oxygen/agentaspect/agentaspect.cpp branches/gui-bullet/spark/lib/oxygen/agentaspect/effector.cpp branches/gui-bullet/spark/lib/oxygen/controlaspect/controlaspect.cpp branches/gui-bullet/spark/lib/oxygen/gamecontrolserver/gamecontrolserver.cpp branches/gui-bullet/spark/lib/oxygen/geometryserver/geometryserver.cpp branches/gui-bullet/spark/lib/oxygen/geometryserver/indexbuffer.cpp branches/gui-bullet/spark/lib/oxygen/monitorserver/monitorserver.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/angularmotor.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/balljoint.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/body.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/bodycontroller.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/boxcollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/capsulecollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/collider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/collisionhandler.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/compoundcollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/concavecollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/conecollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/conetwistjoint.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/contactjointhandler.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/convexcollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/cylindercollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/dynamicbody.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/emptycollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/fixedjoint.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/generic6dofjoint.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/hinge2joint.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/hingejoint.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/joint.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/physicsobject.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/physicsserver.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/planecollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/raycollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/rigidbody.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/sliderjoint.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/softbody.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/space.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/spherecollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/staticbody.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/transformcollider.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/universaljoint.cpp branches/gui-bullet/spark/lib/oxygen/physicsserver/world.cpp branches/gui-bullet/spark/lib/oxygen/sceneserver/basenode.cpp branches/gui-bullet/spark/lib/oxygen/sceneserver/fpscontroller.cpp branches/gui-bullet/spark/lib/oxygen/sceneserver/sceneserver.cpp branches/gui-bullet/spark/lib/oxygen/sceneserver/transform.cpp branches/gui-bullet/spark/lib/oxygen/simulationserver/simcontrolnode.cpp branches/gui-bullet/spark/lib/oxygen/simulationserver/simulationserver.cpp branches/gui-bullet/spark/lib/oxygen/spadesserver/spadesserver.cpp branches/gui-bullet/spark/lib/zeitgeist/class.cpp branches/gui-bullet/spark/lib/zeitgeist/core.cpp branches/gui-bullet/spark/lib/zeitgeist/core.h branches/gui-bullet/spark/lib/zeitgeist/corecontext.cpp branches/gui-bullet/spark/lib/zeitgeist/fileserver/fileserver.cpp branches/gui-bullet/spark/lib/zeitgeist/leaf.cpp branches/gui-bullet/spark/lib/zeitgeist/leaf.h branches/gui-bullet/spark/lib/zeitgeist/node.cpp branches/gui-bullet/spark/lib/zeitgeist/node.h branches/gui-bullet/spark/plugin/accelerometer/accelerometer.cpp branches/gui-bullet/spark/plugin/collisionperceptor/forceresistanceperceptor.cpp branches/gui-bullet/spark/plugin/collisionperceptor/perceptorhandler.cpp branches/gui-bullet/spark/plugin/collisionperceptor/touchperceptorhandler.cpp branches/gui-bullet/spark/plugin/filesystemrar/filesystemrar.cpp branches/gui-bullet/spark/plugin/forceeffector/forceeffector.cpp branches/gui-bullet/spark/plugin/gyrorateperceptor/gyrorateperceptor.cpp branches/gui-bullet/spark/plugin/imageperceptor/imageperceptor.cpp branches/gui-bullet/spark/plugin/inputsdl/inputsystemsdl.cpp branches/gui-bullet/spark/plugin/objimporter/objimporter.cpp branches/gui-bullet/spark/plugin/perfectvisionperceptor/perfectvisionperceptor.cpp branches/gui-bullet/spark/plugin/rosimporter/rosimporter.cpp branches/gui-bullet/spark/plugin/rubysceneimporter/rubysceneimporter.cpp branches/gui-bullet/spark/plugin/sceneeffector/sceneaction.h branches/gui-bullet/spark/plugin/sceneeffector/sceneeffector.cpp branches/gui-bullet/spark/plugin/sparkagent/hinge2effector.cpp branches/gui-bullet/spark/plugin/sparkagent/hingeeffector.cpp branches/gui-bullet/spark/plugin/sparkagent/timeperceptor.cpp branches/gui-bullet/spark/plugin/sparkagent/universaljointeffector.cpp branches/gui-bullet/spark/plugin/sparkmonitor/sparkmonitor.cpp branches/gui-bullet/spark/plugin/sparkmonitor/sparkmonitorclient.cpp branches/gui-bullet/spark/plugin/sparkmonitor/sparkmonitorlogfileserver.cpp branches/gui-bullet/spark/spark/spark.cpp branches/gui-bullet/spark/test/coretest/main.cpp branches/gui-bullet/spark/test/fonttest/main.cpp branches/gui-bullet/spark/test/inputtest/main.cpp branches/gui-bullet/spark/test/scenetest/main.cpp branches/gui-bullet/spark/test/scenetest/md5mesh.cpp Added Paths: ----------- branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao.rsg branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao_hetero.rsg branches/gui-bullet/rcssserver3d/plugin/soccer/soccerruleaspect/soccerruleitem.cpp branches/gui-bullet/rcssserver3d/plugin/soccer/soccerruleaspect/soccerruleitem.h branches/gui-bullet/rcssserver3d/plugin/soccer/soccerruleaspect/soccerruleitem_c.cpp branches/gui-bullet/rcssserver3d/rcssserver3d/naorobottypes.rb branches/gui-bullet/spark/doc/papers/2013/ Removed Paths: ------------- branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao.rsg Property Changed: ---------------- branches/gui-bullet/rcssserver3d/ branches/gui-bullet/spark/ Index: branches/gui-bullet/rcssserver3d =================================================================== --- branches/gui-bullet/rcssserver3d 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d 2013-06-09 18:35:13 UTC (rev 356) Property changes on: branches/gui-bullet/rcssserver3d ___________________________________________________________________ Added: svn:mergeinfo ## -0,0 +1 ## +/trunk/rcssserver3d:329-355 \ No newline at end of property Modified: branches/gui-bullet/rcssserver3d/CMakeLists.txt =================================================================== --- branches/gui-bullet/rcssserver3d/CMakeLists.txt 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/CMakeLists.txt 2013-06-09 18:35:13 UTC (rev 356) @@ -26,7 +26,7 @@ find_package(Spark REQUIRED) find_package(Freetype REQUIRED) -find_package(Boost REQUIRED) +find_package(Boost REQUIRED system) find_package(SDL REQUIRED) find_package(DevIL REQUIRED) find_package(ODE REQUIRED) Modified: branches/gui-bullet/rcssserver3d/ChangeLog =================================================================== --- branches/gui-bullet/rcssserver3d/ChangeLog 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/ChangeLog 2013-06-09 18:35:13 UTC (rev 356) @@ -1,3 +1,50 @@ +2013-06-09 Hedayat Vatankhah <hed...@gm...> + + * plugin/soccer/gamestateaspect/gamestateaspect.h: + * plugin/soccer/gamestateaspect/gamestateaspect.cpp: + * plugin/soccer/agentstate/agentstate.cpp (AgentState::OnUnlink): + * rcssserver3d/naosoccersim.rb: + - control the number of heterogeneous players each team can use. It limits + both the total number of heterogeneous players of each team + (MaxTotalHeteroCount variable in naosoccersim.rb), and the number of + heterogeneous players of each type (MaxHeteroTypeCount variable) + +2013-06-06 Hedayat Vatankhah <hed...@gm...> + + * plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.h: + * plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.cpp: + * plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor_c.cpp: + * data/rsg/agent/nao/naoneckhead.rsg: + - added MyOrientation sensing by Patrick (setSenseMyOrien in rsg file) + +2013-04-25 Hedayat Vatankhah <hed...@gm...> + + * data/rsg/agent/nao/nao_hetero.rsg: + * plugin/soccer/agentstate/agentstate.h: + * plugin/soccer/agentstate/agentstate.cpp: + * plugin/soccer/agentstate/agentstate_c.cpp: + - added robot type to agent state + +2012-11-25 Hedayat Vatankhah <hed...@gm...> + + * data/rsg/agent/nao/nao_hetero.rsg: + - nao_hetero.rsg can be used to create different variants of Nao robot + - nao.rsg a compatbility scene to create default Nao type (type 0) + + * rcssserver3d/naorobottypes.rb: + * rcssserver3d/naosoccersim.rb: + * rcssserver3d/CMakeLists.txt: + - added naorobottypes.rb to initialize Nao heterogeneous robot parameters + +2012-06-21 Hedayat Vatankhah <hed...@gm...> + + * plugin/soccer/soccerruleaspect/soccerruleaspect.cpp: + - let a player kick the ball more than once in kickoff if there are not + more than 2 players inside the game. Should be removed when a proper + "Penalty mode" is added to the simulator + - small cleanup: call SoccerBase::GetGameControlServer() to access + game control server + 2012-05-23 Hedayat Vatankhah <hed...@gm...> * NEWS: Modified: branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/hingejoint.rsg =================================================================== --- branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/hingejoint.rsg 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/hingejoint.rsg 2013-06-09 18:35:13 UTC (rev 356) @@ -6,14 +6,9 @@ $Path_Body1 $Path_Body2 $Anchor_X $Anchor_Y $Anchor_Z $Axis_X $Axis_Y $Axis_Z - $Min $Max + $MinDeg $MaxDeg + $MaxAbsSpeed ) - - (def $PI 3.14159265) - ;(def $MinDeg (eval $Min * (eval 180.0 / $PI))) - ;(def $MaxDeg (eval $Max * (eval 180.0 / $PI))) - (def $MinDeg $Min) - (def $MaxDeg $Max) ; There are 2 types of motor installed on the real Nao, each type has ; 2 types of "Reduction ratio". @@ -23,7 +18,7 @@ ; We don't need to follow them totally that different hingeeffector is using ; different types of motor. ; All the hingeeffector uses the 351.77 - (def $MaxAbsSpeed (eval $PI * (eval 351.77 / 180.0))) + ; (def $MaxAbsSpeed (eval $PI * (eval 351.77 / 180.0))) ; ;Hinge Joint Parameters Deleted: branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao.rsg =================================================================== --- branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao.rsg 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao.rsg 2013-06-09 18:35:13 UTC (rev 356) @@ -1,138 +0,0 @@ -; -*- mode: lisp; -*- - -; -; nao robot description file. -; - -(RSG 0 1) -( - -; -; The torso parameters -; - -(def $TorsoLength 0.1) -(def $TorsoWidth 0.1) -(def $TorsoHeight 0.18) -(def $TorsoMass 1.2171) - -(def $TorsoInitX 0) -(def $TorsoInitY -1) -(def $TorsoInitZ 1.5) - -(def $loadObj (eval Nao.UseTexture)) -; -;Begin Construct of Nao -; -(nd Space - (setName spacenao) - (disableInnerCollision true) - - ; - ;The torso part - ; - (nd AgentAspect - (setName body) - (setLocalPos $TorsoInitX $TorsoInitY $TorsoInitZ) - - (switch $loadObj - (true - (nd Transform - (setLocalRotation -90 0 180) - (nd StaticMesh - (load 'models/naobody.obj') - (setScale $TorsoLength $TorsoLength $TorsoLength) - ) - ) - ) - - (false - (importScene rsg/agent/nao/box_appearance.rsg $TorsoLength $TorsoWidth $TorsoHeight matGrey) - ) - ) - - ;(importScene rsg/agent/nao/box_physics.rsg $TorsoLength $TorsoWidth $TorsoHeight $TorsoMass) - (importScene rsg/agent/nao/box_physics_nocollider.rsg $TorsoLength $TorsoWidth $TorsoHeight $TorsoMass) - (nd BoxCollider - (setName TorsoCollider) - (setBoxLengths $TorsoLength $TorsoWidth $TorsoHeight) - (importScene rsg/agent/nao/contactjointhandler.rsg) - ) - - ;Install effectors and perceptors - (nd StaticMeshInitEffector) - - (nd TimePerceptor) - - (nd AgentState - (setName AgentState) - (nd GameStatePerceptor) - (nd HearPerceptor) - (nd Transform - (nd Cylinder - (setName SelectionMarker) - (setParams 1.0 1.0) - (setScale 0.2 0.2 0.02) - (setMaterial matSelect) - (setTransparent) - ) - ) - ) - - (nd GyroRatePerceptor (setName torso)) - (nd Accelerometer (setName torso)) - - ;(nd HMDPPerceptor) - ;(nd HMDPEffector) - - (nd BeamEffector) - - (nd SayEffector) - - ;(nd VisionPerceptor - ; (setSenseMyPos false) - ; (setStaticSenseAxis false) - ; (addNoise false)) - - (nd AgentSyncEffector) - - );end of AgentAspect - - - ; - ;The neck and head part - ; - (importScene rsg/agent/nao/naoneckhead.rsg $TorsoInitX $TorsoInitY $TorsoInitZ $loadObj) - - - ; - ;The Right Arm - ; - (importScene rsg/agent/nao/naoarm.rsg - 1 r - $TorsoInitX $TorsoInitY $TorsoInitZ $loadObj) - - ; - ;The Left Arm - ; - (importScene rsg/agent/nao/naoarm.rsg - -1 l - $TorsoInitX $TorsoInitY $TorsoInitZ $loadObj) - - ; - ;The Right Leg - ; - (importScene rsg/agent/nao/naoleg.rsg - 1 r - $TorsoInitX $TorsoInitY $TorsoInitZ $loadObj) - - ; - ;The Left Leg - ; - (importScene rsg/agent/nao/naoleg.rsg - -1 l - $TorsoInitX $TorsoInitY $TorsoInitZ $loadObj) - -);end of nd Space - -);end of RSG 0 1 Copied: branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao.rsg (from rev 355, trunk/rcssserver3d/data/rsg/agent/nao/nao.rsg) =================================================================== --- branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao.rsg (rev 0) +++ branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao.rsg 2013-06-09 18:35:13 UTC (rev 356) @@ -0,0 +1,7 @@ +; -*- mode: lisp; -*- + +(RSG 0 1) +( + (importScene rsg/agent/nao/nao_hetero.rsg 0) +) + \ No newline at end of file Copied: branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao_hetero.rsg (from rev 355, trunk/rcssserver3d/data/rsg/agent/nao/nao_hetero.rsg) =================================================================== --- branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao_hetero.rsg (rev 0) +++ branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/nao_hetero.rsg 2013-06-09 18:35:13 UTC (rev 356) @@ -0,0 +1,149 @@ +; -*- mode: lisp; -*- + +; +; nao robot description file. +; + +(RSG 0 1) +( + +(templ $type) + +; +; The torso parameters +; + +;(def $TorsoLength (eval "NaoType["$type"]['tl']")) + +(def $TorsoLength 0.1) +(def $TorsoWidth 0.1) +(def $TorsoHeight 0.18) +(def $TorsoMass 1.2171) + +(def $TorsoInitX 0) +(def $TorsoInitY -1) +(def $TorsoInitZ 1.5) + +(def $loadObj (eval Nao.UseTexture)) +; +;Begin Construct of Nao +; +(nd Space + (setName spacenao) + (disableInnerCollision true) + + ; + ;The torso part + ; + (nd AgentAspect + (setName body) + (setLocalPos $TorsoInitX $TorsoInitY $TorsoInitZ) + + (switch $loadObj + (true + (nd Transform + (setLocalRotation -90 0 180) + (nd StaticMesh + (load 'models/naobody.obj') + (setScale $TorsoLength $TorsoLength $TorsoLength) + ) + ) + ) + + (false + (importScene rsg/agent/nao/box_appearance.rsg $TorsoLength $TorsoWidth $TorsoHeight matGrey) + ) + ) + + ;(importScene rsg/agent/nao/box_physics.rsg $TorsoLength $TorsoWidth $TorsoHeight $TorsoMass) + (importScene rsg/agent/nao/box_physics_nocollider.rsg $TorsoLength $TorsoWidth $TorsoHeight $TorsoMass) + (nd BoxCollider + (setName TorsoCollider) + (setBoxLengths $TorsoLength $TorsoWidth $TorsoHeight) + (importScene rsg/agent/nao/contactjointhandler.rsg) + ) + + ;Install effectors and perceptors + (nd StaticMeshInitEffector) + + (nd TimePerceptor) + + (nd AgentState + (setName AgentState) + (setRobotType $type) + (nd GameStatePerceptor) + (nd HearPerceptor) + (nd Transform + (nd Cylinder + (setName SelectionMarker) + (setParams 1.0 1.0) + (setScale 0.2 0.2 0.02) + (setMaterial matSelect) + (setTransparent) + ) + ) + ) + + (nd GyroRatePerceptor (setName torso)) + (nd Accelerometer (setName torso)) + + ;(nd HMDPPerceptor) + ;(nd HMDPEffector) + + (nd BeamEffector) + + (nd SayEffector) + + ;(nd VisionPerceptor + ; (setSenseMyPos false) + ; (setStaticSenseAxis false) + ; (addNoise false)) + + (nd AgentSyncEffector) + + );end of AgentAspect + + + ; + ;The neck and head part + ; + (importScene rsg/agent/nao/naoneckhead.rsg + $TorsoInitX $TorsoInitY $TorsoInitZ $loadObj + $type) + + + ; + ;The Right Arm + ; + (importScene rsg/agent/nao/naoarm.rsg + 1 r + $TorsoInitX $TorsoInitY $TorsoInitZ $loadObj + $type) + + ; + ;The Left Arm + ; + (importScene rsg/agent/nao/naoarm.rsg + -1 l + $TorsoInitX $TorsoInitY $TorsoInitZ $loadObj + $type) + + ; + ;The Right Leg + ; + (importScene rsg/agent/nao/naoleg.rsg + 1 r + $TorsoInitX $TorsoInitY $TorsoInitZ $loadObj + $type) + + ; + ;The Left Leg + ; + (importScene rsg/agent/nao/naoleg.rsg + -1 l + $TorsoInitX $TorsoInitY $TorsoInitZ $loadObj + $type) + +);end of nd Space + +);end of RSG 0 1 Modified: branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoarm.rsg =================================================================== --- branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoarm.rsg 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoarm.rsg 2013-06-09 18:35:13 UTC (rev 356) @@ -8,6 +8,7 @@ ; (templ $LRFactor $LeftOrRight $Torso_X $Torso_Y $Torso_Z $loadObj + $type ) ; @@ -45,7 +46,8 @@ (def $UpperarmRelShoulderZ 0) (def $ElbowRelUpperArm_X (eval $LRFactor * -0.01)) - (def $ElbowRelUpperArm_Y 0.07) + ;(def $ElbowRelUpperArm_Y 0.07) + (def $ElbowRelUpperArm_Y (eval "NaoType["$type"]['ElbowRelUpperArm_Y']")) (def $ElbowRelUpperArm_Z 0.009) (def $LowerarmRelElbow_X 0) @@ -91,8 +93,12 @@ ; ;Joint Parameters ; + (def $PI 3.14159265) + (def $MaxAbsJointSpeed (eval $PI * (eval 351.77 / 180.0))) + (def $aj1_min -120) (def $aj1_max 120) + (def $aj1_max_abs_speed $MaxAbsJointSpeed) (def $right_aj2_min -95) (def $left_aj2_min -1) @@ -105,9 +111,11 @@ (def $aj2_max_c1 (eval (eval $right_aj2_max + $left_aj2_max) * 0.5)) (def $aj2_max_c2 (eval (eval $right_aj2_max - $left_aj2_max) * 0.5)) (def $aj2_max (eval $aj2_max_c1 + (eval $LRFactor * $aj2_max_c2))) + (def $aj2_max_abs_speed $MaxAbsJointSpeed) (def $aj3_min -120) (def $aj3_max 120) + (def $aj3_max_abs_speed $MaxAbsJointSpeed) (def $right_aj4_min -1) (def $left_aj4_min -90) @@ -120,6 +128,7 @@ (def $aj4_max_c1 (eval (eval $right_aj4_max + $left_aj4_max) * 0.5)) (def $aj4_max_c2 (eval (eval $right_aj4_max - $left_aj4_max) * 0.5)) (def $aj4_max (eval $aj4_max_c1 + (eval $LRFactor * $aj4_max_c2))) + (def $aj4_max_abs_speed $MaxAbsJointSpeed) (def $aj2_Anchor_X (eval -1 * $UpperarmRelShoulderX)) @@ -152,7 +161,8 @@ ../sphereBody ../../body/boxBody 0 0 0 1 0 0 ;1 -1 -1 - $aj1_min $aj1_max) + $aj1_min $aj1_max + $aj1_max_abs_speed) ) ; @@ -175,11 +185,12 @@ (importScene rsg/agent/nao/box_physics.rsg $UpperArmSizeX $UpperArmSizeY $UpperArmSizeZ $UpperArmMass) (importScene rsg/agent/nao/hingejoint.rsg - $Joint2PerName $Joint2EffName - ../boxBody $ShoulderBodyPath - $aj2_Anchor_X $aj2_Anchor_Y $aj2_Anchor_Z - 0 0 1 ;2 -1 -1 - $aj2_min $aj2_max) + $Joint2PerName $Joint2EffName + ../boxBody $ShoulderBodyPath + $aj2_Anchor_X $aj2_Anchor_Y $aj2_Anchor_Z + 0 0 1 ;2 -1 -1 + $aj2_min $aj2_max + $aj2_max_abs_speed) ) ; @@ -195,11 +206,12 @@ (importScene rsg/agent/nao/sphere_physics_nocollider.rsg $ElbowRadius $ElbowMass) (importScene rsg/agent/nao/hingejoint.rsg - $Joint3PerName $Joint3EffName - ../sphereBody $UpperarmBodyPath - 0 0 0 - 0 1 0 ;1 -1 -1 - $aj3_min $aj3_max) + $Joint3PerName $Joint3EffName + ../sphereBody $UpperarmBodyPath + 0 0 0 + 0 1 0 ;1 -1 -1 + $aj3_min $aj3_max + $aj3_max_abs_speed) ) ; @@ -221,11 +233,12 @@ (importScene rsg/agent/nao/box_physics.rsg $LowerArmSizeX $LowerArmSizeY $LowerArmSizeZ $LowerArmMass) (importScene rsg/agent/nao/hingejoint.rsg - $Joint4PerName $Joint4EffName - ../boxBody $ElbowBodyPath - $aj4_Anchor_X $aj4_Anchor_Y $aj4_Anchor_Z - 0 0 1 ;2 -1 -1 - $aj4_min $aj4_max) + $Joint4PerName $Joint4EffName + ../boxBody $ElbowBodyPath + $aj4_Anchor_X $aj4_Anchor_Y $aj4_Anchor_Z + 0 0 1 ;2 -1 -1 + $aj4_min $aj4_max + $aj4_max_abs_speed) (nd ObjectState (setID $LowerarmName) Modified: branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoleg.rsg =================================================================== --- branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoleg.rsg 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoleg.rsg 2013-06-09 18:35:13 UTC (rev 356) @@ -8,6 +8,7 @@ ; (templ $LRFactor $LeftOrRight $Torso_X $Torso_Y $Torso_Z $loadObj + $type ) ; @@ -56,7 +57,8 @@ (def $ThighRelHip2_X 0) (def $ThighRelHip2_Y 0.01) - (def $ThighRelHip2_Z -0.04) + ;(def $ThighRelHip2_Z -0.04) + (def $ThighRelHip2_Z (eval "NaoType["$type"]['ThighRelHip2_Z']")) (def $ShankRelThigh_X 0) (def $ShankRelThigh_Y 0.005) @@ -64,7 +66,8 @@ (def $AnkleRelShank_X 0) (def $AnkleRelShank_Y -0.01) - (def $AnkleRelShank_Z -0.055) + ;(def $AnkleRelShank_Z -0.055) + (def $AnkleRelShank_Z (eval "NaoType["$type"]['AnkleRelShank_Z']")) (def $FootRelAnkle_X 0) (def $FootRelAnkle_Y 0.03) @@ -125,8 +128,12 @@ ; ;Joint Parameters ; + (def $PI 3.14159265) + (def $MaxAbsJointSpeed (eval $PI * (eval 351.77 / 180.0))) + (def $lj1_min -90) (def $lj1_max 1) + (def $lj1_max_abs_speed $MaxAbsJointSpeed) (def $right_lj2_min -45) (def $left_lj2_min -25) @@ -139,15 +146,19 @@ (def $lj2_max_c1 (eval (eval $right_lj2_max + $left_lj2_max) * 0.5)) (def $lj2_max_c2 (eval (eval $right_lj2_max - $left_lj2_max) * 0.5)) (def $lj2_max (eval $lj2_max_c1 + (eval $LRFactor * $lj2_max_c2))) + (def $lj2_max_abs_speed $MaxAbsJointSpeed) (def $lj3_min -25) (def $lj3_max 100) + (def $lj3_max_abs_speed $MaxAbsJointSpeed) (def $lj4_min -130) (def $lj4_max 1) + (def $lj4_max_abs_speed $MaxAbsJointSpeed) (def $lj5_min -45) (def $lj5_max 75) + (def $lj5_max_abs_speed (eval "NaoType["$type"]['lj5_max_abs_speed']")) (def $right_lj6_min -25) (def $left_lj6_min -45) @@ -160,6 +171,7 @@ (def $lj6_max_c1 (eval (eval $right_lj6_max + $left_lj6_max) * 0.5)) (def $lj6_max_c2 (eval (eval $right_lj6_max - $left_lj6_max) * 0.5)) (def $lj6_max (eval $lj6_max_c1 + (eval $LRFactor * $lj6_max_c2))) + (def $lj6_max_abs_speed (eval "NaoType["$type"]['lj6_max_abs_speed']")) (def $lj1_axis_x -0.7071) (def $lj1_axis_y 0) @@ -193,7 +205,8 @@ ../sphereBody ../../body/boxBody 0 0 0 $lj1_axis_x $lj1_axis_y $lj1_axis_z - $lj1_min $lj1_max) + $lj1_min $lj1_max + $lj1_max_abs_speed) ) ; @@ -212,7 +225,8 @@ ../sphereBody $Hip1BodyPath 0 0 0 ;anchor 0 1 0 ;axis - $lj2_min $lj2_max) + $lj2_min $lj2_max + $lj2_max_abs_speed) ) ; @@ -243,11 +257,12 @@ (importScene rsg/agent/nao/hingejoint.rsg - $Joint3PerName $Joint3EffName - ../boxBody $Hip2BodyPath - $lj3_Anchor_X $lj3_Anchor_Y $lj3_Anchor_Z - 1 0 0 ;1 -1 -1 - $lj3_min $lj3_max) + $Joint3PerName $Joint3EffName + ../boxBody $Hip2BodyPath + $lj3_Anchor_X $lj3_Anchor_Y $lj3_Anchor_Z + 1 0 0 ;1 -1 -1 + $lj3_min $lj3_max + $lj3_max_abs_speed) ) @@ -277,11 +292,12 @@ ) (importScene rsg/agent/nao/hingejoint.rsg - $Joint4PerName $Joint4EffName - ../boxBody $ThighBodyPath - 0 -0.01 0.045 - 1 0 0 ;0 -1 -1 - $lj4_min $lj4_max) + $Joint4PerName $Joint4EffName + ../boxBody $ThighBodyPath + 0 -0.01 0.045 + 1 0 0 ;0 -1 -1 + $lj4_min $lj4_max + $lj4_max_abs_speed) ) @@ -301,7 +317,8 @@ ../sphereBody $ShankBodyPath 0 0 0 1 0 0 ;0 -1 -1 - $lj5_min $lj5_max) + $lj5_min $lj5_max + $lj5_max_abs_speed) ) @@ -336,11 +353,12 @@ ) (importScene rsg/agent/nao/hingejoint.rsg - $Joint6PerName $Joint6EffName - ../boxBody $AnkleBodyPath - 0 -0.03 0.035 - 0 1 0 ;1 -1 -1 - $lj6_min $lj6_max) + $Joint6PerName $Joint6EffName + ../boxBody $AnkleBodyPath + 0 -0.03 0.035 + 0 1 0 ;1 -1 -1 + $lj6_min $lj6_max + $lj6_max_abs_speed) (nd ObjectState (setID $FootName) Modified: branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg =================================================================== --- branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg 2013-06-09 18:35:13 UTC (rev 356) @@ -2,7 +2,9 @@ (RSG 0 1) ( - (templ $Torso_X $Torso_Y $Torso_Z $loadObj) + (templ $Torso_X $Torso_Y $Torso_Z $loadObj + $type + ) ; ;neck parameter @@ -27,14 +29,19 @@ ; ;Head effector 1 parameter ; + (def $PI 3.14159265) + (def $MaxAbsJointSpeed (eval $PI * (eval 351.77 / 180.0))) + (def $he1_min -120) (def $he1_max 120) + (def $he1_max_abs_speed $MaxAbsJointSpeed) ; ;Head effector 2 parameter ; (def $he2_min -45) (def $he2_max 45) + (def $he2_max_abs_speed $MaxAbsJointSpeed) ; ;Begin construct neck @@ -51,7 +58,8 @@ ../capsuleBody ../../body/boxBody 0 0 0 0 0 1 - $he1_min $he1_max) + $he1_min $he1_max + $he1_max_abs_speed) ) ; @@ -84,11 +92,13 @@ ;../boxBody ../../body/boxBody 0 0 -0.005 1 0 0 - $he2_min $he2_max) + $he2_min $he2_max + $he2_max_abs_speed) (nd RestrictedVisionPerceptor (setViewCones 120 120) (setSenseMyPos false) + (setSenseMyOrien false) (setSenseBallPos false) (setStaticSenseAxis false) (addNoise true) Modified: branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/universaljoint.rsg =================================================================== --- branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/universaljoint.rsg 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/data/rsg/agent/nao/universaljoint.rsg 2013-06-09 18:35:13 UTC (rev 356) @@ -9,6 +9,7 @@ $Axis2_X $Axis2_Y $Axis2_Z $Min0 $Max0 $Min1 $Max1 + $MaxAbsSpeed ) (def $PI 3.14159265) @@ -26,7 +27,6 @@ (def $Stop_ERP 0.8) (def $FudgeFactor 1e-5) (def $Bounce 1) - (def $MaxAbsSpeed (eval $PI * (eval 351.77 / 180.0))) (nd UniversalJoint (attach $Path_Body1 $Path_Body2) Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/CMakeLists.txt =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/CMakeLists.txt 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/CMakeLists.txt 2013-06-09 18:35:13 UTC (rev 356) @@ -37,6 +37,7 @@ soccercontrolaspect/soccercontrolaspect.h soccernode/soccernode.h soccerruleaspect/soccerruleaspect.h + soccerruleaspect/soccerruleitem.h trainercommandparser/trainercommandparser.h gametimeperceptor/gametimeperceptor.h visionperceptor/visionperceptor.h @@ -106,6 +107,8 @@ soccernode/soccernode_c.cpp soccerruleaspect/soccerruleaspect.cpp soccerruleaspect/soccerruleaspect_c.cpp + soccerruleaspect/soccerruleitem.cpp + soccerruleaspect/soccerruleitem_c.cpp trainercommandparser/trainercommandparser.cpp trainercommandparser/trainercommandparser_c.cpp visionperceptor/visionperceptor.cpp Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/agentcollisionhandler/agentcollisionhandler.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/agentcollisionhandler/agentcollisionhandler.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/agentcollisionhandler/agentcollisionhandler.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -56,5 +56,5 @@ return boost::shared_ptr<AgentState>(); } - return boost::shared_static_cast<AgentState>(agentAspect->FindChildSupportingClass<AgentState>(true)); + return boost::static_pointer_cast<AgentState>(agentAspect->FindChildSupportingClass<AgentState>(true)); } Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -76,6 +76,18 @@ } void +AgentState::SetRobotType(int type) +{ + mRobotType = type; +} + +int +AgentState::GetRobotType() const +{ + return mRobotType; +} + +void AgentState::SetID(const std::string& id, TPerceptType pt) { std::istringstream iss(id); @@ -217,7 +229,7 @@ void AgentState::UpdateHierarchyInternal() { - boost::shared_ptr<RenderNode> node = boost::shared_dynamic_cast<RenderNode>(GetChild("SelectionMarker", true)); + boost::shared_ptr<RenderNode> node = boost::dynamic_pointer_cast<RenderNode>(GetChild("SelectionMarker", true)); if (!node) { GetLog()->Error() << "ERROR: (AgentState::UpdateHierarchyInternal) could not find selection marker\n"; @@ -238,8 +250,9 @@ GetLog()->Error() << "ERROR: (AgentState::OnUnlink) could not get game state\n"; return; } - - game_state->ReturnUniform(GetTeamIndex(), GetUniformNumber()); + + game_state->ReturnUniform(GetTeamIndex(), GetUniformNumber(), + GetRobotType()); } bool Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate.h =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate.h 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate.h 2013-06-09 18:35:13 UTC (rev 356) @@ -59,6 +59,12 @@ /** returns the uniform number as integer */ int GetUniformNumber() const; + /** Set the robot type. */ + void SetRobotType(int type); + + /** returns the robot type */ + int GetRobotType() const; + /** Set the object id for perceptors. * * This method is the same as SetUniformNumber for AgentState. @@ -97,25 +103,25 @@ /** Whether agent is selected */ bool IsSelected() const; - + /** Select agent */ void Select(bool s = true); - + /** Unselect agent */ void UnSelect(); - + /** Backup old touch group and create new empty one */ void NewTouchGroup(); - + /** Get the touch group of the previous step */ boost::shared_ptr<TouchGroup> GetOldTouchGroup(); - + /** Get the current touch group */ boost::shared_ptr<TouchGroup> GetTouchGroup(); - + /** Set the current touch group */ void SetTouchGroup(boost::shared_ptr<TouchGroup> group); - + protected: /** team index */ TTeamIndex mTeamIndex; @@ -123,6 +129,9 @@ /** uniform number */ int mUniformNumber; + /** robot type */ + int mRobotType; + /** motor temperature */ float mTemperature; @@ -161,7 +170,7 @@ /** is this agent selected */ bool mSelected; - + boost::shared_ptr<TouchGroup> mOldTouchGroup; boost::shared_ptr<TouchGroup> mTouchGroup; Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate_c.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate_c.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/agentstate/agentstate_c.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -25,8 +25,25 @@ using namespace boost; using namespace oxygen; +FUNCTION(AgentState,setRobotType) +{ + int inType; + + if ( + (in.GetSize() != 1) || + (! in.GetValue(in.begin(), inType)) + ) + { + return false; + } + + obj->SetRobotType(inType); + return true; +} + void CLASS(AgentState)::DefineClass() { DEFINE_BASECLASS(ObjectState); + DEFINE_FUNCTION(setRobotType); } Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/agentstateperceptor/agentstateperceptor.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/agentstateperceptor/agentstateperceptor.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/agentstateperceptor/agentstateperceptor.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -61,7 +61,7 @@ predicate.parameter.Clear(); boost::shared_ptr<BaseNode> parent = - shared_dynamic_cast<BaseNode>(GetParent().lock()); + dynamic_pointer_cast<BaseNode>(GetParent().lock()); if (parent.get() == 0) { Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/ball/ball.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/ball/ball.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/ball/ball.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -46,7 +46,7 @@ if (mBody.get() == 0) { - mBody = shared_dynamic_cast<RigidBody>(GetChildOfClass("RigidBody")); + mBody = dynamic_pointer_cast<RigidBody>(GetChildOfClass("RigidBody")); } } @@ -60,7 +60,7 @@ // mBallStateAspect during OnLink if (mBallStateAspect.get() == 0) { - mBallStateAspect = shared_dynamic_cast<BallStateAspect> + mBallStateAspect = dynamic_pointer_cast<BallStateAspect> (GetCore()->Get("/sys/server/gamecontrol/BallStateAspect")); if (mBallStateAspect.get() == 0) return; } Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/ballstateaspect/ballstateaspect.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/ballstateaspect/ballstateaspect.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/ballstateaspect/ballstateaspect.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -74,7 +74,7 @@ if (agents.size() > 0) { - mLastCollidingAgent = shared_static_cast<AgentAspect> + mLastCollidingAgent = static_pointer_cast<AgentAspect> (agents.front().lock()); mLastAgentCollisionTime = mGameState->GetTime(); Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/beameffector/beameffector.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/beameffector/beameffector.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/beameffector/beameffector.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -61,7 +61,7 @@ } boost::shared_ptr<BeamAction> beamAction = - shared_dynamic_cast<BeamAction>(mAction); + dynamic_pointer_cast<BeamAction>(mAction); mAction.reset(); Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/catcheffector/catcheffector.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/catcheffector/catcheffector.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/catcheffector/catcheffector.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -83,7 +83,7 @@ } boost::shared_ptr<CatchAction> catchAction = - shared_dynamic_cast<CatchAction>(mAction); + dynamic_pointer_cast<CatchAction>(mAction); mAction.reset(); if (catchAction.get() == 0) { @@ -177,7 +177,7 @@ SoccerBase::GetSoccerRuleAspect(*this,mSoccerRule); - mAgent = shared_dynamic_cast<AgentAspect>(GetParent().lock()); + mAgent = dynamic_pointer_cast<AgentAspect>(GetParent().lock()); if (mAgent.get() == 0) { @@ -188,7 +188,7 @@ } boost::shared_ptr<SphereCollider> geom = - shared_dynamic_cast<SphereCollider>(mAgent->GetChild("geometry")); + dynamic_pointer_cast<SphereCollider>(mAgent->GetChild("geometry")); if (geom.get() == 0) { GetLog()->Error() Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/createeffector/createeffector.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/createeffector/createeffector.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/createeffector/createeffector.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -42,7 +42,7 @@ return; boost::shared_ptr<CreateAction> createAction = - shared_dynamic_cast<CreateAction>(mAction); + dynamic_pointer_cast<CreateAction>(mAction); mAction.reset(); if (createAction.get() == 0) { Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/driveeffector/driveeffector.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/driveeffector/driveeffector.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/driveeffector/driveeffector.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -50,7 +50,7 @@ } boost::shared_ptr<BaseNode> parent = - shared_dynamic_cast<BaseNode>(GetParent().lock()); + dynamic_pointer_cast<BaseNode>(GetParent().lock()); if (parent.get() == 0) { @@ -59,7 +59,7 @@ return false; } - boost::shared_ptr<DriveAction> driveAction = shared_dynamic_cast<DriveAction>(action); + boost::shared_ptr<DriveAction> driveAction = dynamic_pointer_cast<DriveAction>(action); if (driveAction.get() == 0) { @@ -117,7 +117,7 @@ SoccerBase::GetAgentState(*this,mAgentState); boost::shared_ptr<SphereCollider> geom = - shared_dynamic_cast<SphereCollider>(mTransformParent->GetChild("geometry")); + dynamic_pointer_cast<SphereCollider>(mTransformParent->GetChild("geometry")); mMaxDistance = 0.001; if (geom.get() == 0) Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/export.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/export.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/export.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -36,6 +36,7 @@ #include "gamestateaspect/gamestateitem.h" #include "ballstateaspect/ballstateaspect.h" #include "soccerruleaspect/soccerruleaspect.h" +#include "soccerruleaspect/soccerruleitem.h" #include "agentstate/agentstate.h" #include "sexpmonitor/sexpmonitor.h" #include "internalsoccermonitor/internalsoccerrender.h" @@ -63,6 +64,7 @@ ZEITGEIST_EXPORT(GameStateItem); ZEITGEIST_EXPORT(BallStateAspect); ZEITGEIST_EXPORT(SoccerRuleAspect); + ZEITGEIST_EXPORT(SoccerRuleItem); ZEITGEIST_EXPORT(BeamEffector); ZEITGEIST_EXPORT(CatchEffector); ZEITGEIST_EXPORT(CreateEffector); Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/gamestateaspect/gamestateaspect.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/gamestateaspect/gamestateaspect.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/gamestateaspect/gamestateaspect.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -38,14 +38,21 @@ mFupTime = 0; mLastModeChange = 0; mGameHalf = GH_FIRST; + mRobotTypeCount[0].push_back(0); // add count for type 0 (default type) + mRobotTypeCount[1].push_back(0); + mHeteroCount[0] = 0; + mHeteroCount[1] = 0; mScore[0] = 0; mScore[1] = 0; mLastKickOffGameHalf = GH_NONE; mNextHalfKickOff = TI_NONE; mLeftInit = Vector3f(0,0,0); mRightInit = Vector3f(0,0,0); + mAgentRadius = 3.5; mFinished = false; mGamePaused = true; + mMaxHeteroTypeCount = 3; + mMaxTotalHeteroCount = 9; } GameStateAspect::~GameStateAspect() @@ -266,6 +273,82 @@ } bool +GameStateAspect::InsertRobotType(TTeamIndex idx, int type) +{ + int i; + + switch (idx) + { + case TI_LEFT: + i = 0; + break; + case TI_RIGHT: + i = 1; + break; + default: + return false; + } + + if (type) // heterogeneous player + { + if (mHeteroCount[i] == mMaxTotalHeteroCount) + { + GetLog()->Error() + << "ERROR: (GameStateAspect::InsertRobotType) Hetero player" + " count limit reached.\n"; + return false; + } + + ++mHeteroCount[i]; + + if (mRobotTypeCount[i].size() <= type) + mRobotTypeCount[i].resize(type+1); + + if (mRobotTypeCount[i][type] == mMaxHeteroTypeCount) + { + GetLog()->Error() + << "ERROR: (GameStateAspect::InsertRobotType) No more robots " + "of type " << type << " are allowed.\n"; + return false; + } + } + + ++mRobotTypeCount[i][type]; + + return true; +} + +bool +GameStateAspect::EraseRobotType(TTeamIndex idx, int type) +{ + int i; + + switch (idx) + { + case TI_LEFT: + i = 0; + break; + case TI_RIGHT: + i = 1; + break; + default: + return false; + } + + if (mRobotTypeCount[i].size() <= type || !mRobotTypeCount[i][type]) + { + return false; + } + + if (type) // heterogeneous player + --mHeteroCount[i]; + + --mRobotTypeCount[i][type]; + + return true; +} + +bool GameStateAspect::RequestUniform(boost::shared_ptr<AgentState> agentState, std::string teamName, unsigned int unum) { @@ -297,6 +380,15 @@ return false; } + if (!InsertRobotType(idx, agentState->GetRobotType())) + { + GetLog()->Error() + << "ERROR: (GameStateAspect::RequestUniform) cannot insert robot" + " of type " << agentState->GetRobotType() << " to team " + << teamName << "\n"; + return false; + } + agentState->SetUniformNumber(unum); agentState->SetTeamIndex(idx); //agentState->SetPerceptName(teamName, ObjectState::PT_Default); @@ -311,7 +403,7 @@ } bool -GameStateAspect::ReturnUniform(TTeamIndex ti, unsigned int unum) +GameStateAspect::ReturnUniform(TTeamIndex ti, unsigned int unum, int type) { if (! EraseUnum(ti,unum)) { @@ -321,6 +413,14 @@ return false; } + if (!EraseRobotType(ti, type)) + { + GetLog()->Error() + << "ERROR: (GameStateAspect::ReturnUniform) cannot erase robot " + " type " << type << " from team " << ti << "\n"; + return false; + } + return true; } @@ -445,6 +545,9 @@ SoccerBase::GetSoccerVar(*this, "CoinTossForKickOff", coinTossKickOff); if (!coinTossKickOff) mNextHalfKickOff = TI_LEFT; + + SoccerBase::GetSoccerVar(*this, "MaxHeteroTypeCount", mMaxHeteroTypeCount); + SoccerBase::GetSoccerVar(*this, "MaxTotalHeteroCount", mMaxTotalHeteroCount); } int Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/gamestateaspect/gamestateaspect.h =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/gamestateaspect/gamestateaspect.h 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/gamestateaspect/gamestateaspect.h 2013-06-09 18:35:13 UTC (rev 356) @@ -24,6 +24,7 @@ #include <soccercontrolaspect/soccercontrolaspect.h> #include <set> +#include <vector> class AgentState; @@ -87,7 +88,7 @@ std::string teamName, unsigned int unum); /** notifies that a uniform number is free again */ - bool ReturnUniform(TTeamIndex ti, unsigned int unum); + bool ReturnUniform(TTeamIndex ti, unsigned int unum, int type); /** returns the next uniform number not taken for the given team */ int RequestUniformNumber(TTeamIndex ti) const; @@ -133,6 +134,18 @@ contain a uniform number unum and erases it. */ bool EraseUnum(TTeamIndex idx, int unum); + /** + * Adds a robot of the given type to the given team if permitted. + * @return true if permitted + */ + bool InsertRobotType(TTeamIndex idx, int type); + + /** + * Removes a robot of the given type from the given team if exists. + * @return true on success + */ + bool EraseRobotType(TTeamIndex idx, int type); + /** returns the team index corresponding to the given teamName. If the teamname does not exist and less than two teams are registered, the given team name is registered. @@ -170,6 +183,12 @@ /** the set of uniform number for each team */ TUnumSet mUnumSet[2]; + /** the array of robot type counts for each team */ + std::vector<int> mRobotTypeCount[2]; + + /** the number of heterogeneous players for each team */ + int mHeteroCount[2]; + /** the scores of two teams */ int mScore[2]; @@ -187,6 +206,12 @@ /** flag if the game is running or paused (e.g. in goal_left/right state) */ bool mGamePaused; + + /** the maximum number of heterogeneous players of a single type per team */ + int mMaxHeteroTypeCount; + + /** the maximum number of total heterogeneous players for a team */ + int mMaxTotalHeteroCount; }; DECLARE_CLASS(GameStateAspect); Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/gametimeperceptor/gametimeperceptor.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/gametimeperceptor/gametimeperceptor.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/gametimeperceptor/gametimeperceptor.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -65,7 +65,7 @@ void GameTimePerceptor::OnLink() { - mGameState = shared_dynamic_cast<GameStateAspect> + mGameState = dynamic_pointer_cast<GameStateAspect> (GetCore()->Get("/sys/server/gamecontrol/GameStateAspect")); if (mGameState.get() == 0) Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/hmdp_effector/hmdpeffector.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/hmdp_effector/hmdpeffector.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/hmdp_effector/hmdpeffector.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -85,7 +85,7 @@ return; } - boost::shared_ptr<HMDPAction> hMDPAction = shared_dynamic_cast<HMDPAction>(mAction); + boost::shared_ptr<HMDPAction> hMDPAction = dynamic_pointer_cast<HMDPAction>(mAction); mAction.reset(); if (hMDPAction.get() == 0) { @@ -141,7 +141,7 @@ // parent should be a transform, or some other node, which has a // Body-child - mBody = shared_dynamic_cast<RigidBody>(parent->GetChildOfClass("RigidBody")); + mBody = dynamic_pointer_cast<RigidBody>(parent->GetChildOfClass("RigidBody")); if (mBody.get() == 0) { @@ -216,7 +216,7 @@ boost::shared_ptr<Leaf> join = *j_it; boost::shared_ptr<BaseNode> jparent = - shared_dynamic_cast<BaseNode>(join->GetParent().lock()); + dynamic_pointer_cast<BaseNode>(join->GetParent().lock()); std::cout << i << " " << jparent->GetName() << std::endl; i++; @@ -259,7 +259,7 @@ for (TLeafList::iterator j_it = jointList.begin(); j_it != jointList.end(); j_it++) { - boost::shared_ptr<HingeJoint> joint = shared_static_cast<HingeJoint> (*j_it); + boost::shared_ptr<HingeJoint> joint = static_pointer_cast<HingeJoint> (*j_it); servo_angle[i] = joint->GetAngle() - zeroPosServo(i); double tpos = servo_target_pos[i]; float err = servo_gain[i] * (tpos - servo_angle[i]); Modified: branches/gui-bullet/rcssserver3d/plugin/soccer/hmdp_effector/hmdpperceptor.cpp =================================================================== --- branches/gui-bullet/rcssserver3d/plugin/soccer/hmdp_effector/hmdpperceptor.cpp 2013-06-08 21:23:54 UTC (rev 355) +++ branches/gui-bullet/rcssserver3d/plugin/soccer/hmdp_effector/hmdpperceptor.cpp 2013-06-09 18:35:13 UTC (rev 356) @@ -48,10 +48,10 @@ { messageOut = ""; - boost::shared_ptr<Transform> transformParent = shared_static_cast<Transform> ( + boost::shared_ptr<Transform> transformParent = static_pointer_cast<Transform> ( FindParentSupportingClass<Transform> ().lock()); - mBody = shared_static_cast<RigidBody> (transf... [truncated message content] |