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);
}
