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