Boolean ListBoxItem :: targetDrop ( IDMTargetDropEvent &event )
{
IListBox
*tgtLB = (IListBox*)( event.window() );
// Turn off target emphasis:
ListBoxItemProvider
*provider = (ListBoxItemProvider*)( tgtLB->itemProvider() );
provider -> drawEmphasis( tgtLB, TgtLocation( after, nil ) );
// Calculate where dropped on:
TgtLocation
dropLoc = targetLocation( tgtLB, event.dropPosition() );
// Add or replace, based on drop location:
switch ( dropLoc.type )
{
case before:
tgtLB -> add( dropLoc.index, contents() );
break;
case on:
tgtLB -> setItemText( dropLoc.index, contents() );
break;
case after:
tgtLB -> add( dropLoc.index + 1, contents() );
break;
}
// If source and target are the same and item moved
// forward, update source index.
IDMTargetOperation::Handle
tgtOp = IDMTargetOperation::targetOperation();
if ( tgtOp->sourceWindow() == event.window()
&&
tgtOp->operation() == IDMOperation::move )
{
IDMItem::Handle
srcItem = IDMItem::sourceItemFor( tgtOp->item( 1 ) );
unsigned
srcIndex = (unsigned)( srcItem->object() );
if ( dropLoc.type != on
&&
dropLoc.index < srcIndex )
srcItem->setObject( (void*)( srcIndex + 1 ) );
}
return true;
}
void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
{
{
float playstartf[3] = {0,0,100};
if (bspLoader.findVectorByName(&playstartf[0],"info_player_start"))
{
printf("found playerstart\n");
}
else
{
if (bspLoader.findVectorByName(&playstartf[0],"info_player_deathmatch"))
{
printf("found deatchmatch start\n");
}
}
btVector3 playerStart (playstartf[0],playstartf[1],playstartf[2]);
playerStart[2] += 20.f; //start a bit higher
playerStart *= scaling;
//progressBegin("Loading bsp");
for (int i=0;i<bspLoader.m_numleafs;i++)
{
printf("Reading bspLeaf %i from total %i (%f procent)\n",i, bspLoader.m_numleafs,(100.f*(float)i/float(bspLoader.m_numleafs)) );
bool isValidBrush = false;
BSPLeaf& leaf = bspLoader.m_dleafs[i];
for (int b=0;b<leaf.numLeafBrushes;b++)
{
btAlignedObjectArray<btVector3> planeEquations;
int brushid = bspLoader.m_dleafbrushes[leaf.firstLeafBrush+b];
BSPBrush& brush = bspLoader.m_dbrushes[brushid];
if (brush.shaderNum!=-1)
{
if (bspLoader.m_dshaders[ brush.shaderNum ].contentFlags & BSPCONTENTS_SOLID)
{
brush.shaderNum = -1;
for (int p=0;p<brush.numSides;p++)
{
int sideid = brush.firstSide+p;
BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid];
int planeid = brushside.planeNum;
BSPPlane& plane = bspLoader.m_dplanes[planeid];
btVector3 planeEq;
planeEq.setValue(
plane.normal[0],
plane.normal[1],
plane.normal[2],
scaling*-plane.dist);
planeEquations.push_back(planeEq);
isValidBrush=true;
}
if (isValidBrush)
{
btAlignedObjectArray<btVector3> vertices;
btGeometryUtil::getVerticesFromPlaneEquations(planeEquations,vertices);
bool isEntity = false;
btVector3 entityTarget(0.f,0.f,0.f);
addConvexVerticesCollider(vertices,isEntity,entityTarget);
}
}
}
}
}
#define USE_ENTITIES
#ifdef USE_ENTITIES
{
int i;
for (i=0;i<bspLoader.m_num_entities;i++)
{
const BSPEntity& entity = bspLoader.m_entities[i];
const char* cl = bspLoader.getValueForKey(&entity,"classname");
if ( !strcmp( cl, "trigger_push" ) ) {
btVector3 targetLocation(0.f,0.f,0.f);
cl = bspLoader.getValueForKey(&entity,"target");
if ( strcmp( cl, "" ) ) {
//its not empty so ...
/*
//lookup the target position for the jumppad:
const BSPEntity* targetentity = bspLoader.getEntityByValue( "targetname" , cl );
if (targetentity)
{
if (bspLoader.getVectorForKey( targetentity , "origin",&targetLocation[0]))
{
}
}
*/
cl = bspLoader.getValueForKey(&entity,"model");
if ( strcmp( cl, "" ) ) {
// add the model as a brush
if (cl[0] == '*')
{
int modelnr = atoi(&cl[1]);
if ((modelnr >=0) && (modelnr < bspLoader.m_nummodels))
{
const BSPModel& model = bspLoader.m_dmodels[modelnr];
for (int n=0;n<model.numBrushes;n++)
{
btAlignedObjectArray<btVector3> planeEquations;
bool isValidBrush = false;
//convert brush
const BSPBrush& brush = bspLoader.m_dbrushes[model.firstBrush+n];
{
for (int p=0;p<brush.numSides;p++)
{
int sideid = brush.firstSide+p;
BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid];
int planeid = brushside.planeNum;
BSPPlane& plane = bspLoader.m_dplanes[planeid];
btVector3 planeEq;
planeEq.setValue(
plane.normal[0],
plane.normal[1],
plane.normal[2],
scaling*-plane.dist);
planeEquations.push_back(planeEq);
isValidBrush=true;
}
if (isValidBrush)
{
btAlignedObjectArray<btVector3> vertices;
btGeometryUtil::getVerticesFromPlaneEquations(planeEquations,vertices);
bool isEntity=true;
addConvexVerticesCollider(vertices,isEntity,targetLocation);
}
}
}
}
}
else
{
printf("unsupported trigger_push model, md3 ?\n");
}
}
}
}
}
}
#endif //USE_ENTITIES
//progressEnd();
}
}
void stewartGoughNodeNamespace::StewartGoughNode::onSetInstruction(const rexos_module::SetInstructionGoalConstPtr &goal){
REXOS_INFO_STREAM("parsing hardwareStep: " << goal->json);
Json::Reader reader;
Json::Value equipletStepNode;
reader.parse(goal->json, equipletStepNode);
rexos_datatypes::EquipletStep equipletStep(equipletStepNode);
rexos_module::SetInstructionResult result;
result.OID = goal->OID;
rexos_stewart_gough::StewartGoughLocation origin;
// the rotation of the axis of the tangent space against the normal space in radians
double rotationX, rotationY, rotationZ = 0;
// determine the position of the origin and the rotation of the axis
switch(equipletStep.getOriginPlacement().getOriginPlacementType()) {
case rexos_datatypes::OriginPlacement::RELATIVE_TO_IDENTIFIER: {
// set the origin to the result of the lookup
if(equipletStep.getOriginPlacement().getLookupResult().isMember("location") == false) {
throw std::runtime_error("lookup result does not contain location");
} else if(equipletStep.getOriginPlacement().getLookupResult().isMember("rotation") == false) {
throw std::runtime_error("lookup result does not contain rotation");
}
Json::Value location = equipletStep.getOriginPlacement().getLookupResult()["location"];
origin.location.set(location["x"].asDouble(), location["y"].asDouble(), location["z"].asDouble());
origin.location = convertToModuleCoordinate(origin.location);
Json::Value rotation = equipletStep.getOriginPlacement().getLookupResult()["rotation"];
rotationZ = rotation["z"].asDouble();
break;
}
case rexos_datatypes::OriginPlacement::RELATIVE_TO_CURRENT_POSITION: {
// set the origin to the current position of the effector
origin.location.set(stewartGough->getEffectorLocation().location.x, stewartGough->getEffectorLocation().location.y,
stewartGough->getEffectorLocation().location.z);
origin.rotationX = stewartGough->getEffectorLocation().rotationX;
origin.rotationY = stewartGough->getEffectorLocation().rotationY;
origin.rotationZ = stewartGough->getEffectorLocation().rotationZ;
break;
}
case rexos_datatypes::OriginPlacement::RELATIVE_TO_MODULE_ORIGIN: {
// set the origin to the origin of the module (eg set it to 0, 0, 0)
origin.location.set(0, 0, 0);
origin.rotationX = 0;
origin.rotationY = 0;
origin.rotationZ = 0;
break;
}
case rexos_datatypes::OriginPlacement::RELATIVE_TO_EQUIPLET_ORIGIN: {
// set the origin to the origin of the module (eg set it to 0, 0, 0)
origin.location = convertToModuleCoordinate(Vector3(0, 0, 0));
origin.rotationX = 0;
origin.rotationY = 0;
origin.rotationZ = 0;
break;
}
}
// get the vector from the instruction data
Json::Value instructionData = equipletStep.getInstructionData();
if(instructionData.isMember("move") == false) {
throw std::runtime_error("instruction data does not contain move");
}
Json::Value moveCommand = equipletStep.getInstructionData()["move"];
Vector3 offsetVector;
if(moveCommand.isMember("x")) offsetVector.x = moveCommand["x"].asDouble();
else offsetVector.x = stewartGough->getEffectorLocation().location.x;
if(moveCommand.isMember("y")) offsetVector.y = moveCommand["y"].asDouble();
else offsetVector.y = stewartGough->getEffectorLocation().location.y;
if(moveCommand.isMember("z")) offsetVector.z = moveCommand["z"].asDouble();
else offsetVector.z = stewartGough->getEffectorLocation().location.z;
// get the max acceleration
double maxAcceleration;
if(moveCommand.isMember("maxAcceleration") == false) {
REXOS_WARN("move command does not contain maxAcceleration, assuming ");
maxAcceleration = 50.0;
} else {
maxAcceleration = moveCommand["maxAcceleration"].asDouble();
}
// get the rotation from the instruction data
if(instructionData.isMember("rotate") == false) {
throw std::runtime_error("instruction data does not contain rotate");
}
Json::Value rotateCommand = equipletStep.getInstructionData()["rotate"];
double targetRotationX, targetRotationY, targetRotationZ;
if(rotateCommand.isMember("x")) targetRotationX = rotateCommand["x"].asDouble();
else targetRotationX = stewartGough->getEffectorLocation().rotationX;
if(rotateCommand.isMember("y")) targetRotationY = rotateCommand["y"].asDouble();
else targetRotationY = stewartGough->getEffectorLocation().rotationY;
if(rotateCommand.isMember("z")) targetRotationZ = rotateCommand["z"].asDouble();
else targetRotationZ = stewartGough->getEffectorLocation().rotationZ;
// calculate the target vector
Matrix4 rotationMatrix;
rotationMatrix.rotateX(rotationX);
rotationMatrix.rotateY(rotationY);
rotationMatrix.rotateZ(rotationZ);
Vector3 targetVector = origin.location + rotationMatrix * offsetVector;
targetRotationX += origin.rotationX;
targetRotationY += origin.rotationY;
targetRotationZ += origin.rotationZ;
rexos_stewart_gough::StewartGoughLocation targetLocation(targetVector.x, targetVector.y, targetVector.z,
targetRotationX, targetRotationY, targetRotationZ);
REXOS_INFO_STREAM("moving from " << stewartGough->getEffectorLocation() << " to " << targetLocation);
// finally move to point
if(moveToPoint(targetLocation, maxAcceleration)) {
setInstructionActionServer.setSucceeded(result);
} else {
REXOS_WARN("Failed moving to point");
setInstructionActionServer.setAborted(result);
}
}
void WallLevel::CheckValidWord()
{
if(selectedTarget == null || selectedBrick == null)
{
return;
}
string checkWord = selectedTarget->GetPrefix();
checkWord += selectedBrick->GetSuffix();
StateVariable points = mgr->GetStateManager()->GetVar(VAR_POINTS);
if(wordBuilder->IsValidWord(checkWord.c_str()))
{
mgr->GetResourceManager()->PlayAudio(SOUND_DING);
points.intValue++;
}
else
{
mgr->GetResourceManager()->PlayAudio(SOUND_BUZZ);
points.intValue -= 2;
if(points.intValue < 0)
{
points.intValue = 0;
}
}
mgr->GetStateManager()->SetVar(VAR_POINTS, points);
CIwFVec2 curLocation((float)selectedBrick->GetBaseLocation().x, (float)selectedBrick->GetBaseLocation().y);
CIwFVec2 targetLocation((float)selectedTarget->GetBaseLocation().x, (float)selectedTarget->GetBaseLocation().y);
float orientation = IW_ANGLE_TO_RADIANS(0);
CIwFVec2 normal;
normal.x = (float)cos(orientation);
normal.y = (float)sin(orientation);
CIwFVec2 cp1 = curLocation + normal * 120;
CIwFVec2 cp2 = (targetLocation - curLocation) / 2;
cp2 += curLocation + cp2 + normal * cp2.GetLength();
PathFollowingSprite *path = new PathFollowingSprite(mgr);
path->SetTotalPathTimeMillis(2000);
path->AddSegment(new PathSegmentBezier(Convert(curLocation), Convert(cp1), Convert(cp2), Convert(targetLocation)));
path->SetCompletionCallback(selectedBrick, (EventCallbackPtr)&Brick::PlaceBrick, path);
path->SetLoopCount(1);
mgr->RegisterGameObject(path);
selectedBrick->SetParent(path);
selectedBrick->target = selectedTarget;
selectedTarget->Hide();
selectedBrick->staticBrick = true;
selectedTarget->staticTarget = true;
bricks[selectedBrick->xPos] = null;
s3eDebugTracePrintf("%d - Pathing brick %s", mgr->GetCurrentClockTime(), selectedBrick->debugId);
selectedBrick = null;
selectedTarget = null;
}
int main(int argc,char* argv[]) {
ros::init(argc, argv, "planner");
ros::NodeHandle nh; // nodeHandle
ros::Publisher pub_path = nh.advertise<global_planner::Seed>("/path", 1000); //Publisher for Path
ros::Subscriber sub_world_map = nh.subscribe("/world_map",10, update_world_map); //Subscriber for World Map
ros::Subscriber sub_bot_pose = nh.subscribe("/bot_pose", 10 ,update_bot_pose); // topic should same with data published by GPS
ros::Subscriber sub_target_pose = nh.subscribe("/target_Pose", 10 , update_target_pose); // topic published from GPS
// ros::Subscriber sub3 = nh.subscribe("/pose", 1, update_pose);
/* TO DO
Custom Message for array of pose3D (check ROS tut).*/
// pub_path = nh.advertise<geometry_msgs::Twist > ("cmd_vel", 1);
cvNamedWindow("[PLANNER] Map", 0);
navigation::State botLocation(rand()%100,rand()%100,90,0),targetLocation(900,900,90,0);
navigation::AStarSeed planner;
ros::Rate loop_rate(LOOP_RATE);
srand((unsigned int)time(NULL));
int iterations = 100;
while (iterations-- && ros::ok()) {
global_planner::Seed seed;
cv::Mat img = cv::Mat::zeros(1000, 1000, CV_8UC1);
// std::chrono::steady_clock::time_point startC=std::chrono::steady_clock::now();
navigation::addObstacles(img, 5);
struct timeval t,c;
gettimeofday(&t,NULL);
std::pair<std::vector<navigation::StateOfCar>, navigation::Seed> path = planner.findPathToTargetWithAstar(img,botLocation, targetLocation);
// planner.showPath(path.first);
seed.x = path.second.finalState.x();
seed.y = path.second.finalState.y();
seed.theta = path.second.finalState.theta();
seed.costOfseed = path.second.costOfseed;
seed.velocityRatio = path.second.velocityRatio;
seed.leftVelocity = path.second.leftVelocity;
seed.rightVelocity = path.second.rightVelocity;
seed.curvature = path.second.finalState.curvature();
pub_path.publish(seed);
gettimeofday(&c,NULL);
double td = t.tv_sec + t.tv_usec/1000000.0;
double cd = c.tv_sec + c.tv_usec/1000000.0; // time in seconds for thousand iterations
std::cout<<"FPS:"<< 1/(cd-td) <<std::endl;
ros::spinOnce();
loop_rate.sleep();
}
ROS_INFO("Planner Exited");
return NULL;
}
