Menu
▾
▴
[simspark-svn] SF.net SVN: simspark:[354] trunk/rcssserver3d
|
From: <he...@us...> - 2013-06-05 20:03:22
|
Revision: 354
http://sourceforge.net/p/simspark/svn/354
Author: hedayat
Date: 2013-06-05 20:03:19 +0000 (Wed, 05 Jun 2013)
Log Message:
-----------
Added sensing the orientation of the agent by Patrick MacAlpine
Modified Paths:
--------------
trunk/rcssserver3d/ChangeLog
trunk/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg
trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.cpp
trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.h
trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor_c.cpp
Modified: trunk/rcssserver3d/ChangeLog
===================================================================
--- trunk/rcssserver3d/ChangeLog 2013-05-29 05:32:14 UTC (rev 353)
+++ trunk/rcssserver3d/ChangeLog 2013-06-05 20:03:19 UTC (rev 354)
@@ -1,3 +1,11 @@
+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:
+ - Add MyOrientation sensing by Patrick (setSenseMyOrien in rsg file)
+
2013-04-25 Hedayat Vatankhah <hed...@gm...>
* data/rsg/agent/nao/nao_hetero.rsg:
Modified: trunk/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg
===================================================================
--- trunk/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg 2013-05-29 05:32:14 UTC (rev 353)
+++ trunk/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg 2013-06-05 20:03:19 UTC (rev 354)
@@ -98,6 +98,7 @@
(nd RestrictedVisionPerceptor
(setViewCones 120 120)
(setSenseMyPos false)
+ (setSenseMyOrien false)
(setSenseBallPos false)
(setStaticSenseAxis false)
(addNoise true)
Modified: trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.cpp
===================================================================
--- trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.cpp 2013-05-29 05:32:14 UTC (rev 353)
+++ trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.cpp 2013-06-05 20:03:19 UTC (rev 354)
@@ -38,6 +38,7 @@
RestrictedVisionPerceptor::RestrictedVisionPerceptor() : Perceptor(),
mSenseMyPos(false),
+ mSenseMyOrien(false),
mSenseBallPos(false),
mAddNoise(true),
mStaticSenseAxis(true),
@@ -160,7 +161,7 @@
SoccerBase::GetTransformParent(*this,mTransformParent);
// SoccerBase::GetAgentState(*this, mAgentState);
SoccerBase::GetActiveScene(*this,mActiveScene);
-
+
boost::shared_ptr<AgentAspect> agent_aspect =
FindParentSupportingClass<AgentAspect>().lock();
if (agent_aspect == 0)
@@ -176,7 +177,7 @@
{
mAgentAspect = agent_aspect;
}
-
+
mAgentState = static_pointer_cast<AgentState>
(mAgentAspect->GetChildOfClass("AgentState",true));
if (mAgentState == 0)
@@ -272,7 +273,7 @@
}
boost::shared_ptr<Transform> j = od.mObj->GetTransformParent();
-
+
if (j.get() == 0)
{
continue; // this should never happen
@@ -280,7 +281,7 @@
od.mRelPos = j->GetWorldTransform().Pos() - myPos;
od.mDist = od.mRelPos.Length();
-
+
visibleNodes[node].push_back(od);
}
}
@@ -298,14 +299,14 @@
boost::shared_ptr<AgentAspect> agent_aspect =
dynamic_pointer_cast<AgentAspect>(node);
if (agent_aspect != 0)
- {
+ {
boost::shared_ptr<AgentAspect> aspect =
agent_aspect->FindParentSupportingClass<AgentAspect>().lock();
if (aspect != 0)
{
agent_aspect = aspect;
}
-
+
boost::shared_ptr<AgentState> agent_state = static_pointer_cast<AgentState>
(agent_aspect->GetChildOfClass("AgentState",true));
if (agent_state.get() == 0 ||
@@ -314,7 +315,7 @@
{
return;
}
-
+
ParameterList& element = predicate.parameter.AddList();
element.AddValue(std::string("P"));
@@ -461,21 +462,32 @@
element.AddValue(sensedMyPos[2]);
}
+ if (mSenseMyOrien)
+ {
+ // Orientation
+ TTeamIndex ti = mAgentState->GetTeamIndex();
+ Vector3f sensedMyUp = SoccerBase::FlipView(mTransformParent->GetWorldTransform().Up(), ti);
+
+ ParameterList& element = predicate.parameter.AddList();
+ element.AddValue(std::string("myorien"));
+ element.AddValue(gRadToDeg(gArcTan2(sensedMyUp[1], sensedMyUp[0])));
+ }
+
if (mSenseBallPos)
{
TTeamIndex ti = mAgentState->GetTeamIndex();
boost::shared_ptr<Ball> ball;
SoccerBase::GetBall(*this, ball);
Vector3f sensedBallPos = SoccerBase::FlipView(ball->GetWorldTransform().Pos(), ti);
-
+
ParameterList& element = predicate.parameter.AddList();
element.AddValue(std::string("ballpos"));
element.AddValue(sensedBallPos[0]);
element.AddValue(sensedBallPos[1]);
element.AddValue(sensedBallPos[2]);
-
+
}
-
+
if (mSenseLine)
{
SenseLine(predicate);
@@ -578,21 +590,32 @@
element.AddValue(sensedMyPos[2]);
}
+ if (mSenseMyOrien)
+ {
+ // Orientation
+ TTeamIndex ti = mAgentState->GetTeamIndex();
+ Vector3f sensedMyUp = SoccerBase::FlipView(mTransformParent->GetWorldTransform().Up(), ti);
+
+ ParameterList& element = predicate.parameter.AddList();
+ element.AddValue(std::string("myorien"));
+ element.AddValue(gRadToDeg(gArcTan2(sensedMyUp[1], sensedMyUp[0])));
+ }
+
if (mSenseBallPos)
{
TTeamIndex ti = mAgentState->GetTeamIndex();
boost::shared_ptr<Ball> ball;
SoccerBase::GetBall(*this, ball);
Vector3f sensedBallPos = SoccerBase::FlipView(ball->GetWorldTransform().Pos(), ti);
-
+
ParameterList& element = predicate.parameter.AddList();
element.AddValue(std::string("ballpos"));
element.AddValue(sensedBallPos[0]);
element.AddValue(sensedBallPos[1]);
element.AddValue(sensedBallPos[2]);
-
+
}
-
+
if (mSenseLine)
{
SenseLine(predicate);
@@ -665,6 +688,12 @@
}
void
+RestrictedVisionPerceptor::SetSenseMyOrien(bool sense)
+{
+ mSenseMyOrien = sense;
+}
+
+void
RestrictedVisionPerceptor::SetSenseBallPos(bool sense)
{
mSenseBallPos = sense;
@@ -691,7 +720,7 @@
// object is too close
return false;
}
-
+
const int hAngle_2 = mHViewCone >> 1;
if (gAbs(od.mTheta) > hAngle_2)
{
@@ -737,7 +766,7 @@
{
Vector3f bp3 = ld.mBeginPoint.mRelPos;
Vector3f ep3 = ld.mEndPoint.mRelPos;
-
+
if ( bp3.y() < focalLength && ep3.y() >= focalLength )
{
float t = ( focalLength - ep3.y() ) / ( bp3.y() - ep3.y() );
@@ -807,7 +836,7 @@
}
}
}
-
+
if (seeBeginPoint && seeEndPoint)
{
// make some noise
Modified: trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.h
===================================================================
--- trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.h 2013-05-29 05:32:14 UTC (rev 353)
+++ trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor.h 2013-06-05 20:03:19 UTC (rev 354)
@@ -105,7 +105,10 @@
//! Turn sensing of agent position on/off
void SetSenseMyPos(bool sense);
- //! Turn sensing of agent position on/off
+ //! Turn sensing of agent orientation on/off
+ void SetSenseMyOrien(bool sense);
+
+ //! Turn sensing of ball position on/off
void SetSenseBallPos(bool sense);
// turn sensing of lines on/off
@@ -211,6 +214,9 @@
//! true, if the absolute position of the agent is sensed.
bool mSenseMyPos;
+
+ //! true, if the absolute orientation of the agent's camera in the x-y plane is sensed.
+ bool mSenseMyOrien;
//! true, if the absolute position of the ball is sensed.
bool mSenseBallPos;
Modified: trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor_c.cpp
===================================================================
--- trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor_c.cpp 2013-05-29 05:32:14 UTC (rev 353)
+++ trunk/rcssserver3d/plugin/soccer/restrictedvisionperceptor/restrictedvisionperceptor_c.cpp 2013-06-05 20:03:19 UTC (rev 354)
@@ -80,6 +80,22 @@
return true;
}
+FUNCTION(RestrictedVisionPerceptor,setSenseMyOrien)
+{
+ bool inSenseMyOrien;
+
+ if (
+ (in.GetSize() != 1) ||
+ (! in.GetValue(in.begin(),inSenseMyOrien))
+ )
+ {
+ return false;
+ }
+
+ obj->SetSenseMyOrien(inSenseMyOrien);
+ return true;
+}
+
FUNCTION(RestrictedVisionPerceptor,setSenseBallPos)
{
bool inSenseBallPos;
@@ -188,6 +204,7 @@
DEFINE_FUNCTION(setNoiseParams);
DEFINE_FUNCTION(addNoise);
DEFINE_FUNCTION(setSenseMyPos);
+ DEFINE_FUNCTION(setSenseMyOrien);
DEFINE_FUNCTION(setSenseBallPos);
DEFINE_FUNCTION(setStaticSenseAxis);
DEFINE_FUNCTION(setViewCones);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
