/**
* Returns a set of suggestions for the given input touch points. The commitPoint argument indicates
* whether to prematurely commit the suggested words up to the given point for sentence-level
* suggestion.
*
* Note: Currently does not support concurrent calls across threads. Continuous suggestion is
* automatically activated for sequential calls that share the same starting input.
* TODO: Stop detecting continuous suggestion. Start using traverseSession instead.
*/
void Suggest::getSuggestions(ProximityInfo *pInfo, void *traverseSession,
int *inputXs, int *inputYs, int *times, int *pointerIds, int *inputCodePoints,
int inputSize, const float weightOfLangModelVsSpatialModel,
SuggestionResults *const outSuggestionResults) const {
PROF_INIT;
PROF_TIMER_START(0);
const float maxSpatialDistance = TRAVERSAL->getMaxSpatialDistance();
DicTraverseSession *tSession = static_cast<DicTraverseSession *>(traverseSession);
tSession->setupForGetSuggestions(pInfo, inputCodePoints, inputSize, inputXs, inputYs, times,
pointerIds, maxSpatialDistance, TRAVERSAL->getMaxPointerCount());
// TODO: Add the way to evaluate cache
initializeSearch(tSession);
PROF_TIMER_END(0);
PROF_TIMER_START(1);
// keep expanding search dicNodes until all have terminated.
while (tSession->getDicTraverseCache()->activeSize() > 0) {
expandCurrentDicNodes(tSession);
tSession->getDicTraverseCache()->advanceActiveDicNodes();
tSession->getDicTraverseCache()->advanceInputIndex(inputSize);
}
PROF_TIMER_END(1);
PROF_TIMER_START(2);
SuggestionsOutputUtils::outputSuggestions(
SCORING, tSession, weightOfLangModelVsSpatialModel, outSuggestionResults);
PROF_TIMER_END(2);
}
const Path&
OccupancyMap::prepareShortestPaths(double x, double y,
double min_distance, double max_distance,
double max_occ_dist,
bool allow_unknown) {
endpoints_.clear();
if (map_ == NULL) {
ROS_WARN("OccupancyMap::prepareShortestPaths() Map not set");
return endpoints_;
}
if (map_->max_occ_dist < max_occ_dist) {
ROS_ERROR("OccupancyMap::prepareShortestPaths() CSpace has been calculated "
"up to %f, but max_occ_dist=%.2f",
map_->max_occ_dist, max_occ_dist);
ROS_BREAK();
}
initializeSearch(x, y);
Node curr_node;
while (nextNode(max_occ_dist, &curr_node, allow_unknown)) {
double node_dist = curr_node.true_cost * map_->scale;
if (min_distance <= node_dist && node_dist < max_distance) {
float x = MAP_WXGX(map_, curr_node.coord.first);
float y = MAP_WYGY(map_, curr_node.coord.second);
endpoints_.push_back(Eigen::Vector2f(x, y));
} else if (node_dist > max_distance) {
break;
}
}
return endpoints_;
}
Path OccupancyMap::astar(double startx, double starty,
double stopx, double stopy,
double max_occ_dist /* = 0.0 */,
bool allow_unknown /* = false */) {
Path path;
if (map_ == NULL) {
ROS_WARN("OccupancyMap::astar() Map not set");
return path;
}
int stopi = MAP_GXWX(map_, stopx), stopj = MAP_GYWY(map_, stopy);
if (!MAP_VALID(map_, stopi ,stopj)) {
ROS_ERROR("OccupancyMap::astar() Invalid stopping position");
ROS_BREAK();
}
if (map_->max_occ_dist < max_occ_dist) {
ROS_ERROR("OccupancyMap::astar() CSpace has been calculated up to %f, "
"but max_occ_dist=%.2f",
map_->max_occ_dist, max_occ_dist);
ROS_BREAK();
}
initializeSearch(startx, starty);
// Set stop to use heuristic
stopi_ = stopi;
stopj_ = stopj;
bool found = false;
Node curr_node;
while (nextNode(max_occ_dist, &curr_node, allow_unknown)) {
if (curr_node.coord.first == stopi && curr_node.coord.second == stopj) {
found = true;
break;
}
}
// Recreate path
if (found) {
buildPath(stopi, stopj, &path);
}
return Path(path.rbegin(), path.rend());
}
void OccupancyMap::prepareAllShortestPaths(double x, double y,
double max_occ_dist,
bool allow_unknown) {
if (map_ == NULL) {
ROS_WARN("OccupancyMap::prepareAllShortestPaths() Map not set");
return;
}
if (map_->max_occ_dist < max_occ_dist) {
ROS_ERROR("OccupancyMap::shortestToDests() CSpace has been calculated "
"up to %f, but max_occ_dist=%.2f",
map_->max_occ_dist, max_occ_dist);
ROS_BREAK();
}
initializeSearch(x, y);
Node curr_node;
while (nextNode(max_occ_dist, &curr_node, allow_unknown)) {
;
}
}
void Thread_RRT::planPath(const Thread* start, const Thread* goal, vector<Thread_Motion>& movements, vector<Thread*>& intermediateThread)
{
RRT_Node startNode = RRT_Node(start);
RRT_Node goalNode = RRT_Node(goal);
initializeSearch(startNode, goalNode);
intermediateThread.push_back(new Thread(start));
intermediateThread.push_back(new Thread(goal));
/*
intermediateThread.push_back(new Thread(start));
MatrixXd resampled_1(33,3);
startNode.thread->getPoints(resampled_1);
MatrixXd resampled_2(17,3);
startNode.thread->getPoints(resampled_2);
intermediateThread.push_back(new Thread(startNode.thread, 17, 2));
intermediateThread.push_back(new Thread(resampled_2, 2, startNode.thread->length()));*/
bool goalReached = false;
double bestDistToGoal = DBL_MAX;
while (!goalReached && currNodes.size() < MAX_NUM_NODES)
{
std::cout << "curr ind" << currNodes.size() << std::endl;
//get a goal for this iteration
RRT_Node currGoal;
getNextGoal(goalNode, currGoal);
//find the nearest neighbor
int indClosest;
findClosestNode(currGoal, indClosest);
//find motion
Thread_Motion* nextMotion = new Thread_Motion();
findThreadMotion(currNodes[indClosest]->this_node, currGoal, *nextMotion);
//add new node
currNodes.push_back(new RRT_Node_And_Edge(currNodes[indClosest], nextMotion));
currNodes[indClosest]->this_node.applyMotion(currNodes.back()->this_node, *nextMotion);
double thisDistToGoal = goalNode.distanceToNode(currNodes.back()->this_node);
if (thisDistToGoal < bestDistToGoal)
{
bestDistToGoal = thisDistToGoal;
std::cout << "new best dist: " << bestDistToGoal << std::endl;
}
if (goalNode.distanceToNode(currNodes.back()->this_node) < GOAL_DISTANCE_THRESH)
{
goalReached = true;
}
}
int finalIndClosest;
findClosestNode(goalNode, finalIndClosest);
//count how many movements we need for the best solution
int numThreadsBetween = -1;
RRT_Node_And_Edge* currPtr = currNodes[finalIndClosest];
while (currPtr != NULL)
{
numThreadsBetween ++;
currPtr = currPtr->last_node;
}
std::cout << "num threads between: " << numThreadsBetween << std::endl;
//set the movements and intermediate threads for return
movements.resize(numThreadsBetween);
intermediateThread.resize(numThreadsBetween);
int movementInd = numThreadsBetween-1;
currPtr = currNodes[finalIndClosest];
while (movementInd >= 0)
{
movements[movementInd] = *(currPtr->motion_from_last);
intermediateThread[movementInd] = currPtr->this_node.thread;
currPtr->this_node.thread = NULL; //so we don't delete it!
movementInd--;
currPtr = currPtr->last_node;
}
/*intermediateThread.resize(currNodes.size());
for (int i=0; i < intermediateThread.size(); i++)
{
intermediateThread[i] = new Thread(currNodes[i]->this_node.thread);
}*/
//cleanup();
/*
movements.resize(5);
movements[0].pos_movement = Vector3d(7.0, -5.0, 1.0);
movements[0].tan_rotation = Eigen::AngleAxisd (M_PI/90.0, Vector3d(1.0, 0.0, 0.0));
movements[1].pos_movement = Vector3d(7.0, -4.0, 2.0);
movements[1].tan_rotation = Eigen::AngleAxisd (M_PI/100.0, Vector3d(1.0, 0.2, 0.0));
movements[2].pos_movement = Vector3d(6.0, -6.0, 3.0);
movements[2].tan_rotation = Eigen::AngleAxisd (M_PI/100.0, Vector3d(1.0, 0.0, 0.2));
movements[3].pos_movement = Vector3d(8.0, -4.0, 3.0);
movements[3].tan_rotation = Eigen::AngleAxisd (M_PI/80.0, Vector3d(1.0, 0.1, 0.1));
movements[4].pos_movement = Vector3d(6.0, -5.0, 2.0);
movements[4].tan_rotation = Eigen::AngleAxisd (M_PI/90.0, Vector3d(1.0, 0.1, -0.1));
intermediateThread.resize(0);
Thread* curr = new Thread(start);
Thread* next;
for (int i=0; i < movements.size(); i++)
{
next = movements[i].applyMotion(curr);
intermediateThread.push_back(next);
curr = next;
}
*/
}
