double PathFollowingGeometry::getCurveRadius()
{
if (!this->has_curve_radius) {
PointArr points = this->getCurvatureDetectionPoints();
if (!points.empty()) {
Point center = this->getFeedforwardCurveCenterPoint();
//Equation 13
double radius = 0;
for (PointArrIt it = points.begin(); it < points.end(); it++) {
radius += distance(*it, center);
}
radius /= points.size();
this->curve_radius = radius;
//std::stringstream sstr;
//sstr << center;
//ROS_INFO("Getting curve radius %f, pts.size %d. %.2f, %.2f", radius, (unsigned int)points.size(), center.x, center.y);
this->has_curve_radius = true;
}
else {
//ROS_WARN("Failed to calculate radius. Detection points are empty");
}
}
return this->curve_radius;
}
void PathFollowingGeometry::updateLocalFrame()
{
//Transform points to robot-centered coordinate frame
//Depending on where (left or right) feedback path point is
//make reference distance positive or negative
//Transformation
this->feedback_path_point_in_local_frame = this->localFramePoint(this->getFeedbackPathPoint());
this->feedback_point_in_local_frame = this->localFramePoint(this->getFeedbackPoint());
this->feedforward_point_in_local_frame = this->localFramePoint(this->getFeedforwardPoint());
this->deviation_path_point_in_local_frame = this->localFramePoint(this->getDeviationPathPoint());
PointArr points = this->getCurvatureDetectionPoints();
this->curvature_detection_points_in_local_frame.clear();
for (PointArrIt it = points.begin(); it < points.end(); it++) {
this->curvature_detection_points_in_local_frame.push_back(this->localFramePoint(*it));
}
this->has_local_frame = true;
}
Point PathFollowingGeometry::getClosestPointFromSet(Point referencePoint, PointArr pointSet)
{
Point result;
if (pointSet.size() > 0) {
result = pointSet.at(0);
double closest_distance = distance(referencePoint, result);
for (PointArrIt it = pointSet.begin()+1; it < pointSet.end(); it++) {
double new_distance = distance(referencePoint, *it);
//ROS_INFO("%.2f,%.2f -> %.2f,%.2f = %.4f", referencePoint.x, referencePoint.y, (*it).x, (*it).y, new_distance);
if (new_distance < closest_distance) {
closest_distance = new_distance;
result = *it;
}
}
// ROS_WARN("Shortest %.2f,%.2f -> %.2f,%.2f = %.4f", referencePoint.x, referencePoint.y, result.x, result.y, closest_distance);
}
else {
result = referencePoint;
}
return result;
}
void MotionControlNode::callbackGoal(const lunabotics::Goal::ConstPtr &msg)
{
this->_waypoints.clear();
this->_motionConstraints.point_turn_angle_accuracy = msg->angleAccuracy;
this->_motionConstraints.point_turn_distance_accuracy = msg->distanceAccuracy;
//Specify params
Point goal = Point_from_geometry_msgs_Point(msg->point);
//ROS_INFO("Requesting path between (%.1f,%.1f) and (%.1f,%.1f)",
// this->_currentPose.position.x, this->_currentPose.position.y,
// goal.x, goal.y);
float resolution;
PathPtr path = this->findPath(this->_currentPose, goal, resolution);
if (path->is_initialized()) {
std::stringstream sstr;
nav_msgs::Path pathMsg;
pathMsg.header.stamp = ros::Time::now();
pathMsg.header.seq = this->_sequence++;
pathMsg.header.frame_id = "map";
PointArr corner_points = path->cornerPoints(resolution);
PointArr pts;
if (this->_steeringMode == lunabotics::proto::ACKERMANN) {
unsigned int size = corner_points.size();
if (size > 2) {
delete this->_trajectory;
this->_trajectory = new Trajectory();
Point startPoint = corner_points.at(0);
Point endPoint = corner_points.at(size-1);
IndexedPointArr closest_obstacles = path->closestObstaclePoints(resolution);
unsigned int obst_size = closest_obstacles.size();
pts.push_back(startPoint);
//Get bezier quadratic curves for each point-turn
for (unsigned int i = 1; i < size-1; i++) {
Point q0, q2;
Point prev = corner_points.at(i-1);
Point curr = corner_points.at(i);
Point next = corner_points.at(i+1);
bool hasObstacle = false;
Point obstaclePoint;
//Since obstacle is the center of occupied cell, we want p to be at its edge
if (obst_size > 0) {
int start = std::min(obst_size-1, i-1);
for (int j = start; j >= 0; j--) {
IndexedPoint indexedObstacle = closest_obstacles.at(j);
if (indexedObstacle.index == (int)i) {
hasObstacle = true;
obstaclePoint = indexedObstacle.point;
break;
}
}
}
if (i == 1) {
q0 = prev;
}
else {
q0 = midPoint(prev, curr);
}
if (i == size-2) {
q2 = next;
}
else {
q2 = midPoint(next, curr);
}
TrajectorySegment s;
if (hasObstacle) {
Point p = midPoint(obstaclePoint, curr);
s.curve = CreateConstrainedBezierCurve(q0, curr, q2, p, this->_motionConstraints.bezier_segments_num);
//ROS_INFO("Curve from tetragonal q0=(%f,%f) q1=(%f,%f), q2=(%f,%f), p=(%f,%f)", q0.x, q0.y, curr.x, curr.y, q2.x, q2.y, p.x, p.y);
}
else {
s.curve = new BezierCurve(q0, curr, q2, this->_motionConstraints.bezier_segments_num);
//ROS_INFO("Curve without tetragonal q0=(%f,%f) q1=(%f,%f), q2=(%f,%f)", q0.x, q0.y, curr.x, curr.y, q2.x, q2.y);
}
this->_trajectory->appendSegment(s);
}
pts = this->_trajectory->getPoints();
pts.push_back(endPoint);
ROS_WARN("Trajectory max curvature %f (Min ICR radius %f m)", this->_trajectory->maxCurvature(), 1/this->_trajectory->maxCurvature());
}
else {
pts = corner_points;
}
}
else {
pts = corner_points;
}
int poseSeq = 0;
for (PointArr::iterator it = pts.begin(); it != pts.end(); it++) {
Point pt = *it;
//float x_m = node.x*resolution;
//float y_m = node.y*resolution;
//float x_m = pt.x;
//float y_m = pt.y;
this->_waypoints.push_back(pt);
// sstr << "->(" << x_m << "," << y_m << ")";
geometry_msgs::PoseStamped pose;
pose.header.seq = poseSeq++;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = "map";
pose.pose.position = geometry_msgs_Point_from_Point(pt);
pose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
pathMsg.poses.push_back(pose);
}
this->_PIDHelper->setTrajectory(pts);
this->_waypointsIt = this->_steeringMode == lunabotics::proto::ACKERMANN ? this->_waypoints.begin() : this->_waypoints.begin()+1;
// ROS_INFO("Returned path: %s", sstr.str().c_str());
//Point waypoint = this->_waypoints.at(0);
//ROS_INFO("Heading towards (%.1f,%.1f)", (*this->_waypointsIt).x, (*this->_waypointsIt).y);
this->_publisherPath.publish(pathMsg);
this->_autonomyEnabled = true;
lunabotics::ControlParams controlParamsMsg;
controlParamsMsg.driving = this->_autonomyEnabled;
controlParamsMsg.next_waypoint_idx = this->_waypointsIt < this->_waypoints.end() ? this->_waypointsIt-this->_waypoints.begin()+1 : 0;
this->_publisherControlParams.publish(controlParamsMsg);
}
else {
ROS_INFO("Path is empty");
}
delete path;
}