00001 #ifndef _URDF_MODEL_MARKER_H_
00002 #define _URDF_MODEL_MARKER_H_
00003
00004
00005 #include <ros/ros.h>
00006
00007 #include <tf/tf.h>
00008 #include <tf/transform_listener.h>
00009 #include <tf/transform_broadcaster.h>
00010
00011 #include <interactive_markers/interactive_marker_server.h>
00012
00013 #include <interactive_markers/menu_handler.h>
00014 #include <jsk_interactive_marker/SetPose.h>
00015 #include <jsk_interactive_marker/MarkerSetPose.h>
00016
00017 #include <math.h>
00018 #include <jsk_interactive_marker/MarkerMenu.h>
00019 #include <jsk_interactive_marker/MarkerPose.h>
00020 #include <jsk_interactive_marker/MoveObject.h>
00021
00022 #include <std_msgs/Int8.h>
00023 #include <std_msgs/String.h>
00024 #include <sensor_msgs/JointState.h>
00025
00026 #include "urdf_parser/urdf_parser.h"
00027 #include <iostream>
00028 #include <fstream>
00029
00030 #include <diagnostic_updater/diagnostic_updater.h>
00031 #include <diagnostic_updater/publisher.h>
00032
00033 #include <jsk_pcl_ros/Int32Stamped.h>
00034
00035 #include <jsk_topic_tools/time_accumulator.h>
00036
00037 using namespace urdf;
00038 using namespace std;
00039
00040
00041 class UrdfModelMarker {
00042 public:
00043
00044 UrdfModelMarker(string file, boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server);
00045 UrdfModelMarker(string model_name, string model_file, string frame_id, geometry_msgs::PoseStamped root_pose, geometry_msgs::Pose root_offset, double scale_factor, string mode, bool robot_mode, bool registration, string fixed_link, bool use_robot_description, bool use_visible_color, map<string, double> initial_pose_map, int index, boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server);
00046 UrdfModelMarker();
00047
00048 void addMoveMarkerControl(visualization_msgs::InteractiveMarker &int_marker, boost::shared_ptr<const Link> link, bool root);
00049 void addInvisibleMeshMarkerControl(visualization_msgs::InteractiveMarker &int_marker, boost::shared_ptr<const Link> link, const std_msgs::ColorRGBA &color);
00050 void addGraspPointControl(visualization_msgs::InteractiveMarker &int_marker, std::string link_frame_name_);
00051
00052 void publishBasePose( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00053 void publishBasePose( geometry_msgs::Pose pose, std_msgs::Header header);
00054 void publishMarkerPose ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
00055 void publishMarkerPose ( geometry_msgs::Pose pose, std_msgs::Header header, std::string marker_name);
00056 void publishMarkerMenu( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int menu );
00057 void publishMoveObject( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00058 void publishJointState( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00059 void republishJointState( sensor_msgs::JointState js);
00060
00061 void proc_feedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, string parent_frame_id, string frame_id);
00062
00063 void graspPoint_feedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, string link_name);
00064 void moveCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00065 void setPoseCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00066 void hideMarkerCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00067 void hideAllMarkerCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00068
00069 void hideModelMarkerCB( const std_msgs::EmptyConstPtr &msg);
00070 void showModelMarkerCB( const std_msgs::EmptyConstPtr &msg);
00071 void setUrdfCB( const std_msgs::StringConstPtr &msg);
00072
00073 visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource, const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale, const std_msgs::ColorRGBA &color, bool use_color);
00074 visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource, const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale);
00075 visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource, const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale, const std_msgs::ColorRGBA &color);
00076
00077
00078
00079 visualization_msgs::InteractiveMarkerControl makeCylinderMarkerControl(const geometry_msgs::PoseStamped &stamped, double length, double radius, const std_msgs::ColorRGBA &color, bool use_color);
00080 visualization_msgs::InteractiveMarkerControl makeBoxMarkerControl(const geometry_msgs::PoseStamped &stamped, Vector3 dim, const std_msgs::ColorRGBA &color, bool use_color);
00081 visualization_msgs::InteractiveMarkerControl makeSphereMarkerControl(const geometry_msgs::PoseStamped &stamped, double rad, const std_msgs::ColorRGBA &color, bool use_color);
00082
00083 void getJointState(boost::shared_ptr<const Link> link, sensor_msgs::JointState &js);
00084 void setJointState(boost::shared_ptr<const Link> link, const sensor_msgs::JointStateConstPtr &js);
00085 void setJointAngle(boost::shared_ptr<const Link> link, double joint_angle);
00086 geometry_msgs::Pose getRootPose(geometry_msgs::Pose pose);
00087 geometry_msgs::PoseStamped getOriginPoseStamped();
00088 void setOriginalPose(boost::shared_ptr<const Link> link);
00089 void addChildLinkNames(boost::shared_ptr<const Link> link, bool root, bool init);
00090 void addChildLinkNames(boost::shared_ptr<const Link> link, bool root, bool init, bool use_color, int color_index);
00091
00092 geometry_msgs::Transform Pose2Transform(geometry_msgs::Pose pose_msg);
00093 geometry_msgs::Pose Transform2Pose( const geometry_msgs::Transform tf_msg);
00094 geometry_msgs::Pose UrdfPose2Pose( const urdf::Pose pose);
00095 void CallSetDynamicTf(string parent_frame_id, string frame_id, geometry_msgs::Transform transform);
00096
00097 int main(string file);
00098
00099 void graspPointCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00100 void jointMoveCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00101 void resetMarkerCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00102 void resetBaseMarkerCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00103 void resetBaseMsgCB( const std_msgs::EmptyConstPtr &msg);
00104 void resetBaseCB();
00105
00106 void resetRobotBase();
00107 void registrationCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00108
00109 void setRootPoseCB( const geometry_msgs::PoseStampedConstPtr &msg );
00110 void setRootPose( geometry_msgs::PoseStamped ps);
00111 void resetJointStatesCB( const sensor_msgs::JointStateConstPtr &msg);
00112 void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
00113
00114
00115 protected:
00116 ros::NodeHandle nh_;
00117 ros::NodeHandle pnh_;
00118 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server_;
00119
00120
00121 boost::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_;
00122 jsk_topic_tools::TimeAccumulator reset_joint_states_check_time_acc_;
00123 jsk_topic_tools::TimeAccumulator dynamic_tf_check_time_acc_;
00124
00125 ros::Publisher pub_;
00126 ros::Publisher pub_move_;
00127 ros::Publisher pub_move_object_;
00128 ros::Publisher pub_joint_state_;
00129 ros::Publisher pub_base_pose_;
00130 ros::Publisher pub_selected_;
00131 ros::Publisher pub_selected_index_;
00132
00133
00134 ros::Subscriber sub_reset_joints_;
00135 ros::Subscriber sub_set_root_pose_;
00136 ros::Subscriber hide_marker_;
00137 ros::Subscriber show_marker_;
00138 ros::Subscriber sub_set_urdf_;
00139
00140 ros::ServiceServer serv_reset_;
00141 ros::ServiceServer serv_set_;
00142 ros::ServiceServer serv_markers_set_;
00143 ros::ServiceServer serv_markers_del_;
00144
00145 interactive_markers::MenuHandler model_menu_;
00146
00147 tf::TransformListener tfl_;
00148 tf::TransformBroadcaster tfb_;
00149
00150 ros::ServiceClient dynamic_tf_publisher_client;
00151
00152 std::string marker_name;
00153 std::string server_name;
00154 std::string base_frame;
00155 std::string move_base_frame;
00156 std::string target_frame;
00157
00158
00159 boost::shared_ptr<ModelInterface> model;
00160 std::string model_name_;
00161 std::string frame_id_;
00162 std::string model_file_;
00163 geometry_msgs::Pose root_pose_;
00164 geometry_msgs::Pose root_offset_;
00165 double scale_factor_;
00166 bool robot_mode_;
00167 bool registration_;
00168 bool use_dynamic_tf_;
00169 string mode_;
00170 string fixed_link_;
00171 bool use_robot_description_;
00172 bool use_visible_color_;
00173 std::string tf_prefix_;
00174 map<string, double> initial_pose_map_;
00175 int index_;
00176 ros::Time init_stamp_;
00177
00178 struct graspPoint{
00179 graspPoint(){
00180 displayMoveMarker = false;
00181 displayGraspPoint = false;
00182 pose.orientation.x = 0;
00183 pose.orientation.y = 0;
00184 pose.orientation.z = 0;
00185 pose.orientation.w = 1;
00186 }
00187 bool displayGraspPoint;
00188 bool displayMoveMarker;
00189 geometry_msgs::Pose pose;
00190 };
00191
00192
00193 struct linkProperty{
00194 linkProperty(){
00195 displayMoveMarker = false;
00196 displayModelMarker = true;
00197 mesh_file = "";
00198 }
00199 bool displayMoveMarker;
00200 bool displayModelMarker;
00201 graspPoint gp;
00202 string frame_id;
00203 string movable_link;
00204
00205 geometry_msgs::Pose pose;
00206 geometry_msgs::Pose origin;
00207 geometry_msgs::Pose initial_pose;
00208 urdf::Vector3 joint_axis;
00209 double joint_angle;
00210 int rotation_count;
00211 string mesh_file;
00212 };
00213
00214 map<string, linkProperty> linkMarkerMap;
00215 };
00216
00217 geometry_msgs::Pose getPose( XmlRpc::XmlRpcValue val);
00218 double getXmlValue( XmlRpc::XmlRpcValue val );
00219
00220 #endif