as_msgs

[yeziheng123]

"Hello, I am new to this algorithm and I would like to learn more about it. Would it be possible to open source the message package for this project so that I can study it? Thank you."

[origovi]

You do not need the whole as_msgs specification, you can guess what is needed from the code.
What you need to do is change all appearances of as_msgs in the code for your I/O message package. Here is all you need to know:

1. as_msgs::Cone is the message that represents a cone. This cone has the two coordinates (x, y) in two frames (local, referencing the car and global, referencing the starting point), only global is mandatory for Urinay to run in newest version, and a unique id that will be immutable throughout the run, this way, Urinay can relate two observations of the same cone (and thus update its position). As seen in the Node constructor, it just takes these elements and saves them.
   

   urinay/src/structures/Node.cpp

   Lines 29 to 30 in 717f033

   	Node::Node(const as_msgs::Cone &c)
   	: Node(c.position_baseLink.x, c.position_baseLink.y, c.position_global.x, c.position_global.y, c.id) {}

2. as_msgs::CarState is the message that contains the 3D car position and heading (referenced to the starting point).
   As seen in the function below, the whole purpose of the callback is to obtain a valid transform (this->localTf_, which is a Eigen::Affine3d) from our position and heading. Note: position and heading do NOT need to be 3D.
   

   urinay/src/modules/WayComputer.cpp

   Lines 263 to 274 in 717f033

   	void WayComputer::stateCallback(const as_msgs::CarState::ConstPtr &data) {
   	geometry_msgs::Pose pose;
   	pose.position = data->odom.position;
   	tf::Quaternion qAux;
   	qAux.setRPY(0.0, 0.0, data->odom.heading);
   	tf::quaternionTFToMsg(qAux, pose.orientation);
   	tf::poseMsgToEigen(pose, this->localTf_);
   	this->localTf_ = this->localTf_.inverse();
   	this->localTfValid_ = true;
   	}

3. as_msgs::PathLimits is the output message from Urinay, it represents a midline (set of points) and the track limits (two sets of points for right and left limits).
   

   urinay/src/modules/WayComputer.cpp

   Lines 341 to 370 in 717f033

   	as_msgs::PathLimits WayComputer::getPathLimits() const {
   	as_msgs::PathLimits res;
   	res.stamp = ros::Time::now();
   	// res.replan indicates if the Way is different from last iteration's
   	res.replan = this->way_ != this->lastWay_;
   	// Fill path
   	std::vector<Point> path = this->wayToPublish_.getPath();
   	res.path.reserve(path.size());
   	for (const Point &p : path) {
   	res.path.push_back(p.gmPoint());
   	}
   	// Fill Tracklimits
   	Tracklimits tracklimits = this->wayToPublish_.getTracklimits();
   	res.tracklimits.stamp = res.stamp;
   	res.tracklimits.left.reserve(tracklimits.first.size());
   	for (const Node &n : tracklimits.first) {
   	res.tracklimits.left.push_back(n.cone());
   	}
   	for (const Node &n : tracklimits.second) {
   	res.tracklimits.right.push_back(n.cone());
   	}
   	// res.tracklimits.replan indicates if the n midpoints in front of the car
   	// have varied from last iteration
   	res.tracklimits.replan = this->way_.quinEhLobjetiuDeLaSevaDiresio(this->lastWay_);
   	return res;
   	}

4. There are additional information that Urinay provides/takes that is not essential like, e.g. cone detection confidence and two different path replan flags. These are just to meet some criteria.

[yeziheng123]

Thank you very much for your reply. I may still not fully understand this algorithm. Could you please tell me if it can be used for real-time path planning and track constraints (assuming real-time detection of cones has been achieved in an unknown environment)? Also, I would like to further study this algorithm. Have you published any papers related to this algorithm or could you provide more open source information about your university's autonomous vehicles?

[origovi]

Absolutely, it is real-time and it has been tested on-track and it really works. I do not think a paper is necessary.
Please for any further questions, contact me at origovi2000@gmail.com . This issue is closed.