// Signed distance between a point and a polygon
double FFPoint::signedDistanceToPolygon(
size_t& nvert, double* vertx, double* verty, bool expanding){
/* signed distance between a point and a polygon. */
double d = distanceToSegment(vertx[0], verty[0], vertx[nvert-1], verty[nvert-1]);
double di;
for ( size_t i = 0; i < nvert-1; i++ ){
di = distanceToSegment(vertx[i], verty[i], vertx[i+1], verty[i+1]);
if ( di < d ) d = di;
}
if ( pointInPolygon(nvert, vertx, verty) xor expanding ) return -d;
return d;
}
void drawFilledPolygon(point* verticies, int numberOfVerticies, unsigned char shade) {
int x, y;
box myBox;
if(getSetting() == 0) { return; }
myBox = getBox(verticies, numberOfVerticies);
for(y = myBox.topL.y; y <= myBox.botR.y; y++){
for(x = myBox.topL.x; x <= myBox.botR.x; x++){
if(pointInPolygon(verticies, numberOfVerticies, makePoint(x, y))) {
drawPoint(makePoint(x, y), shade);
}
}
}
drawPolygon(verticies, numberOfVerticies, 0xF);
}
bool CollisionSystem::collision_circle_polygon(const std::shared_ptr<AbstractComponent> pos1component,
const std::shared_ptr<AbstractComponent> col1component,
const std::shared_ptr<AbstractComponent> pos2component,
const std::shared_ptr<AbstractComponent> col2component) {
//Position Attributes for the first entity
//the position
auto pos1 = std::vector<float>(2);
pos1[0] = (pos1component->getAttribute_float(ATTRIBUTE_POSITION_X));
pos1[1] = (pos1component->getAttribute_float(ATTRIBUTE_POSITION_Y));
//the offset position
auto offset1 = std::vector<float>(2);
offset1[0] = (pos1component->getAttribute_float(ATTRIBUTE_OFFSET_X));
offset1[1] = (pos1component->getAttribute_float(ATTRIBUTE_OFFSET_Y));
//the offset collision
offset1[0] += col1component->getAttribute_float(ATTRIBUTE_OFFSET_X);
offset1[1] += col1component->getAttribute_float(ATTRIBUTE_OFFSET_Y);
//the rotation (not relevant in the first implementation)
auto rot1 = pos1component->getAttribute_float(ATTRIBUTE_ROTATION);
//the origin
auto origin1 = std::vector<float>(2);
origin1[0] = (col1component->getAttribute_float(ATTRIBUTE_ORIGIN_X));
origin1[1] = (col1component->getAttribute_float(ATTRIBUTE_ORIGIN_Y));
//get the rectangle bounds
auto radius1 = col1component->getAttribute_float(ATTRIBUTE_RADIUS);
//Position Attributes for the second entity
//the position
auto pos2 = std::vector<float>(2);
pos2[0] = (pos2component->getAttribute_float(ATTRIBUTE_POSITION_X));
pos2[1] = (pos2component->getAttribute_float(ATTRIBUTE_POSITION_Y));
//the offset position
auto offset2 = std::vector<float>(2);
offset2[0] = (pos2component->getAttribute_float(ATTRIBUTE_OFFSET_X));
offset2[1] = (pos2component->getAttribute_float(ATTRIBUTE_OFFSET_Y));
//the offset collision
offset2[0] += col2component->getAttribute_float(ATTRIBUTE_OFFSET_X);
offset2[1] += col2component->getAttribute_float(ATTRIBUTE_OFFSET_Y);
//the rotation (not relevent in the first implementation)
auto rot2 = pos2component->getAttribute_float(ATTRIBUTE_ROTATION);
//the origin
auto origin2 = std::vector<float>(2);
origin2[0] = (col2component->getAttribute_float(ATTRIBUTE_ORIGIN_X));
origin2[1] = (col2component->getAttribute_float(ATTRIBUTE_ORIGIN_Y));
//grab the polygon points
auto polygonPointsTemp = col2component->getAttribute_floatArray(ATTRIBUTE_POLYGON_POINTS);
//represents the center of our circle
pos1[0] += offset1[0] - origin1[0] + radius1;
pos1[1] += offset1[1] - origin1[1] + radius1;
//we need to copy the points and include the changes in the origin and offset
// of our polygon
auto correctedPolygon = componentFloatArrayType(*polygonPointsTemp);
int numPoints = correctedPolygon.size() / 2;
assert(correctedPolygon.size() % 2 == 0 && "Odd number of values in float array, should be even");
//represents the corrected positioning of our polygon
pos2[0] += offset2[0] - origin2[0];
pos2[1] += offset2[1] - origin2[1];
//perform point corrections based on the offset and the origin
for (int i = 0; i < numPoints; i++) {
//x-component
correctedPolygon[i*2] += pos2[0];
//y-component
correctedPolygon[i*2+1] += pos2[1];
}
//form points for our circle to represent the extents of
// our circle
auto circlePolygon = componentFloatArrayType(SYNTH_CIRCLE_POINT_NUM*2);
//our radius step
float angleStep = 2 * PI / SYNTH_CIRCLE_POINT_NUM;
for (int i = 0; i < SYNTH_CIRCLE_POINT_NUM; i++) {
circlePolygon[i*2] = radius1 * cosf(angleStep*i) + pos1[0];
circlePolygon[i*2+1] = radius1 * sinf(angleStep*i) + pos1[1];
}
//we have a rectangular polygon and a polygon
// the first test performed checks to see whether the vertices of our
// rectangle lie within the bounds of our polygon
// the polygon is assumed to be a convex polygon, and no
// checks at this time will be performed to make this assumption
// TODO check if the polygon is convex
//to perform the vertice check, we perform a binary search within the convex shape
// O(logn) complexity, the implementation is in AE_Utilities
//NOTE: we perform the check on the 4 points
int circleNumPoints = SYNTH_CIRCLE_POINT_NUM;
for (int i = 0; i < circleNumPoints; i++) {
if(pointInPolygon(
circlePolygon[i*2], circlePolygon[i*2+1],
correctedPolygon)) {
return true;
} //END if(pointInPolygon(...
}//END for (int i = 0; i < rectNumPoints; i++) {
//next, the edge check is performed between the two polygons
if (checkPolygonLineIntersections(correctedPolygon, circlePolygon)) {
return true;
}
return false;
}
void PolygonNode::getElementsByPos(const glm::vec2& pos, vector<NodePtr>& pElements)
{
if (reactsToMouseEvents() && pointInPolygon(pos, m_Pts)) {
pElements.push_back(getSharedThis());
}
}
int main(int argc, char * argv [])
{
int n_records, i, j;
char * tc_path = argv[1];
char * fl_dir = argv[2];
char fl_path [256];
printf("%s\n", tc_path);
printf("%s\n", fl_dir);
SHPHandle
HSHP_polygon = SHPOpen(tc_path, "rb");
if(!HSHP_polygon){
puts("Couldn't open tc_path");
return 0;
}
SHPGetInfo(HSHP_polygon, &n_records, NULL, NULL, NULL);
printf("n_records: %d\n", n_records);
SHPObject* shapes [n_records];
for(i = 0; i < n_records; i++){
shapes[i] = SHPReadObject(HSHP_polygon, i);
if(!shapes[i]){
puts("Failed reading shape object.");
return 0;
}
}
SHPClose(HSHP_polygon);
struct dirent *entry;
DIR *dp;
dp = opendir(fl_dir);
if (dp == NULL) {
perror("opendir: Path does not exist or could not be read.");
return 0;
}
while ((entry = readdir(dp)))
{
sprintf(fl_path, "%s/%s", fl_dir, entry->d_name);
printf("%s\n",fl_path);
char end [8];
strncpy(end, fl_path + strlen(fl_path) - 3, 3);
printf("END -- %s\n", end);
if( strcmp(end, "shp") != 0)
continue;
SHPHandle
HSHP_fl = SHPOpen(fl_path, "rb");
if(!HSHP_fl){
puts("Couldn't open fl_path");
continue;
}
SHPObject * fline = SHPReadObject(HSHP_fl, 0);
if(!fline){
puts("Error reading flight line.");
return 0;
}
for(i = 0; i < n_records; i++){
SHPObject * shape = shapes[i];
for(j = 0; j < shape->nVertices; j++){
if(pointInPolygon(fline, shape->padfX[j], shape->padfY[j])){
printf("\t%d\n", i);
break;
}
}
}
SHPDestroyObject(fline);
SHPClose(HSHP_fl);
}
closedir(dp);
return 0;
}
void executeCb(const frontier_exploration::ExploreTaskGoalConstPtr &goal)
{
success_ = false;
moving_ = false;
explore_costmap_ros_->resetLayers();
//create costmap services
ros::ServiceClient updateBoundaryPolygon = private_nh_.serviceClient<frontier_exploration::UpdateBoundaryPolygon>("explore_costmap/explore_boundary/update_boundary_polygon");
ros::ServiceClient getNextFrontier = private_nh_.serviceClient<frontier_exploration::GetNextFrontier>("explore_costmap/explore_boundary/get_next_frontier");
//wait for move_base and costmap services
if(!move_client_.waitForServer() || !updateBoundaryPolygon.waitForExistence() || !getNextFrontier.waitForExistence()){
as_.setAborted();
return;
}
//set region boundary on costmap
if(ros::ok() && as_.isActive()){
frontier_exploration::UpdateBoundaryPolygon srv;
srv.request.explore_boundary = goal->explore_boundary;
if(updateBoundaryPolygon.call(srv)){
ROS_INFO("Region boundary set");
}else{
ROS_ERROR("Failed to set region boundary");
as_.setAborted();
return;
}
}
//loop until all frontiers are explored
ros::Rate rate(frequency_);
while(ros::ok() && as_.isActive()){
frontier_exploration::GetNextFrontier srv;
//placeholder for next goal to be sent to move base
geometry_msgs::PoseStamped goal_pose;
//get current robot pose in frame of exploration boundary
tf::Stamped<tf::Pose> robot_pose;
explore_costmap_ros_->getRobotPose(robot_pose);
//provide current robot pose to the frontier search service request
tf::poseStampedTFToMsg(robot_pose,srv.request.start_pose);
//evaluate if robot is within exploration boundary using robot_pose in boundary frame
geometry_msgs::PoseStamped eval_pose = srv.request.start_pose;
if(eval_pose.header.frame_id != goal->explore_boundary.header.frame_id){
tf_listener_.transformPose(goal->explore_boundary.header.frame_id, srv.request.start_pose, eval_pose);
}
//check if robot is not within exploration boundary and needs to return to center of search area
if(goal->explore_boundary.polygon.points.size() > 0 && !pointInPolygon(eval_pose.pose.position,goal->explore_boundary.polygon)){
//check if robot has explored at least one frontier, and promote debug message to warning
if(success_){
ROS_WARN("Robot left exploration boundary, returning to center");
}else{
ROS_DEBUG("Robot not initially in exploration boundary, traveling to center");
}
//get current robot position in frame of exploration center
geometry_msgs::PointStamped eval_point;
eval_point.header = eval_pose.header;
eval_point.point = eval_pose.pose.position;
if(eval_point.header.frame_id != goal->explore_center.header.frame_id){
geometry_msgs::PointStamped temp = eval_point;
tf_listener_.transformPoint(goal->explore_center.header.frame_id, temp, eval_point);
}
//set goal pose to exploration center
goal_pose.header = goal->explore_center.header;
goal_pose.pose.position = goal->explore_center.point;
goal_pose.pose.orientation = tf::createQuaternionMsgFromYaw( yawOfVector(eval_point.point, goal->explore_center.point) );
}else if(getNextFrontier.call(srv)){ //if in boundary, try to find next frontier to search
ROS_DEBUG("Found frontier to explore");
success_ = true;
goal_pose = feedback_.next_frontier = srv.response.next_frontier;
retry_ = 5;
}else{ //if no frontier found, check if search is successful
ROS_DEBUG("Couldn't find a frontier");
//search is succesful
if(retry_ == 0 && success_){
ROS_WARN("Finished exploring room");
as_.setSucceeded();
boost::unique_lock<boost::mutex> lock(move_client_lock_);
move_client_.cancelGoalsAtAndBeforeTime(ros::Time::now());
return;
}else if(retry_ == 0 || !ros::ok()){ //search is not successful
ROS_ERROR("Failed exploration");
as_.setAborted();
return;
}
ROS_DEBUG("Retrying...");
retry_--;
//try to find frontier again, without moving robot
continue;
}
//if above conditional does not escape this loop step, search has a valid goal_pose
//check if new goal is close to old goal, hence no need to resend
if(!moving_ || !pointsNearby(move_client_goal_.target_pose.pose.position,goal_pose.pose.position,goal_aliasing_*0.5)){
ROS_DEBUG("New exploration goal");
move_client_goal_.target_pose = goal_pose;
boost::unique_lock<boost::mutex> lock(move_client_lock_);
if(as_.isActive()){
move_client_.sendGoal(move_client_goal_, boost::bind(&FrontierExplorationServer::doneMovingCb, this, _1, _2),0,boost::bind(&FrontierExplorationServer::feedbackMovingCb, this, _1));
moving_ = true;
}
lock.unlock();
}
//check if continuous goal updating is enabled
if(frequency_ > 0){
//sleep for specified frequency and then continue searching
rate.sleep();
}else{
//wait for movement to finish before continuing
while(ros::ok() && as_.isActive() && moving_){
ros::WallDuration(0.1).sleep();
}
}
}
//goal should never be active at this point
ROS_ASSERT(!as_.isActive());
}
bool Polygon<T>::pointInPolygon(const Coord<T>& c) const {
return pointInPolygon(c.lat, c.lon);
}
bool Polygon<T>::operator&(const Coord<T>& c) {
return pointInPolygon(c);
}
/**
* - classifyies points in/outside crown
* - filters based on z values
* - writes points to file
*/
int extractPoints(int shapeIndex, FILE * fpOut)
{
printf("\tCrown Index : %d\n", (shapeIndex));
printf("\tN_POINTS : %d \n", N_POINTS);
int point, pointsInCrown, nAbove2m;
float * zValues, zThreshold;
double xValue, yValue, zValue;
SHPObject * pShpObj;
pointsInCrown = 0;
pShpObj = SHPReadObject(H_SHP, shapeIndex);
/**
* For each point determine if it lies within the current tree crown.
*/
for(point = 0; point < N_POINTS; point++)
{
xValue = CHM[0][point];
yValue = CHM[1][point];
zValue = CHM[2][point];
if(pointInPolygon(pShpObj, xValue, yValue))
{
POINT_CLASSIFICATION[point] = 1;
if(pointsInCrown == ARRAY_SIZE)
if((Z_VALUES = resize(Z_VALUES, &ARRAY_SIZE)) == NULL)
return 0;
Z_VALUES[pointsInCrown++] = zValue;
}
else{
POINT_CLASSIFICATION[point] = 0;
}
}
printf("\tPoints in Crown: %d\n", pointsInCrown);
if(pointsInCrown == 0){
return 0;
}
qsort(Z_VALUES, pointsInCrown, sizeof(float), floatCompare);
//zValues -> all points > 2 meters
zValues = pointsAbove2m(&nAbove2m, pointsInCrown);
zThreshold =
computeTreeCrownHeight(zValues, nAbove2m, 0.5);
if(zThreshold < 0)
return 0;
printf("\tHeight threshold: %f\n", zThreshold);
if(!writeToFile(fpOut, POINT_CLASSIFICATION, CHM, &N_POINTS, zThreshold))
return 0;
SHPDestroyObject(pShpObj);
return pointsInCrown;
}
