void LocalMoveBase::movebase(void)
{
std_msgs::String state;
bool done=false;
ros::Rate r(RATE);
while (ros::ok())
{
if(NewPath)
{
ChoosePath();//选择路线
PublishState("StartMoveZ");
NewPath=false;
int i=1;
int length=MPath[FIANLPATH].poses.size();
ROS_INFO("has %d goals",length-1);
ccr.runBehavior();
while (i<length)
{
//PublishState("RotateZ");
ROS_INFO("--------------------->");
if(i<(length-2))
{
//判断下一个目标点是否有效
done=mLocalCostmap->isNextGoalValid(MPath[FIANLPATH],i-1);
if(!done)
{
break;
}
}
done=moveToGoal_Forward(MPath[FIANLPATH].poses[i-1],MPath[FIANLPATH].poses[i],i);
if(!done)
{
break;
}
//清空一下周围地图
ccr.runBehavior();
i+=1;
ros::spinOnce();
}
if(done)
{
if(rotateToGoalDirection(MPath[FIANLPATH].poses[i-2],MPath[FIANLPATH].poses[i-1]))
{
PublishState("GoalReachedZ");
ccr.runBehavior();
}
}
else
{
}
}
ros::spinOnce();
r.sleep();
}
}
void UpdateGhost(Unit &ghost)
{
ChoosePath(ghost);
TryChase(ghost, Pacman);
if(CanMove(ghost) == true)
{
if(MoveForward(ghost, Speed * 0.5f))
{
ghost.Rotating = false;
}
}
else
{
ghost.Face = (Direction) (rand() % 4);
}
}
