Vertex Region::findClosestRegionPoint(const Vertex &point) const {
// Determine whether the point is inside a hole. If that is the case, the closest
// point on the edge of the hole is determined
int polygonIdx = 0;
{
for (uint i = 1; i < _polygons.size(); ++i) {
if (_polygons[i].isPointInPolygon(point)) {
polygonIdx = i;
break;
}
}
}
const Polygon &polygon = _polygons[polygonIdx];
assert(polygon.vertexCount > 1);
// For each line of the polygon, calculate the point that is cloest to the given point
// The point of this set with the smallest distance to the given point is the result.
Vertex closestVertex = findClosestPointOnLine(polygon.vertices[0], polygon.vertices[1], point);
int closestVertexDistance2 = closestVertex.distance(point);
for (int i = 1; i < polygon.vertexCount; ++i) {
int j = (i + 1) % polygon.vertexCount;
Vertex curVertex = findClosestPointOnLine(polygon.vertices[i], polygon.vertices[j], point);
if (curVertex.distance(point) < closestVertexDistance2) {
closestVertex = curVertex;
closestVertexDistance2 = curVertex.distance(point);
}
}
// Determine whether the point is really within the region. This must not be so, as a result of rounding
// errors can occur at the edge of polygons
if (isPointInRegion(closestVertex))
return closestVertex;
else {
// Try to construct a point within the region - 8 points are tested in the immediate vacinity
// of the point
if (isPointInRegion(closestVertex + Vertex(-2, -2)))
return closestVertex + Vertex(-2, -2);
else if (isPointInRegion(closestVertex + Vertex(0, -2)))
return closestVertex + Vertex(0, -2);
else if (isPointInRegion(closestVertex + Vertex(2, -2)))
return closestVertex + Vertex(2, -2);
else if (isPointInRegion(closestVertex + Vertex(-2, 0)))
return closestVertex + Vertex(-2, 0);
else if (isPointInRegion(closestVertex + Vertex(0, 2)))
return closestVertex + Vertex(0, 2);
else if (isPointInRegion(closestVertex + Vertex(-2, 2)))
return closestVertex + Vertex(-2, 2);
else if (isPointInRegion(closestVertex + Vertex(-2, 0)))
return closestVertex + Vertex(2, 2);
else if (isPointInRegion(closestVertex + Vertex(2, 2)))
return closestVertex + Vertex(2, 2);
// If no point could be found that way that lies within the region, find the next point
closestVertex = polygon.vertices[0];
int shortestVertexDistance2 = polygon.vertices[0].sqrDist(point);
{
for (int i = 1; i < polygon.vertexCount; i++) {
int curDistance2 = polygon.vertices[i].sqrDist(point);
if (curDistance2 < shortestVertexDistance2) {
closestVertex = polygon.vertices[i];
shortestVertexDistance2 = curDistance2;
}
}
}
warning("Clostest vertex forced because edgepoint was outside region.");
return closestVertex;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "path_follower"); //Ros Initialize
ros::start();
ros::Rate loop_rate(T); //Set Ros frequency to 50/s (fast)
ros::NodeHandle n;
ros::Subscriber pathSub;
ros::Subscriber poseEstSub;
ros::Subscriber centroidEstSub;
ros::Publisher goalPub;
ros::Publisher velPub;
ros::Publisher centroidPub;
ros::Publisher gaussPosition;
pathSub = n.subscribe<geometry_msgs::PoseArray>("/path", 1, pathCallback);
poseEstSub = n.subscribe<geometry_msgs::PoseStamped>("/poseEstimation", 1, poseEstCallback);
centroidEstSub = n.subscribe<tf2_msgs::TFMessage>("/poseEstimationC", 1, centroidEstCallback);
goalPub = n.advertise<geometry_msgs::PoseStamped>("/goal_pose", 1000, true);
centroidPub = n.advertise<geometry_msgs::PoseArray>("/gauss", 1000, true);
velPub = n.advertise<geometry_msgs::Twist>("/path_vel", 1000, true);
gaussPosition = n.advertise<geometry_msgs::PoseStamped>("/poseEstimation", 1000, true);
// Initialize msgs and flags
newPath = false;
closestPointOnLine.pose.position.x = 0;
closestPointOnLine.pose.position.y = 0;
constVelTerm.linear.x = 0;
constVelTerm.linear.y = 0;
constVelTerm.linear.z = 0;
constVelTerm.angular.x = 0;
constVelTerm.angular.y = 0;
constVelTerm.angular.z = 0;
gauss.poses.resize(50);
gPos.header.frame_id="gauss";
while (ros::ok())
{
ros::spinOnce();
if(newPath) // check if a new path has been set
{
findIndexOfLastPointOnPath();
if(isOpenLoop) // path given is OPEN
{
newPath = false; // reset flag
goalPose.pose = (pathPose.poses)[0]; // publish first point on path
goalPose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
if (pathPose.header.frame_id.compare("SWARM")==0){
gauss.poses[0]=goalPose.pose;
gauss.poses[0].position.z=.25;
gPos.pose=goalPose.pose;
gPos.pose.position.z=.25;
centroidPub.publish(gauss);
gaussPosition.publish(gPos);
}
else {
goalPub.publish(goalPose);
}
while( distanceFormula(pathPose.poses[0].position.x, poseEst.pose.position.x,
pathPose.poses[0].position.y, poseEst.pose.position.y) >= BOUNDARY_RADIUS ) // FIXME: Implement this sleep cycle as a function
{
ros::spinOnce();
loop_rate.sleep();
if(newPath || !ros::ok())
{
break;
}
}
closestPointIndex = 0; // initialize to first point in path
while(closestPointIndex != lastPointOnPathIndex) // use interpolation
{
if(newPath || !ros::ok()) // FIXME: break out if a different pose is published
{
break;
}
//std::cout << "goalPose" << gPos <<"\n\n";
//ROS_INFO("on interpolation loop OPEN\n");
findClosestPointOnLine();
closestPointOnLine.pose.orientation = tf::createQuaternionMsgFromYaw(0);
calcConstVelTerm();
// std::cout << "Goal pose:\n" << closestPointOnLine << "\n\n";
//std::cout << "Constant vel:\n" << constVelTerm << "\n\n";
velPub.publish(constVelTerm);
if (pathPose.header.frame_id.compare("SWARM")==0){
gauss.poses[0]=goalPose.pose;
gauss.poses[0].position.z=.25;
gPos.pose=goalPose.pose;
gPos.pose.position.z=.25;
centroidPub.publish(gauss);
gaussPosition.publish(gPos);
}
else {
goalPub.publish(goalPose);
}
//pathPose.poses[closestPointIndex].position.x = 0;
//pathPose.poses[closestPointIndex].position.y = 0;
calcClosestPointOnPath();
ros::spinOnce();
loop_rate.sleep();
}
goalPose.pose = (pathPose.poses)[lastPointOnPathIndex]; // publish final point on path
goalPose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
if (pathPose.header.frame_id.compare("SWARM")==0){
gauss.poses[0]=goalPose.pose;
gauss.poses[0].position.z=.25;
gPos.pose=goalPose.pose;
gPos.pose.position.z=.25;
centroidPub.publish(gauss);
gaussPosition.publish(gPos);
}
else {
goalPub.publish(goalPose);
}
//pathPose.poses[closestPointIndex].position.x = 0;
//pathPose.poses[closestPointIndex].position.y = 0;
constVelTerm.linear.x = 0;
constVelTerm.linear.y = 0;
velPub.publish(constVelTerm);
// FIXME: reset path variables
//prevClosestPointIndex = 0;
}
else // path given is CLOSED
{
newPath = false; // reset flag and set stopping condition for while loop
calcClosestPointOnPath();
sortPathArray(); // array now starts at the closest point index
closestPointIndex = 0;
while( !newPath || ros::ok() ) // while no new path has been published
{
//ROS_INFO("on interpolation loop CLOSED");
findClosestPointOnLine();
closestPointOnLine.pose.orientation = tf::createQuaternionMsgFromYaw(0);
calcConstVelTerm();
//std::cout << "Goal pose:\n" << closestPointOnLine << "\n\n";
//std::cout << "Constant vel:\n" << constVelTerm << "\n\n";
velPub.publish(constVelTerm);
goalPub.publish(closestPointOnLine);
calcClosestPointOnPath();
if (closestPointIndex == lastPointOnPathIndex)
{
closestPointIndex = 0;
}
ros::spinOnce();
loop_rate.sleep();
}
constVelTerm.linear.x = 0;
constVelTerm.linear.y = 0;
velPub.publish(constVelTerm);
//ROS_INFO("finished CLOSED loop");
}
}
loop_rate.sleep();
}
}
