missing_tf.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 #include <tf/tf.h>
35 
37 
38 #include <math.h>
39 
41 
42 using namespace visualization_msgs;
43 
44 Marker makeBox( InteractiveMarker &msg )
45 {
46  Marker marker;
47 
48  marker.type = Marker::CUBE;
49  marker.scale.x = msg.scale * 0.45;
50  marker.scale.y = msg.scale * 0.45;
51  marker.scale.z = msg.scale * 0.45;
52  marker.color.r = 0.5;
53  marker.color.g = 0.5;
54  marker.color.b = 0.5;
55  marker.color.a = 1.0;
56 
57  return marker;
58 }
59 
60 InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
61 {
62  InteractiveMarkerControl control;
63  control.always_visible = true;
64  control.markers.push_back( makeBox(msg) );
65  msg.controls.push_back( control );
66 
67  return msg.controls.back();
68 }
69 
70 // %Tag(6DOF)%
71 void make6DofMarker( bool fixed )
72 {
73  InteractiveMarker int_marker;
74  int_marker.header.frame_id = "/base_link";
75  int_marker.header.stamp = ros::Time::now();
76  int_marker.scale = 1;
77 
78  int_marker.name = "simple_6dof";
79  int_marker.description = "Simple 6-DOF Control";
80 
81  // insert a box
82  makeBoxControl(int_marker);
83 
84  InteractiveMarkerControl control;
85 
86  if ( fixed )
87  {
88  int_marker.name += "_fixed";
89  int_marker.description += "\n(fixed orientation)";
90  control.orientation_mode = InteractiveMarkerControl::FIXED;
91  }
92 
93  control.orientation.w = 1;
94  control.orientation.x = 1;
95  control.orientation.y = 0;
96  control.orientation.z = 0;
97  control.name = "rotate_x";
98  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
99  int_marker.controls.push_back(control);
100  control.name = "move_x";
101  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
102  int_marker.controls.push_back(control);
103 
104  control.orientation.w = 1;
105  control.orientation.x = 0;
106  control.orientation.y = 1;
107  control.orientation.z = 0;
108  control.name = "rotate_z";
109  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
110  int_marker.controls.push_back(control);
111  control.name = "move_z";
112  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
113  int_marker.controls.push_back(control);
114 
115  control.orientation.w = 1;
116  control.orientation.x = 0;
117  control.orientation.y = 0;
118  control.orientation.z = 1;
119  control.name = "rotate_y";
120  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
121  int_marker.controls.push_back(control);
122  control.name = "move_y";
123  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
124  int_marker.controls.push_back(control);
125 
126  server->insert(int_marker);
127 }
128 
130 {
131  static tf::TransformBroadcaster br;
132  static bool sending = true;
133 
134  geometry_msgs::Pose pose;
135  pose.orientation.w = 1;
136 
137  ros::Time time = ros::Time::now();
138 
139  std_msgs::Header header;
140  header.frame_id = "/base_link";
141  header.stamp = time;
142 
143  int seconds = time.toSec();
144 
145  ROS_INFO( "%.3f", time.toSec() );
146 
147  if ( seconds % 4 < 2 )
148  {
149  if (!sending) ROS_INFO("on");
150  sending = true;
151  }
152  else
153  {
154  if (sending) ROS_INFO("off");
155  sending = false;
156  header.frame_id = "missing_frame";
157  }
158 
159  server->setPose("simple_6dof",pose,header);
160  server->applyChanges();
161 
162  tf::Transform t;
163  t.setOrigin(tf::Vector3(0.0, 0.0, 1.0));
164  t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
165  br.sendTransform(tf::StampedTransform(t, time, "base_link", "bursty_frame"));
166 }
167 
168 int main(int argc, char** argv)
169 {
170  ros::init(argc, argv, "bursty_tf");
171  ros::NodeHandle n;
172 
173  server.reset( new interactive_markers::InteractiveMarkerServer("bursty_tf","",false) );
174 
175  // create a timer to update the published transforms
176  ros::Timer frame_timer = n.createTimer(ros::Duration(0.5), frameCallback);
177 
178  make6DofMarker(false);
179  server->applyChanges();
180 
181  ros::spin();
182 }
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server
Definition: missing_tf.cpp:40
Marker makeBox(InteractiveMarker &msg)
Definition: missing_tf.cpp:44
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
void make6DofMarker(bool fixed)
Definition: missing_tf.cpp:71
InteractiveMarkerControl & makeBoxControl(InteractiveMarker &msg)
Definition: missing_tf.cpp:60
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
#define ROS_INFO(...)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void sendTransform(const StampedTransform &transform)
int main(int argc, char **argv)
Definition: missing_tf.cpp:168
static Time now()
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
void frameCallback(const ros::TimerEvent &)
Definition: missing_tf.cpp:129


interactive_markers
Author(s): David Gossow, William Woodall
autogenerated on Thu Oct 8 2020 04:02:35