TEST(TfListenerSingleton, testLookupTransformWithDuration){
boost::mutex mutex;
tf::TransformListener* tf_listener;
mutex.lock();
tf_listener = new tf::TransformListener(ros::Duration(30.0));
mutex.unlock();
std::string from_frame("base");
std::string to_frame("head");
tf::StampedTransform transform;
transform = jsk_recognition_utils::lookupTransformWithDuration(
/*listener=*/tf_listener,
/*to_frame=*/to_frame,
/*from_frame=*/from_frame,
/*time=*/ros::Time(),
/*duration=*/ros::Duration(1.0));
ASSERT_STREQ("base", transform.frame_id_.c_str());
ASSERT_STREQ("head", transform.child_frame_id_.c_str());
ASSERT_EQ(0, transform.getOrigin().getX());
ASSERT_EQ(0, transform.getOrigin().getY());
ASSERT_EQ(1, transform.getOrigin().getZ());
ASSERT_EQ(0, transform.getRotation().getX());
ASSERT_EQ(0, transform.getRotation().getY());
ASSERT_EQ(0, transform.getRotation().getZ());
ASSERT_EQ(1, transform.getRotation().getW());
}
void
debug_save_image( const image_buf &i, const std::string &fn, int64_t sampNum, const std::vector<std::string> &chans, const std::string &type, const media::metadata &options )
{
std::vector<media::track_description> tds;
tds.push_back( media::TRACK_VIDEO );
tds.back().rate( media::sample_rate( 1, 1 ) );
tds.back().offset( sampNum );
tds.back().duration( 1 );
for ( auto &o: options )
tds.back().set_option( o.first, o.second );
base::uri fnU( fn );
if ( ! fnU )
fnU.set_scheme( "file" );
media::container oc = media::writer::open( fnU, tds, options );
oc.video_tracks()[0]->store( sampNum, to_frame( i, chans, type ) );
std::cout << "Saved debug image to '" << fn << "', frame " << sampNum << std::endl;
}
void Game::mouse_released(sf::Vector2f coord) {
if (mouse_down) {
mouse_down = false;
int group, column, row;
int status = locate_card(coord, group, column, row);
if (valid_placement(status, group, column, row)) { //move cards to new location if valid
anim_move_card();
for (int i = 0; i < cursor.size(); i++) {
Position p = cursor[i].get_position_data();
p.loc.second = sf::Vector3i(group, column, row + i + 1);
p.coord.second = field_rect[group][column].getPosition();
if (group != HOME) {
p.coord.second.y += VERT_CARD_SPACING * (row + i + 1);
}
cursor[i].init_animation(STATE_ANIMATION_MOVING_CARD, p, to_frame(0.1f));
transit.push_back(cursor[i]);
}
cursor.clear();
} else {
anim_return_card();
}
}
}
void Game::mouse_pressed(sf::Vector2f coord) {
if (master_state == STATE_ANIMATION) {
skip = true;
while (master_state != STATE_PLAYING) {
update();
}
skip = false;
if (detailed_state != STATE_ANIMATION_FLIP_CARD && detailed_state != STATE_ANIMATION_MOVING_CARD) {
return;
}
}
int group, column, row;
int status = locate_card(coord, group, column, row);
if (status >= HOME && status <= PLAY_FIELD || status == DECK && column == 1) { //clicked some valid location on field
//check if clicking on flipped deck
if (status == DECK && row != field[DECK][1].size() - 1) {
return;
}
//click on a face up card
if (field[group][column][row].get_flip_state() == CARD_FACE_UP) { //add card(s) to cursor
mouse_down = true;
for (int i = row; i < field[group][column].size(); i++) {
Card c = field[group][column][i];
Position p = c.get_position_data();
p.coord.first = field[group][column][i].get_center();
p.loc.first = sf::Vector3i(group, column, i);
c.set_position_data(p);
cursor.push_back(c);
field[group][column].erase(field[group][column].begin() + i--);
mouse_moved(coord);
}
//check if ace is moved out of home area
if (cursor.size() == 1 && cursor.front().get_value() == 1) {
for (int i = 0; i < ace_locations.size(); i++) {
if (ace_locations[i] == cursor.front().get_suit()) {
ace_locations[i] = -1;
break;
}
}
}
} else { //card is face down, see if its at the front of the pile
if (field[group][column].size() - 1 == row) {
anim_flip_card();
field[group][column][row].init_animation(STATE_ANIMATION_FLIP_CARD, to_frame(0.3f));
transit.push_back(field[group][column][row]);
field[group][column].pop_back();
}
}
} else if (status == DECK || status == EMPTY_SPOT && group == DECK) { //if deck group was clicked
if (field[DECK][0].empty()) { //deck is empty, return deck contents
if (!field[DECK][1].empty()) {
anim_flip_deck();
for (int i = 0; i < field[DECK][1].size(); i++) {
field[DECK][1][i].set_flip_state(CARD_FACE_DOWN);
}
}
} else if (status == DECK) { //move and flip card
anim_move_card();
//recorrect the position of all previous cards
int flipped_counter = 1;
if (three_card_rule) {
flipped_counter = field[DECK][0].size() >= 3 ? field[DECK][1].size() - 3 : 0;
}
for (int i = flipped_counter; i < field[DECK][1].size(); i++) {
Card c = field[DECK][1][i];
if (c.get_center() == field_rect[DECK][1].getPosition()) continue;
Position p = c.get_position_data();
p.coord.second = field_rect[DECK][1].getPosition();
c.init_animation(STATE_ANIMATION_MOVING_CARD, p, to_frame(0.25f));
transit.push_back(c);
field[DECK][1].erase(field[DECK][1].begin() + i--);
}
//launch animation for moving card
flipped_counter = 1;
if (three_card_rule) {
flipped_counter = field[DECK][0].size() >= 3 ? 3 : field[DECK][0].size();
}
for (int i = 0; i < flipped_counter; i++) {
Card c = field[DECK][0].back();
c.set_flip_state(CARD_FACE_UP);
Position p = c.get_position_data();
p.coord.second = field_rect[DECK][1].getPosition();
p.coord.second.x += HORZ_CARD_SPACING * i;
p.loc.second = sf::Vector3i(DECK, 1, field[DECK][1].size());
c.init_animation(STATE_ANIMATION_MOVING_CARD, p, to_frame(0.25f));
transit.push_back(c);
field[DECK][0].pop_back();
}
}
}
}
void Game::update() {
if (master_state == STATE_ANIMATION) {
frame_counter++;
if (skip || frame_counter >= to_frame(frame_delay)) {
frame_counter = 0;
if (detailed_state == STATE_ANIMATION_INITIALIZING_DECK) {
if (card_counter <= 27) {
//destination of card
int col = col_tracker.x;
sf::Vector2f destination = field_rect[PLAY_FIELD][col].getPosition();
destination.y += VERT_CARD_SPACING * col_tracker.y;
int animation_type = STATE_ANIMATION_MOVING_CARD;
if (col == col_tracker.y) {
animation_type = STATE_ANIMATION_MOVE_AND_FLIP_CARD;
}
Position p(destination, sf::Vector3i(DECK, 0, field[DECK][0].size()), sf::Vector3i(PLAY_FIELD, col, col_tracker.y));
field[DECK][0].back().init_animation(animation_type, p, to_frame(0.3f));
transit.push_back(field[DECK][0].back());
field[DECK][0].pop_back();
card_counter++;
col_tracker.y++;
if (col_tracker.y > col_tracker.x) {
col_tracker.x++;
col_tracker.y = 0;
}
}
} else if (detailed_state == STATE_ANIMATION_RETURN_CARD) {
if (!cursor.empty()) {
Card c = cursor.front();
Position p = c.get_position_data();
p.coord.second = p.coord.first;
p.loc.second = p.loc.first;
c.init_animation(STATE_ANIMATION_MOVING_CARD, p, to_frame(0.2f));
transit.push_back(c);
cursor.erase(cursor.begin());
}
} else if (detailed_state == STATE_ANIMATION_FLIP_DECK) {
if (!field[DECK][1].empty()) {
Card c = field[DECK][1].back();
Position p = c.get_position_data();
p.coord.second = field_rect[DECK][0].getPosition();
p.loc.second = sf::Vector3i(DECK, 0, field[DECK][0].size());
c.init_animation(STATE_ANIMATION_MOVING_CARD, p, to_frame(0.3f));
transit.push_back(c);
field[DECK][1].pop_back();
}
} else if (detailed_state == STATE_ANIMATION_SOLVE_DECK) {
//locate card with lowest value on left most position
Position p(sf::Vector3i(-1, -1, -1));
int min_card_val = -1;
for (int i = 0; i < field[PLAY_FIELD].size(); i++) {
if (field[PLAY_FIELD][i].empty()) continue;
if (min_card_val == -1 || field[PLAY_FIELD][i].back().get_value() < min_card_val) {
p.loc.first = sf::Vector3i(PLAY_FIELD, i, field[PLAY_FIELD][i].size() - 1);
p.coord.first = field[PLAY_FIELD][i].back().get_center();
min_card_val = field[PLAY_FIELD][i].back().get_value();
}
}
//possibility that there are no cards on field
//check if we found a card
if (p.loc.first.x != -1) {
//card to move
Card c = field[PLAY_FIELD][p.loc.first.y].back();
//locate home location to move it
int col;
for (col = 0; col < field[HOME].size(); col++) {
//check if the column is empty
if (field[HOME][col].empty() && ace_locations[col] == -1) {
if (c.get_value() == 1) { //if we have an ace, place it
ace_locations[col] = c.get_suit();
break;
}
} else if (ace_locations[col] == c.get_suit()) {
//found pile of same suit
break;
}
}
//launch moving animation
p.loc.second = sf::Vector3i(HOME, col, field[HOME][col].size() - 1);
p.coord.second = field_rect[HOME][col].getPosition();
c.init_animation(STATE_ANIMATION_MOVING_CARD, p, to_frame(0.5f));
transit.push_back(c);
field[PLAY_FIELD][p.loc.first.y].pop_back();
}
}
}
//update aimations
for (int i = 0; i < transit.size(); i++) {
//check if we should skip the animation or not
if (skip) {
transit[i].finish();
} else {
transit[i].next();
}
//removed animations that are finished, add it to its new location
if (transit[i].animation_finish()) {
Position p = transit[i].get_position_data();
sf::Vector3i loc = p.loc.second;
field[loc.x][loc.y].push_back(transit[i]);
transit.erase(transit.begin() + i--);
//check if the animation sequence is over
if (transit.empty()) {
if (detailed_state == STATE_ANIMATION_INITIALIZING_DECK) {
if (card_counter > 27) {
master_state = STATE_PLAYING;
}
} else if (detailed_state == STATE_ANIMATION_FLIP_DECK) {
if (field[DECK][1].empty()) {
master_state = STATE_PLAYING;
}
} else if (detailed_state == STATE_ANIMATION_SOLVE_DECK) {
if (has_won()) {
master_state = STATE_PLAYING;
}
} else if (cursor.empty()) {
master_state = STATE_PLAYING;
}
//this is here because any card moving action requires animation
//during transition, cards are not all present on the field
if (!solvable && is_solvable()) {
solvable = true;
}
}
}
}
}
if (!won && has_won()) {
won = true;
}
}
