/**
Get targetPoint service
@param &req navigation::getTargetPoint::Request
@param &res navigation::getTargetPoint::Response
@return success bool
*/
bool Pathplanner::getTargetPoint(navigation::getTargetPoint::Request &req,
navigation::getTargetPoint::Response &res)
{
// Set the loop rate to 1 hz
ros::Rate marekNapRate(10);
int currentInteration = 0;
int maxLoopCount = 10;
while(!this->poseUpdated && currentInteration <= maxLoopCount && ros::ok())
{
ROS_INFO("Pose hasn't been updated! Marek says take a nap!");
// Sleep
marekNapRate.sleep();
currentInteration++;
}
geometry_msgs::PointStamped currentTarget;
currentTarget = getTargetPoint();
// Set the response
res.targetPoint = currentTarget;
ROS_INFO("Current target: X: %f, Y: %f", res.targetPoint.point.x, res.targetPoint.point.y);
return true;
}
// Compute steer value.
float Driver::getSteer()
{
float targetAngle;
vec2f target = getTargetPoint();
targetAngle = atan2(target.y - car->_pos_Y, target.x - car->_pos_X);
targetAngle -= car->_yaw;
NORM_PI_PI(targetAngle);
return targetAngle / car->_steerLock;
}
