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
00019 getMarkerPose(req.child_marker_name, child_pose_stamped);
00020 if (req.parent_topic_name == std::string("") || topic_server_name_ == req.parent_topic_name)
00021 {
00022 if (!registerAssociationItself(req.parent_marker_name, topic_server_name_, req.child_marker_name, child_pose_stamped))
00023 return false;
00024 }
00025 else{
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
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
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
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
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
00164 if (need_apply_change)
00165 interactive_markers::InteractiveMarkerServer::applyChanges();
00166
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)
00178 {
00179
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
00190 if (need_apply_change){
00191 interactive_markers::InteractiveMarkerServer::applyChanges();
00192 }
00193
00194
00195
00196
00197 if (feedback -> event_type != visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP)
00198 {
00199 return;
00200 }
00201
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()
00221 {
00222
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
00234 interactive_markers::InteractiveMarkerServer::applyChanges();
00235
00236
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
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
00266 ParentAndChildInteractiveMarkerServer::setCallback(int_marker.name, feedback_cb, feedback_type);
00267 }
00268 bool ParentAndChildInteractiveMarkerServer::erase(const std::string &name)
00269 {
00270
00271 callback_map_.erase(name);
00272 removeParent(name);
00273 return interactive_markers::InteractiveMarkerServer::erase(name);
00274 }
00275 }