visualization_utils.hpp
Go to the documentation of this file.
1 /*
2  * visualization_utils
3  * Copyright (c) 2019, Samsung Research America
4  *
5  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
6  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
7  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
8  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
9  *
10  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
11  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
12  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
13  * CONDITIONS.
14  *
15  */
16 
17 /* Author: Steven Macenski */
18 
19 #ifndef SLAM_TOOLBOX_VISUALIZATION_UTILS_H_
20 #define SLAM_TOOLBOX_VISUALIZATION_UTILS_H_
21 
22 namespace vis_utils
23 {
24 
25 inline visualization_msgs::Marker toVertexMarker(
26  const std::string& frame,
27  const std::string& ns,
28  const double& scale)
29 {
30  visualization_msgs::Marker marker;
31 
32  marker.header.frame_id = frame;
33  marker.header.stamp = ros::Time::now();
34  marker.ns = ns;
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;
41  marker.color.r = 1.0;
42  marker.color.g = 0;
43  marker.color.b = 0.0;
44  marker.color.a = 1.;
45  marker.action = visualization_msgs::Marker::ADD;
46  marker.lifetime = ros::Duration(0.);
47 
48  return marker;
49 }
50 
51 inline visualization_msgs::Marker toEdgeMarker(
52  const std::string& frame,
53  const std::string& ns,
54  const double& width)
55 {
56  visualization_msgs::Marker marker;
57 
58  marker.header.frame_id = frame;
59  marker.header.stamp = ros::Time::now();
60  marker.ns = ns;
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;
66  marker.scale.y = 0;
67  marker.scale.z = 0;
68  marker.color.r = 0.0;
69  marker.color.g = 0;
70  marker.color.b = 1.0;
71  marker.color.a = 1.;
72  marker.action = visualization_msgs::Marker::ADD;
73  marker.lifetime = ros::Duration(0.);
74 
75  return marker;
76 }
77 
78 inline visualization_msgs::InteractiveMarker toInteractiveMarker(
79  visualization_msgs::Marker& marker,
80  const double& scale)
81 {
82  // marker basics
83  visualization_msgs::InteractiveMarker int_marker;
84  int_marker.header.frame_id = marker.header.frame_id;
85  int_marker.header.stamp = ros::Time::now();
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;
91 
92  // translate control
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 );
105 
106  // rotate 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 );
118 
119  return int_marker;
120 }
121 
122 inline void toNavMap(
123  const karto::OccupancyGrid* occ_grid,
124  nav_msgs::OccupancyGrid& map)
125 {
126  // Translate to ROS format
127  kt_int32s width = occ_grid->GetWidth();
128  kt_int32s height = occ_grid->GetHeight();
130  occ_grid->GetCoordinateConverter()->GetOffset();
131 
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())
136  {
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);
142  }
143 
144  for (kt_int32s y = 0; y < height; y++)
145  {
146  for (kt_int32s x = 0; x < width; x++)
147  {
148  kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));
149  switch (value)
150  {
152  map.data[MAP_IDX(map.info.width, x, y)] = -1;
153  break;
155  map.data[MAP_IDX(map.info.width, x, y)] = 100;
156  break;
158  map.data[MAP_IDX(map.info.width, x, y)] = 0;
159  break;
160  default:
161  ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
162  break;
163  }
164  }
165  }
166  return;
167 }
168 
169 } // end namespace
170 
171 #endif //SLAM_TOOLBOX_VISUALIZATION_UTILS_H_
vis_utils
Definition: visualization_utils.hpp:22
karto::OccupancyGrid
Definition: Karto.h:6008
karto::Vector2::GetX
const T & GetX() const
Definition: Karto.h:1028
vis_utils::toNavMap
void toNavMap(const karto::OccupancyGrid *occ_grid, nav_msgs::OccupancyGrid &map)
Definition: visualization_utils.hpp:122
kt_int32s
int32_t kt_int32s
Definition: Types.h:51
MAP_IDX
#define MAP_IDX(sx, i, j)
Definition: toolbox_types.hpp:34
karto::Grid::GetCoordinateConverter
CoordinateConverter * GetCoordinateConverter() const
Definition: Karto.h:4917
karto::Vector2::GetY
const T & GetY() const
Definition: Karto.h:1046
ROS_WARN
#define ROS_WARN(...)
karto::Grid::GetWidth
kt_int32s GetWidth() const
Definition: Karto.h:4843
vis_utils::toVertexMarker
visualization_msgs::Marker toVertexMarker(const std::string &frame, const std::string &ns, const double &scale)
Definition: visualization_utils.hpp:25
karto::GridStates_Free
@ GridStates_Free
Definition: Karto.h:4448
karto::Vector2< kt_double >
vis_utils::toInteractiveMarker
visualization_msgs::InteractiveMarker toInteractiveMarker(visualization_msgs::Marker &marker, const double &scale)
Definition: visualization_utils.hpp:78
karto::Grid::GetValue
T GetValue(const Vector2< kt_int32s > &rGrid) const
Definition: Karto.h:4907
karto::CoordinateConverter::GetOffset
const Vector2< kt_double > & GetOffset() const
Definition: Karto.h:4550
karto::GridStates_Occupied
@ GridStates_Occupied
Definition: Karto.h:4447
kt_int8u
uint8_t kt_int8u
Definition: Types.h:46
karto::GridStates_Unknown
@ GridStates_Unknown
Definition: Karto.h:4446
ros::Duration
vis_utils::toEdgeMarker
visualization_msgs::Marker toEdgeMarker(const std::string &frame, const std::string &ns, const double &width)
Definition: visualization_utils.hpp:51
ros::Time::now
static Time now()
karto::Grid::GetHeight
kt_int32s GetHeight() const
Definition: Karto.h:4852


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:56