void createTask() { task.reset(new controlit::task_library::OrientVectorToPlaneTask()); // Get parameter goalVector controlit::Parameter * p = task->lookupParameter("normalVector"); EXPECT_TRUE(p) << "Unable to get parameter \"normalVector\"."; // For an OrientVectorToVectorTask, the goalVector parameter has 3 dimensions: x, y, z. // Set parameter goalVector to be (1, 1, 0) Vector goalPos(3); goalPos << 0, 0, 1; s = p->set(goalPos); EXPECT_TRUE(s) << s.what(); // Get parameter bodyFrameVector p = task->lookupParameter("bodyFrameVector"); EXPECT_TRUE(p) << "Unable to get parameter \"bodyFrameVector\"."; // For a OrientVectorToVectorTask, the bodyFrameVector parameter has 3 dimensions: x, y, z. // Set parameter bodyFrameVector to be (1, 0, 0) Vector bodyFrameVector(3); bodyFrameVector << 1, 0, 0; s = p->set(bodyFrameVector); EXPECT_TRUE(s) << s.what(); // Get parameter kp p = task->lookupParameter("kp"); EXPECT_TRUE(p) << "Unable to get parameter \"kp\"."; // Set parameter kp to be 1 double kp = 1; s = p->set(kp); EXPECT_TRUE(s) << s.what(); // Get parameter kd p = task->lookupParameter("kd"); EXPECT_TRUE(p) << "Unable to get parameter \"kd\"."; // Set parameter kd = 1 double kd = 1; s = p->set(kd); EXPECT_TRUE(s) << s.what(); // Get parameter maxVelocity p = task->lookupParameter("maxVelocity"); EXPECT_TRUE(p) << "Unable to get parameter \"maxVelocity\"."; // Set parameter maxVelocity to be 0 double maxVelocity = 0; s = p->set(maxVelocity); EXPECT_TRUE(s) << s.what(); // Get parameter bodyName p = task->lookupParameter("bodyName"); EXPECT_TRUE(p) << "Unable to get parameter \"bodyName\"."; // Set parameter bodyName to be revolute1DoF_2 s = p->set("revolute1DoF_2"); EXPECT_TRUE(s) << s.what(); }
void AiForce::Attack(const Vec2i &pos) { RemoveDeadUnit(); if (Units.size() == 0) { this->Attacking = false; return; } if (!this->Attacking) { // Remember the original force position so we can return there after attack if (this->Role == AiForceRoleDefend || (this->Role == AiForceRoleAttack && this->State == AiForceAttackingState_Waiting)) { this->HomePos = this->Units[this->Units.size() - 1]->tilePos; } this->Attacking = true; } Vec2i goalPos(pos); if (Map.Info.IsPointOnMap(goalPos) == false) { /* Search in entire map */ const CUnit *enemy = NULL; AiForceEnemyFinder<AIATTACK_BUILDING>(*this, &enemy); if (enemy) { goalPos = enemy->tilePos; } } this->GoalPos = goalPos; if (Map.Info.IsPointOnMap(goalPos) == false) { DebugPrint("%d: Need to plan an attack with transporter\n" _C_ AiPlayer->Player->Index); if (State == AiForceAttackingState_Waiting && !PlanAttack()) { DebugPrint("%d: Can't transport\n" _C_ AiPlayer->Player->Index); Attacking = false; } return; } // Send all units in the force to enemy. this->State = AiForceAttackingState_Attacking; for (size_t i = 0; i != this->Units.size(); ++i) { CUnit *const unit = this->Units[i]; if (unit->Container == NULL) { const int delay = i / 5; // To avoid lot of CPU consuption, send them with a small time difference. unit->Wait = delay; if (unit->Type->CanAttack) { CommandAttack(*unit, goalPos, NULL, FlushCommands); } else { CommandMove(*unit, goalPos, FlushCommands); } } } }
int chooseBestBot(std::list<int>& freeBots, const Tactic::Param* tParam) const { int minv = *(freeBots.begin()); int mindis = 10000; Point2D<int> goalPos(-(HALF_FIELD_MAXX),OUR_GOAL_MAXY); for (std::list<int>::iterator it = freeBots.begin(); it != freeBots.end(); ++it) { float dis_from_ball = (state->homePos[*it] - goalPos).absSq();//state->home_goalpoints[2] is center of our goal if(dis_from_ball < mindis) { dis_from_ball = mindis; minv = *it; } } return minv; } // chooseBestBot
qreal FlyThroughTask::calculateFlightPerformance(const QList<Position> &positions, const QPolygonF &geoPoly, const UAVParameters &) { //First, see if one of the points is within the polygon foreach(const Position& pos, positions) { if (geoPoly.containsPoint(pos.lonLat(), Qt::OddEvenFill)) return this->maxTaskPerformance(); } //if that fails, take the distance to the last point Position goalPos(geoPoly.boundingRect().center(), positions.first().altitude()); const Position& last = positions.last(); QVector3D enuPos = Conversions::lla2enu(last, goalPos); qreal dist = enuPos.length(); const qreal stdDev = 90.0; qreal toRet = 100*FlightTask::normal(dist,stdDev,2000); return qMin<qreal>(toRet,this->maxTaskPerformance()); }
void ControlCompute(void) { #if TIME_BETWEEN_ORDERS static long time_reached = -1; long now; now = timeMicros(); #endif goal_t* current_goal = FifoCurrentGoal(); RobotStateUpdate(); // clear emergency everytime, it will be reset if necessary ControlUnsetStop(EMERGENCY_BIT); ControlUnsetStop(SLOWGO_BIT); if (abs(control.speeds.linear_speed) > 1) { int direction; if (control.speeds.linear_speed > 0) { direction = EM_FORWARD; } else { direction = EM_BACKWARD; } if (emergency_status[direction].phase == FIRST_STOP) { ControlSetStop(EMERGENCY_BIT); } else if (emergency_status[direction].phase == SLOW_GO) { ControlSetStop(SLOWGO_BIT); } } if (control.status_bits & EMERGENCY_BIT || control.status_bits & PAUSE_BIT || control.status_bits & TIME_ORDER_BIT) { stopRobot(); } else { switch (current_goal->type) { case TYPE_ANG: goalAngle(current_goal); break; case TYPE_POS: goalPos(current_goal); break; case TYPE_PWM: goalPwm(current_goal); break; case TYPE_SPD: goalSpd(current_goal); break; default: stopRobot(); break; } } applyPwm(); if (current_goal->is_reached) { control.last_finished_id = current_goal->ID; FifoNextGoal(); ControlPrepareNewGoal(); #if TIME_BETWEEN_ORDERS time_reached = now; } if (time_reached > 0 && (now - time_reached) < TIME_BETWEEN_ORDERS*1000000) { ControlSetStop(TIME_ORDER_BIT); } else { ControlUnsetStop(TIME_ORDER_BIT); time_reached = -1; #endif } }
void AiForce::Attack(const Vec2i &pos) { bool isDefenceForce = false; RemoveDeadUnit(); if (Units.size() == 0) { this->Attacking = false; this->State = AiForceAttackingState_Waiting; return; } if (!this->Attacking) { // Remember the original force position so we can return there after attack if (this->Role == AiForceRoleDefend || (this->Role == AiForceRoleAttack && this->State == AiForceAttackingState_Waiting)) { this->HomePos = this->Units[this->Units.size() - 1]->tilePos; } this->Attacking = true; } Vec2i goalPos(pos); bool isNaval = false; for (size_t i = 0; i != this->Units.size(); ++i) { CUnit *const unit = this->Units[i]; if (unit->Type->UnitType == UnitTypeNaval && unit->Type->CanAttack) { isNaval = true; break; } } bool isTransporter = false; for (size_t i = 0; i != this->Units.size(); ++i) { CUnit *const unit = this->Units[i]; if (unit->Type->CanTransport() && unit->IsAgressive() == false) { isTransporter = true; break; } } if (Map.Info.IsPointOnMap(goalPos) == false) { /* Search in entire map */ const CUnit *enemy = NULL; if (isTransporter) { AiForceEnemyFinder<AIATTACK_AGRESSIVE>(*this, &enemy); } else if (isNaval) { AiForceEnemyFinder<AIATTACK_ALLMAP>(*this, &enemy); } else { AiForceEnemyFinder<AIATTACK_BUILDING>(*this, &enemy); } if (enemy) { goalPos = enemy->tilePos; } } else { isDefenceForce = true; } if (Map.Info.IsPointOnMap(goalPos) == false || isTransporter) { DebugPrint("%d: Need to plan an attack with transporter\n" _C_ AiPlayer->Player->Index); if (State == AiForceAttackingState_Waiting && !PlanAttack()) { DebugPrint("%d: Can't transport\n" _C_ AiPlayer->Player->Index); Attacking = false; } return; } if (this->State == AiForceAttackingState_Waiting && isDefenceForce == false) { Vec2i resultPos; NewRallyPoint(goalPos, &resultPos); this->GoalPos = resultPos; this->State = AiForceAttackingState_GoingToRallyPoint; } else { this->GoalPos = goalPos; this->State = AiForceAttackingState_Attacking; } // Send all units in the force to enemy. CUnit *leader = NULL; for (size_t i = 0; i != this->Units.size(); ++i) { CUnit *const unit = this->Units[i]; if (unit->IsAgressive()) { leader = unit; break; } } for (size_t i = 0; i != this->Units.size(); ++i) { CUnit *const unit = this->Units[i]; if (unit->Container == NULL) { const int delay = i / 5; // To avoid lot of CPU consuption, send them with a small time difference. unit->Wait = delay; if (unit->IsAgressive()) { CommandAttack(*unit, this->GoalPos, NULL, FlushCommands); } else { if (leader) { CommandDefend(*unit, *leader, FlushCommands); } else { CommandMove(*unit, this->GoalPos, FlushCommands); } } } } }
void HermesMoveArmActionServer::moveArm(const hermes_move_arm_action::MoveArmGoalConstPtr& goal) { // this callback function is executed each time a request (= goal message) comes in for this service server ROS_INFO("MoveArm Action Server: Received a request for arm %i.", goal->arm); if (goal->goal_position.header.frame_id.compare("/world") != 0) { ROS_ERROR("The goal position coordinates are not provided in the correct frame. The required frame is '/world' but '%s' was provided.", goal->goal_position.header.frame_id.c_str()); return; } // this command sends a feedback message to the caller, here we transfer that the task is completed 25% // hermes_move_arm_action::MoveArmFeedback feedback; // feedback.percentageDone = 25; // move_arm_action_server_.publishFeedback(feedback); // move the arm // ... //goal->goal_position.pose.position.x //(x,y,z) in meters????? //goal->goal_position.pose.orientation.w //(w,x,y,z) Quaternion in rad //goal->goal_position.header.frame_id // Reference frame tf::Quaternion quaternion; tf::quaternionMsgToTF(goal->goal_position.pose.orientation, quaternion); tf::Transform goalPos(quaternion, tf::Vector3(goal->goal_position.pose.position.x,goal->goal_position.pose.position.y,goal->goal_position.pose.position.z)); // Transform goalPos to Robot Reference frame hermes_reference_frames_service::HermesFrame::Request req_frames; hermes_reference_frames_service::HermesFrame::Response res_frames; if(goal->arm == hermes_move_arm_action::MoveArmGoal::LEFTARM) // Depends of arm req_frames.frame = hermes_reference_frames_service::HermesFrame::Request::WORLDTLEFTARM; else if(goal->arm == hermes_move_arm_action::MoveArmGoal::RIGHTARM) // Depends of arm req_frames.frame = hermes_reference_frames_service::HermesFrame::Request::WORLDTRIGHTARM; ros::service::call("reference_frames_service", req_frames, res_frames); // Transform world to base robot tf::Quaternion qua_wTr(res_frames.quaternion[0],res_frames.quaternion[1],res_frames.quaternion[2],res_frames.quaternion[3]); tf::Transform wTr(qua_wTr, tf::Vector3(res_frames.position[0],res_frames.position[1],res_frames.position[2])); tf::Transform rTobj; rTobj = wTr.inverse()*goalPos; std::cout << "rTobj: " << std::endl; for (int i=0; i<3; ++i) { std::cout << rTobj.getBasis()[i].getX() << "\t"; std::cout << rTobj.getBasis()[i].getY() << "\t"; std::cout << rTobj.getBasis()[i].getZ() <<std::endl; } std::cout << "XYZ: " << std::endl; std::cout << rTobj.getOrigin().getX() <<std::endl; std::cout << rTobj.getOrigin().getY() <<std::endl; std::cout << rTobj.getOrigin().getZ() <<std::endl; // tf::Vector3 rotX=rTobj.getBasis()[0]; // tf::Vector3 rotY=rTobj.getBasis()[1]; // tf::Vector3 rotZ=rTobj.getBasis()[2]; // // // KDL::Vector rotXkdl(rotX.getX(),rotX.getY(),rotX.getZ()); // KDL::Vector rotYkdl(rotY.getX(),rotY.getY(),rotY.getZ()); // KDL::Vector rotZkdl(rotZ.getX(),rotZ.getY(),rotZ.getZ()); // // KDL::Rotation rot(rotXkdl,rotYkdl,rotZkdl); //Get init position std::vector<float> q_init; if(goal->arm == hermes_move_arm_action::MoveArmGoal::LEFTARM) hermesinterface.get_leftJoints(q_init); else if(goal->arm == hermes_move_arm_action::MoveArmGoal::RIGHTARM) hermesinterface.get_rightJoints(q_init); // todo: call hermes_arm_kdl ikine service hermes_arm_kdl::ikine::Request req_kdl; hermes_arm_kdl::ikine::Response res_kdl; for (int i=0; i<7; i++) req_kdl.jointAngles_init[i] = q_init[i]; req_kdl.position[0] = rTobj.getOrigin().getX(); req_kdl.position[1] = rTobj.getOrigin().getY(); req_kdl.position[2] = rTobj.getOrigin().getZ(); for (int i=0; i<3; i++) { req_kdl.rotation[3*i] = rTobj.getBasis()[i].getX(); req_kdl.rotation[3*i+1] = rTobj.getBasis()[i].getY(); req_kdl.rotation[3*i+2] = rTobj.getBasis()[i].getZ(); } ros::service::call("arm_kdl_service_ikine_server", req_kdl, res_kdl); // Move the arm with res_kdl std::vector<float> jointAngles(7); for (int i=0; i<7; i++) jointAngles[i] = res_kdl.jointAngles[i]; std::cout << "jointAngles:" << std::endl; for (int i=0; i<7; i++) std::cout << jointAngles[i] << std::endl; //ARMS DON'T MOVE if(goal->arm == hermes_move_arm_action::MoveArmGoal::LEFTARM) hermesinterface.moveLeftArm(jointAngles); else if(goal->arm == hermes_move_arm_action::MoveArmGoal::RIGHTARM) hermesinterface.moveRightArm(jointAngles); hermes_move_arm_action::MoveArmResult res; res.return_value.val = arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS; // put in there some error code on errors // this sends the response back to the caller move_arm_action_server_.setSucceeded(res); // if failed, use: move_arm_action_server_.setAborted(res); }