static void test_q_new(void)
{
Queue* q = q_new();
CU_ASSERT_PTR_NOT_NULL_FATAL(q);
CU_ASSERT_PTR_NULL(q_dequeue(q));
CU_ASSERT_EQUAL(q_size(q), 0);
q_free(q);
}
void initQueue() {
Q = q_new();
currentBlock=malloc(sizeof(ThrdCtlBlk));
currentBlock->tid = 0;
ucontext_t context, *currentContext = &context;
getcontext(currentContext);
currentBlock->p=currentContext;
isInit = 1;
}
Query *maq_new()
{
Query *self = q_new(Query);
self->type = MATCH_ALL_QUERY;
self->to_s = &maq_to_s;
self->hash = &maq_hash;
self->eq = &maq_eq;
self->destroy_i = &q_destroy_i;
self->create_weight_i = &maw_new;
return self;
}
static void test_q_dequeue(void)
{
Queue* q = q_new();
int first;
int status = q_enqueue(q, &first);
int second;
status = q_enqueue(q, &second);
CU_ASSERT_PTR_EQUAL(q_dequeue(q), &first);
CU_ASSERT_EQUAL(q_size(q), 1);
CU_ASSERT_PTR_EQUAL(q_dequeue(q), &second);
CU_ASSERT_EQUAL(q_size(q), 0);
q_free(q);
}
Query *fq_new(Query *query, Filter *filter)
{
Query *self = q_new(FilteredQuery);
FQQ(self)->query = query;
FQQ(self)->filter = filter;
self->type = FILTERED_QUERY;
self->to_s = &fq_to_s;
self->destroy_i = &fq_destroy;
self->create_weight_i = &fq_new_weight;
return self;
}
/**
* This will push objects off a square.
*
* The methodology is to load all objects on the square into a queue. Replace
* the previous square with a type that does not allow for objects. Drop the
* objects. Last, put the square back to its original type.
*/
void push_object(int y, int x)
{
/* Save the original terrain feature */
struct feature *feat_old = square_feat(cave, y, x);
struct object *obj = square_object(cave, y, x);
struct queue *queue = q_new(z_info->floor_size);
bool glyph = square_iswarded(cave, y, x);
/* Push all objects on the square, stripped of pile info, into the queue */
while (obj) {
struct object *next = obj->next;
q_push_ptr(queue, obj);
/* Orphan the object */
obj->next = NULL;
obj->prev = NULL;
obj->iy = 0;
obj->ix = 0;
/* Next object */
obj = next;
}
/* Disassociate the objects from the square */
cave->squares[y][x].obj = NULL;
/* Set feature to an open door */
square_force_floor(cave, y, x);
square_add_door(cave, y, x, false);
/* Drop objects back onto the floor */
while (q_len(queue) > 0) {
/* Take object from the queue */
obj = q_pop_ptr(queue);
/* Drop the object */
drop_near(cave, &obj, 0, y, x, false);
}
/* Reset cave feature and rune if needed */
square_set_feat(cave, y, x, feat_old->fidx);
if (glyph)
square_add_ward(cave, y, x);
q_free(queue);
}
mock_server_t *
mock_server_new ()
{
mock_server_t *server = (mock_server_t *)bson_malloc0 (sizeof (mock_server_t));
server->request_timeout_msec = get_future_timeout_ms ();
_mongoc_array_init (&server->autoresponders,
sizeof (autoresponder_handle_t));
_mongoc_array_init (&server->worker_threads,
sizeof (mongoc_thread_t));
mongoc_cond_init (&server->cond);
mongoc_mutex_init (&server->mutex);
server->q = q_new ();
server->start_time = bson_get_monotonic_time ();
return server;
}
mock_server_t *
mock_server_new ()
{
mock_server_t *server = (mock_server_t *)bson_malloc0 (sizeof (mock_server_t));
server->request_timeout_msec = 10 * 1000;
_mongoc_array_init (&server->autoresponders,
sizeof (autoresponder_handle_t));
_mongoc_array_init (&server->worker_threads,
sizeof (mongoc_thread_t));
mongoc_cond_init (&server->cond);
mongoc_mutex_init (&server->mutex);
server->q = q_new ();
server->start_time = bson_get_monotonic_time ();
if (test_framework_getenv_bool ("MONGOC_TEST_SERVER_VERBOSE")) {
server->verbose = true;
}
return server;
}
static void *
worker_thread (void *data)
{
worker_closure_t *closure = (worker_closure_t *) data;
mock_server_t *server = closure->server;
mongoc_stream_t *client_stream = closure->client_stream;
mongoc_buffer_t buffer;
mongoc_rpc_t *rpc = NULL;
bool handled;
bson_error_t error;
int32_t msg_len;
sync_queue_t *requests;
sync_queue_t *replies;
request_t *request;
mongoc_array_t autoresponders;
ssize_t i;
autoresponder_handle_t handle;
reply_t *reply;
#ifdef MONGOC_ENABLE_SSL
bool ssl;
#endif
ENTRY;
/* queue of client replies sent over this worker's connection */
replies = q_new ();
#ifdef MONGOC_ENABLE_SSL
mongoc_mutex_lock (&server->mutex);
ssl = server->ssl;
mongoc_mutex_unlock (&server->mutex);
if (ssl) {
if (!mongoc_stream_tls_handshake_block (client_stream, "localhost",
TIMEOUT, &error)) {
mongoc_stream_close (client_stream);
mongoc_stream_destroy (client_stream);
RETURN (NULL);
}
}
#endif
_mongoc_buffer_init (&buffer, NULL, 0, NULL, NULL);
_mongoc_array_init (&autoresponders, sizeof (autoresponder_handle_t));
again:
/* loop, checking for requests to receive or replies to send */
bson_free (rpc);
rpc = NULL;
if (_mongoc_buffer_fill (&buffer, client_stream, 4, 10, &error) > 0) {
assert (buffer.len >= 4);
memcpy (&msg_len, buffer.data + buffer.off, 4);
msg_len = BSON_UINT32_FROM_LE (msg_len);
if (msg_len < 16) {
MONGOC_WARNING ("No data");
GOTO (failure);
}
if (_mongoc_buffer_fill (&buffer, client_stream, (size_t) msg_len, -1,
&error) == -1) {
MONGOC_WARNING ("%s():%d: %s", BSON_FUNC, __LINE__, error.message);
GOTO (failure);
}
assert (buffer.len >= (unsigned) msg_len);
/* copies message from buffer */
request = request_new (&buffer, msg_len, server, client_stream,
closure->port, replies);
memmove (buffer.data, buffer.data + buffer.off + msg_len,
buffer.len - msg_len);
buffer.off = 0;
buffer.len -= msg_len;
mongoc_mutex_lock (&server->mutex);
_mongoc_array_copy (&autoresponders, &server->autoresponders);
mongoc_mutex_unlock (&server->mutex);
test_suite_mock_server_log ("%5.2f %hu -> %hu %s",
mock_server_get_uptime_sec (server),
closure->port, server->port, request->as_str);
/* run responders most-recently-added-first */
handled = false;
for (i = server->autoresponders.len - 1; i >= 0; i--) {
handle = _mongoc_array_index (&server->autoresponders,
autoresponder_handle_t,
i);
if (handle.responder (request, handle.data)) {
/* responder destroyed request and enqueued a reply in "replies" */
handled = true;
request = NULL;
break;
}
}
if (!handled) {
/* pass to the main thread via the queue */
requests = mock_server_get_queue (server);
q_put (requests, (void *) request);
}
}
if (_mock_server_stopping (server)) {
GOTO (failure);
}
reply = q_get (replies, 10);
if (reply) {
_mock_server_reply_with_stream (server, reply, client_stream);
_reply_destroy (reply);
}
if (_mock_server_stopping (server)) {
GOTO (failure);
}
GOTO (again);
failure:
_mongoc_array_destroy (&autoresponders);
_mongoc_buffer_destroy (&buffer);
mongoc_stream_close (client_stream);
mongoc_stream_destroy (client_stream);
bson_free (rpc);
bson_free (closure);
_mongoc_buffer_destroy (&buffer);
while ((reply = q_get_nowait (replies))) {
_reply_destroy (reply);
}
q_destroy (replies);
RETURN (NULL);
}
/*@only@*/ sorted_intlist si_new(void) {
sorted_intlist si = (sorted_intlist)
malloc(sizeof(struct sorted_intlist_struct));
si->q = q_new(si_comparator, NULL /* don't free */);
return(si);
}
void RRT::plan(const Eigen::VectorXd &start, const KDL::Frame &x_goal, double goal_tolerance, std::list<KDL::Frame > &path_x, std::list<Eigen::VectorXd > &path_q, MarkerPublisher &markers_pub) {
V_.clear();
E_.clear();
path_x.clear();
path_q.clear();
int q_new_idx = 0;
RRTState state_start;
kin_model_->calculateFk(state_start.T_B_E_, effector_name_, start);
state_start.q_ = start;
V_[0] = state_start;
bool goal_found = false;
int m_id = 0;
for (int step = 0; step < 1200; step++) {
bool sample_goal = randomUniform(0,1) < 0.05;
KDL::Frame x_rand;
if (sample_goal) {
x_rand = x_goal;
}
else {
if (!sampleFree(x_rand)) {
std::cout << "ERROR: RRT::plan: could not sample free space" << std::endl;
return;
}
}
// get the closest pose
int x_nearest_idx = nearest(x_rand);
RRTState state_nearest( V_.find(x_nearest_idx)->second );
const KDL::Frame &x_nearest = state_nearest.T_B_E_;
KDL::Frame x_new;
// get the new pose
steer(x_nearest, x_rand, 2.0, 170.0/180.0*3.1415, x_new);
// std::cout << x_nearest.p[0] << " " << x_nearest.p[1] << " " << x_nearest.p[2] << std::endl;
// std::cout << x_new.p[0] << " " << x_new.p[1] << " " << x_new.p[2] << std::endl;
Eigen::VectorXd q_new(ndof_);
bool added_new = false;
KDL::Frame T_B_E;
if (collisionFree(state_nearest.q_, x_nearest, x_new, 0, q_new, T_B_E)) {
added_new = true;
RRTState state_new;
state_new.T_B_E_ = T_B_E;//x_new;
state_new.q_ = q_new;
q_new_idx++;
V_[q_new_idx] = state_new;
E_[q_new_idx] = x_nearest_idx;
m_id = markers_pub.addVectorMarker(q_new_idx, x_nearest.p, T_B_E.p, 0, 0.7, 0, 0.5, 0.01, "base");
KDL::Twist goal_diff( KDL::diff(T_B_E, x_goal, 1.0) );
if (goal_diff.vel.Norm() < 0.03 && goal_diff.rot.Norm() < 10.0/180.0*3.1415) {
goal_found = true;
std::cout << "goal found" << std::endl;
}
}
else {
KDL::Twist goal_diff( KDL::diff(x_new, x_goal, 1.0) );
if (goal_diff.vel.Norm() < 0.03 && goal_diff.rot.Norm() < 10.0/180.0*3.1415) {
std::list<int> nodes_to_remove_list;
getPath(x_nearest_idx, nodes_to_remove_list);
if (nodes_to_remove_list.size() > 1) {
nodes_to_remove_list.pop_front();
std::set<int> nodes_to_remove;
int removed_nodes_count = 0;
for (std::list<int>::const_iterator it = nodes_to_remove_list.begin(); it != nodes_to_remove_list.end(); it++) {
nodes_to_remove.insert( (*it) );
}
while (nodes_to_remove.size() > 0) {
std::set<int> orphan_nodes;
for (std::map<int, int >::const_iterator e_it = E_.begin(); e_it != E_.end(); e_it++) {
if (nodes_to_remove.find(e_it->second) != nodes_to_remove.end()) {
orphan_nodes.insert(e_it->first);
}
}
for (std::set<int>::const_iterator it = nodes_to_remove.begin(); it != nodes_to_remove.end(); it++) {
V_.erase( (*it) );
E_.erase( (*it) );
markers_pub.addEraseMarkers( (*it), (*it)+1);
removed_nodes_count++;
}
nodes_to_remove = orphan_nodes;
}
std::cout << "removed nodes: " << removed_nodes_count << std::endl;
}
}
if (sample_goal) {
// remove the nearest node to the goal
// V_.erase(x_nearest_idx);
// E_.erase(x_nearest_idx);
// markers_pub.addEraseMarkers(x_nearest_idx, x_nearest_idx+1);
}
}
if (added_new) {
std::cout << "step " << step << " nearest_idx " << x_nearest_idx << std::endl;
}
markers_pub.publish();
ros::spinOnce();
// getchar();
if (goal_found) {
break;
}
}
/*
double min_cost = 0.0;
int min_goal_idx = -1;
for (std::map<int, Eigen::VectorXd >::const_iterator v_it = V_.begin(); v_it != V_.end(); v_it++) {
double dist = (v_it->second - x_goal).norm();
double c = cost(v_it->first);
if (dist < goal_tolerance && (min_goal_idx < 0 || min_cost > c)) {
min_cost = c;
min_goal_idx = v_it->first;
}
}
if (min_goal_idx == -1) {
// path not found
return;
}
*/
if (goal_found) {
std::list<int > idx_path;
getPath(q_new_idx, idx_path);
for (std::list<int >::const_iterator p_it = idx_path.begin(); p_it != idx_path.end(); p_it++) {
const RRTState ¤t = V_.find(*p_it)->second;
path_q.push_back(current.q_);
path_x.push_back(current.T_B_E_);
}
}
/*
int q_vec_idx = V_[q_new_idx].q_vec_.size()-1;
std::list<int > idx_path;
getPath(q_new_idx, idx_path);
for (std::list<int >::const_reverse_iterator p_it = idx_path.rbegin(); p_it != idx_path.rend(); p_it++) {
// std::list<int >::const_reverse_iterator p_it2 = p_it;
// p_it2++;
const RRTState ¤t = V_.find(*p_it)->second;
// if (p_it2 == idx_path.rend()) {
// path_q.push_back(current.q_vec_[q_vec_idx]->second);
// }
// else {
path_q.push_back(current.q_vec_[q_vec_idx].second);
q_vec_idx = current.q_vec_[q_vec_idx].first;
// }
}
std::reverse(path_q.begin(), path_q.end());
*/
/*
for (std::list<int >::const_iterator p_it = idx_path.begin(); p_it != idx_path.end(); p_it++) {
const RRTState ¤t = V_.find(*p_it)->second;
if (p_it == idx_path.begin()) {
path_q.push_back(current.q_vec_[0]);
}
else {
std::list<int >::const_iterator p_it_prev = p_it;
p_it_prev--;
const RRTState &prev = V_.find(*p_it_prev)->second;
path_q.push_back(current.q_vec_[0]);
}
}
*/
}
