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 
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 {
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  geometry_msgs::TransformStamped stf;
163  stf.header.frame_id = "base_link";
164  stf.header.stamp = time;
165  stf.child_frame_id = "bursty_frame";
166  stf.transform.translation = toMsg(tf2::Vector3(0.0, 0.0, 1.0));
167  stf.transform.rotation = toMsg(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
168 
169  br.sendTransform(stf);
170 }
171 
172 int main(int argc, char** argv)
173 {
174  ros::init(argc, argv, "bursty_tf");
175  ros::NodeHandle n;
176 
177  server.reset( new interactive_markers::InteractiveMarkerServer("bursty_tf","",false) );
178 
179  // create a timer to update the published transforms
180  ros::Timer frame_timer = n.createTimer(ros::Duration(0.5), frameCallback);
181 
182  make6DofMarker(false);
183  server->applyChanges();
184 
185  ros::spin();
186 }
main
int main(int argc, char **argv)
Definition: missing_tf.cpp:172
interactive_marker_server.h
boost::shared_ptr
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros.h
TimeBase< Time, Duration >::toSec
double toSec() const
frameCallback
void frameCallback(const ros::TimerEvent &)
Definition: missing_tf.cpp:129
make6DofMarker
void make6DofMarker(bool fixed)
Definition: missing_tf.cpp:71
transform_broadcaster.h
tf2_ros::TransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
ros::TimerEvent
interactive_markers::InteractiveMarkerServer
Definition: interactive_marker_server.h:56
ros::Time
server
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server
Definition: missing_tf.cpp:40
tf2_ros::TransformBroadcaster
tf2::Quaternion
tf2_geometry_msgs.h
toMsg
B toMsg(const A &a)
makeBoxControl
InteractiveMarkerControl & makeBoxControl(InteractiveMarker &msg)
Definition: missing_tf.cpp:60
ros::spin
ROSCPP_DECL void spin()
makeBox
Marker makeBox(InteractiveMarker &msg)
Definition: missing_tf.cpp:44
header
const std::string header
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::createTimer
Timer createTimer(Duration period, const TimerCallback &callback, bool oneshot=false, bool autostart=true) const
ros::Duration
ros::Timer
ros::NodeHandle
ros::Time::now
static Time now()


interactive_markers
Author(s): David Gossow, William Woodall
autogenerated on Fri Oct 27 2023 02:31:54