cube.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, 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 #include <ros/ros.h>
32 
34 
35 #include <math.h>
36 
37 #include <tf/LinearMath/Vector3.h>
38 
39 
40 using namespace visualization_msgs;
41 
43 
44 std::vector< tf::Vector3 > positions;
45 
46 void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
47 {
48  switch ( feedback->event_type )
49  {
50  case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
51  {
52  //compute difference vector for this cube
53 
54  tf::Vector3 fb_pos(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z);
55  unsigned index = atoi( feedback->marker_name.c_str() );
56 
57  if ( index > positions.size() )
58  {
59  return;
60  }
61  tf::Vector3 fb_delta = fb_pos - positions[index];
62 
63  // move all markers in that direction
64  for ( unsigned i=0; i<positions.size(); i++ )
65  {
66  float d = fb_pos.distance( positions[i] );
67  float t = 1 / (d*5.0+1.0) - 0.2;
68  if ( t < 0.0 ) t=0.0;
69 
70  positions[i] += t * fb_delta;
71 
72  if ( i == index ) {
73  ROS_INFO_STREAM( d );
74  positions[i] = fb_pos;
75  }
76 
77  geometry_msgs::Pose pose;
78  pose.position.x = positions[i].x();
79  pose.position.y = positions[i].y();
80  pose.position.z = positions[i].z();
81 
82  std::stringstream s;
83  s << i;
84  server->setPose( s.str(), pose );
85  }
86 
87 
88  break;
89  }
90  }
91  server->applyChanges();
92 }
93 
94 InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
95 {
96  InteractiveMarkerControl control;
97  control.always_visible = true;
98  control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
99  control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
100  control.independent_marker_orientation = true;
101 
102  Marker marker;
103 
104  marker.type = Marker::CUBE;
105  marker.scale.x = msg.scale;
106  marker.scale.y = msg.scale;
107  marker.scale.z = msg.scale;
108  marker.color.r = 0.65+0.7*msg.pose.position.x;
109  marker.color.g = 0.65+0.7*msg.pose.position.y;
110  marker.color.b = 0.65+0.7*msg.pose.position.z;
111  marker.color.a = 1.0;
112 
113  control.markers.push_back( marker );
114  msg.controls.push_back( control );
115 
116  return msg.controls.back();
117 }
118 
119 
120 void makeCube( )
121 {
122  int side_length = 10;
123  float step = 1.0/ (float)side_length;
124  int count = 0;
125 
126  positions.reserve( side_length*side_length*side_length );
127 
128  for ( double x=-0.5; x<0.5; x+=step )
129  {
130  for ( double y=-0.5; y<0.5; y+=step )
131  {
132  for ( double z=0.0; z<1.0; z+=step )
133  {
134  InteractiveMarker int_marker;
135  int_marker.header.frame_id = "base_link";
136  int_marker.scale = step;
137 
138  int_marker.pose.position.x = x;
139  int_marker.pose.position.y = y;
140  int_marker.pose.position.z = z;
141 
142  positions.push_back( tf::Vector3(x,y,z) );
143 
144  std::stringstream s;
145  s << count;
146  int_marker.name = s.str();
147 
148  makeBoxControl(int_marker);
149 
150  server->insert( int_marker );
151  server->setCallback( int_marker.name, &processFeedback );
152 
153  count++;
154  }
155  }
156  }
157 }
158 
159 int main(int argc, char** argv)
160 {
161  ros::init(argc, argv, "cube");
162 
163  server.reset( new interactive_markers::InteractiveMarkerServer("cube") );
164 
165  ros::Duration(0.1).sleep();
166 
167  ROS_INFO("initializing..");
168  makeCube();
169  server->applyChanges();
170  ROS_INFO("ready.");
171 
172  ros::spin();
173 
174  server.reset();
175 }
d
std::vector< tf::Vector3 > positions
Definition: cube.cpp:44
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: cube.cpp:46
XmlRpcServer s
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
ROSCPP_DECL void spin(Spinner &spinner)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void makeCube()
Definition: cube.cpp:120
#define ROS_INFO(...)
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server
Definition: cube.cpp:42
unsigned int step
#define ROS_INFO_STREAM(args)
int main(int argc, char **argv)
Definition: cube.cpp:159
InteractiveMarkerControl & makeBoxControl(InteractiveMarker &msg)
Definition: cube.cpp:94


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Fri Feb 1 2019 03:33:34