jsk_interactive_marker: parent_and_child_interactive_marker

Go to the documentation of this file.
00001 #include <jsk_interactive_marker/parent_and_child_interactive_marker_server.h>
00002 
00003 namespace jsk_interactive_marker
00004 {
00005   ParentAndChildInteractiveMarkerServer::ParentAndChildInteractiveMarkerServer(const std::string &topic_ns, const std::string &server_id, bool spin_thread) : InteractiveMarkerServer(topic_ns, server_id, spin_thread)
00006   {
00007     topic_server_name_ = topic_ns;
00008     std::string get_marker_pose_service_name = topic_ns + "/get_marker_pose";
00009     std::string set_parent_service_name = topic_ns + "/set_parent_marker";
00010     std::string remove_parent_service_name = topic_ns + "/remove_parent_marker";
00011     get_marker_pose_srv_ = n_.advertiseService(get_marker_pose_service_name, &ParentAndChildInteractiveMarkerServer::getMarkerPoseService, this);
00012     set_parent_srv_ = n_.advertiseService(set_parent_service_name, &ParentAndChildInteractiveMarkerServer::setParentService, this);
00013     remove_parent_srv_ = n_.advertiseService(remove_parent_service_name, &ParentAndChildInteractiveMarkerServer::removeParentService, this);
00014   }
00015   bool ParentAndChildInteractiveMarkerServer::setParentService(jsk_interactive_marker::SetParentMarker::Request &req, jsk_interactive_marker::SetParentMarker::Response &res)
00016   {
00017     geometry_msgs::PoseStamped child_pose_stamped;
00018     // get current pose stamped
00019     getMarkerPose(req.child_marker_name, child_pose_stamped);
00020     if (req.parent_topic_name == std::string("") || topic_server_name_ == req.parent_topic_name) // self association
00021     {
00022       if (!registerAssociationItself(req.parent_marker_name, topic_server_name_, req.child_marker_name, child_pose_stamped))
00023         return false;
00024     }
00025     else{ // refer to different server
00026       if (registerAssociationWithOtherNode(req.parent_marker_name, req.parent_topic_name, req.child_marker_name, child_pose_stamped))
00027       {
00028 
00029         if (parent_subscriber_nums_.find(req.parent_topic_name) == parent_subscriber_nums_.end() || parent_subscriber_nums_[req.parent_topic_name] == 0 )
00030         {
00031           // register subscriber
00032           parent_subscriber_nums_[req.parent_topic_name] = 1;
00033           parent_update_subscribers_[req.parent_topic_name] = n_.subscribe<visualization_msgs::InteractiveMarkerUpdate>(req.parent_topic_name + "/update", 1, boost::bind(&ParentAndChildInteractiveMarkerServer::parentUpdateCb, this, _1, req.parent_topic_name));
00034           parent_feedback_subscribers_[req.parent_topic_name] = n_.subscribe<visualization_msgs::InteractiveMarkerFeedback>(req.parent_topic_name + "/feedback", 1, boost::bind(&ParentAndChildInteractiveMarkerServer::parentFeedbackCb, this, _1, req.parent_topic_name));
00035         }
00036         else
00037         {
00038           parent_subscriber_nums_[req.parent_topic_name] += 1;
00039         }
00040       }
00041       else
00042       {
00043         return false;
00044       }
00045     }
00046     if (! (callback_map_.find(req.child_marker_name)!=callback_map_.end()))
00047     {
00048       setCallback(req.child_marker_name, NULL, DEFAULT_FEEDBACK_CB);
00049     }
00050     if ((req.parent_topic_name == std::string("") || topic_server_name_ == req.parent_topic_name) && ! (callback_map_.find(req.parent_marker_name)!=callback_map_.end()))
00051     {
00052       setCallback(req.parent_marker_name, NULL, DEFAULT_FEEDBACK_CB);
00053     }
00054     return true;
00055   }
00056   bool ParentAndChildInteractiveMarkerServer::registerAssociationItself(std::string parent_marker_name, std::string parent_topic_name, std::string child_marker_name, geometry_msgs::PoseStamped child_pose_stamped)
00057   {
00058     geometry_msgs::PoseStamped parent_pose_stamped;
00059     getMarkerPose(parent_marker_name, parent_pose_stamped);
00060     return registerAssociation(parent_marker_name, parent_topic_name, child_marker_name, child_pose_stamped, parent_pose_stamped);
00061   }
00062   bool ParentAndChildInteractiveMarkerServer::registerAssociationWithOtherNode(std::string parent_marker_name, std::string parent_topic_name, std::string child_marker_name, geometry_msgs::PoseStamped child_pose_stamped)
00063   {
00064     geometry_msgs::PoseStamped parent_pose_stamped;
00065     ros::ServiceClient client = n_.serviceClient<jsk_interactive_marker::GetTransformableMarkerPose>(parent_topic_name + "/get_marker_pose");
00066     jsk_interactive_marker::GetTransformableMarkerPose srv;
00067     srv.request.target_name = parent_marker_name;
00068     if (!client.call(srv))
00069     {
00070         return false;
00071     }
00072     parent_pose_stamped = srv.response.pose_stamped;
00073     return registerAssociation(parent_marker_name, parent_topic_name, child_marker_name, child_pose_stamped, parent_pose_stamped);
00074   }
00075   bool ParentAndChildInteractiveMarkerServer::registerAssociation(std::string parent_marker_name, std::string parent_topic_name, std::string child_marker_name, geometry_msgs::PoseStamped child_pose_stamped, geometry_msgs::PoseStamped parent_pose_stamped)
00076   {
00077     geometry_msgs::PoseStamped parent_pose_stamped_transed;
00078     if (tf_listener_.waitForTransform(child_pose_stamped.header.frame_id,
00079                                        parent_pose_stamped.header.frame_id, parent_pose_stamped.header.stamp, ros::Duration(1.0)))
00080     {
00081       tf_listener_.transformPose(child_pose_stamped.header.frame_id, parent_pose_stamped, parent_pose_stamped_transed);
00082     }
00083     else
00084     {
00085       return false;
00086     }
00087     Eigen::Affine3d parent_pose_eigened, child_pose_eigened;
00088     tf::poseMsgToEigen(parent_pose_stamped_transed.pose, parent_pose_eigened);
00089     tf::poseMsgToEigen(child_pose_stamped.pose, child_pose_eigened);
00090     association_list_[child_marker_name] = jsk_interactive_marker::ParentMarkerInformation(parent_topic_name, parent_marker_name, parent_pose_eigened.inverse() * child_pose_eigened);
00091     return true;
00092   }
00093 
00094   bool ParentAndChildInteractiveMarkerServer::removeParentService(jsk_interactive_marker::RemoveParentMarker::Request &req, jsk_interactive_marker::RemoveParentMarker::Response &res)
00095   {
00096     return removeParent(req.child_marker_name);
00097   }
00098   bool ParentAndChildInteractiveMarkerServer::removeParent(std::string child_marker_name)
00099   {
00100     std::string parent_topic_name = association_list_[child_marker_name].parent_topic_name;
00101     parent_subscriber_nums_[parent_topic_name] -= 1;
00102     if (parent_subscriber_nums_[parent_topic_name] == 0)
00103     {
00104       parent_feedback_subscribers_[parent_topic_name].shutdown();
00105       parent_update_subscribers_[parent_topic_name].shutdown();
00106     }
00107     setCallback(child_marker_name, NULL, 200);
00108     association_list_.erase(child_marker_name);
00109     return true;
00110   }
00111 
00112   bool ParentAndChildInteractiveMarkerServer::getMarkerPoseService(jsk_interactive_marker::GetTransformableMarkerPose::Request &req, jsk_interactive_marker::GetTransformableMarkerPose::Response &res)
00113   {
00114     geometry_msgs::PoseStamped pose_stamped;
00115     if (getMarkerPose(req.target_name, pose_stamped))
00116       {
00117         res.pose_stamped = pose_stamped;
00118       }
00119     else
00120       return false;
00121   }
00122   bool ParentAndChildInteractiveMarkerServer::getMarkerPose(std::string target_name, geometry_msgs::PoseStamped &pose_stamped)
00123   {
00124     visualization_msgs::InteractiveMarker focus_marker;
00125     if (get(target_name, focus_marker))
00126       {
00127         pose_stamped.pose = focus_marker.pose;
00128         pose_stamped.header = focus_marker.header;
00129         return true;
00130       }
00131     return false;
00132   }
00133   void ParentAndChildInteractiveMarkerServer::parentUpdateCb(const visualization_msgs::InteractiveMarkerUpdateConstPtr &update, std::string parent_topic_name)
00134   {
00135     // keep relative pose
00136     bool need_apply_change = false;
00137     std::map <std::string, ParentMarkerInformation>::iterator assoc_it = association_list_.begin();
00138     while (assoc_it != association_list_.end()){
00139       for (size_t i=0; i<update->markers.size(); i++){
00140         if (assoc_it -> second.parent_topic_name == parent_topic_name && assoc_it -> second.parent_marker_name == update -> markers[i].name){
00141           renewPoseWithParent(assoc_it, update -> markers[i].pose, update -> markers[i].header);
00142           need_apply_change = true;
00143         }
00144       }
00145       ++assoc_it;
00146     }
00147     // apply change
00148     if (need_apply_change)
00149       interactive_markers::InteractiveMarkerServer::applyChanges();
00150   }
00151   void ParentAndChildInteractiveMarkerServer::parentFeedbackCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, std::string parent_topic_name)
00152   {
00153     // keep relative pose
00154     bool need_apply_change = false;
00155     std::map <std::string, ParentMarkerInformation>::iterator assoc_it = association_list_.begin();
00156     while (assoc_it != association_list_.end()){
00157       if (assoc_it -> second.parent_topic_name == parent_topic_name && assoc_it -> second.parent_marker_name == feedback -> marker_name){
00158         renewPoseWithParent(assoc_it, feedback -> pose, feedback -> header);
00159         need_apply_change = true;
00160       }
00161       ++assoc_it;
00162     }
00163     // apply change
00164     if (need_apply_change)
00165       interactive_markers::InteractiveMarkerServer::applyChanges();
00166     // apply change
00167   }
00168   void ParentAndChildInteractiveMarkerServer::renewPoseWithParent(std::map <std::string, ParentMarkerInformation>::iterator assoc_it, geometry_msgs::Pose parent_pose, std_msgs::Header parent_header)
00169   {
00170     Eigen::Affine3d parent_pose_eigened, child_new_pose_eigened;
00171     tf::poseMsgToEigen(parent_pose, parent_pose_eigened);
00172     child_new_pose_eigened = parent_pose_eigened * assoc_it -> second.relative_pose;
00173     geometry_msgs::Pose child_new_pose;
00174     tf::poseEigenToMsg(child_new_pose_eigened, child_new_pose);
00175     setPose(assoc_it -> first, child_new_pose, parent_header);
00176   }
00177   void ParentAndChildInteractiveMarkerServer::selfFeedbackCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) // for updating relative pose with feedback
00178   {
00179     // keep relative pose
00180     bool need_apply_change = false;
00181     std::map <std::string, ParentMarkerInformation>::iterator assoc_it = association_list_.begin();
00182     while (assoc_it != association_list_.end()){
00183       if (assoc_it -> second.parent_topic_name == topic_server_name_ && assoc_it -> second.parent_marker_name == feedback -> marker_name){
00184         renewPoseWithParent(assoc_it, feedback -> pose, feedback -> header);
00185         need_apply_change = true;
00186       }
00187       ++assoc_it;
00188     }
00189     // apply change
00190     if (need_apply_change){
00191       interactive_markers::InteractiveMarkerServer::applyChanges();
00192     }
00193 
00194     // check self parent
00195     // renew all
00196     // only when mouse is removed because it takes much cost for service call
00197     if (feedback -> event_type != visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP)
00198     {
00199       return;
00200     }
00201     // only changes feedback marker -> parent
00202     geometry_msgs::PoseStamped feedback_pose_stamped;
00203     feedback_pose_stamped.pose = feedback->pose;
00204     feedback_pose_stamped.header = feedback->header;
00205     assoc_it = association_list_.begin();
00206     while (assoc_it != association_list_.end()){
00207       if (assoc_it->first == feedback->marker_name){
00208         if (topic_server_name_ == assoc_it->second.parent_topic_name)
00209         {
00210           registerAssociationItself(assoc_it->second.parent_marker_name, assoc_it->second.parent_topic_name, assoc_it->first, feedback_pose_stamped);
00211         }
00212         else
00213         {
00214           registerAssociationWithOtherNode(assoc_it->second.parent_marker_name, assoc_it->second.parent_topic_name, assoc_it->first, feedback_pose_stamped);
00215         }
00216       }
00217       ++assoc_it;
00218     }
00219   }
00220   void ParentAndChildInteractiveMarkerServer::applyChanges() // for updating relative pose with update
00221   {
00222     // keep relative pose
00223     std::map <std::string, ParentMarkerInformation>::iterator assoc_it = association_list_.begin();
00224     while (assoc_it != association_list_.end()){
00225       if(assoc_it->second.parent_topic_name == topic_server_name_)
00226       {
00227         visualization_msgs::InteractiveMarker int_marker;
00228         get(assoc_it->second.parent_marker_name, int_marker);
00229         renewPoseWithParent(assoc_it, int_marker.pose, int_marker.header);
00230       }
00231       ++assoc_it;
00232     }
00233     // apply change
00234     interactive_markers::InteractiveMarkerServer::applyChanges();
00235     // check self parent
00236     // renew all relative affine
00237     assoc_it = association_list_.begin();
00238     while (assoc_it != association_list_.end()){
00239       if (topic_server_name_ == assoc_it->second.parent_topic_name)
00240       {
00241         geometry_msgs::PoseStamped feedback_pose_stamped;
00242         visualization_msgs::InteractiveMarker int_marker;
00243         get(assoc_it->first, int_marker);
00244         feedback_pose_stamped.pose = int_marker.pose; feedback_pose_stamped.header = int_marker.header;
00245         registerAssociationItself(assoc_it->second.parent_marker_name, assoc_it->second.parent_topic_name, assoc_it->first, feedback_pose_stamped);
00246       }
00247       ++assoc_it;
00248     }
00249   }
00250   bool ParentAndChildInteractiveMarkerServer::setCallback(const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type)
00251   {
00252     // synthesize cbs
00253     callback_map_[name] = std::make_shared<FeedbackSynthesizer> (boost::bind(&ParentAndChildInteractiveMarkerServer::selfFeedbackCb, this, _1), feedback_cb);
00254     return interactive_markers::InteractiveMarkerServer::setCallback(name, boost::bind(&FeedbackSynthesizer::call_func, *callback_map_[name], _1), feedback_type);
00255   }
00256   void ParentAndChildInteractiveMarkerServer::insert(const visualization_msgs::InteractiveMarker &int_marker)
00257   {
00258     callback_map_.erase(int_marker.name);
00259     removeParent(int_marker.name);
00260     interactive_markers::InteractiveMarkerServer::insert(int_marker);
00261   }
00262   void ParentAndChildInteractiveMarkerServer::insert(const visualization_msgs::InteractiveMarker &int_marker, FeedbackCallback feedback_cb, uint8_t feedback_type)
00263   {
00264     insert(int_marker);
00265     // synthesize cbs
00266     ParentAndChildInteractiveMarkerServer::setCallback(int_marker.name, feedback_cb, feedback_type);
00267   }
00268   bool ParentAndChildInteractiveMarkerServer::erase(const std::string &name)
00269   {
00270     // erase cb
00271     callback_map_.erase(name);
00272     removeParent(name);
00273     return interactive_markers::InteractiveMarkerServer::erase(name);
00274   }
00275 }