ros/src/dirt_detection/src/dirt_detector.cpp
/*!
 * Generates a new subjective opinion for the given Pose
 */
Generates a new subjective opinion for the given Pose
Generates a new subjective opinion for the given Pose
knowledge_aggregator_msgs::SO DirtDetector::getSubjectiveOpinion(geometry_msgs::PoseStamped pose_stamped_laser_map,
		bool isDirt) {
	float x_tgt = pose_stamped_laser_map.pose.position.x;
	float y_tgt = pose_stamped_laser_map.pose.position.y;
	float x_src = r_pose.position.x;
	float y_src = r_pose.position.y;
	float unit = 1.0;
	float belief = 0.0;
	float disbelief = 0.0;
	float uncertainty = 0.0;
	float atomicity = 0.5;
	float distance = std::hypot((x_tgt - x_src), (y_tgt - y_src));
	float u = 0.0;
        if (distance >= scan.range_max)
            u = 0.99;
        else
            u = distance / scan.range_max;
	uncertainty = std::min(unit, u);
	if (isDirt) {
		belief = unit - uncertainty;
		disbelief = 0.0;
	} else {
		disbelief = unit - uncertainty;
		belief = 0.0;
	}
	knowledge_aggregator_msgs::SO so;
	so.pose = pose_stamped_laser_map.pose;
	so.belief = belief;
	so.disbelief = disbelief;
	so.uncertainty = uncertainty;
	so.base_rate = atomicity;
	return so;
}
knowledge_aggregator_msgs :: SO DirtDetector :: getSubjectiveOpinion ( geometry_msgs :: PoseStamped pose_stamped_laser_map , bool isDirt ) { float x_tgt = pose_stamped_laser_map . pose . position . x ; float y_tgt = pose_stamped_laser_map . pose . position . y ; float x_src = r_pose . position . x ; float y_src = r_pose . position . y ; float unit = 1.0 ; float belief = 0.0 ; float disbelief = 0.0 ; float uncertainty = 0.0 ; float atomicity = 0.5 ; float distance = std :: hypot ( ( x_tgt - x_src ) , ( y_tgt - y_src ) ) ; float u = 0.0 ; if ( distance >= scan . range_max ) u = 0.99 ; else u = distance / scan . range_max ; uncertainty = std :: min ( unit , u ) ; if ( isDirt ) { belief = unit - uncertainty ; disbelief = 0.0 ; } else { disbelief = unit - uncertainty ; belief = 0.0 ; } knowledge_aggregator_msgs :: SO so ; so . pose = pose_stamped_laser_map . pose ; so . belief = belief ; so . disbelief = disbelief ; so . uncertainty = uncertainty ; so . base_rate = atomicity ; return so ; }
Generates a new subjective opinion for the given Pose
Generates a new subjective opinion for the given Pose
//float distance = map.info.resolution * std::hypot((x_tgt - x_src), (y_tgt - y_src));