8 std::string get_marker_pose_service_name = topic_ns +
"/get_marker_pose";
9 std::string set_parent_service_name = topic_ns +
"/set_parent_marker";
10 std::string remove_parent_service_name = topic_ns +
"/remove_parent_marker";
17 geometry_msgs::PoseStamped child_pose_stamped;
20 if (req.parent_topic_name == std::string(
"") ||
topic_server_name_ == req.parent_topic_name)
58 geometry_msgs::PoseStamped parent_pose_stamped;
60 return registerAssociation(parent_marker_name, parent_topic_name, child_marker_name, child_pose_stamped, parent_pose_stamped);
64 geometry_msgs::PoseStamped parent_pose_stamped;
66 jsk_interactive_marker::GetTransformableMarkerPose
srv;
67 srv.request.target_name = parent_marker_name;
72 parent_pose_stamped =
srv.response.pose_stamped;
73 return registerAssociation(parent_marker_name, parent_topic_name, child_marker_name, child_pose_stamped, parent_pose_stamped);
77 geometry_msgs::PoseStamped parent_pose_stamped_transed;
79 parent_pose_stamped.header.frame_id, parent_pose_stamped.header.stamp,
ros::Duration(1.0)))
87 Eigen::Affine3d parent_pose_eigened, child_pose_eigened;
100 std::string parent_topic_name =
association_list_[child_marker_name].parent_topic_name;
114 geometry_msgs::PoseStamped pose_stamped;
117 res.pose_stamped = pose_stamped;
124 visualization_msgs::InteractiveMarker focus_marker;
125 if (
get(target_name, focus_marker))
127 pose_stamped.pose = focus_marker.pose;
128 pose_stamped.header = focus_marker.header;
136 bool need_apply_change =
false;
137 std::map <std::string, ParentMarkerInformation>::iterator assoc_it =
association_list_.begin();
139 for (
size_t i=0; i<
update->markers.size(); i++){
140 if (assoc_it -> second.parent_topic_name == parent_topic_name && assoc_it -> second.parent_marker_name ==
update -> markers[i].name){
142 need_apply_change =
true;
148 if (need_apply_change)
154 bool need_apply_change =
false;
155 std::map <std::string, ParentMarkerInformation>::iterator assoc_it =
association_list_.begin();
157 if (assoc_it -> second.parent_topic_name == parent_topic_name && assoc_it -> second.parent_marker_name == feedback -> marker_name){
159 need_apply_change =
true;
164 if (need_apply_change)
170 Eigen::Affine3d parent_pose_eigened, child_new_pose_eigened;
172 child_new_pose_eigened = parent_pose_eigened * assoc_it -> second.relative_pose;
173 geometry_msgs::Pose child_new_pose;
175 setPose(assoc_it -> first, child_new_pose, parent_header);
180 bool need_apply_change =
false;
181 std::map <std::string, ParentMarkerInformation>::iterator assoc_it =
association_list_.begin();
183 if (assoc_it -> second.parent_topic_name ==
topic_server_name_ && assoc_it -> second.parent_marker_name == feedback -> marker_name){
185 need_apply_change =
true;
190 if (need_apply_change){
197 if (feedback -> event_type != visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP)
202 geometry_msgs::PoseStamped feedback_pose_stamped;
203 feedback_pose_stamped.pose = feedback->pose;
204 feedback_pose_stamped.header = feedback->header;
207 if (assoc_it->first == feedback->marker_name){
210 registerAssociationItself(assoc_it->second.parent_marker_name, assoc_it->second.parent_topic_name, assoc_it->first, feedback_pose_stamped);
223 std::map <std::string, ParentMarkerInformation>::iterator assoc_it =
association_list_.begin();
227 visualization_msgs::InteractiveMarker int_marker;
228 get(assoc_it->second.parent_marker_name, int_marker);
241 geometry_msgs::PoseStamped feedback_pose_stamped;
242 visualization_msgs::InteractiveMarker int_marker;
243 get(assoc_it->first, int_marker);
244 feedback_pose_stamped.pose = int_marker.pose; feedback_pose_stamped.header = int_marker.header;
245 registerAssociationItself(assoc_it->second.parent_marker_name, assoc_it->second.parent_topic_name, assoc_it->first, feedback_pose_stamped);