00001 #include <ros/ros.h> 00002 #include <interactive_markers/interactive_marker_server.h> 00003 00004 #include <interactive_markers/menu_handler.h> 00005 #include <jsk_interactive_marker/SetPose.h> 00006 #include <jsk_interactive_marker/MarkerSetPose.h> 00007 00008 #include <geometry_msgs/PoseStamped.h> 00009 #include <std_msgs/Empty.h> 00010 #include <std_msgs/Float32.h> 00011 00012 00013 class PointCloudConfigMarker{ 00014 public: 00015 struct MarkerControlConfig{ 00016 MarkerControlConfig(): marker_id(0), resolution_(0.05){ 00017 00018 } 00019 MarkerControlConfig(double s): marker_id(0), resolution_(0.05){ 00020 size.x = s; 00021 size.y = s; 00022 size.z = s; 00023 } 00024 geometry_msgs::Pose pose; 00025 geometry_msgs::Vector3 size; 00026 int marker_id; 00027 double resolution_; 00028 }; 00029 00030 visualization_msgs::Marker makeBoxMarker(geometry_msgs::Vector3 size); 00031 visualization_msgs::Marker makeTextMarker(geometry_msgs::Vector3 size); 00032 visualization_msgs::InteractiveMarker makeBoxInteractiveMarker(MarkerControlConfig mconfig, std::string name); 00033 void moveBoxCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00034 00035 visualization_msgs::Marker makeMarkerMsg( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00036 00037 void publishMarkerMsg( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00038 void cancelCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00039 void clearCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00040 void clearBoxCB( const std_msgs::Empty::ConstPtr &msg); 00041 void clearBox(); 00042 00043 void changeResolutionCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00044 void changeBoxSize(geometry_msgs::Vector3 size); 00045 void changeBoxSizeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00046 00047 00048 void updateBoxInteractiveMarker(); 00049 void changeBoxResolution(const std_msgs::Float32::ConstPtr &msg); 00050 void publishCurrentPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00051 void publishCurrentPose(const geometry_msgs::PoseStamped::ConstPtr &pose); 00052 void updatePoseCB(const geometry_msgs::PoseStamped::ConstPtr &pose); 00053 void addBoxCB(const std_msgs::Empty::ConstPtr &msg); 00054 interactive_markers::MenuHandler makeMenuHandler(); 00055 PointCloudConfigMarker (); 00056 private: 00057 ros::NodeHandle nh_; 00058 ros::NodeHandle pnh_; 00059 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server_; 00060 visualization_msgs::InteractiveMarkerFeedbackConstPtr latest_feedback_; 00061 ros::Publisher pub_; 00062 ros::Publisher current_pose_pub_; 00063 00064 00065 ros::Subscriber pose_update_sub_; 00066 ros::Subscriber add_box_sub_; 00067 ros::Subscriber clear_box_sub_; 00068 ros::Subscriber change_box_size_sub_; 00069 ros::Subscriber change_box_resolution_sub_; 00070 00071 interactive_markers::MenuHandler menu_handler; 00072 interactive_markers::MenuHandler::EntryHandle resolution_menu_; 00073 interactive_markers::MenuHandler::EntryHandle checked_resolution_menu_; 00074 interactive_markers::MenuHandler::EntryHandle resolution_20cm_menu_; 00075 interactive_markers::MenuHandler::EntryHandle resolution_10cm_menu_; 00076 interactive_markers::MenuHandler::EntryHandle resolution_5cm_menu_; 00077 00078 interactive_markers::MenuHandler::EntryHandle box_size_menu_; 00079 interactive_markers::MenuHandler::EntryHandle checked_box_size_menu_; 00080 interactive_markers::MenuHandler::EntryHandle box_size_100_menu_; 00081 interactive_markers::MenuHandler::EntryHandle box_size_50_menu_; 00082 interactive_markers::MenuHandler::EntryHandle box_size_25_menu_; 00083 00084 00085 std::string server_name; 00086 std::string marker_name; 00087 std::string base_frame; 00088 double size_; 00089 MarkerControlConfig marker_control_config; 00090 };