3 #include "visualization_msgs/Marker.h" 4 #include "visualization_msgs/MarkerArray.h" 12 publishText(
int id,
float x,
float y,
const std::string & text)
14 visualization_msgs::Marker marker;
15 marker.header.frame_id =
"/base_link";
18 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
19 marker.action = visualization_msgs::Marker::ADD;
20 marker.pose.orientation.x = 0.0;
21 marker.pose.orientation.y = 0.0;
22 marker.pose.orientation.z = 0.0;
23 marker.pose.orientation.w = 1.0;
33 marker.pose.position.x = x;
34 marker.pose.position.y = y;
41 int id,
float x,
float y,
42 float r,
float g,
float b,
float a,
43 bool use_embedded_materials,
bool mesh)
45 using visualization_msgs::Marker;
48 marker.header.frame_id =
"/base_link";
52 marker.type = Marker::MESH_RESOURCE;
54 marker.type = Marker::SPHERE;
56 marker.action = Marker::ADD;
57 marker.pose.orientation.x = 0.0;
58 marker.pose.orientation.y = 0.0;
59 marker.pose.orientation.z = 0.1;
60 marker.pose.orientation.w = 1.0;
68 marker.frame_locked =
true;
69 marker.mesh_resource =
"package://rviz/src/test/meshes/pr2-base.dae";
70 marker.mesh_use_embedded_materials = use_embedded_materials;
72 marker.pose.position.x = x;
73 marker.pose.position.y = y;
87 publishText(
id, x - 1, y,
"mesh, use_embedded_materials = true");
89 publishText(
id, x, y - 1,
"rbg: 0.0 0.0 0.0, a: 0");
93 publishText(
id, x, y - 1,
"rbg: 1.0 1.0 1.0, a: 1");
97 publishText(
id, x, y - 1,
"rbg: 1.0 1.0 1.0, a: 0.5");
101 publishText(
id, x, y - 1,
"rbg: 1.0 1.0 1.0, a: 0");
105 publishText(
id, x, y - 1,
"rbg: 1.0 0.0 0.0, a: 1");
109 publishText(
id, x, y - 1,
"rbg: 1.0 0.0 0.0, a: 0.5");
113 publishText(
id, x, y - 1,
"rbg: 1.0 0.0 0.0, a: 0");
117 publishText(
id, x, y - 1,
"rbg: 1.0 0.5 0.5, a: 1");
121 publishText(
id, x, y - 1,
"rbg: 1.0 0.5 0.5, a: 0.5");
125 publishText(
id, x, y - 1,
"rbg: 1.0 0.5 0.5, a: 0");
132 publishText(
id, x - 1, y,
"mesh, use_embedded_materials = false");
157 publishText(
id, x - 1, y,
"sphere, use_embedded_materials = true");
182 publishText(
id, x - 1, y,
"sphere, use_embedded_materials = false");
200 publishMesh(
id, x, y, 1, .5, .5, .5,
false,
false);
211 int main(
int argc,
char** argv)
213 ros::init(argc, argv,
"mesh_marker_test");
216 bool latch_only =
false;
218 if (std::string(argv[1]) ==
"--latch-only") {
223 g_marker_pub = n.
advertise<visualization_msgs::Marker>(
"mesh_markers", 0, latch_only);
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void publishMesh(int id, float x, float y, float r, float g, float b, float a, bool use_embedded_materials, bool mesh)
TFSIMD_FORCE_INLINE const tfScalar & y() const
ROSCPP_DECL void spin(Spinner &spinner)
void publishCallback(const ros::TimerEvent &)
ros::Publisher g_marker_pub
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void publishText(int id, float x, float y, const std::string &text)
int main(int argc, char **argv)