crossing_visualization.cpp
Go to the documentation of this file.
00001 #include <lama_common/crossing_visualization.h>
00002 
00003 namespace lama_common
00004 {
00005 
00008 visualization_msgs::Marker getCrossingCenterMarker(const std::string& frame_id, const lama_msgs::Crossing& crossing)
00009 {
00010         visualization_msgs::Marker m;
00011         m.header.frame_id = frame_id;
00012         m.ns = "crossing_center";
00013         m.type = visualization_msgs::Marker::SPHERE;
00014         m.pose.position.x = crossing.center.x;
00015         m.pose.position.y = crossing.center.y;
00016         m.pose.position.z = 0;
00017         m.pose.orientation.w = 1.0;
00018         m.scale.x = 2 * crossing.radius;
00019         m.scale.y = 2 * crossing.radius;
00020         m.scale.z = 1;
00021         m.color.r = 1.0;
00022         m.color.g = 1.0;
00023         m.color.a = 0.5;
00024         return m;
00025 }
00026 
00029 visualization_msgs::Marker getFrontiersMarker(const std::string& frame_id, const lama_msgs::Crossing& crossing)
00030 {
00031         visualization_msgs::Marker m;
00032         m.header.frame_id = frame_id;
00033         m.ns = "frontiers";
00034         m.type = visualization_msgs::Marker::LINE_LIST;
00035         m.pose.orientation.w = 1.0;
00036         m.scale.x = 0.1;
00037         m.color.r = 0.0;
00038         m.color.g = 0.0;
00039         m.color.b = 1.0;
00040         m.color.a = 0.5;
00041 
00042   geometry_msgs::Point p;
00043   for(size_t i = 0; i < crossing.frontiers.size(); ++i)
00044         {
00045     // Frontier itself.
00046     p.x = crossing.frontiers[i].p1.x;
00047     p.y = crossing.frontiers[i].p1.y;
00048                 m.points.push_back(p);
00049     p.x = crossing.frontiers[i].p2.x;
00050     p.y = crossing.frontiers[i].p2.y;
00051                 m.points.push_back(p);
00052     // Line from robot with angle frontier.angle.
00053     p.x = 0;
00054     p.y = 0;
00055     m.points.push_back(p);
00056     p.x = 4 * crossing.radius * std::cos(crossing.frontiers[i].angle);
00057     p.y = 4 * crossing.radius * std::sin(crossing.frontiers[i].angle);
00058     m.points.push_back(p);
00059         }
00060         return m;
00061 }
00062 
00063 } // namespace lama_common


lama_common
Author(s): Gaël Ecorchard , Karel Košnar , Vojtěch Vonásek
autogenerated on Thu Jun 6 2019 22:02:03