/** 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; }