本文整理汇总了C++中WayPoints::getWaypointOrientation方法的典型用法代码示例。如果您正苦于以下问题：C++ WayPoints::getWaypointOrientation方法的具体用法？C++ WayPoints::getWaypointOrientation怎么用？C++ WayPoints::getWaypointOrientation使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类WayPoints的用法示例。
在下文中一共展示了WayPoints::getWaypointOrientation方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: DisplayDetectionRange
static void DisplayDetectionRange(const int &crosswalk_id, const int &num, const EControl &kind)
{
// set up for marker array
visualization_msgs::MarkerArray marker_array;
visualization_msgs::Marker crosswalk_marker;
visualization_msgs::Marker waypoint_marker_stop;
visualization_msgs::Marker waypoint_marker_decelerate;
visualization_msgs::Marker stop_line;
crosswalk_marker.header.frame_id = "/map";
crosswalk_marker.header.stamp = ros::Time();
crosswalk_marker.id = 0;
crosswalk_marker.type = visualization_msgs::Marker::SPHERE_LIST;
crosswalk_marker.action = visualization_msgs::Marker::ADD;
waypoint_marker_stop = crosswalk_marker;
waypoint_marker_decelerate = crosswalk_marker;
stop_line = crosswalk_marker;
stop_line.type = visualization_msgs::Marker::CUBE;
// set each namespace
crosswalk_marker.ns = "Crosswalk Detection";
waypoint_marker_stop.ns = "Stop Detection";
waypoint_marker_decelerate.ns = "Decelerate Detection";
stop_line.ns = "Stop Line";
// set scale and color
double scale = 2*_detection_range;
waypoint_marker_stop.scale.x = scale;
waypoint_marker_stop.scale.y = scale;
waypoint_marker_stop.scale.z = scale;
waypoint_marker_stop.color.a = 0.2;
waypoint_marker_stop.color.r = 0.0;
waypoint_marker_stop.color.g = 1.0;
waypoint_marker_stop.color.b = 0.0;
waypoint_marker_stop.frame_locked = true;
scale = 2*(_detection_range + _deceleration_range);
waypoint_marker_decelerate.scale.x = scale;
waypoint_marker_decelerate.scale.y = scale;
waypoint_marker_decelerate.scale.z = scale;
waypoint_marker_decelerate.color.a = 0.15;
waypoint_marker_decelerate.color.r = 1.0;
waypoint_marker_decelerate.color.g = 1.0;
waypoint_marker_decelerate.color.b = 0.0;
waypoint_marker_decelerate.frame_locked = true;
if (_obstacle_waypoint > -1) {
stop_line.pose.position = _path_dk.getWaypointPosition(_obstacle_waypoint);
stop_line.pose.orientation = _path_dk.getWaypointOrientation(_obstacle_waypoint);
}
stop_line.pose.position.z += 1.0;
stop_line.scale.x = 0.1;
stop_line.scale.y = 15.0;
stop_line.scale.z = 2.0;
stop_line.color.a = 0.3;
stop_line.color.r = 1.0;
stop_line.color.g = 0.0;
stop_line.color.b = 0.0;
stop_line.lifetime = ros::Duration(0.1);
stop_line.frame_locked = true;
if (crosswalk_id > 0)
scale = vmap.getDetectionPoints(crosswalk_id).width;
crosswalk_marker.scale.x = scale;
crosswalk_marker.scale.y = scale;
crosswalk_marker.scale.z = scale;
crosswalk_marker.color.a = 0.5;
crosswalk_marker.color.r = 0.0;
crosswalk_marker.color.g = 1.0;
crosswalk_marker.color.b = 0.0;
crosswalk_marker.frame_locked = true;
// set marker points coordinate
for (int i = 0; i < _search_distance; i++) {
if (num < 0 || i+num > _path_dk.getSize() - 1)
break;
geometry_msgs::Point point;
point = _path_dk.getWaypointPosition(num+i);
waypoint_marker_stop.points.push_back(point);
if (i > _deceleration_search_distance)
continue;
waypoint_marker_decelerate.points.push_back(point);
}
if (crosswalk_id > 0) {
for (const auto &p : vmap.getDetectionPoints(crosswalk_id).points)
crosswalk_marker.points.push_back(p);
}
// publish marker
marker_array.markers.push_back(crosswalk_marker);
marker_array.markers.push_back(waypoint_marker_stop);
marker_array.markers.push_back(waypoint_marker_decelerate);
//.........这里部分代码省略.........