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