Go to the documentation of this file.00001 #ifndef JSK_INTERACTIVE_MARKER_PARENT_AND_CHILD_INTERACTIVE_MARKER_SERVER_H_
00002 #define JSK_INTERACTIVE_MARKER_PARENT_AND_CHILD_INTERACTIVE_MARKER_SERVER_H_
00003
00004 #include <interactive_markers/interactive_marker_server.h>
00005 #include <std_srvs/Empty.h>
00006 #include <jsk_interactive_marker/GetTransformableMarkerPose.h>
00007 #include <jsk_interactive_marker/SetParentMarker.h>
00008 #include <jsk_interactive_marker/RemoveParentMarker.h>
00009 #include <tf/transform_listener.h>
00010 #include <eigen_conversions/eigen_msg.h>
00011 #include <boost/shared_ptr.hpp>
00012
00013 namespace jsk_interactive_marker
00014 {
00015 class ParentMarkerInformation{
00016 public:
00017 ParentMarkerInformation(std::string p_t_n, std::string p_m_n, Eigen::Affine3d r_p)
00018 {
00019 parent_topic_name = p_t_n;
00020 parent_marker_name = p_m_n;
00021 relative_pose = r_p;
00022 }
00023 ParentMarkerInformation()
00024 {
00025 }
00026 std::string parent_topic_name;
00027 std::string parent_marker_name;
00028 Eigen::Affine3d relative_pose;
00029 };
00030
00031 class ParentAndChildInteractiveMarkerServer: public interactive_markers::InteractiveMarkerServer
00032 {
00033 public:
00034 class FeedbackSynthesizer{
00035 public:
00036 FeedbackSynthesizer(FeedbackCallback c1, FeedbackCallback c2) {cb1=c1; cb2=c2;}
00037 FeedbackCallback cb1, cb2;
00038 void call_func(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {if(cb1) cb1(feedback); if(cb2) cb2(feedback);}
00039 };
00040
00041 ParentAndChildInteractiveMarkerServer(const std::string &topic_ns, const std::string &server_id="", bool spin_thread = false);
00042 bool setParentService(jsk_interactive_marker::SetParentMarker::Request &req, jsk_interactive_marker::SetParentMarker::Response &res);
00043 void renewPoseWithParent(std::map <std::string, ParentMarkerInformation>::iterator assoc_it_, geometry_msgs::Pose parent_pose, std_msgs::Header parent_header);
00044 bool removeParentService(jsk_interactive_marker::RemoveParentMarker::Request &req, jsk_interactive_marker::RemoveParentMarker::Response &res);
00045 bool removeParent(std::string child_marker_name);
00046 bool registerAssociationItself(std::string parent_marker_name, std::string parent_topic_name, std::string child_marker_name, geometry_msgs::PoseStamped child_pose_stamped);
00047 bool registerAssociationWithOtherNode(std::string parent_marker_name, std::string parent_topic_name, std::string child_marker_name, geometry_msgs::PoseStamped child_pose_stamped);
00048 bool 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);
00049 bool getMarkerPose(std::string target_name, geometry_msgs::PoseStamped &pose_stamped);
00050 bool getMarkerPoseService(jsk_interactive_marker::GetTransformableMarkerPose::Request &req,jsk_interactive_marker::GetTransformableMarkerPose::Response &res);
00051 geometry_msgs::Pose getRelativePose(geometry_msgs::PoseStamped parent_pose_stamped, geometry_msgs::PoseStamped child_pose_stamped);
00052 void selfFeedbackCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
00053 void parentUpdateCb(const visualization_msgs::InteractiveMarkerUpdateConstPtr &update, std::string parent_topic_name);
00054 void parentFeedbackCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, std::string parent_topic_name);
00055
00056 void applyChanges();
00057 bool setCallback(const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type=DEFAULT_FEEDBACK_CB);
00058 void insert(const visualization_msgs::InteractiveMarker &int_marker);
00059 void insert(const visualization_msgs::InteractiveMarker &int_marker, FeedbackCallback feedback_cb, uint8_t feedback_type=DEFAULT_FEEDBACK_CB);
00060 bool erase(const std::string &name);
00061 ros::NodeHandle n_;
00062
00063 ros::ServiceServer set_parent_srv_;
00064 ros::ServiceServer remove_parent_srv_;
00065 ros::ServiceServer get_marker_pose_srv_;
00066 tf::TransformListener tf_listener_;
00067 std::string topic_server_name_;
00068
00069 std::map <std::string, ParentMarkerInformation> association_list_;
00070
00071
00072 std::map <std::string, ros::Subscriber> parent_update_subscribers_;
00073 std::map <std::string, ros::Subscriber> parent_feedback_subscribers_;
00074 std::map <std::string, std::shared_ptr <FeedbackSynthesizer> > callback_map_;
00075 std::map <std::string, int> parent_subscriber_nums_;
00076 };
00077 }
00078
00079 #endif