29 if (nh_tracker.
hasParam(
"update_rate"))
40 ROS_ERROR(
"No chain_tip_link specified. Aborting!");
50 ROS_ERROR(
"No root_frame specified. Setting to 'base_link'!");
54 if (nh_tracker.
hasParam(
"target_frame"))
60 ROS_ERROR(
"No target_frame specified. Aborting!");
64 if (nh_tracker.
hasParam(
"movable_trans"))
68 if (nh_tracker.
hasParam(
"movable_rot"))
78 ROS_INFO(
"Waiting for StartTrackingServer...");
82 ROS_INFO(
"Waiting for StartLookatServer...");
86 ROS_INFO(
"Waiting for StopServer...");
88 ROS_INFO(
"InteractiveFrameTarget: All Services available!");
90 bool transform_available =
false;
91 while (!transform_available)
96 transform_available =
true;
119 visualization_msgs::InteractiveMarkerControl control;
120 control.always_visible =
true;
121 control.orientation.w = 1;
122 control.orientation.x = 1;
123 control.orientation.y = 0;
124 control.orientation.z = 0;
126 visualization_msgs::Marker box_marker;
127 box_marker.type = visualization_msgs::Marker::CUBE;
128 box_marker.scale.x = 0.1;
129 box_marker.scale.y = 0.1;
130 box_marker.scale.z = 0.1;
131 box_marker.color.r = 0.0;
132 box_marker.color.g = 0.5;
133 box_marker.color.b = 0.5;
134 box_marker.color.a = 0.5;
135 visualization_msgs::InteractiveMarkerControl control_3d;
136 control_3d.always_visible =
true;
139 control.name =
"move_x";
140 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
142 control.name =
"move_y";
143 control.orientation.x = 0;
144 control.orientation.y = 1;
145 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
147 control.name =
"move_z";
148 control.orientation.y = 0;
149 control.orientation.z = 1;
150 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
152 control_3d.name =
"move_3D";
153 control_3d.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D;
157 control.orientation.w = 1;
158 control.orientation.x = 1;
159 control.orientation.y = 0;
160 control.orientation.z = 0;
161 control.name =
"rotate_x";
162 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
164 control.name =
"rotate_y";
165 control.orientation.x = 0;
166 control.orientation.y = 1;
167 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
169 control.name =
"rotate_z";
170 control.orientation.y = 0;
171 control.orientation.z = 1;
172 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
174 control_3d.name =
"rotate_3D";
175 control_3d.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_3D;
179 control_3d.name =
"move_rotate_3D";
180 control_3d.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D;
182 control_3d.markers.push_back(box_marker);
194 visualization_msgs::Marker menu_marker;
195 menu_marker.scale.x = 0.1;
196 menu_marker.scale.y = 0.1;
197 menu_marker.scale.z = 0.1;
198 menu_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
200 menu_marker.color.r = 0.0;
201 menu_marker.color.g = 0.0;
202 menu_marker.color.b = 1.0;
203 menu_marker.color.a = 0.85;
205 visualization_msgs::InteractiveMarkerControl control_menu;
206 control_menu.always_visible =
true;
207 control_menu.interaction_mode = visualization_msgs::InteractiveMarkerControl::MENU;
208 control_menu.name =
"menu_control";
210 control_menu.markers.push_back(menu_marker);
219 ROS_INFO(
"INTERACTIVE_MARKER...initialized!");
235 bool transform_available =
false;
236 while (!transform_available)
241 transform_available =
true;
250 geometry_msgs::Pose new_pose;
264 cob_srvs::SetString srv;
268 if (success && srv.response.success)
282 cob_srvs::SetString srv;
286 if (success && srv.response.success)
303 std_srvs::Trigger srv;
306 if (success && srv.response.success)
328 target_pose_.
setOrigin(tf::Vector3(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z));