40 using namespace visualization_msgs;
46 void processFeedback(
const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
48 switch ( feedback->event_type )
50 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
54 tf::Vector3 fb_pos(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z);
55 unsigned index = atoi( feedback->marker_name.c_str() );
61 tf::Vector3 fb_delta = fb_pos -
positions[index];
64 for (
unsigned i=0; i<
positions.size(); i++ )
67 float t = 1 / (
d*5.0+1.0) - 0.2;
77 geometry_msgs::Pose pose;
84 server->setPose(
s.str(), pose );
96 InteractiveMarkerControl control;
97 control.always_visible =
true;
98 control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
99 control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
100 control.independent_marker_orientation =
true;
104 marker.type = Marker::CUBE;
105 marker.scale.x = msg.scale;
106 marker.scale.y = msg.scale;
107 marker.scale.z = msg.scale;
108 marker.color.r = 0.65+0.7*msg.pose.position.x;
109 marker.color.g = 0.65+0.7*msg.pose.position.y;
110 marker.color.b = 0.65+0.7*msg.pose.position.z;
111 marker.color.a = 1.0;
113 control.markers.push_back( marker );
114 msg.controls.push_back( control );
116 return msg.controls.back();
122 int side_length = 10;
123 float step = 1.0/ (float)side_length;
126 positions.reserve( side_length*side_length*side_length );
128 for (
double x=-0.5;
x<0.5;
x+=
step )
130 for (
double y=-0.5;
y<0.5;
y+=
step )
132 for (
double z=0.0;
z<1.0;
z+=
step )
159 int main(
int argc,
char** argv)