void MindTarget::createService() {
sensorArea = new SensorArea( this );
effectorArea = new EffectorArea( this );
// call target with sensors and effectors
sensorArea -> createSensorArea();
effectorArea -> createEffectorArea();
// call final
createTarget( sensorArea , effectorArea );
}
void Slang::init(const std::string &Triple, const std::string &CPU,
const std::vector<std::string> &Features) {
if (mInitialized)
return;
createDiagnostic();
llvm::install_fatal_error_handler(LLVMErrorHandler, mDiagnostics.getPtr());
createTarget(Triple, CPU, Features);
createFileManager();
createSourceManager();
mInitialized = true;
return;
}
void Slang::init(const std::string &Triple, const std::string &CPU,
const std::vector<std::string> &Features,
clang::DiagnosticsEngine *DiagEngine,
DiagnosticBuffer *DiagClient) {
if (mInitialized)
return;
mDiagEngine = DiagEngine;
mDiagClient = DiagClient;
mDiag.reset(new clang::Diagnostic(mDiagEngine));
initDiagnostic();
llvm::install_fatal_error_handler(LLVMErrorHandler, mDiagEngine);
createTarget(Triple, CPU, Features);
createFileManager();
createSourceManager();
mInitialized = true;
}
/*ARGSUSED*/
static int
createFunc(int operandLen, char *operand[], int object, cmdOptions_t *options,
void *addArgs)
{
int ret;
switch (object) {
case TARGET:
ret = createTarget(operandLen, operand, options);
break;
case INITIATOR:
ret = createInitiator(operandLen, operand, options);
break;
case TPGT:
ret = createTpgt(operandLen, operand, options);
break;
default:
(void) fprintf(stderr, "%s: %s\n",
cmdName, gettext("unknown object"));
ret = 1;
break;
}
return (ret);
}
void HelloWorld::createTargets() {
createTarget("brick_2.png", CCPointMake(675.0, FLOOR_HEIGHT), 0.0f, false, false, false);
createTarget("brick_1.png", CCPointMake(741.0, FLOOR_HEIGHT), 0.0f, false, false, false);
createTarget("brick_1.png", CCPointMake(741.0, FLOOR_HEIGHT+23.0), 0.0f, false, false, false);
createTarget("brick_3.png", CCPointMake(673.0, FLOOR_HEIGHT+46.0), 0.0f, false, false, false);
createTarget("brick_1.png", CCPointMake(707.0, FLOOR_HEIGHT+58.0), 0.0f, false, false, false);
createTarget("brick_1.png", CCPointMake(707.0, FLOOR_HEIGHT+81.0), 0.0f, false, false, false);
createTarget("head_dog.png", CCPointMake(702.0, FLOOR_HEIGHT), 0.0f, true, false, true);
createTarget("head_cat.png", CCPointMake(680.0, FLOOR_HEIGHT+58.0), 0.0f, true, false, true);
createTarget("head_dog.png", CCPointMake(740.0, FLOOR_HEIGHT+58.0), 0.0f, true, false, true);
// 2 bricks at the right of the first block
createTarget("brick_2.png", CCPointMake(770.0, FLOOR_HEIGHT), 0.0f, false, false, false);
createTarget("brick_2.png", CCPointMake(770.0, FLOOR_HEIGHT+46.0), 0.0f, false, false, false);
// The dog between the blocks
createTarget("head_dog.png", CCPointMake(830.0, FLOOR_HEIGHT), 0.0f, true, false, true);
// Second block
createTarget("brick_platform.png", CCPointMake(839.0, FLOOR_HEIGHT), 0.0f, false, true, false);
createTarget("brick_2.png", CCPointMake(854.0, FLOOR_HEIGHT+28.0), 0.0f, false, false, false);
createTarget("brick_2.png", CCPointMake(854.0, FLOOR_HEIGHT+28.0+46.0), 0.0f, false, false, false);
createTarget("head_cat.png", CCPointMake(881.0, FLOOR_HEIGHT+28.0), 0.0f, true, false, true);
createTarget("brick_2.png", CCPointMake(909.0, FLOOR_HEIGHT+28.0), 0.0f, false, false, false);
createTarget("brick_1.png", CCPointMake(909.0, FLOOR_HEIGHT+28.0+46.0), 0.0f, false, false, false);
createTarget("brick_1.png", CCPointMake(909.0, FLOOR_HEIGHT+28.0+46.0+23.0), 0.0f, false, false, false);
createTarget("brick_2.png", CCPointMake(882.0, FLOOR_HEIGHT+108.0), 90.0f, false, false, false);
}
updated = false;
return *this;
}
};
static vector<cv::Point3f> createTarget ()
{
vector<cv::Point3f> ret;
ret.push_back (cv::Point3f (-12, -9, 0));
ret.push_back (cv::Point3f (+12, -9, 0));
ret.push_back (cv::Point3f (+12, +9, 0));
ret.push_back (cv::Point3f (-12, +9, 0));
return ret;
}
static vector<cv::Point3f> targetPoints = createTarget ();
class Processor
{
Mat input, binary;
Mat red, green, blue;
Mat imask, omask, temp;
Mat greenOverBlue, greenOverRed, brightGreen;
int index;
time_t last;
void initMatrices ()
{
#define MATINIT(x) x.create (input.rows, input.cols, CV_8UC1)
MATINIT (red);
MATINIT (green);
void relativeSection(ImageWriter& iw)
{
iw.setLabel("relative");
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
WorldModel2D wm;
// wm.addEntity(createBox(geo::Vec2(), geo::Vec2()));
// wm.addEntity(createBox(7.63, 4.09, true), fromXYADegrees(0.085, 0.065, 0));
wm.addEntity(createBox(geo::Vec2(0, -4.09), geo::Vec2(7.63, 0), true), fromXYADegrees(-3.75, 2.1, 0));
int idx_couch = wm.entities.size();
wm.addEntity(createBox(0.691667, 1.45833), fromXYADegrees(-0.529166, 0.404165, 0));
wm.addEntity(createBox(0.45833, 0.6), fromXYADegrees(-1.69583, 0.375, 0));
wm.addEntity(createBox(0.95833, 0.516667), fromXYADegrees(-1.72917, -0.616667, 0));
wm.addEntity(createBox(0.96666, 0.50833), fromXYADegrees(-1.75, 1.37084, 0));
wm.addEntity(createBox(1.6, 0.30834), fromXYADegrees(-0.45, -1.8375, 0));
int idx_table = wm.entities.size();
wm.addEntity(createBox(0.65, 0.95), fromXYADegrees(1.6, 0.4, 0));
int idx_cabinet = wm.entities.size();
wm.addEntity(createBox(0.55, 1.28333), fromXYADegrees(3.54167, 0.558333, 0));
wm.addEntity(createCircle(0.1), fromXYA(0.358333, 1.81667, 0));
wm.addEntity(createCircle(0.1), fromXYA(-1.73333, -1.78333, 0));
int idx_plant = wm.entities.size();
wm.addEntity(createCircle(0.1), fromXYA(3.18333, 1.68333, 0));
Model2D model;
Contour2D& c = model.addContour();
c.addPoint(-3.73333 + 3.3, -1 + 1.6);
c.addPoint(-3.15 + 3.3, -1 + 1.6);
c.addPoint(-3.15 + 3.3, -1.46 + 1.6);
c.addPoint(-2.36 + 3.3, -1.46 + 1.6);
c.addPoint(-2.36 + 3.3, -1.97 + 1.6);
c.addPoint(-3.73333 + 3.3, -1.97 + 1.6);
wm.addEntity(model, fromXYADegrees(-3.3, -1.6, 0));
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Canvas canvas = iw.nextCanvas();
drawImage(canvas, iw.image_path() + "/livingroom.jpg", 0.9);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
drawWorld(canvas, wm);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
int idx_lrf = wm.entities.size();
wm.addEntity(createLRFPose(), fromXYADegrees(0.533333, -1.24167, 145), Color(0, 150, 0, 2));
drawWorld(canvas, wm);
iw.process(canvas);
int idx_target = wm.entities.size();
wm.addEntity(createTarget(), fromXYADegrees(-0.533333, -0.725, 0), Color(255, 0, 0, 2));
drawWorld(canvas, wm);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Moved couch
canvas = iw.nextCanvas();
drawImage(canvas, iw.image_path() + "/livingroom2.jpg", 0.9);
drawWorld(canvas, wm);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// canvas = iw.nextCanvas();
drawWorldModelAbsolute(canvas, wm, wm.entities[0].pose.t);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
canvas = iw.nextCanvas();
drawImage(canvas, iw.image_path() + "/livingroom2.jpg", 0.9);
drawWorld(canvas, wm);
iw.process(canvas);
std::vector<Link> links;
links.push_back(Link(0, idx_target));
links.push_back(Link(0, idx_lrf));
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
canvas = iw.nextCanvas();
drawImage(canvas, iw.image_path() + "/livingroom2.jpg", 0.9);
drawWorld(canvas, wm);
iw.process(canvas);
links.clear();
links.push_back(Link(idx_couch, idx_target));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
links.push_back(Link(idx_lrf, idx_couch));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
WorldModel2D wm2;
wm2.entities.push_back(wm.entities[idx_lrf]);
wm2.entities.push_back(wm.entities[idx_couch]);
wm2.entities.push_back(wm.entities[idx_target]);
links.clear();
links.push_back(Link(0, 1));
links.push_back(Link(1, 2));
canvas = iw.nextCanvas();
drawWorldModelSceneGraph(canvas, wm2, links);
iw.process(canvas);
canvas = iw.nextCanvas();
drawImage(canvas, iw.image_path() + "/livingroom2.jpg", 0.9);
wm.entities[idx_target].pose.t.y += 0.7;
wm.entities[idx_couch].pose.t.y += 0.7;
links.clear();
links.push_back(Link(idx_couch, idx_target));
links.push_back(Link(idx_lrf, idx_couch));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
links.push_back(Link(idx_couch, 2));
links.push_back(Link(2, 3));
links.push_back(Link(2, 4));
links.push_back(Link(idx_lrf, idx_table));
links.push_back(Link(idx_table, idx_cabinet));
links.push_back(Link(idx_cabinet, idx_plant));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Soccer field
iw.setLabel("relative-soccer");
//Click: [ -2.0125 -0.0125 ]
//Click: [ 0.8375 -1.1 ]
//Click: [ 2.075 0.025 ]
//Click: [ 2 1.7125 ] ball
wm.entities.clear();
wm.addEntity(createSoccerFieldModel(), geo::Transform2::identity(), Color(255, 255, 255, 2));
wm.addEntity(createCircle(0.2), fromXYA(2, 1.7125, 0), Color(255, 220, 0, 2));
wm.addEntity(createTurtleModel(), fromXYADegrees(0.8375, -1.1 , 60), Color(0, 0, 0, 2));
// wm.addEntity(createTurtleModel(), fromXYADegrees(-1.1, -0.15, -90), Color(0, 0, 0, 2));
// wm.addEntity(createTurtleModel(), fromXYADegrees(2.275, 0.025, 105), Color(0, 0, 0, 2));
canvas = iw.nextCanvas();
drawSoccerField(canvas);
drawWorld(canvas, wm);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
drawWorldModelAbsolute(canvas, wm, geo::Vec2(-4, 2.67));
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
links.clear();
links.push_back(Link(2, 1));
canvas = iw.nextCanvas();
drawSoccerField(canvas);
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
wm.addEntity(createTurtleModel(), fromXYADegrees(-1.5, -0.15, -10), Color(0, 0, 0, 2));
canvas = iw.nextCanvas();
drawSoccerField(canvas);
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
links.push_back(Link(3, 2));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
drawArrow(canvas, wm.entities[3].pose.t, wm.entities[1].pose.t, Color(150, 150, 150, 2), true);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
canvas = iw.nextCanvas();
drawSoccerField(canvas);
geo::Vec2 ball_pos_rel = wm.entities[2].pose.inverse() * wm.entities[1].pose.t;
geo::Transform2 offset = geo::Transform2::identity();
offset.setRotation(-0.3);
wm.entities[2].pose = wm.entities[2].pose * offset;
wm.entities[1].pose.t = wm.entities[2].pose * ball_pos_rel;
drawWorldModelSceneGraph(canvas, wm, links);
drawArrow(canvas, wm.entities[3].pose.t, wm.entities[1].pose.t, Color(150, 150, 150, 2), true);
iw.process(canvas);
// Turn back
wm.entities[2].pose = wm.entities[2].pose * offset.inverse();
wm.entities[1].pose.t = wm.entities[2].pose * ball_pos_rel;
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
canvas = iw.nextCanvas();
drawSoccerField(canvas);
drawWorld(canvas, wm);
iw.process(canvas);
// wm.addEntity(createTurtleModel(), fromXYADegrees(2.275, 0.025, 105), Color(0, 0, 0, 2));
wm.addEntity(createTurtleModel(), fromXYADegrees(0.375, 0.65, -105), Color(0, 0, 0, 2));
drawWorld(canvas, wm);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
links.clear();
links.push_back(Link(2, 1));
links.push_back(Link(2, 4));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
// links.push_back(Link(4, 1));
// drawWorldModelSceneGraph(canvas, wm, links);
// iw.process(canvas);
links.push_back(Link(3, 2));
links.push_back(Link(3, 4));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
cv::Mat temp = canvas.image.clone();
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
canvas = iw.nextCanvas();
canvas.image = temp.clone();
drawTriangle(canvas, wm.entities[1].pose.t, wm.entities[2].pose.t, wm.entities[4].pose.t, Color(0, 255, 255, 2));
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Replace 3th turtle by field feature
wm.entities[4] = Entity2D(Model2D(), fromXYA(0, 0.7, 0), Color());
canvas = iw.nextCanvas();
drawSoccerField(canvas);
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
drawTriangle(canvas, wm.entities[1].pose.t, wm.entities[2].pose.t, wm.entities[4].pose.t, Color(0, 255, 255, 2));
iw.process(canvas);
}
int main( int argc,
char * argv[] )
{
osg::ref_ptr< osg::Group > root = new osg::Group();
btDynamicsWorld * dynamicsWorld = initPhysics();
// Create target
createTarget( root.get(), dynamicsWorld );
// Make the ground plane
osg::MatrixTransform * ground = createOSGBox( osg::Vec3( 10, 10, .01 ) );
root->addChild( ground );
btRigidBody * groundBody = createBTBox( ground, osg::Vec3( 0, 0, -10 ) );
dynamicsWorld->addRigidBody( groundBody );
// Make animated box.
osg::MatrixTransform * box = createOSGBox( osg::Vec3( .5, .5, .5 ) );
osg::AnimationPathCallback * apc = new osg::AnimationPathCallback(
createAnimationPath( osg::Vec3( -6, -10, -9 ),
osg::Vec3( 15.5, 18, 0 ), 5 ),
0, 1 );
box->setUpdateCallback( apc );
root->addChild( box );
// Animated box, bullet code
btRigidBody * boxBody = createBTBox( box, osg::Vec3( -9, -3, -9 ) );
boxBody->setCollisionFlags( boxBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT );
boxBody->setActivationState( DISABLE_DEACTIVATION );
dynamicsWorld->addRigidBody( boxBody );
// Animated box, osgBullet Code
osgbCollision::RefBulletObject< btRigidBody >* boxRigid =
new osgbCollision::RefBulletObject< btRigidBody >( boxBody );
box->setUserData( boxRigid );
osgbDynamics::RigidBodyAnimation * rba = new osgbDynamics::RigidBodyAnimation;
apc->setNestedCallback( rba );
osgViewer::Viewer viewer;
viewer.setUpViewInWindow( 10, 30, 800, 600 );
osgGA::TrackballManipulator * tb = new osgGA::TrackballManipulator();
tb->setHomePosition( osg::Vec3( 20, -24, 3 ),
osg::Vec3( 2, 0, -10 ),
osg::Vec3( 0, 0, 1 ) );
viewer.setCameraManipulator( tb );
viewer.setSceneData( root.get() );
double currSimTime;
double prevSimTime = viewer.getFrameStamp()->getSimulationTime();
viewer.realize();
while( !viewer.done() )
{
currSimTime = viewer.getFrameStamp()->getSimulationTime();
dynamicsWorld->stepSimulation( currSimTime - prevSimTime );
prevSimTime = currSimTime;
viewer.frame();
}
return( 0 );
}
