/*
* Callback function of destination subscriber.
* Sets dest_ to received pose.
* @param[in] dest Pointer to received pose.
* */
void destCallback(const geometry_msgs::Pose::ConstPtr& dest)
{
double destX = dest->position.x;
double destY = dest->position.y;
double currX = odometer_.getOrigin().getX();
double currY = odometer_.getOrigin().getY();
// Look towards destination
lookTowards(destX, destY);
// Search for area with most features between +-30 degrees
double bestYaw = searchMaxFeatures(0.5, 0.8);
double distance = getDistance(destX, destY);
// If distance is further than 1m, go halfway first
if (distance > 1.0)
{
// Get distance to destination and angle to the area with most features to plan path
double yaw, pitch, roll, alpha;
odometer_.getBasis().getEulerYPR(yaw, pitch, roll);
alpha = bestYaw - yaw;
// Get stopover point
tf::Vector3 vec(destX - currX, destY - currY, 0.0);
vec = vec.rotate(tf::Vector3(0, 0, 1), alpha);
vec *= 0.5/cos(alpha);
double midX = currX + vec.getX();
double midY = currY + vec.getY();
// Turn to area with most features and move to the point in mid distance
setYaw(bestYaw);
goTo(midX, midY);
// Run sBA to optimize Pose
odometer_.runSBA(50, false);
// Recursively repeat until distance < 1m
destCallback(dest);
}
else
{
setYaw(bestYaw);
// Move to destination
goTo(destX, destY);
// Try to optimize pose again and correct position
odometer_.runSBA(50, false);
goTo(destX, destY);
}
}
void Transformable::lookTowards(
vec3f front_vec
)
{
lookTowards(front_vec, vec3f(0.0f, 1.0f, 0.0f));
}
