void hotspot::simulation(){
if(!enableSimulation)
{
tree->addParent(uav);
tree->currentchildren(HexSamples);
enableSimulation=true;
}
// compute cost and info
MatrixXf nei(6,2),subgoal(6,2);
VectorXf MeasurementAttribute(6);
nei=array2Mat(HexSamples);
subgoal=repeatMat(target);
if(step%3==0)
MeasurementAttribute=distance(subgoal,subgoal);
else
MeasurementAttribute=distance(subgoal,nei);
//
updateMeasurement(MeasurementAttribute.data());
// termination condition
float near=sqrt(pow(target[0]-uav[0],2)+pow(target[1]-uav[1],2));
ROS_INFO_STREAM("step:= "<<step<< " distance:= "<<near);
step++;
sleep(1);
return (step<50&&near>1)?simulation():optimal_path_publish();
}
void AckermannKalmanFilter::updateFrontRightWheelDisplacementSteering(double
dRightWheel, double steering, double timestamp) {
const double phi_r = std::atan(std::tan(steering) * getWheelBase() /
(getWheelBase() + getFrontWheelTrack() * 0.5 * std::tan(steering)));
updateMeasurement(dRightWheel * std::cos(phi_r),
ackermannKalmanFilterParameters_.frontRightWheelVariance,
(Eigen::Matrix<double, 1, 2>()
<< 1, getFrontWheelTrack() * 0.5).finished(), timestamp);
}
void AckermannKalmanFilter::updateSteering(double steering, double
timestamp) {
if (std::fabs(x_k_(0)) < 1e-1)
return;
updateMeasurement(std::tan(steering),
ackermannKalmanFilterParameters_.frontLeftWheelVariance,
(Eigen::Matrix<double, 1, 2>()
<< -getWheelBase() * x_k_(1) / x_k_(0) / x_k_(0),
getWheelBase() / x_k_(0)).finished(), timestamp);
}
void hotspot::pathSeqToMes(float reading[6],int n){
float hex_mes[6];int sequence[6];
for(int i=0;i<6;i++)
hex_mes[i]=1000;
tree->pathSequence(sequence);
for(int j=0;j<n;j++){
if(sequence[j]==-1){
ROS_ERROR("measurement update error");
continue;
}
ROS_INFO(" sequence %d",sequence[j]);
hex_mes[sequence[j] ]=reading[j];
}
updateMeasurement(hex_mes);
}
