本文整理汇总了C++中WayPoints::getWaypointPose方法的典型用法代码示例。如果您正苦于以下问题:C++ WayPoints::getWaypointPose方法的具体用法?C++ WayPoints::getWaypointPose怎么用?C++ WayPoints::getWaypointPose使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类WayPoints
的用法示例。
在下文中一共展示了WayPoints::getWaypointPose方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: verifyFollowing
static bool verifyFollowing()
{
double slope = 0;
double intercept = 0;
getLinearEquation(_current_waypoints.getWaypointPosition(1),_current_waypoints.getWaypointPosition(2),&slope,&intercept);
double displacement = getDistanceBetweenLineAndPoint(_current_pose.pose.position,slope,intercept);
double relative_angle = getRelativeAngle(_current_waypoints.getWaypointPose(1),_current_pose.pose);
//ROS_INFO("side diff : %lf , angle diff : %lf",displacement,relative_angle);
if(displacement < g_displacement_threshold || relative_angle < g_relative_angle_threshold){
//ROS_INFO("Following : True");
return true;
}
else{
//ROS_INFO("Following : False");
return false;
}
}