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 % 2 < 1 )
00148 {
00149 tf::Transform t;
00150 t.setOrigin(tf::Vector3(0.0, 0.0, 1.0));
00151 t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
00152 if (!sending) ROS_INFO("on");
00153 sending = true;
00154 br.sendTransform(tf::StampedTransform(t, time, "base_link", "bursty_frame"));
00155 usleep(10000);
00156 }
00157 else
00158 {
00159 if (sending) ROS_INFO("off");
00160 sending = false;
00161 }
00162
00163 server->setPose("simple_6dof",pose,header);
00164 server->applyChanges();
00165 }
00166
00167 int main(int argc, char** argv)
00168 {
00169 ros::init(argc, argv, "bursty_tf");
00170 ros::NodeHandle n;
00171
00172 server.reset( new interactive_markers::InteractiveMarkerServer("bursty_tf","",false) );
00173
00174
00175 ros::Timer frame_timer = n.createTimer(ros::Duration(0.5), frameCallback);
00176
00177 make6DofMarker(false);
00178 server->applyChanges();
00179
00180 ros::spin();
00181 }