geometry_msgs::Point PathHandler::getPointOnPathWithDist(geometry_msgs::Point p, double dist)
{
if (getPathSize() <= 1) // nothing to do here
{
ROS_ERROR("Called getPointOnPathWithDist with a too small path!");
return p;
}
geometry_msgs::Point result;
double d = 0.0;
uint32_t i = 0;
for (i = mCurrentPathIndex; i < getPathSize() - 1; i++)
{
d = distanceBetweenPoints(p, mPath[i+1].position);
if (dist >= d)
dist = dist - d;
else
break;
p = mPath[i+1].position;
}
if (i < getPathSize() - 1)
{
double scale = dist / distanceBetweenPoints(p, mPath[i+1].position);
result.x = p.x + scale * (mPath[i+1].position.x - p.x);
result.y = p.y + scale * (mPath[i+1].position.y - p.y);
}
else
result = mPath[getPathSize() - 1].position;
return result;
}
PathNode* PathFinder::getPathNodeAtIndex(int aIndex)
{
if(aIndex >= 0 && aIndex < getPathSize())
{
return m_PathNodeFinal.at(aIndex);
}
return NULL;
}
void PathHandler::scanCb(const sensor_msgs::LaserScan &scan)
{
if (mFollowState != FOLLOW_STATE_BUSY || updateCurrentPosition() == false || getPathSize() == 0)
return;
uint32_t size = (scan.angle_max - scan.angle_min) / scan.angle_increment;
for (uint32_t i = 0; i < size; i++)
{
if (scan.ranges[i] >= scan.range_max)
continue;
geometry_msgs::PointStamped tmp, p;
tmp.header.frame_id = "/base_link";
tmp.header.stamp = ros::Time(0);
tmp.point.x = scan.ranges[i] * cosf(scan.angle_min + scan.angle_increment * i);
tmp.point.y = scan.ranges[i] * sinf(scan.angle_min + scan.angle_increment * i);
try
{
mTransformListener.transformPoint("/map", tmp, p);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
return;
}
for (uint32_t j = mCurrentPathIndex; j < getPathSize() - 1; j++)
{
geometry_msgs::Point closest = closestPointOnLine(p.point, mPath[j].position, mPath[j+1].position);
if (distanceBetweenPoints(p.point, closest) <= ROBOT_RADIUS)
{
ROS_INFO("Detected obstacle on path, resending goal. Distance: %f", distanceBetweenPoints(p.point, closest));
resendGoal();
return;
}
}
}
}
int
findLongestNumTwo(int startNum, int endNum)
{
int result = 0;
int tempResult = 0;
int i = 0;
int j = 0;
for(i = startNum; i <= endNum; i++)
{
//printf("\n==============start\n");
tempResult = getPathSize(i);
//printf("[%d] num of path %d\n", i, tempResult);
//printf("================end\n");
if(result < tempResult)
{
result = tempResult;
}
}
return result;
}
/*
Returns the file path to append data to if the path is valid.
Returns NULL if file path is not valid
*/
char *getValidFilePath(node *root) {
char *currPath = getcwd(NULL, 0); //get_current_dir_name();
if(currPath == NULL) {
printf("ERROR: Could not get current path\n");
return NULL;
}
int size = getPathSize(root);
char *path = (char *) malloc((size+1) * sizeof(char));
if(path == NULL) {
printf("ERROR: Not enough memory to malloc\n");
return NULL;
}
strncpy(path, root->name, strlen(root->name)+1);
root = root->next;
while(root != NULL) {
strncat(path, root->name, strlen(root->name)+1);
root = root->next;
}
char temp[strlen(path) + 1];
strncpy(temp, path, strlen(path) + 1);
char *requestDir = dirname(temp);
if(strncmp(requestDir, currPath, strlen(currPath)+1) != 0) {
if(strncmp(requestDir, "/tmp", 5) != 0 || strlen(requestDir) != 4) {
printf("ERROR: File path must be in the current directory or /tmp\n");
free(currPath);
free(path);
return NULL;
}
}
free(currPath);
return path;
}
int Path::getClosestWaypoint()
{
// std::cout << "==GetClosestWaypoint==" << std::endl;
if (!getPathSize())
return -1;
int closest_threshold = 5;
//decide search radius
for (int ratio = 1; ratio < closest_threshold; ratio++) {
double distance_threshold = 2 * ratio * getInterval(); //meter
// std::cout << "distance_threshold : " << distance_threshold << std::endl;
std::vector<int> waypoint_candidates;
//search closest candidate
for (int i = 1; i < getPathSize(); i++) {
//std::cout << waypoint << std::endl;
//skip waypoint behind vehicle
if (transformWaypoint(i).x() < 0)
continue;
if (getDistance(i) < distance_threshold) {
waypoint_candidates.push_back(i);
//std::cout << "waypoint = " << i << " distance = " << getDistance(i) << std::endl;
}
}
if (waypoint_candidates.size() == 0) {
continue;
}
//search substraction minimum between candidate and previous closest
int substraction_minimum = 0;
int decided_waypoint = 0;
int initial_minimum = 0;
//decide initial minimum
for (unsigned int i = 0; i < waypoint_candidates.size(); i++) {
substraction_minimum = waypoint_candidates[i] - closest_waypoint_;
if (substraction_minimum < 0)
continue;
if (substraction_minimum >= 0) {
decided_waypoint = waypoint_candidates[i];
initial_minimum = i;
break;
}
}
//calc closest
for (unsigned int i = initial_minimum; i < waypoint_candidates.size(); i++) {
int sub = waypoint_candidates[i] - closest_waypoint_;
//std::cout << "closest candidates : " << waypoint_candidates[i] << " sub : " << sub << std::endl;
if (sub < 0)
continue;
if (sub < substraction_minimum) {
decided_waypoint = waypoint_candidates[i];
substraction_minimum = sub;
}
}
if (decided_waypoint >= closest_waypoint_) {
closest_waypoint_ = decided_waypoint;
return decided_waypoint;
}
}
return -1;
}
/**
* Updates path logic.
*/
void PathHandler::updatePath()
{
geometry_msgs::Twist command;
publishState();
// no path? nothing to handle
if (getPathSize() == 0)
return;
// update our position if possible.
if (updateCurrentPosition() == false)
{
ROS_ERROR("Failed to update robot position.");
return;
}
if (mCurrentPathIndex < getPathSize() - 1) // we are moving along a line segment in the path
{
geometry_msgs::Point robotPos;
robotPos.x = mRobotPosition.getOrigin().x();
robotPos.y = mRobotPosition.getOrigin().y();
geometry_msgs::Point closestOnPath = closestPointOnLine(robotPos, mPath[mCurrentPathIndex].position, mPath[mCurrentPathIndex + 1].position);
//ROS_INFO("robotPos[%lf, %lf], closestOnPath[%lf, %lf]", robotPos.x, robotPos.y, closestOnPath.x, closestOnPath.y);
if (distanceBetweenPoints(closestOnPath, robotPos) > mResetDistanceTolerance)
{
ROS_INFO("Drove too far away from path, re-sending goal.");
mFollowState = FOLLOW_STATE_IDLE;
resendGoal();
clearPath();
return;
}
geometry_msgs::Point pointOnPath = getPointOnPathWithDist(closestOnPath, mLookForwardDistance);
//ROS_INFO("closestOnPath[%lf, %lf], pointOnPath[%lf, %lf]", closestOnPath.x, closestOnPath.y, pointOnPath.x, pointOnPath.y);
double angle = angleTo(pointOnPath);
/*double robotYaw = tf::getYaw(mRobotPosition.getRotation()); // only used for printing
ROS_INFO("Follow state: %d Robot pos: (%lf, %lf, %lf). Target pos: (%lf, %lf, %lf). RobotYaw: %lf. FocusYaw: %lf", mFollowState, mRobotPosition.getOrigin().getX(),
mRobotPosition.getOrigin().getY(), mRobotPosition.getOrigin().getZ(), pointOnPath.x,
pointOnPath.y, pointOnPath.z, robotYaw, angle);*/
//ROS_INFO("Angle to path: %f", angle);
command.linear.x = getScaledLinearSpeed(angle);
command.angular.z = getScaledAngularSpeed(angle);
if (distanceBetweenPoints(closestOnPath, mPath[mCurrentPathIndex + 1].position) < mDistanceTolerance)
{
//ROS_INFO("Moving to next line segment on path.");
mCurrentPathIndex++;
}
publishLocalPath(command.linear.x * 3, angle);
}
else // only rotate for final yaw
{
double yaw = tf::getYaw(mPath[getPathSize() - 1].orientation);
btVector3 orientation = btVector3(cos(yaw), sin(yaw), 0.0).rotate(btVector3(0,0,1), -tf::getYaw(mRobotPosition.getRotation()));
double angle = atan2(orientation.getY(), orientation.getX());
//ROS_INFO("Angle to final orientation: %f", angle);
if (fabs(angle) > mFinalYawTolerance)
command.angular.z = getScaledAngularSpeed(angle, true);
else
{
// path is done!
mFollowState = FOLLOW_STATE_FINISHED;
clearPath();
}
publishLocalPath(1.0, angle);
}
mCommandPub.publish(command);
}
