door_foot.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <interactive_markers/tools.h>
00003 #include <jsk_interactive_marker/door_foot.h>
00004 #include <jsk_interactive_marker/interactive_marker_utils.h>
00005 
00006 using namespace std;
00007 
00008 
00009 visualization_msgs::Marker DoorFoot::makeRWallMarker(){
00010   visualization_msgs::Marker marker;
00011   marker.type = visualization_msgs::Marker::CUBE;
00012   marker.scale.x = 0.05;
00013   marker.scale.y = 0.7;
00014   marker.scale.z = 2.0;
00015   
00016   marker.pose.position.y = - marker.scale.y / 2;
00017   marker.pose.position.z = marker.scale.z / 2;
00018   marker.pose.orientation.w = 1.0;
00019 
00020   marker.color.r = 0.0;
00021   marker.color.g = 1.0;
00022   marker.color.b = 0.0;
00023   marker.color.a = 0.9;
00024 
00025   return marker;
00026 }
00027 
00028 visualization_msgs::Marker DoorFoot::makeLWallMarker(){
00029   visualization_msgs::Marker marker;
00030   marker.type = visualization_msgs::Marker::CUBE;
00031   marker.scale.x = 0.05;
00032   marker.scale.y = 0.7;
00033   marker.scale.z = 2.0;
00034   
00035   marker.pose.position.y = 0.914 + marker.scale.y / 2;
00036   marker.pose.position.z = marker.scale.z / 2;
00037   marker.pose.orientation.w = 1.0;
00038 
00039   marker.color.r = 0.0;
00040   marker.color.g = 1.0;
00041   marker.color.b = 0.0;
00042   marker.color.a = 0.9;
00043 
00044   return marker;
00045 }
00046 
00047 
00048 visualization_msgs::Marker DoorFoot::makeDoorMarker(){
00049   visualization_msgs::Marker marker;
00050   marker.type = visualization_msgs::Marker::CUBE;
00051   marker.scale.x = 0.05;
00052   marker.scale.y = 0.914;
00053   marker.scale.z = 2.0;
00054   
00055   marker.pose.position.y = marker.scale.y / 2;
00056   marker.pose.position.z = marker.scale.z / 2;
00057   marker.pose.orientation.w = 1.0;
00058 
00059   marker.color.r = 1.0;
00060   marker.color.g = 1.0;
00061   marker.color.b = 0.0;
00062   marker.color.a = 0.3;
00063 
00064   return marker;
00065 }
00066 
00067 visualization_msgs::Marker DoorFoot::makeKnobMarker(){
00068   visualization_msgs::Marker marker;
00069   marker.type = visualization_msgs::Marker::CUBE;
00070   marker.scale.x = 0.03;
00071   marker.scale.y = 0.1;
00072   marker.scale.z = 0.03;
00073   
00074   marker.pose.position.x = -0.05;
00075   marker.pose.position.y = 0.914 - 0.1 - marker.scale.y/2 ;
00076   marker.pose.position.z = 0.9398;//37 in
00077   marker.pose.orientation.w = 1.0;
00078 
00079   marker.color.r = 1.0;
00080   marker.color.g = 0.0;
00081   marker.color.b = 0.0;
00082   marker.color.a = 1.0;
00083 
00084   return marker;
00085 }
00086 
00087 visualization_msgs::Marker DoorFoot::makeKnobMarker(int position){
00088   double size = 0.02;
00089   visualization_msgs::Marker marker;
00090   marker.type = visualization_msgs::Marker::CUBE;
00091   marker.scale.x = 0.02;
00092   marker.scale.y = size;
00093   marker.scale.z = 0.03;
00094   
00095   marker.pose.position.x = -0.07;
00096   marker.pose.position.y = 0.914 - marker.scale.y/2 - position * size;
00097   marker.pose.position.z = 0.9398;//37 in
00098   marker.pose.orientation.w = 1.0;
00099   
00100   switch(position % 5){
00101   case 0:
00102     marker.color.r = 1.0;
00103     marker.color.g = 0.0;
00104     marker.color.b = 0.0;
00105     break;
00106   case 1:
00107     marker.color.r = 0.5;
00108     marker.color.g = 0.0;
00109     marker.color.b = 0.5;
00110     break;
00111   case 2:
00112     marker.color.r = 0.0;
00113     marker.color.g = 0.0;
00114     marker.color.b = 1.0;
00115     break;
00116   case 3:
00117     marker.color.r = 0.0;
00118     marker.color.g = 0.5;
00119     marker.color.b = 0.7;
00120     break;
00121   case 4:
00122     marker.color.r = 0.0;
00123     marker.color.g = 1.0;
00124     marker.color.b = 0.0;
00125     break;
00126   }
00127   marker.color.a = 0.7;
00128 
00129   return marker;
00130 }
00131 
00132 
00133 visualization_msgs::Marker DoorFoot::makeRFootMarker(){
00134   geometry_msgs::Pose pose;
00135   if(push){
00136     //Right Foot Position
00137     pose.position.x = -0.523;
00138     pose.position.y = 0.270;
00139     pose.position.z = 0;
00140     pose.orientation.w = 0.766;
00141     pose.orientation.x = 0;
00142     pose.orientation.y = 0;
00143     pose.orientation.z = 0.642788;
00144   }else{
00145     pose.position.x = -0.8;
00146     pose.position.y = 0.0;
00147     pose.position.z = -0.910;
00148     pose.orientation.w = 1.0;
00149   }
00150 
00151   return makeFootMarker(pose, true);
00152 }
00153 visualization_msgs::Marker DoorFoot::makeLFootMarker(){
00154   geometry_msgs::Pose pose;
00155   if(push){
00156     pose.position.x = -0.747;
00157     pose.position.y = 0.310;
00158     pose.position.z = 0;
00159     pose.orientation.w = 0.766;
00160     pose.orientation.x = 0;
00161     pose.orientation.y = 0;
00162     pose.orientation.z = 0.642788;
00163   }else{
00164     pose.position.x = -0.8;
00165     pose.position.y = -0.3;
00166     pose.position.z = -0.910;
00167     pose.orientation.w = 1.0;
00168   }
00169   return makeFootMarker(pose, false);
00170 
00171 }
00172 
00173 
00174 visualization_msgs::Marker DoorFoot::makeFootMarker(geometry_msgs::Pose pose, bool right){
00175   visualization_msgs::Marker marker;
00176   marker.type = visualization_msgs::Marker::CUBE;
00177   double PADDING_PARAM = 0.01;
00178   marker.scale.x = 0.27 + PADDING_PARAM;
00179   marker.scale.y = 0.14 + PADDING_PARAM;
00180   marker.scale.z = 0.05;
00181 
00182   marker.pose = pose;
00183 
00184   if(right){
00185     marker.color.r = 1.0;
00186     marker.color.g = 1.0;
00187     marker.color.b = 0.0;
00188   }else{
00189     marker.color.r = 1.0;
00190     marker.color.g = 0.0;
00191     marker.color.b = 1.0;
00192   }
00193   marker.color.a = 1.0;
00194 
00195   return marker;
00196 }
00197 
00198 
00199 visualization_msgs::InteractiveMarker DoorFoot::makeInteractiveMarker(){
00200   visualization_msgs::InteractiveMarker mk;
00201   mk.header.frame_id = "/map";
00202   mk.header.stamp = ros::Time(0);
00203   mk.name = marker_name;
00204   mk.scale = 0.3;
00205   mk.pose = door_pose;
00206 
00207   visualization_msgs::InteractiveMarkerControl triangleMarker;
00208   triangleMarker.always_visible = true;
00209   triangleMarker.markers.push_back( makeRWallMarker());
00210   triangleMarker.markers.push_back( makeLWallMarker());
00211   triangleMarker.markers.push_back( makeDoorMarker());
00212 
00213   if(use_color_knob){
00214     for(int i=0;i<12;i++){
00215       triangleMarker.markers.push_back( makeKnobMarker(i));
00216     }
00217   }else{
00218     triangleMarker.markers.push_back( makeKnobMarker());
00219   }
00220 
00221   if (footstep_show_initial_p_) {
00222     for(size_t i=0; i<2; i++){
00223       if(foot_list[i].header.frame_id == "right"){
00224         triangleMarker.markers.push_back( makeFootMarker( foot_list[i].pose, true ));
00225       }else{
00226         triangleMarker.markers.push_back( makeFootMarker( foot_list[i].pose, false ));
00227       }
00228     }
00229   }
00230   else {
00231     int first_index = footstep_index_ * 2;
00232     int second_index = footstep_index_ * 2 + 1;
00233     if (foot_list.size() > first_index) {
00234       triangleMarker.markers.push_back( makeFootMarker( foot_list[first_index].pose,
00235                                                         foot_list[first_index].header.frame_id == "right"));
00236     }
00237     if (foot_list.size() > second_index) {
00238       triangleMarker.markers.push_back( makeFootMarker( foot_list[second_index].pose,
00239                                                         foot_list[second_index].header.frame_id == "right"));
00240     }
00241   }
00242   
00243   
00244 
00245   mk.controls.push_back( triangleMarker );
00246   
00247   im_helpers::add6DofControl(mk, true);
00248   return mk;
00249 }
00250 
00251 void DoorFoot::moveBoxCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00252 {
00253   door_pose = feedback->pose;
00254   //  std::cout << "moved" << std::endl;
00255 }
00256 
00257 void DoorFoot::showStandLocationCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
00258   footstep_show_initial_p_ = true;
00259   updateBoxInteractiveMarker();
00260 }
00261 void DoorFoot::showNextStepCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
00262   if (foot_list.size() > 2) {
00263     if (footstep_show_initial_p_) {
00264       footstep_index_ = 1;
00265       footstep_show_initial_p_ = false;
00266     }
00267     else {
00268       ++footstep_index_;
00269       if (foot_list.size() / 2 == footstep_index_) {
00270         footstep_index_ = 1;
00271       }
00272     }
00273   }
00274   updateBoxInteractiveMarker();
00275 }
00276 void DoorFoot::showPreviousStepCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
00277   if (foot_list.size() > 2) {
00278     if (footstep_show_initial_p_) {
00279       footstep_index_ = 1;
00280       footstep_show_initial_p_ = false;
00281     }
00282     else {
00283       --footstep_index_;
00284       if (footstep_index_ == 0) {
00285         footstep_index_ = (foot_list.size() - 1)/ 2;
00286       }
00287     }
00288   }
00289   updateBoxInteractiveMarker();
00290 }
00291 
00292 void DoorFoot::pushDoorCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
00293   push = true;
00294   updateBoxInteractiveMarker();
00295 }
00296 
00297 void DoorFoot::pullDoorCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
00298   push = false;
00299   updateBoxInteractiveMarker();
00300 }
00301 
00302 
00303 interactive_markers::MenuHandler DoorFoot::makeMenuHandler(){
00304   interactive_markers::MenuHandler mh;
00305   mh.insert("Push Door", boost::bind( &DoorFoot::pushDoorCb, this, _1));
00306   mh.insert("Pull Door", boost::bind( &DoorFoot::pullDoorCb, this, _1));
00307   mh.insert("Show Initial Stand Location", boost::bind( &DoorFoot::showStandLocationCb, this, _1));
00308   mh.insert("Show Next Step", boost::bind( &DoorFoot::showNextStepCb, this, _1));
00309   mh.insert("Show Previous Step", boost::bind( &DoorFoot::showPreviousStepCb, this, _1));
00310   return mh;
00311 }
00312 
00313 
00314 void DoorFoot::updateBoxInteractiveMarker(){
00315   visualization_msgs::InteractiveMarker boxIM = makeInteractiveMarker();
00316 
00317   server_->insert(boxIM,
00318                   boost::bind( &DoorFoot::moveBoxCb, this, _1 ));
00319   menu_handler.apply(*server_, marker_name);
00320   server_->applyChanges();
00321 }
00322 
00323 DoorFoot::DoorFoot () : nh_(), pnh_("~") {
00324   footstep_show_initial_p_ = true;
00325   footstep_index_ = -1;
00326   pnh_.param("server_name", server_name, std::string ("") );
00327   pnh_.param("size", size_, 1.0 );
00328   pnh_.param("marker_name", marker_name, std::string ("door_marker") );
00329   pnh_.param("push", push, true);
00330   pnh_.param("use_color_knob", use_color_knob, true);
00331 
00332   XmlRpc::XmlRpcValue v;
00333   pnh_.param("foot_list", v, v);
00334   for(int i=0; i< v.size(); i++){
00335     XmlRpc::XmlRpcValue foot = v[i];
00336     geometry_msgs::Pose p = getPose(foot["pose"]);
00337     geometry_msgs::PoseStamped footPose;
00338     footPose.pose = p;
00339     footPose.header.frame_id.assign(foot["leg"]);
00340     foot_list.push_back(footPose);
00341   }
00342 
00343   if ( server_name == "" ) {
00344     server_name = ros::this_node::getName();
00345   }
00346   server_.reset( new interactive_markers::InteractiveMarkerServer(server_name));
00347 
00348   menu_handler = makeMenuHandler();
00349   updateBoxInteractiveMarker();
00350   return;
00351 }
00352 
00353 
00354 int main(int argc, char** argv)
00355 {
00356   ros::init(argc, argv, "door_foot_marker");
00357   DoorFoot triFoot;
00358   int counter = 0;
00359   ros::spin();
00360   return 0;
00361 }
00362 
00363 
00364 geometry_msgs::Pose getPose( XmlRpc::XmlRpcValue val){
00365   geometry_msgs::Pose p;
00366   XmlRpc::XmlRpcValue pos = val["position"];
00367   p.position.x = getXmlValue(pos["x"]);
00368   p.position.y = getXmlValue(pos["y"]);
00369   p.position.z = getXmlValue(pos["z"]);
00370 
00371   XmlRpc::XmlRpcValue ori = val["orientation"];
00372   p.orientation.x = getXmlValue(ori["x"]);
00373   p.orientation.y = getXmlValue(ori["y"]);
00374   p.orientation.z = getXmlValue(ori["z"]);
00375   p.orientation.w = getXmlValue(ori["w"]);
00376 
00377   return p;
00378 }
00379 
00380 double getXmlValue( XmlRpc::XmlRpcValue val ){
00381   switch(val.getType()){
00382   case XmlRpc::XmlRpcValue::TypeInt:
00383     return (double)((int)val);
00384   case XmlRpc::XmlRpcValue::TypeDouble:
00385     return (double)val;
00386   default:
00387     return 0;
00388   }
00389 }
00390   


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