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