void PatternMatching::calcMarkersNextDistanceAngle(std::vector<Marker *>&markers) { //float R = (float) (360*PI*180); for (unsigned int i=0; i < markers.size(); ++i) { if ((i+1) != markers.size()) { markers[i]->next_dist = euclideanDistance2D(markers[i]->center, markers[i+1]->center); markers[i]->next_angle_dist = (markers[i+1]->angle - markers[i]->angle); //Revisar!! } else { markers[i]->next_dist = euclideanDistance2D(markers[0]->center, markers[i]->center); markers[i]->next_angle_dist = (markers[0]->angle + ( (float) R - markers[i]->angle) ); //Revisar!! } } }
double calculateReward(State goal, int lookahead) { ROS_INFO("Calculating reward for state. Lookahead is %d", lookahead); double reward = -1; //get the data from /goal , may be store it in a list and use //assuming the goal gets with only one person double normSum=0.0; if (lookahead == 1) { for (std::vector<State>::iterator state = pomdp.states.begin(); state != pomdp.states.end(); state++) { if (goal.pose.position.x != state->pose.position.x && goal.pose.position.y != state->pose.position.y) { //only only 95 out of 100 times there is a person actually if (state->obs_weight == 1/pomdp.num_states) { ROS_INFO("State observation weight is 1/pomdp.num_states!"); //as per map thats unreachable/unexplored so keep it zero continue; } else { state->obs_weight = state->obs_weight + (1 / euclideanDistance2D(goal, *state) *.95 ); //ROS_INFO("euc dist %f",euclideanDistance2D(goal, *state)); if (state->obs_weight > reward) { reward = state->obs_weight; } } } else { state->obs_weight=0.8; ROS_INFO("goal is an internal state, reward set to very high"); } normSum=normSum+state->obs_weight; } normaliseDistribution(normSum); ROS_INFO("Norm sum is %f.", normSum); } /*else { for (std::vector<State>::iterator state = pomdp.states.begin(); state != pomdp.states.end(); state++) { if (goal != *state) { if (state->obs_weight == 0) { continue; } state->obs_weight = calculateReward(*state, lookahead - 1) + (1 / euclideanDistance2D(goal, *state) * 0.95) + state->obs_weight; if (state->obs_weight > reward) { reward = state->obs_weight; } } else { } } }*/ ROS_INFO("Calculated a reward of %f.", reward); return reward; }