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


interactive_markers
Author(s): David Gossow, William Woodall
autogenerated on Mon Feb 28 2022 22:33:22