bool
FootstepNavigation::setGoal(const geometry_msgs::PoseStampedConstPtr goal_pose)
{
return setGoal(goal_pose->pose.position.x,
goal_pose->pose.position.y,
tf::getYaw(goal_pose->pose.orientation));
}
void Enemy::think()
{
GameObject* bestEnemy = nullptr;
Math::Vec2 newGoal(0);
if(g_circleCollision(m_map->getPlayer()->getPosition(),m_map->getPlayer()->getShinyRadius(),getPosition(),getShinyRadius()))
{
newGoal += determineReaction(m_map->getPlayer());
}
for(int i = 0;i < m_map->getNumberOfObjects(); i++)
{
Enemy* currentEnemy = m_map->getEnemy(i);
if(this!=currentEnemy)
{
if(g_circleCollision(currentEnemy->getPosition(),currentEnemy->getShinyRadius(),getPosition(),getShinyRadius()))
{
newGoal += determineReaction(currentEnemy);
}
}
}
if(newGoal.length()==0){
newGoal = Math::Vec2(g_rand->Uniform(-0.1f,0.1f),g_rand->Uniform(-0.1f,0.1f));
}
setGoal(getPosition()+newGoal);
}
void Character::update(float delta)
{
Sifteo::Float2 dir;
if (_dir.x == 0 && _dir.y == 0 && _isDir)
{
dir.x = 0;
dir.y = 0;
}
else if (_isDir)
{
Sifteo::Float2 prevPos = _pos;
Sifteo::Float2 velocity = (_dir).normalize() * delta * _maxSpeed;
_pos += velocity;
dir = (_dir).normalize();
if (_pos.x < 16 || _pos.x > MAP_SIZE * 128 - 16 - 32)
{
_pos.x = prevPos.x;
}
if (_pos.y < 16 - 32 || _pos.y > MAP_SIZE * 128 - 16 - 32)
{
_pos.y = prevPos.y;
}
}
else
{
if (_goalAlive == false)
{
if (static_cast<unsigned int>(gRandom.random() * 2))
{
_spriteId = 1;
return ;
}
else
setGoal(static_cast<int>(_goal.x / Sifteo::LCD_height), static_cast<int>(_goal.y / Sifteo::LCD_width), _maxSpeed / 4);
}
Sifteo::Float2 prevDir = _goal - _pos;
Sifteo::Float2 velocity = (prevDir).normalize() * delta * _speed;
_pos += velocity;
Sifteo::Float2 dir = (_goal - _pos).normalize();
if ((dir.x < 0) != (prevDir.x < 0) || (dir.y < 0) != (prevDir.y < 0))
{
_goalAlive = 0;
_pos = _goal;
_spriteId = 0;
return ;
}
}
int x = (dir.x < 0.5f && dir.x > -0.5f) ? 0 :
(dir.x > 0.5f) ? -1 : 1;
int y = (dir.y < 0.5f && dir.y > -0.5f) ? 0 :
(dir.y > 0.5f) ? -1 : 1;
int add = (dir.x > 0.5f) ? 1 :
(dir.y < -0.5f) ? 2 :
(dir.x < -0.5f) ? 3 : 0;
_spriteId = ((_spriteId + 1) % 4) + add * 4;
}
AREXPORT ArActionGoto::ArActionGoto(const char *name, ArPose goal,
double closeDist, double speed,
double speedToTurnAt, double turnAmount) :
ArAction(name, "Goes to the given goal.")
{
myDirectionToTurn = myCurTurnDir = 1;
myTurnedBack = false;
myPrinting = false;
setNextArgument(ArArg("goal", &myGoal, "ArPose to go to. (ArPose)"));
setGoal(goal);
setNextArgument(ArArg("close dist", &myCloseDist,
"Distance that is close enough to goal. (mm)"));
myCloseDist = closeDist;
setNextArgument(ArArg("speed", &mySpeed,
"Speed to travel to goal at. (mm/sec)"));
mySpeed = speed;
setNextArgument(ArArg("speed to turn at", &mySpeedToTurnAt,
"Speed to start obstacle avoiding at (mm/sec)"));
mySpeedToTurnAt = speedToTurnAt;
setNextArgument(ArArg("amount to turn", &myTurnAmount,
"Amount to turn when avoiding (deg)"));
myTurnAmount = turnAmount;
}
void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double threshold)
{
clearGoal();
auto gs(std::make_shared<GoalState>(si_));
gs->setState(goal);
gs->setThreshold(threshold);
setGoal(gs);
}
void EGraphManager<HeuristicType>::updateManager(){
setGoal();
initEGraph(true);
validateEGraph(true);
clock_t precomputeTSPcosts_t0 = clock();
precomputeTSPcosts();
precomputeTSPcostsClock += clock() - precomputeTSPcosts_t0;
}
void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double threshold)
{
clearGoal();
auto *gs = new GoalState(si_);
gs->setState(goal);
gs->setThreshold(threshold);
setGoal(GoalPtr(gs));
}
worldContainer::worldContainer(coord Location, coord Goal, double Charge, double Base, double HitPoints)
{
setLocation(Location);
setGoal(Goal);
setCharge(Charge);
setBase(Base);
setHitPoints(HitPoints);
}
void ReferenceInterpolator::setState(double pos, double vel, double acc)
{
v_ = vel;
x_ = pos;
a_ = acc;
if (!done())
setGoal(x_goal_);
}
void Squad::assist(TilePosition mGoal)
{
if (mGoal.x() == -1 || mGoal.y() == -1) return;
if (currentState != STATE_ASSIST)
{
if (!isUnderAttack() && currentState == STATE_DEFEND)
{
currentState = STATE_ASSIST;
setGoal(mGoal);
}
}
else
{
if (goal.x() == -1)
{
setGoal(mGoal);
}
}
}
AREXPORT void ArActionGotoStraight::setGoalRel(double dist,
double deltaHeading,
bool backToGoal,
bool justDistance)
{
ArPose goal;
goal.setX(dist * ArMath::cos(deltaHeading));
goal.setY(dist * ArMath::sin(deltaHeading));
goal = myRobot->getToGlobalTransform().doTransform(goal);
setGoal(goal, backToGoal, justDistance);
}
bool ReferenceGenerator::setGoal(const std::string& joint_name, double position, JointGoalInfo& info)
{
control_msgs::FollowJointTrajectoryGoal goal_msg;
goal_msg.trajectory.joint_names.push_back(joint_name);
trajectory_msgs::JointTrajectoryPoint p;
p.positions.push_back(position);
goal_msg.trajectory.points.push_back(p);
return setGoal(goal_msg, info.id, info.s_error);
}
void Squad::defend(TilePosition mGoal)
{
if (mGoal.x() == -1 || mGoal.y() == -1) return;
if (currentState != STATE_DEFEND)
{
if (currentState == STATE_ASSIST && !isUnderAttack())
{
currentState = STATE_DEFEND;
}
}
setGoal(mGoal);
}
worldContainer::worldContainer()
{
coord temp;
temp.x = 0;
temp.y = 0;
temp.z = 0;
setLocation(temp);
setGoal(temp);
setHitPoints(0);
setCharge(0);
setBase(0);
}
void LocalPlanner::setPose(const geometry_msgs::PoseStamped input)
{
pose.x = input.pose.position.x ;
pose.y = input.pose.position.y ;
pose.z = input.pose.position.z ;
setVelocity(input.header.stamp);
previous_pose_x = pose.x ;
previous_pose_z = pose.z ;
last_pose_time = input.header.stamp;
setGoal();
setLimits();
// octomapCloud.crop(octomap::point3d(min_cache.x,min_cache.y,min_cache.z),octomap::point3d(half_cache.x,half_cache.y,half_cache.z));
}
void KoulesSetup::initialize(unsigned int numKoules, const std::string& plannerName,
const std::vector<double>& stateVec)
{
const ob::StateSpacePtr& space = getStateSpace();
space->setup();
// setup start state
ob::ScopedState<> start(space);
if (stateVec.size() == space->getDimension())
space->copyFromReals(start.get(), stateVec);
else
{
// Pick koule positions evenly radially distributed, but at a linearly
// increasing distance from the center. The ship's initial position is
// at the center. Initial velocities are 0.
std::vector<double> startVec(space->getDimension(), 0.);
double r, theta = boost::math::constants::pi<double>(), delta = 2.*theta / numKoules;
startVec[0] = .5 * sideLength;
startVec[1] = .5 * sideLength;
startVec[4] = .5 * delta;
for (unsigned int i = 0; i < numKoules; ++i, theta += delta)
{
r = .1 + i * .1 / numKoules;
startVec[4 * i + 5] = .5 * sideLength + r * cos(theta);
startVec[4 * i + 6] = .5 * sideLength + r * sin(theta);
}
space->copyFromReals(start.get(), startVec);
}
setStartState(start);
// set goal
setGoal(std::make_shared<KoulesGoal>(si_));
// set propagation step size
si_->setPropagationStepSize(propagationStepSize);
// set min/max propagation steps
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
// set directed control sampler; when using the PDST planner, propagate as long as possible
bool isPDST = plannerName == "pdst";
const ompl::base::GoalPtr& goal = getGoal();
si_->setDirectedControlSamplerAllocator(
[&goal, isPDST](const ompl::control::SpaceInformation *si)
{
return KoulesDirectedControlSamplerAllocator(si, goal, isPDST);
});
// set planner
setPlanner(getConfiguredPlannerInstance(plannerName));
// set validity checker
setStateValidityChecker(std::make_shared<KoulesStateValidityChecker>(si_));
// set state propagator
setStatePropagator(std::make_shared<KoulesStatePropagator>(si_));
}
bool ReferenceGenerator::setGoal(const std::vector<std::string>& joint_names, const std::vector<double>& positions,
JointGoalInfo& info)
{
if (joint_names.empty() || joint_names.size() != positions.size())
return false;
control_msgs::FollowJointTrajectoryGoal goal_msg;
goal_msg.trajectory.joint_names = joint_names;
trajectory_msgs::JointTrajectoryPoint p;
p.positions = positions;
goal_msg.trajectory.points.push_back(p);
return setGoal(goal_msg, info.id, info.s_error);
}
/**
* @todo Document function
*/
bool Tile::reset()
{
if (hasMine)
{
mineFactory->releaseMine(hasMine);
hasMine = NULL;
}
if (hasCharacter)
{
characterFactory->releaseCharacter(hasCharacter);
hasCharacter = NULL;
}
setGoal(false);
return true;
}
// partially initialises the thief and 4 detectives from a data file
// You may need to modify this function, to initialise all
// important agent information
void initialiseAgents(char *filename, Agent agents[], int maxCycles, Graph g){
FILE *fp;
fp = fopen (filename, "r"); // open data file
assert (fp != NULL);
int stamina;
Vertex start;
Vertex end;
char name[5];
int i;
int strategy;
fscanf(fp, "%d %d %d %[^\n]", &stamina,&start,&end,name);
agents[THIEF] = initAgent(start,maxCycles,stamina,RANDOM,g,name);
setGoal(agents[THIEF], end);
for(i=1; i<=NUM_DETECTIVES; i++){
fscanf(fp, "%d %d %d %[^\n]", &stamina,&start,&strategy,name);
agents[i] = initAgent(start,maxCycles,stamina,strategy,g,name);
}
fclose(fp);
}
void Squad::attack(TilePosition mGoal)
{
if (mGoal.x() == -1 || mGoal.y() == -1) return;
if (currentState != STATE_ATTACK)
{
if (!isUnderAttack())
{
if (isActive())
{
currentState = STATE_ATTACK;
}
}
}
if (isActive())
{
setGoal(mGoal);
}
}
void
FootstepNavigation::goalPoseCallback(
const geometry_msgs::PoseStampedConstPtr& goal_pose)
{
// check if the execution is locked
if (ivExecutingFootsteps)
{
ROS_INFO("Already performing a navigation task. Wait until it is "
"finished.");
return;
}
if (setGoal(goal_pose))
{
// this check enforces a planning from scratch if necessary (dependent on
// planning direction)
if (ivForwardSearch)
replan();
else
plan();
}
}
void RTAStar::plan(const Eigen::VectorXd &q_start, const KDL::Frame &x_goal, std::list<Eigen::VectorXd > &path_q, MarkerPublisher &markers_pub) {
path_q.clear();
V_.clear();
E_.clear();
setGoal(x_goal);
std::vector<double > tmp_vec;
tmp_vec.resize(transform_delta_vec_.size(), 0.0);
int q_new_idx_ = 0;
{
RTAStarState state_start( transform_delta_vec_.size() );
state_start.h_value_ = 0.0;
state_start.q_ = q_start;
state_start.dq_.resize(ndof_);
state_start.dq_.setZero();
state_start.parent_idx_ = -1;
// state_start.T_B_E_ = KDL::Frame(KDL::Vector(q_start(0), q_start(1), q_start(2)));
kin_model_->calculateFk(state_start.T_B_E_, effector_name_, q_start);
V_.insert(std::make_pair(0, state_start));
}
int current_node_idx = 0;
while (true) {
std::map<int, RTAStarState >::iterator v_it = V_.find(current_node_idx);
if (v_it == V_.end()) {
std::cout << "ERROR: RTAStar::plan v_it == V_.end() " << current_node_idx << std::endl;
return;
}
if ((v_it->second.T_B_E_.p - x_goal.p).Norm() < 0.08 && v_it->second.q_.innerSize() == ndof_) {
std::cout << "goal reached" << std::endl;
while (true) {
path_q.push_front(v_it->second.q_);
current_node_idx = v_it->second.parent_idx_;
if (current_node_idx < 0) {
break;
}
v_it = V_.find(current_node_idx);
if (v_it == V_.end()) {
std::cout << "ERROR: v_it == V_.end() " << current_node_idx << std::endl;
}
}
return;
}
// From a given current state, the neighbouring states are generated
for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) {
// the neighbour state was not yet visited from the current node
if (v_it->second.neighbour_nodes_[tr_idx] == -1) {
// create a new state and add it to the graph
RTAStarState new_state( transform_delta_vec_.size() );
new_state.T_B_E_ = transform_delta_vec_[tr_idx] * v_it->second.T_B_E_;
// check if this neighbour state already exists
int state_idx = -1;//checkIfStateExists(new_state.T_B_E_);
if (state_idx == -1) {
// create a new state
q_new_idx_++;
new_state.h_value_ = getCostH(new_state.T_B_E_);
new_state.parent_idx_ = v_it->first;
new_state.parent_tr_idx_ = tr_idx;
v_it->second.neighbour_nodes_[tr_idx] = q_new_idx_;
V_.insert(std::make_pair(q_new_idx_, new_state));
markers_pub.addSinglePointMarker(q_new_idx_, new_state.T_B_E_.p, 0, 1, 0, 1, 0.05, "world");
}
else {
// add existing state to neighbour list
v_it->second.neighbour_nodes_[tr_idx] = state_idx;
}
}
}
// The heuristic function, augmented by lookahead search, is applied to each,
// and then the cost of the edge to each neighbouring state is added to this value,
// resulting in an f value for each neighbour of the current state.
bool validTransitionExists = false;
for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) {
int neighbour_idx = v_it->second.neighbour_nodes_[tr_idx];
if (neighbour_idx >= 0) {
std::map<int, RTAStarState >::const_iterator n_it = V_.find(neighbour_idx);
if (n_it == V_.end()) {
std::cout << "ERROR: RTAStar::plan n_it == V_.end() " << neighbour_idx << std::endl;
return;
}
// calculate the lookahead heuristics for each neighbour and add the cost from current state to neighbour
double cost_h = lookahead(n_it->second.T_B_E_, 5);
tmp_vec[tr_idx] = cost_h + getCostLine(v_it->second.T_B_E_, n_it->second.T_B_E_);
std::cout << " cost_h " << tr_idx << " " << cost_h << std::endl;
validTransitionExists = true;
}
else {
tmp_vec[tr_idx] = 10000.0;
}
}
if (!validTransitionExists) {
// remove the current node
std::cout << " no possible transition " << v_it->first << std::endl;
std::map<int, RTAStarState >::iterator parent_it = V_.find(v_it->second.parent_idx_);
if (parent_it == V_.end()) {
std::cout << "no parent node" << std::endl;
return;
}
parent_it->second.neighbour_nodes_[v_it->second.parent_tr_idx_] = -2;
markers_pub.addSinglePointMarker(v_it->first, v_it->second.T_B_E_.p, 0, 0, 0, 1, 0.05, "world");
V_.erase(current_node_idx);
current_node_idx = parent_it->first;
markers_pub.publish();
ros::spinOnce();
continue;
}
// The node with the minimum f value is chosen for the new current state and a move to that state
// is executed. At the same time, the next best f value is stored at the previous current state.
// get the two smallest values
double min1 = -1.0, min2 = -1.0;
int min1_idx = -1;
for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) {
if (min1 < 0 || min1 > tmp_vec[tr_idx]) {
min1 = tmp_vec[tr_idx];
min1_idx = tr_idx;
}
}
for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) {
if (tr_idx == min1_idx) {
continue;
}
if (min2 < 0 || min2 > tmp_vec[tr_idx]) {
min2 = tmp_vec[tr_idx];
}
}
std::cout << "current_node_idx " << current_node_idx << " min: " << min1 << " " << min2 << std::endl;
// execute the move
int next_node_idx = v_it->second.neighbour_nodes_[min1_idx];
if (next_node_idx < 0) {
std::cout << "ERROR: next_node_idx < 0: " << next_node_idx << std::endl;
return;
}
std::map<int, RTAStarState >::iterator n_it = V_.find(next_node_idx);
// std::cout << n_it->second.T_B_E_.p[0] << " " << n_it->second.T_B_E_.p[1] << " " << n_it->second.T_B_E_.p[2] << std::endl;
// check if the next state is possible
if (!v_it->second.simulated_nodes_[min1_idx]) {
// simulate
n_it->second.q_.resize(ndof_);
n_it->second.dq_.resize(ndof_);
KDL::Frame nT_B_E;
std::list<KDL::Frame > path_x;
std::list<Eigen::VectorXd > path_q;
KDL::Frame T_B_E(n_it->second.T_B_E_);
bool col_free = collisionFree(v_it->second.q_, v_it->second.dq_, v_it->second.T_B_E_, n_it->second.T_B_E_, 0, n_it->second.q_, n_it->second.dq_, nT_B_E, &path_x, &path_q);
int similar_state = checkIfStateExists(n_it->first, n_it->second);
if (!col_free || similar_state >=0) {
n_it->second.h_value_ = 100.0;
if (!col_free) {
std::cout << " invalid state change, removing node " << next_node_idx << std::endl;
}
else {
std::cout << " joining states " << next_node_idx << " to " << similar_state << std::endl;
}
V_.erase(next_node_idx);
v_it->second.neighbour_nodes_[min1_idx] = -2;
markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 0, 0, 1, 0.05, "world");
markers_pub.publish();
ros::spinOnce();
continue;
}
v_it->second.simulated_nodes_[min1_idx] = true;
}
/* int similar_state = checkIfStateExists(n_it->first, n_it->second);
if (similar_state >= 0) {
// TODO: possible infinite loop!
// join states
v_it->second.neighbour_nodes_[min1_idx] = similar_state;
V_.erase(next_node_idx);
std::cout << "joining states: " << next_node_idx << " to " << similar_state << std::endl;
next_node_idx = similar_state;
}
*/
/*
if (!col_free) {isPoseValid(n_it->second.T_B_E_)) {
n_it->second.h_value_ = 10.0;
markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 0, 0, 1, 0.05, "world");
markers_pub.publish();
ros::spinOnce();
std::cout << " invalid pose" << std::endl;
continue;
}
*/
double min_h = 10000.0;
int min_idx = -1;
for (std::map<int, RTAStarState >::const_iterator v2_it = V_.begin(); v2_it != V_.end(); v2_it++) {
if (v2_it->second.h_value_ < min_h && v2_it->second.q_.innerSize() == ndof_) {
min_h = v2_it->second.h_value_;
min_idx = v2_it->first;
}
// markers_pub.addVectorMarker(v2_it->first+6000, v2_it->second.T_B_E_.p + KDL::Vector(0,0,0.05), v2_it->second.T_B_E_.p + KDL::Vector(0,0,0.05 + v2_it->second.h_value_*0.1), 0, 0.7, 0, 0.5, 0.01, "world");
}
next_node_idx = min_idx;
markers_pub.addSinglePointMarker(v_it->first, v_it->second.T_B_E_.p, 1, 0, 0, 1, 0.05, "world");
markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 0, 1, 1, 0.05, "world");
markers_pub.publish();
ros::spinOnce();
// getchar();
markers_pub.addSinglePointMarker(v_it->first, v_it->second.T_B_E_.p, 0, 1, 0, 1, 0.05, "world");
markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 1, 0, 1, 0.05, "world");
// save the second minimum value
v_it->second.h_value_ = min2;
std::cout << "changing state to " << next_node_idx << std::endl;
// go to the next state
current_node_idx = next_node_idx;
// getchar();
}
}
AREXPORT ArActionDesired *ArActionGoto::fire(ArActionDesired currentDesired)
{
double angle;
double dist;
double vel;
/* If myGoal changed since the last time setGoal() was called (it's an action
* argument) call setGoal() to reset to the new goal.
*/
if (myGoal.findDistanceTo(myOldGoal) > 5)
setGoal(myGoal);
// if we're there we don't do anything
if (myState == STATE_ACHIEVED_GOAL || myState == STATE_NO_GOAL)
return NULL;
dist = myRobot->getPose().findDistanceTo(myGoal);
if (dist < myCloseDist && ArMath::fabs(myRobot->getVel() < 5))
{
if (myPrinting)
printf("Achieved goal\n");
myState = STATE_ACHIEVED_GOAL;
myDesired.setVel(0);
myDesired.setDeltaHeading(0);
return &myDesired;
}
if (myPrinting)
printf("%.0f ", dist);
// see where we want to point
angle = myRobot->getPose().findAngleTo(myGoal);
if (ArMath::fabs(ArMath::subAngle(angle, myRobot->getTh())) > 120)
{
myCurTurnDir *= -1;
}
// see if somethings in front of us
if (currentDesired.getMaxVelStrength() > 0 &&
currentDesired.getMaxVel() < mySpeedToTurnAt)
{
if (myPrinting)
printf("Something slowing us down. ");
myDesired.setDeltaHeading(myTurnAmount * myCurTurnDir);
}
else
{
if (myPrinting)
printf("Can speed up and turn back again. ");
// see if we want to just point at the goal or not
if (ArMath::fabs(
ArMath::subAngle(angle,
ArMath::addAngle(myTurnAmount *
myCurTurnDir * -1,
myRobot->getTh())))
> myTurnAmount/2)
{
if (myPrinting)
printf("Pointing to goal ");
myDesired.setHeading(angle);
}
else
{
if (myPrinting)
printf("turning back ");
myDesired.setDeltaHeading(myTurnAmount * myCurTurnDir * -1);
}
}
if (dist < myCloseDist && ArMath::fabs(myRobot->getVel() < 5))
{
if (myPrinting)
printf("#achieved\n");
myState = STATE_ACHIEVED_GOAL;
myDesired.setVel(0);
myDesired.setDeltaHeading(0);
}
// if we're close, stop
else if (dist < myCloseDist)
{
if (myPrinting)
printf("#stop\n");
myDesired.setVel(0);
}
else
{
vel = sqrt(dist * 200 * 2);
if (vel > mySpeed)
vel = mySpeed;
if (myPrinting)
printf("#go %.0f\n", vel);
myDesired.setVel(vel);
}
return &myDesired;
}
bool EpicNavCorePlugin::makePlan(const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
std::vector<geometry_msgs::PoseStamped> &plan)
{
if (!initialized) {
ROS_ERROR("Error[EpicNavCorePlugin::makePlan]: EpicNavCorePlugin has not been initialized yet.");
return false;
}
// Set the goal location on the potential map.
unsigned int x_coord = 0;
unsigned int y_coord = 0;
if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, x_coord, y_coord)) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not convert goal to cost map location.");
x_coord = 0;
y_coord = 0;
}
setGoal(x_coord, y_coord);
ROS_INFO("Information[EpicNavCorePlugin::makePlan]: Solving harmonic function...");
int result = harmonic_complete_gpu(&harmonic, NUM_THREADS_GPU);
if (result != EPIC_SUCCESS) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not execute GPU version of 'epic' library.");
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Trying CPU fallback...");
result = harmonic_complete_cpu(&harmonic);
}
if (result == EPIC_SUCCESS) {
ROS_INFO("Information[EpicNavCorePlugin::makePlan]: Successfully solved harmonic function!");
} else {
ROS_ERROR("Error[EpicNavCorePlugin::makePlan]: Failed to solve harmonic function.");
return false;
}
//pub_potential.publish(harmonic.u);
plan.clear();
plan.push_back(start);
/*
if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, xCoord, yCoord)) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not convert start to cost map location.");
xCoord = 0;
yCoord = 0;
}
*/
float x = 0.0f;
float y = 0.0f;
if (!worldToMap(start.pose.position.x, start.pose.position.y, x, y)) {
ROS_WARN("Warning[EpicNavCorePlugin::makePlan]: Could not convert start to floating point cost map location.");
}
float step_size = 0.05f;
float cd_precision = 0.5f;
unsigned int max_length = harmonic.m[0] * harmonic.m[1] / step_size;
unsigned int k = 0;
float *raw_plan = nullptr;
result = harmonic_compute_path_2d_cpu(&harmonic, x, y, step_size, cd_precision, max_length, k, raw_plan);
if (result != EPIC_SUCCESS) {
ROS_ERROR("Error[EpicNavCorePlugin::makePlan]: Failed to compute the path.");
if (raw_plan != nullptr) {
delete [] raw_plan;
}
return false;
}
geometry_msgs::PoseStamped new_goal = goal;
for (unsigned int i = 1; i < k; i++) {
float x = raw_plan[2 * i + 0];
float y = raw_plan[2 * i + 1];
float plan_theta = std::atan2(y - raw_plan[2 * (i - 1) + 1], x - raw_plan[2 * (i - 1) + 0]);
float plan_x = 0.0f;
float plan_y = 0.0f;
mapToWorld(x, y, plan_x, plan_y);
new_goal.pose.position.x = plan_x;
new_goal.pose.position.y = plan_y;
new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(plan_theta);
plan.push_back(new_goal);
}
delete [] raw_plan;
raw_plan = nullptr;
plan.push_back(goal);
publishPlan(plan);
return true;
}
bool Arcade::init(){
if(!Layer::init()){
return false;
}
//inicial
tap_begin = Point::ZERO;
visibleSize = Director::getInstance()->getVisibleSize();
origin = Director::getInstance()->getVisibleOrigin();
//dificultad
setDelayRollBoard(DELAY_ROLL_BOARD);
setDelayBeforeFall(DELAY_BEFORE_FALL);
setDelayStopCombo(DELAY_STOP_COMBO);
setRandomizeBall(DEFAULT_RANDOMIZE_BALL);
setRowsStart(DEFAULT_ROWS_START);
//objetivos
setGoal(SCORE_WIN);
setScoreWin(400);
setComboWin(6);
_stop = false; _goto_menu = false;
_snd_take = String("musica y sonidos/baja.ogg");
_snd_drop = String("musica y sonidos/sube.ogg");
_snd_collide = String("musica y sonidos/choca_perla.ogg");
//set key
this->setKeypadEnabled(true);
background = Sprite::create("mapas/mapa1.png");
background->setPosition(Point(
visibleSize.width/2 + origin.x,
visibleSize.height/2 + origin.y
)
);
this->addChild(background, -10);
auto border = Sprite::create("efectos/barraArriba.png");
border->setPosition(Point(visibleSize.width/2 + origin.x,
visibleSize.height - border->getContentSize().height/4 + origin.y));
this->addChild(border, 9999);
//iniciale el tablaro
//y el populador del tablero
board = Board::create();
Node* board_view = board->getView();
this->addChild(board_view);
board_view->setPosition(Point(visibleSize.width/2 + origin.x, visibleSize.height/2 + origin.y));
board->attachMatch(CC_CALLBACK_2(Arcade::onMatchSpheres, this));
board->attachFall(CC_CALLBACK_3(Arcade::onFallSpheres, this));
board->attachDropSphere(CC_CALLBACK_1(Arcade::onAnimateExploitSphere, this));
board->attachEndBoard(CC_CALLBACK_1(Arcade::onReachEndBoard,this));
board_populater = new BoardPopulaterRandom(board);
board_populater->setStartRows(getRowsStart());
board_populater->populate();
board->setPopulater(board_populater);
player = Indian::create();
player->retain();
Node* player_view = player->getView();
this->addChild(player_view);
player->jumpTo(3);
_score_label = LabelTTF::create("0000","default", 60);
this->addChild(_score_label, 99999);
_score_label->setPosition(Point(origin.x + _score_label->getContentSize().width,
origin.y + visibleSize.height - _score_label->getContentSize().height/2));
//variables de juego
setTimeStart(DEFAULT_TIME_START);
_time = getTimeStart();
_clock_label = LabelTTF::create(String::createWithFormat("%02i", _time)->getCString(), "default", 60);
this->addChild(_clock_label, 99999);
_clock_label->setPosition(Point(visibleSize.width/2 + origin.x,
visibleSize.height + origin.y - _clock_label->getContentSize().height/3));
_time_over = false;
_score = 0;
setScoreCombo(DEFAULT_SCORE_COMBO);
setScoreMatch(DEFAULT_SCORE_MATCH);
auto dispatcher = Director::getInstance()->getEventDispatcher();
auto listener = EventListenerTouchOneByOne::create();
listener->onTouchBegan = CC_CALLBACK_2(Arcade::onTouchBegan, this);
listener->onTouchMoved = CC_CALLBACK_2(Arcade::onTouchMoved, this);
listener->onTouchEnded = CC_CALLBACK_2(Arcade::onTouchEnded, this);
dispatcher->addEventListenerWithSceneGraphPriority(listener, this);
this->schedule(schedule_selector(Arcade::updateBoard), 1.0f);
CocosDenshion::SimpleAudioEngine::sharedEngine()->stopBackgroundMusic(true);
PLAY_MUSIC("musica y sonidos/juego.ogg");
_time_roll_board.initWithTarget(this, schedule_selector(Arcade::updateRollBoard));
_time_roll_board.setInterval(getDelayRollBoard());
_time_combo.initWithTarget(this, schedule_selector(Arcade::updateCombo));
_time_combo.setInterval(0.5f);
_in_combo = false; _time_elapsed_combo = 0;
//tutorial
help_down = Sprite::create("tutorial/1paso.png"); help_down->retain();
help_up = Sprite::create("tutorial/2paso.png"); help_up->retain();
help_now = NULL;
return true;
}
/**
* runs through all active tiles(tiles within 'area'), and updates them
* according to the characters and mines on them.
*
* @note we may want to reset "this{Tile, Mine, Character}" after each yCount (xCount?)
* @note this function is turning out to be bigger and and more complex than I
* thought. We may want to break it up into more manageable peices.
*
* @return true on success
*/
bool World::update()
{
if (DEBUG > 1)
{
printf("World::update(): in World::update()\n");
}
// variables to be used
int xCount = 0;
int yCount = 0;
// area is defined in constructor and header
Tile *thisTile = NULL;
Mine *thisMine = NULL;
Character *thisCharacter = NULL;
updatetime++;
//
static bool goalExists;
static sf::Clock timer;
int curentTime = timer.getElapsedTime().asMilliseconds();
if (DEBUG > 1)
{
printf("timer is: %d \r ",curentTime);
}
// curentTime = timer.getElapsedTime().asMilliseconds(); // Allready has this value.
if(curentTime > 100)
{
timer.restart();
curentTime = timer.getElapsedTime().asSeconds();
// start of operations
//#pragma omp for schedule(dynamic)
// #pragma omp parallel for
for (yCount = 0; yCount < area; yCount++)
{
for (xCount = 0; xCount < area; xCount++)
{
thisTile = map[xCount][yCount];
if (!thisTile->getIsWall())
{
if ((thisMine = thisTile->getHasMine()))
{
if (thisMine->visibilityCountDown() > 0)
{
thisTile->setFloor(1);
}
else
{
thisTile->setFloor(0);
}
}
if (thisTile->getIsGoal())
{
thisTile->setFloor(2);
goalExists = true;
if (thisTile->getHasCharacter())
{
thisTile->getHasCharacter()->updatePoints(2);
thisTile->setGoal(false);
goalExists = false;
if (DEBUG > 1)
{
printf("World::Update(): Character hit flag\n");
}
}
}
#pragma omp critical(characterMovement)
{
if ((thisCharacter = thisTile->getHasCharacter()))
{
if(thisCharacter->getLastUpdate() != updatetime)
{
// scoreboard->setNewElement(thisCharacter->getID(), thisCharacter->getPoints());
thisCharacter->useController(thisCharacter, window);
if (thisCharacter->getMinePlaced())
{
placeMine(thisCharacter, thisTile);
}
if (moveCharacter(thisCharacter, xCount, yCount))
{
thisTile->setCharacter(NULL);
}
thisCharacter->resetDirection();
}
if (goalExists)
{
if (thisCharacter->getIsNpc())
{
if ((thisCharacter->getLastAiUpdate() <= updatetime-(14-difficulty)) || !thisCharacter->isStack())
{
//#pragma omp critical(astar)
//{
npcController.aStar(map, thisCharacter);
//}
thisCharacter->setLastAiUpdate(updatetime);
}
}
}
thisCharacter->setLastUpdate(updatetime);
}
}
} // end if (!thisTile->getIsWall())
} // end xCount
// int th_id = omp_get_thread_num();
// printf ("World::update: numTh: %d, thId: %d\r",
// omp_get_num_threads(), th_id);
} // end yCount
if (!goalExists) // If there is no goal then make one;
{
setGoal();
}
//scoreboard->printList();
}
return true;
}
trajectory::InterpolatedPtr planOMPL(
const statespace::StateSpace::State* _start,
constraint::TestablePtr _goalTestable,
constraint::SampleablePtr _goalSampler,
statespace::ConstStateSpacePtr _stateSpace,
statespace::InterpolatorPtr _interpolator,
distance::DistanceMetricPtr _dmetric,
constraint::SampleablePtr _sampler,
constraint::TestablePtr _validityConstraint,
constraint::TestablePtr _boundsConstraint,
constraint::ProjectablePtr _boundsProjector,
double _maxPlanTime,
double _maxDistanceBtwValidityChecks)
{
if (_goalTestable == nullptr)
{
throw std::invalid_argument("Testable goal is nullptr.");
}
if (_goalSampler == nullptr)
{
throw std::invalid_argument("Sampleable goal is nullptr.");
}
if (_goalTestable->getStateSpace() != _stateSpace)
{
throw std::invalid_argument("Testable goal does not match StateSpace");
}
if (_goalSampler->getStateSpace() != _stateSpace)
{
throw std::invalid_argument("Sampleable goal does not match StateSpace");
}
auto si = getSpaceInformation(
_stateSpace,
_interpolator,
std::move(_dmetric),
std::move(_sampler),
std::move(_validityConstraint),
std::move(_boundsConstraint),
std::move(_boundsProjector),
_maxDistanceBtwValidityChecks);
// Set the start and goal
auto pdef = ompl_make_shared<::ompl::base::ProblemDefinition>(si);
auto sspace
= ompl_static_pointer_cast<GeometricStateSpace>(si->getStateSpace());
auto start = sspace->allocState(_start);
pdef->addStartState(start); // copies
sspace->freeState(start);
auto goalRegion = ompl_make_shared<GoalRegion>(
si, std::move(_goalTestable), _goalSampler->createSampleGenerator());
pdef->setGoal(goalRegion);
auto planner = ompl_make_shared<PlannerType>(si);
return planOMPL(
planner,
pdef,
std::move(_stateSpace),
std::move(_interpolator),
_maxPlanTime);
}
void demo_7_main_core_0() {
int threads = TOTAL_PROC_NUM;
// Initialize obstacle map from heightmap
if (1) {
logStart("initialize obstacle map");
// Recode heightmap to global_obstacles
int a, b, c;
for (a = 0; a < WORLD_SIZE_X; a++) {
for (b = 0; b < WORLD_SIZE_Y; b++) {
int height = getHeightmapValue(a, b);
// some regions cannot be flown over
if (height > 174)
height = 255;
for (c = 0; c < height; c++) {
global_obstacles[a][b][c] = 1;
}
for (c = height; c < WORLD_SIZE_Z; c++) {
global_obstacles[a][b][c] = 0;
}
}
}
logEnd("initialize obstacle map");
}
// Set goal
setGoal(WORLD_SIZE_X - 3, WORLD_SIZE_Y - 3, getHeightmapValue(WORLD_SIZE_X - 3, WORLD_SIZE_Y - 3) + 2);
// Set current position
setPosition(3, 3, getHeightmapValue(3, 3) + 2);
// Init weightmap from obstacle map
if (1) {
int a, b, c;
logStart("initialize weightmap from obstacle map");
for (a = 0; a < WORLD_SIZE_X; a++) {
for (b = 0; b < WORLD_SIZE_Y; b++) {
for (c = 0; c < WORLD_SIZE_Z; c++) {
if (isGoal(a, b, c)) {
world[a][b][c] = WEIGHT_SINK;
} else if (isObstacle(a, b, c)) {
world[a][b][c] = WEIGHT_OBSTACLE;
} else {
world[a][b][c] = WEIGHT_INIT;
}
}
}
}
logEnd("initialize weightmap from obstacle map");
}
// Propagate weights with selected algorithm
promote_world(ITERATIONS, threads);
// Find waypoints to goal
findWaypoints();
printf("Iterations: %i\n", ITERATIONS);
return;
}
//处理网络数据
void Robot::processNetEpoll(void)
{
//接受Epoll返回的状态值
RobotNetEpoll::EpollDataStatus epollStatus;
//左轮速度，右轮速度
int leftSpeed, rightSpeed;
//nenents：当前触发的事件数，s为开始(Start)/e为结束(End)/b为基(Base)
int nevents, i, s, e, b;
int fd;
while(true)
{
//阻塞接收可用事件数
nevents = netEpoll->handleEvents();
//对每一个事件进行处理
for(i = 0; i < nevents; ++i)
{
b = 0;
//初始化缓冲区长度
bufLen = SOCKET_BUFF_SIZE;
//处理事件，并将收到的数据拷贝到socketBuf中。需要关注缓冲区溢出问题
//这里的fd都为值结果参数，bufLen是以指针形式传入
//------------------
//epollStatus返回事件类型,socketBuf返回数据首地址，bufLen返回数据长度， fd返回当前事件来源描述符
//------------------
epollStatus = netEpoll->DispactchEvent(i, socketBuf, &bufLen, fd);
//如果值结果参数为初始化长度，则认为数据过多，溢出处理
if(unlikely(bufLen == SOCKET_BUFF_SIZE)) continue;
switch(epollStatus)
{ //有数据返回处理
case RobotNetEpoll::EpollSuccessWithData://{
//使用while，是为了预防一次性接受到多组数据帧
while(1)
{
//检索完整的数据帧
s = b;
while((s < bufLen) && (socketBuf[s] != RobotPacket::Start)) {++s;}
if(s >= bufLen)
break;
e = s + 1;
while((s < bufLen) && (socketBuf[e] != RobotPacket::End)) {++e;}
if(e >= bufLen)
break;
b = e + 1;
//对帧的完整性进行初选,依据是长度
if((*(socketBuf + s + 6) + 8) != (e-s))
continue;
//对于完整的帧进行封装
robotPacketExternal.finalizePacket(socketBuf + s, e - s + 1);
#ifdef PACKET_HAVE_SUM_VERIFY
//对帧的完整性进行校验，依据是校验码
if(!robotPacketExternal.verifyCheckSum())
continue;
#endif
//robotPacketExternal.printHex();
//获取帧的属性
switch(robotPacketExternal.getCommandByIndex())
{
#ifdef ROBOT_SENSOR_DATA_DELAY_SEND
case RobotPacketExternal::UPLINK_DATA_RETURN://0xB3
{
minAck = robotPacketExternal.getIdByIndex();
PRINT("收到ACK编号为%d",minAck);
}
break;
#endif
//数据帧为带参数的控制帧
case RobotPacketExternal::DOWNLINK_CONTROL_WITHARGS_COMMAND://0x33
{
//控制目标为灯
if(unlikely(RobotPacketExternal::OBJECT_LIGHT == robotPacketExternal.getByteByIndex(4)))//0x04
{
//灯关
if(0x03 == robotPacketExternal.getByteByIndex(7))
{
robotLightTask->setLight(0, 0x00);
robotLightTask->doTask();
mediator->send(robotLightTask->getTaskBuf(), robotLightTask->getTaskBufLen());
}
//灯开
else if(0x02 == robotPacketExternal.getByteByIndex(7))
{
robotLightTask->setLight(0, 0x01);
robotLightTask->doTask();
mediator->send(robotLightTask->getTaskBuf(), robotLightTask->getTaskBufLen());
}
break;
}
//<模式切换>
else if(unlikely(RobotPacketExternal::OBJECT_MODE == robotPacketExternal.getByteByIndex(4)))//0x03
{
//modeLock.setMode(!modeLock.getMode());
//
//上一个状态为手柄模式,转换为智能避障模式，否则转换为手柄模式
if(modeLock.getMode() == RobotModeLock::MODE_JOYSTICK)
{
mediator->serialResume();
modeLock.setMode(RobotModeLock::MODE_WANDER);
}
else
{
mediator->serialSuspend();
modeLock.setMode(RobotModeLock::MODE_JOYSTICK);
}
PRINT("《《《《《《《收到模式切换命令，切换为到[%s]》》》》》》\n",
(modeLock.getMode() == RobotModeLock::MODE_JOYSTICK) ? "手柄模式": "智能模式");
#ifdef ROBOT_HAVE_LOG
#endif
//robotPoint.setPose(0,0,0);
//oldRobotPoint.setPose(0, 0, 0);
break;
}
//<电机控制>
else if(likely(RobotPacketExternal::OBJECT_MOTOR == robotPacketExternal.getByteByIndex(4)))//0x01
{
if(modeLock.getMode() == RobotModeLock::MODE_JOYSTICK)
{
leftSpeed = judgeLeftSpeed(
robotPacketExternal.getByteByIndex(7),
robotPacketExternal.getByteByIndex(8));
rightSpeed = judgeRightSpeed(
robotPacketExternal.getByteByIndex(7),
robotPacketExternal.getByteByIndex(8));
/*
if(leftSpeed > 0 && rightSpeed > 0)//前进
if(disObs[3] <= 70) break;
if(leftSpeed < 0 && rightSpeed < 0)//后退
if(disObs[9] <= 70) break;*/
PRINT("1发送给机器人的控制速度:左轮[%hd]-右轮[%hd]\n",leftSpeed, rightSpeed);
#ifdef ROBOT_HAVE_LOG
#endif
robotWheelTask->setWheel(leftSpeed, rightSpeed);
robotWheelTask->doTask();
mediator->send(robotWheelTask->getTaskBuf(), robotWheelTask->getTaskBufLen());
}
}
//<上位机设置机器人目标坐标点>
else if(unlikely(RobotPacketExternal::OBJECT_COORS== robotPacketExternal.getByteByIndex(4)))//0x013
{
int tx = RobotMath::charToSignedShort(
robotPacketExternal.getByteByIndex(7),
robotPacketExternal.getByteByIndex(8));
int ty = RobotMath::charToSignedShort(
robotPacketExternal.getByteByIndex(9),
robotPacketExternal.getByteByIndex(10));
PRINT("设置机器人坐标为%d,%d",tx,ty);
setGoal(tx, ty, 0);
}
//<收到前面机器人存在信息>
else if(unlikely(RobotPacketExternal::OBJECT_LINKS_FRONT== robotPacketExternal.getByteByIndex(4)))//0x0D
{
//robotPacketExternal.printHex();
//前面机器人编号
frontRobotNo = robotPacketExternal.getByteByIndex(5);
map<int, string>::iterator iter = socketMap.find(frontRobotNo);
if(iter != socketMap.end())
{
setHaveFrontRobot(true);
//如前边的机器人已经连接
if(!getFrontRobotIsConnected())
{
setFrontRobotIp((iter->second).c_str(), strlen((iter->second).c_str()));
semaphoreFlag = 1;
//将信号灯挂出
semaPhore->Post();
}
else
{
//如果新设置的前方机器人IP地为新的IP地址
if(strcmp(frontRobotIp, (iter->second).c_str()) != 0)
{
frontRobotSocket->close();
RELEASE_HEAP_MEMORY(frontRobotSocket);
frontRobotSocket = new RobotSocket();
setFrontRobotIp((iter->second).c_str(), strlen((iter->second).c_str()));
semaphoreFlag = 1;
//将信号灯挂出
semaPhore->Post();
}
}
}
}
//<收到后面机器人存在信息>
else if(unlikely(RobotPacketExternal::OBJECT_LINKS_BACK== robotPacketExternal.getByteByIndex(4)))//0x0E
{
//robotPacketExternal.printHex();
//后面机器人编号
backRobotNo = robotPacketExternal.getByteByIndex(5);
map<int, string>::iterator iter = socketMap.find(backRobotNo);
if(iter != socketMap.end())
{
//setBackRobotIp(socketMap[backRobotNo].c_str(), sizeof(socketMap[backRobotNo].c_str()));
//PRINT("%s",(iter->second).c_str());
setHaveBackRobot(true);
if(!getBackRobotIsConnected())
{
setBackRobotIp((iter->second).c_str(), strlen((iter->second).c_str()));
semaphoreFlag = 1;
semaPhore->Post();
}
else
{
//如果新设置的后方机器人IP地位新的IP地址
if(strcmp(backRobotIp,(iter->second).c_str()) != 0)
{
backRobotSocket->close();
RELEASE_HEAP_MEMORY(backRobotSocket);
backRobotSocket = new RobotSocket();
setBackRobotIp((iter->second).c_str(), strlen((iter->second).c_str()));
semaphoreFlag = 1;
//将信号灯挂出
semaPhore->Post();
}// for if
}// for else
}//for if(iter)
}
else if(unlikely(RobotPacketExternal::OBJECT_ALL_STOP== robotPacketExternal.getByteByIndex(4)))//0x0E
{
modeLock.setMode(RobotModeLock::MODE_JOYSTICK);
}
break;
}//end for " case RobotPacketExternal::DOWNLINK_CONTROL_WITHARGS_COMMAND"
break;
//机器人之间的信息交换
case RobotPacketExternal::BETWEEN_ROBOTS_INFO_CHANGE:
{
if(fd == frontRobotFd)
{
frontRobotCtlWord = RobotMath::charToShort(robotPacketExternal.getByteByIndex(4),
robotPacketExternal.getByteByIndex(5));
//收到编队前方机器人信息
frontRobotPoint.setX(
RobotMath::charToSignedShort(robotPacketExternal.getByteByIndex(7),
robotPacketExternal.getByteByIndex(8)));
frontRobotPoint.setY(
RobotMath::charToSignedShort(robotPacketExternal.getByteByIndex(9),
robotPacketExternal.getByteByIndex(10)));
frontRobotPoint.setTh(
RobotMath::charToSignedShort(robotPacketExternal.getByteByIndex(11),
robotPacketExternal.getByteByIndex(12)));
//设置路径规划目标
setGoal(frontRobotPoint);
}
else if(fd == backRobotFd)
{
backRobotCtlWord = RobotMath::charToShort(robotPacketExternal.getByteByIndex(4),
robotPacketExternal.getByteByIndex(5));
//收到编队后方机器人信息
backRobotPoint.setX(
RobotMath::charToSignedShort(robotPacketExternal.getByteByIndex(7),
robotPacketExternal.getByteByIndex(8)));
backRobotPoint.setY(
RobotMath::charToSignedShort(robotPacketExternal.getByteByIndex(9),
robotPacketExternal.getByteByIndex(10)));
backRobotPoint.setTh(
RobotMath::charToSignedShort(robotPacketExternal.getByteByIndex(11),
robotPacketExternal.getByteByIndex(12)));
}
else
::close(fd);
}// end for "case RobotPacketExternal::BETWEEN_ROBOTS_INFO_CHANGE"
break;
case RobotPacketExternal::DOWNLINK_QUERY_COMMAND: break;
case RobotPacketExternal::DOWNLINK_CONTROL_NOARGS_COMMAND:break;
default:break;
}//end for " switch(robotPacketExternal.getCommandByIndex())"
}//end for " while(1) "
break;
case RobotNetEpoll::EpollAccept:
{
robotPacketExternal.finalizePacket(socketBuf, bufLen);
char tmp[20];
sprintf(tmp,"%s", robotPacketExternal.getBuf());
PRINT("当前IP地址为[%s]的主机连接到该机器人", tmp);
if(0 == memcmp(tmp, consoleIp, strlen(consoleIp)))
{
PRINT("该连接为控制台，已经准备完毕!");
consoleSocket->copy(fd, true);
consoleSocket->setNonBlock();
//consoleSocket->setKeepAlive();
//控制台已经连接
setConsoleIsConnected(true);
#ifdef ROBOT_SENSOR_DATA_DELAY_SEND
minAck = maxAck = 1;
ACK = 0;
//控制台链接好，则可以将保存的文件发送过去
semaphoreFlag = 0;
semaPhore->Post();
#endif
}
else if(0 == memcmp(tmp, frontRobotIp, strlen(frontRobotIp)))
{
//编队前方机器人连接
frontRobotFd = fd;
//accept前方机器人的连接请求
setFrontRobotIsAccepted(true);
}
else if(0 == memcmp(tmp, backRobotIp, strlen(backRobotIp)))
{
//编队后方机器人连接
backRobotFd = fd;
//accept后方机器人的连接请求
setBackRobotIsAccepted(true);
}
}//end for " case RobotNetEpoll::EpollAccept"
break;
case RobotNetEpoll::EpollError:
{
if(fd == consoleSocket->getFD())
{
PRINT("-->链接到控制台网络被异常断开-\n");
setConsoleIsConnected(false);
#ifdef ROBOT_SENSOR_DATA_DELAY_SEND
semaphoreFlag = -1;
semaPhore->Post();
#endif
}
}
break;
case RobotNetEpoll::EpollSuccessNoData:break;
case RobotNetEpoll::EpollSocketClosed:
{
//某个套接字失效
if(frontRobotFd == fd)
{
//失效前方连接套接字
frontRobotFd = -1;
setFrontRobotIsAccepted(false);
PRINT("-->编队队伍中前方机器人已经主动断开至该机器人的连接<--\n");
}
else if(backRobotFd == fd)
{
backRobotFd = -1;
setBackRobotIsAccepted(false);
PRINT("-->编队队伍中后方机器人已经主动断开至该机器人的连接<--\n");
}
else if(fd == consoleSocket->getFD())
{
PRINT("-->控制台已经主动断开至该机器人的连接<--\n");
setConsoleIsConnected(false);
#ifdef ROBOT_SENSOR_DATA_DELAY_SEND
semaphoreFlag = -1;
semaPhore->Post();
#endif
}
else if(fd == UDPBroadcastSocket->getFD())
{
//throw RobotException("UDP服务器出错");
RELEASE_HEAP_MEMORY(UDPBroadcastSocket);
//重启UDP服务器
initUDPServer();
}
}// end for " case RobotNetEpoll::EpollSocketClosed"
// break; } //end for case
}//end for " switch(epollStatus)"
}//end for " for(i = 0; i < nevents; ++i)"
} //end for "while(true)"
}
//action 开始方法
AREXPORT ArActionDesired *Avoid::fire(
ArActionDesired currentDesired)
{
//设置目标点偏了，纠正
if(myGoal.findDistanceTo(myOldGoal) > 5){
setGoal(myGoal);
}
//当前位置与目标点之间的夹角
poseAngle = myRobot->getPose().findAngleTo(myGoal);
//重新设定目标点后进行方位对准，指向目标点
if(!ACHIEVE_ANGLE){
//poseAngle = myRobot->getPose().findAngleTo(myGoal);
if(ArMath::fabs(poseAngle - myRobot->getTh())>1){
myRobot->setHeading(myRobot->getPose().findAngleTo(myGoal));
printf("try to pose to the target1, angle is %f \n", myRobot->getPose().findAngleTo(myGoal));
}
else{
ACHIEVE_ANGLE = true;
printf("ACHIEVE_ANGLE\n");
}
return &myDesired;
}
// 到达目标点或者已经取消了目标点，什么不做返回
if (myState == STATE_ACHIEVED_GOAL || myState == STATE_NO_GOAL){
return NULL;
}
//定义左边距离与右边距离
double leftDist, rightDist;
//定义前方距离和探测到最小距离的声呐环上的那个声呐在声呐环坐标系下的夹角
double dist, angle;
//小车距离目标点的距离
double dist_to_goal;
//左右两边声呐测距减去侧边安全距离之后的值
double left_err;
double right_err;
//车的指向和目标指向的夹角，得做处理
double angle_attractive;
//车左右轮速度
double Vl, Vr;
double X,Y;
//重要参数，避障转向的比例系数
double k1 = 0.2; //0.2
//较远距离找目标点调整参数，有关角度
double k2 = 1.8; // 1.8
//较远距离找目标点调整参数，有关距离
double kp2 = 0.10; // 0.1
//较近距离找目标点调整参数，有关角度
//double kp3_angle = 0.8;
//较近距离找目标点调整参数，有关距离
double kp3_dist = 0.6; // 0.6 //2
//进入目标点的误差
double err_goal = 0;
//直线行走的小车最大速度
double car_val_normal = 200; //300
//转向时用的速度
double car_val_turn = 100; //150;
//探测前方距离，利用-40度到40度之间的声呐来探测
dist = myRobot->checkRangeDevicesCurrentPolar(-40, 40, &angle);
//计算车体位置到目标点距离
dist_to_goal = myRobot->getPose().findDistanceTo(myGoal);
//- myRobot->getRobotRadius());
//计算左边距离
leftDist = myRobot->checkRangeDevicesCurrentPolar(60, 120);
//- myRobot->getRobotRadius());
//计算右边距离
rightDist = myRobot->checkRangeDevicesCurrentPolar(-120, -60);
//myRobot->getRobotRadius());
//如果左边或者右边距离大于4000mm，很有可能差声波反射走了，很有可能为伪数据，
//改用20到120度之间的声呐再测一次，提高数据可信度
if(leftDist >4000){
leftDist = myRobot->checkRangeDevicesCurrentPolar(20, 120) ;
//myRobot->getRobotRadius());
}
if(rightDist >4000){
rightDist = myRobot->checkRangeDevicesCurrentPolar(-120, -20) ;
//myRobot->getRobotRadius());
}
//如果前方距离大于1000，也有可能数据不准，利用-70到70度之间的数据再测一次
//提高数据可信度
if(dist >1000){
dist = myRobot->checkRangeDevicesCurrentPolar(-70, 70, &angle) ;
//- myRobot->getRobotRadius());
}
//计算车体当前指向与目标点之间的夹角，该角度很诡异，后期需要重新认真检查判断
angle_attractive = ArMath::fabs(ArMath::subAngle(myRobot->getPose().findAngleTo(myGoal), myRobot->getTh()));
//车体当前指向与目标点之间的夹角，该角度很诡异，我这里简单的做处理（大于90度用180去减），上次调试已经发现该处理不对
//，后期调试请先把该角度处理正确<优先级很高！>
if(angle_attractive>90){
angle_attractive = 180 -angle_attractive;
}
//屏蔽周围过大环境,提高抗干扰性，初始设置为1800mm
if(leftDist>1800){
leftDist = 1800;
}
if(rightDist>1800){
rightDist = 1800;
}
//减去设定的边缘安全距离
left_err = leftDist - myObsDist_side;
right_err = rightDist - myObsDist_side;
X = myRobot->getX();
Y = myRobot->getY();
myDesired.reset();
//部分调试信息
printf("\n\n**");
printf("dist = %f \n", dist);
printf("distacne between my pose to goal = %f\n",dist_to_goal);
printf("left_err = %f \n", left_err);
printf("right_err = %f \n", right_err);
printf("angle_attractive = %f \n", angle_attractive);
printf("poseAngle = %f \n", poseAngle);
printf("X = %f \n", X);
printf("Y = %f \n", Y);
printf("*******************************\n\n");
//到达目标点做相应处理，进入目标点为圆心的600mm 环开始做处理
if(dist_to_goal<1000){
if(dist_to_goal<200){
//进入200mm 环，判断为到达目标点
//myRobot->clearDirectMotion();
//因为定时处理，所以到达目标点后第一个周期先停车
if(!STOP_CAR_FIRST){
myRobot->stop();
STOP_CAR_FIRST = true;
printf("STOP_CAR_FIRST = true\n");
}
printf("achieved the goal\n");
//设置全局状态：到达目标点
myState = STATE_ACHIEVED_GOAL;
STOP_CAR_FIRST = false;
ACHIEVE_ANGLE = false;
return &myDesired;
}
//否则先调整姿态对准目标点
if(ArMath::fabs(poseAngle - myRobot->getTh())>5){
if(!TURN_TO_GOAL){
myRobot->stop();
TURN_TO_GOAL = true;
printf("TURN_TO_GOAL = true\n");
}
myRobot->setHeading(myRobot->getPose().findAngleTo(myGoal));
printf("try to pose to the target2, angle is %f \n", myRobot->getPose().findAngleTo(myGoal));
printf("当前位姿信息= %f\n：",myRobot->getTh());
}
else{
//对准目标点之后然后一直直线开过去
TURN_TO_GOAL = false;
err_goal = dist_to_goal-200;
Vl = kp3_dist*err_goal;
Vr = kp3_dist*err_goal;
//做个限速
if(Vl<100){
Vl = Vr = 100;
}
myRobot->setVel2(Vl, Vr);
printf("near the goal\n");
printf("V = %F \n", Vr);
printf("already pose to the target \n");
}
return &myDesired;
}
//前方没有障碍物
//if((dist>myObsDist_front)||(minimum_Dist>myObsDist_front)){
if(dist>myObsDist_front){
//但是两边都太窄了，过不去，后退
if((right_err<0)&&(left_err<0)){
myRobot->setVel2(-100, -100);
printf("front distacne is large, but the left and right distacne is too narrow, go back \n");
}
else if((right_err < 0)&&(left_err>0))
{
//或者右窄左宽，左转
Vl = car_val_turn - k1*rightDist;
Vr = car_val_turn + k1*rightDist;
myRobot->setVel2(Vl, Vr);
printf("front distacne is large, go left\n");
}
else if((left_err < 0)&&(right_err>0))
{
//或者左宽右窄，右转
Vl = car_val_turn + k1*leftDist;
Vr = car_val_turn - k1*leftDist;
myRobot->setVel2(Vl, Vr);
printf("front distacne is large, go right\n");
}
else{
//前面空荡荡，那就找目标点，机器人在目标位置的左边，右转追寻目标点
if((((myRobot->getPose().getY() - myGoal.getY())>270)&&(ArMath::fabs(poseAngle)<=90)) \
||(((myRobot->getPose().getY() - myGoal.getY())<-270)&&(ArMath::fabs(poseAngle)>=90))){ //100
// Vl = car_val_turn + kp2*((myRobot->getPose().getY() - myGoal.getY())) + k2*angle_attractive;
// Vr = car_val_turn - kp2*((myRobot->getPose().getY() - myGoal.getY())) - k2*angle_attractive;
myRobot->setHeading(poseAngle);
myRobot->setVel(200);
// myRobot->setVel2(Vl, Vr);
printf("go for the goal\n");
}
//前面空荡荡，那就找目标点，避障之后机器人在目标位置的右边，左转追寻目标点
else if((((myRobot->getPose().getY() - myGoal.getY())>270)&&(ArMath::fabs(poseAngle)>90)) \
||(((myRobot->getPose().getY() - myGoal.getY())<-270)&&(ArMath::fabs(poseAngle)<90))){ //100
// Vl = car_val_turn + kp2*((myRobot->getPose().getY() - myGoal.getY())) - k2*angle_attractive;
// Vr = car_val_turn - kp2*((myRobot->getPose().getY() - myGoal.getY())) + k2*angle_attractive;
// myRobot->setVel2(Vl, Vr);
myRobot->setHeading(poseAngle);
myRobot->setVel(200);
printf("go for the goal\n");
}
//目标点在容错方位范围内，切换为快速直奔目标点
else{
//car_val_normal = 300;
Vl = Vr = car_val_normal;
myRobot->setVel2(Vl, Vr);
printf("no obstacle,go head\n");
}
}
return &myDesired;
}
//遇到障碍物了，在安全距离的0.8倍之外，先减速做好避寒准备
//if((dist>myObsDist_front*0.8)||(minimum_Dist>myObsDist_front*0.8)){
if(dist>myObsDist_front*0.8){
car_val_normal = 250*dist/myObsDist_front;
if (car_val_normal > 250){
car_val_normal = 250;
}
//左右两边都是障碍物，进入死胡同了，后退
if((right_err<0)&&(left_err<0)){
myRobot->setVel2(-50, -50);
printf("near obstacle, but the left and right distacne is too narrow,go back \n");
}
//右边小左边大，左转
else if ((right_err < 0)&&(left_err>0))
{
Vl = car_val_turn - k1*rightDist;
Vr = car_val_turn + k1*rightDist;
myRobot->setVel2(Vl, Vr);
printf(" near obstacle, go left\n");
}
//右边大左边小，右转
else if ((left_err < 0)&&(right_err>0))
{
Vl = car_val_turn + k1*leftDist;
Vr = car_val_turn - k1*leftDist;
myRobot->setVel2(Vl, Vr);
printf("near obstacle, go right\n");
}
else{
//减速直走
Vl = Vr = car_val_normal;
myRobot->setVel2(Vl, Vr);
printf("near obstacle,slow down\n");
}
return &myDesired;
}
//障碍物距离在0.4到0.8安全距离范围中，进行避障
// if((dist >0.4*myObsDist_front)||(minimum_Dist>myObsDist_front*0.4)){
if(dist >0.4*myObsDist_front){
//左转避障
if(leftDist>rightDist){
rightDist = myRobot->checkRangeDevicesCurrentPolar(-190, -20);
if(rightDist>800){
rightDist = 800;
}
Vl = car_val_turn - k1*rightDist ;//+ k2*angle_attractive;
Vr = car_val_turn + k1*rightDist ;//- k2*angle_attractive;
myRobot->setVel2(Vl, Vr);
printf("turn left\n");
}
else{
//右转避障
leftDist = myRobot->checkRangeDevicesCurrentPolar(20, 190);
if(leftDist>800){
leftDist = 800;
}
Vl = car_val_turn + k1*leftDist; //- k2*angle_attractive;
Vr = car_val_turn - k1*leftDist; //+ k2*angle_attractive;
myRobot->setVel2(Vl, Vr);
printf("turn right\n");
}
}//else if((dist <= 0.4*myObsDist_front)||(minimum_Dist>myObsDist_front)){ //小于0.4倍的安全距离，眼看就要撞上了，紧急处理
else if(dist <= 0.4*myObsDist_front){ //小于0.4倍的安全距离，眼看就要撞上了，紧急处理
//左边还有空间，向左边逃离
if((left_err > right_err)&&(right_err > -100)){
myRobot->setVel2(-70, 70);
printf("too close to the obstacle ,try to escape and turn left\n");
}
//右边还有空间，向右边逃离
else if ((right_err > left_err)&&(left_err > -100))
{
myRobot->setVel2(70,-70);
printf("too close to the obstacle ,try to escape and turn right\n");
}
else{
//两边都没空间了，只能倒退
myRobot->setVel2(-70, -70);
printf("too close to the obstacle ,go back -_-!\n");
}
}
return &myDesired;
}
