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;
}
Point PathFollowingGeometry::getFeedforwardCurveCenterPoint()
{
if (!this->has_feedforward_center) {
PointArr points = this->getCurvatureDetectionPoints();
if (points.size() >= 5) {
Point p1 = points.at(0);
Point p2 = points.at(2);
Point p3 = points.at(4);
//ROS_INFO("p1 %.2f,%2.f; p2 %.2f,%2.f; p3 %.2f,%2.f", p1.x, p1.y, p2.x, p2.y, p3.x, p3.y);
//Equation 12
double l12 = (p2.y-p1.y)/(p2.x-p1.x);
double l23 = (p3.y-p2.y)/(p3.x-p2.x);
//Equation 10
this->feedforward_center.x = -(p1.y+p2.y)/2+(p2.y+p3.y)/2
-(p1.x+p2.x)/(2*l12)+(p2.x+p3.x)/(2*l23);
//Equation 11
this->feedforward_center.y = -(p2.y+p3.y)/2-this->feedforward_center.x/l23*this->feedforward_center.x+(p2.x+p3.x)/(2*l23);
this->has_feedforward_center = true;
}
else {
//ROS_WARN("Can't get curve center. Expected 5 detection points.");
}
}
return this->feedforward_center;
}
void GpsItem::download_tracklog()
{
wstatus = W_DOWNLOADING;
progress = 0;
Gipsy::notify(this, "gps_changed");
if (gpstype == G_COMPEO || gpstype == G_IQ) {
for (size_t i=0; i < selected_tracks.size(); i++) {
string igccontent = gps->download_igc(selected_tracks[i], _progress_updater, (void *)this);
if (igccontent.size()) {
Igc *igc = new OriginalIgc(igccontent);
igc->gpsname = gps->gpsname;
igc->gpsunitid = gps->gpsunitid;
obtained_tracklog(igc, false);
}
}
wstatus = W_DOWNCOMPLETE;
Gipsy::notify(this, "gps_changed");
} else {
gps->selected_tracks = selected_tracks;
PointArr parr = gps->download_tracklog(_progress_updater, (void *)this);
if (gpstype == GPS_MLR)
wstatus = W_CONNECTED;
else
wstatus = W_DOWNCOMPLETE;
Gipsy::notify(this, "gps_changed");
// If tracklog is not empty...
if (!parr.size())
return;
// Add tracklog to downloaded_tracklog vector
// AddRef will be done in the notification handler
Igc *igc = new Igc(parr, gps->gpsname, gps->gpsunitid);
// Add custom signed fields
igc->x_params.push_back(pair<string,string>("Xversion", GIPSY_VERSION));
time_t now = time(NULL);
char tmptime[31];
struct tm mnow;
strftime(tmptime, 30, "%Y-%m-%d %H:%M:%S GMT", gmtime_r(&now, &mnow));
igc->x_params.push_back(pair<string,string>("XDownloadtime", tmptime));
igc->gen_g_record(); // Generate g-record on the downloaded tracklog
obtained_tracklog(igc, true);
}
}
/* Create new tracklog from select break-sections */
NS_IMETHODIMP Tracklog::IgcBreakSelect(uint32_t count, int32_t *bpoints,
IGPSIGC **_retval)
{
PointArr selection;
int lastidx = -1;
int bpointcount;
if (!count) {
*_retval = NULL;
return NS_OK;
}
IgcBreakCount(&bpointcount);
for (unsigned int i=0; i < count; i++) {
int idx = bpoints[i];
if (idx < 0 || idx >= bpointcount)
return NS_ERROR_UNEXPECTED;
// Check that the input array is sorted
if (idx < lastidx)
return NS_ERROR_UNEXPECTED;
lastidx = idx;
int bstart;
int len;
IgcBreak(idx, &bstart);
IgcBreakLen(idx, &len);
for (int j=0;j < len; j++)
selection.push_back(igc->tracklog[j + bstart]);
}
Igc *newigc = new Igc(*igc);
newigc->tracklog = selection;
if (igc->validate()) {
if (! newigc->gen_g_record()) // Failed crypto library?
return NS_ERROR_UNEXPECTED;
}
*_retval = new Tracklog(newigc);
NS_ADDREF(*_retval);
return NS_OK;
}
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;
}
int main(int argc, char **argv)
{
//cout << "Getting ports" << endl;
//PortList ports = get_ports(true);
Gps *gps = make_gps("/dev/ttyUSB0", GPS_FLYMASTER);
cout << "GPS type: " << gps->gpsname << ", unitid: " << gps->gpsunitid << endl;
cerr << "Downloading" << endl;
gps->selected_tracks.push_back(0);
try {
PointArr result = gps->download_tracklog(NULL, NULL);
for (int i=0; i < result.size(); i++) {
Trackpoint *point = &result[i];
cout << point->lat << " " << point->lon << " " << point->new_trk << " " << point->time << endl;
}
} catch (Exception e) {
cerr << e.error << endl;
exit(1);
}
/*
for (unsigned int i=0; i < ports.size(); i++) {
cout << "Device: " << ports[i].device << ", devname: " << ports[i].devname << endl;
try {
Gps *gps = make_gps(ports[i].device, GPS_GARMIN);
if (gps) {
cout << "GPS type: " << gps->gpsname << ", unitid: " << gps->gpsunitid << endl;
PointArr result;
result = gps->download_tracklog(NULL, NULL);
for (int i=0; i < result.size(); i++) {
Trackpoint *point = &result[i];
cout << point->lat << " " << point->lon << " " << point->new_trk << " " << point->time << endl;
}
cout << "done" << endl;
} else {
cout << "GPS open failed" << endl;
}
} catch (Exception e) {
cout << "Got exception, GPS open failed: " << e.error << endl;
}
}
*/
}
PointArr PathFollowingGeometry::interpolate(Point p1, Point p2)
{
PointArr result;
double length_between_waypoints = distance(p1, p2);
int temp = round(length_between_waypoints*INTERPOLATION_PTS_PER_M);
unsigned int num_points = std::max(1, temp);
//ROS_INFO("Interpolating %f with %d pts", length_between_waypoints, num_points);
result.push_back(p1);
for (unsigned int i = 1; i < num_points-1; i++) {
double fraction = (double)(i/(double)num_points);
//ROS_INFO("Fraction %f", fraction);
double x = p1.x+(p2.x-p1.x)*fraction;
double y = p1.y+(p2.y-p1.y)*fraction;
result.push_back(CreatePoint(x, y));
}
result.push_back(p2);
return result;
}
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;
}
/* Expect that the array is sorted */
NS_IMETHODIMP Tracklog::IgcSelectPoints(uint32_t count, int32_t *points,
IGPSIGC **_retval)
{
PointArr selection;
int lastidx = -1;
if (!count) {
*_retval = NULL;
return NS_OK;
}
for (unsigned int i=0; i < count; i++) {
int idx = points[i];
if (idx < 0 || idx >= (int) igc->tracklog.size())
return NS_ERROR_UNEXPECTED;
// Check that the input array is sorted
if (idx < lastidx)
return NS_ERROR_UNEXPECTED;
lastidx = idx;
selection.push_back(igc->tracklog[idx]);
}
Igc *newigc = new Igc(*igc);
newigc->tracklog = selection;
if (igc->validate())
newigc->gen_g_record();
*_retval = new Tracklog(newigc);
NS_ADDREF(*_retval);
return NS_OK;
}
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;
}
NS_IMETHODIMP Tracklog::IgcLoadGPX(nsILocalFile *file)
{
nsresult rv;
int64_t fsize;
nsCOMPtr<nsIDOMParser> domparser = do_CreateInstance("@mozilla.org/xmlextras/domparser;1", &rv);
if (NS_FAILED(rv))
return rv;
file->GetFileSize(&fsize);
nsCOMPtr<nsIFileInputStream> instream=
do_CreateInstance(NS_LOCALFILEINPUTSTREAM_CONTRACTID, &rv);
if (NS_FAILED(rv))
return rv;
rv = instream->Init(file, PR_RDONLY, 0, nsIFileInputStream::CLOSE_ON_EOF);
if (NS_FAILED(rv))
return rv;
nsCOMPtr<nsIDOMDocument> document;
rv = domparser->ParseFromStream(instream, "UTF-8", fsize, "text/xml", getter_AddRefs(document));
if (NS_FAILED(rv))
return rv;
nsCOMPtr<nsIDOMNodeList> seglist;
rv = document->GetElementsByTagName(NS_LITERAL_STRING("trkseg"), getter_AddRefs(seglist));
if (NS_FAILED(rv))
return rv;
uint32_t listlength;
seglist->GetLength(&listlength);
PointArr parr;
for (uint32_t i=0; i < listlength; i++) {
nsCOMPtr<nsIDOMNodeList> pointlist;
nsCOMPtr<nsIDOMNode> trkseg;
seglist->Item(i, getter_AddRefs(trkseg));
rv = document->GetElementsByTagName(NS_LITERAL_STRING("trkpt"), getter_AddRefs(pointlist));
if (NS_FAILED(rv))
return rv;
uint32_t pointnum;
pointlist->GetLength(&pointnum);
for (uint32_t j=0; j < pointnum; j++) {
nsCOMPtr<nsIDOMNode> trkpt_n;
pointlist->Item(j, getter_AddRefs(trkpt_n));
nsCOMPtr<nsIDOMElement> trkpt(do_QueryInterface(trkpt_n));
Trackpoint point;
point.new_trk = (j == 0);
/* Lat & Lon */
nsAutoString lstr;
trkpt->GetAttribute(NS_LITERAL_STRING("lat"), lstr);
point.lat = my_atof(NS_ConvertUTF16toUTF8(lstr).get());
trkpt->GetAttribute(NS_LITERAL_STRING("lon"), lstr);
point.lon = my_atof(NS_ConvertUTF16toUTF8(lstr).get());
string alt = get_subelement(NS_LITERAL_STRING("ele"), trkpt);
point.gpsalt = my_atof(alt.c_str());
string time = get_subelement(NS_LITERAL_STRING("time"), trkpt);
point.time = decode_time(time);
parr.push_back(point);
}
}
igc = new Igc(parr, "GPX Import", 0);
make_break_points();
return NS_OK;
}
/* Download tracklog using A300, A301 & A302 protocols */
PointArr GarminGps::A30xdownload(int proto, dt_callback cb, void *arg)
{
uint8_t pid = 0;
Data data;
PointArr result;
uint16_t cmdid;
Trackpoint point;
int pktcount = 0;
bool suspend = false; // If true, do not save trackpoints
int dataproto = (proto == 300) ? 300 : proto_data[proto][1];
send_command(Cmnd_Transfer_Trk);
int exp_count = get_records();
if (!exp_count) // No trackpoints expected
return result;
while (pid != Pid_Xfer_Complete) {
phys->recv_packet(pid, data);
if (cb) {
bool rv = cb(arg, pktcount, exp_count);
if (!rv) { // Abort
send_command(Cmnd_Abort_Transfer);
throw Exception("Aborted.");
}
}
pktcount++;
switch (pid) {
case Pid_Trk_Hdr:
// if the protocol is not D310 || D312
if (proto_data[proto][0] != 310 && proto_data[proto][0] != 312)
break;
// If we are "high version", download all tracklogs
if (gpsid >= NEW_VERSION)
break;
// else throw out everything that is not 'active log'
if (!strncmp((const char *)&data.buffer[2], "ACTIVE LOG", data.size-2))
suspend = false;
else
suspend = true;
break;
case Pid_Trk_Data:
if (!suspend) {
point = decode_trk_data(data, dataproto);
// Discard if latitude > 90
// Foretrex signals lost fix with 0x7FFFFFFF values, but latitude
// should be in (-90,90) anyway
if (point.lat > 90 || point.lat < -90)
;
// If the point is older then the last point, discard the
// last point
else if (gpsid < NEW_VERSION && result.size() && point.time < result[result.size() - 1].time)
result[result.size() - 1] = point;
else
result.push_back(point);
}
break;
case Pid_Xfer_Complete:
cmdid = le16_to_host(*((uint16_t *)data.buffer));
if (cmdid != Cmnd_Transfer_Trk)
throw Exception("Bad command id on command completion.");
break;
default:
throw Exception("Unexpected data packed.");
}
}
return result;
}
