void extracted_orientation(Eigen::Vector3d current_position, std::string file_dir, std::string result_dir) {
std::string x, y, z, ax, ay, az;
std::ostringstream pose;
pose << "orientations_for_position_" << current_position(0) << "_" << current_position(1) << "_" << current_position(2);
std::string my_file_name = pose.str();
std::ofstream current_file;
current_file.open(result_dir+my_file_name+".csv");
//std::ifstream my_csv_file("/home/ghanim/git/baxter_workspace.csv");
std::ifstream my_csv_file(file_dir);
start_index.push_back(extracted_orientations.size());
while (std::getline(my_csv_file,x,',')) {
std::getline(my_csv_file, y, ',') ;
std::getline(my_csv_file, z, ',') ;
std::getline(my_csv_file, ax, ',') ;
std::getline(my_csv_file, ay, ',') ;
std::getline(my_csv_file, az, my_csv_file.widen('\n'));
Eigen::Vector3d current_point, current_orientation;
current_point << std::stod(x), std::stod(y), std::stod(z);
if(current_point == current_position) {
current_orientation << std::stod(ax), std::stod(ay), std::stod(az);
extracted_orientations.push_back(current_orientation);
current_file << current_orientation(0) << "," << current_orientation(1) << "," << current_orientation(2) << "\n";
}
}
current_file.close();
processed_points.push_back(current_position);
finish_index.push_back(extracted_orientations.size());
}
void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& message )
{
++messages_received_;
if( !validateFloats( *message ))
{
setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
return;
}
setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
if( last_used_message_ )
{
Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, last_used_message_->pose.pose.position.y, last_used_message_->pose.pose.position.z);
Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z);
Ogre::Quaternion last_orientation(last_used_message_->pose.pose.orientation.w, last_used_message_->pose.pose.orientation.x, last_used_message_->pose.pose.orientation.y, last_used_message_->pose.pose.orientation.z);
Ogre::Quaternion current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z);
if( (last_position - current_position).length() < position_tolerance_property_->getFloat() &&
(last_orientation - current_orientation).normalise() < angle_tolerance_property_->getFloat() )
{
return;
}
}
Arrow* arrow = new Arrow( scene_manager_, scene_node_, 0.8f, 0.05f, 0.2f, 0.2f );
transformArrow( message, arrow );
QColor color = color_property_->getColor();
arrow->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
float length = length_property_->getFloat();
Ogre::Vector3 scale( length, length, length );
arrow->setScale( scale );
arrows_.push_back( arrow );
last_used_message_ = message;
context_->queueRender();
}
