void WaypointSaver::outputProcessing(geometry_msgs::Pose current_pose, double velocity) const { std::ofstream ofs(filename_.c_str(), std::ios::app); static geometry_msgs::Pose previous_pose; static bool receive_once = false; // first subscribe if (!receive_once) { ofs << std::fixed << std::setprecision(4) << current_pose.position.x << "," << current_pose.position.y << "," << current_pose.position.z << "," << tf::getYaw(current_pose.orientation) << std::endl; receive_once = true; displayMarker(current_pose, 0); previous_pose = current_pose; } else { double distance = sqrt(pow((current_pose.position.x - previous_pose.position.x), 2) + pow((current_pose.position.y - previous_pose.position.y), 2)); // if car moves [interval] meter if (distance > interval_) { ofs << std::fixed << std::setprecision(4) << current_pose.position.x << "," << current_pose.position.y << "," << current_pose.position.z << "," << tf::getYaw(current_pose.orientation) << "," << velocity << std::endl; displayMarker(current_pose, velocity); previous_pose = current_pose; } } }
void Box::displayAdditionals(sf::RenderWindow& window) { displayMarker(window); displayInfo(window); if(!hitNumberTimer.timeReached()) { float posX = body->GetPosition().x * SCALE -10; float posY = body->GetPosition().y * SCALE - BOX_SIZE/2 -15; text_damageTaken.setPosition(posX, posY); window.draw(text_damageTaken); } }