interactive_marker_test.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 
31 // %Tag(fullSource)%
32 #include <ros/ros.h>
33 
35 
36 // create an interactive marker server on the topic namespace simple_marker
38 
39 visualization_msgs::InteractiveMarker makeMarker(float r, float g, float b)
40 {
41  // create an interactive marker for our server
42  visualization_msgs::InteractiveMarker int_marker;
43  int_marker.header.frame_id = "/base_link";
44  int_marker.name = "my_marker";
45  int_marker.description = "Simple 1-DOF Control";
46 
47  // create a box marker
48  visualization_msgs::Marker box_marker;
49  box_marker.type = visualization_msgs::Marker::CUBE;
50  box_marker.scale.x = 0.45;
51  box_marker.scale.y = 0.45;
52  box_marker.scale.z = 0.45;
53  box_marker.color.r = r;
54  box_marker.color.g = g;
55  box_marker.color.b = b;
56  box_marker.color.a = 1.0;
57 
58  // create a non-interactive control which contains the box
59  visualization_msgs::InteractiveMarkerControl box_control;
60  box_control.always_visible = true;
61  box_control.markers.push_back(box_marker);
62 
63  // add the control to the interactive marker
64  int_marker.controls.push_back(box_control);
65 
66  // create a control which will move the box
67  // this control does not contain any markers,
68  // which will cause RViz to insert two arrows
69  visualization_msgs::InteractiveMarkerControl linear_control;
70  linear_control.name = "move_x";
71  linear_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
72 
73  // add the control to the interactive marker
74  int_marker.controls.push_back(linear_control);
75 
76  return int_marker;
77 }
78 
79 bool is_red = false;
80 
81 void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
82 {
83  ROS_INFO_STREAM(feedback->marker_name << " is now at " << feedback->pose.position.x << ", "
84  << feedback->pose.position.y << ", "
85  << feedback->pose.position.z);
86 
87  bool changed = false;
88  visualization_msgs::InteractiveMarker int_marker;
89 
90  // red when x < 0, green otherwise. Update marker color when x
91  // crosses boundary.
92  if (feedback->pose.position.x < 0 && !is_red)
93  {
94  printf("turning red.\n");
95  is_red = true;
96  int_marker = makeMarker(1, 0, 0);
97  changed = true;
98  }
99  if (feedback->pose.position.x >= 0 && is_red)
100  {
101  printf("turning green.\n");
102  is_red = false;
103  int_marker = makeMarker(0, 1, 0);
104  changed = true;
105  }
106 
107  if (changed)
108  {
109  printf("changed.\n");
110  int_marker.pose = feedback->pose;
111  server->insert(int_marker);
112  server->applyChanges();
113  }
114 }
115 
116 visualization_msgs::InteractiveMarker makeCrazyMarker(bool linear)
117 {
118  // create an interactive marker for our server
119  visualization_msgs::InteractiveMarker int_marker;
120  int_marker.header.frame_id = "/base_link";
121  int_marker.name = "crazy_marker";
122  int_marker.description = "Unusual 1-DOF Control";
123 
124  // create a box marker
125  visualization_msgs::Marker box_marker;
126  box_marker.type = visualization_msgs::Marker::CUBE;
127  box_marker.scale.x = 1;
128  box_marker.scale.y = .3;
129  box_marker.scale.z = .3;
130  box_marker.color.r = .3;
131  box_marker.color.g = .1;
132  box_marker.color.b = 1;
133  box_marker.color.a = 1.0;
134  box_marker.pose.position.y = -1.0;
135 
136  // create a non-interactive control which contains the box
137  visualization_msgs::InteractiveMarkerControl box_control;
138  box_control.always_visible = true;
139  box_control.markers.push_back(box_marker);
140  box_control.name = "crazy";
141  if (linear)
142  {
143  box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
144  }
145  else
146  {
147  box_control.orientation.w = 1;
148  box_control.orientation.x = 0;
149  box_control.orientation.y = 1;
150  box_control.orientation.z = 0;
151  box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
152  }
153 
154  // add the control to the interactive marker
155  int_marker.controls.push_back(box_control);
156  int_marker.pose.position.y = 3;
157 
158  return int_marker;
159 }
160 
161 bool is_linear = true;
162 
163 void processCrazyFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
164 {
165  ROS_INFO_STREAM(feedback->marker_name
166  << " is now at pos " << feedback->pose.position.x << ", " << feedback->pose.position.y
167  << ", " << feedback->pose.position.z << "; quat " << feedback->pose.orientation.x
168  << ", " << feedback->pose.orientation.y << ", " << feedback->pose.orientation.z << ", "
169  << feedback->pose.orientation.w);
170 
171  bool changed = false;
172  visualization_msgs::InteractiveMarker int_marker;
173 
174  // linear when x < 0, rotary otherwise. Update when x
175  // crosses boundary.
176  if (feedback->pose.orientation.z < 0 && !is_linear)
177  {
178  printf("turning linear.\n");
179  is_linear = true;
180  int_marker = makeCrazyMarker(true);
181  changed = true;
182  }
183  if (feedback->pose.position.x > 0 && is_linear)
184  {
185  printf("turning rotary.\n");
186  is_linear = false;
187  int_marker = makeCrazyMarker(false);
188  changed = true;
189  }
190 
191  if (changed)
192  {
193  printf("changed.\n");
194  int_marker.pose.position.x = 0;
195  int_marker.pose.orientation.x = 0;
196  int_marker.pose.orientation.y = 0;
197  int_marker.pose.orientation.z = 0;
198  int_marker.pose.orientation.w = 1;
199  server->insert(int_marker);
200  server->applyChanges();
201  }
202 }
203 
204 int main(int argc, char** argv)
205 {
206  ros::init(argc, argv, "interactive_marker_test");
207 
208  // create an interactive marker server on the topic namespace simple_marker
209  server = new interactive_markers::InteractiveMarkerServer("simple_marker");
210 
211  // create an interactive marker for our server
212  visualization_msgs::InteractiveMarker int_marker = makeMarker(0, 1, 0);
213 
214  // add the interactive marker to our collection &
215  // tell the server to call processFeedback() when feedback arrives for it
216  server->insert(int_marker, &processFeedback);
217 
218  // create an interactive marker for our server
219  visualization_msgs::InteractiveMarker crazy_marker = makeCrazyMarker(true);
220 
221  // add the interactive marker to our collection &
222  // tell the server to call processCrazyFeedback() when feedback arrives for it
223  server->insert(crazy_marker, &processCrazyFeedback);
224 
225  // 'commit' changes and send to all clients
226  server->applyChanges();
227 
228  // start the ROS main loop
229  ros::spin();
230 }
231 // %Tag(fullSource)%
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
visualization_msgs::InteractiveMarker makeCrazyMarker(bool linear)
visualization_msgs::InteractiveMarker makeMarker(float r, float g, float b)
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void processCrazyFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
ROSCPP_DECL void spin()
interactive_markers::InteractiveMarkerServer * server
INTERACTIVE_MARKERS_PUBLIC void insert(const visualization_msgs::InteractiveMarker &int_marker)
#define ROS_INFO_STREAM(args)
INTERACTIVE_MARKERS_PUBLIC void applyChanges()
int main(int argc, char **argv)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Fri Aug 12 2022 02:06:08