Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
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
00176 ros::Timer frame_timer = n.createTimer(ros::Duration(0.5), frameCallback);
00177
00178 make6DofMarker(false);
00179 server->applyChanges();
00180
00181 ros::spin();
00182 }