interactive_marker_interface.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 
3 #include <tf/tf.h>
4 #include <tf/transform_listener.h>
5 #include <tf/transform_broadcaster.h>
6 
7 #include <interactive_markers/interactive_marker_server.h>
8 #include <jsk_interactive_marker/interactive_marker_helpers.h>
9 
10 #include <interactive_markers/menu_handler.h>
11 #include <jsk_interactive_marker/SetPose.h>
12 #include <jsk_interactive_marker/MarkerSetPose.h>
13 
14 #include <math.h>
15 #include <jsk_interactive_marker/MarkerMenu.h>
16 #include <jsk_interactive_marker/MarkerPose.h>
17 
18 #include <std_msgs/Int8.h>
19 
20 #include <jsk_interactive_marker/interactive_marker_interface.h>
21 #include <jsk_interactive_marker/interactive_marker_utils.h>
22 
23 #include <dynamic_tf_publisher/SetDynamicTF.h>
24 
25 #include <kdl/frames_io.hpp>
26 #include <tf_conversions/tf_kdl.h>
27 
28 using namespace im_utils;
29 
30 visualization_msgs::InteractiveMarker InteractiveMarkerInterface::make6DofControlMarker( std::string name, geometry_msgs::PoseStamped &stamped, float scale, bool fixed_position, bool fixed_rotation){
31 
32  visualization_msgs::InteractiveMarker int_marker;
33  int_marker.header = stamped.header;
34  int_marker.name = name;
35  int_marker.scale = scale;
36  int_marker.pose = stamped.pose;
37 
38  visualization_msgs::InteractiveMarkerControl control;
39 
40  //x axis
41  if(fixed_rotation){
42  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
43  }else{
44  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
45  }
46 
47  control.orientation.w = 1;
48  control.orientation.x = 1;
49  control.orientation.y = 0;
50  control.orientation.z = 0;
51  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
52  int_marker.controls.push_back(control);
53 
54  if(fixed_position){
55  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
56  }else{
57  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
58  }
59 
60  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
61  int_marker.controls.push_back(control);
62 
63 
64  //y axis
65  if(fixed_rotation){
66  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
67  }else{
68  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
69  }
70  control.orientation.w = 1;
71  control.orientation.x = 0;
72  control.orientation.y = 1;
73  control.orientation.z = 0;
74  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
75  int_marker.controls.push_back(control);
76 
77  if(fixed_position){
78  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
79  }else{
80  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
81  }
82 
83  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
84  int_marker.controls.push_back(control);
85 
86  //z axis
87  if(fixed_rotation){
88  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
89  }else{
90  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
91  }
92 
93  control.orientation.w = 1;
94  control.orientation.x = 0;
95  control.orientation.y = 0;
96  control.orientation.z = 1;
97  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
98  int_marker.controls.push_back(control);
99  if(fixed_position){
100  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
101  }else{
102  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
103  }
104  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
105  int_marker.controls.push_back(control);
106 
107  return int_marker;
108 }
109 
110 
111 
112 void InteractiveMarkerInterface::proc_feedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) {
113  if(feedback->control_name.find("center_sphere") != std::string::npos){
114  proc_feedback(feedback, jsk_interactive_marker::MarkerPose::SPHERE_MARKER);
115  }else{
116  proc_feedback(feedback, jsk_interactive_marker::MarkerPose::GENERAL);
117  }
118 
119 }
120 
121 void InteractiveMarkerInterface::proc_feedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int type ) {
122 
123  jsk_interactive_marker::MarkerPose mp;
124  mp.pose.header = feedback->header;
125  mp.pose.pose = feedback->pose;
126  mp.marker_name = feedback->marker_name;
127  mp.type = type;
128  pub_.publish( mp );
129 
130  //update Marker Pose Status
131  control_state_.marker_pose_.pose = feedback->pose;
132  control_state_.marker_pose_.header = feedback->header;
133 
134  pub_marker_tf(feedback->header, feedback->pose);
135 
136 }
137 
138 void InteractiveMarkerInterface::pub_marker_tf ( std_msgs::Header header, geometry_msgs::Pose pose){
139  geometry_msgs::Transform tf;
140  tf.translation.x = pose.position.x;
141  tf.translation.y = pose.position.y;
142  tf.translation.z = pose.position.z;
143  tf.rotation = pose.orientation;
144 
145  dynamic_tf_publisher::SetDynamicTF SetTf;
146  SetTf.request.freq = 10;
147  SetTf.request.cur_tf.header.stamp = ros::Time::now();
148  SetTf.request.cur_tf.header.frame_id = header.frame_id;
149  SetTf.request.cur_tf.child_frame_id = "/moving_marker";
150  SetTf.request.cur_tf.transform = tf;
151  dynamic_tf_publisher_client_.call(SetTf);
152 }
153 
154 void InteractiveMarkerInterface::pub_marker_pose ( std_msgs::Header header, geometry_msgs::Pose pose, std::string name, int type ) {
155  jsk_interactive_marker::MarkerPose mp;
156  mp.pose.header = header;
157  mp.pose.pose = pose;
158  mp.marker_name = name;
159  mp.type = type;
160  pub_.publish( mp );
161 }
162 
163 
164 
165 
166 void InteractiveMarkerInterface::pub_marker_menuCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int menu){
167  jsk_interactive_marker::MarkerMenu m;
168  m.marker_name = feedback->marker_name;
169  m.menu=menu;
170  pub_move_.publish(m);
171 }
172 
173 void InteractiveMarkerInterface::pub_marker_menuCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int menu, int type){
174  jsk_interactive_marker::MarkerMenu m;
175  m.marker_name = feedback->marker_name;
176  m.menu = menu;
177  m.type = type;
178  pub_move_.publish(m);
179 }
180 
181 
182 void InteractiveMarkerInterface::pub_marker_menu(std::string marker, int menu, int type){
183  jsk_interactive_marker::MarkerMenu m;
184  m.marker_name = marker;
185  m.menu=menu;
186  m.type = type;
187  pub_move_.publish(m);
188 }
189 
190 void InteractiveMarkerInterface::pub_marker_menu(std::string marker, int menu){
191  pub_marker_menu(marker, menu, 0);
192 }
193 
194 void InteractiveMarkerInterface::IMSizeLargeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
195  geometry_msgs::PoseStamped pose;
196  pose.header = feedback->header;
197  pose.pose = feedback->pose;
198  changeMarkerMoveMode(feedback->marker_name, 0, 0.5, pose);
199 }
200 
201 void InteractiveMarkerInterface::IMSizeMiddleCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
202  geometry_msgs::PoseStamped pose;
203  pose.header = feedback->header;
204  pose.pose = feedback->pose;
205  changeMarkerMoveMode(feedback->marker_name, 0, 0.3, pose);
206 }
207 
208 void InteractiveMarkerInterface::IMSizeSmallCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
209  geometry_msgs::PoseStamped pose;
210  pose.header = feedback->header;
211  pose.pose = feedback->pose;
212  changeMarkerMoveMode(feedback->marker_name, 0, 0.1, pose);
213 }
214 
215 void InteractiveMarkerInterface::changeMoveModeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
216  changeMarkerMoveMode(feedback->marker_name,0);
217  pub_marker_menu(feedback->marker_name,13);
218 
219 }
220 void InteractiveMarkerInterface::changeMoveModeCb1( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
221  changeMarkerMoveMode(feedback->marker_name,1);
222  pub_marker_menu(feedback->marker_name,13);
223 
224 }
225 void InteractiveMarkerInterface::changeMoveModeCb2( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
226  changeMarkerMoveMode(feedback->marker_name,2);
227  pub_marker_menu(feedback->marker_name,13);
228 }
229 
230 void InteractiveMarkerInterface::changeForceModeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
231  ROS_INFO("%s changeForceMode",feedback->marker_name.c_str());
232  changeMarkerForceMode(feedback->marker_name,0);
233  pub_marker_menu(feedback->marker_name,12);
234 
235 }
236 void InteractiveMarkerInterface::changeForceModeCb1( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
237  ROS_INFO("%s changeForceMode1",feedback->marker_name.c_str());
238  changeMarkerForceMode(feedback->marker_name,1);
239  pub_marker_menu(feedback->marker_name,12);
240 
241 }
242 void InteractiveMarkerInterface::changeForceModeCb2( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
243  ROS_INFO("%s changeForceMode2",feedback->marker_name.c_str());
244  changeMarkerForceMode(feedback->marker_name,2);
245  pub_marker_menu(feedback->marker_name,12);
246 }
247 
248 void InteractiveMarkerInterface::targetPointMenuCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
249  ROS_INFO("targetPointMenu callback");
250 
251  control_state_.head_on_ ^= true;
252  control_state_.init_head_goal_ = true;
253 
254  if(control_state_.head_on_){
255  menu_head_.setCheckState(head_target_handle_, interactive_markers::MenuHandler::CHECKED);
256  control_state_.look_auto_on_ = false;
257  menu_head_.setCheckState(head_auto_look_handle_, interactive_markers::MenuHandler::UNCHECKED);
258  pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::HEAD_TARGET_POINT, jsk_interactive_marker::MarkerMenu::HEAD_MARKER);
259  }else{
260  menu_head_.setCheckState(head_target_handle_, interactive_markers::MenuHandler::UNCHECKED);
261  }
262  menu_head_.reApply(*server_);
263 
264  initControlMarkers();
265 }
266 
267 void InteractiveMarkerInterface::lookAutomaticallyMenuCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
268  ROS_INFO("targetPointMenu callback");
269 
270  control_state_.head_on_ = false;
271  control_state_.look_auto_on_ ^= true;
272  control_state_.init_head_goal_ = true;
273 
274  if(control_state_.look_auto_on_){
275  menu_head_.setCheckState(head_auto_look_handle_, interactive_markers::MenuHandler::CHECKED);
276  menu_head_.setCheckState(head_target_handle_, interactive_markers::MenuHandler::UNCHECKED);
277  pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::PUBLISH_MARKER, jsk_interactive_marker::MarkerMenu::HEAD_MARKER);
278  }else{
279  menu_head_.setCheckState(head_auto_look_handle_, interactive_markers::MenuHandler::UNCHECKED);
280  pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::HEAD_TARGET_POINT, jsk_interactive_marker::MarkerMenu::HEAD_MARKER);
281  }
282  menu_head_.reApply(*server_);
283  initControlMarkers();
284 }
285 
286 
287 void InteractiveMarkerInterface::ConstraintCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
288 {
289  menu_handler.setCheckState( h_mode_last2, interactive_markers::MenuHandler::UNCHECKED );
290  h_mode_last2 = feedback->menu_entry_id;
291  menu_handler.setCheckState( h_mode_last2, interactive_markers::MenuHandler::CHECKED );
292 
293  switch(h_mode_last2-h_mode_constrained){
294  case 0:
295  pub_marker_menu(feedback->marker_name,jsk_interactive_marker::MarkerMenu::MOVE_CONSTRAINT_T);
296  ROS_INFO("send 23");
297  break;
298  case 1:
299  pub_marker_menu(feedback->marker_name,jsk_interactive_marker::MarkerMenu::MOVE_CONSTRAINT_NIL);
300  ROS_INFO("send 24");
301  break;
302  default:
303  ROS_INFO("Switching Arm Error");
304  break;
305  }
306 
307  menu_handler.reApply( *server_ );
308  server_->applyChanges();
309 
310 }
311 
312 void InteractiveMarkerInterface::useTorsoCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
313 {
314  if(feedback->menu_entry_id == use_torso_t_menu_){
315  menu_handler.setCheckState( use_torso_t_menu_ , interactive_markers::MenuHandler::CHECKED );
316  menu_handler.setCheckState( use_torso_nil_menu_ , interactive_markers::MenuHandler::UNCHECKED );
317  menu_handler.setCheckState( use_fullbody_menu_ , interactive_markers::MenuHandler::UNCHECKED );
318  pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::USE_TORSO_T);
319  }else if(feedback->menu_entry_id == use_torso_nil_menu_){
320  menu_handler.setCheckState( use_torso_t_menu_ , interactive_markers::MenuHandler::UNCHECKED );
321  menu_handler.setCheckState( use_torso_nil_menu_ , interactive_markers::MenuHandler::CHECKED );
322  menu_handler.setCheckState( use_fullbody_menu_ , interactive_markers::MenuHandler::UNCHECKED );
323  pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::USE_TORSO_NIL);
324  }else{
325  menu_handler.setCheckState( use_torso_t_menu_ , interactive_markers::MenuHandler::UNCHECKED );
326  menu_handler.setCheckState( use_torso_nil_menu_ , interactive_markers::MenuHandler::UNCHECKED );
327  menu_handler.setCheckState( use_fullbody_menu_ , interactive_markers::MenuHandler::CHECKED );
328  pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::USE_FULLBODY);
329  }
330  menu_handler.reApply( *server_ );
331  server_->applyChanges();
332 
333 }
334 
335 void InteractiveMarkerInterface::usingIKCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
336 {
337  if(feedback->menu_entry_id == start_ik_menu_){
338  menu_handler.setCheckState( start_ik_menu_ , interactive_markers::MenuHandler::CHECKED );
339  menu_handler.setCheckState( stop_ik_menu_ , interactive_markers::MenuHandler::UNCHECKED );
340  pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::PLAN);
341  }else if(feedback->menu_entry_id == stop_ik_menu_){
342  menu_handler.setCheckState( start_ik_menu_ , interactive_markers::MenuHandler::UNCHECKED );
343  menu_handler.setCheckState( stop_ik_menu_ , interactive_markers::MenuHandler::CHECKED );
344  pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::CANCEL_PLAN);
345  }
346  menu_handler.reApply( *server_ );
347  server_->applyChanges();
348 
349 }
350 
351 void InteractiveMarkerInterface::toggleStartIKCb( const std_msgs::EmptyConstPtr &msg)
352 {
353  interactive_markers::MenuHandler::CheckState check_state;
354  if(menu_handler.getCheckState( start_ik_menu_ , check_state)){
355 
356  if(check_state == interactive_markers::MenuHandler::CHECKED){
357  //stop ik
358  menu_handler.setCheckState( start_ik_menu_ , interactive_markers::MenuHandler::UNCHECKED );
359  menu_handler.setCheckState( stop_ik_menu_ , interactive_markers::MenuHandler::CHECKED );
360  pub_marker_menu("", jsk_interactive_marker::MarkerMenu::CANCEL_PLAN);
361 
362  }else{
363  //start ik
364  menu_handler.setCheckState( start_ik_menu_ , interactive_markers::MenuHandler::CHECKED );
365  menu_handler.setCheckState( stop_ik_menu_ , interactive_markers::MenuHandler::UNCHECKED );
366  pub_marker_menu("" , jsk_interactive_marker::MarkerMenu::PLAN);
367  }
368 
369  menu_handler.reApply( *server_ );
370  server_->applyChanges();
371  }
372 }
373 
374 
375 void InteractiveMarkerInterface::modeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
376 {
377  menu_handler.setCheckState( h_mode_last, interactive_markers::MenuHandler::UNCHECKED );
378  h_mode_last = feedback->menu_entry_id;
379  menu_handler.setCheckState( h_mode_last, interactive_markers::MenuHandler::CHECKED );
380 
381  switch(h_mode_last - h_mode_rightarm){
382  case 0:
383  changeMoveArm( feedback->marker_name, jsk_interactive_marker::MarkerMenu::SET_MOVE_RARM);
384  break;
385  case 1:
386  changeMoveArm( feedback->marker_name, jsk_interactive_marker::MarkerMenu::SET_MOVE_LARM);
387  break;
388  case 2:
389  changeMoveArm( feedback->marker_name, jsk_interactive_marker::MarkerMenu::SET_MOVE_ARMS);
390  break;
391  default:
392  ROS_INFO("Switching Arm Error");
393  break;
394  }
395  menu_handler.reApply( *server_ );
396  server_->applyChanges();
397 }
398 
399 void InteractiveMarkerInterface::changeMoveArm( std::string m_name, int menu ){
400  switch(menu){
401  case jsk_interactive_marker::MarkerMenu::SET_MOVE_RARM:
402  pub_marker_menu(m_name,jsk_interactive_marker::MarkerMenu::SET_MOVE_RARM);
403  control_state_.move_arm_ = ControlState::RARM;
404  ROS_INFO("move Rarm");
405  changeMarkerMoveMode( marker_name.c_str(), 0, 0.5, control_state_.marker_pose_);
406  break;
407  case jsk_interactive_marker::MarkerMenu::SET_MOVE_LARM:
408  pub_marker_menu(m_name,jsk_interactive_marker::MarkerMenu::SET_MOVE_LARM);
409  control_state_.move_arm_ = ControlState::LARM;
410  ROS_INFO("move Larm");
411  changeMarkerMoveMode( marker_name.c_str(), 0, 0.5, control_state_.marker_pose_);
412  break;
413  case jsk_interactive_marker::MarkerMenu::SET_MOVE_ARMS:
414  pub_marker_menu(m_name,jsk_interactive_marker::MarkerMenu::SET_MOVE_ARMS);
415  control_state_.move_arm_ = ControlState::ARMS;
416  ROS_INFO("move Arms");
417  changeMarkerMoveMode( marker_name.c_str(), 0, 0.5, control_state_.marker_pose_);
418  break;
419  default:
420  ROS_INFO("Switching Arm Error");
421  break;
422  }
423 }
424 
425 void InteractiveMarkerInterface::setOriginCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, bool origin_hand){
426  if(origin_hand){
427  control_state_.move_origin_state_ = ControlState::HAND_ORIGIN;
428  changeMarkerMoveMode( marker_name.c_str(), 0, 0.5, control_state_.marker_pose_);
429  if(control_state_.move_arm_ == ControlState::RARM){
430  pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::SET_ORIGIN_RHAND);}else{
431  pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::SET_ORIGIN_LHAND);}
432  }else{
433  control_state_.move_origin_state_ = ControlState::DESIGNATED_ORIGIN;
434  changeMarkerMoveMode( marker_name.c_str(), 0, 0.5, control_state_.marker_pose_);
435  pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::SET_ORIGIN);
436  }
437 
438 
439 }
440 
441 
442 void InteractiveMarkerInterface::ikmodeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
443 {
444  //menu_handler.setCheckState( h_mode_last3, interactive_markers::MenuHandler::UNCHECKED );
445  //h_mode_last3 = feedback->menu_entry_id;
446  //menu_handler.setCheckState( h_mode_last3, interactive_markers::MenuHandler::CHECKED );
447 
448  if(feedback->menu_entry_id == rotation_t_menu_){
449  menu_handler.setCheckState( rotation_nil_menu_, interactive_markers::MenuHandler::UNCHECKED );
450  pub_marker_menu(feedback->marker_name,jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_T);
451  ROS_INFO("Rotation Axis T");
452  }else{
453  menu_handler.setCheckState( rotation_t_menu_, interactive_markers::MenuHandler::UNCHECKED );
454  pub_marker_menu(feedback->marker_name ,jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_NIL);
455  ROS_INFO("Rotation Axis NIL");
456  }
457 
458 
459  menu_handler.setCheckState( feedback->menu_entry_id, interactive_markers::MenuHandler::CHECKED );
460 
461  menu_handler.reApply( *server_ );
462  server_->applyChanges();
463 }
464 
465 
466 void InteractiveMarkerInterface::toggleIKModeCb( const std_msgs::EmptyConstPtr &msg)
467 {
468  interactive_markers::MenuHandler::CheckState check_state;
469  if(menu_handler.getCheckState( rotation_t_menu_ , check_state)){
470  if(check_state == interactive_markers::MenuHandler::CHECKED){
471  //rotation axis nil
472  menu_handler.setCheckState( rotation_t_menu_ , interactive_markers::MenuHandler::UNCHECKED );
473  menu_handler.setCheckState( rotation_nil_menu_ , interactive_markers::MenuHandler::CHECKED );
474  pub_marker_menu("", jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_NIL);
475 
476  }else{
477  //rotation_axis t
478  menu_handler.setCheckState( rotation_t_menu_ , interactive_markers::MenuHandler::CHECKED );
479  menu_handler.setCheckState( rotation_nil_menu_ , interactive_markers::MenuHandler::UNCHECKED );
480  pub_marker_menu("" , jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_T);
481  }
482 
483  menu_handler.reApply( *server_ );
484  server_->applyChanges();
485  }
486 }
487 
488 void InteractiveMarkerInterface::marker_menu_cb( const jsk_interactive_marker::MarkerMenuConstPtr &msg){
489  switch (msg->menu){
490  case jsk_interactive_marker::MarkerMenu::SET_MOVE_RARM:
491  case jsk_interactive_marker::MarkerMenu::SET_MOVE_LARM:
492  case jsk_interactive_marker::MarkerMenu::SET_MOVE_ARMS:
493  changeMoveArm(msg->marker_name, msg->menu);
494  break;
495  case jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_NIL:
496  case jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_T:
497  {
498  std_msgs::EmptyConstPtr empty;
499  toggleIKModeCb(empty);
500  }
501  break;
502  case jsk_interactive_marker::MarkerMenu::PLAN:
503  case jsk_interactive_marker::MarkerMenu::CANCEL_PLAN:
504  {
505  std_msgs::EmptyConstPtr empty;
506  toggleStartIKCb(empty);
507  }
508  break;
509  default:
510  pub_marker_menu(msg->marker_name , msg->menu, msg->type);
511  break;
512  }
513 }
514 
515 
516 void InteractiveMarkerInterface::updateHeadGoal( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
517 {
518  ros::Time now = ros::Time(0);
519 
520  switch ( feedback->event_type )
521  {
522  case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
523  ROS_INFO_STREAM( feedback->marker_name << " was clicked on." );
524  break;
525  case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
526  ROS_INFO_STREAM( "Marker " << feedback->marker_name
527  << " control " << feedback->control_name
528  << " menu_entry_id " << feedback->menu_entry_id);
529  break;
530  case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
531  //proc_feedback(feedback, jsk_interactive_marker::MarkerPose::HEAD_MARKER);
532  break;
533  }
534 }
535 
536 void InteractiveMarkerInterface::updateBase( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
537 {
538  switch ( feedback->event_type )
539  {
540  case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
541  proc_feedback(feedback, jsk_interactive_marker::MarkerPose::BASE_MARKER);
542  break;
543  }
544 }
545 
546 void InteractiveMarkerInterface::updateFinger( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, std::string hand)
547 {
548  ros::Time now = ros::Time(0);
549 
550  switch ( feedback->event_type )
551  {
552  case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
553  if(hand == "rhand"){
554  control_state_.r_finger_on_ ^= true;
555  }else if(hand == "lhand"){
556  control_state_.l_finger_on_ ^= true;
557  }
558  initControlMarkers();
559  ROS_INFO_STREAM( hand << feedback->marker_name << " was clicked on." );
560  break;
561  case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
562  ROS_INFO_STREAM( hand << feedback->marker_name << " was clicked on." );
563  if(hand == "rhand"){
564  proc_feedback(feedback, jsk_interactive_marker::MarkerPose::RFINGER_MARKER);
565  }
566  if(hand == "lhand"){
567  proc_feedback(feedback, jsk_interactive_marker::MarkerPose::LFINGER_MARKER);
568  }
569 
570  //proc_feedback(feedback);
571  break;
572  }
573 }
574 
575 
576 //im_mode
577 //0:normal move 1:operationModel 2:operationalModelFirst
578 void InteractiveMarkerInterface::changeMarkerForceMode( std::string mk_name , int im_mode){
579  ROS_INFO("changeMarkerForceMode marker:%s mode:%d\n",mk_name.c_str(),im_mode);
580  interactive_markers::MenuHandler reset_handler;
581  menu_handler_force = reset_handler;
582  menu_handler_force1 = reset_handler;
583  menu_handler_force2 = reset_handler;
584 
585  geometry_msgs::PoseStamped pose;
586  pose.header.frame_id = base_frame;
587  if ( target_frame != "" ) {
588  /*
589  tf::StampedTransform stf;
590  geometry_msgs::TransformStamped mtf;
591  tfl_.lookupTransform(target_frame, base_frame,
592  ros::Time(0), stf);
593  tf::transformStampedTFToMsg(stf, mtf);
594  pose.pose.position.x = mtf.transform.translation.x;
595  pose.pose.position.y = mtf.transform.translation.y;
596  pose.pose.position.z = mtf.transform.translation.z;
597  pose.pose.orientation = mtf.transform.rotation;
598  pose.header = mtf.header;
599  */
600  }
601  visualization_msgs::InteractiveMarker mk;
602  // mk.name = marker_name.c_str();
603  mk.name = mk_name.c_str();
604  mk.scale = 0.5;
605  mk.header = pose.header;
606  mk.pose = pose.pose;
607 
608  // visualization_msgs::InteractiveMarker mk =
609  // im_helpers::make6DofMarker(marker_name.c_str(), pose, 0.5,
610  // true, false );
611  visualization_msgs::InteractiveMarkerControl control;
612 
613  if ( false )
614  {
615  //int_marker.name += "_fixed";
616  //int_marker.description += "\n(fixed orientation)";
617  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
618  }
619 
620  control.orientation.w = 1;
621  control.orientation.x = 1;
622  control.orientation.y = 0;
623  control.orientation.z = 0;
624  control.name = "rotate_x";
625  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
626  mk.controls.push_back(control);
627  control.name = "move_x";
628  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
629  mk.controls.push_back(control);
630 
631  control.orientation.w = 1;
632  control.orientation.x = 0;
633  control.orientation.y = 1;
634  control.orientation.z = 0;
635  control.name = "rotate_z";
636  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
637  mk.controls.push_back(control);
638  // control.name = "move_z";
639  // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
640  // mk.controls.push_back(control);
641 
642  control.orientation.w = 1;
643  control.orientation.x = 0;
644  control.orientation.y = 0;
645  control.orientation.z = 1;
646  control.name = "rotate_y";
647  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
648  mk.controls.push_back(control);
649  // control.name = "move_y";
650  // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
651  // mk.controls.push_back(control);
652 
653  //add furuta
654  switch(im_mode){
655  case 0:
656  menu_handler_force.insert("MoveMode",boost::bind( &InteractiveMarkerInterface::changeMoveModeCb, this, _1));
657  menu_handler_force.insert("Delete Force",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::DELETE_FORCE));
658  server_->insert( mk );
659  server_->setCallback( mk.name,
660  boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
661  menu_handler_force.apply(*server_,mk.name);
662 
663  server_->applyChanges();
664  break;
665  case 1:
666  mk.scale = 0.5;
667  menu_handler_force1.insert("MoveMode",boost::bind( &InteractiveMarkerInterface::changeMoveModeCb1, this, _1));
668  menu_handler_force1.insert("Delete Force",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::DELETE_FORCE));
669  server_->insert( mk );
670  server_->setCallback( mk.name,
671  boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
672  menu_handler_force1.apply(*server_,mk.name);
673 
674  server_->applyChanges();
675  break;
676  case 2:
677  mk.scale = 0.5;
678  menu_handler_force2.insert("MoveMode",boost::bind( &InteractiveMarkerInterface::changeMoveModeCb2, this, _1));
679  menu_handler_force2.insert("Delete Force",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::DELETE_FORCE));
680  server_->insert( mk );
681  server_->setCallback( mk.name,
682  boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
683  menu_handler_force2.apply(*server_,mk.name);
684 
685  server_->applyChanges();
686  break;
687  default:
688  break;
689  }
690 
691 
692  //menu_handler_force.insert("ResetForce",boost::bind( &InteractiveMarkerInterface::resetForceCb, this, _1));
693 
694  std::list<visualization_msgs::InteractiveMarker>::iterator it = imlist.begin();
695 
696  while( it != imlist.end() ) // listの末尾まで
697  {
698  if(it->name == mk_name.c_str()){
699  imlist.erase(it);
700  break;
701  }
702  it++;
703  }
704  imlist.push_back( mk );
705 
706  /*
707  it = imlist.begin();
708  while( it != imlist.end() ) // listの末尾まで
709  {
710  server_->insert( *it );
711 
712  server_->setCallback( it->name,
713  boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
714 
715  menu_handler_force.apply(*server_,it->name);
716  it++;
717  }*/
718  ROS_INFO("add mk");
719  /* add mk */
720 
721 }
722 
723 void InteractiveMarkerInterface::initBodyMarkers(void){
724  geometry_msgs::PoseStamped ps;
725  ps.header.stamp = ros::Time(0);
726 
727  double scale_factor = 1.02;
728 
729  //for head
730  ps.header.frame_id = head_link_frame_;
731  visualization_msgs::InteractiveMarker im =
732  im_helpers::makeMeshMarker(head_link_frame_, head_mesh_, ps, scale_factor);
733  makeIMVisible(im);
734  server_->insert(im);
735  menu_head_.apply(*server_, head_link_frame_);
736 
737 
738  if(hand_type_ == "sandia_hand"){
739  geometry_msgs::PoseStamped ps;
740  ps.header.stamp = ros::Time(0);
741 
742  ps.header.frame_id = "/right_f0_base";
743  for(int i=0;i<4;i++){
744  for(int j=0;j<3;j++){
745  visualization_msgs::InteractiveMarker fingerIm =
746  makeSandiaHandInteractiveMarker(ps, "right", i, j);
747  makeIMVisible(fingerIm);
748  server_->insert(fingerIm, boost::bind( &InteractiveMarkerInterface::updateFinger, this, _1, "rhand"));
749  }
750  }
751 
752  ps.header.frame_id = "/left_f0_base";
753  for(int i=0;i<4;i++){
754  for(int j=0;j<3;j++){
755  visualization_msgs::InteractiveMarker fingerIm =
756  makeSandiaHandInteractiveMarker(ps, "left", i, j);
757  makeIMVisible(fingerIm);
758  server_->insert(fingerIm, boost::bind( &InteractiveMarkerInterface::updateFinger, this, _1, "lhand"));
759  }
760  }
761 
762  }else{
763  //for right hand
764  for(int i=0; i<rhand_mesh_.size(); i++){
765  ps.header.frame_id = rhand_mesh_[i].link_name;
766  ps.pose.orientation = rhand_mesh_[i].orientation;
767  visualization_msgs::InteractiveMarker handIm =
768  im_helpers::makeMeshMarker( rhand_mesh_[i].link_name, rhand_mesh_[i].mesh_file, ps, scale_factor);
769  makeIMVisible(handIm);
770  server_->insert(handIm);
771  }
772  }
773 }
774 
775 
776 void InteractiveMarkerInterface::initControlMarkers(void){
777  //Head Marker
778  if(control_state_.head_on_ && control_state_.init_head_goal_){
779  control_state_.init_head_goal_ = false;
780  head_goal_pose_.header.stamp = ros::Time(0);
781 
782  visualization_msgs::InteractiveMarker HeadGoalIm =
783  im_helpers::makeHeadGoalMarker( "head_point_goal", head_goal_pose_, 0.1);
784  makeIMVisible(HeadGoalIm);
785  server_->insert(HeadGoalIm,
786  boost::bind( &InteractiveMarkerInterface::updateHeadGoal, this, _1));
787  menu_head_target_.apply(*server_,"head_point_goal");
788  }
789  if(!control_state_.head_on_){
790  server_->erase("head_point_goal");
791  }
792 
793  //Base Marker
794  if(control_state_.base_on_ ){
795  geometry_msgs::PoseStamped ps;
796  ps.pose.orientation.w = 1;
797  ps.header.frame_id = move_base_frame;
798  ps.header.stamp = ros::Time(0);
799  visualization_msgs::InteractiveMarker baseIm =
800  InteractiveMarkerInterface::makeBaseMarker( "base_control", ps, 0.75, false);
801  makeIMVisible(baseIm);
802  server_->insert(baseIm,
803  boost::bind( &InteractiveMarkerInterface::updateBase, this, _1 ));
804 
805  menu_base_.apply(*server_,"base_control");
806  }else{
807  server_->erase("base_control");
808  }
809 
810  //finger Control Marker
811  if(use_finger_marker_ && control_state_.r_finger_on_){
812  geometry_msgs::PoseStamped ps;
813  ps.header.stamp = ros::Time(0);
814  ps.header.frame_id = "/right_f0_base";
815 
816  server_->insert(makeFingerControlMarker("right_finger", ps),
817  boost::bind( &InteractiveMarkerInterface::updateFinger, this, _1, "rhand"));
818  menu_finger_r_.apply(*server_,"right_finger");
819  }else{
820  server_->erase("right_finger");
821  }
822 
823  if(use_finger_marker_ && control_state_.l_finger_on_){
824  geometry_msgs::PoseStamped ps;
825  ps.header.stamp = ros::Time(0);
826  ps.header.frame_id = "/left_f0_base";
827 
828  server_->insert(makeFingerControlMarker("left_finger", ps),
829  boost::bind( &InteractiveMarkerInterface::updateFinger, this, _1, "lhand"));
830  menu_finger_l_.apply(*server_,"left_finger");
831  }else{
832  server_->erase("left_finger");
833  }
834 
835  server_->applyChanges();
836 }
837 
838 
839 visualization_msgs::InteractiveMarker InteractiveMarkerInterface::makeBaseMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed)
840 {
841  visualization_msgs::InteractiveMarker mk;
842  mk.header = stamped.header;
843  mk.name = name;
844  mk.scale = scale;
845  mk.pose = stamped.pose;
846 
847  visualization_msgs::InteractiveMarkerControl control;
848 
849  control.orientation.w = 1;
850  control.orientation.x = 1;
851  control.orientation.y = 0;
852  control.orientation.z = 0;
853 
854  control.name = "move_x";
855  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
856  mk.controls.push_back(control);
857 
858  control.orientation.w = 1;
859  control.orientation.x = 0;
860  control.orientation.y = 1;
861  control.orientation.z = 0;
862  control.name = "rotate_z";
863  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
864  mk.controls.push_back(control);
865 
866  control.orientation.w = 1;
867  control.orientation.x = 0;
868  control.orientation.y = 0;
869  control.orientation.z = 1;
870  control.name = "move_y";
871  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
872  mk.controls.push_back(control);
873  return mk;
874 
875 }
876 
877 
878 
879 void InteractiveMarkerInterface::initHandler(void){
880  //use_arm=2;
881 
882  bool use_menu;
883  pnh_.param("force_mode_menu", use_menu, false );
884  if(use_menu){
885  menu_handler.insert("ForceMode",boost::bind( &InteractiveMarkerInterface::changeForceModeCb, this, _1));
886  }
887 
888  pnh_.param("move_menu", use_menu, false );
889  if(use_menu){
890  pnh_.param("move_safety_menu", use_menu, false );
891  if(use_menu){
892  interactive_markers::MenuHandler::EntryHandle sub_menu_move_;
893  sub_menu_move_ = menu_handler.insert( "Move" );
894  menu_handler.insert( sub_menu_move_,"Plan",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::PLAN));
895  menu_handler.insert( sub_menu_move_,"Execute",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::EXECUTE));
896  menu_handler.insert( sub_menu_move_,"Plan And Execute",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::PLAN_EXECUTE));
897  menu_handler.insert( sub_menu_move_,"Cancel", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::CANCEL_PLAN));
898  }else{
899  menu_handler.insert("Move",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MOVE));
900  }
901  }
902 
903  pnh_.param("change_using_ik_menu", use_menu, false );
904  if(use_menu){
905  interactive_markers::MenuHandler::EntryHandle sub_menu_move_;
906  sub_menu_move_ = menu_handler.insert( "Whether To Use IK" );
907  start_ik_menu_ = menu_handler.insert( sub_menu_move_,"Start IK",boost::bind( &InteractiveMarkerInterface::usingIKCb, this, _1));
908  menu_handler.setCheckState( start_ik_menu_, interactive_markers::MenuHandler::CHECKED );
909 
910  stop_ik_menu_ = menu_handler.insert( sub_menu_move_,"Stop IK",boost::bind( &InteractiveMarkerInterface::usingIKCb, this, _1));
911  menu_handler.setCheckState( stop_ik_menu_, interactive_markers::MenuHandler::UNCHECKED );
912  }
913 
914  //menu_handler.insert("Touch It", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::TOUCH));
915  pnh_.param("touch_it_menu", use_menu, false );
916  if(use_menu){
917 
918  interactive_markers::MenuHandler::EntryHandle sub_menu_handle_touch_it;
919  sub_menu_handle_touch_it = menu_handler.insert( "Touch It" );
920 
921  // menu_handler.insert( sub_menu_handle_touch_it, "Preview", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::TOUCHIT_PREV));
922  menu_handler.insert( sub_menu_handle_touch_it, "Execute", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::TOUCHIT_EXEC));
923  menu_handler.insert( sub_menu_handle_touch_it, "Cancel", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::TOUCHIT_CANCEL));
924  }
925  pnh_.param("look_hand_menu", use_menu, false );
926  if(use_menu){
927 
928 
929  interactive_markers::MenuHandler::EntryHandle sub_menu_handle_look_hand;
930  sub_menu_handle_look_hand = menu_handler.insert( "Look hand" );
931 
932  menu_handler.insert( sub_menu_handle_look_hand, "rarm", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::LOOK_RARM));
933  menu_handler.insert( sub_menu_handle_look_hand, "larm", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::LOOK_LARM));
934  }
935 
936  pnh_.param("force_move_menu", use_menu, false );
937  if(use_menu){
938  menu_handler.insert("Force Move", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::FORCE_MOVE));
939  }
940 
941  pnh_.param("pick_menu", use_menu, false );
942  if(use_menu){
943  menu_handler.insert("Pick", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::PICK));
944  }
945 
946  pnh_.param("grasp_menu", use_menu, false );
947  if(use_menu){
948  menu_handler.insert("Grasp", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::START_GRASP));
949  }
950 
951  pnh_.param("harf_grasp_menu", use_menu, false );
952  if(use_menu){
953  menu_handler.insert("Harf Grasp", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::HARF_GRASP));
954  }
955 
956 
957  pnh_.param("stop_grasp_menu", use_menu, false );
958  if(use_menu){
959  menu_handler.insert("Stop Grasp", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::STOP_GRASP));
960  }
961 
962  pnh_.param("set_origin_menu", use_menu, false );
963  if(use_menu){
964  //menu_handler.insert("Set Origin To Hand", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::SET_ORIGIN));
965  menu_handler.insert("Set Origin To Hand", boost::bind( &InteractiveMarkerInterface::setOriginCb, this, _1, true));
966 
967  menu_handler.insert("Set Origin", boost::bind( &InteractiveMarkerInterface::setOriginCb, this, _1, false));
968  }
969 
970  /*
971  pnh_.param("set_origin_menu", use_menu, false );
972  if(use_menu){
973  menu_handler.insert("Set Origin", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::SET_ORIGIN));
974  }
975 
976  pnh_.param("set_origin_to_rhand_menu", use_menu, false );
977  if(use_menu){
978  menu_handler.insert("Set Origin To RHand", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::SET_ORIGIN_RHAND));
979  }
980 
981  pnh_.param("set_origin_to_lhand_menu", use_menu, false );
982  if(use_menu){
983  menu_handler.insert("Set Origin To LHand", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::SET_ORIGIN_LHAND));
984  }
985  */
986 
987  pnh_.param("reset_marker_pos_menu", use_menu, false );
988  if(use_menu){
989  menu_handler.insert("Reset Marker Position", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::RESET_COORDS));
990  }
991 
992  pnh_.param("manipulation_mode_menu", use_menu, false );
993  if(use_menu){
994  menu_handler.insert("Manipulation Mode", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MANIP_MODE));
995  }
996 
997  // menu_handler.insert("ResetForce",boost::bind( &InteractiveMarkerInterface::resetForceCb, this, _1));
998 
999  //menu_handler.insert("OperationModel",boost::bind( &InteractiveMarkerInterface::AutoMoveCb, this, _1));
1000 
1001  // menu_handler.insert("StartTeaching",boost::bind( &InteractiveMarkerInterface::StartTeachingCb, this, _1));
1002 
1003 
1004  /* sub_menu_handle2 = menu_handler.insert( "Constraint" );
1005 
1006  h_mode_last2 = menu_handler.insert( sub_menu_handle2, "constrained", boost::bind( &InteractiveMarkerInterface::ConstraintCb,this, _1 ));
1007  menu_handler.setCheckState( h_mode_last2, interactive_markers::MenuHandler::UNCHECKED );
1008  h_mode_constrained = h_mode_last2;
1009  h_mode_last2 = menu_handler.insert( sub_menu_handle2, "unconstrained", boost::bind( &InteractiveMarkerInterface::ConstraintCb,this, _1 ));
1010  menu_handler.setCheckState( h_mode_last2, interactive_markers::MenuHandler::CHECKED );
1011  */
1012 
1013  // menu_handler.insert("StopTeaching",boost::bind( &InteractiveMarkerInterface::StopTeachingCb, this, _1));
1014  //menu_handler.setCheckState(menu_handler.insert("SetForce",boost::bind( &InteractiveMarkerInterface::enableCb, this, _1)),interactive_markers::MenuHandler::UNCHECKED);
1015 
1016 
1017  pnh_.param("select_arm_menu", use_menu, false );
1018  if(use_menu){
1019  sub_menu_handle = menu_handler.insert( "SelectArm" );
1020  h_mode_last = menu_handler.insert( sub_menu_handle, "Right Arm", boost::bind( &InteractiveMarkerInterface::modeCb,this, _1 ));
1021  menu_handler.setCheckState( h_mode_last, interactive_markers::MenuHandler::CHECKED );
1022  h_mode_rightarm = h_mode_last;
1023  h_mode_last = menu_handler.insert( sub_menu_handle, "Left Arm", boost::bind( &InteractiveMarkerInterface::modeCb,this, _1 ));
1024  menu_handler.setCheckState( h_mode_last, interactive_markers::MenuHandler::UNCHECKED );
1025  h_mode_last = menu_handler.insert( sub_menu_handle, "Both Arms", boost::bind( &InteractiveMarkerInterface::modeCb,this, _1 ));
1026  menu_handler.setCheckState( h_mode_last, interactive_markers::MenuHandler::UNCHECKED );
1027  h_mode_last = h_mode_rightarm;
1028  }
1029 
1030  pnh_.param("ik_mode_menu", use_menu, false );
1031  if(use_menu){
1032  sub_menu_handle_ik = menu_handler.insert( "IK mode" );
1033 
1034  rotation_t_menu_ = menu_handler.insert( sub_menu_handle_ik, "6D (Position + Rotation)", boost::bind( &InteractiveMarkerInterface::ikmodeCb,this, _1 ));
1035  menu_handler.setCheckState( rotation_t_menu_ , interactive_markers::MenuHandler::CHECKED );
1036  rotation_nil_menu_ = menu_handler.insert( sub_menu_handle_ik, "3D (Position)", boost::bind( &InteractiveMarkerInterface::ikmodeCb,this, _1 ));
1037  menu_handler.setCheckState( rotation_nil_menu_, interactive_markers::MenuHandler::UNCHECKED );
1038  }
1039 
1040  pnh_.param("use_torso_menu", use_menu, false );
1041  if(use_menu){
1042  use_torso_menu_ = menu_handler.insert( "Links To Use" );
1043 
1044  use_torso_nil_menu_ = menu_handler.insert( use_torso_menu_, "Arm", boost::bind( &InteractiveMarkerInterface::useTorsoCb,this, _1 ));
1045  menu_handler.setCheckState( use_torso_nil_menu_, interactive_markers::MenuHandler::UNCHECKED );
1046  use_torso_t_menu_ = menu_handler.insert( use_torso_menu_, "Arm and Torso", boost::bind( &InteractiveMarkerInterface::useTorsoCb,this, _1 ));
1047  menu_handler.setCheckState( use_torso_t_menu_, interactive_markers::MenuHandler::UNCHECKED );
1048  use_fullbody_menu_ = menu_handler.insert( use_torso_menu_, "Fullbody", boost::bind( &InteractiveMarkerInterface::useTorsoCb,this, _1 ));
1049  menu_handler.setCheckState( use_fullbody_menu_, interactive_markers::MenuHandler::CHECKED );
1050 
1051  }
1052 
1053 
1054 
1055  interactive_markers::MenuHandler::EntryHandle sub_menu_handle_im_size;
1056  sub_menu_handle_im_size = menu_handler.insert( "IMsize" );
1057  menu_handler.insert( sub_menu_handle_im_size, "Large", boost::bind( &InteractiveMarkerInterface::IMSizeLargeCb, this, _1));
1058  menu_handler.insert( sub_menu_handle_im_size, "Middle", boost::bind( &InteractiveMarkerInterface::IMSizeMiddleCb, this, _1));
1059  menu_handler.insert( sub_menu_handle_im_size, "Small", boost::bind( &InteractiveMarkerInterface::IMSizeSmallCb, this, _1));
1060 
1061  pnh_.param("publish_marker_menu", use_menu, false );
1062  if(use_menu){
1063  //menu_handler.insert("ManipulationMode", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MANIP_MODE));
1064  menu_handler.insert("Publish Marker",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::PUBLISH_MARKER));
1065 
1066  }
1067 
1068 
1069 
1070 
1071  //--------- menu_handler 1 ---------------
1072  menu_handler1.insert("ForceMode",boost::bind( &InteractiveMarkerInterface::changeForceModeCb1, this, _1));
1073 
1074  //--------- menu_handler 2 ---------------
1075  menu_handler2.insert("ForceMode",boost::bind( &InteractiveMarkerInterface::changeForceModeCb2, this, _1));
1076  menu_handler2.insert("Move",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MOVE));
1077 
1078 
1079  /* porting from PR2 marker control */
1080  /* head marker */
1081 
1082  //menu_head_.insert("Take Snapshot", boost::bind( &InteractiveMarkerInterface::snapshotCB, this ) );
1083 
1084 
1085 
1086  head_target_handle_ = menu_head_.insert( "Target Point",
1087  boost::bind( &InteractiveMarkerInterface::targetPointMenuCB, this, _1 ) );
1088  menu_head_.setCheckState(head_target_handle_, interactive_markers::MenuHandler::UNCHECKED);
1089 
1090  head_auto_look_handle_ = menu_head_.insert( "Look Automatically", boost::bind( &InteractiveMarkerInterface::lookAutomaticallyMenuCB,
1091  this, _1 ) );
1092  menu_head_.setCheckState(head_auto_look_handle_, interactive_markers::MenuHandler::CHECKED);
1093 
1094  menu_head_target_.insert( "Look At",
1095  boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1, jsk_interactive_marker::MarkerPose::HEAD_MARKER));
1096  //boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MOVE, jsk_interactive_marker::MarkerMenu::HEAD_MARKER));
1097 
1098 
1099  /*
1100  projector_handle_ = menu_head_.insert("Projector", boost::bind( &InteractiveMarkerInterface::projectorMenuCB,
1101  this, _1 ) );
1102  menu_head_.setCheckState(projector_handle_, MenuHandler::UNCHECKED);
1103  */
1104 
1105  /*
1106  menu_head_.insert( "Move Head To Center", boost::bind( &InteractiveMarkerInterface::centerHeadCB,
1107  this ) );
1108  */
1109 
1110 
1111  /* base move menu*/
1112  pnh_.param("use_base_marker", use_menu, false );
1113  control_state_.base_on_ = use_menu;
1114 
1115  menu_base_.insert("Base Move",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MOVE, jsk_interactive_marker::MarkerMenu::BASE_MARKER));
1116  menu_base_.insert("Reset Marker Position",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::RESET_COORDS, jsk_interactive_marker::MarkerMenu::BASE_MARKER));
1117 
1118  /*finger move menu*/
1119  pnh_.param("use_finger_marker", use_finger_marker_, false );
1120 
1121  menu_finger_r_.insert("Move Finger",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MOVE, jsk_interactive_marker::MarkerMenu::RFINGER_MARKER));
1122  menu_finger_r_.insert("Reset Marker",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::RESET_COORDS, jsk_interactive_marker::MarkerMenu::RFINGER_MARKER));
1123 
1124  menu_finger_l_.insert("Move Finger",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MOVE, jsk_interactive_marker::MarkerMenu::LFINGER_MARKER));
1125  menu_finger_l_.insert("Reset Marker",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::RESET_COORDS, jsk_interactive_marker::MarkerMenu::LFINGER_MARKER));
1126 
1127 }
1128 
1129 void InteractiveMarkerInterface::addHandMarker(visualization_msgs::InteractiveMarker &im,std::vector < UrdfProperty > urdf_vec){
1130  if(urdf_vec.size() > 0){
1131  for(int i=0; i<urdf_vec.size(); i++){
1132  UrdfProperty up = urdf_vec[i];
1133  if(up.model){
1134  KDL::Frame origin_frame;
1135  tf::poseMsgToKDL(up.pose, origin_frame);
1136 
1137  LinkConstSharedPtr hand_root_link;
1138  hand_root_link = up.model->getLink(up.root_link_name);
1139  if(!hand_root_link){
1140  hand_root_link = up.model->getRoot();
1141  }
1142  im_utils::addMeshLinksControl(im, hand_root_link, origin_frame, !up.use_original_color, up.color, up.scale);
1143  for(int j=0; j<im.controls.size(); j++){
1144  if(im.controls[j].interaction_mode == visualization_msgs::InteractiveMarkerControl::BUTTON){
1145  im.controls[j].interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D;
1146  im.controls[j].name = "center_sphere";
1147  }
1148  }
1149  }else{
1150  addSphereMarker(im, up.scale, up.color);
1151  }
1152  }
1153  }else{
1154  double center_marker_size = 0.2;
1155  //gray
1156  std_msgs::ColorRGBA color;
1157  color.r = color.g = color.b = 0.7;
1158  color.a = 0.5;
1159  addSphereMarker(im, center_marker_size, color);
1160  }
1161 }
1162 
1163 void InteractiveMarkerInterface::addSphereMarker(visualization_msgs::InteractiveMarker &im, double scale, std_msgs::ColorRGBA color){
1164  visualization_msgs::Marker sphereMarker;
1165  sphereMarker.type = visualization_msgs::Marker::SPHERE;
1166 
1167  sphereMarker.scale.x = scale;
1168  sphereMarker.scale.y = scale;
1169  sphereMarker.scale.z = scale;
1170 
1171  sphereMarker.color = color;
1172 
1173  visualization_msgs::InteractiveMarkerControl sphereControl;
1174  sphereControl.name = "center_sphere";
1175 
1176  sphereControl.markers.push_back(sphereMarker);
1177  sphereControl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D;
1178  im.controls.push_back(sphereControl);
1179 }
1180 
1181 
1182 void InteractiveMarkerInterface::makeCenterSphere(visualization_msgs::InteractiveMarker &mk, double mk_size){
1183  std::vector < UrdfProperty > null_urdf;
1184  if(control_state_.move_origin_state_ == ControlState::HAND_ORIGIN){
1185  if(control_state_.move_arm_ == ControlState::RARM){
1186  addHandMarker(mk, rhand_urdf_);
1187  }else if(control_state_.move_arm_ == ControlState::LARM){
1188  addHandMarker(mk, lhand_urdf_);
1189  }else{
1190  addHandMarker(mk, null_urdf);
1191  }
1192  }else{
1193  addHandMarker(mk, null_urdf);
1194  }
1195 
1196  //sphereControl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D;
1197  //mk.controls.push_back(sphereControl);
1198 }
1199 
1200 //im_mode
1201 //0:normal move 1:operationModel 2:operationalModelFirst
1202 void InteractiveMarkerInterface::changeMarkerMoveMode( std::string mk_name , int im_mode){
1203  switch(im_mode){
1204  case 0:
1205  changeMarkerMoveMode( mk_name, im_mode , 0.5);
1206  break;
1207  case 1:
1208  case 2:
1209  changeMarkerMoveMode( mk_name, im_mode , 0.3);
1210  break;
1211  default:
1212  changeMarkerMoveMode( mk_name, im_mode , 0.3);
1213  break;
1214  }
1215 }
1216 
1217 void InteractiveMarkerInterface::changeMarkerMoveMode( std::string mk_name , int im_mode, float mk_size){
1218  geometry_msgs::PoseStamped pose;
1219  pose.header.frame_id = base_frame;
1220  pose.pose.orientation.w = 1.0;
1221  changeMarkerMoveMode( mk_name, im_mode , mk_size, pose);
1222 }
1223 
1224 void InteractiveMarkerInterface::changeMarkerMoveMode( std::string mk_name , int im_mode, float mk_size, geometry_msgs::PoseStamped dist_pose){
1225  ROS_INFO("changeMarkerMoveMode marker:%s mode:%d\n",mk_name.c_str(),im_mode);
1226 
1227  control_state_.marker_pose_ = dist_pose;
1228 
1229  interactive_markers::MenuHandler reset_handler;
1230 
1231  geometry_msgs::PoseStamped pose;
1232 
1233  if ( target_frame != "" ) {
1234  /*
1235  tf::StampedTransform stf;
1236  geometry_msgs::TransformStamped mtf;
1237  tfl_.lookupTransform(target_frame, base_frame,
1238  ros::Time(0), stf);
1239  tf::transformStampedTFToMsg(stf, mtf);
1240  pose.pose.position.x = mtf.transform.translation.x;
1241  pose.pose.position.y = mtf.transform.translation.y;
1242  pose.pose.position.z = mtf.transform.translation.z;
1243  pose.pose.orientation = mtf.transform.rotation;
1244  pose.header = mtf.header;
1245  */
1246  }else{
1247  pose = dist_pose;
1248  }
1249 
1250  visualization_msgs::InteractiveMarker mk;
1251  //0:normal move 1:operationModel 2:operationalModelFirst
1252 
1253  switch(im_mode){
1254  case 0:
1255  pose.header.stamp = ros::Time(0);
1256 
1257  mk = make6DofControlMarker(mk_name.c_str(), pose, mk_size,
1258  true, false );
1259 
1260  if(use_center_sphere_){
1261  makeCenterSphere(mk, mk_size);
1262  }
1263 
1264  makeIMVisible(mk);
1265 
1266  server_->insert( mk );
1267  server_->setCallback( mk.name,
1268  boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
1269  menu_handler.apply(*server_,mk.name);
1270  server_->applyChanges();
1271  break;
1272  case 1:
1273  mk = im_helpers::make6DofMarker(mk_name.c_str(), pose, mk_size,
1274  true, false );
1275  mk.description = mk_name.c_str();
1276  makeIMVisible(mk);
1277  server_->insert( mk );
1278  server_->setCallback( mk.name,
1279  boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
1280  menu_handler1.apply(*server_,mk.name);
1281  server_->applyChanges();
1282  break;
1283 
1284  case 2:
1285  mk = im_helpers::make6DofMarker(mk_name.c_str(), pose, mk_size,
1286  true, false );
1287  mk.description = mk_name.c_str();
1288  makeIMVisible(mk);
1289 
1290  server_->insert( mk );
1291  server_->setCallback( mk.name,
1292  boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
1293  menu_handler2.apply(*server_,mk.name);
1294  server_->applyChanges();
1295  break;
1296  default:
1297  mk = im_helpers::make6DofMarker(mk_name.c_str(), pose, mk_size,
1298  true, false );
1299  mk.description = mk_name.c_str();
1300 
1301  server_->insert( mk );
1302  server_->setCallback( mk.name,
1303  boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
1304  server_->applyChanges();
1305  break;
1306  }
1307 
1308  std::list<visualization_msgs::InteractiveMarker>::iterator it = imlist.begin();
1309 
1310  while( it != imlist.end() )
1311  {
1312  if(it->name == mk_name.c_str()){
1313  imlist.erase(it);
1314  break;
1315  }
1316  it++;
1317  }
1318  imlist.push_back( mk );
1319 }
1320 
1321 void InteractiveMarkerInterface::changeMarkerOperationModelMode( std::string mk_name ){
1322  interactive_markers::MenuHandler reset_handler;
1323  menu_handler = reset_handler;
1324  geometry_msgs::PoseStamped pose;
1325  pose.header.frame_id = base_frame;
1326  /*
1327  if ( target_frame != "" ) {
1328  tf::StampedTransform stf;
1329  geometry_msgs::TransformStamped mtf;
1330  tfl_.lookupTransform(target_frame, base_frame,
1331  ros::Time(0), stf);
1332  tf::transformStampedTFToMsg(stf, mtf);
1333  pose.pose.position.x = mtf.transform.translation.x;
1334  pose.pose.position.y = mtf.transform.translation.y;
1335  pose.pose.position.z = mtf.transform.translation.z;
1336  pose.pose.orientation = mtf.transform.rotation;
1337  pose.header = mtf.header;
1338  }*/
1339 
1340  visualization_msgs::InteractiveMarker mk =
1341 
1342  im_helpers::make6DofMarker(mk_name.c_str(), pose, 0.5,
1343  true, false );
1344  mk.description = mk_name.c_str();
1345  menu_handler.insert("ForceMode",boost::bind( &InteractiveMarkerInterface::changeForceModeCb, this, _1));
1346 
1347  std::list<visualization_msgs::InteractiveMarker>::iterator it = imlist.begin(); // イテレータ
1348 
1349  while( it != imlist.end() ) // listの末尾まで
1350  {
1351  if(it->name == mk_name.c_str()){
1352  imlist.erase(it);
1353  break;
1354  }
1355  it++;
1356  }
1357  imlist.push_back( mk );
1358  server_->insert( mk );
1359 
1360  server_->setCallback( mk.name,
1361  boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
1362 
1363  menu_handler.apply(*server_,mk.name);
1364  server_->applyChanges();
1365 }
1366 
1367 
1368 //InteractiveMarkerInterface::InteractiveMarkerInterface () : nh_(), pnh_("~"), tfl_(nh_) {
1369 InteractiveMarkerInterface::InteractiveMarkerInterface () : nh_(), pnh_("~") {
1370  pnh_.param("marker_name", marker_name, std::string ( "100") );
1371  pnh_.param("server_name", server_name, std::string ("") );
1372  pnh_.param("base_frame", base_frame, std::string ("/base_link") );
1373  pnh_.param("move_base_frame", move_base_frame, std::string ("/base_link") );
1374  pnh_.param("target_frame", target_frame, std::string ("") );
1375  //pnh_.param("fix_marker", fix_marker, true);
1376 
1377  if ( server_name == "" ) {
1378  server_name = ros::this_node::getName();
1379  }
1380 
1381  pub_ = pnh_.advertise<jsk_interactive_marker::MarkerPose> ("pose", 1);
1382  pub_update_ = pnh_.advertise<geometry_msgs::PoseStamped> ("pose_update", 1);
1383  pub_move_ = pnh_.advertise<jsk_interactive_marker::MarkerMenu> ("marker_menu", 1);
1384 
1385  serv_set_ = pnh_.advertiseService("set_pose",
1386  &InteractiveMarkerInterface::set_cb, this);
1387  serv_markers_set_ = pnh_.advertiseService("set_markers",
1388  &InteractiveMarkerInterface::markers_set_cb, this);
1389  serv_markers_del_ = pnh_.advertiseService("del_markers",
1390  &InteractiveMarkerInterface::markers_del_cb, this);
1391  serv_reset_ = pnh_.advertiseService("reset_pose",
1392  &InteractiveMarkerInterface::reset_cb, this);
1393 
1394  sub_marker_pose_ = pnh_.subscribe<geometry_msgs::PoseStamped> ("move_marker", 1, boost::bind( &InteractiveMarkerInterface::move_marker_cb, this, _1));
1395  sub_marker_menu_ = pnh_.subscribe<jsk_interactive_marker::MarkerMenu> ("select_marker_menu", 1, boost::bind( &InteractiveMarkerInterface::marker_menu_cb, this, _1));
1396 
1397  sub_toggle_start_ik_ = pnh_.subscribe<std_msgs::Empty> ("toggle_start_ik", 1, boost::bind( &InteractiveMarkerInterface::toggleStartIKCb, this, _1));
1398 
1399  sub_toggle_ik_mode_ = pnh_.subscribe<std_msgs::Empty> ("toggle_ik_mode", 1, boost::bind( &InteractiveMarkerInterface::toggleIKModeCb, this, _1));
1400 
1401  ros::service::waitForService("set_dynamic_tf", -1);
1402  dynamic_tf_publisher_client_ = nh_.serviceClient<dynamic_tf_publisher::SetDynamicTF>("set_dynamic_tf", true);
1403 
1404  server_.reset( new interactive_markers::InteractiveMarkerServer(server_name));
1405 
1406  pnh_.param<std::string>("head_link_frame", head_link_frame_, "head_tilt_link");
1407  pnh_.param<std::string>("head_mesh", head_mesh_, "package://pr2_description/meshes/head_v0/head_tilt.dae");
1408 
1409  pnh_.param<std::string>("hand_type", hand_type_, "GENERIC");
1410 
1411  pnh_.param("use_head_marker", use_body_marker_, false );
1412  pnh_.param("use_center_sphere", use_center_sphere_, false );
1413 
1414  XmlRpc::XmlRpcValue v;
1415  pnh_.param("mesh_config", v, v);
1416  loadMeshes(v);
1417 
1418  head_goal_pose_.pose.position.x = 1.0;
1419  head_goal_pose_.pose.position.z = 1.0;
1420  head_goal_pose_.header.frame_id = base_frame;
1421 
1422  initHandler();
1423  if(use_body_marker_){
1424  initBodyMarkers();
1425  }
1426  initControlMarkers();
1427  changeMarkerMoveMode(marker_name.c_str(),0);
1428 }
1429 
1430 void InteractiveMarkerInterface::loadMeshes(XmlRpc::XmlRpcValue val){
1431  loadUrdfFromYaml(val, "r_hand", rhand_urdf_);
1432  loadUrdfFromYaml(val, "l_hand", lhand_urdf_);
1433 }
1434 
1435 void InteractiveMarkerInterface::loadUrdfFromYaml(XmlRpc::XmlRpcValue val, std::string name, std::vector<UrdfProperty>& mesh){
1436  if(val.hasMember(name)){
1437  for(int i=0; i< val[name].size(); i++){
1438  XmlRpc::XmlRpcValue nval = val[name][i];
1439  UrdfProperty up;
1440  //urdf file
1441  if(nval.hasMember("urdf_file")){
1442  std::string urdf_file = (std::string)nval["urdf_file"];
1443  std::cerr << "load urdf file: " << urdf_file << std::endl;
1444  up.model = im_utils::getModelInterface(urdf_file);
1445  }else if(nval.hasMember("urdf_param")){
1446  std::string urdf_param = (std::string)nval["urdf_param"];
1447  std::string urdf_model;
1448  nh_.getParam(urdf_param, urdf_model);
1449  up.model = parseURDF(urdf_model);
1450  }
1451 
1452  if(nval.hasMember("root_link")){
1453  std::string root_link_name = (std::string)nval["root_link"];
1454  std::cerr << "root link name: " << root_link_name << std::endl;
1455  up.root_link_name = root_link_name;
1456  }else{
1457  up.root_link_name = "";
1458  }
1459 
1460  up.pose.orientation.w = 1.0;
1461  //pose
1462  if(nval.hasMember("pose")){
1463  XmlRpc::XmlRpcValue pose = nval["pose"];
1464  if(pose.hasMember("position")){
1465  XmlRpc::XmlRpcValue position = pose["position"];
1466  up.pose.position.x = (double)position["x"];
1467  up.pose.position.y = (double)position["y"];
1468  up.pose.position.z = (double)position["z"];
1469  }
1470 
1471  if(pose.hasMember("orientation")){
1472  XmlRpc::XmlRpcValue orient = pose["orientation"];
1473  up.pose.orientation.x = (double)orient["x"];
1474  up.pose.orientation.y = (double)orient["y"];
1475  up.pose.orientation.z = (double)orient["z"];
1476  up.pose.orientation.w = (double)orient["w"];
1477  }
1478  }
1479 
1480  if(nval.hasMember("color")){
1481  XmlRpc::XmlRpcValue color = nval["color"];
1482  up.color.r = (double)color["r"];
1483  up.color.g = (double)color["g"];
1484  up.color.b = (double)color["b"];
1485  up.color.a = (double)color["a"];
1486  }else{
1487  up.color.r = 1.0;
1488  up.color.g = 1.0;
1489  up.color.b = 0.0;
1490  up.color.a = 0.7;
1491  }
1492  if(nval.hasMember("scale")){
1493  up.scale = (double)nval["scale"];
1494  }else{
1495  up.scale = 1.05; //make bigger a bit
1496  }
1497  mesh.push_back(up);
1498  }
1499  }
1500 }
1501 
1502 
1503 bool InteractiveMarkerInterface::markers_set_cb ( jsk_interactive_marker::MarkerSetPose::Request &req,
1504  jsk_interactive_marker::MarkerSetPose::Response &res ) {
1505  bool setalready = false;
1506 
1507  std::list<visualization_msgs::InteractiveMarker>::iterator it = imlist.begin();
1508  while( it != imlist.end() ) // listの末尾まで
1509  {
1510  if( it->name == req.marker_name){
1511  setalready = true;
1512  break;
1513  }
1514  it++;
1515  }
1516 
1517  if(setalready){
1518  server_->setPose(req.marker_name, req.pose.pose, req.pose.header);
1519  server_->applyChanges();
1520  return true;
1521  }else{
1522  /*
1523  if(req.marker_name==0){
1524  changeMarkerMoveMode(name,2);
1525  }else{
1526  changeMarkerMoveMode(name,1);
1527  }
1528  server_->setPose(name, req.pose.pose, req.pose.header);
1529  // menu_handler.apply(*server_,mk.name)Z
1530  server_->applyChanges();
1531  return true;
1532  */
1533  return true;
1534  }
1535 }
1536 
1537 bool InteractiveMarkerInterface::markers_del_cb ( jsk_interactive_marker::MarkerSetPose::Request &req,
1538  jsk_interactive_marker::MarkerSetPose::Response &res ) {
1539 
1540  server_->erase(req.marker_name);
1541  server_->applyChanges();
1542  std::list<visualization_msgs::InteractiveMarker>::iterator it = imlist.begin();
1543  while( it != imlist.end() ) // listの末尾まで
1544  {
1545  if( it->name == req.marker_name){
1546  imlist.erase(it);
1547  break;
1548  }
1549  it++;
1550  }
1551 
1552  return true;
1553 
1554 }
1555 
1556 void InteractiveMarkerInterface::move_marker_cb ( const geometry_msgs::PoseStampedConstPtr &msg){
1557  pub_marker_tf(msg->header, msg->pose);
1558 
1559  pub_marker_pose( msg->header, msg->pose, marker_name, jsk_interactive_marker::MarkerPose::GENERAL);
1560 
1561  server_->setPose(marker_name, msg->pose, msg->header);
1562  server_->applyChanges();
1563 }
1564 
1565 
1566 bool InteractiveMarkerInterface::set_cb ( jsk_interactive_marker::MarkerSetPose::Request &req,
1567  jsk_interactive_marker::MarkerSetPose::Response &res ) {
1568 
1569  if ( req.markers.size() > 0 ) {
1570  visualization_msgs::InteractiveMarker mk;
1571  if ( server_->get(req.marker_name, mk) ) {
1572  visualization_msgs::InteractiveMarkerControl mkc;
1573  mkc.name = "additional_marker";
1574  mkc.always_visible = true;
1575  mkc.markers = req.markers;
1576  // delete added marker
1577  for ( std::vector<visualization_msgs::InteractiveMarkerControl>::iterator it
1578  = mk.controls.begin();
1579  it != mk.controls.end(); it++ ) {
1580  if ( it->name == mkc.name ){
1581  mk.controls.erase( it );
1582  break;
1583  }
1584  }
1585  mk.controls.push_back( mkc );
1586  }
1587  }
1588  std::string mName = req.marker_name;
1589  if(mName == ""){
1590  mName = marker_name;
1591  }
1592  pub_marker_tf(req.pose.header, req.pose.pose);
1593 
1594  server_->setPose(mName, req.pose.pose, req.pose.header);
1595  server_->applyChanges();
1596  pub_update_.publish(req.pose);
1597  return true;
1598 }
1599 
1600 bool InteractiveMarkerInterface::reset_cb ( jsk_interactive_marker::SetPose::Request &req,
1601  jsk_interactive_marker::SetPose::Response &res ) {
1602  geometry_msgs::PoseStamped pose;
1603  pose.header.frame_id = base_frame;
1604  if ( target_frame != "" ) {
1605  /*
1606  tf::StampedTransform stf;
1607  geometry_msgs::TransformStamped mtf;
1608  tfl_.lookupTransform(base_frame, target_frame,
1609  ros::Time(0), stf);
1610  tf::transformStampedTFToMsg(stf, mtf);
1611  pose.pose.position.x = mtf.transform.translation.x;
1612  pose.pose.position.y = mtf.transform.translation.y;
1613  pose.pose.position.z = mtf.transform.translation.z;
1614  pose.pose.orientation = mtf.transform.rotation;
1615  // pose.header = mtf.header;
1616  // pose.header.stamp = ros::Time::Now();
1617  // pose.header.frame_id = target_frame;
1618  server_->setPose(marker_name, pose.pose, pose.header);
1619  */
1620  } else {
1621  server_->setPose(marker_name, pose.pose);
1622  }
1623  server_->applyChanges();
1624  return true;
1625 }
1626 
1627 
1628 void InteractiveMarkerInterface::makeIMVisible(visualization_msgs::InteractiveMarker &im){
1629  for(int i=0; i<im.controls.size(); i++){
1630  im.controls[i].always_visible = true;
1631  }
1632 }
1633 
1634 int main(int argc, char** argv)
1635 {
1636  ros::init(argc, argv, "jsk_marker_interface");
1637  InteractiveMarkerInterface imi;
1638  ros::spin();
1639 
1640  return 0;
1641 }
ros::NodeHandle::serviceClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
InteractiveMarkerInterface::move_marker_cb
void move_marker_cb(const geometry_msgs::PoseStampedConstPtr &msg)
Definition: interactive_marker_interface.cpp:1556
im_utils::makeFingerControlMarker
visualization_msgs::InteractiveMarker makeFingerControlMarker(const char *name, geometry_msgs::PoseStamped ps)
Definition: interactive_marker_utils.cpp:299
interactive_markers::MenuHandler::UNCHECKED
UNCHECKED
transform_listener.h
InteractiveMarkerInterface::changeMarkerMoveMode
void changeMarkerMoveMode(std::string mk_name, int im_mode)
Definition: interactive_marker_interface.cpp:1202
InteractiveMarkerInterface::markers_del_cb
bool markers_del_cb(jsk_interactive_marker::MarkerSetPose::Request &req, jsk_interactive_marker::MarkerSetPose::Response &res)
Definition: interactive_marker_interface.cpp:1537
main
int main(int argc, char **argv)
Definition: interactive_marker_interface.cpp:1634
interactive_markers::MenuHandler::EntryHandle
uint32_t EntryHandle
InteractiveMarkerInterface::changeMarkerForceMode
void changeMarkerForceMode(std::string mk_name, int im_mode)
Definition: interactive_marker_interface.cpp:578
InteractiveMarkerInterface::changeForceModeCb
void changeForceModeCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:230
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
InteractiveMarkerInterface::markers_set_cb
bool markers_set_cb(jsk_interactive_marker::MarkerSetPose::Request &req, jsk_interactive_marker::MarkerSetPose::Response &res)
Definition: interactive_marker_interface.cpp:1503
InteractiveMarkerInterface::lookAutomaticallyMenuCB
void lookAutomaticallyMenuCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:267
im_utils::getModelInterface
ModelInterfaceSharedPtr getModelInterface(std::string model_file)
Definition: interactive_marker_utils.cpp:258
InteractiveMarkerInterface::head_goal_pose_
geometry_msgs::PoseStamped head_goal_pose_
Definition: interactive_marker_interface.h:278
InteractiveMarkerInterface::rhand_urdf_
std::vector< UrdfProperty > rhand_urdf_
Definition: interactive_marker_interface.h:285
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
InteractiveMarkerInterface::usingIKCb
void usingIKCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:335
XmlRpc::XmlRpcValue::size
int size() const
target_frame
std::string target_frame
interactive_marker_utils.h
InteractiveMarkerInterface::changeMoveModeCb1
void changeMoveModeCb1(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:220
im_helpers::makeHeadGoalMarker
visualization_msgs::InteractiveMarker makeHeadGoalMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale)
Definition: interactive_marker_helpers.cpp:260
InteractiveMarkerInterface::updateFinger
void updateFinger(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, std::string hand)
Definition: interactive_marker_interface.cpp:546
InteractiveMarkerInterface::modeCb
void modeCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:375
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
InteractiveMarkerInterface::IMSizeLargeCb
void IMSizeLargeCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:194
tf_kdl.h
InteractiveMarkerInterface::changeForceModeCb2
void changeForceModeCb2(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:242
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
im_helpers::make6DofMarker
visualization_msgs::InteractiveMarker make6DofMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed, bool view_facing)
Definition: interactive_marker_helpers.cpp:420
transform_broadcaster.h
InteractiveMarkerInterface::makeCenterSphere
void makeCenterSphere(visualization_msgs::InteractiveMarker &mk, double mk_size)
Definition: interactive_marker_interface.cpp:1182
InteractiveMarkerInterface::pub_marker_pose
void pub_marker_pose(std_msgs::Header header, geometry_msgs::Pose pose, std::string name, int type)
Definition: interactive_marker_interface.cpp:154
InteractiveMarkerInterface::pub_marker_menuCb
void pub_marker_menuCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int menu)
Definition: interactive_marker_interface.cpp:166
InteractiveMarkerInterface::addHandMarker
void addHandMarker(visualization_msgs::InteractiveMarker &im, std::vector< UrdfProperty > urdf_vec)
Definition: interactive_marker_interface.cpp:1129
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
interactive_markers::MenuHandler
interactive_markers::InteractiveMarkerServer
im_utils
Definition: interactive_marker_utils.h:47
pose
pose
InteractiveMarkerInterface::toggleStartIKCb
void toggleStartIKCb(const std_msgs::EmptyConstPtr &msg)
Definition: interactive_marker_interface.cpp:351
InteractiveMarkerInterface::setOriginCb
void setOriginCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, bool origin_hand)
Definition: interactive_marker_interface.cpp:425
InteractiveMarkerInterface::makeBaseMarker
visualization_msgs::InteractiveMarker makeBaseMarker(const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed)
Definition: interactive_marker_interface.cpp:839
interactive_marker_interface.h
InteractiveMarkerInterface::pub_marker_tf
void pub_marker_tf(std_msgs::Header header, geometry_msgs::Pose pose)
Definition: interactive_marker_interface.cpp:138
InteractiveMarkerInterface::IMSizeSmallCb
void IMSizeSmallCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:208
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
boost::shared_ptr
InteractiveMarkerInterface::makeIMVisible
void makeIMVisible(visualization_msgs::InteractiveMarker &im)
Definition: interactive_marker_interface.cpp:1628
interactive_marker_helpers.h
InteractiveMarkerInterface::changeMoveArm
void changeMoveArm(std::string m_name, int menu)
Definition: interactive_marker_interface.cpp:399
InteractiveMarkerInterface::proc_feedback
void proc_feedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:112
InteractiveMarkerInterface::loadUrdfFromYaml
void loadUrdfFromYaml(XmlRpc::XmlRpcValue val, std::string name, std::vector< UrdfProperty > &mesh)
Definition: interactive_marker_interface.cpp:1435
im_utils::makeSandiaHandInteractiveMarker
visualization_msgs::InteractiveMarker makeSandiaHandInteractiveMarker(geometry_msgs::PoseStamped ps, std::string hand, int finger, int link)
Definition: interactive_marker_utils.cpp:383
link_name
link_name
ROS_INFO
#define ROS_INFO(...)
InteractiveMarkerInterface::changeMoveModeCb
void changeMoveModeCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:215
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
InteractiveMarkerInterface::loadMeshes
void loadMeshes(XmlRpc::XmlRpcValue val)
Definition: interactive_marker_interface.cpp:1430
InteractiveMarkerInterface::marker_menu_cb
void marker_menu_cb(const jsk_interactive_marker::MarkerMenuConstPtr &msg)
Definition: interactive_marker_interface.cpp:488
tf::poseMsgToKDL
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
menu_handler.h
InteractiveMarkerInterface::reset_cb
bool reset_cb(jsk_interactive_marker::SetPose::Request &req, jsk_interactive_marker::SetPose::Response &res)
Definition: interactive_marker_interface.cpp:1600
InteractiveMarkerInterface::ConstraintCb
void ConstraintCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:287
InteractiveMarkerInterface::lhand_urdf_
std::vector< UrdfProperty > lhand_urdf_
Definition: interactive_marker_interface.h:285
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
InteractiveMarkerInterface::toggleIKModeCb
void toggleIKModeCb(const std_msgs::EmptyConstPtr &msg)
Definition: interactive_marker_interface.cpp:466
InteractiveMarkerInterface::make6DofControlMarker
visualization_msgs::InteractiveMarker make6DofControlMarker(std::string name, geometry_msgs::PoseStamped &stamped, float scale, bool fixed_position, bool fixed_rotation)
Definition: interactive_marker_interface.cpp:30
InteractiveMarkerInterface::updateHeadGoal
void updateHeadGoal(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:516
InteractiveMarkerInterface::imlist
std::list< visualization_msgs::InteractiveMarker > imlist
Definition: interactive_marker_interface.h:223
InteractiveMarkerInterface::set_cb
bool set_cb(jsk_interactive_marker::MarkerSetPose::Request &req, jsk_interactive_marker::MarkerSetPose::Response &res)
Definition: interactive_marker_interface.cpp:1566
XmlRpc::XmlRpcValue::hasMember
bool hasMember(const std::string &name) const
im_utils::addMeshLinksControl
void addMeshLinksControl(visualization_msgs::InteractiveMarker &im, LinkConstSharedPtr link, KDL::Frame previous_frame, bool use_color, std_msgs::ColorRGBA color, double scale)
Definition: interactive_marker_utils.cpp:146
im_helpers::makeMeshMarker
visualization_msgs::InteractiveMarker makeMeshMarker(const std::string &name, const std::string &mesh_resource, const geometry_msgs::PoseStamped &stamped, float scale)
Definition: interactive_marker_helpers.cpp:308
mesh_file
mesh_file
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
interactive_markers::MenuHandler::CHECKED
CHECKED
InteractiveMarkerInterface::updateBase
void updateBase(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:536
InteractiveMarkerInterface::pub_marker_menu
void pub_marker_menu(std::string marker, int menu, int type)
Definition: interactive_marker_interface.cpp:182
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
InteractiveMarkerInterface::addSphereMarker
void addSphereMarker(visualization_msgs::InteractiveMarker &im, double scale, std_msgs::ColorRGBA color)
Definition: interactive_marker_interface.cpp:1163
v
GLfloat v[8][3]
ros::Time::now
static Time now()
InteractiveMarkerInterface::server_
std::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
Definition: interactive_marker_interface.h:157
interactive_marker_server.h
InteractiveMarkerInterface::changeMarkerOperationModelMode
void changeMarkerOperationModelMode(std::string mk_name)
Definition: interactive_marker_interface.cpp:1321
interactive_markers::MenuHandler::CheckState
CheckState
InteractiveMarkerInterface::useTorsoCb
void useTorsoCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:312
InteractiveMarkerInterface::changeForceModeCb1
void changeForceModeCb1(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:236
dummy_camera.now
now
Definition: dummy_camera.py:16
InteractiveMarkerInterface::changeMoveModeCb2
void changeMoveModeCb2(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:225
InteractiveMarkerInterface::IMSizeMiddleCb
void IMSizeMiddleCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:201
InteractiveMarkerInterface::ikmodeCb
void ikmodeCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:442
ros::service::waitForService
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)
InteractiveMarkerInterface::targetPointMenuCB
void targetPointMenuCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_marker_interface.cpp:248

jsk_interactive_marker
Author(s): furuta
autogenerated on Sat Mar 20 2021 03:03:33