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 std::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 std::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 }
00063 else {
00064 for (int i = 0; i < umm_vec_size; i++) {
00065
00066 ps.pose = msg->poses[i];
00067 umm_vec_[i]->setRootPose(ps);
00068 }
00069 for (int i = umm_vec_size; i < msg_size; i++) {
00070
00071 ps.pose = msg->poses[i];
00072 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_)));
00073 }
00074 }
00075 }
00076
00077 void init() {
00078
00079 model_name_.assign(model_config_["name"]);
00080
00081
00082 model_description_ = "";
00083 if (model_config_.hasMember("description")) {
00084 model_description_.assign(model_config_["description"]);
00085 }
00086
00087 scale_factor_ = 1.02;
00088 if (model_config_.hasMember("scale")) {
00089 scale_factor_ = getXmlValue(model_config_["scale"]);
00090 }
00091
00092 if (model_config_.hasMember("pose")) {
00093 pose_stamped_.pose = getPose(model_config_["pose"]);
00094 }
00095 else {
00096 pose_stamped_.pose = geometry_msgs::Pose();
00097 pose_stamped_.pose.orientation.w = 1.0;
00098 }
00099 pose_stamped_.header.stamp = ros::Time::now();
00100
00101 if (model_config_.hasMember("offset")) {
00102 root_offset_ = getPose(model_config_["offset"]);
00103 }
00104 else {
00105 root_offset_ = geometry_msgs::Pose();
00106 root_offset_.orientation.w = 1.0;
00107 }
00108
00109
00110 use_visible_color_ = false;
00111 if (model_config_.hasMember("use_visible_color")) {
00112 use_visible_color_ = model_config_["use_visible_color"];
00113 }
00114 ROS_INFO_STREAM("use_visible_color: " << use_visible_color_);
00115
00116 frame_id_.assign(model_config_["frame-id"]);
00117
00118
00119 mode_ = "model";
00120 model_file_ = "";
00121 registration_ = false;
00122 if (model_config_.hasMember("registration")) {
00123 registration_ = model_config_["registration"];
00124 mode_ = "registration";
00125 }
00126
00127 if (model_config_.hasMember("fixed_link")) {
00128 XmlRpc::XmlRpcValue fixed_links = model_config_["fixed_link"];
00129 if (fixed_links.getType() == XmlRpc::XmlRpcValue::TypeString) {
00130 fixed_link_.push_back( fixed_links );
00131 }
00132 else if (fixed_links.getType() == XmlRpc::XmlRpcValue::TypeArray) {
00133 for(int i=0; i< fixed_links.size(); i++) {
00134 fixed_link_.push_back(fixed_links[i]);
00135 }
00136 }
00137 }
00138
00139 if (model_config_.hasMember("model")) {
00140 model_file_.assign(model_config_["model"]);
00141 }
00142 use_robot_description_ = false;
00143 if (model_config_.hasMember("use_robot_description")) {
00144 model_file_ = "/robot_description";
00145 use_robot_description_ = model_config_["use_robot_description"];
00146 }
00147 if (model_config_.hasMember("model_param")) {
00148 use_robot_description_ = true;
00149 model_file_.assign(model_config_["model_param"]);
00150 }
00151 if (model_config_["robot"]) {
00152 mode_ = "robot";
00153 }
00154 if (model_config_.hasMember("mode")) {
00155 mode_.assign(model_config_["mode"]);
00156 }
00157
00158 robot_mode_ = model_config_["robot"];
00159
00160
00161 if (model_config_.hasMember("initial_joint_state")) {
00162 XmlRpc::XmlRpcValue initial_pose = model_config_["initial_joint_state"];
00163 for(int i=0; i< initial_pose.size(); i++) {
00164 XmlRpc::XmlRpcValue v = initial_pose[i];
00165 string name;
00166 double position;
00167 if (v.hasMember("name") && v.hasMember("position")) {
00168 name.assign(v["name"]);
00169 position = getXmlValue(v["position"]);
00170 initial_pose_map_[name] = position;
00171 }
00172 }
00173 }
00174
00175
00176 if (model_config_.hasMember("display")) {
00177 display_ = model_config_["display"];
00178 }
00179 else {
00180 display_ = true;
00181 }
00182 ROS_INFO("Loading model config");
00183 ROS_INFO("model_name: %s", model_name_.c_str());
00184 ROS_INFO("model_description: %s", model_description_.c_str());
00185 ROS_INFO("scale_factor: %f", scale_factor_);
00186 ROS_INFO_STREAM("pose_stamped: " << pose_stamped_);
00187 ROS_INFO_STREAM("root_offset: " << root_offset_);
00188 ROS_INFO_STREAM("frame_id: " << frame_id_);
00189 ROS_INFO_STREAM("mode: " << mode_);
00190 ROS_INFO_STREAM("registration: " << registration_);
00191
00192
00193
00194 ROS_INFO_STREAM("model_file: " << model_file_);
00195 ROS_INFO_STREAM("use_robot_description: " << use_robot_description_);
00196 }
00197
00198 void addUrdfMarker() {
00199 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_);
00200 }
00201
00202 UrdfModelSettings(XmlRpc::XmlRpcValue model, std::shared_ptr<interactive_markers::InteractiveMarkerServer> server) : pnh_("~") {
00203 model_config_ = model;
00204 server_ = server;
00205 init();
00206
00207 if (display_) {
00208 addUrdfMarker();
00209 }
00210 display_marker_sub_ = pnh_.subscribe<geometry_msgs::PoseArray> (model_name_ + "/pose_array", 1, boost::bind( &UrdfModelSettings::displayMarkerArrayCB, this, _1));
00211 }
00212 };
00213
00214
00215
00216 int main(int argc, char** argv)
00217 {
00218 ros::init(argc, argv, "jsk_model_marker_interface");
00219 ros::NodeHandle n;
00220 ros::NodeHandle pnh_("~");
00221
00222 string server_name;
00223 pnh_.param("server_name", server_name, std::string ("") );
00224 if (server_name == "") {
00225 server_name = ros::this_node::getName();
00226 }
00227
00228 std::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
00229 server.reset( new interactive_markers::InteractiveMarkerServer(server_name, "", false) );
00230
00231 XmlRpc::XmlRpcValue v;
00232 pnh_.param("model_config", v, v);
00233 for (int i=0; i< v.size(); i++) {
00234 new UrdfModelSettings(v[i], server);
00235 }
00236 ros::spin();
00237 return 0;
00238 }
00239
00240