Go to the documentation of this file.00001 #include "urdf_parser/urdf_parser.h"
00002 #include <iostream>
00003 #include <memory>
00004 #include <interactive_markers/tools.h>
00005 #include <jsk_interactive_marker/urdf_model_marker.h>
00006 #include <jsk_interactive_marker/interactive_marker_utils.h>
00007 #include <jsk_interactive_marker/interactive_marker_helpers.h>
00008 #include <geometry_msgs/PoseArray.h>
00009
00010 using namespace urdf;
00011 using namespace std;
00012
00013 class UrdfModelSettings {
00014 private:
00015 ros::NodeHandle pnh_;
00016 ros::Subscriber display_marker_sub_;
00017 XmlRpc::XmlRpcValue model_config_;
00018 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server_;
00019 string model_name_;
00020 double scale_factor_;
00021 geometry_msgs::PoseStamped pose_stamped_;
00022 geometry_msgs::Pose root_offset_;
00023 bool use_visible_color_;
00024 string mode_;
00025 string model_file_;
00026 bool registration_;
00027 string fixed_link_;
00028 string frame_id_;
00029 bool use_robot_description_;
00030 bool robot_mode_;
00031 bool display_;
00032 map<string, double> initial_pose_map_;
00033
00034 typedef boost::shared_ptr<UrdfModelMarker> umm_ptr;
00035 typedef vector<umm_ptr> umm_vec;
00036 umm_vec umm_vec_;
00037
00038 public:
00039 void displayMarkerArrayCB( const geometry_msgs::PoseArrayConstPtr &msg){
00040 std_msgs::Header header = msg->header;
00041 geometry_msgs::PoseStamped ps;
00042 ps.header = header;
00043
00044 int msg_size = msg->poses.size();
00045 int umm_vec_size = umm_vec_.size();
00046
00047 if(msg_size <= umm_vec_size){
00048 for (int i = 0; i < msg_size; i++){
00049
00050 ps.pose = msg->poses[i];
00051 umm_vec_[i]->setRootPose(ps);
00052 }
00053 for (int i = msg_size; i< umm_vec_size; i++){
00054
00055 ps.pose.position.x = ps.pose.position.y = ps.pose.position.z = 1000000;
00056 ps.pose.orientation.w = 1;
00057 umm_vec_[i]->setRootPose(ps);
00058 }
00059 }else{
00060 for (int i = 0; i < umm_vec_size; i++){
00061
00062 ps.pose = msg->poses[i];
00063 umm_vec_[i]->setRootPose(ps);
00064 }
00065 for (int i = umm_vec_size; i < msg_size; i++){
00066
00067 ps.pose = msg->poses[i];
00068 umm_vec_.push_back(umm_ptr(new UrdfModelMarker(model_name_, model_file_, header.frame_id, ps, root_offset_, scale_factor_, mode_ , robot_mode_, registration_,fixed_link_, use_robot_description_, use_visible_color_, initial_pose_map_, i, server_)));
00069 }
00070 }
00071 }
00072
00073 void init(){
00074
00075 model_name_.assign(model_config_["name"]);
00076
00077 scale_factor_ = 1.02;
00078 if(model_config_.hasMember("scale")){
00079 scale_factor_ = getXmlValue(model_config_["scale"]);
00080 }
00081
00082 if(model_config_.hasMember("pose")){
00083 pose_stamped_.pose = getPose(model_config_["pose"]);
00084 }else{
00085 pose_stamped_.pose = geometry_msgs::Pose();
00086 pose_stamped_.pose.orientation.w = 1.0;
00087 }
00088 pose_stamped_.header.stamp = ros::Time::now();
00089
00090 if(model_config_.hasMember("offset")){
00091 root_offset_ = getPose(model_config_["offset"]);
00092 }else{
00093 root_offset_ = geometry_msgs::Pose();
00094 root_offset_.orientation.w = 1.0;
00095 }
00096
00097
00098 use_visible_color_ = false;
00099 if(model_config_.hasMember("use_visible_color")){
00100 use_visible_color_ = model_config_["use_visible_color"];
00101 }
00102
00103 frame_id_.assign(model_config_["frame-id"]);
00104
00105
00106 mode_ = "model";
00107 model_file_ = "";
00108 registration_ = false;
00109 fixed_link_ = "";
00110 if(model_config_.hasMember("registration")){
00111 registration_ = model_config_["registration"];
00112 mode_ = "registration";
00113 if(model_config_.hasMember("fixed-link")){
00114 fixed_link_.assign( model_config_["fixed-link"]);
00115 }
00116 }
00117 if(model_config_.hasMember("model")){
00118 model_file_.assign(model_config_["model"]);
00119 }
00120 use_robot_description_ = false;
00121 if(model_config_.hasMember("use_robot_description")){
00122 model_file_ = "/robot_description";
00123 use_robot_description_ = model_config_["use_robot_description"];
00124 }
00125 if(model_config_.hasMember("model_param")){
00126 use_robot_description_ = true;
00127 model_file_.assign(model_config_["model_param"]);
00128 }
00129 if(model_config_["robot"]){
00130 mode_ = "robot";
00131 }
00132 if(model_config_.hasMember("mode")){
00133 mode_.assign(model_config_["mode"]);
00134 }
00135
00136 robot_mode_ = model_config_["robot"];
00137
00138
00139 if(model_config_.hasMember("initial_joint_state")){
00140 XmlRpc::XmlRpcValue initial_pose = model_config_["initial_joint_state"];
00141 for(int i=0; i< initial_pose.size(); i++){
00142 XmlRpc::XmlRpcValue v = initial_pose[i];
00143 string name;
00144 double position;
00145 if(v.hasMember("name") && v.hasMember("position")){
00146 name.assign(v["name"]);
00147 position = getXmlValue(v["position"]);
00148 initial_pose_map_[name] = position;
00149 }
00150 }
00151 }
00152
00153
00154 if(model_config_.hasMember("display")){
00155 display_ = model_config_["display"];
00156 }else{
00157 display_ = true;
00158 }
00159 }
00160
00161 void addUrdfMarker(){
00162 new UrdfModelMarker(model_name_, model_file_, frame_id_, pose_stamped_ ,root_offset_, scale_factor_, mode_ , robot_mode_, registration_,fixed_link_, use_robot_description_, use_visible_color_,initial_pose_map_, -1, server_);
00163 }
00164
00165 UrdfModelSettings(XmlRpc::XmlRpcValue model, boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server) : pnh_("~"){
00166 model_config_ = model;
00167 server_ = server;
00168 init();
00169
00170 if(display_){
00171 addUrdfMarker();
00172 }
00173 display_marker_sub_ = pnh_.subscribe<geometry_msgs::PoseArray> (model_name_ + "/pose_array", 1, boost::bind( &UrdfModelSettings::displayMarkerArrayCB, this, _1));
00174 }
00175 };
00176
00177
00178
00179 int main(int argc, char** argv)
00180 {
00181 ros::init(argc, argv, "jsk_model_marker_interface");
00182 ros::NodeHandle n;
00183 ros::NodeHandle pnh_("~");
00184
00185
00186 string server_name;
00187 pnh_.param("server_name", server_name, std::string ("") );
00188 if ( server_name == "" ) {
00189 server_name = ros::this_node::getName();
00190 }
00191
00192 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
00193 server.reset( new interactive_markers::InteractiveMarkerServer(server_name, "", false) );
00194
00195 XmlRpc::XmlRpcValue v;
00196 pnh_.param("model_config", v, v);
00197 for(int i=0; i< v.size(); i++){
00198 new UrdfModelSettings(v[i], server);
00199 }
00200 ros::spin();
00201 return 0;
00202 }
00203
00204