19 #ifndef SLAM_TOOLBOX_VISUALIZATION_UTILS_H_
20 #define SLAM_TOOLBOX_VISUALIZATION_UTILS_H_
26 const std::string& frame,
27 const std::string& ns,
30 visualization_msgs::Marker marker;
32 marker.header.frame_id = frame;
35 marker.type = visualization_msgs::Marker::SPHERE;
36 marker.pose.position.z = 0.0;
37 marker.pose.orientation.w = 1.;
38 marker.scale.x = scale;
39 marker.scale.y = scale;
40 marker.scale.z = scale;
45 marker.action = visualization_msgs::Marker::ADD;
52 const std::string& frame,
53 const std::string& ns,
56 visualization_msgs::Marker marker;
58 marker.header.frame_id = frame;
61 marker.type = visualization_msgs::Marker::LINE_STRIP;
62 marker.pose.position.z = 0.0;
63 marker.pose.orientation.w = 1.;
64 marker.points.resize(2);
65 marker.scale.x = width;
72 marker.action = visualization_msgs::Marker::ADD;
79 visualization_msgs::Marker& marker,
83 visualization_msgs::InteractiveMarker int_marker;
84 int_marker.header.frame_id = marker.header.frame_id;
86 int_marker.name = std::to_string(marker.id);
87 int_marker.pose.orientation.w = 1.;
88 int_marker.pose.position.x = marker.pose.position.x;
89 int_marker.pose.position.y = marker.pose.position.y;
90 int_marker.scale = scale;
93 visualization_msgs::InteractiveMarkerControl control;
94 control.orientation_mode =
95 visualization_msgs::InteractiveMarkerControl::FIXED;
96 control.always_visible =
true;
97 control.orientation.w = 0;
98 control.orientation.x = 0.7071;
99 control.orientation.y = 0;
100 control.orientation.z = 0.7071;
101 control.interaction_mode =
102 visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
103 control.markers.push_back( marker );
104 int_marker.controls.push_back( control );
107 visualization_msgs::InteractiveMarkerControl control_rot;
108 control_rot.orientation_mode =
109 visualization_msgs::InteractiveMarkerControl::FIXED;
110 control_rot.always_visible =
true;
111 control_rot.orientation.w = 0;
112 control_rot.orientation.x = 0.7071;
113 control_rot.orientation.y = 0;
114 control_rot.orientation.z = 0.7071;
115 control_rot.interaction_mode =
116 visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
117 int_marker.controls.push_back( control_rot );
124 nav_msgs::OccupancyGrid& map)
132 if(map.info.width != (
unsigned int) width ||
133 map.info.height != (
unsigned int) height ||
134 map.info.origin.position.x != offset.
GetX() ||
135 map.info.origin.position.y != offset.
GetY())
137 map.info.origin.position.x = offset.
GetX();
138 map.info.origin.position.y = offset.
GetY();
139 map.info.width = width;
140 map.info.height = height;
141 map.data.resize(map.info.width * map.info.height);
152 map.data[
MAP_IDX(map.info.width, x, y)] = -1;
155 map.data[
MAP_IDX(map.info.width, x, y)] = 100;
158 map.data[
MAP_IDX(map.info.width, x, y)] = 0;
161 ROS_WARN(
"Encountered unknown cell value at %d, %d", x, y);
171 #endif //SLAM_TOOLBOX_VISUALIZATION_UTILS_H_