22 #include "visualization_thread.h" 47 rosnode->advertise<visualization_msgs::MarkerArray>
48 (
"visualization_marker_array", 100,
true);
56 visualization_msgs::MarkerArray m;
58 for (
size_t i = 0; i < last_id_num_; ++i) {
59 visualization_msgs::Marker delop;
60 delop.header.frame_id =
"/map";
61 delop.header.stamp = ros::Time::now();
62 delop.ns =
"navgraph_generator";
64 delop.action = visualization_msgs::Marker::DELETE;
65 m.markers.push_back(delop);
76 visualization_msgs::MarkerArray m;
77 unsigned int idnum = 0;
79 for (
auto &o : obstacles_) {
80 visualization_msgs::Marker text;
81 text.header.frame_id = cfg_global_frame_;
82 text.header.stamp = ros::Time::now();
83 text.ns =
"navgraph_generator";
85 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
86 text.action = visualization_msgs::Marker::ADD;
87 text.pose.position.x = o.second.x;
88 text.pose.position.y = o.second.y;
89 text.pose.position.z = .15;
90 text.pose.orientation.w = 1.;
92 text.color.r = text.color.g = text.color.b = 1.0f;
94 text.lifetime = ros::Duration(0, 0);
96 m.markers.push_back(text);
98 visualization_msgs::Marker sphere;
99 sphere.header.frame_id = cfg_global_frame_;
100 sphere.header.stamp = ros::Time::now();
101 sphere.ns =
"navgraph_generator";
103 sphere.type = visualization_msgs::Marker::SPHERE;
104 sphere.action = visualization_msgs::Marker::ADD;
105 sphere.pose.position.x = o.second.x;
106 sphere.pose.position.y = o.second.y;
107 sphere.pose.position.z = 0.05;
108 sphere.pose.orientation.w = 1.;
109 sphere.scale.x = 0.05;
110 sphere.scale.y = 0.05;
111 sphere.scale.z = 0.05;
112 sphere.color.r = 1.0;
113 sphere.color.g = sphere.color.b = 0.;
114 sphere.color.a = 1.0;
115 sphere.lifetime = ros::Duration(0, 0);
116 m.markers.push_back(sphere);
119 for (
auto &o : map_obstacles_) {
120 visualization_msgs::Marker text;
121 text.header.frame_id = cfg_global_frame_;
122 text.header.stamp = ros::Time::now();
123 text.ns =
"navgraph_generator";
125 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
126 text.action = visualization_msgs::Marker::ADD;
127 text.pose.position.x = o.second.x;
128 text.pose.position.y = o.second.y;
129 text.pose.position.z = .15;
130 text.pose.orientation.w = 1.;
132 text.color.r = text.color.g = text.color.b = 1.0f;
134 text.lifetime = ros::Duration(0, 0);
136 m.markers.push_back(text);
138 visualization_msgs::Marker sphere;
139 sphere.header.frame_id = cfg_global_frame_;
140 sphere.header.stamp = ros::Time::now();
141 sphere.ns =
"navgraph_generator";
143 sphere.type = visualization_msgs::Marker::SPHERE;
144 sphere.action = visualization_msgs::Marker::ADD;
145 sphere.pose.position.x = o.second.x;
146 sphere.pose.position.y = o.second.y;
147 sphere.pose.position.z = 0.05;
148 sphere.pose.orientation.w = 1.;
149 sphere.scale.x = 0.05;
150 sphere.scale.y = 0.05;
151 sphere.scale.z = 0.05;
152 sphere.color.r = sphere.color.g = 1.0;
154 sphere.color.a = 1.0;
155 sphere.lifetime = ros::Duration(0, 0);
156 m.markers.push_back(sphere);
159 for (
size_t i = idnum; i < last_id_num_; ++i) {
160 visualization_msgs::Marker delop;
161 delop.header.frame_id = cfg_global_frame_;
162 delop.header.stamp = ros::Time::now();
163 delop.ns =
"navgraph_generator";
165 delop.action = visualization_msgs::Marker::DELETE;
166 m.markers.push_back(delop);
168 last_id_num_ = idnum;
181 const NavGraphGeneratorThread::ObstacleMap &map_obstacles,
182 const NavGraphGeneratorThread::PoiMap &pois)
184 obstacles_ = obstacles;
185 map_obstacles_ = map_obstacles;
Fawkes library namespace.
void publish(const NavGraphGeneratorThread::ObstacleMap &obstacles, const NavGraphGeneratorThread::ObstacleMap &map_obstacles, const NavGraphGeneratorThread::PoiMap &pois)
Trigger publishing of visualization.
Thread class encapsulation of pthreads.
NavGraphGeneratorVisualizationThread()
Constructor.
virtual void loop()
Code to execute in the thread.
void wakeup()
Wake up thread.
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
virtual void init()
Initialize the thread.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Configuration * config
This is the Configuration member used to access the configuration.
virtual void finalize()
Finalize the thread.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.