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);
}
}
