urdf_writer.cpp
Go to the documentation of this file.
00001 
00002 
00003 // yliu 07/22/2011
00004 
00005 #include "ros/ros.h"
00006 #include <geometry_msgs/TransformStamped.h>
00007 #include <tf/transform_broadcaster.h>
00008 #include <fstream>
00009 #include <string>
00010 #include <time.h>
00011 #include <sstream>
00012 #include <iomanip>
00013 #include <boost/regex.hpp>
00014 #include <kdl/frames.hpp>
00015 #include <kdl_parser/kdl_parser.hpp>
00016 
00017 
00018 class URDF_Writer
00019 {
00020 public:
00021         ros::NodeHandle nh;
00022         ros::Subscriber sub; 
00023         std::string parent_frame_id;
00024         std::string child_frame_id; 
00025 
00026         KDL::Tree kdl_tree;
00027         
00028         std::string urdf_tree;
00029         std::string output_filename;
00030 
00031         URDF_Writer()
00032         {
00033                 
00034                 nh.getParam("robot_description", urdf_tree);
00035                 //nh.param("robot_description", urdf_tree, std::string() );  // string ( ) -- Construct string object, content is initialized to an empty string
00036                 nh.getParam("urdf_output_filename", output_filename);
00037                 
00038                 sub = nh.subscribe("camera_pose_static_transform_update", 10,  &URDF_Writer::MyCallbackFunc, this); //
00039                 // ISO C++ forbids taking the address of an unqualified or parenthesized non-static member function to form a pointer to member function.  Say ‘&URDF_Writer::MyCallbackFunc’
00040 
00041                                 
00042                 if (!kdl_parser::treeFromString(urdf_tree, kdl_tree)) //create a KDL Tree from parameter server
00043                 {
00044                       ROS_ERROR("Failed to construct kdl tree");
00045                       //return false;
00046                 }
00047         }
00048 
00049 
00050 
00051         void MyCallbackFunc(const geometry_msgs::TransformStamped::ConstPtr& msg)
00052         {       
00053                 
00054                 //printf("From: %s\n", msg->header.frame_id.c_str());
00055                 //printf("To  : %s\n", msg->child_frame_id.c_str());
00056                 //printf("Q = [%f, %f , %f, %f]\n",msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z, msg->transform.rotation.w);
00057                 //printf("p = [%f, %f, %f]\n\n", msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z);
00058 
00059 
00060 
00061                 // STEPS:
00062                 // remove xml snippet from previous calibration
00063                 // locate </robot> tag
00064                 // create a new xml snippet, insert, write to disk
00065                 // check if the tree is still OK. 
00066 
00067 
00068                 // -----
00069                 // KDL::
00070                 KDL::Rotation R = KDL::Rotation::Quaternion(msg->transform.rotation.x,
00071                                                             msg->transform.rotation.y,
00072                                                             msg->transform.rotation.z,
00073                                                             msg->transform.rotation.w);
00074                 parent_frame_id = msg->header.frame_id;
00075                 child_frame_id = msg->child_frame_id;
00076                 
00077                 double Roll, Pitch, Yaw;
00078                 double px, py, pz;
00079 
00080                 px = msg->transform.translation.x;
00081                 py = msg->transform.translation.y;
00082                 pz = msg->transform.translation.z;
00083 
00084                 R.GetRPY(Roll, Pitch, Yaw);
00085 
00086 
00087 
00088                 // ------------------------------
00089                 // removing out-dated description  
00090                 std::string::const_iterator start, end; 
00091 
00092                 std::string snippet_to_remove_a = "((^)|(^ +))<joint name=\"" + child_frame_id +  "_joint\"" + ".*" + "</joint>\n";
00093                 boost::regex re1a(snippet_to_remove_a.c_str());
00094                 std::string snippet_to_remove_b = "((^)|(^ +))<link name=\"" + child_frame_id  + ".*" + "/>\n";
00095                 boost::regex re1b(snippet_to_remove_b.c_str());
00096                 std::string snippet_to_remove_c = "((^)|(^ +))<!-- added after running camera_pose_calibration -->\n";
00097                 boost::regex re1c(snippet_to_remove_c.c_str());
00098 
00099                 boost::match_results<std::string::const_iterator> match1;  
00100                 boost::match_flag_type flag1 = boost::match_default;
00101 
00102                 printf("\nRemoving out-dated description xml snippet that was previously added by urdf_writer (if found)...\n");
00103 
00104                 start = urdf_tree.begin(); 
00105                 end = urdf_tree.end(); 
00106                 if (regex_search(start, end, match1, re1a, flag1)) 
00107                 {       
00108                         urdf_tree.erase(match1.position(), match1[0].second-match1[0].first);
00109                         // match1[0].first : The start of the sequence of characters that matched the regular expression
00110                         // match1[0].second : The end of the sequence of characters that matched the regular expression         
00111                         //printf("%d\n", match1[0].second-match1[0].first);
00112                         printf("...\n");
00113                 }
00114 
00115                 start = urdf_tree.begin(); 
00116                 end = urdf_tree.end(); 
00117                 if (regex_search(start, end, match1, re1b, flag1)) 
00118                 {       
00119                         urdf_tree.erase(match1.position(), match1[0].second-match1[0].first);
00120                         //printf("%d\n", match1[0].second-match1[0].first);
00121                         printf("...\n");
00122                 }
00123 
00124                 start = urdf_tree.begin(); 
00125                 end = urdf_tree.end();
00126                 if (regex_search(start, end, match1, re1c, flag1)) 
00127                 {       
00128                         urdf_tree.erase(match1.position(), match1[0].second-match1[0].first);
00129                         //printf("%d\n", match1[0].second-match1[0].first);
00130                         printf("...\n");
00131                 }
00132 
00133 
00134                 // ----------------------
00135 
00136                 printf("Adding the xml snippet for the new camera frame to the urdf file ...\n\n");
00137 
00138 
00139 
00140 
00141 
00142                 // ----------------------
00143                 // locating </robot> tag
00144 
00145                 start = urdf_tree.begin(); 
00146                 end = urdf_tree.end(); 
00147 
00148                 boost::match_results<std::string::const_iterator> match2;  //smatch
00149                 boost::match_flag_type flags = boost::match_default;
00150 
00151                 int location = 0;
00152                 boost::regex re2("</robot>");
00153                 if(regex_search(start, end, match2, re2, flags))   //since there is only one occurrence of </robot> in the .urdf file
00154                 {
00155                         location =  match2.position();
00156                         //printf("location at: %d\n", location);
00157                         //urdf_tree.insert(match2[0].second-urdf_tree.begin()),"ddyy"); // y. d. :)
00158                 }
00159 
00160 
00161 
00162 
00163                 // ----------------------
00164                 // creating description xml snippet, insert to urdf_tree and write to disk
00165 
00166                 std::ostringstream buffer1, buffer2;
00167                 buffer1 << Roll <<" " << Pitch <<" "<< Yaw;
00168                 buffer2 << px << " " << py << " " << pz;  //
00169                 std::string urdf_snippet =
00170                  "  <!-- added after running camera_pose_calibration -->\n"
00171                  "  <joint name=\""+ child_frame_id +  "_joint\" type=\"fixed\">" + "\n" +
00172                  "     <origin rpy=\"" + buffer1.str() +  "\" xyz=\"" + buffer2.str() + "\"/>" + "\n" +
00173                  "     <parent link=\"" + parent_frame_id + "\"/>" + "\n" +
00174                  "     <child link=\"" + child_frame_id + "\"/>" + "\n" +
00175                  "  </joint>" + "\n" +
00176                  "  <link name=\""+ child_frame_id + "\" type=\"camera\"/>" + "\n";
00177 
00178                 urdf_tree.insert(location, urdf_snippet);
00179 
00180                 std::ofstream wr(output_filename.c_str(), std::ios::trunc); // in ~/.ros
00181 
00182                 if (wr.is_open())
00183                 {
00184                         wr<<urdf_tree;
00185                         printf("New urdf file generated.\n");
00186                         printf("File path:  %s\n\n", output_filename.c_str());
00187                 }
00188                 else
00189                 {
00190                         printf("Error opening file.\n\n");
00191                 }
00192 
00193                 wr.close();
00194 
00195                 
00196 
00197                 // ---------------------------------------
00198                 // checking the integrity of the urdf tree | warning only...
00199 
00200                 std::string ss="\"" + parent_frame_id +"\""; 
00201                 std::string::const_iterator s1,e1;
00202                 s1 = urdf_tree.begin();
00203                 e1 = urdf_tree.end();
00204                 boost::match_results<std::string::const_iterator> m;
00205                 boost::match_flag_type f = boost::match_default;
00206 
00207                 ss = "<link name=" + ss ;
00208                 //printf("%s\n", ss.c_str());
00209                 boost::regex pa(ss);  
00210 
00211                 if(regex_search(s1, e1, m, pa, f)) 
00212                 {
00213                         printf("The parent link of [%s_joint] is verifed in the updated .urdf file (at %d)\n", child_frame_id.c_str(), int(m.position()));
00214                 }
00215                 else
00216                 {
00217                         ROS_WARN("The parent link of [%s_joint] seems missing in the updated .urdf file.\n You will receive an error if you do\n\n $ rosrun urdf check_urdf your_urdf.urdf\n\n", 
00218                                         child_frame_id.c_str()  );
00219                 }       
00220 
00221 
00222         }
00223 
00224 };
00225 
00226 
00227 
00228 int main(int argc, char **argv)
00229 {
00230         ros::init(argc, argv, "urdf_writer");
00231 
00232         URDF_Writer my_urdf_writer;     
00233 
00234         ros::spin();
00235 
00236 
00237         return 0;
00238 
00239 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_toolkits
Author(s): Yiping Liu
autogenerated on Thu Aug 15 2013 12:02:52