interactive_marker_interface.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <tf/tf.h>
00004 #include <tf/transform_listener.h>
00005 #include <tf/transform_broadcaster.h>
00006 
00007 #include <interactive_markers/interactive_marker_server.h>
00008 #include <jsk_interactive_marker/interactive_marker_helpers.h>
00009 
00010 #include <interactive_markers/menu_handler.h>
00011 #include <jsk_interactive_marker/SetPose.h>
00012 #include <jsk_interactive_marker/MarkerSetPose.h>
00013 
00014 #include <math.h>
00015 #include <jsk_interactive_marker/MarkerMenu.h>
00016 #include <jsk_interactive_marker/MarkerPose.h>
00017 
00018 #include <std_msgs/Int8.h>
00019 
00020 #include <jsk_interactive_marker/interactive_marker_interface.h>
00021 #include <jsk_interactive_marker/interactive_marker_utils.h>
00022 
00023 #include <dynamic_tf_publisher/SetDynamicTF.h>
00024 
00025 #include <kdl/frames_io.hpp>
00026 #include <tf_conversions/tf_kdl.h>
00027 
00028 using namespace im_utils;
00029 
00030 visualization_msgs::InteractiveMarker InteractiveMarkerInterface::make6DofControlMarker( std::string name, geometry_msgs::PoseStamped &stamped, float scale, bool fixed_position, bool fixed_rotation){
00031   
00032   visualization_msgs::InteractiveMarker int_marker;
00033   int_marker.header =  stamped.header;
00034   int_marker.name = name;
00035   int_marker.scale = scale;
00036   int_marker.pose = stamped.pose;
00037 
00038   visualization_msgs::InteractiveMarkerControl control;
00039     
00040   //x axis
00041   if(fixed_rotation){
00042     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00043   }else{
00044     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
00045   }
00046 
00047   control.orientation.w = 1;
00048   control.orientation.x = 1;
00049   control.orientation.y = 0;
00050   control.orientation.z = 0;
00051   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00052   int_marker.controls.push_back(control);
00053 
00054   if(fixed_position){
00055     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00056   }else{
00057     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
00058   }
00059     
00060   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00061   int_marker.controls.push_back(control);
00062     
00063 
00064   //y axis
00065   if(fixed_rotation){
00066     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00067   }else{
00068     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
00069   }
00070   control.orientation.w = 1;
00071   control.orientation.x = 0;
00072   control.orientation.y = 1;
00073   control.orientation.z = 0;
00074   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00075   int_marker.controls.push_back(control);
00076 
00077   if(fixed_position){
00078     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00079   }else{
00080     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
00081   }
00082 
00083   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00084   int_marker.controls.push_back(control);
00085     
00086   //z axis
00087   if(fixed_rotation){
00088     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00089   }else{
00090     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
00091   }
00092 
00093   control.orientation.w = 1;
00094   control.orientation.x = 0;
00095   control.orientation.y = 0;
00096   control.orientation.z = 1;
00097   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00098   int_marker.controls.push_back(control);
00099   if(fixed_position){
00100     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00101   }else{
00102     control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
00103   }
00104   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00105   int_marker.controls.push_back(control);
00106     
00107   return int_marker;
00108 }
00109 
00110 
00111 
00112 void InteractiveMarkerInterface::proc_feedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) {
00113   if(feedback->control_name.find("center_sphere") != std::string::npos){
00114     proc_feedback(feedback, jsk_interactive_marker::MarkerPose::SPHERE_MARKER);
00115   }else{
00116     proc_feedback(feedback, jsk_interactive_marker::MarkerPose::GENERAL);
00117   }
00118   
00119 }
00120 
00121 void InteractiveMarkerInterface::proc_feedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int type ) {
00122 
00123   jsk_interactive_marker::MarkerPose mp;
00124   mp.pose.header = feedback->header;
00125   mp.pose.pose = feedback->pose;
00126   mp.marker_name = feedback->marker_name;
00127   mp.type = type;
00128   pub_.publish( mp );
00129 
00130   //update Marker Pose Status
00131   control_state_.marker_pose_.pose = feedback->pose;
00132   control_state_.marker_pose_.header = feedback->header;
00133 
00134   pub_marker_tf(feedback->header, feedback->pose);
00135 
00136 }
00137 
00138 void InteractiveMarkerInterface::pub_marker_tf ( std_msgs::Header header, geometry_msgs::Pose pose){
00139   geometry_msgs::Transform tf;
00140   tf.translation.x = pose.position.x;
00141   tf.translation.y = pose.position.y;
00142   tf.translation.z = pose.position.z;
00143   tf.rotation = pose.orientation;
00144   
00145   dynamic_tf_publisher::SetDynamicTF SetTf;
00146   SetTf.request.freq = 10;
00147   SetTf.request.cur_tf.header.stamp = ros::Time::now();
00148   SetTf.request.cur_tf.header.frame_id = header.frame_id;
00149   SetTf.request.cur_tf.child_frame_id = "/moving_marker";
00150   SetTf.request.cur_tf.transform = tf;
00151   dynamic_tf_publisher_client_.call(SetTf);
00152 }
00153 
00154 void InteractiveMarkerInterface::pub_marker_pose ( std_msgs::Header header, geometry_msgs::Pose pose, std::string name, int type ) {
00155   jsk_interactive_marker::MarkerPose mp;
00156   mp.pose.header = header;
00157   mp.pose.pose = pose;
00158   mp.marker_name = name;
00159   mp.type = type;
00160   pub_.publish( mp );
00161 }
00162 
00163 
00164 
00165 
00166 void InteractiveMarkerInterface::pub_marker_menuCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int menu){
00167   jsk_interactive_marker::MarkerMenu m;
00168   m.marker_name = feedback->marker_name;
00169   m.menu=menu;
00170   pub_move_.publish(m);
00171 }
00172 
00173 void InteractiveMarkerInterface::pub_marker_menuCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int menu, int type){
00174   jsk_interactive_marker::MarkerMenu m;
00175   m.marker_name = feedback->marker_name;
00176   m.menu = menu;
00177   m.type = type;
00178   pub_move_.publish(m);
00179 }
00180 
00181 
00182 void InteractiveMarkerInterface::pub_marker_menu(std::string marker, int menu, int type){
00183   jsk_interactive_marker::MarkerMenu m;
00184   m.marker_name = marker;
00185   m.menu=menu;
00186   m.type = type;
00187   pub_move_.publish(m);
00188 }
00189 
00190 void InteractiveMarkerInterface::pub_marker_menu(std::string marker, int menu){
00191   pub_marker_menu(marker, menu, 0);
00192 }
00193 
00194 void InteractiveMarkerInterface::IMSizeLargeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00195   geometry_msgs::PoseStamped pose;
00196   pose.header = feedback->header;
00197   pose.pose = feedback->pose;
00198   changeMarkerMoveMode(feedback->marker_name, 0, 0.5, pose);
00199 }
00200 
00201 void InteractiveMarkerInterface::IMSizeMiddleCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00202   geometry_msgs::PoseStamped pose;
00203   pose.header = feedback->header;
00204   pose.pose = feedback->pose;
00205   changeMarkerMoveMode(feedback->marker_name, 0, 0.3, pose);
00206 }
00207 
00208 void InteractiveMarkerInterface::IMSizeSmallCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00209   geometry_msgs::PoseStamped pose;
00210   pose.header = feedback->header;
00211   pose.pose = feedback->pose;
00212   changeMarkerMoveMode(feedback->marker_name, 0, 0.1, pose);
00213 }
00214 
00215 void InteractiveMarkerInterface::changeMoveModeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00216   changeMarkerMoveMode(feedback->marker_name,0);
00217   pub_marker_menu(feedback->marker_name,13);
00218 
00219 }
00220 void InteractiveMarkerInterface::changeMoveModeCb1( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00221   changeMarkerMoveMode(feedback->marker_name,1);
00222   pub_marker_menu(feedback->marker_name,13);
00223 
00224 }
00225 void InteractiveMarkerInterface::changeMoveModeCb2( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00226   changeMarkerMoveMode(feedback->marker_name,2);
00227   pub_marker_menu(feedback->marker_name,13);
00228 }
00229 
00230 void InteractiveMarkerInterface::changeForceModeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00231   ROS_INFO("%s changeForceMode",feedback->marker_name.c_str());
00232   changeMarkerForceMode(feedback->marker_name,0);
00233   pub_marker_menu(feedback->marker_name,12);
00234 
00235 }
00236 void InteractiveMarkerInterface::changeForceModeCb1( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00237   ROS_INFO("%s changeForceMode1",feedback->marker_name.c_str());
00238   changeMarkerForceMode(feedback->marker_name,1);
00239   pub_marker_menu(feedback->marker_name,12);
00240 
00241 }
00242 void InteractiveMarkerInterface::changeForceModeCb2( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00243   ROS_INFO("%s changeForceMode2",feedback->marker_name.c_str());
00244   changeMarkerForceMode(feedback->marker_name,2);
00245   pub_marker_menu(feedback->marker_name,12);
00246 }
00247 
00248 void InteractiveMarkerInterface::targetPointMenuCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00249   ROS_INFO("targetPointMenu callback");
00250 
00251   control_state_.head_on_ ^= true;
00252   control_state_.init_head_goal_ = true;
00253   
00254   if(control_state_.head_on_){
00255     menu_head_.setCheckState(head_target_handle_, interactive_markers::MenuHandler::CHECKED);
00256     control_state_.look_auto_on_ = false;
00257     menu_head_.setCheckState(head_auto_look_handle_, interactive_markers::MenuHandler::UNCHECKED);
00258     pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::HEAD_TARGET_POINT, jsk_interactive_marker::MarkerMenu::HEAD_MARKER);
00259   }else{
00260     menu_head_.setCheckState(head_target_handle_, interactive_markers::MenuHandler::UNCHECKED);
00261   }
00262   menu_head_.reApply(*server_);
00263 
00264   initControlMarkers();
00265 }
00266 
00267 void InteractiveMarkerInterface::lookAutomaticallyMenuCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00268   ROS_INFO("targetPointMenu callback");
00269 
00270   control_state_.head_on_ = false;
00271   control_state_.look_auto_on_ ^= true;
00272   control_state_.init_head_goal_ = true;
00273 
00274   if(control_state_.look_auto_on_){
00275     menu_head_.setCheckState(head_auto_look_handle_, interactive_markers::MenuHandler::CHECKED);
00276     menu_head_.setCheckState(head_target_handle_, interactive_markers::MenuHandler::UNCHECKED);
00277     pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::PUBLISH_MARKER, jsk_interactive_marker::MarkerMenu::HEAD_MARKER);
00278   }else{
00279     menu_head_.setCheckState(head_auto_look_handle_, interactive_markers::MenuHandler::UNCHECKED);
00280     pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::HEAD_TARGET_POINT, jsk_interactive_marker::MarkerMenu::HEAD_MARKER);
00281   }
00282   menu_head_.reApply(*server_);
00283   initControlMarkers();
00284 }
00285 
00286 
00287 void InteractiveMarkerInterface::ConstraintCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00288 {
00289   menu_handler.setCheckState( h_mode_last2, interactive_markers::MenuHandler::UNCHECKED );
00290   h_mode_last2 = feedback->menu_entry_id;
00291   menu_handler.setCheckState( h_mode_last2, interactive_markers::MenuHandler::CHECKED );
00292 
00293   switch(h_mode_last2-h_mode_constrained){
00294   case 0:
00295     pub_marker_menu(feedback->marker_name,jsk_interactive_marker::MarkerMenu::MOVE_CONSTRAINT_T);
00296     ROS_INFO("send 23");
00297     break;
00298   case 1:
00299     pub_marker_menu(feedback->marker_name,jsk_interactive_marker::MarkerMenu::MOVE_CONSTRAINT_NIL);
00300     ROS_INFO("send 24");
00301     break;
00302   default:
00303     ROS_INFO("Switching Arm Error");
00304     break;
00305   }
00306 
00307   menu_handler.reApply( *server_ );
00308   server_->applyChanges();
00309 
00310 }
00311 
00312 void InteractiveMarkerInterface::useTorsoCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00313 {
00314   if(feedback->menu_entry_id == use_torso_t_menu_){
00315     menu_handler.setCheckState( use_torso_t_menu_ , interactive_markers::MenuHandler::CHECKED );
00316     menu_handler.setCheckState( use_torso_nil_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00317     menu_handler.setCheckState( use_fullbody_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00318     pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::USE_TORSO_T);
00319   }else if(feedback->menu_entry_id == use_torso_nil_menu_){
00320     menu_handler.setCheckState( use_torso_t_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00321     menu_handler.setCheckState( use_torso_nil_menu_ , interactive_markers::MenuHandler::CHECKED );
00322     menu_handler.setCheckState( use_fullbody_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00323     pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::USE_TORSO_NIL);
00324   }else{
00325     menu_handler.setCheckState( use_torso_t_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00326     menu_handler.setCheckState( use_torso_nil_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00327     menu_handler.setCheckState( use_fullbody_menu_ , interactive_markers::MenuHandler::CHECKED );
00328     pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::USE_FULLBODY);
00329   }
00330   menu_handler.reApply( *server_ );
00331   server_->applyChanges();
00332 
00333 }
00334 
00335 void InteractiveMarkerInterface::usingIKCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00336 {
00337   if(feedback->menu_entry_id == start_ik_menu_){
00338     menu_handler.setCheckState( start_ik_menu_ , interactive_markers::MenuHandler::CHECKED );
00339     menu_handler.setCheckState( stop_ik_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00340     pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::PLAN);
00341   }else if(feedback->menu_entry_id == stop_ik_menu_){
00342     menu_handler.setCheckState( start_ik_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00343     menu_handler.setCheckState( stop_ik_menu_ , interactive_markers::MenuHandler::CHECKED );
00344     pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::CANCEL_PLAN);
00345   }
00346   menu_handler.reApply( *server_ );
00347   server_->applyChanges();
00348 
00349 }
00350 
00351 void InteractiveMarkerInterface::toggleStartIKCb( const std_msgs::EmptyConstPtr &msg)
00352 {
00353   interactive_markers::MenuHandler::CheckState check_state;
00354   if(menu_handler.getCheckState( start_ik_menu_ , check_state)){
00355 
00356     if(check_state == interactive_markers::MenuHandler::CHECKED){
00357       //stop ik
00358       menu_handler.setCheckState( start_ik_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00359       menu_handler.setCheckState( stop_ik_menu_ , interactive_markers::MenuHandler::CHECKED );
00360       pub_marker_menu("", jsk_interactive_marker::MarkerMenu::CANCEL_PLAN);
00361 
00362     }else{
00363       //start ik
00364       menu_handler.setCheckState( start_ik_menu_ , interactive_markers::MenuHandler::CHECKED );
00365       menu_handler.setCheckState( stop_ik_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00366       pub_marker_menu("" , jsk_interactive_marker::MarkerMenu::PLAN);
00367     }
00368 
00369     menu_handler.reApply( *server_ );
00370     server_->applyChanges();
00371   }
00372 }
00373 
00374 
00375 void InteractiveMarkerInterface::modeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00376 {
00377   menu_handler.setCheckState( h_mode_last, interactive_markers::MenuHandler::UNCHECKED );
00378   h_mode_last = feedback->menu_entry_id;
00379   menu_handler.setCheckState( h_mode_last, interactive_markers::MenuHandler::CHECKED );
00380 
00381   switch(h_mode_last - h_mode_rightarm){
00382   case 0:
00383     changeMoveArm( feedback->marker_name, jsk_interactive_marker::MarkerMenu::SET_MOVE_RARM);
00384     break;
00385   case 1:
00386     changeMoveArm( feedback->marker_name, jsk_interactive_marker::MarkerMenu::SET_MOVE_LARM);
00387     break;
00388   case 2:
00389     changeMoveArm( feedback->marker_name, jsk_interactive_marker::MarkerMenu::SET_MOVE_ARMS);
00390     break;
00391   default:
00392     ROS_INFO("Switching Arm Error");
00393     break;
00394   }
00395   menu_handler.reApply( *server_ );
00396   server_->applyChanges();
00397 }
00398 
00399 void InteractiveMarkerInterface::changeMoveArm( std::string m_name, int menu ){
00400   switch(menu){
00401   case jsk_interactive_marker::MarkerMenu::SET_MOVE_RARM:
00402     pub_marker_menu(m_name,jsk_interactive_marker::MarkerMenu::SET_MOVE_RARM);
00403     control_state_.move_arm_ = ControlState::RARM;
00404     ROS_INFO("move Rarm");
00405     changeMarkerMoveMode( marker_name.c_str(), 0, 0.5, control_state_.marker_pose_);
00406     break;
00407   case jsk_interactive_marker::MarkerMenu::SET_MOVE_LARM:
00408     pub_marker_menu(m_name,jsk_interactive_marker::MarkerMenu::SET_MOVE_LARM);
00409     control_state_.move_arm_ = ControlState::LARM;
00410     ROS_INFO("move Larm");
00411     changeMarkerMoveMode( marker_name.c_str(), 0, 0.5, control_state_.marker_pose_);
00412     break;
00413   case jsk_interactive_marker::MarkerMenu::SET_MOVE_ARMS:
00414     pub_marker_menu(m_name,jsk_interactive_marker::MarkerMenu::SET_MOVE_ARMS);
00415     control_state_.move_arm_ = ControlState::ARMS;
00416     ROS_INFO("move Arms");
00417     changeMarkerMoveMode( marker_name.c_str(), 0, 0.5, control_state_.marker_pose_);
00418     break;
00419   default:
00420     ROS_INFO("Switching Arm Error");
00421     break;
00422   }
00423 }
00424 
00425 void InteractiveMarkerInterface::setOriginCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback,  bool origin_hand){
00426   if(origin_hand){
00427     control_state_.move_origin_state_ = ControlState::HAND_ORIGIN;
00428     changeMarkerMoveMode( marker_name.c_str(), 0, 0.5, control_state_.marker_pose_);
00429     if(control_state_.move_arm_ == ControlState::RARM){
00430       pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::SET_ORIGIN_RHAND);}else{
00431       pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::SET_ORIGIN_LHAND);}
00432   }else{
00433     control_state_.move_origin_state_ = ControlState::DESIGNATED_ORIGIN;
00434     changeMarkerMoveMode( marker_name.c_str(), 0, 0.5, control_state_.marker_pose_);
00435     pub_marker_menuCb(feedback, jsk_interactive_marker::MarkerMenu::SET_ORIGIN);
00436   }
00437   
00438 
00439 }
00440 
00441 
00442 void InteractiveMarkerInterface::ikmodeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00443 {
00444   //menu_handler.setCheckState( h_mode_last3, interactive_markers::MenuHandler::UNCHECKED );
00445   //h_mode_last3 = feedback->menu_entry_id;
00446   //menu_handler.setCheckState( h_mode_last3, interactive_markers::MenuHandler::CHECKED );
00447 
00448   if(feedback->menu_entry_id == rotation_t_menu_){
00449     menu_handler.setCheckState( rotation_nil_menu_, interactive_markers::MenuHandler::UNCHECKED );
00450     pub_marker_menu(feedback->marker_name,jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_T);
00451     ROS_INFO("Rotation Axis T");
00452   }else{
00453     menu_handler.setCheckState( rotation_t_menu_, interactive_markers::MenuHandler::UNCHECKED );
00454     pub_marker_menu(feedback->marker_name ,jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_NIL);
00455     ROS_INFO("Rotation Axis NIL");
00456   }
00457 
00458 
00459   menu_handler.setCheckState( feedback->menu_entry_id, interactive_markers::MenuHandler::CHECKED );
00460 
00461   menu_handler.reApply( *server_ );
00462   server_->applyChanges();
00463 }
00464 
00465 
00466 void InteractiveMarkerInterface::toggleIKModeCb( const std_msgs::EmptyConstPtr &msg)
00467 {
00468   interactive_markers::MenuHandler::CheckState check_state;
00469   if(menu_handler.getCheckState( rotation_t_menu_ , check_state)){
00470     if(check_state == interactive_markers::MenuHandler::CHECKED){
00471       //rotation axis nil
00472       menu_handler.setCheckState( rotation_t_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00473       menu_handler.setCheckState( rotation_nil_menu_ , interactive_markers::MenuHandler::CHECKED );
00474       pub_marker_menu("", jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_NIL);
00475 
00476     }else{
00477       //rotation_axis t
00478       menu_handler.setCheckState( rotation_t_menu_ , interactive_markers::MenuHandler::CHECKED );
00479       menu_handler.setCheckState( rotation_nil_menu_ , interactive_markers::MenuHandler::UNCHECKED );
00480       pub_marker_menu("" , jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_T);
00481     }
00482 
00483     menu_handler.reApply( *server_ );
00484     server_->applyChanges();
00485   }
00486 }
00487 
00488 void InteractiveMarkerInterface::marker_menu_cb( const jsk_interactive_marker::MarkerMenuConstPtr &msg){
00489   switch (msg->menu){
00490   case jsk_interactive_marker::MarkerMenu::SET_MOVE_RARM:
00491   case jsk_interactive_marker::MarkerMenu::SET_MOVE_LARM:
00492   case jsk_interactive_marker::MarkerMenu::SET_MOVE_ARMS:
00493     changeMoveArm(msg->marker_name, msg->menu);
00494     break;
00495   case jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_NIL:
00496   case jsk_interactive_marker::MarkerMenu::IK_ROTATION_AXIS_T:
00497     {
00498       std_msgs::EmptyConstPtr empty;
00499       toggleIKModeCb(empty);
00500     }
00501     break;
00502   case jsk_interactive_marker::MarkerMenu::PLAN:
00503   case jsk_interactive_marker::MarkerMenu::CANCEL_PLAN:
00504     {
00505       std_msgs::EmptyConstPtr empty;
00506       toggleStartIKCb(empty);
00507     }
00508     break;
00509   default:
00510     pub_marker_menu(msg->marker_name , msg->menu, msg->type);
00511     break;
00512   }
00513 }
00514 
00515 
00516 void InteractiveMarkerInterface::updateHeadGoal( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00517 {
00518   ros::Time now = ros::Time(0);
00519 
00520   switch ( feedback->event_type )
00521     {
00522     case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
00523       ROS_INFO_STREAM( feedback->marker_name << " was clicked on." );
00524       break;
00525     case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
00526       ROS_INFO_STREAM(    "Marker " << feedback->marker_name
00527                           << " control " << feedback->control_name
00528                           << " menu_entry_id " << feedback->menu_entry_id);
00529       break;
00530     case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
00531       //proc_feedback(feedback, jsk_interactive_marker::MarkerPose::HEAD_MARKER);
00532       break;
00533     }
00534 }
00535 
00536 void InteractiveMarkerInterface::updateBase( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00537 {
00538   switch ( feedback->event_type )
00539     {
00540     case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
00541       proc_feedback(feedback, jsk_interactive_marker::MarkerPose::BASE_MARKER);
00542       break;
00543     }
00544 }
00545 
00546 void InteractiveMarkerInterface::updateFinger( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, std::string hand)
00547 {
00548   ros::Time now = ros::Time(0);
00549 
00550   switch ( feedback->event_type )
00551     {
00552     case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
00553       if(hand == "rhand"){
00554         control_state_.r_finger_on_ ^= true;
00555       }else if(hand == "lhand"){
00556         control_state_.l_finger_on_ ^= true;
00557       }
00558       initControlMarkers();
00559       ROS_INFO_STREAM( hand << feedback->marker_name << " was clicked on." );
00560       break;
00561     case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
00562       ROS_INFO_STREAM( hand << feedback->marker_name << " was clicked on." );
00563       if(hand == "rhand"){
00564         proc_feedback(feedback, jsk_interactive_marker::MarkerPose::RFINGER_MARKER);
00565       }
00566       if(hand == "lhand"){
00567         proc_feedback(feedback, jsk_interactive_marker::MarkerPose::LFINGER_MARKER);
00568       }
00569 
00570       //proc_feedback(feedback);
00571       break;
00572     }
00573 }
00574 
00575 
00576 //im_mode
00577 //0:normal move  1:operationModel 2:operationalModelFirst
00578 void InteractiveMarkerInterface::changeMarkerForceMode( std::string mk_name , int im_mode){
00579   ROS_INFO("changeMarkerForceMode  marker:%s  mode:%d\n",mk_name.c_str(),im_mode);
00580   interactive_markers::MenuHandler reset_handler;
00581   menu_handler_force = reset_handler;
00582   menu_handler_force1 = reset_handler;
00583   menu_handler_force2 = reset_handler;
00584 
00585   geometry_msgs::PoseStamped pose;
00586   pose.header.frame_id = base_frame;
00587   if ( target_frame != "" ) {
00588     /*
00589       tf::StampedTransform stf;
00590       geometry_msgs::TransformStamped mtf;
00591       tfl_.lookupTransform(target_frame, base_frame,
00592       ros::Time(0), stf);
00593       tf::transformStampedTFToMsg(stf, mtf);
00594       pose.pose.position.x = mtf.transform.translation.x;
00595       pose.pose.position.y = mtf.transform.translation.y;
00596       pose.pose.position.z = mtf.transform.translation.z;
00597       pose.pose.orientation = mtf.transform.rotation;
00598       pose.header = mtf.header;
00599     */
00600   }
00601   visualization_msgs::InteractiveMarker mk;
00602   //    mk.name = marker_name.c_str();
00603   mk.name = mk_name.c_str();
00604   mk.scale = 0.5;
00605   mk.header = pose.header;
00606   mk.pose = pose.pose;
00607 
00608   // visualization_msgs::InteractiveMarker mk =
00609   //   im_helpers::make6DofMarker(marker_name.c_str(), pose, 0.5,
00610   //                              true, false );
00611   visualization_msgs::InteractiveMarkerControl control;
00612     
00613   if ( false )
00614     {
00615       //int_marker.name += "_fixed";
00616       //int_marker.description += "\n(fixed orientation)";
00617       control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00618     }
00619 
00620   control.orientation.w = 1;
00621   control.orientation.x = 1;
00622   control.orientation.y = 0;
00623   control.orientation.z = 0;
00624   control.name = "rotate_x";
00625   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00626   mk.controls.push_back(control);
00627   control.name = "move_x";
00628   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00629   mk.controls.push_back(control);
00630 
00631   control.orientation.w = 1;
00632   control.orientation.x = 0;
00633   control.orientation.y = 1;
00634   control.orientation.z = 0;
00635   control.name = "rotate_z";
00636   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00637   mk.controls.push_back(control);
00638   // control.name = "move_z";
00639   // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00640   // mk.controls.push_back(control);
00641 
00642   control.orientation.w = 1;
00643   control.orientation.x = 0;
00644   control.orientation.y = 0;
00645   control.orientation.z = 1;
00646   control.name = "rotate_y";
00647   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00648   mk.controls.push_back(control);
00649   // control.name = "move_y";
00650   // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00651   // mk.controls.push_back(control);
00652 
00653   //add furuta
00654   switch(im_mode){
00655   case 0:
00656     menu_handler_force.insert("MoveMode",boost::bind( &InteractiveMarkerInterface::changeMoveModeCb, this, _1));
00657     menu_handler_force.insert("Delete Force",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::DELETE_FORCE));
00658     server_->insert( mk );
00659     server_->setCallback( mk.name,
00660                           boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
00661     menu_handler_force.apply(*server_,mk.name);
00662       
00663     server_->applyChanges();
00664     break;
00665   case 1:
00666     mk.scale = 0.5;
00667     menu_handler_force1.insert("MoveMode",boost::bind( &InteractiveMarkerInterface::changeMoveModeCb1, this, _1));
00668     menu_handler_force1.insert("Delete Force",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::DELETE_FORCE));
00669     server_->insert( mk );
00670     server_->setCallback( mk.name,
00671                           boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
00672     menu_handler_force1.apply(*server_,mk.name);
00673     
00674     server_->applyChanges();
00675     break;
00676   case 2:
00677     mk.scale = 0.5;
00678     menu_handler_force2.insert("MoveMode",boost::bind( &InteractiveMarkerInterface::changeMoveModeCb2, this, _1));
00679     menu_handler_force2.insert("Delete Force",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::DELETE_FORCE));
00680     server_->insert( mk );
00681     server_->setCallback( mk.name,
00682                           boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
00683     menu_handler_force2.apply(*server_,mk.name);
00684  
00685     server_->applyChanges();
00686     break;
00687   default:
00688     break;
00689   }
00690 
00691 
00692   //menu_handler_force.insert("ResetForce",boost::bind( &InteractiveMarkerInterface::resetForceCb, this, _1));
00693 
00694   std::list<visualization_msgs::InteractiveMarker>::iterator it = imlist.begin();
00695 
00696   while( it != imlist.end() )  // listの末尾まで
00697     {
00698       if(it->name == mk_name.c_str()){
00699         imlist.erase(it);
00700         break;
00701       }
00702       it++;
00703     }
00704   imlist.push_back( mk );
00705 
00706   /*
00707     it = imlist.begin();
00708     while( it != imlist.end() )  // listの末尾まで
00709     {
00710     server_->insert( *it );
00711         
00712     server_->setCallback( it->name,
00713     boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
00714         
00715     menu_handler_force.apply(*server_,it->name);
00716     it++;
00717     }*/
00718   ROS_INFO("add mk");
00719   /* add mk */
00720 
00721 }
00722 
00723 void InteractiveMarkerInterface::initBodyMarkers(void){
00724   geometry_msgs::PoseStamped ps;
00725   ps.header.stamp = ros::Time(0);
00726 
00727   double scale_factor = 1.02;
00728 
00729   //for head
00730   ps.header.frame_id = head_link_frame_;
00731   visualization_msgs::InteractiveMarker im =
00732     im_helpers::makeMeshMarker(head_link_frame_, head_mesh_, ps, scale_factor);
00733   makeIMVisible(im);
00734   server_->insert(im);
00735   menu_head_.apply(*server_, head_link_frame_);
00736 
00737 
00738   if(hand_type_ == "sandia_hand"){
00739     geometry_msgs::PoseStamped ps;
00740     ps.header.stamp = ros::Time(0);
00741 
00742     ps.header.frame_id = "/right_f0_base";
00743     for(int i=0;i<4;i++){
00744       for(int j=0;j<3;j++){
00745         visualization_msgs::InteractiveMarker fingerIm = 
00746           makeSandiaHandInteractiveMarker(ps, "right", i, j);
00747         makeIMVisible(fingerIm);
00748         server_->insert(fingerIm, boost::bind( &InteractiveMarkerInterface::updateFinger, this, _1, "rhand"));
00749       }
00750     }
00751 
00752     ps.header.frame_id = "/left_f0_base";
00753     for(int i=0;i<4;i++){
00754       for(int j=0;j<3;j++){
00755         visualization_msgs::InteractiveMarker fingerIm = 
00756           makeSandiaHandInteractiveMarker(ps, "left", i, j);
00757         makeIMVisible(fingerIm);
00758         server_->insert(fingerIm, boost::bind( &InteractiveMarkerInterface::updateFinger, this, _1, "lhand"));
00759       }
00760     }
00761 
00762   }else{
00763     //for right hand
00764     for(int i=0; i<rhand_mesh_.size(); i++){
00765       ps.header.frame_id = rhand_mesh_[i].link_name;
00766       ps.pose.orientation = rhand_mesh_[i].orientation;
00767       visualization_msgs::InteractiveMarker handIm =
00768         im_helpers::makeMeshMarker( rhand_mesh_[i].link_name, rhand_mesh_[i].mesh_file, ps, scale_factor);
00769       makeIMVisible(handIm);
00770       server_->insert(handIm);
00771     }
00772   }
00773 }
00774 
00775 
00776 void InteractiveMarkerInterface::initControlMarkers(void){
00777   //Head Marker
00778   if(control_state_.head_on_ && control_state_.init_head_goal_){
00779     control_state_.init_head_goal_ = false;
00780     head_goal_pose_.header.stamp = ros::Time(0);
00781     
00782     visualization_msgs::InteractiveMarker HeadGoalIm =
00783       im_helpers::makeHeadGoalMarker( "head_point_goal", head_goal_pose_, 0.1);
00784     makeIMVisible(HeadGoalIm);
00785     server_->insert(HeadGoalIm,
00786                     boost::bind( &InteractiveMarkerInterface::updateHeadGoal, this, _1));
00787     menu_head_target_.apply(*server_,"head_point_goal");
00788   }
00789   if(!control_state_.head_on_){
00790     server_->erase("head_point_goal");
00791   }
00792 
00793   //Base Marker
00794   if(control_state_.base_on_ ){
00795     geometry_msgs::PoseStamped ps;
00796     ps.pose.orientation.w = 1;
00797     ps.header.frame_id = move_base_frame;
00798     ps.header.stamp = ros::Time(0);
00799     visualization_msgs::InteractiveMarker baseIm =
00800       InteractiveMarkerInterface::makeBaseMarker( "base_control", ps, 0.75, false);
00801     makeIMVisible(baseIm);
00802     server_->insert(baseIm,
00803                     boost::bind( &InteractiveMarkerInterface::updateBase, this, _1 ));
00804 
00805     menu_base_.apply(*server_,"base_control");
00806   }else{
00807     server_->erase("base_control");
00808   }
00809 
00810   //finger Control Marker
00811   if(use_finger_marker_ && control_state_.r_finger_on_){
00812     geometry_msgs::PoseStamped ps;
00813     ps.header.stamp = ros::Time(0);
00814     ps.header.frame_id = "/right_f0_base";
00815 
00816     server_->insert(makeFingerControlMarker("right_finger", ps),
00817                     boost::bind( &InteractiveMarkerInterface::updateFinger, this, _1, "rhand"));
00818     menu_finger_r_.apply(*server_,"right_finger");
00819   }else{
00820     server_->erase("right_finger");
00821   }
00822 
00823   if(use_finger_marker_ && control_state_.l_finger_on_){
00824     geometry_msgs::PoseStamped ps;
00825     ps.header.stamp = ros::Time(0);
00826     ps.header.frame_id = "/left_f0_base";
00827 
00828     server_->insert(makeFingerControlMarker("left_finger", ps),
00829                     boost::bind( &InteractiveMarkerInterface::updateFinger, this, _1, "lhand"));
00830     menu_finger_l_.apply(*server_,"left_finger");
00831   }else{
00832     server_->erase("left_finger");
00833   }
00834 
00835   server_->applyChanges();
00836 }
00837 
00838 
00839 visualization_msgs::InteractiveMarker InteractiveMarkerInterface::makeBaseMarker( const char *name, const geometry_msgs::PoseStamped &stamped, float scale, bool fixed)
00840 {
00841   visualization_msgs::InteractiveMarker mk;
00842   mk.header =  stamped.header;
00843   mk.name = name;
00844   mk.scale = scale;
00845   mk.pose = stamped.pose;
00846 
00847   visualization_msgs::InteractiveMarkerControl control;
00848   
00849   control.orientation.w = 1;
00850   control.orientation.x = 1;
00851   control.orientation.y = 0;
00852   control.orientation.z = 0;
00853 
00854   control.name = "move_x";
00855   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00856   mk.controls.push_back(control);
00857 
00858   control.orientation.w = 1;
00859   control.orientation.x = 0;
00860   control.orientation.y = 1;
00861   control.orientation.z = 0;
00862   control.name = "rotate_z";
00863   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00864   mk.controls.push_back(control);
00865 
00866   control.orientation.w = 1;
00867   control.orientation.x = 0;
00868   control.orientation.y = 0;
00869   control.orientation.z = 1;
00870   control.name = "move_y";
00871   control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00872   mk.controls.push_back(control);
00873   return mk;
00874 
00875 }
00876 
00877 
00878 
00879 void InteractiveMarkerInterface::initHandler(void){
00880   //use_arm=2;
00881 
00882   bool use_menu;
00883   pnh_.param("force_mode_menu", use_menu, false );
00884   if(use_menu){
00885     menu_handler.insert("ForceMode",boost::bind( &InteractiveMarkerInterface::changeForceModeCb, this, _1));
00886   }
00887     
00888   pnh_.param("move_menu", use_menu, false );
00889   if(use_menu){
00890     pnh_.param("move_safety_menu", use_menu, false );
00891     if(use_menu){
00892       interactive_markers::MenuHandler::EntryHandle sub_menu_move_;
00893       sub_menu_move_ = menu_handler.insert( "Move" );
00894       menu_handler.insert( sub_menu_move_,"Plan",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::PLAN));
00895       menu_handler.insert( sub_menu_move_,"Execute",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::EXECUTE));
00896       menu_handler.insert( sub_menu_move_,"Plan And Execute",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::PLAN_EXECUTE));
00897       menu_handler.insert( sub_menu_move_,"Cancel", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::CANCEL_PLAN));
00898     }else{
00899       menu_handler.insert("Move",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MOVE));
00900     }
00901   }
00902 
00903   pnh_.param("change_using_ik_menu", use_menu, false );
00904   if(use_menu){
00905     interactive_markers::MenuHandler::EntryHandle sub_menu_move_;
00906     sub_menu_move_ = menu_handler.insert( "Whether To Use IK" );
00907     start_ik_menu_ = menu_handler.insert( sub_menu_move_,"Start IK",boost::bind( &InteractiveMarkerInterface::usingIKCb, this, _1));
00908     menu_handler.setCheckState( start_ik_menu_, interactive_markers::MenuHandler::CHECKED );
00909 
00910     stop_ik_menu_ = menu_handler.insert( sub_menu_move_,"Stop IK",boost::bind( &InteractiveMarkerInterface::usingIKCb, this, _1));
00911     menu_handler.setCheckState( stop_ik_menu_, interactive_markers::MenuHandler::UNCHECKED );
00912   }
00913 
00914   //menu_handler.insert("Touch It", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::TOUCH));
00915   pnh_.param("touch_it_menu", use_menu, false );
00916   if(use_menu){
00917 
00918     interactive_markers::MenuHandler::EntryHandle sub_menu_handle_touch_it;
00919     sub_menu_handle_touch_it = menu_handler.insert( "Touch It" );
00920 
00921     //  menu_handler.insert( sub_menu_handle_touch_it, "Preview", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::TOUCHIT_PREV));
00922     menu_handler.insert( sub_menu_handle_touch_it, "Execute", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::TOUCHIT_EXEC));
00923     menu_handler.insert( sub_menu_handle_touch_it, "Cancel", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::TOUCHIT_CANCEL));
00924   }
00925   pnh_.param("look_hand_menu", use_menu, false );
00926   if(use_menu){
00927 
00928 
00929     interactive_markers::MenuHandler::EntryHandle sub_menu_handle_look_hand;
00930     sub_menu_handle_look_hand = menu_handler.insert( "Look hand" );
00931 
00932     menu_handler.insert( sub_menu_handle_look_hand, "rarm", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::LOOK_RARM));
00933     menu_handler.insert( sub_menu_handle_look_hand, "larm", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::LOOK_LARM));
00934   }
00935 
00936   pnh_.param("force_move_menu", use_menu, false );
00937   if(use_menu){
00938     menu_handler.insert("Force Move", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::FORCE_MOVE));
00939   }
00940 
00941   pnh_.param("pick_menu", use_menu, false );
00942   if(use_menu){
00943     menu_handler.insert("Pick", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::PICK));
00944   }
00945 
00946   pnh_.param("grasp_menu", use_menu, false );
00947   if(use_menu){
00948     menu_handler.insert("Grasp", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::START_GRASP));
00949   }
00950 
00951   pnh_.param("harf_grasp_menu", use_menu, false );
00952   if(use_menu){
00953     menu_handler.insert("Harf Grasp", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::HARF_GRASP));
00954   }
00955 
00956 
00957   pnh_.param("stop_grasp_menu", use_menu, false );
00958   if(use_menu){
00959     menu_handler.insert("Stop Grasp", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::STOP_GRASP));
00960   }
00961 
00962   pnh_.param("set_origin_menu", use_menu, false );
00963   if(use_menu){
00964     //menu_handler.insert("Set Origin To Hand", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::SET_ORIGIN));
00965     menu_handler.insert("Set Origin To Hand", boost::bind( &InteractiveMarkerInterface::setOriginCb, this, _1, true));
00966 
00967     menu_handler.insert("Set Origin", boost::bind( &InteractiveMarkerInterface::setOriginCb, this, _1, false));
00968   }
00969 
00970   /*
00971   pnh_.param("set_origin_menu", use_menu, false );
00972   if(use_menu){
00973     menu_handler.insert("Set Origin", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::SET_ORIGIN));
00974   }
00975 
00976   pnh_.param("set_origin_to_rhand_menu", use_menu, false );
00977   if(use_menu){
00978     menu_handler.insert("Set Origin To RHand", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::SET_ORIGIN_RHAND));
00979   }
00980 
00981   pnh_.param("set_origin_to_lhand_menu", use_menu, false );
00982   if(use_menu){
00983     menu_handler.insert("Set Origin To LHand", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::SET_ORIGIN_LHAND));
00984   }
00985   */
00986 
00987   pnh_.param("reset_marker_pos_menu", use_menu, false );
00988   if(use_menu){
00989     menu_handler.insert("Reset Marker Position", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::RESET_COORDS));
00990   }
00991 
00992   pnh_.param("manipulation_mode_menu", use_menu, false );
00993   if(use_menu){
00994     menu_handler.insert("Manipulation Mode", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MANIP_MODE));
00995   }
00996 
00997   //    menu_handler.insert("ResetForce",boost::bind( &InteractiveMarkerInterface::resetForceCb, this, _1));
00998     
00999   //menu_handler.insert("OperationModel",boost::bind( &InteractiveMarkerInterface::AutoMoveCb, this, _1));
01000     
01001   //    menu_handler.insert("StartTeaching",boost::bind( &InteractiveMarkerInterface::StartTeachingCb, this, _1));
01002 
01003 
01004   /*    sub_menu_handle2 = menu_handler.insert( "Constraint" );
01005     
01006         h_mode_last2 = menu_handler.insert( sub_menu_handle2, "constrained", boost::bind( &InteractiveMarkerInterface::ConstraintCb,this, _1 ));
01007         menu_handler.setCheckState( h_mode_last2, interactive_markers::MenuHandler::UNCHECKED );
01008         h_mode_constrained = h_mode_last2;
01009         h_mode_last2 = menu_handler.insert( sub_menu_handle2, "unconstrained", boost::bind( &InteractiveMarkerInterface::ConstraintCb,this, _1 ));
01010         menu_handler.setCheckState( h_mode_last2, interactive_markers::MenuHandler::CHECKED );
01011   */
01012     
01013   //    menu_handler.insert("StopTeaching",boost::bind( &InteractiveMarkerInterface::StopTeachingCb, this, _1));
01014   //menu_handler.setCheckState(menu_handler.insert("SetForce",boost::bind( &InteractiveMarkerInterface::enableCb, this, _1)),interactive_markers::MenuHandler::UNCHECKED);
01015     
01016 
01017   pnh_.param("select_arm_menu", use_menu, false );
01018   if(use_menu){
01019     sub_menu_handle = menu_handler.insert( "SelectArm" );
01020     h_mode_last = menu_handler.insert( sub_menu_handle, "Right Arm", boost::bind( &InteractiveMarkerInterface::modeCb,this, _1 ));
01021     menu_handler.setCheckState( h_mode_last, interactive_markers::MenuHandler::CHECKED );
01022     h_mode_rightarm = h_mode_last;
01023     h_mode_last = menu_handler.insert( sub_menu_handle, "Left Arm", boost::bind( &InteractiveMarkerInterface::modeCb,this, _1 ));
01024     menu_handler.setCheckState( h_mode_last, interactive_markers::MenuHandler::UNCHECKED );
01025     h_mode_last = menu_handler.insert( sub_menu_handle, "Both Arms", boost::bind( &InteractiveMarkerInterface::modeCb,this, _1 ));
01026     menu_handler.setCheckState( h_mode_last, interactive_markers::MenuHandler::UNCHECKED );
01027     h_mode_last = h_mode_rightarm;
01028   }
01029 
01030   pnh_.param("ik_mode_menu", use_menu, false );
01031   if(use_menu){
01032     sub_menu_handle_ik = menu_handler.insert( "IK mode" );
01033 
01034     rotation_t_menu_ = menu_handler.insert( sub_menu_handle_ik, "6D (Position + Rotation)", boost::bind( &InteractiveMarkerInterface::ikmodeCb,this, _1 ));
01035     menu_handler.setCheckState( rotation_t_menu_ , interactive_markers::MenuHandler::CHECKED );
01036     rotation_nil_menu_ = menu_handler.insert( sub_menu_handle_ik, "3D (Position)", boost::bind( &InteractiveMarkerInterface::ikmodeCb,this, _1 ));
01037     menu_handler.setCheckState( rotation_nil_menu_, interactive_markers::MenuHandler::UNCHECKED );
01038   }
01039 
01040   pnh_.param("use_torso_menu", use_menu, false );
01041   if(use_menu){
01042     use_torso_menu_ = menu_handler.insert( "Links To Use" );
01043 
01044     use_torso_nil_menu_ = menu_handler.insert( use_torso_menu_, "Arm", boost::bind( &InteractiveMarkerInterface::useTorsoCb,this, _1 ));
01045     menu_handler.setCheckState( use_torso_nil_menu_, interactive_markers::MenuHandler::UNCHECKED );
01046     use_torso_t_menu_ = menu_handler.insert( use_torso_menu_, "Arm and Torso", boost::bind( &InteractiveMarkerInterface::useTorsoCb,this, _1 ));
01047     menu_handler.setCheckState( use_torso_t_menu_, interactive_markers::MenuHandler::UNCHECKED );
01048     use_fullbody_menu_ = menu_handler.insert( use_torso_menu_, "Fullbody", boost::bind( &InteractiveMarkerInterface::useTorsoCb,this, _1 ));
01049     menu_handler.setCheckState( use_fullbody_menu_, interactive_markers::MenuHandler::CHECKED );
01050 
01051   }
01052 
01053 
01054 
01055   interactive_markers::MenuHandler::EntryHandle sub_menu_handle_im_size;
01056   sub_menu_handle_im_size = menu_handler.insert( "IMsize" );
01057   menu_handler.insert( sub_menu_handle_im_size, "Large", boost::bind( &InteractiveMarkerInterface::IMSizeLargeCb, this, _1));
01058   menu_handler.insert( sub_menu_handle_im_size, "Middle", boost::bind( &InteractiveMarkerInterface::IMSizeMiddleCb, this, _1));
01059   menu_handler.insert( sub_menu_handle_im_size, "Small", boost::bind( &InteractiveMarkerInterface::IMSizeSmallCb, this, _1));
01060 
01061   pnh_.param("publish_marker_menu", use_menu, false );
01062   if(use_menu){
01063     //menu_handler.insert("ManipulationMode", boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MANIP_MODE));
01064     menu_handler.insert("Publish Marker",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::PUBLISH_MARKER));
01065 
01066   }
01067 
01068 
01069 
01070 
01071   //--------- menu_handler 1 ---------------
01072   menu_handler1.insert("ForceMode",boost::bind( &InteractiveMarkerInterface::changeForceModeCb1, this, _1));
01073 
01074   //--------- menu_handler 2 ---------------
01075   menu_handler2.insert("ForceMode",boost::bind( &InteractiveMarkerInterface::changeForceModeCb2, this, _1));
01076   menu_handler2.insert("Move",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MOVE));
01077 
01078 
01079   /* porting from PR2 marker control */
01080   /* head marker */
01081 
01082   //menu_head_.insert("Take Snapshot", boost::bind( &InteractiveMarkerInterface::snapshotCB, this ) );
01083 
01084 
01085 
01086   head_target_handle_ = menu_head_.insert( "Target Point", 
01087                                            boost::bind( &InteractiveMarkerInterface::targetPointMenuCB, this, _1 ) );
01088   menu_head_.setCheckState(head_target_handle_, interactive_markers::MenuHandler::UNCHECKED);
01089 
01090   head_auto_look_handle_ = menu_head_.insert( "Look Automatically", boost::bind( &InteractiveMarkerInterface::lookAutomaticallyMenuCB,
01091                                                                                  this, _1 ) );
01092   menu_head_.setCheckState(head_auto_look_handle_, interactive_markers::MenuHandler::CHECKED);
01093   
01094   menu_head_target_.insert( "Look At",
01095                             boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1, jsk_interactive_marker::MarkerPose::HEAD_MARKER));
01096   //boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MOVE, jsk_interactive_marker::MarkerMenu::HEAD_MARKER));
01097 
01098 
01099   /*
01100     projector_handle_ = menu_head_.insert("Projector", boost::bind( &InteractiveMarkerInterface::projectorMenuCB,
01101     this, _1 ) );
01102     menu_head_.setCheckState(projector_handle_, MenuHandler::UNCHECKED);
01103   */
01104     
01105   /*
01106     menu_head_.insert( "Move Head To Center", boost::bind( &InteractiveMarkerInterface::centerHeadCB,
01107     this ) );
01108   */
01109 
01110 
01111   /* base move menu*/
01112   pnh_.param("use_base_marker", use_menu, false );
01113   control_state_.base_on_ = use_menu;
01114 
01115   menu_base_.insert("Base Move",boost::bind( &InteractiveMarkerInterface::pub_marker_menuCb, this, _1, jsk_interactive_marker::MarkerMenu::MOVE, jsk_interactive_marker::MarkerMenu::BASE_MARKER));
01116   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));
01117 
01118   /*finger move menu*/
01119   pnh_.param("use_finger_marker", use_finger_marker_, false );
01120 
01121   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));
01122   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));
01123 
01124   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));
01125   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));
01126 
01127 }
01128 
01129 void InteractiveMarkerInterface::addHandMarker(visualization_msgs::InteractiveMarker &im,std::vector < UrdfProperty > urdf_vec){
01130   if(urdf_vec.size() > 0){
01131     for(int i=0; i<urdf_vec.size(); i++){
01132       UrdfProperty up = urdf_vec[i];
01133       if(up.model){
01134         KDL::Frame origin_frame;
01135         tf::poseMsgToKDL(up.pose, origin_frame);
01136 
01137         LinkConstSharedPtr hand_root_link;
01138         hand_root_link = up.model->getLink(up.root_link_name);
01139         if(!hand_root_link){
01140           hand_root_link = up.model->getRoot();
01141         }
01142         im_utils::addMeshLinksControl(im, hand_root_link, origin_frame, !up.use_original_color, up.color, up.scale);
01143         for(int j=0; j<im.controls.size(); j++){
01144           if(im.controls[j].interaction_mode == visualization_msgs::InteractiveMarkerControl::BUTTON){
01145             im.controls[j].interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D;
01146             im.controls[j].name = "center_sphere";
01147           }
01148         }
01149       }else{
01150         addSphereMarker(im, up.scale, up.color);
01151       }
01152     }
01153   }else{
01154     double center_marker_size = 0.2;
01155     //gray
01156     std_msgs::ColorRGBA color;
01157     color.r = color.g = color.b = 0.7;
01158     color.a = 0.5;
01159     addSphereMarker(im, center_marker_size, color);
01160   }
01161 }
01162 
01163 void InteractiveMarkerInterface::addSphereMarker(visualization_msgs::InteractiveMarker &im, double scale, std_msgs::ColorRGBA color){
01164     visualization_msgs::Marker sphereMarker;
01165     sphereMarker.type = visualization_msgs::Marker::SPHERE;
01166 
01167     sphereMarker.scale.x = scale;
01168     sphereMarker.scale.y = scale;
01169     sphereMarker.scale.z = scale;
01170 
01171     sphereMarker.color = color;
01172 
01173     visualization_msgs::InteractiveMarkerControl sphereControl;
01174     sphereControl.name = "center_sphere";
01175 
01176     sphereControl.markers.push_back(sphereMarker);
01177     sphereControl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D;
01178     im.controls.push_back(sphereControl);
01179 }
01180 
01181 
01182 void InteractiveMarkerInterface::makeCenterSphere(visualization_msgs::InteractiveMarker &mk, double mk_size){
01183   std::vector < UrdfProperty > null_urdf;
01184   if(control_state_.move_origin_state_ == ControlState::HAND_ORIGIN){
01185     if(control_state_.move_arm_ == ControlState::RARM){
01186       addHandMarker(mk, rhand_urdf_);
01187     }else if(control_state_.move_arm_ == ControlState::LARM){
01188       addHandMarker(mk, lhand_urdf_);
01189     }else{
01190       addHandMarker(mk, null_urdf);
01191     }
01192   }else{
01193     addHandMarker(mk, null_urdf);
01194   }
01195 
01196   //sphereControl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D;
01197   //mk.controls.push_back(sphereControl);
01198 }
01199 
01200 //im_mode
01201 //0:normal move  1:operationModel 2:operationalModelFirst
01202 void InteractiveMarkerInterface::changeMarkerMoveMode( std::string mk_name , int im_mode){
01203   switch(im_mode){
01204   case 0:
01205     changeMarkerMoveMode( mk_name, im_mode , 0.5);
01206     break;
01207   case 1:
01208   case 2:
01209     changeMarkerMoveMode( mk_name, im_mode , 0.3);
01210     break;
01211   default:
01212     changeMarkerMoveMode( mk_name, im_mode , 0.3);
01213     break;
01214   }
01215 }
01216 
01217 void InteractiveMarkerInterface::changeMarkerMoveMode( std::string mk_name , int im_mode, float mk_size){
01218   geometry_msgs::PoseStamped pose;
01219   pose.header.frame_id = base_frame;
01220   pose.pose.orientation.w = 1.0;
01221   changeMarkerMoveMode( mk_name, im_mode , mk_size, pose);
01222 }
01223 
01224 void InteractiveMarkerInterface::changeMarkerMoveMode( std::string mk_name , int im_mode, float mk_size, geometry_msgs::PoseStamped dist_pose){
01225   ROS_INFO("changeMarkerMoveMode  marker:%s  mode:%d\n",mk_name.c_str(),im_mode);
01226   
01227   control_state_.marker_pose_ = dist_pose;
01228 
01229   interactive_markers::MenuHandler reset_handler;
01230 
01231   geometry_msgs::PoseStamped pose;
01232 
01233   if ( target_frame != "" ) {
01234     /*
01235       tf::StampedTransform stf;
01236       geometry_msgs::TransformStamped mtf;
01237       tfl_.lookupTransform(target_frame, base_frame,
01238       ros::Time(0), stf);
01239       tf::transformStampedTFToMsg(stf, mtf);
01240       pose.pose.position.x = mtf.transform.translation.x;
01241       pose.pose.position.y = mtf.transform.translation.y;
01242       pose.pose.position.z = mtf.transform.translation.z;
01243       pose.pose.orientation = mtf.transform.rotation;
01244       pose.header = mtf.header;
01245     */
01246   }else{
01247     pose = dist_pose;
01248   }
01249 
01250   visualization_msgs::InteractiveMarker mk;
01251   //0:normal move  1:operationModel 2:operationalModelFirst
01252 
01253   switch(im_mode){
01254   case 0:
01255     pose.header.stamp = ros::Time(0);
01256 
01257     mk = make6DofControlMarker(mk_name.c_str(), pose, mk_size,
01258                                true, false );
01259 
01260     if(use_center_sphere_){
01261       makeCenterSphere(mk, mk_size);
01262     }
01263 
01264     makeIMVisible(mk);
01265 
01266     server_->insert( mk );
01267     server_->setCallback( mk.name,
01268                           boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
01269     menu_handler.apply(*server_,mk.name);
01270     server_->applyChanges();
01271     break;
01272   case 1:
01273     mk = im_helpers::make6DofMarker(mk_name.c_str(), pose, mk_size,
01274                                     true, false );
01275     mk.description = mk_name.c_str();
01276     makeIMVisible(mk);
01277     server_->insert( mk );
01278     server_->setCallback( mk.name,
01279                           boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
01280     menu_handler1.apply(*server_,mk.name);
01281     server_->applyChanges();
01282     break;
01283       
01284   case 2:
01285     mk = im_helpers::make6DofMarker(mk_name.c_str(), pose, mk_size,
01286                                     true, false );
01287     mk.description = mk_name.c_str();
01288     makeIMVisible(mk);
01289       
01290     server_->insert( mk );
01291     server_->setCallback( mk.name,
01292                           boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
01293     menu_handler2.apply(*server_,mk.name);
01294     server_->applyChanges();
01295     break;
01296   default:
01297     mk = im_helpers::make6DofMarker(mk_name.c_str(), pose, mk_size,
01298                                     true, false );
01299     mk.description = mk_name.c_str();
01300 
01301     server_->insert( mk );
01302     server_->setCallback( mk.name,
01303                           boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
01304     server_->applyChanges();
01305     break;
01306   }
01307 
01308   std::list<visualization_msgs::InteractiveMarker>::iterator it = imlist.begin();
01309 
01310   while( it != imlist.end() )
01311     {
01312       if(it->name == mk_name.c_str()){
01313         imlist.erase(it);
01314         break;
01315       }
01316       it++;
01317     }
01318   imlist.push_back( mk );
01319 }
01320 
01321 void InteractiveMarkerInterface::changeMarkerOperationModelMode( std::string mk_name ){
01322   interactive_markers::MenuHandler reset_handler;
01323   menu_handler = reset_handler;
01324   geometry_msgs::PoseStamped pose;
01325   pose.header.frame_id = base_frame;
01326   /*
01327     if ( target_frame != "" ) {
01328     tf::StampedTransform stf;
01329     geometry_msgs::TransformStamped mtf;
01330     tfl_.lookupTransform(target_frame, base_frame,
01331     ros::Time(0), stf);
01332     tf::transformStampedTFToMsg(stf, mtf);
01333     pose.pose.position.x = mtf.transform.translation.x;
01334     pose.pose.position.y = mtf.transform.translation.y;
01335     pose.pose.position.z = mtf.transform.translation.z;
01336     pose.pose.orientation = mtf.transform.rotation;
01337     pose.header = mtf.header;
01338     }*/
01339 
01340   visualization_msgs::InteractiveMarker mk =
01341     
01342     im_helpers::make6DofMarker(mk_name.c_str(), pose, 0.5,
01343                                true, false );
01344   mk.description = mk_name.c_str();
01345   menu_handler.insert("ForceMode",boost::bind( &InteractiveMarkerInterface::changeForceModeCb, this, _1));
01346 
01347   std::list<visualization_msgs::InteractiveMarker>::iterator it = imlist.begin(); // イテレータ
01348 
01349   while( it != imlist.end() )  // listの末尾まで
01350     {
01351       if(it->name == mk_name.c_str()){
01352         imlist.erase(it);
01353         break;
01354       }
01355       it++;
01356     }
01357   imlist.push_back( mk );
01358   server_->insert( mk );
01359      
01360   server_->setCallback( mk.name,
01361                         boost::bind( &InteractiveMarkerInterface::proc_feedback, this, _1) );
01362      
01363   menu_handler.apply(*server_,mk.name);
01364   server_->applyChanges();
01365 }
01366 
01367 
01368 //InteractiveMarkerInterface::InteractiveMarkerInterface () : nh_(), pnh_("~"), tfl_(nh_) {
01369 InteractiveMarkerInterface::InteractiveMarkerInterface () : nh_(), pnh_("~") {
01370   pnh_.param("marker_name", marker_name, std::string ( "100") );
01371   pnh_.param("server_name", server_name, std::string ("") );
01372   pnh_.param("base_frame", base_frame, std::string ("/base_link") );
01373   pnh_.param("move_base_frame", move_base_frame, std::string ("/base_link") );
01374   pnh_.param("target_frame", target_frame, std::string ("") );
01375   //pnh_.param("fix_marker", fix_marker, true);
01376 
01377   if ( server_name == "" ) {
01378     server_name = ros::this_node::getName();
01379   }
01380 
01381   pub_ =  pnh_.advertise<jsk_interactive_marker::MarkerPose> ("pose", 1);
01382   pub_update_ =  pnh_.advertise<geometry_msgs::PoseStamped> ("pose_update", 1);
01383   pub_move_ =  pnh_.advertise<jsk_interactive_marker::MarkerMenu> ("marker_menu", 1);
01384 
01385   serv_set_ = pnh_.advertiseService("set_pose",
01386                                     &InteractiveMarkerInterface::set_cb, this);
01387   serv_markers_set_ = pnh_.advertiseService("set_markers",
01388                                             &InteractiveMarkerInterface::markers_set_cb, this);
01389   serv_markers_del_ = pnh_.advertiseService("del_markers",
01390                                             &InteractiveMarkerInterface::markers_del_cb, this);
01391   serv_reset_ = pnh_.advertiseService("reset_pose",
01392                                       &InteractiveMarkerInterface::reset_cb, this);
01393 
01394   sub_marker_pose_ = pnh_.subscribe<geometry_msgs::PoseStamped> ("move_marker", 1, boost::bind( &InteractiveMarkerInterface::move_marker_cb, this, _1));
01395   sub_marker_menu_ = pnh_.subscribe<jsk_interactive_marker::MarkerMenu> ("select_marker_menu", 1, boost::bind( &InteractiveMarkerInterface::marker_menu_cb, this, _1));
01396 
01397   sub_toggle_start_ik_ = pnh_.subscribe<std_msgs::Empty> ("toggle_start_ik", 1, boost::bind( &InteractiveMarkerInterface::toggleStartIKCb, this, _1));
01398   
01399   sub_toggle_ik_mode_ = pnh_.subscribe<std_msgs::Empty> ("toggle_ik_mode", 1, boost::bind( &InteractiveMarkerInterface::toggleIKModeCb, this, _1));
01400 
01401   ros::service::waitForService("set_dynamic_tf", -1);
01402   dynamic_tf_publisher_client_ = nh_.serviceClient<dynamic_tf_publisher::SetDynamicTF>("set_dynamic_tf", true);
01403 
01404   server_.reset( new interactive_markers::InteractiveMarkerServer(server_name));
01405 
01406   pnh_.param<std::string>("head_link_frame", head_link_frame_, "head_tilt_link");
01407   pnh_.param<std::string>("head_mesh", head_mesh_, "package://pr2_description/meshes/head_v0/head_tilt.dae");
01408 
01409   pnh_.param<std::string>("hand_type", hand_type_, "GENERIC");
01410 
01411   pnh_.param("use_head_marker", use_body_marker_, false );
01412   pnh_.param("use_center_sphere", use_center_sphere_, false );
01413 
01414   XmlRpc::XmlRpcValue v;
01415   pnh_.param("mesh_config", v, v);
01416   loadMeshes(v);
01417 
01418   head_goal_pose_.pose.position.x = 1.0;
01419   head_goal_pose_.pose.position.z = 1.0;
01420   head_goal_pose_.header.frame_id = base_frame;
01421 
01422   initHandler();
01423   if(use_body_marker_){
01424     initBodyMarkers();
01425   }
01426   initControlMarkers();
01427   changeMarkerMoveMode(marker_name.c_str(),0);
01428 }
01429 
01430 void InteractiveMarkerInterface::loadMeshes(XmlRpc::XmlRpcValue val){
01431   loadUrdfFromYaml(val, "r_hand", rhand_urdf_);
01432   loadUrdfFromYaml(val, "l_hand", lhand_urdf_);
01433 }
01434 
01435 void InteractiveMarkerInterface::loadUrdfFromYaml(XmlRpc::XmlRpcValue val, std::string name, std::vector<UrdfProperty>& mesh){
01436   if(val.hasMember(name)){
01437     for(int i=0; i< val[name].size(); i++){
01438       XmlRpc::XmlRpcValue nval = val[name][i];
01439       UrdfProperty up;
01440       //urdf file
01441       if(nval.hasMember("urdf_file")){
01442         std::string urdf_file = (std::string)nval["urdf_file"];
01443         std::cerr << "load urdf file: " << urdf_file << std::endl;
01444         up.model = im_utils::getModelInterface(urdf_file);
01445       }else if(nval.hasMember("urdf_param")){
01446         std::string urdf_param = (std::string)nval["urdf_param"];
01447         std::string urdf_model;
01448         nh_.getParam(urdf_param, urdf_model);
01449         up.model = parseURDF(urdf_model);
01450       }
01451 
01452       if(nval.hasMember("root_link")){
01453         std::string root_link_name = (std::string)nval["root_link"];
01454         std::cerr << "root link name: " << root_link_name << std::endl;
01455         up.root_link_name = root_link_name;
01456       }else{
01457         up.root_link_name = "";
01458       }
01459 
01460       up.pose.orientation.w = 1.0;
01461       //pose
01462       if(nval.hasMember("pose")){
01463         XmlRpc::XmlRpcValue pose = nval["pose"];
01464         if(pose.hasMember("position")){
01465           XmlRpc::XmlRpcValue position = pose["position"];
01466           up.pose.position.x = (double)position["x"];
01467           up.pose.position.y = (double)position["y"];
01468           up.pose.position.z = (double)position["z"];
01469         }
01470 
01471         if(pose.hasMember("orientation")){
01472           XmlRpc::XmlRpcValue orient = pose["orientation"];
01473           up.pose.orientation.x = (double)orient["x"];
01474           up.pose.orientation.y = (double)orient["y"];
01475           up.pose.orientation.z = (double)orient["z"];
01476           up.pose.orientation.w = (double)orient["w"];
01477         }
01478       }
01479 
01480       if(nval.hasMember("color")){
01481         XmlRpc::XmlRpcValue color = nval["color"];
01482         up.color.r = (double)color["r"];
01483         up.color.g = (double)color["g"];
01484         up.color.b = (double)color["b"];
01485         up.color.a = (double)color["a"];
01486       }else{
01487         up.color.r = 1.0;
01488         up.color.g = 1.0;
01489         up.color.b = 0.0;
01490         up.color.a = 0.7;
01491       }
01492       if(nval.hasMember("scale")){
01493         up.scale = (double)nval["scale"];
01494       }else{
01495         up.scale = 1.05; //make bigger a bit
01496       }
01497       mesh.push_back(up);
01498     }
01499   }
01500 }
01501 
01502 
01503 bool InteractiveMarkerInterface::markers_set_cb ( jsk_interactive_marker::MarkerSetPose::Request &req,
01504                                                   jsk_interactive_marker::MarkerSetPose::Response &res ) {
01505   bool setalready = false;
01506 
01507   std::list<visualization_msgs::InteractiveMarker>::iterator it = imlist.begin();
01508   while( it != imlist.end() )  // listの末尾まで
01509     {
01510       if( it->name == req.marker_name){
01511         setalready = true;
01512         break;
01513       }
01514       it++;
01515     }
01516 
01517   if(setalready){
01518     server_->setPose(req.marker_name, req.pose.pose, req.pose.header);
01519     server_->applyChanges();
01520     return true;
01521   }else{
01522     /*
01523       if(req.marker_name==0){
01524       changeMarkerMoveMode(name,2);
01525       }else{
01526       changeMarkerMoveMode(name,1);
01527       }
01528       server_->setPose(name, req.pose.pose, req.pose.header);
01529       //    menu_handler.apply(*server_,mk.name)Z
01530       server_->applyChanges();
01531       return true;
01532     */
01533     return true;
01534   }
01535 }
01536   
01537 bool InteractiveMarkerInterface::markers_del_cb ( jsk_interactive_marker::MarkerSetPose::Request &req,
01538                                                   jsk_interactive_marker::MarkerSetPose::Response &res ) {
01539   
01540   server_->erase(req.marker_name);
01541   server_->applyChanges();
01542   std::list<visualization_msgs::InteractiveMarker>::iterator it = imlist.begin();
01543   while( it != imlist.end() )  // listの末尾まで
01544     {
01545       if( it->name == req.marker_name){
01546         imlist.erase(it);
01547         break;
01548       }
01549       it++;
01550     }
01551   
01552   return true;
01553     
01554 }
01555 
01556 void InteractiveMarkerInterface::move_marker_cb ( const geometry_msgs::PoseStampedConstPtr &msg){
01557   pub_marker_tf(msg->header, msg->pose);
01558 
01559   pub_marker_pose( msg->header, msg->pose, marker_name, jsk_interactive_marker::MarkerPose::GENERAL);
01560 
01561   server_->setPose(marker_name, msg->pose, msg->header);
01562   server_->applyChanges();
01563 }
01564 
01565 
01566 bool InteractiveMarkerInterface::set_cb ( jsk_interactive_marker::MarkerSetPose::Request &req,
01567                                           jsk_interactive_marker::MarkerSetPose::Response &res ) {
01568 
01569   if ( req.markers.size() > 0 ) {
01570     visualization_msgs::InteractiveMarker mk;
01571     if ( server_->get(req.marker_name, mk) ) {
01572       visualization_msgs::InteractiveMarkerControl mkc;
01573       mkc.name = "additional_marker";
01574       mkc.always_visible = true;
01575       mkc.markers = req.markers;
01576       // delete added marker
01577       for ( std::vector<visualization_msgs::InteractiveMarkerControl>::iterator it
01578               =  mk.controls.begin();
01579             it != mk.controls.end(); it++ ) {
01580         if ( it->name == mkc.name ){
01581           mk.controls.erase( it );
01582           break;
01583         }
01584       }
01585       mk.controls.push_back( mkc );
01586     }
01587   }
01588   std::string mName = req.marker_name;
01589   if(mName == ""){
01590     mName = marker_name;
01591   }
01592   pub_marker_tf(req.pose.header, req.pose.pose);
01593 
01594   server_->setPose(mName, req.pose.pose, req.pose.header);
01595   server_->applyChanges();
01596   pub_update_.publish(req.pose);
01597   return true;
01598 }
01599 
01600 bool InteractiveMarkerInterface::reset_cb ( jsk_interactive_marker::SetPose::Request &req,
01601                                             jsk_interactive_marker::SetPose::Response &res ) {
01602   geometry_msgs::PoseStamped pose;
01603   pose.header.frame_id = base_frame;
01604   if ( target_frame != "" ) {
01605     /*
01606       tf::StampedTransform stf;
01607       geometry_msgs::TransformStamped mtf;
01608       tfl_.lookupTransform(base_frame, target_frame,
01609       ros::Time(0), stf);
01610       tf::transformStampedTFToMsg(stf, mtf);
01611       pose.pose.position.x = mtf.transform.translation.x;
01612       pose.pose.position.y = mtf.transform.translation.y;
01613       pose.pose.position.z = mtf.transform.translation.z;
01614       pose.pose.orientation = mtf.transform.rotation;
01615       // pose.header = mtf.header;
01616       // pose.header.stamp = ros::Time::Now();
01617       // pose.header.frame_id = target_frame;
01618       server_->setPose(marker_name, pose.pose, pose.header);
01619     */
01620   } else {
01621     server_->setPose(marker_name, pose.pose);
01622   }
01623   server_->applyChanges();
01624   return true;
01625 }
01626 
01627 
01628 void InteractiveMarkerInterface::makeIMVisible(visualization_msgs::InteractiveMarker &im){
01629   for(int i=0; i<im.controls.size(); i++){
01630     im.controls[i].always_visible = true;
01631   }
01632 }
01633 
01634 int main(int argc, char** argv)
01635 {
01636   ros::init(argc, argv, "jsk_marker_interface");
01637   InteractiveMarkerInterface imi;
01638   ros::spin();
01639 
01640   return 0;
01641 }


jsk_interactive_marker
Author(s): furuta
autogenerated on Wed May 1 2019 02:40:31