void Hand_filter::update(tf::Vector3 p, tf::Quaternion& q){
if(b_first){
p_filter_buffer.push_back(p);
q_filter_buffer.push_back(q);
if(p_filter_buffer.size() == p_filter_buffer.capacity()){
b_first = false;
ROS_INFO("====== hand filter ======");
// ROS_INFO("buffer full: %d",p_filter_buffer.size());
ROS_INFO("p: %f %f %f",p.x(),p.y(),p.z());
ROS_INFO("q: %f %f %f %f",q.x(),q.y(),q.z(),q.w());
k_position(0) = p.x();k_position(1) = p.y(); k_position(2) = p.z();
kalman_filter.Init(k_position);
q_tmp = q;
p_tmp = p;
}
}else{
/// Orientation filter
if(jumped(q,q_tmp,q_threashold)){
ROS_INFO("q jumped !");
q = q_tmp;
}
q_attractor(q,up);
q = q_tmp.slerp(q,0.1);
/// Position filter
if(!jumped(p,p_tmp,p_threashold)){
k_measurement(0) = p.x();
k_measurement(1) = p.y();
k_measurement(2) = p.z();
}else{
ROS_INFO("p jumped !");
k_measurement(0) = p_tmp.x();
k_measurement(1) = p_tmp.y();
k_measurement(2) = p_tmp.z();
}
kalman_filter.Update(k_measurement,0.001);
kalman_filter.GetPosition(k_position);
p.setValue(k_position(0),k_position(1),k_position(2));
q_tmp = q;
p_tmp = p;
}
}
void VPlayer::setTime(qint64 _time)
{
if(mp){
libvlc_media_player_set_time(mp,_time);
emit jumped(_time);
}
}
void Jumps::update(tf::Vector3& origin,tf::Quaternion& orientation){
if(bFirst){
origin_buffer.push_back(origin);
orientation_buffer.push_back(orientation);
if(origin_buffer.size() == origin_buffer.capacity()){
bFirst = false;
ROS_INFO("====== jump filter full ======");
}
}else{
origin_tmp = origin_buffer[origin_buffer.size()-1];
orientation_tmp = orientation_buffer[orientation_buffer.size()-1];
if(bDebug){
std::cout<< "=== jum debug === " << std::endl;
std::cout<< "p : " << origin.x() << "\t" << origin.y() << "\t" << origin.z() << std::endl;
std::cout<< "p_tmp: " << origin_tmp.x() << "\t" << origin_tmp.y() << "\t" << origin_tmp.z() << std::endl;
std::cout<< "p_dis: " << origin.distance(origin_tmp) << std::endl;
std::cout<< "q : " << orientation.x() << "\t" << orientation.y() << "\t" << orientation.z() << "\t" << orientation.w() << std::endl;
std::cout<< "q_tmp: " << orientation_tmp.x() << "\t" << orientation_tmp.y() << "\t" << orientation_tmp.z() << "\t" << orientation_tmp.w() << std::endl;
std::cout<< "q_dis: " << dist(orientation,orientation_tmp) << std::endl;
}
/// Position jump
if(jumped(origin,origin_tmp,origin_threashold)){
ROS_INFO("position jumped !");
origin = origin_tmp;
// exit(0);
}else{
origin_buffer.push_back(origin);
}
/// Orientation jump
if(jumped(orientation,orientation_tmp,orientation_threashold)){
ROS_INFO("orientation jumped !");
orientation = orientation_tmp;
//exit(0);
}else{
orientation_buffer.push_back(orientation);
}
}
}
void QPlayer::setTime(qint64 _time)
{
QMetaObject::invokeMethod(mp, "setPosition",
Qt::BlockingQueuedConnection,
Q_ARG(qint64, _time));
skipTimeChanged = true;
emit jumped(_time);
}
int main(int argc, char *argv[])
{
char *semi_big = NULL;
char *big = NULL;
char *small = NULL;
char *other_small = NULL;
int i;
int j;
semi_big = malloc (900000);
big = malloc (1000001);
free(semi_big);
free(big);
if (big[1000] > 0x0) jumped();
if (semi_big[1000] > 0x0) jumped();
small = malloc (10000);
free(small);
if (big[2000] > 0x0) jumped();
if (semi_big[2000] > 0x0) jumped();
big = NULL;
{
big = malloc (1000001);
free(big);
if (small[10] > 0x0) jumped();
if (big[10] > 0x0) jumped();
}
for (i = 0; i < 100; i++) {
other_small = malloc(10000);
for (j = 0; j < 10000; j++)
other_small[j] = 0x1;
}
if (small[10] > 0x0) jumped();
return 0;
}
QPlayer::QPlayer(QObject *parent) :
APlayer(parent)
{
ins = this;
setObjectName("QPlayer");
state = Stop;
manuallyStopped = false;
waitingForBegin = false;
skipTimeChanged = false;
mp = (new QPlayerThread(this))->getMediaPlayer();
mp->setVolume(Config::getValue("/Playing/Volume", 50));
connect<void(QMediaPlayer::*)(QMediaPlayer::Error)>(mp, &QMediaPlayer::error, this, [this](int error){
if ((State)mp->state() == Play){
manuallyStopped = true;
}
emit errorOccurred(error);
});
connect(mp, &QMediaPlayer::volumeChanged, this, &QPlayer::volumeChanged);
connect(mp, &QMediaPlayer::stateChanged, this, [this](int _state){
if (_state == Stop){
if (!manuallyStopped&&Config::getValue("/Playing/Loop", false)){
stateChanged(state = Loop);
play();
emit jumped(0);
}
else{
stateChanged(state = Stop);
emit reach(manuallyStopped);
}
manuallyStopped = false;
}
else{
manuallyStopped = false;
if (_state == Play&&state == Stop){
waitingForBegin = true;
}
else{
emit stateChanged(state = _state);
}
}
});
connect(mp, &QMediaPlayer::positionChanged, this, [this](qint64 time){
if (waitingForBegin&&time > 0){
waitingForBegin = false;
ARender::instance()->setMusic(!mp->isVideoAvailable());
emit stateChanged(state = Play);
emit begin();
}
if (!skipTimeChanged){
emit timeChanged(time);
}
else{
skipTimeChanged = false;
}
});
connect(mp, &QMediaPlayer::mediaChanged, this, [this](){
emit mediaChanged(getMedia());
});
connect(mp, &QMediaPlayer::playbackRateChanged, this, &QPlayer::rateChanged);
}
