/**
* @name push_queue
*
* Add this state into the priority queue.
*/
void Wordrec::push_queue(HEAP *queue, STATE *state, FLOAT32 worst_priority,
FLOAT32 priority, bool debug) {
HEAPENTRY entry;
if (priority < worst_priority) {
if (SizeOfHeap (queue) >= MaxSizeOfHeap(queue)) {
if (debug) tprintf("Heap is Full\n");
return;
}
entry.Data = (char *) new_state (state);
num_pushed++;
entry.Key = priority;
HeapStore(queue, &entry);
}
}
WORD svas_rsc(VOID)
{
if (!get_file(STSASRES))
return(FALSE);
else
{
mouse_form(HGLASS);
write_files();
rvrt_files();
mouse_form(ARROW);
new_state(FILE_STATE);
rsc_title();
return(TRUE);
}
}
int main(int argc, char *argv[])
{
FILE *code_file;
int max_steps = 10000;
state_ptr s = new_state(MEM_SIZE);
mem_t saver = copy_reg(s->r);
mem_t savem;
int step = 0;
exc_t e = EXC_NONE;
if (argc < 2 || argc > 3)
usage(argv[0]);
code_file = fopen(argv[1], "r");
if (!code_file) {
fprintf(stderr, "Can't open code file '%s'\n", argv[1]);
exit(1);
}
if (!load_mem(s->m, code_file, 1)) {
printf("Exiting\n");
return 1;
}
savem = copy_mem(s->m);
if (argc > 2)
max_steps = atoi(argv[2]);
for (step = 0; step < max_steps && e == EXC_NONE; step++)
e = step_state(s, stdout);
printf("Stopped in %d steps at PC = 0x%x. Exception '%s', CC %s\n",
step, s->pc, exc_name(e), cc_name(s->cc));
printf("Changes to registers:\n");
diff_reg(saver, s->r, stdout);
printf("\nChanges to memory:\n");
diff_mem(savem, s->m, stdout);
free_state(s);
free_reg(saver);
free_mem(savem);
return 0;
}
w_rc_t
bf_prefetch_thread_t::request(
const lpid_t& pid,
latch_mode_t mode
)
{
FUNC(bf_prefetch_thread_t::request);
w_assert3(mode != LATCH_NL); // MUST latch the page
CRITICAL_SECTION(cs, _prefetch_mutex);
if(get_error()) {
w_rc_t rc(_fix_error.delegate());
return rc;
}
int i = _f;
bf_prefetch_thread_t::frame_info &inf = _info[i];
DBGTHRD(<<"request! i=" << i
<< " pid " << pid
<< " mode " << int(mode)
<< " old status " << int(inf._status)
);
w_assert3(inf._status == pf_init);
// There should always be one available -- at least
// when used with scan TODO -- make more general
INC_TSTAT(bf_prefetch_requests);
/* Assert that we haven't got a frame read from disk
* and never used (fetched)
*/
inf._pid = pid;
inf._mode = mode;
new_state(i, pf_request);
w_assert3(inf._status == pf_requested);
cs.exit();
DBGTHRD(<< "released mutex; signalling...");
DO_PTHREAD(pthread_cond_signal(&_activate));
DBGTHRD(<< "returning from request");
return _fix_error;
}
/**
* Transition model
*
* Find the result of applying a move to a state
*
* @param state a state
* @param action an action
* @return a new state, which is the result of applying an action to state, if
* action is invalid or no move is possible return null.
*/
struct State * result( const struct State * state, const struct Move * action )
{
char new_board[SIZE][SIZE];
/* copy old board */
for( int i = 0; i < SIZE; i++ )
for( int j = 0; j < SIZE; j++ )
new_board[ i ][ j ] = state->board[ i ][ j ];
/* validate move */
if( validate_action( state, action ) )
{
/* get resulting board state
* by removing all pieces between start and end action
*/
if( action->start_row == action->end_row )
{
/* blank column */
if( action->start_col < action->end_col )
for( int i = action->start_col; i < action->end_col; i++ )
new_board[ action->start_row ][ i ] = 'O';
else
for( int i = action->start_col; i > action->end_col; i-- )
new_board[ action->start_row ][ i ] = 'O';
}
else
{
/* blank row */
if( action->start_row < action->end_row )
for( int i = action->start_row; i < action->end_row; i++ )
new_board[ i ][ action->start_col ] = 'O';
else
for( int i = action->start_row; i > action->end_row; i-- )
new_board[ i ][ action->start_col ] = 'O';
}
/* set current piece */
new_board[ action->end_row ][ action->end_col ] = state->player;
/* return changed board */
return new_state( new_board, opposite_player( state->player ) );
}
return NULL;
}
static void
build_state_for(Grammar *g, State *s, Elem *e) {
int j;
Item *i;
State *ss = NULL;
for (j = 0; j < s->items.n; j++) {
i = s->items.v[j];
if (i->kind != ELEM_END && i->kind == e->kind &&
i->e.term_or_nterm == e->e.term_or_nterm)
{
if (!ss) ss = new_state();
insert_item(ss, next_elem(i));
}
}
if (ss)
add_goto(s, build_closure(g, ss), e);
}
void StateStack::applyDelayedOrders()
{
for (auto &itr : delayed_orders)
{
if (itr.action == Action::Pop)
{
if (!state_stack.empty()) state_stack.pop();
}
else if (itr.action == Action::Push)
{
std::unique_ptr<State> new_state(createState(itr.id));
new_state->setTransitionState(Transitions::OnEnter);
state_stack.push(std::move(new_state));
}
}
delayed_orders.clear();
}
NORET
bf_prefetch_thread_t:: ~bf_prefetch_thread_t()
{
FUNC(bf_prefetch_thread_t::~bf_prefetch_thread_t);
if(_info) {
CRITICAL_SECTION(cs, _prefetch_mutex);
int j;
for(j=0; j<_n; j++) {
new_state(j, pf_destructor);
}
cs.exit();
delete[] _info;
_info = 0;
}
DO_PTHREAD(pthread_mutex_destroy(&_prefetch_mutex));
DO_PTHREAD(pthread_cond_destroy(&_activate));
}
/**
* Human player goes second
*
* @param game_state the current state of the game
* @return a new game state
*/
struct State * human_player_second( struct State * game_state )
{
char input[ INPUT_SIZE ];
struct State * state;
struct Move * move;
/* must remove piece orthoganally adjacent */
printf( "Please remove a piece of your color adjacent to the piece removed\n" );
/* get input */
do
{
printf( "Please enter a move: " );
fgets( input, INPUT_SIZE, stdin );
move = translate_first_in_move( input );
/* make sure piece is valid */
if( move != NULL )
{
if( validate_second_in_move( game_state, move ) == 0 )
{
/* invalid move */
Free( move, sizeof( struct Move ) );
}
else
break;
}
}
while( 1 );
printf( "Move chosen: " );
print_single_move( move );
/* create a new state */
state = new_state( game_state->board, opposite_player( game_state->player ) );
/* apply move */
state->board[ move->start_row ][ move->start_col ] = 'O';
return state;
}
struct GameNode * human_player_first( struct GameNode * game_state )
{
char input[ INPUT_SIZE ];
struct State * state;
struct Move * move;
struct GameNode * new_game_state;
printf( "To begin game, please remove one piece from the board.\n" );
printf( "Valid moves are from the 4 center squares, or one of the corners.\n" );
printf( "Piece removed must be a piece of your color!\n" );
/* get input */
do
{
printf( "Please enter a move: " );
fgets( input, INPUT_SIZE, stdin );
move = translate_first_in_move( input );
/* make sure piece is of players color */
if( game_state->state->board[ move->start_row ][ move->start_col ] != 'B' )
{
/* invalid move */
Free( move, sizeof( struct Move ) );
}
}
while( move == NULL );
/* create a new state */
state = new_state( game_state->state->board, opposite_player( game_state->state->player ) );
/* apply move */
state->board[ move->start_row ][ move->start_col ] = 'O';
print_move( move );
/* create a new game node */
new_game_state = new_game_node( state, game_state );
/* add new node to parent node */
add_child_game_node( game_state, new_game_state );
return new_game_state;
}
/**
* i is an index into the array of temporary states
*/
int save_state (int i) {
int p; // p is the
int k; // k gets the custom codepoint of state 'i's last transition
int h,hc;
hc=hashcode(i,(k=codes[owf[i]]));
h=abs(hc)%HASHS;
while ((p = states_hash_table[h]) != 0 && !state_cmp(p,i)) h=(h+HASHC2)%HASHS;
if (p==0) { // we did not find an equivalent state, but an empty slot
if (HASHS - states_count < HASHS / 10 ) {
hashresize();
h=abs(hc)%HASHS; // re-compute the hash slot to start with
while (states_hash_table[h] != 0) h=(h+HASHC2)%HASHS; // re-find an open slot
}
states_hash_table[h] = new_state(i,k);
hash_codes[h] = hc;
states_count++;
}
return states_hash_table[h];
}
single_undo_item *single_undo_item_builder::create_undo ()
{
single_undo_item *single_item = new single_undo_item (m_items_container);
for (auto &pair : m_changed_items)
{
int id = pair.first;
abstract_state_t *old_state = pair.second.get ();
undoable *item = m_items_container->get_item (id);
unique_ptr<abstract_state_t> new_state (item ? item->create_state () : nullptr);
if (!old_state && !new_state)
continue;
abstract_state_diff_t *diff = old_state ? old_state->create_diff (old_state, new_state.get ()) : new_state->create_diff (old_state, new_state.get ());
single_item->add_item_diff (id, diff);
}
m_changed_items.clear ();
return single_item;
}
action_state_t* action_t::get_state( const action_state_t* other )
{
action_state_t* s = nullptr;
if ( state_cache )
{
s = state_cache;
state_cache = s -> next;
}
else
s = new_state();
s -> action = this;
if ( ! other )
s -> initialize();
else
s -> copy_state( other );
return s;
}
void ds1204_device::device_start()
{
new_state( STATE_STOP );
m_dqr = DQ_HIGH_IMPEDANCE;
memset( m_command, 0, sizeof( m_command ) );
memset( m_compare_register, 0, sizeof( m_compare_register ) );
save_item( NAME( m_rst ) );
save_item( NAME( m_clk ) );
save_item( NAME( m_dqw ) );
save_item( NAME( m_dqr ) );
save_item( NAME( m_state ) );
save_item( NAME( m_bit ) );
save_item( NAME( m_command ) );
save_item( NAME( m_compare_register ) );
save_item( NAME( m_unique_pattern ) );
save_item( NAME( m_identification ) );
save_item( NAME( m_security_match ) );
save_item( NAME( m_secure_memory ) );
}
LOCAL WORD opn_rsc(VOID)
{
mouse_form(HGLASS);
if (open_files(FALSE))
{
if (read_files())
{
new_state(FILE_STATE);
rsc_title();
}
}
else
{
mouse_form(ARROW);
return(FALSE);
}
rcs_trpan = 0;
redo_trees();
mouse_form(ARROW);
return(TRUE);
}
void Tresidual<T>::jacobian( Tmatrix<T>& J, const Tvector<T>& x_k ) const
{
Tvector<T> new_state( ORDER, 0.0 ); // New state vector
Tvector<T> f_eval( ORDER, 0.0 ); // Function evaluation
Tvector<T> f_at_new_state( ORDER, 0.0); // Function evaluation at the new state
for ( std::size_t alpha = 0; alpha < ORDER; ++alpha ) // Row index
{
for ( std::size_t beta = 0; beta < ORDER; ++beta ) // Column index
{
Tvector<T> delta(ORDER,0.0); // Perturbation vector
delta[ beta ] += DELTA;
new_state = x_k + delta; // Perturb the known value
// Perurbed function evaluation
residual_function( f_at_new_state, new_state );
residual_function( f_eval, x_k );
// Approximate the entry in the Jacobian
J(alpha,beta) = ( f_at_new_state[ alpha ] - f_eval[ alpha ] ) / DELTA;
}
}
}
state_unit_t* state_add_terminal(state_unit_t *st_un)
{
edge_t *e = NULL;
state_t *st = new_state(1, 0);
if (!st)
goto err;
set_state_terminal(st);
if (!(e = new_epsilon_edge()))
goto err;
state_add_inedge(st, e);
state_add_outedge(st_un->tail, e);
st_un->tail = st;
return st_un;
err:
state_destory(st);
free(e);
return NULL;
}
void simulator_ctt::execute_assign(
statet &state,
const program_formulat::formula_goto_programt::instructiont &instruction)
{
statet new_state(state);
new_state.detatch();
for(program_formulat::assignst::const_iterator
it=instruction.code.assigns.begin();
it!=instruction.code.assigns.end();
it++)
if(it->in_use)
{
assert(it->variable<program_formula.variables.size());
new_state.data_w().set_var(
it->variable,
0,
instantiate(state, 0, it->value));
}
// do constraint
formulat instantiated_constraint=
instantiate(
state,
new_state,
0,
instruction.code.constraint);
new_state.data_w().guard=
formula_container.gen_and(
state.data_w().guard,
instantiated_constraint);
state.swap(new_state);
}
void su_fork(su_state *s, int narg) {
value_t v;
su_state *ns;
narg++;
v.type = SU_BOOLEAN;
su_thread_indisposable(s);
spin_lock(&s->msi->thread_pool_lock);
su_thread_disposable(s);
ns = new_state(s);
if (!ns) {
s->stack_top -= narg;
v.obj.b = 0;
push_value(s, &v);
spin_unlock(&s->msi->thread_pool_lock);
return;
}
ns->interrupt = 0x0;
ns->narg = narg - 1;
ns->pc = 0xffff;
ns->string_builder = NULL;
ns->errtop = ns->ferrtop = -1;
ns->stack[SU_GLOBAL_INDEX] = s->stack[SU_GLOBAL_INDEX];
su_assert(s, !thread_init(&thread_boot, (void*)ns), "Could not create thread!");
s->stack_top -= narg;
v.obj.b = 1;
push_value(s, &v);
spin_unlock(&s->msi->thread_pool_lock);
}
TEST(LoadingAndFK, SimpleRobot)
{
static const std::string MODEL1 =
"<?xml version=\"1.0\" ?>"
"<robot name=\"myrobot\">"
"<link name=\"base_link\">"
" <inertial>"
" <mass value=\"2.81\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.099 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision name=\"my_collision\">"
" <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"</robot>";
static const std::string MODEL1_INFO =
"Model myrobot in frame odom_combined, of dimension 3\n"
"Joint values bounds:\n"
" base_joint/x [DBL_MIN, DBL_MAX]\n"
" base_joint/y [DBL_MIN, DBL_MAX]\n"
" base_joint/theta [-3.14159, 3.14159]\n"
"Available groups: \n"
" base (of dimension 3):\n"
" joints:\n"
" base_joint\n"
" links:\n"
" base_link\n"
" roots:\n"
" base_joint";
static const std::string SMODEL1 =
"<?xml version=\"1.0\" ?>"
"<robot name=\"myrobot\">"
"<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
"<group name=\"base\">"
"<joint name=\"base_joint\"/>"
"</group>"
"</robot>";
boost::shared_ptr<urdf::ModelInterface> urdfModel = urdf::parseURDF(MODEL1);
boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model());
srdfModel->initString(*urdfModel, SMODEL1);
planning_models::KinematicModelPtr model(new planning_models::KinematicModel(urdfModel, srdfModel));
planning_models::KinematicState state(model);
EXPECT_EQ((unsigned int)3, state.getVariableCount());
state.setToDefaultValues();
const std::vector<planning_models::KinematicState::JointState*>& joint_states = state.getJointStateVector();
EXPECT_EQ((unsigned int)1, joint_states.size());
EXPECT_EQ((unsigned int)3, joint_states[0]->getVariableValues().size());
std::stringstream ssi;
model->printModelInfo(ssi);
EXPECT_TRUE(sameStringIgnoringWS(MODEL1_INFO, ssi.str())) << ssi.str();
std::map<std::string, double> joint_values;
joint_values["base_joint/x"] = 10.0;
joint_values["base_joint/y"] = 8.0;
//testing incomplete state
std::vector<std::string> missing_states;
state.setStateValues(joint_values, missing_states);
ASSERT_EQ(missing_states.size(), 1);
EXPECT_EQ(missing_states[0], std::string("base_joint/theta"));
joint_values["base_joint/theta"] = 0.0;
state.setStateValues(joint_values, missing_states);
ASSERT_EQ(missing_states.size(), 0);
EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().x(), 1e-5);
EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().y(), 1e-5);
EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().z(), 1e-5);
//making sure that values get copied
planning_models::KinematicState new_state(state);
EXPECT_NEAR(10.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().translation().x(), 1e-5);
EXPECT_NEAR(8.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().translation().y(), 1e-5);
EXPECT_NEAR(0.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().translation().z(), 1e-5);
const std::map<std::string, unsigned int>& ind_map = model->getJointVariablesIndexMap();
std::vector<double> jv(state.getVariableCount(), 0.0);
jv[ind_map.at("base_joint/x")] = 10.0;
jv[ind_map.at("base_joint/y")] = 8.0;
jv[ind_map.at("base_joint/theta")] = 0.0;
state.setStateValues(jv);
EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().x(), 1e-5);
EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().y(), 1e-5);
EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().z(), 1e-5);
}
static int
get_state(int symbol)
{
int key;
short *isp1;
short *isp2;
short *iend;
core *sp;
int found;
int n;
#ifdef TRACE
fprintf(stderr, "Entering get_state(%d)\n", symbol);
#endif
isp1 = kernel_base[symbol];
iend = kernel_end[symbol];
n = iend - isp1;
key = *isp1;
assert(0 <= key && key < nitems);
sp = state_set[key];
if (sp)
{
found = 0;
while (!found)
{
if (sp->nitems == n)
{
found = 1;
isp1 = kernel_base[symbol];
isp2 = sp->items;
while (found && isp1 < iend)
{
if (*isp1++ != *isp2++)
found = 0;
}
}
if (!found)
{
if (sp->link)
{
sp = sp->link;
}
else
{
sp = sp->link = new_state(symbol);
found = 1;
}
}
}
}
else
{
state_set[key] = sp = new_state(symbol);
}
return (sp->number);
}
void RTAStar::plan(const Eigen::VectorXd &q_start, const KDL::Frame &x_goal, std::list<Eigen::VectorXd > &path_q, MarkerPublisher &markers_pub) {
path_q.clear();
V_.clear();
E_.clear();
setGoal(x_goal);
std::vector<double > tmp_vec;
tmp_vec.resize(transform_delta_vec_.size(), 0.0);
int q_new_idx_ = 0;
{
RTAStarState state_start( transform_delta_vec_.size() );
state_start.h_value_ = 0.0;
state_start.q_ = q_start;
state_start.dq_.resize(ndof_);
state_start.dq_.setZero();
state_start.parent_idx_ = -1;
// state_start.T_B_E_ = KDL::Frame(KDL::Vector(q_start(0), q_start(1), q_start(2)));
kin_model_->calculateFk(state_start.T_B_E_, effector_name_, q_start);
V_.insert(std::make_pair(0, state_start));
}
int current_node_idx = 0;
while (true) {
std::map<int, RTAStarState >::iterator v_it = V_.find(current_node_idx);
if (v_it == V_.end()) {
std::cout << "ERROR: RTAStar::plan v_it == V_.end() " << current_node_idx << std::endl;
return;
}
if ((v_it->second.T_B_E_.p - x_goal.p).Norm() < 0.08 && v_it->second.q_.innerSize() == ndof_) {
std::cout << "goal reached" << std::endl;
while (true) {
path_q.push_front(v_it->second.q_);
current_node_idx = v_it->second.parent_idx_;
if (current_node_idx < 0) {
break;
}
v_it = V_.find(current_node_idx);
if (v_it == V_.end()) {
std::cout << "ERROR: v_it == V_.end() " << current_node_idx << std::endl;
}
}
return;
}
// From a given current state, the neighbouring states are generated
for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) {
// the neighbour state was not yet visited from the current node
if (v_it->second.neighbour_nodes_[tr_idx] == -1) {
// create a new state and add it to the graph
RTAStarState new_state( transform_delta_vec_.size() );
new_state.T_B_E_ = transform_delta_vec_[tr_idx] * v_it->second.T_B_E_;
// check if this neighbour state already exists
int state_idx = -1;//checkIfStateExists(new_state.T_B_E_);
if (state_idx == -1) {
// create a new state
q_new_idx_++;
new_state.h_value_ = getCostH(new_state.T_B_E_);
new_state.parent_idx_ = v_it->first;
new_state.parent_tr_idx_ = tr_idx;
v_it->second.neighbour_nodes_[tr_idx] = q_new_idx_;
V_.insert(std::make_pair(q_new_idx_, new_state));
markers_pub.addSinglePointMarker(q_new_idx_, new_state.T_B_E_.p, 0, 1, 0, 1, 0.05, "world");
}
else {
// add existing state to neighbour list
v_it->second.neighbour_nodes_[tr_idx] = state_idx;
}
}
}
// The heuristic function, augmented by lookahead search, is applied to each,
// and then the cost of the edge to each neighbouring state is added to this value,
// resulting in an f value for each neighbour of the current state.
bool validTransitionExists = false;
for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) {
int neighbour_idx = v_it->second.neighbour_nodes_[tr_idx];
if (neighbour_idx >= 0) {
std::map<int, RTAStarState >::const_iterator n_it = V_.find(neighbour_idx);
if (n_it == V_.end()) {
std::cout << "ERROR: RTAStar::plan n_it == V_.end() " << neighbour_idx << std::endl;
return;
}
// calculate the lookahead heuristics for each neighbour and add the cost from current state to neighbour
double cost_h = lookahead(n_it->second.T_B_E_, 5);
tmp_vec[tr_idx] = cost_h + getCostLine(v_it->second.T_B_E_, n_it->second.T_B_E_);
std::cout << " cost_h " << tr_idx << " " << cost_h << std::endl;
validTransitionExists = true;
}
else {
tmp_vec[tr_idx] = 10000.0;
}
}
if (!validTransitionExists) {
// remove the current node
std::cout << " no possible transition " << v_it->first << std::endl;
std::map<int, RTAStarState >::iterator parent_it = V_.find(v_it->second.parent_idx_);
if (parent_it == V_.end()) {
std::cout << "no parent node" << std::endl;
return;
}
parent_it->second.neighbour_nodes_[v_it->second.parent_tr_idx_] = -2;
markers_pub.addSinglePointMarker(v_it->first, v_it->second.T_B_E_.p, 0, 0, 0, 1, 0.05, "world");
V_.erase(current_node_idx);
current_node_idx = parent_it->first;
markers_pub.publish();
ros::spinOnce();
continue;
}
// The node with the minimum f value is chosen for the new current state and a move to that state
// is executed. At the same time, the next best f value is stored at the previous current state.
// get the two smallest values
double min1 = -1.0, min2 = -1.0;
int min1_idx = -1;
for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) {
if (min1 < 0 || min1 > tmp_vec[tr_idx]) {
min1 = tmp_vec[tr_idx];
min1_idx = tr_idx;
}
}
for (int tr_idx = 0; tr_idx < transform_delta_vec_.size(); tr_idx++) {
if (tr_idx == min1_idx) {
continue;
}
if (min2 < 0 || min2 > tmp_vec[tr_idx]) {
min2 = tmp_vec[tr_idx];
}
}
std::cout << "current_node_idx " << current_node_idx << " min: " << min1 << " " << min2 << std::endl;
// execute the move
int next_node_idx = v_it->second.neighbour_nodes_[min1_idx];
if (next_node_idx < 0) {
std::cout << "ERROR: next_node_idx < 0: " << next_node_idx << std::endl;
return;
}
std::map<int, RTAStarState >::iterator n_it = V_.find(next_node_idx);
// std::cout << n_it->second.T_B_E_.p[0] << " " << n_it->second.T_B_E_.p[1] << " " << n_it->second.T_B_E_.p[2] << std::endl;
// check if the next state is possible
if (!v_it->second.simulated_nodes_[min1_idx]) {
// simulate
n_it->second.q_.resize(ndof_);
n_it->second.dq_.resize(ndof_);
KDL::Frame nT_B_E;
std::list<KDL::Frame > path_x;
std::list<Eigen::VectorXd > path_q;
KDL::Frame T_B_E(n_it->second.T_B_E_);
bool col_free = collisionFree(v_it->second.q_, v_it->second.dq_, v_it->second.T_B_E_, n_it->second.T_B_E_, 0, n_it->second.q_, n_it->second.dq_, nT_B_E, &path_x, &path_q);
int similar_state = checkIfStateExists(n_it->first, n_it->second);
if (!col_free || similar_state >=0) {
n_it->second.h_value_ = 100.0;
if (!col_free) {
std::cout << " invalid state change, removing node " << next_node_idx << std::endl;
}
else {
std::cout << " joining states " << next_node_idx << " to " << similar_state << std::endl;
}
V_.erase(next_node_idx);
v_it->second.neighbour_nodes_[min1_idx] = -2;
markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 0, 0, 1, 0.05, "world");
markers_pub.publish();
ros::spinOnce();
continue;
}
v_it->second.simulated_nodes_[min1_idx] = true;
}
/* int similar_state = checkIfStateExists(n_it->first, n_it->second);
if (similar_state >= 0) {
// TODO: possible infinite loop!
// join states
v_it->second.neighbour_nodes_[min1_idx] = similar_state;
V_.erase(next_node_idx);
std::cout << "joining states: " << next_node_idx << " to " << similar_state << std::endl;
next_node_idx = similar_state;
}
*/
/*
if (!col_free) {isPoseValid(n_it->second.T_B_E_)) {
n_it->second.h_value_ = 10.0;
markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 0, 0, 1, 0.05, "world");
markers_pub.publish();
ros::spinOnce();
std::cout << " invalid pose" << std::endl;
continue;
}
*/
double min_h = 10000.0;
int min_idx = -1;
for (std::map<int, RTAStarState >::const_iterator v2_it = V_.begin(); v2_it != V_.end(); v2_it++) {
if (v2_it->second.h_value_ < min_h && v2_it->second.q_.innerSize() == ndof_) {
min_h = v2_it->second.h_value_;
min_idx = v2_it->first;
}
// markers_pub.addVectorMarker(v2_it->first+6000, v2_it->second.T_B_E_.p + KDL::Vector(0,0,0.05), v2_it->second.T_B_E_.p + KDL::Vector(0,0,0.05 + v2_it->second.h_value_*0.1), 0, 0.7, 0, 0.5, 0.01, "world");
}
next_node_idx = min_idx;
markers_pub.addSinglePointMarker(v_it->first, v_it->second.T_B_E_.p, 1, 0, 0, 1, 0.05, "world");
markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 0, 1, 1, 0.05, "world");
markers_pub.publish();
ros::spinOnce();
// getchar();
markers_pub.addSinglePointMarker(v_it->first, v_it->second.T_B_E_.p, 0, 1, 0, 1, 0.05, "world");
markers_pub.addSinglePointMarker(n_it->first, n_it->second.T_B_E_.p, 0, 1, 0, 1, 0.05, "world");
// save the second minimum value
v_it->second.h_value_ = min2;
std::cout << "changing state to " << next_node_idx << std::endl;
// go to the next state
current_node_idx = next_node_idx;
// getchar();
}
}
int get_command(pState state){
//magic len command subcommand data checkval
//message is len + 8 (2 magic, 2 len, 4 checkval)
//magic = 0xa55a, short len, short command, var subcommand, var data, int checkval
// unsigned char *buf = malloc(MAX_MESSAGE_LEN+8);
unsigned char buf[MAX_MESSAGE_LEN+8];
unsigned int checkval = 0;
unsigned int calc_checkval = 0;
unsigned short magic = 0;
unsigned short len = 0;
unsigned short command = 0xaa;
unsigned short subcommand = 0xffff;
unsigned short sensorID = 0xffff;
unsigned int sensor_address = 0xffff;
unsigned int coefficient = 0xffff;
unsigned short temp = 0xffff;
unsigned short numSteps = 0xffff;
unsigned short *tempP = NULL;
unsigned short seconds = 0;
pStep steps = NULL;
int ret = -1;
unsigned int fw = 0;
bzero(buf,MAX_MESSAGE_LEN+8);
//get magic
get_bytes(buf ,2);
magic = get_short(buf);
if (magic != MAGIC){
prime_buf(buf);
send(buf,12);
//free(buf);
return 1;
}
//get len
get_bytes(buf+2,2);
len = get_short(buf+2);
//check len
if (len > MAX_MESSAGE_LEN){
prime_buf(buf);
send(buf,12);
//free(buf);
return 1;
}
//get the rest of message except checkval
get_bytes(buf+4, len);
//get checkval
checkval = get_int(buf+len);
//compare to calculated from message
calc_checkval = check_val( buf, len);
if ( checkval != calc_checkval ){
//bad check_val
prime_buf(buf);
send(buf,12);
//free(buf);
return 1;
}
//get command
command = get_short(buf+4);
switch (command) {
case 1:{ //set power state
subcommand = get_short(buf+6);
prime_buf(buf);
if (subcommand == 0x0000){
power_off(state);
}
if (subcommand == 0x0001){
power_on(state);
}
if (subcommand > 0x0001){
//bad subcommand
}
send(buf, 12);
break;
}
case 2:{//set temp
temp = get_short(buf+6);
prime_buf(buf);
if (set_temp(state,temp) == 2){
buf[6] = 1;
}
send(buf, 12);
break;
}
case 3:{ //add sensor
sensorID = get_short(buf+6);
sensor_address = get_int(buf+8);
coefficient = get_int(buf+12);
unsigned int sensorTemp = get_int(buf+16);
//check count < 10 buf[6] = 0x08, sensor_ID is unique/not in use buf[6] = 0x07,
ret = add_sensor(state, sensorID, sensor_address, coefficient, sensorTemp);
prime_buf(buf);
if (ret == 2){buf[6]=0x07;}
if (ret == 3){buf[6]=0x08;}
send(buf, 12);
break;
}
case 4:{ //remove sensor
sensorID = get_short(buf+6);
ret = remove_sensor(state,sensorID);
prime_buf(buf);
if (ret == 1){buf[6]=0x06;}
send(buf, 12);
break;
}
case 5:{ //set smoke sensor
subcommand = get_short(buf+6);
prime_buf(buf);
if (subcommand == 0x0000){
smoke_off(state);
}
if (subcommand == 0x0001){
smoke_on(state);
}
if (subcommand > 0x0001){
//bad subcommand
//no response
}
send(buf, 12);
break;
}
case 6:{ //set program
numSteps = get_short(buf+6);
steps = (pStep)(buf+8) ;
ret = add_steps(state,numSteps,steps);
prime_buf(buf);
if (ret == 3){buf[6]=3;}
if (ret == 2){buf[6]=2;}
if (ret == 1){buf[6]=1;}
send(buf, 12);
break;
}
case 7:{//get program
prime_buf(buf);
unsigned int lenz = 0;
unsigned int program[30];
bzero(program,120);
buf[4]=1;
buf[6]=7;
get_program(state, &lenz, program);
lenz = lenz*3*sizeof(int);
cgc_memcpy(buf+8,&lenz,sizeof(int));
cgc_memcpy(buf+12,program,lenz);
send(buf,lenz + 12);
break;
}
case 8:{//get status
prime_buf(buf);
buf[4]=1;
buf[6]=8;
unsigned int status[6];
int len = 24;
ret = get_status(state,status);
cgc_memcpy(buf+8,&len,sizeof(int));
cgc_memcpy(buf+12,status,24);
send(buf,36);
break;
}
case 9:{//simulate seconds
seconds = get_short(buf+6);
prime_buf(buf);
unsigned int bufsize = 12;
unsigned int histSize = 0;
ret = simulate_seconds(state, seconds);
unsigned int currentTime = state->currentTime;
unsigned int setTemp = state->setTemp;
if (ret == 0 ){
buf[6] = 9;
cgc_memcpy(buf+8, ¤tTime ,sizeof(int));
send(buf,12);
}
if(ret == 2){
buf[4] = 1;
buf[6] = 0xc;
histSize = state->historyPosition*sizeof(int);
unsigned int *pHistoryList = state->history;
unsigned int historyListSize = state->historyPosition;
unsigned int ambientTemp = state->ambientTemp;
new_state(state);
if (historyListSize > 0){
cgc_memcpy(buf+8, &historyListSize, sizeof(historyListSize));
cgc_memcpy(buf+12, pHistoryList, histSize);
bufsize = histSize + 12;
}
cgc_memcpy(buf+bufsize, &ambientTemp, sizeof(unsigned int));
bufsize+=4;
cgc_memcpy(buf+bufsize, &setTemp, sizeof(unsigned int));
bufsize+=4;
send(buf, bufsize);
//new_state(state);
}
break;
}
case 0xa:{ //validate firmware
unsigned int fw = validate_fw(state);
prime_buf(buf);
buf[4]=1;
buf[6]=0xa;
buf[8]=4;
cgc_memcpy(buf+12, &fw,sizeof(int) );
send(buf, 16);
break;
}
case 0xb:{//cgc_read sensor list
prime_buf(buf);
int len = 0;
buf[4]=1;
buf[6]=0xb;
len = state->sensorCount * (sizeof(int)*4);
//buff is filled with sensor bytes
unsigned int sensorList[40*sizeof(int)];
get_sensors(state,sensorList);
cgc_memcpy(buf+8,&len,sizeof(int));
cgc_memcpy(buf+12,sensorList,len);
send(buf, len+12);
break;
}
case 0xc:{//set ambient temp
int ambientTemp = get_signed_int(buf+6);
prime_buf(buf);
if (set_ambient_temp(state,ambientTemp) == 2){
buf[6] = 1;
}
send(buf, 12);
break;
}
case 0xff:{
//free(buf);
exit_normal();
break;
}
default:{
//bad command
prime_buf(buf);
buf[6]=5;
send(buf,12);
break;
}
}
// free(buf);
return 0;
}
static void uni_call(SOCKET *sock,unsigned char mid)
{
char buffer[MAX_ATM_ADDR_LEN+1];
int error;
switch (mid) {
case ATM_MSG_STATUS: /* 5.5.6.12 */
{
CALL_STATE state;
/*
* NOTE: T322 isn't implemented yet, but when it is, make sure
* to only stop it on STATUS iff the cause is
* ATM_CV_RESP_STAT_ENQ. Supplementary services break if
* you stop on any STATUS.
*/
state = q_fetch(&in_dsc,QF_call_state);
if (state == cs_null) break; /* clear call */
if (sock->call_state == cs_rel_req || sock->call_state ==
cs_rel_ind) return;
if (state != sock->call_state)
diag(COMPONENT,DIAG_WARN,"STATUS %s received in state %s",
cs_name[state],cs_name[sock->call_state]);
}
return;
default:
;
}
switch (mid) {
case ATM_MSG_CALL_PROC: /* CONNECTING, WAIT_REL, REL_REQ */
if (sock->state == ss_wait_rel || sock->state == ss_rel_req) {
send_status(sock->sig,sock,0,ATM_CV_INCOMP_MSG,
ATM_MSG_CALL_PROC);
return;
}
if (sock->state != ss_connecting) break;
/* check for 2nd CALL_PROC @@@ */
STOP_TIMER(sock);
if (q_present(&in_dsc,QG_conn_id)) {
int vpci;
vpci = q_fetch(&in_dsc,QF_vpi);
sock->pvc.sap_family = AF_ATMPVC;
sock->pvc.sap_addr.itf = get_itf(sock->sig,&vpci);
sock->pvc.sap_addr.vpi = vpci;
sock->pvc.sap_addr.vci = q_fetch(&in_dsc,QF_vci);
diag(COMPONENT,DIAG_DEBUG,"ITF.VPI.VCI: %d.%d.%d",
sock->pvc.sap_addr.itf,sock->pvc.sap_addr.vpi,
sock->pvc.sap_addr.vci);
}
START_TIMER(sock,T310);
sock->call_state = cs_out_proc;
return;
case ATM_MSG_CONNECT: /* CONNECTING, REL_REQ */
if (sock->state == ss_rel_req) {
send_status(sock->sig,sock,0,ATM_CV_INCOMP_MSG,ATM_MSG_CONNECT);
return;
}
if (sock->state != ss_connecting) break;
STOP_TIMER(sock);
if (q_present(&in_dsc,QG_conn_id)) {
int vpci;
vpci = q_fetch(&in_dsc,QF_vpi);
sock->pvc.sap_family = AF_ATMPVC;
sock->pvc.sap_addr.itf = get_itf(sock->sig,&vpci);
sock->pvc.sap_addr.vpi = vpci;
sock->pvc.sap_addr.vci = q_fetch(&in_dsc,QF_vci);
diag(COMPONENT,DIAG_DEBUG,"ITF.VPI.VCI: %d/%d.%d",
sock->pvc.sap_addr.itf,sock->pvc.sap_addr.vpi,
sock->pvc.sap_addr.vci);
}
error = 0;
if (!sock->pvc.sap_addr.vpi && !sock->pvc.sap_addr.vci)
error = -EPROTO;
/* more problems */
if (error) {
set_error(sock,error);
send_release(sock,0); /* @@@ cause follows reason ??? */
START_TIMER(sock,T308_1);
new_state(sock,ss_rel_req);
return;
}
send_connect_ack(sock);
/* @@@ fill in sock->remote */
/* @@@ fill in traffic parameters */
send_kernel(sock->id,kptr_null,as_okay,0,&sock->pvc,NULL,
&sock->local,&sock->sap,&sock->qos);
new_state(sock,ss_connected);
#if defined(Q2963_1) || defined(DYNAMIC_UNI)
sock->owner = 1;
#endif
if (atm2text(buffer,MAX_ATM_ADDR_LEN+1,(struct sockaddr *)
&sock->remote,0) < 0) strcpy(buffer,"<invalid>");
diag(COMPONENT,DIAG_INFO,"Active open succeeded (CR 0x%06X, "
"ID %s, to %s)",sock->call_ref,kptr_print(&sock->id),buffer);
return;
case ATM_MSG_CONN_ACK: /* ACCEPTING, WAIT_REL, REL_REQ */
diag(COMPONENT,DIAG_DEBUG,"CA vpi.vci=%d.%d",
sock->pvc.sap_addr.vpi,sock->pvc.sap_addr.vci);
if (sock->state == ss_wait_rel || sock->state == ss_rel_req) {
send_status(sock->sig,sock,0,ATM_CV_INCOMP_MSG,
ATM_MSG_CONN_ACK);
return;
}
if (sock->state != ss_accepting) break;
STOP_TIMER(sock);
send_kernel(sock->id,kptr_null,as_okay,0,NULL,NULL,&sock->local,
&sock->sap,NULL);
new_state(sock,ss_connected);
#if defined(Q2963_1) || defined(DYNAMIC_UNI)
sock->owner = 0;
#endif
if (atm2text(buffer,MAX_ATM_ADDR_LEN+1, (struct sockaddr *)
&sock->remote,0) < 0) strcpy(buffer,"<invalid>");
diag(COMPONENT,DIAG_INFO,"Passive open succeeded (CR 0x%06X, "
"ID %s, from %s)",sock->call_ref,kptr_print(&sock->id),buffer);
return;
case ATM_MSG_RELEASE: /* all states */
{
unsigned char cause;
cause = q_fetch(&in_dsc,QF_cause);
diag(COMPONENT,DIAG_DEBUG,"Cause %d (%s)",cause,cause > 127 ?
"invalid cause" : cause_text[cause]);
}
switch (sock->state) {
case ss_connecting:
set_error(sock,-ECONNREFUSED);
/* fall through */
case ss_accepting:
set_error(sock,-ECONNRESET); /* ERESTARTSYS ? */
send_release_complete(sock->sig,sock->call_ref,0);
SEND_ERROR(sock->id,sock->error);
STOP_TIMER(sock);
free_sock(sock);
return;
case ss_rel_req:
send_close(sock);
/* fall through */
case ss_wait_rel:
STOP_TIMER(sock);
free_sock(sock);
return;
#if defined(Q2963_1) || defined(DYNAMIC_UNI)
case ss_mod_req:
#endif
STOP_TIMER(sock);
/* fall through */
#if defined(Q2963_1) || defined(DYNAMIC_UNI)
case ss_mod_lcl:
case ss_mod_rcv:
case ss_mod_fin_ok:
case ss_mod_fin_fail:
case ss_mod_fin_ack:
#endif
case ss_connected:
diag(COMPONENT,DIAG_INFO,"Passive close (CR 0x%06X)",
sock->call_ref);
#if defined(Q2963_1) || defined(DYNAMIC_UNI)
if (timer_handler(sock->conn_timer) == on_T361)
STOP_TIMER(sock);
#endif
send_close(sock);
new_state(sock,ss_rel_ind);
return;
case ss_indicated:
/* fall through */
case ss_proceeding:
send_release_complete(sock->sig,sock->call_ref,0);
new_state(sock,ss_zombie);
/* fall through */
case ss_rel_ind:
return;
default:
send_release_complete(sock->sig,sock->call_ref,0);
/* @@@ should be ATM_CV_INCOMP_MSG */
break;
}
break;
case ATM_MSG_RESTART:
set_error(sock,-ENETRESET);
/* fall through */
case ATM_MSG_STATUS: /* fall through when clearing */
case ATM_MSG_REL_COMP: /* basically any state (except LISTENING and
ZOMBIE) */
{
unsigned char cause;
if (mid != ATM_MSG_REL_COMP || !q_present(&in_dsc,QF_cause))
cause = 0;
else {
cause = q_fetch(&in_dsc,QF_cause);
diag(COMPONENT,DIAG_DEBUG,"Cause %d (%s)",cause,
cause > 127 ? "invalid cause" : cause_text[cause]);
}
switch (sock->state) {
case ss_connecting:
set_error(sock,cause == ATM_CV_UNALLOC ?
-EADDRNOTAVAIL : cause == ATM_CV_RES_UNAVAIL ||
#if defined(UNI31) || defined(UNI40) || defined(DYNAMIC_UNI)
cause == ATM_CV_UCR_UNAVAIL_NEW ||
#endif
cause == ATM_CV_NO_ROUTE_DEST ? -EHOSTUNREACH :
cause == ATM_CV_NUM_CHANGED ? -EREMCHG :
cause == ATM_CV_DEST_OOO ? -EHOSTDOWN :
-ECONNREFUSED);
/* fall through */
case ss_accepting:
set_error(sock,-ECONNRESET); /* ERESTARTSYS ? */
SEND_ERROR(sock->id,sock->error);
STOP_TIMER(sock);
free_sock(sock);
return;
case ss_rel_req:
send_close(sock);
/* fall through */
case ss_wait_rel:
STOP_TIMER(sock);
free_sock(sock);
return;
#if defined(Q2963_1) || defined(DYNAMIC_UNI)
case ss_mod_req:
#endif
STOP_TIMER(sock);
/* fall through */
#if defined(Q2963_1) || defined(DYNAMIC_UNI)
case ss_mod_lcl:
case ss_mod_rcv:
case ss_mod_fin_ok:
case ss_mod_fin_fail:
case ss_mod_fin_ack:
#endif
case ss_connected:
diag(COMPONENT,DIAG_INFO,"Passive close (CR 0x%06X)",
sock->call_ref);
#if defined(Q2963_1) || defined(DYNAMIC_UNI)
if (timer_handler(sock->conn_timer) == on_T361)
STOP_TIMER(sock);
#endif
send_close(sock);
/* fall through */
case ss_rel_ind:
new_state(sock,ss_wait_close);
return;
case ss_indicated:
/* fall through */
case ss_proceeding:
new_state(sock,ss_zombie);
return;
default:
break;
}
break; /* fail */
}
case ATM_MSG_ALERTING:
/*
* We basically ignore this junk message, except for the connection
* identifier it may carry.
*/
if (q_present(&in_dsc,QG_conn_id)) {
int vpci;
vpci = q_fetch(&in_dsc,QF_vpi);
sock->pvc.sap_family = AF_ATMPVC;
sock->pvc.sap_addr.itf = get_itf(sock->sig,&vpci);
sock->pvc.sap_addr.vpi = vpci;
sock->pvc.sap_addr.vci = q_fetch(&in_dsc,QF_vci);
diag(COMPONENT,DIAG_DEBUG,"ITF.VPI.VCI: %d.%d.%d",
sock->pvc.sap_addr.itf,sock->pvc.sap_addr.vpi,
sock->pvc.sap_addr.vci);
}
return;
case ATM_MSG_NOTIFY:
/* silently ignore this junk */
return;
#if defined(Q2963_1) || defined(DYNAMIC_UNI)
/*
* Buglet ahead: should actually test "call_state"
*/
case ATM_MSG_MODIFY_REQ:
if (!(sock->sig->uni & S_Q2963_1)) goto _default;
if (sock->state != ss_connected || sock->owner) break;
sock->new_qos = sock->qos;
if (q_present(&in_dsc,QF_fw_pcr_01))
sock->new_qos.rxtp.max_pcr = q_fetch(&in_dsc,QF_fw_pcr_01);
if (q_present(&in_dsc,QF_bw_pcr_01))
sock->new_qos.txtp.max_pcr = q_fetch(&in_dsc,QF_bw_pcr_01);
send_kernel(sock->id,kptr_null,as_modify,
ATM_MF_INC_RSV | ATM_MF_DEC_RSV | ATM_MF_DEC_SHP,
NULL,NULL,NULL,NULL,&sock->new_qos);
new_state(sock,ss_mod_rcv);
return;
case ATM_MSG_MODIFY_ACK:
if (!(sock->sig->uni & S_Q2963_1)) goto _default;
if (sock->state != ss_mod_req) break;
STOP_TIMER(sock);
sock->qos = sock->new_qos;
if (q_present(&in_dsc,QG_bbrt)) send_conn_avail(sock);
send_kernel(sock->id,kptr_null,as_modify,ATM_MF_SET,NULL,NULL,NULL,
NULL,&sock->qos);
new_state(sock,ss_mod_fin_ok);
return;
case ATM_MSG_MODIFY_REJ:
if (!(sock->sig->uni & S_Q2963_1)) goto _default;
if (sock->state != ss_mod_req) break;
STOP_TIMER(sock);
sock->error = -EAGAIN;
send_kernel(sock->id,kptr_null,as_modify,ATM_MF_SET,NULL,NULL,NULL,
NULL,&sock->qos);
new_state(sock,ss_mod_fin_fail);
return;
case ATM_MSG_CONN_AVAIL:
if (!(sock->sig->uni & S_Q2963_1)) goto _default;
if (sock->state != ss_connected || sock->owner) break;
STOP_TIMER(sock);
send_kernel(sock->id,kptr_null,as_modify,ATM_MF_SET,NULL,NULL,NULL,
NULL,&sock->qos);
new_state(sock,ss_mod_fin_ack);
return;
_default: /* jump here if we don't want to understand a message */
#endif
default:
diag(COMPONENT,DIAG_WARN,"Bad signaling message %d",mid);
send_status(sock->sig,sock,0,ATM_CV_UNKNOWN_MSG_TYPE,mid);
return;
}
diag(COMPONENT,DIAG_WARN,
"Signaling message %s is incompatible with state %s/%s (%d?%d)",
mid2name(mid),state_name[sock->state],cs_name[sock->call_state],
(int) sock->state,(int) sock->call_state);
send_status(sock->sig,sock,0,ATM_CV_INCOMP_MSG,mid);
}
/**
* Human vs compter
*
* Plays vs the computer
* @param string for filename
* @param agent_color the agent color
* @return 1 if human player won, else return 0
*/
int human_vs_computer( char *file, char agent_color )
{
char board[ SIZE ][ SIZE ];
char player = toupper( agent_color );
/* set up board */
setup_board(file,board);
/* create a new state */
struct State * state = new_state( board, 'B' );
print_state( state );
/* START GAME */
/* start game ( B first ) */
struct State * temp_state;
if( player == state->player )
{
temp_state = computer_player_first( state );
Free( state, sizeof( struct State ) );
state = temp_state;
}
else
{
temp_state = human_player_first( state );
Free( state, sizeof( struct State ) );
state = temp_state;
}
/* second move */
printf( "\n" );
print_state( state );
if( player == state->player )
{
temp_state = computer_player_second( state );
Free( state, sizeof( struct State ) );
state = temp_state;
}
else
{
temp_state = human_player_second( state );
Free( state, sizeof( struct State ) );
state = temp_state;
}
/* regular game */
for( ;; )
{
printf( "\n" );
print_state( state );
if( state->player == player )
{
temp_state = computer_player( state );
Free( state, sizeof( struct State ) );
state = temp_state;
}
else
{
temp_state = human_player( state );
Free( state, sizeof( struct State ) );
state = temp_state;
}
if( terminal_test( state ) == 1 )
{
printf( "\n\n%c wins!!!\n", opposite_player( state->player ) );
break;
}
}
return player == opposite_player( state->player );
}
TEST(LoadingAndFK, SimpleRobot)
{
static const std::string MODEL1 =
"<?xml version=\"1.0\" ?>"
"<robot name=\"myrobot\">"
"<link name=\"base_link\">"
" <inertial>"
" <mass value=\"2.81\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.099 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision name=\"my_collision\">"
" <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"</robot>";
static const std::string MODEL1_INFO =
"Complete model state dimension = 3\n"
"State bounds: [-3.14159, 3.14159] [-DBL_MAX, DBL_MAX] [-DBL_MAX, DBL_MAX]\n"
"Root joint : base_joint \n"
"Available groups: base \n"
"Group base has 1 roots: base_joint \n";
std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
planning_models::KinematicModel::MultiDofConfig config("base_joint");
config.type = "Planar";
config.parent_frame_id = "odom_combined";
config.child_frame_id = "base_link";
multi_dof_configs.push_back(config);
urdf::Model urdfModel;
urdfModel.initString(MODEL1);
std::vector<std::string> gc_joints;
gc_joints.push_back("base_joint");
std::vector<std::string> empty;
std::vector<planning_models::KinematicModel::GroupConfig> gcs;
gcs.push_back(planning_models::KinematicModel::GroupConfig("base", gc_joints, empty));
planning_models::KinematicModel* model(new planning_models::KinematicModel(urdfModel,gcs,multi_dof_configs));
//bracketing so the state gets destroyed before we bring down the model
{
planning_models::KinematicState state(model);
EXPECT_EQ((unsigned int)3, state.getDimension());
state.setKinematicStateToDefault();
const std::vector<planning_models::KinematicState::JointState*>& joint_states = state.getJointStateVector();
EXPECT_EQ((unsigned int)1, joint_states.size());
EXPECT_EQ((unsigned int)3, joint_states[0]->getJointStateValues().size());
std::stringstream ssi;
state.printStateInfo(ssi);
EXPECT_TRUE(sameStringIgnoringWS(MODEL1_INFO, ssi.str())) << ssi.str();
std::map<std::string, double> joint_values;
joint_values["planar_x"]=10.0;
joint_values["planar_y"]=8.0;
//testing incomplete state
std::vector<std::string> missing_states;
EXPECT_FALSE(state.setKinematicState(joint_values,
missing_states));
ASSERT_EQ(missing_states.size(),1);
EXPECT_EQ(missing_states[0],std::string("planar_th"));
joint_values["planar_th"]=0.0;
EXPECT_TRUE(state.setKinematicState(joint_values,
missing_states));
ASSERT_EQ(missing_states.size(),0);
EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
//making sure that values get copied
planning_models::KinematicState new_state(state);
EXPECT_NEAR(10.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
EXPECT_NEAR(8.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
EXPECT_NEAR(0.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
const std::map<std::string, unsigned int>& ind_map = state.getKinematicStateIndexMap();
std::vector<double> jv(state.getDimension(), 0.0);
jv[ind_map.at("planar_x")] = 10.0;
jv[ind_map.at("planar_y")] = 8.0;
jv[ind_map.at("planar_th")] = 0.0;
state.setKinematicState(jv);
EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
}
delete model;
}
int
get_state (int symbol)
{
int key;
short *isp1;
short *isp2;
short *iend;
core *sp;
int found;
int n;
#ifdef TRACE
fprintf(stderr, "Entering get_state, symbol = %d\n", symbol);
#endif
isp1 = kernel_base[symbol];
iend = kernel_end[symbol];
n = iend - isp1;
/* add up the target state's active item numbers to get a hash key */
key = 0;
while (isp1 < iend)
key += *isp1++;
key = key % STATE_TABLE_SIZE;
sp = state_table[key];
if (sp)
{
found = 0;
while (!found)
{
if (sp->nitems == n)
{
found = 1;
isp1 = kernel_base[symbol];
isp2 = sp->items;
while (found && isp1 < iend)
{
if (*isp1++ != *isp2++)
found = 0;
}
}
if (!found)
{
if (sp->link)
{
sp = sp->link;
}
else /* bucket exhausted and no match */
{
sp = sp->link = new_state(symbol);
found = 1;
}
}
}
}
else /* bucket is empty */
{
state_table[key] = sp = new_state(symbol);
}
return (sp->number);
}
int main(void)
{
struct state *state;
union protocol_tx *t, *t2;
struct protocol_gateway_payment payment;
u8 *prev_txhashes;
enum input_ecode e;
struct protocol_tx_id txid;
unsigned int bad_input;
bool too_old, already_known;
const struct block *b;
struct protocol_block_id prevs[PROTOCOL_NUM_PREV_IDS];
struct protocol_input inputs[1];
pseudorand_init();
state = new_state(true);
prev_txhashes = make_prev_txhashes(state, &genesis, helper_addr(1));
memset(prevs, 0, sizeof(prevs));
prevs[0] = genesis.sha;
w = new_working_block(state, block_difficulty(&genesis.bi),
prev_txhashes, tal_count(prev_txhashes),
block_height(&genesis.bi) + 1,
next_shard_order(&genesis),
prevs, helper_addr(1));
/* Create a block with a gateway tx in it. */
payment.send_amount = cpu_to_le32(1000);
payment.output_addr = *helper_addr(0);
t = create_from_gateway_tx(state, helper_gateway_public_key(),
1, &payment, false, helper_gateway_key(state));
hash_tx(t, &txid);
log_unusual(state->log, "Gateway tx is ");
log_add_struct(state->log, struct protocol_tx_id, &txid);
e = add_pending_tx(state, t, &txid, &bad_input,
&too_old, &already_known);
assert(e == ECODE_INPUT_OK);
assert(!already_known);
assert(state->pending->num_unknown == 0);
assert(num_pending_known(state) == 1);
b = solve_pending(state);
/* Now we can spend it. */
prev_txhashes = make_prev_txhashes(state, b, helper_addr(1));
prevs[0] = b->sha;
prevs[1] = genesis.sha;
w = new_working_block(state, block_difficulty(&b->bi),
prev_txhashes, tal_count(prev_txhashes),
block_height(&b->bi) + 1,
next_shard_order(b),
prevs, helper_addr(1));
hash_tx(t, &inputs[0].input);
inputs[0].output = 0;
inputs[0].unused = 0;
t2 = t = create_normal_tx(state, helper_addr(1),
500, 500 - PROTOCOL_FEE(500), 1, true, inputs,
helper_private_key(state, 0));
hash_tx(t, &txid);
e = add_pending_tx(state, t, &txid, &bad_input,
&too_old, &already_known);
assert(e == ECODE_INPUT_OK);
assert(!already_known);
assert(state->pending->num_unknown == 0);
assert(num_pending_known(state) == 1);
log_unusual(state->log, "Normal tx is ");
log_add_struct(state->log, struct protocol_tx_id, &txid);
/* Should recognize double spend */
t = create_normal_tx(state, helper_addr(2),
300, 700 - PROTOCOL_FEE(300), 1, true, inputs,
helper_private_key(state, 0));
hash_tx(t, &txid);
e = add_pending_tx(state, t, &txid, &bad_input,
&too_old, &already_known);
assert(e == ECODE_INPUT_DOUBLESPEND);
assert(!already_known);
assert(state->pending->num_unknown == 0);
assert(num_pending_known(state) == 1);
b = solve_pending(state);
/* The first should be included. */
assert(num_txs(b) == 1);
/* There should be nothing left. */
assert(state->pending->num_unknown == 0);
assert(num_pending_known(state) == 0);
/* Now retry double spend. */
t = create_normal_tx(state, helper_addr(2),
300, 700 - PROTOCOL_FEE(300), 1, true, inputs,
helper_private_key(state, 0));
hash_tx(t, &txid);
e = add_pending_tx(state, t, &txid, &bad_input,
&too_old, &already_known);
assert(e == ECODE_INPUT_DOUBLESPEND);
assert(!too_old);
assert(!already_known);
/* Clear inputhash manually. */
inputhash_del_tx(&state->inputhash, t2);
dump_inputhash(state, &state->inputhash);
tal_free(state);
return 0;
}
State *match_state(void)
{
State *s = new_state('\0', NULL, NULL);
s->t = Matched;
return s;
}
/*
* run_tty_sim - Run the simulator in TTY mode
*/
static void run_tty_sim()
{
int icount = 0;
exc_t status = EXC_NONE;
cc_t result_cc = 0;
int byte_cnt = 0;
mem_t mem0, reg0;
state_ptr isa_state = NULL;
/* In TTY mode, the default object file comes from stdin */
if (!object_file) {
object_file = stdin;
}
/* Initializations */
if (verbosity >= 2)
sim_set_dumpfile(stdout);
sim_init();
/* Emit simulator name */
printf("%s\n", simname);
byte_cnt = load_mem(mem, object_file, 1);
if (byte_cnt == 0) {
fprintf(stderr, "No lines of code found\n");
exit(1);
} else if (verbosity >= 2) {
printf("%d bytes of code read\n", byte_cnt);
}
fclose(object_file);
if (do_check) {
isa_state = new_state(0);
free_mem(isa_state->r);
free_mem(isa_state->m);
isa_state->m = copy_mem(mem);
isa_state->r = copy_mem(reg);
isa_state->cc = cc;
}
mem0 = copy_mem(mem);
reg0 = copy_mem(reg);
icount = sim_run(instr_limit, &status, &result_cc);
if (verbosity > 0) {
printf("%d instructions executed\n", icount);
printf("Exception status = %s\n", exc_name(status));
printf("Condition Codes: %s\n", cc_name(result_cc));
printf("Changed Register State:\n");
diff_reg(reg0, reg, stdout);
printf("Changed Memory State:\n");
diff_mem(mem0, mem, stdout);
}
if (do_check) {
exc_t e = EXC_NONE;
int step;
bool_t match = TRUE;
for (step = 0; step < instr_limit && e == EXC_NONE; step++) {
e = step_state(isa_state, stdout);
}
if (diff_reg(isa_state->r, reg, NULL)) {
match = FALSE;
if (verbosity > 0) {
printf("ISA Register != Pipeline Register File\n");
diff_reg(isa_state->r, reg, stdout);
}
}
if (diff_mem(isa_state->m, mem, NULL)) {
match = FALSE;
if (verbosity > 0) {
printf("ISA Memory != Pipeline Memory\n");
diff_mem(isa_state->m, mem, stdout);
}
}
if (isa_state->cc != result_cc) {
match = FALSE;
if (verbosity > 0) {
printf("ISA Cond. Codes (%s) != Pipeline Cond. Codes (%s)\n",
cc_name(isa_state->cc), cc_name(result_cc));
}
}
if (match) {
printf("ISA Check Succeeds\n");
} else {
printf("ISA Check Fails\n");
}
}
}
