Go to the documentation of this file.00001 #ifndef __TRANSFORMABLE_INTERACTIVE_SERVER_H__
00002 #define __TRANSFORMABLE_INTERACTIVE_SERVER_H__
00003
00004 #include <ros/ros.h>
00005 #include <interactive_markers/interactive_marker_server.h>
00006 #include <jsk_interactive_marker/transformable_object.h>
00007 #include <jsk_interactive_marker/yaml_menu_handler.h>
00008 #include <jsk_interactive_marker/parent_and_child_interactive_marker_server.h>
00009 #include <std_msgs/Float32.h>
00010 #include <std_msgs/String.h>
00011 #include <std_srvs/Empty.h>
00012 #include <geometry_msgs/PoseStamped.h>
00013 #include <map>
00014 #include <jsk_rviz_plugins/OverlayText.h>
00015 #include <iostream>
00016 #include <sstream>
00017 #include <tf/transform_listener.h>
00018 #include <dynamic_reconfigure/server.h>
00019 #include <jsk_interactive_marker/InteractiveSettingConfig.h>
00020 #include <jsk_interactive_marker/GetTransformableMarkerPose.h>
00021 #include <jsk_interactive_marker/SetTransformableMarkerPose.h>
00022 #include <jsk_interactive_marker/GetTransformableMarkerColor.h>
00023 #include <jsk_interactive_marker/SetTransformableMarkerColor.h>
00024 #include <jsk_interactive_marker/GetTransformableMarkerFocus.h>
00025 #include <jsk_interactive_marker/SetTransformableMarkerFocus.h>
00026 #include <jsk_interactive_marker/GetMarkerDimensions.h>
00027 #include <jsk_interactive_marker/SetMarkerDimensions.h>
00028 #include <jsk_interactive_marker/GetType.h>
00029 #include <jsk_interactive_marker/GetTransformableMarkerExistence.h>
00030 #include <jsk_interactive_marker/MarkerDimensions.h>
00031 #include <jsk_interactive_marker/PoseStampedWithName.h>
00032
00033 using namespace std;
00034
00035 namespace jsk_interactive_marker
00036 {
00037 class TransformableInteractiveServer{
00038 public:
00039 TransformableInteractiveServer();
00040 ~TransformableInteractiveServer();
00041
00042 void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00043 void setRadius(std_msgs::Float32 msg);
00044 void setSmallRadius(std_msgs::Float32 msg);
00045 void setX(std_msgs::Float32 msg);
00046 void setY(std_msgs::Float32 msg);
00047 void setZ(std_msgs::Float32 msg);
00048
00049 void setPose( const geometry_msgs::PoseStampedConstPtr &msg_ptr , bool for_interactive_control=false);
00050 void addPose(geometry_msgs::Pose msg);
00051 void addPoseRelative(geometry_msgs::Pose msg);
00052
00053 void setControlRelativePose(geometry_msgs::Pose msg);
00054
00055 void setColor(std_msgs::ColorRGBA msg);
00056
00057 void insertNewBox( std::string frame_id, std::string name, std::string description );
00058 void insertNewCylinder( std::string frame_id, std::string name, std::string description );
00059 void insertNewTorus( std::string frame_id, std::string name, std::string description );
00060 void insertNewMesh( std::string frame_id, std::string name, std::string description , std::string mesh_resource, bool mesh_use_embedded_materials);
00061
00062 void insertNewObject(TransformableObject* tobject, std::string name);
00063 void eraseObject(std::string name);
00064 void eraseAllObject();
00065 void eraseFocusObject();
00066
00067 void run();
00068 void focusTextPublish();
00069 void focusPosePublish();
00070 void focusObjectMarkerNamePublish();
00071 void focusInteractiveManipulatorDisplay();
00072
00073 void enableInteractiveManipulatorDisplay(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback,
00074 const bool enable);
00075
00076 void updateTransformableObject(TransformableObject* tobject);
00077
00078 bool getPoseService(jsk_interactive_marker::GetTransformableMarkerPose::Request &req,jsk_interactive_marker::GetTransformableMarkerPose::Response &res, bool for_interactive_control);
00079 bool setPoseService(jsk_interactive_marker::SetTransformableMarkerPose::Request &req,jsk_interactive_marker::SetTransformableMarkerPose::Response &res, bool for_interactive_control);
00080 bool getColorService(jsk_interactive_marker::GetTransformableMarkerColor::Request &req,jsk_interactive_marker::GetTransformableMarkerColor::Response &res);
00081 bool setColorService(jsk_interactive_marker::SetTransformableMarkerColor::Request &req,jsk_interactive_marker::SetTransformableMarkerColor::Response &res);
00082 bool getFocusService(jsk_interactive_marker::GetTransformableMarkerFocus::Request &req,jsk_interactive_marker::GetTransformableMarkerFocus::Response &res);
00083 bool setFocusService(jsk_interactive_marker::SetTransformableMarkerFocus::Request &req,jsk_interactive_marker::SetTransformableMarkerFocus::Response &res);
00084 bool getTypeService(jsk_interactive_marker::GetType::Request &req,jsk_interactive_marker::GetType::Response &res);
00085 bool getExistenceService(jsk_interactive_marker::GetTransformableMarkerExistence::Request &req,jsk_interactive_marker::GetTransformableMarkerExistence::Response &res);
00086 bool setDimensionsService(jsk_interactive_marker::SetMarkerDimensions::Request &req,jsk_interactive_marker::SetMarkerDimensions::Response &res);
00087 bool getDimensionsService(jsk_interactive_marker::GetMarkerDimensions::Request &req,jsk_interactive_marker::GetMarkerDimensions::Response &res);
00088 bool hideService(std_srvs::Empty::Request& req,
00089 std_srvs::Empty::Response& res);
00090 bool showService(std_srvs::Empty::Request& req,
00091 std_srvs::Empty::Response& res);
00092 void publishMarkerDimensions();
00093
00094 bool requestMarkerOperateService(jsk_rviz_plugins::RequestMarkerOperate::Request &req,jsk_rviz_plugins::RequestMarkerOperate::Response &res);
00095 virtual void configCallback(InteractiveSettingConfig &config, uint32_t level);
00096 void SetInitialInteractiveMarkerConfig( TransformableObject* tobject );
00097
00098 void tfTimerCallback(const ros::TimerEvent&);
00099 bool setPoseWithTfTransformation(TransformableObject* tobject, geometry_msgs::PoseStamped pose_stamped, bool for_interactive_control=false);
00100
00101 std::string focus_object_marker_name_;
00102 ros::NodeHandle* n_;
00103
00104 boost::mutex mutex_;
00105
00106 ros::Subscriber setcolor_sub_;
00107 ros::Subscriber setpose_sub_;
00108 ros::Subscriber setcontrolpose_sub_;
00109 ros::Subscriber addpose_sub_;
00110 ros::Subscriber addpose_relative_sub_;
00111
00112 ros::Subscriber setcontrol_relative_sub_;
00113
00114 ros::Subscriber set_r_sub_;
00115 ros::Subscriber set_sm_r_sub_;
00116 ros::Subscriber set_h_sub_;
00117 ros::Subscriber set_x_sub_;
00118 ros::Subscriber set_y_sub_;
00119 ros::Subscriber set_z_sub_;
00120
00121 ros::ServiceServer hide_srv_;
00122 ros::ServiceServer show_srv_;
00123 ros::ServiceServer get_pose_srv_;
00124 ros::ServiceServer get_control_pose_srv_;
00125 ros::ServiceServer set_pose_srv_;
00126 ros::ServiceServer set_control_pose_srv_;
00127 ros::ServiceServer get_color_srv_;
00128 ros::ServiceServer set_color_srv_;
00129 ros::ServiceServer get_focus_srv_;
00130 ros::ServiceServer set_focus_srv_;
00131 ros::ServiceServer get_type_srv_;
00132 ros::ServiceServer get_exist_srv_;
00133 ros::ServiceServer set_dimensions_srv;
00134 ros::ServiceServer get_dimensions_srv;
00135 ros::Publisher marker_dimensions_pub_;
00136 ros::ServiceServer request_marker_operate_srv_;
00137
00138 std::shared_ptr <dynamic_reconfigure::Server<InteractiveSettingConfig> > config_srv_;
00139
00140 ros::Subscriber setrad_sub_;
00141 ros::Publisher focus_name_text_pub_;
00142 ros::Publisher focus_pose_text_pub_;
00143 ros::Publisher focus_object_marker_name_pub_;
00144 ros::Publisher pose_pub_;
00145 ros::Publisher pose_with_name_pub_;
00146 interactive_markers::InteractiveMarkerServer* server_;
00147 map<string, TransformableObject*> transformable_objects_map_;
00148 std::shared_ptr<tf::TransformListener> tf_listener_;
00149 int torus_udiv_;
00150 int torus_vdiv_;
00151 jsk_interactive_marker::InteractiveSettingConfig config_;
00152 bool strict_tf_;
00153 int interactive_manipulator_orientation_;
00154 ros::Timer tf_timer;
00155 std::shared_ptr <YamlMenuHandler> yaml_menu_handler_ptr_;
00156 };
00157 }
00158
00159 #endif