missing_tf.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 #include <ros/ros.h>
00032 
00033 #include <tf/transform_broadcaster.h>
00034 #include <tf/tf.h>
00035 
00036 #include <interactive_markers/interactive_marker_server.h>
00037 
00038 #include <math.h>
00039 
00040 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
00041 
00042 using namespace visualization_msgs;
00043 
00044 Marker makeBox( InteractiveMarker &msg )
00045 {
00046   Marker marker;
00047 
00048   marker.type = Marker::CUBE;
00049   marker.scale.x = msg.scale * 0.45;
00050   marker.scale.y = msg.scale * 0.45;
00051   marker.scale.z = msg.scale * 0.45;
00052   marker.color.r = 0.5;
00053   marker.color.g = 0.5;
00054   marker.color.b = 0.5;
00055   marker.color.a = 1.0;
00056 
00057   return marker;
00058 }
00059 
00060 InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
00061 {
00062   InteractiveMarkerControl control;
00063   control.always_visible = true;
00064   control.markers.push_back( makeBox(msg) );
00065   msg.controls.push_back( control );
00066 
00067   return msg.controls.back();
00068 }
00069 
00070 // %Tag(6DOF)%
00071 void make6DofMarker( bool fixed )
00072 {
00073   InteractiveMarker int_marker;
00074   int_marker.header.frame_id = "/base_link";
00075   int_marker.header.stamp = ros::Time::now();
00076   int_marker.scale = 1;
00077 
00078   int_marker.name = "simple_6dof";
00079   int_marker.description = "Simple 6-DOF Control";
00080 
00081   // insert a box
00082   makeBoxControl(int_marker);
00083 
00084   InteractiveMarkerControl control;
00085 
00086   if ( fixed )
00087   {
00088     int_marker.name += "_fixed";
00089     int_marker.description += "\n(fixed orientation)";
00090     control.orientation_mode = InteractiveMarkerControl::FIXED;
00091   }
00092 
00093   control.orientation.w = 1;
00094   control.orientation.x = 1;
00095   control.orientation.y = 0;
00096   control.orientation.z = 0;
00097   control.name = "rotate_x";
00098   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00099   int_marker.controls.push_back(control);
00100   control.name = "move_x";
00101   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00102   int_marker.controls.push_back(control);
00103 
00104   control.orientation.w = 1;
00105   control.orientation.x = 0;
00106   control.orientation.y = 1;
00107   control.orientation.z = 0;
00108   control.name = "rotate_z";
00109   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00110   int_marker.controls.push_back(control);
00111   control.name = "move_z";
00112   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00113   int_marker.controls.push_back(control);
00114 
00115   control.orientation.w = 1;
00116   control.orientation.x = 0;
00117   control.orientation.y = 0;
00118   control.orientation.z = 1;
00119   control.name = "rotate_y";
00120   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00121   int_marker.controls.push_back(control);
00122   control.name = "move_y";
00123   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00124   int_marker.controls.push_back(control);
00125 
00126   server->insert(int_marker);
00127 }
00128 
00129 void frameCallback(const ros::TimerEvent&)
00130 {
00131   static tf::TransformBroadcaster br;
00132   static bool sending = true;
00133 
00134   geometry_msgs::Pose pose;
00135   pose.orientation.w = 1;
00136 
00137   ros::Time time = ros::Time::now();
00138 
00139   std_msgs::Header header;
00140   header.frame_id = "/base_link";
00141   header.stamp = time;
00142 
00143   int seconds = time.toSec();
00144 
00145   ROS_INFO( "%.3f", time.toSec() );
00146 
00147   if ( seconds % 4 < 2 )
00148   {
00149     if (!sending) ROS_INFO("on");
00150     sending = true;
00151   }
00152   else
00153   {
00154     if (sending) ROS_INFO("off");
00155     sending = false;
00156     header.frame_id = "missing_frame";
00157   }
00158 
00159   server->setPose("simple_6dof",pose,header);
00160   server->applyChanges();
00161 
00162   tf::Transform t;
00163   t.setOrigin(tf::Vector3(0.0, 0.0, 1.0));
00164   t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
00165   br.sendTransform(tf::StampedTransform(t, time, "base_link", "bursty_frame"));
00166 }
00167 
00168 int main(int argc, char** argv)
00169 {
00170   ros::init(argc, argv, "bursty_tf");
00171   ros::NodeHandle n;
00172 
00173   server.reset( new interactive_markers::InteractiveMarkerServer("bursty_tf","",false) );
00174 
00175   // create a timer to update the published transforms
00176   ros::Timer frame_timer = n.createTimer(ros::Duration(0.5), frameCallback);
00177 
00178   make6DofMarker(false);
00179   server->applyChanges();
00180 
00181   ros::spin();
00182 }


interactive_markers
Author(s): David Gossow
autogenerated on Sat Aug 27 2016 03:37:45