Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22850] pkg/trunk
|
From: <mee...@us...> - 2009-08-25 17:25:24
|
Revision: 22850
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22850&view=rev
Author: meeussen
Date: 2009-08-25 17:25:12 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
kdl parser supports parsing form file, string, xml and robot model. Put in new namespace to avoid conflicts with old parser
Modified Paths:
--------------
pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/dom_parser.hpp
pkg/trunk/stacks/mechanism/kdl_parser/src/dom_parser.cpp
pkg/trunk/stacks/mechanism/kdl_parser/test/example_dom.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/robot.cpp
pkg/trunk/stacks/mechanism/robot_state_publisher/src/state_publisher.cpp
pkg/trunk/stacks/mechanism/robot_state_publisher/test/test_publisher.cpp
pkg/trunk/util/pr2_ik/src/pr2_ik_solver.cpp
Modified: pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/dom_parser.hpp
===================================================================
--- pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/dom_parser.hpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/dom_parser.hpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -43,9 +43,12 @@
using namespace std;
-namespace KDL{
+namespace kdl_parser{
-bool treeFromRobotModel(urdf::Model& robot_model, Tree& tree);
+bool treeFromFile(const string& file, KDL::Tree& tree);
+bool treeFromString(const string& xml, KDL::Tree& tree);
+bool treeFromXml(TiXmlElement *root, KDL::Tree& tree);
+bool treeFromRobotModel(urdf::Model& robot_model, KDL::Tree& tree);
}
#endif
Modified: pkg/trunk/stacks/mechanism/kdl_parser/src/dom_parser.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/kdl_parser/src/dom_parser.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/kdl_parser/src/dom_parser.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -36,9 +36,12 @@
#include "kdl_parser/dom_parser.hpp"
#include <kdl/frames_io.hpp>
+
+
using namespace std;
+using namespace KDL;
-namespace KDL{
+namespace kdl_parser{
// construct vector
@@ -125,12 +128,47 @@
}
+bool treeFromFile(const string& file, Tree& tree)
+{
+ TiXmlDocument urdf_xml;
+ urdf_xml.LoadFile(file);
+ TiXmlElement *root = urdf_xml.FirstChildElement("robot");
+ if (!root){
+ cerr << "Could not parse the xml" << endl;
+ return false;
+ }
+ return treeFromXml(root, tree);
+}
+
+bool treeFromString(const string& xml, Tree& tree)
+{
+ TiXmlDocument urdf_xml;
+ urdf_xml.Parse(xml.c_str());
+ TiXmlElement *root = urdf_xml.FirstChildElement("robot");
+ if (!root){
+ cerr << "Could not parse the xml" << endl;
+ return false;
+ }
+ return treeFromXml(root, tree);
+}
+
+bool treeFromXml(TiXmlElement *root, Tree& tree)
+{
+ urdf::Model robot_model;
+ if (!robot_model.initFile("pr2.urdf")){
+ cerr << "Could not generate robot model" << endl;
+ return false;
+ }
+ return treeFromRobotModel(robot_model, tree);
+}
+
+
bool treeFromRobotModel(urdf::Model& robot_model, Tree& tree)
{
tree = Tree(robot_model.getRoot()->name);
- // add all children
+ // add all children
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
return false;
Modified: pkg/trunk/stacks/mechanism/kdl_parser/test/example_dom.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/kdl_parser/test/example_dom.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/kdl_parser/test/example_dom.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -50,7 +50,7 @@
{cerr << "Could not generate robot model" << endl; return false;}
Tree my_tree;
- if (!treeFromRobotModel(robot_model, my_tree))
+ if (!kdl_parser::treeFromRobotModel(robot_model, my_tree))
{cerr << "Could not extract kdl tree" << endl; return false;}
// walk through tree
Modified: pkg/trunk/stacks/mechanism/mechanism_model/src/robot.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_model/src/robot.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/mechanism_model/src/robot.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -58,7 +58,7 @@
}
// Constructs a kdl robot model from the robot description.
- if (!treeFromRobotModel(robot_description, robot_model_)){
+ if (!kdl_parser::treeFromRobotModel(robot_description, robot_model_)){
ROS_ERROR("Mechanism Model failed to construct a kdl tree from the robot model");
return false;
}
Modified: pkg/trunk/stacks/mechanism/robot_state_publisher/src/state_publisher.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/robot_state_publisher/src/state_publisher.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/robot_state_publisher/src/state_publisher.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -73,7 +73,7 @@
// constructs a kdl tree from the robot model
Tree tree;
- if (!treeFromRobotModel(robot_model, tree)){
+ if (!kdl_parser::treeFromRobotModel(robot_model, tree)){
ROS_ERROR("Failed to extract kdl tree from robot model");
return -1;
}
Modified: pkg/trunk/stacks/mechanism/robot_state_publisher/test/test_publisher.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/robot_state_publisher/test/test_publisher.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/robot_state_publisher/test/test_publisher.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -73,7 +73,7 @@
// constructs a kdl tree from the robot model
Tree tree;
- if (!treeFromRobotModel(robot_model, tree))
+ if (!kdl_parser::treeFromRobotModel(robot_model, tree))
ROS_ERROR("Failed to extract kdl tree from robot model");
publisher = new JointStateListener(tree);
Modified: pkg/trunk/util/pr2_ik/src/pr2_ik_solver.cpp
===================================================================
--- pkg/trunk/util/pr2_ik/src/pr2_ik_solver.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/util/pr2_ik/src/pr2_ik_solver.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -178,7 +178,7 @@
// create robot chain from root to tip
KDL::Tree tree;
- if (!KDL::treeFromRobotModel(robot_model_, tree))
+ if (!kdl_parser::treeFromRobotModel(robot_model_, tree))
ROS_ERROR("Could not initialize tree object");
if (!tree.getChain(root_name, tip_name, chain_))
ROS_ERROR("Could not initialize chain object");
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
