Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10893] pkg/trunk
|
From: <is...@us...> - 2009-02-10 03:55:25
|
Revision: 10893
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10893&view=rev
Author: isucan
Date: 2009-02-10 02:36:32 +0000 (Tue, 10 Feb 2009)
Log Message:
-----------
undoing my 'fix'
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/util/self_watch/manifest.xml
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp
pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
pkg/trunk/world_models/robot_models/planning_models/manifest.xml
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp
Removed Paths:
-------------
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h
pkg/trunk/world_models/robot_models/planning_models/src/urdf.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-02-10 02:36:32 UTC (rev 10893)
@@ -185,13 +185,13 @@
return true;
}
- virtual void setRobotDescription(planning_models::URDF *file)
+ virtual void setRobotDescription(robot_desc::URDF *file)
{
KinematicStateMonitor::setRobotDescription(file);
if (m_kmodel)
{
std::vector<std::string> links;
- planning_models::URDF::Group *g = file->getGroup("collision_check");
+ robot_desc::URDF::Group *g = file->getGroup("collision_check");
if (g && g->hasFlag("collision"))
links = g->linkNames;
m_collisionSpace->lock();
@@ -218,9 +218,9 @@
protected:
- void addSelfCollisionGroups(unsigned int cid, planning_models::URDF *model)
+ void addSelfCollisionGroups(unsigned int cid, robot_desc::URDF *model)
{
- std::vector<planning_models::URDF::Group*> groups;
+ std::vector<robot_desc::URDF::Group*> groups;
model->getGroups(groups);
m_collisionSpace->lock();
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-10 02:36:32 UTC (rev 10893)
@@ -43,7 +43,7 @@
#include <ros/time.h>
#include <ros/console.h>
-#include <planning_models/urdf.h>
+#include <urdf/URDF.h>
#include <planning_models/kinematic.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
@@ -130,7 +130,7 @@
void setRobotDescriptionFromData(const char *data)
{
- planning_models::URDF *file = new planning_models::URDF();
+ robot_desc::URDF *file = new robot_desc::URDF();
if (file->loadString(data))
setRobotDescription(file);
else
@@ -139,14 +139,14 @@
void setRobotDescriptionFromFile(const char *filename)
{
- planning_models::URDF *file = new planning_models::URDF();
+ robot_desc::URDF *file = new robot_desc::URDF();
if (file->loadFile(filename))
setRobotDescription(file);
else
delete file;
}
- virtual void setRobotDescription(planning_models::URDF *file)
+ virtual void setRobotDescription(robot_desc::URDF *file)
{
if (m_urdf)
delete m_urdf;
@@ -296,7 +296,7 @@
ros::Node *m_node;
tf::TransformListener m_tf;
- planning_models::URDF *m_urdf;
+ robot_desc::URDF *m_urdf;
planning_models::KinematicModel *m_kmodel;
// info about the pose; this is not placed in the robot's kinematic state
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-02-10 02:36:32 UTC (rev 10893)
@@ -13,6 +13,7 @@
<depend package="robot_msgs" />
<depend package="robot_srvs" />
+ <depend package="wg_robot_description_parser"/>
<depend package="planning_models"/>
<depend package="collision_space"/>
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -389,7 +389,7 @@
return result;
}
- virtual void setRobotDescription(planning_models::URDF *file)
+ virtual void setRobotDescription(robot_desc::URDF *file)
{
CollisionSpaceMonitor::setRobotDescription(file);
defaultPosition();
@@ -688,12 +688,12 @@
void createMotionPlanningInstances(RKPModel* model)
{
std::map<std::string, std::string> options;
- planning_models::URDF::Group *group = m_urdf->getGroup(model->kmodel->getURDFGroup(model->groupName));
+ robot_desc::URDF::Group *group = m_urdf->getGroup(model->kmodel->getURDFGroup(model->groupName));
options.clear();
if (group)
{
- const planning_models::URDF::Map &data = group->data;
+ const robot_desc::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "RRT");
}
@@ -703,7 +703,7 @@
options.clear();
if (group)
{
- const planning_models::URDF::Map &data = group->data;
+ const robot_desc::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "LazyRRT");
}
model->addLazyRRT(options);
@@ -711,7 +711,7 @@
options.clear();
if (group)
{
- const planning_models::URDF::Map &data = group->data;
+ const robot_desc::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "EST");
}
model->addEST(options);
@@ -719,7 +719,7 @@
options.clear();
if (group)
{
- const planning_models::URDF::Map &data = group->data;
+ const robot_desc::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "SBL");
}
model->addSBL(options);
@@ -727,7 +727,7 @@
options.clear();
if (group)
{
- const planning_models::URDF::Map &data = group->data;
+ const robot_desc::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "IKSBL");
}
model->addIKSBL(options);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -200,7 +200,7 @@
}
}
- virtual void setRobotDescription(planning_models::URDF *file)
+ virtual void setRobotDescription(robot_desc::URDF *file)
{
CollisionSpaceMonitor::setRobotDescription(file);
Modified: pkg/trunk/util/self_watch/manifest.xml
===================================================================
--- pkg/trunk/util/self_watch/manifest.xml 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/util/self_watch/manifest.xml 2009-02-10 02:36:32 UTC (rev 10893)
@@ -8,6 +8,7 @@
<depend package="robot_msgs" />
+ <depend package="wg_robot_description_parser" />
<depend package="planning_models" />
<depend package="collision_space" />
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -66,7 +66,7 @@
if (getParam("robot_description", content))
{
// parse the description
- planning_models::URDF *file = new planning_models::URDF();
+ robot_desc::URDF *file = new robot_desc::URDF();
if (file->loadString(content.c_str()))
{
// create a kinematic model out of the parsed description
@@ -97,7 +97,7 @@
// get the list of links that are enabled for collision checking
std::vector<std::string> links;
- planning_models::URDF::Group *g = file->getGroup("collision_check");
+ robot_desc::URDF::Group *g = file->getGroup("collision_check");
if (g && g->hasFlag("collision"))
links = g->linkNames;
@@ -117,7 +117,7 @@
// get the self collision groups and add them to the collision space
int nscgroups = 0;
- std::vector<planning_models::URDF::Group*> groups;
+ std::vector<robot_desc::URDF::Group*> groups;
file->getGroups(groups);
for (unsigned int i = 0 ; i < groups.size() ; ++i)
if (groups[i]->hasFlag("self_collision"))
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -98,7 +98,7 @@
urdf_->clear();
urdf_->loadString(content.c_str());
- kinematic_model_->build( content );
+ kinematic_model_->build( *urdf_ );
kinematic_model_->defaultState();
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -163,7 +163,7 @@
delete kinematic_model_;
kinematic_model_ = new planning_models::KinematicModel();
kinematic_model_->setVerbose( false );
- kinematic_model_->build( description_param_ );
+ kinematic_model_->build( file );
kinematic_model_->reduceToRobotFrame();
robot_->update( kinematic_model_, target_frame_ );
Modified: pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-10 02:36:32 UTC (rev 10893)
@@ -2,7 +2,7 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(planning_models)
rospack_add_boost_directories()
-rospack_add_library(planning_models src/urdf.cpp src/kinematic.cpp)
+rospack_add_library(planning_models src/kinematic.cpp)
# Unit tests
rospack_add_gtest(test_kinematic test/test_kinematic.cpp)
Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-10 02:36:32 UTC (rev 10893)
@@ -37,7 +37,7 @@
#ifndef KINEMATIC_ROBOT_MODEL_
#define KINEMATIC_ROBOT_MODEL_
-#include <planning_models/urdf.h>
+#include <urdf/URDF.h>
#include <LinearMath/btTransform.h>
#include <iostream>
@@ -183,7 +183,7 @@
virtual void updateVariableTransform(const double *params) = 0;
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot) = 0;
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot) = 0;
};
/** A fixed joint */
@@ -195,7 +195,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
};
/** A planar joint */
@@ -212,7 +212,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
};
/** A floating joint */
@@ -229,7 +229,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
};
/** A prismatic joint */
@@ -247,7 +247,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
btVector3 axis;
double limit[2];
@@ -268,7 +268,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
btVector3 axis;
btVector3 anchor;
@@ -374,7 +374,7 @@
const double* computeTransform(const double *params, int groupID = -1);
/** Extract the information needed by the joint given the URDF description */
- void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
};
@@ -558,7 +558,7 @@
}
void build(const std::string &description, bool ignoreSensors = false);
- virtual void build(const URDF &model, bool ignoreSensors = false);
+ virtual void build(const robot_desc::URDF &model, bool ignoreSensors = false);
bool isBuilt(void) const;
StateParams* newStateParams(void);
@@ -617,19 +617,19 @@
private:
/** Build the needed datastructure for a joint */
- void buildChainJ(Robot *robot, Link *parent, Joint *joint, const URDF::Link *urdfLink, const URDF &model);
+ void buildChainJ(Robot *robot, Link *parent, Joint *joint, const robot_desc::URDF::Link *urdfLink, const robot_desc::URDF &model);
/** Build the needed datastructure for a link */
- void buildChainL(Robot *robot, Joint *parent, Link *link, const URDF::Link *urdfLink, const URDF &model);
+ void buildChainL(Robot *robot, Joint *parent, Link *link, const robot_desc::URDF::Link *urdfLink, const robot_desc::URDF &model);
/** Construct the list of groups the model knows about (the ones marked with the 'plan' attribute) */
- void constructGroupList(const URDF &model);
+ void constructGroupList(const robot_desc::URDF &model);
/* compute the parameter names */
void computeParameterNames(void);
/** Allocate a joint of appropriate type, depending on the loaded link */
- Joint* createJoint(const URDF::Link* urdfLink);
+ Joint* createJoint(const robot_desc::URDF::Link* urdfLink);
};
Deleted: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h 2009-02-10 02:36:32 UTC (rev 10893)
@@ -1,851 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/** \author Ioan Sucan */
-
-#ifndef PLANNING_MODELS_URDF_PARSER_
-#define PLANNING_MODELS_URDF_PARSER_
-
-#include <tinyxml/tinyxml.h>
-#include <iostream>
-#include <string>
-#include <vector>
-#include <map>
-
-/**
- @mainpage
-
- @htmlinclude ../../manifest.html
-
- Universal Robot Description Format (URDF) parser. The URDF class
- is able to load a robot description from multiple files, string
- buffers, streams and produce a datastructure similar to the XML
- structure. Expression evaluation and file inclusion are performed
- if needed. Checks are performed to make sure the document is
- correct: values that are inconsistend are reported, typos in the
- names, unknown tags, etc. For more documentation, see the URDF
- description. */
-
-/** Namespace that contains the URDF parser */
-namespace planning_models
-{
- /** This class contains a complete parser for URDF documents.
- Datastructures that resemble the ones specified in the XML
- document are instantiated, easy-access pointers are computed
- (to access a datastructure from another). Checks are made to
- make sure the parsed data is correct. Since actuators,
- transmissions and controllers are potentially robot specific,
- their data is kept as XML elements.
- */
- class URDF
- {
- public:
-
- /** This class encapsulates data that can be attached to various tags in the format */
- class Map
- {
- public:
-
- Map(void)
- {
- }
-
- virtual ~Map(void)
- {
- }
-
- /** Get the set of flags used for the processed <map> tags. Each <map> tag has one flag. Multiple tags may
- have the same flag. */
- void getMapTagFlags(std::vector<std::string> &flags) const;
-
- /** Given a specific flag, retrieve the names of the <map> tags that have the given flag */
- void getMapTagNames(const std::string &flag, std::vector<std::string> &names) const;
-
- /** Given a name and a flag, retrieve the defined map (string, string) */
- std::map<std::string, std::string> getMapTagValues(const std::string &flag, const std::string &name) const;
-
- /** Given a name and a flag, retrieve the defined map (string, XML) */
- std::map<std::string, const TiXmlElement*> getMapTagXML(const std::string &flag, const std::string &name) const;
-
- /** Check whether a key under empty flag and empty name has been defined */
- bool hasDefault(const std::string &key) const;
-
- /** Retrieve the value of a key (string) under empty flag and empty name has been defined */
- std::string getDefaultValue(const std::string &key) const;
-
- /** Retrieve the value of a key (XML) under empty flag and empty name has been defined */
- const TiXmlElement* getDefaultXML(const std::string &key) const;
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- /** Add a string element to the map */
- void add(const std::string &flag, const std::string &name, const std::string &key, const std::string &value);
-
- /** Add an XML element to the map */
- void add(const std::string &flag, const std::string &name, const std::string &key, const TiXmlElement *value);
-
- Map& operator=(const Map &rhs)
- {
- m_data = rhs.m_data;
- return *this;
- }
- protected:
-
- struct Element
- {
- Element(void)
- {
- xml = NULL;
- str = NULL;
- }
-
- ~Element(void)
- {
- if (str)
- delete str;
- }
-
- std::string *str; // allocated locally
- const TiXmlElement *xml; // allocated in the XML document
- };
-
- std::map < std::string, std::map < std::string, std::map < std::string, Element > > > m_data;
- };
-
-
- /** Forward declaration of groups */
- struct Group;
-
- /** This class defines link instances. It contains instances of collision, visual, inertial and joint descriptions. */
- struct Link
- {
-
- /** Class for link geometry instances */
- struct Geometry
- {
- struct Shape
- {
- virtual ~Shape(void)
- {
- }
- };
-
- struct Sphere : public Shape
- {
- Sphere(void) : Shape()
- {
- radius = 0.0;
- }
-
- double radius;
- };
-
- struct Box : public Shape
- {
- Box(void) : Shape()
- {
- size[0] = size[1] = size[2] = 0.0;
- }
-
- double size[3];
- };
-
- struct Cylinder : public Shape
- {
- Cylinder(void) : Shape()
- {
- length = radius = 0.0;
- }
-
- double length, radius;
- };
-
- struct Mesh : public Shape
- {
- Mesh(void) : Shape()
- {
- scale[0] = scale[1] = scale[2] = 1.0;
- }
-
- std::string filename;
- double scale[3];
- };
-
- Geometry(void)
- {
- type = UNKNOWN;
- shape = NULL;
- isSet["name"] = false;
- isSet["size"] = false;
- isSet["length"] = false;
- isSet["radius"] = false;
- isSet["filename"] = false;
- isSet["scale"] = false;
- }
-
- virtual ~Geometry(void)
- {
- if (shape)
- delete shape;
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- enum
- {
- UNKNOWN, SPHERE, BOX, CYLINDER, MESH
- } type;
- std::string name;
- Shape *shape;
- Map data;
- std::map<std::string, bool> isSet;
- };
-
-
- /** Class for link joint instances (connects a link to its parent) */
- struct Joint
- {
- Joint(void)
- {
- axis[0] = axis[1] = axis[2] = 0.0;
- anchor[0] = anchor[1] = anchor[2] = 0.0;
- damping = 0.0;
- friction = 0.0;
- limit[0] = limit[1] = 0.0;
- safetyLength[0] = safetyLength[1] = 0.0;
- velocityLimit = 0.0;
- effortLimit = 0.0;
- type = UNKNOWN;
- pjointMimic = NULL;
- fMimicMult = 1.0; fMimicOffset = 0.0;
- isSet["name"] = false;
- isSet["type"] = false;
- isSet["axis"] = false;
- isSet["anchor"] = false;
- isSet["damping"] = false;
- isSet["friction"] = false;
- isSet["limit"] = false;
- isSet["safetyLengthMin"] = false;
- isSet["safetyLengthMax"] = false;
- isSet["effortLimit"] = false;
- isSet["pjointMimic"] = false;
- isSet["fMimicMult"] = false;
- isSet["fMimicOffset"] = false;
- isSet["velocityLimit"] = false;
- isSet["calibration"] = false;
- }
-
- virtual ~Joint(void)
- {
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- enum
- {
- UNKNOWN, FIXED, REVOLUTE, PRISMATIC, PLANAR, FLOATING
- } type;
- std::string name;
- double axis[3]; // vector describing the axis of rotation: (x,y,z)
- double anchor[3]; // point about which the axis defines the rotation: (x,y,z)
- double damping; // damping coefficient
- double friction; // friction coefficient
- double limit[2]; // the joint limits: (min, max)
- double safetyLength[2]; // the joint limits: (min, max)
- double effortLimit;
- double velocityLimit;
- std::string calibration;
- Map data;
- std::map<std::string, bool> isSet;
-
- // only valid for joints that mimic other joint's values
- Joint* pjointMimic; // if not NULL, this joint mimics pjointMimic
- double fMimicMult, fMimicOffset; // the multiplication and offset coeffs. Ie, X = mult*Y+offset
- // where Y is pjointMimic's joint value, X is this joint's value.
- };
-
- /** Class for link collision component instances */
- struct Collision
- {
- Collision(void)
- {
- xyz[0] = xyz[1] = xyz[2] = 0.0;
- rpy[0] = rpy[1] = rpy[2] = 0.0;
- verbose = false;
- geometry = NULL;
- isSet["name"] = false;
- isSet["verbose"] = false;
- isSet["xyz"] = false;
- isSet["rpy"] = false;
- isSet["geometry"] = false;
- }
-
- virtual ~Collision(void)
- {
- if (geometry)
- delete geometry;
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- std::string name;
- bool verbose;
- double xyz[3];
- double rpy[3];
- Geometry *geometry;
- Map data;
- std::map<std::string, bool> isSet;
- };
-
- /** Class for link inertial component instances */
- struct Inertial
- {
- Inertial(void)
- {
- mass = 0.0;
- com[0] = com[1] = com[2] = 0.0;
- inertia[0] = inertia[1] = inertia[2] = inertia[3] = inertia[4] = inertia[5] = 0.0;
- isSet["name"] = false;
- isSet["mass"] = false;
- isSet["inertia"] = false;
- isSet["com"] = false;
- }
-
- virtual ~Inertial(void)
- {
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- std::string name;
- double mass;
- /** Ixx Ixy Ixz Iyy Iyz Izz */
- double inertia[6];
- double com[3];
- Map data;
- std::map<std::string, bool> isSet;
- };
-
- /** Class for link visual component instances */
- struct Visual
- {
- Visual(void)
- {
- xyz[0] = xyz[1] = xyz[2] = 0.0;
- rpy[0] = rpy[1] = rpy[2] = 0.0;
- geometry = NULL;
- isSet["name"] = false;
- isSet["xyz"] = false;
- isSet["rpy"] = false;
- isSet["geometry"] = false;
- }
-
- virtual ~Visual(void)
- {
- if (geometry)
- delete geometry;
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- std::string name;
- double xyz[3];
- double rpy[3];
- Geometry *geometry;
- Map data;
- std::map<std::string, bool> isSet;
- };
-
- Link(void)
- {
- parent = NULL;
- xyz[0] = xyz[1] = xyz[2] = 0.0;
- rpy[0] = rpy[1] = rpy[2] = 0.0;
- inertial = NULL;
- visual = NULL;
- collision = NULL;
- joint = NULL;
- isSet["name"] = false;
- isSet["parent"] = false;
- isSet["inertial"] = false;
- isSet["visual"] = false;
- isSet["collision"] = false;
- isSet["joint"] = false;
- isSet["xyz"] = false;
- isSet["rpy"] = false;
- }
-
- virtual ~Link(void)
- {
- if (inertial)
- delete inertial;
- if (visual)
- delete visual;
- if (collision)
- delete collision;
- if (joint)
- delete joint;
- }
-
- /** Check if the link instance is inside some group */
- bool insideGroup(Group *group) const;
-
- /** Check if the link instance is inside some group */
- bool insideGroup(const std::string &group) const;
-
- /** Check if the link instance is also a sensor */
- virtual bool canSense(void) const;
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- Link *parent;
- std::string parentName;
- std::string name;
- std::vector<Link*> children;
-
- Inertial *inertial;
- Visual *visual;
- Collision *collision;
- Joint *joint;
-
- double rpy[3];
- double xyz[3];
- Map data;
-
- std::vector<Group*> groups;
- std::vector<bool> inGroup;
-
- std::map<std::string, bool> isSet;
- };
-
- /** Class for defining sensors. This is basically a link with a few extra parameters */
- struct Sensor : public Link
- {
- Sensor(void)
- {
- type = UNKNOWN;
- isSet["type"] = false;
- isSet["calibration"] = false;
- }
-
- virtual ~Sensor(void)
- {
- }
-
- virtual bool canSense(void) const;
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- enum
- {
- UNKNOWN, LASER, CAMERA, STEREO_CAMERA
- } type;
- std::string calibration;
- };
-
- /** Class for defining frames. A frame is attached to a link,
- as its parent or as its child. Frames do not have
- geometric or inertial properties. They are used to define
- coordinate frames. */
- struct Frame
- {
- Frame(void)
- {
- link = NULL;
- type = UNKNOWN;
- xyz[0] = xyz[1] = xyz[2] = 0.0;
- rpy[0] = rpy[1] = rpy[2] = 0.0;
- isSet["name"] = false;
- isSet["parent"] = false;
- isSet["child"] = false;
- isSet["rpy"] = false;
- isSet["xyz"] = false;
- }
-
- virtual ~Frame(void)
- {
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- std::string name;
- std::string linkName;
- Link* link;
-
- enum
- {
- UNKNOWN, PARENT, CHILD
- } type;
-
- double rpy[3];
- double xyz[3];
- Map data;
-
- std::vector<Group*> groups;
- std::map<std::string, bool> isSet;
- };
-
- /** A class that represents a link chain */
- struct Chain
- {
- Chain(void)
- {
- }
-
- virtual ~Chain(void)
- {
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- std::string name;
- std::string root;
- std::string tip;
- std::vector<Link*> links;
- };
-
- /** A class that represents groups of links or frames (or both) */
- struct Group
- {
- Group(void)
- {
- }
-
- virtual ~Group(void)
- {
- }
-
- /** Check if this group has a specific flag */
- bool hasFlag(const std::string &flag) const;
-
- /** Check if a specific link is a root in this group */
- bool isRoot(const Link* link) const;
-
- /** Check if the group is empty */
- bool empty(void) const;
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- std::string name;
- std::vector<std::string> flags;
- std::vector<std::string> linkNames;
- std::vector<Link*> links;
- std::vector<Link*> linkRoots;
- std::vector<std::string> frameNames;
- std::vector<Frame*> frames;
- Map data;
- };
-
- /** Constructor. If a filename if specified as argument, that file is parsed. */
- URDF(const char *filename = NULL)
- {
- m_paths.push_back("");
- m_errorCount = 0;
- m_rememberUnknownTags = false;
- m_verbose = true;
- if (filename)
- loadFile(filename);
- }
-
- /** Destructor. Frees all memory */
- virtual ~URDF(void)
- {
- freeMemory();
- }
-
- /** Clear all datastructures. The parser can be used for a new document */
- virtual void clear(void);
-
- /** Parse the content of the specified file */
- virtual bool loadFile(const char *filename);
-
- /** Parse the content of the specified file */
- virtual bool loadFile(FILE *file);
-
- /** Parse the content of the specified buffer */
- virtual bool loadString(const char *data);
-
- /** Parse the content of the specified stream */
- virtual bool loadStream(std::istream &is);
-
- /** Print the parsed datastructure */
- virtual void print(std::ostream &out = std::cout) const;
-
- /** Set the verbosity. Default is true */
- void setVerbose(bool verbose);
-
- /** If set to true, unknown tags are stored in a list instead
- of presenting error messages. Default is false. */
- void rememberUnknownTags(bool remember);
-
- /** Returns the robot name */
- const std::string& getRobotName(void) const;
-
- /** Returns the number of individual robot parts that connect to the environment */
- unsigned int getDisjointPartCount(void) const;
-
- /** Return a robot part that connects to the environment */
- Link* getDisjointPart(unsigned int index) const;
-
- /** Check if a specific link connects to the environment */
- bool isRoot(const Link* link) const;
-
- /** Retrieve a link by its name */
- Link* getLink(const std::string &name) const;
-
- /** Retrieve a link by its joint name (connecting to the link's parent) */
- Link* getJointLink(const std::string &name) const;
-
- /** Get the list of all links. The array is sorted alphabetically by name. */
- void getLinks(std::vector<Link*> &links) const;
-
- /** Retrieve a frame by its name */
- Frame* getFrame(const std::string &name) const;
-
- /** Get the list of all frames. The array is sorted alphabetically by name. */
- void getFrames(std::vector<Frame*> &frames) const;
-
- /** Get the list of all group names. The array is sorted alphabetically. */
- void getGroupNames(std::vector<std::string> &groups) const;
-
- /** Retrieve a group by its name */
- Group* getGroup(const std::string &name) const;
-
- /** Get the list of all groups. The array is sorted alphabetically by name. */
- void getGroups(std::vector<Group*> &groups) const;
-
- /** Retrieve a chain by its name */
- Chain* getChain(const std::string &name) const;
-
- /** Get the list of all chains. The array is sorted alphabetically by name. */
- void getChains(std::vector<Chain*> &chains) const;
-
- /** Get the data that was defined at top level */
- const Map& getMap(void) const;
-
- /** Get the defined resource path for this document */
- std::string getResourceLocation(void) const;
-
- /** Try to evaluate the constant as a double value */
- double getConstantValue(const std::string &name, bool *error = NULL) const;
-
- /** Try to retrieve the constant as a string value */
- std::string getConstantString(const std::string &name, bool *error = NULL) const;
-
- /** Get the list of actuator tags */
- void getActuators(std::vector<const TiXmlElement*> &actuators) const;
-
- /** Get the list of transmission tags */
- void getTransmissions(std::vector<const TiXmlElement*> &transmissions) const;
-
- /** Get the list of controller tags */
- void getControllers(std::vector<const TiXmlElement*> &controllers) const;
-
- /** Get the list of unknown tags. Only available if
- rememberUnknownTags(true) was called before parsing. */
- void getUnknownTags(std::vector<const TiXmlNode*> &unknownTags) const;
-
- /** Check if the links form a cycle */
- bool containsCycle(unsigned int index) const;
-
- /** Simple checks to make sure the parsed values are correct */
- void sanityCheck(void) const;
-
- /** Return the number of encountered errors */
- unsigned int getErrorCount(void) const;
-
- protected:
-
- /** Free the memory allocated in this class */
- void freeMemory(void);
-
- /** Clear temporary datastructures used in parsing */
- void clearTemporaryData(void);
-
- /** Clear the memory used by the XML parser*/
- void clearDocs(void);
-
-
-
- /** Add the path to a given filename as a known path */
- virtual void addPath(const char *filename);
-
- /** Find a file using the set of known paths */
- virtual char* findFile(const char *filename);
-
-
-
- /** Add default constants like M_PI */
- virtual void defaultConstants(void);
-
- /** Parse the URDF document */
- virtual bool parse(const TiXmlNode *node);
-
- /** Parse the <link> tag */
- void loadLink(const TiXmlNode *node);
- /** Parse the <sensor> tag */
- void loadSensor(const TiXmlNode *node);
- /** Parse the <frame> tag */
- void loadFrame(const TiXmlNode *node);
- /** Parse the <joint> tag */
- void loadJoint(const TiXmlNode *node, const std::string &defaultName, Link::Joint *joint);
- /** Parse the <geometry> tag */
- void loadGeometry(const TiXmlNode *node, const std::string &defaultName, Link::Geometry *geometry);
- /** Parse the <collision> tag */
- void loadCollision(const TiXmlNode *node, const std::string &defaultName, Link::Collision *collision);
- /** Parse the <visual> tag */
- void loadVisual(const TiXmlNode *node, const std::string &defaultName, Link::Visual *visual);
- /** Parse the <inertial> tag */
- void loadInertial(const TiXmlNode *node, const std::string &defaultName, Link::Inertial *inertial);
- /** Parse the <data> tag */
- void loadMap(const TiXmlNode *node, Map *data);
-
- /** Parse a list of string expressions and convert them to doubles (this relies on the loaded constants) */
- unsigned int loadDoubleValues(const TiXmlNode *node, unsigned int count, double *vals, const char *attrName = NULL, bool warn = false);
- /** Parse a list of string expressions and convert them to doubles (this relies on the loaded constants) */
- unsigned int loadDoubleValues(const std::string& data, unsigned int count, double *vals, const TiXmlNode *node = NULL);
- /** Parse a list of strings and convert them to booleans */
- unsigned int loadBoolValues (const TiXmlNode *node, unsigned int count, bool *vals, const char *attrName = NULL, bool warn = false);
- /** Parse a list of string expressions and convert them to doubles (this relies on the loaded constants) */
- unsigned int loadBoolValues(const std::string& data, unsigned int count, bool *vals, const TiXmlNode *node = NULL);
-
-
- /** Depending on whether the node is a clone or not, get the proper list of children and attibutes for this tag */
- void getChildrenAndAttributes(const TiXmlNode* node, std::vector<const TiXmlNode*> &children, std::vector<const TiXmlAttribute*> &attributes) const;
-
- /** Extract and return the value of the name attribute from a list of attributes */
- std::string extractName(std::vector<const TiXmlAttribute*> &attributes, const std::string &defaultName) const;
-
- /** Behaviour for unknown nodes. Default is to output a message letting the user know that a specific node is unknown */
- virtual void unknownNode(const TiXmlNode* node);
-
- /** Replace include declarations with the indicated file */
- bool replaceIncludes(TiXmlElement *elem);
-
- /** Print the error location, if known */
- void errorLocation(const TiXmlNode* node = NULL) const;
-
- /** Output an error message */
- void errorMessage(const std::string& msg) const;
-
- /** Compute the easy-access pointers inside the parsed datastructures */
- virtual void linkDatastructure(void);
-
- /** The name of the file where the parsing started */
- std::string m_source;
-
- /** A string that defines the base path for resources pointed to from the loaded document */
- std::string m_resourceLocation;
-
- /** The list of paths the parser knows about when it sees an <include> directive */
- std::vector<std::string> m_paths;
-
- /** The robot name */
- std::string m_name;
-
- /** Contains the list of parsed links and sensors */
- std::map<std::string, Link*> m_links;
-
- /** Contains the list of parsed groups */
- std::map<std::string, Group*> m_groups;
-
- /** Contains the list of parsed frames */
- std::map<std::string, Frame*> m_frames;
-
- /** The list of parsed chains */
- std::map<std::string, Chain*> m_chains;
-
- /** Contains information specified in <data> tags at the top level */
- Map m_data;
-
- /** Additional datastructure containing a list links that are connected to the environment */
- std::vector<Link*> m_linkRoots;
-
- /** The list of constants */
- std::map<std::string, std::string> m_constants;
-
- /** The list of constant blocks */
- std::map<std::string, const TiXmlNode*> m_constBlocks;
-
- /** The list of actuator tags */
- std::vector<const TiXmlElement*> m_actuators;
-
- /** The list of controller tags */
- std::vector<const TiXmlElement*> m_controllers;
-
- /** The list of transmission tags */
- std::vector<const TiXmlElement*> m_transmissions;
-
- /** If this is set to true, unknown tags will be simply added
- to a list, no error messages related to unknown tags will
- be presented. */
- bool m_rememberUnknownTags;
-
- /** The list of unknown tags (maintained only if rememberUnknownTags(true) was previosuly called) */
- std::vector<const TiXmlNode*> m_unknownTags;
-
- /** Verbosity flag */
- bool m_verbose;
-
- /** Counter for errors */
- mutable unsigned int m_errorCount;
-
- private:
-
- /* temporary storage for information during parsing; should not be used elsewhere */
-
- /** Temporary datastructure for keeping track link collision components */
- std::map<std::string, Link::Collision*> m_collision;
- /** Temporary datastructure for keeping track link joints */
- std::map<std::string, Link::Joint*> m_joints;
- /** Temporary datastructure for keeping track link inertial components */
- std::map<std::string, Link::Inertial*> m_inertial;
- /** Temporary datastructure for keeping track link visual components */
- std::map<std::string, Link::Visual*> m_visual;
- /** Temporary datastructure for keeping track link geometry */
- std::map<std::string, Link::Geometry*> m_geoms;
-
-
- /** List of nodes to be processed after loading the constants and templates (temporary) */
- std::vector<const TiXmlNode*> m_stage2;
- /** List of loaded documents. The documents are not cleared after parse since <data> tags may contain pointers to XML datastructures from these documents */
- std::vector<TiXmlDocument*> m_docs;
-
- /** Approximate location in parsed file, if known (used for error messages) */
- std::string m_location;
- };
-
-} // planning_models
-
-#endif //PLANNING_MODELS_URDF_PARSER
-
Modified: pkg/trunk/world_models/robot_models/planning_models/manifest.xml
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/manifest.xml 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/world_models/robot_models/planning_models/manifest.xml 2009-02-10 02:36:32 UTC (rev 10893)
@@ -8,8 +8,7 @@
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/RobotDescriptionFormat</url>
- <depend package="tinyxml"/>
- <depend package="math_expr"/>
+ <depend package="wg_robot_description_parser"/>
<depend package="bullet"/>
<export>
Modified: pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -146,7 +146,7 @@
// the joint remains identity
}
-void planning_models::KinematicModel::FixedJoint::extractInformation(const URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::FixedJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
{
// we need no data
}
@@ -162,7 +162,7 @@
varTrans.setRotation(newQuat);
}
-void planning_models::KinematicModel::PlanarJoint::extractInformation(const URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::PlanarJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
{
robot->stateBounds.insert(robot->stateBounds.end(), 4, 0.0);
robot->stateBounds.push_back(-M_PI);
@@ -176,7 +176,7 @@
varTrans.setRotation(btQuaternion(btScalar(params[3]), btScalar(params[4]), btScalar(params[5]), btScalar(params[6])));
}
-void planning_models::KinematicModel::FloatingJoint::extractInformation(const URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::FloatingJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
{
robot->stateBounds.insert(robot->stateBounds.end(), 14, 0.0);
robot->floatingJoints.push_back(robot->stateDimension);
@@ -189,7 +189,7 @@
varTrans.setOrigin(newOrigin);
}
-void planning_models::KinematicModel::PrismaticJoint::extractInformation(const URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::PrismaticJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
{
axis.setX(btScalar(urdfLink->joint->axis[0]));
axis.setY(btScalar(urdfLink->joint->axis[1]));
@@ -215,7 +215,7 @@
varTrans *= part3;
}
-void planning_models::KinematicModel::RevoluteJoint::extractInformation(const URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::RevoluteJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
{
axis.setX(urdfLink->joint->axis[0]);
axis.setY(urdfLink->joint->axis[1]);
@@ -240,33 +240,33 @@
robot->stateBounds.push_back(limit[1]);
}
-void planning_models::KinematicModel::Link::extractInformation(const URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::Link::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
{
/* compute the geometry for this link */
switch (urdfLink->collision->geometry->type)
{
- case URDF::Link::Geometry::BOX:
+ case robot_desc::URDF::Link::Geometry::BOX:
{
Box *box = new Box();
- const double *size = static_cast<const URDF::Link::Geometry::Box*>(urdfLink->collision->geometry->shape)->size;
+ const double *size = static_cast<const robot_desc::URDF::Link::Geometry::Box*>(urdfLink->collision->geometry->shape)->size;
box->size[0] = size[0];
box->size[1] = size[1];
box->size[2] = size[2];
shape = box;
}
break;
- case URDF::Link::Geometry::SPHERE:
+ case robot_desc::URDF::Link::Geometry::SPHERE:
{
Sphere *sphere = new Sphere();
- sphere->radius = static_cast<const URDF::Link::Geometry::Sphere*>(urdfLink->collision->geometry->shape)->radius;
+ sphere->radius = static_cast<const robot_desc::URDF::Link::Geometry::Sphere*>(urdfLink->collision->geometry->shape)->radius;
shape = sphere;
}
break;
- case URDF::Link::Geometry::CYLINDER:
+ case robot_desc::URDF::Link::Geometry::CYLINDER:
{
Cylinder *cylinder = new Cylinder();
- cylinder->length = static_cast<const URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->length;
- cylinder->radius = static_cast<const URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->radius;
+ cylinder->length = static_cast<const robot_desc::URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->length;
+ cylinder->radius = static_cast<const robot_desc::URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->radius;
shape = cylinder;
}
break;
@@ -291,7 +291,7 @@
m_verbose = verbose;
}
-void planning_models::KinematicModel::constructGroupList(const URDF &model)
+void planning_models::KinematicModel::constructGroupList(const robot_desc::URDF &model)
{
std::string rname = model.getRobotName();
std::vector<std::string> allGroups;
@@ -358,13 +358,13 @@
void planning_models::KinematicModel::build(const std::string &description, bool ignoreSensors)
{
- URDF *file = new URDF();
+ robot_desc::URDF *file = new robot_desc::URDF();
file->loadString(description.c_str());
build(*file, ignoreSensors);
delete file;
}
-void planning_models::KinematicModel::build(const URDF &model, bool ignoreSensors)
+void planning_models::KinematicModel::build(const robot_desc::URDF &model, bool ignoreSensors)
{
if (m_built)
{
@@ -383,7 +383,7 @@
for (unsigned int i = 0 ; i < model.getDisjointPartCount() ; ++i)
{
- const URDF::Link *link = model.getDisjointPart(i);
+ const robot_desc::URDF::Link *link = model.getDisjointPart(i);
if (link->canSense() && m_ignoreSensors)
continue;
Robot *rb = new Robot(this);
@@ -503,7 +503,7 @@
names.push_back(joints[i]->name);
}
-void planning_models::KinematicModel::buildChainJ(Robot *robot, Link *parent, Joint* joint, const URDF::Link* urdfLink, const URDF &model)
+void planning_models::KinematicModel::buildChainJ(Robot *robot, Link *parent, Joint* joint, const robot_desc::URDF::Link* urdfLink, const robot_desc::URDF &model)
{
joint->before = parent;
joint->after = new Link();
@@ -546,7 +546,7 @@
buildChainL(robot, joint, joint->after, urdfLink, model);
}
-void planning_models::KinematicModel::buildChainL(Robot *robot, Joint *parent, Link* link, const URDF::Link* urdfLink, const URDF &model)
+void planning_models::KinematicModel::buildChainL(Robot *robot, Joint *parent, Link* link, const robot_desc::URDF::Link* urdfLink, const robot_desc::URDF &model)
{
link->name = urdfLink->name;
link->before = parent;
@@ -565,24 +565,24 @@
}
}
-planning_models::KinematicModel::Joint* planning_models::KinematicModel::createJoint(const URDF::Link* urdfLink)
+planning_models::KinematicModel::Joint* planning_models::KinematicModel::createJoint(const robot_desc::URDF::Link* urdfLink)
{
Joint *newJoint = NULL;
switch (urdfLink->joint->type)
{
- case URDF::Link::Joint::FIXED:
+ case robot_desc::URDF::Link::Joint::FIXED:
newJoint = new FixedJoint();
break;
- case URDF::Link::Joint::FLOATING:
+ case robot_desc::URDF::Link::Joint::FLOATING:
newJoint = new FloatingJoint();
break;
- case URDF::Link::Joint::PLANAR:
+ case robot_desc::URDF::Link::Joint::PLANAR:
newJoint = new PlanarJoint();
break;
- case URDF::Link::Joint::PRISMATIC:
+ case robot_desc::URDF::Link::Joint::PRISMATIC:
newJoint = new PrismaticJoint();
break;
- case URDF::Link::Joint::REVOLUTE:
+ case robot_desc::URDF::Link::Joint::REVOLUTE:
newJoint = new RevoluteJoint();
break;
default:
Deleted: pkg/trunk/world_models/robot_models/planning_models/src/urdf.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/src/urdf.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/world_models/robot_models/planning_models/src/urdf.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -1,2456 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/** \author Ioan Sucan */
-
-#include <planning_models/urdf.h>
-#include <math_expr/MathExpression.h>
-#include <boost/algorithm/string.hpp>
-#include <boost/lexical_cast.hpp>
-#include <algorithm>
-#include <cstring>
-#include <fstream>
-#include <sstream>
-#include <queue>
-
-
-namespace planning_models {
-
- /* Macro to mark the fact a certain member variable was set. Also
- prints a warning if the same member was set multiple times. */
-#define MARK_SET(node, owner, variable) \
- { \
- if (owner->isSet[#variable]) { \
- errorMessage("'" + std::string(#variable) + \
- "' already set"); \
- errorLocation(node); } \
- else \
- owner->isSet[#variable] = true; \
- }
-
-
- /* Operator for sorting objects by name */
- template<typename T>
- struct SortByName
- {
- bool operator()(const T *a, const T *b) const
- {
- return a->name < b->name;
- }
- };
-
- void URDF::freeMemory(void)
- {
- clearDocs();
-
- for (std::map<std::string, Link*>::iterator i = m_links.begin() ; i != m_links.end() ; i++)
- delete i->second;
- for (std::map<std::string, Group*>::iterator i = m_groups.begin() ; i != m_groups.end() ; i++)
- delete i->second;
- for (std::map<std::string, Frame*>::iterator i = m_frames.begin() ; i != m_frames.end() ; i++)
- delete i->second;
- for (std::map<std::string, Chain*>::iterator i = m_chains.begin() ; i != m_chains.end() ; i++)
- delete i->second;
- }
-
- void URDF::clear(void)
- {
- freeMemory();
- m_name.clear();
- m_source.clear();
- m_resourceLocation.clear();
- m_location.clear();
- m_links.clear();
- m_frames.clear();
- m_groups.clear();
- m_chains.clear();
- m_paths.clear();
- m_paths.push_back("");
- m_linkRoots.clear();
- clearTemporaryData();
- m_errorCount = 0;
- }
-
- void URDF::setVerbose(bool verbose)
- {
- m_verbose = verbose;
- }
-
- void URDF::rememberUnknownTags(bool remember)
- {
- m_rememberUnknownTags = remember;
- }
-
- unsigned int URDF::getErrorCount(void) const
- {
- return m_errorCount;
- }
-
- const std::string& URDF::getRobotName(void) const
- {
- return m_name;
- }
-
- std::string URDF::getResourceLocation(void) const
- {
- return m_resourceLocation;
- }
-
- URDF::Link* URDF::getLink(const std::string &name) const
- {
- std::map<std::string, Link*>::const_iterator it = m_links.find(name);
- return it == m_links.end() ? NULL : it->second;
- }
-
- URDF::Link* URDF::getJointLink(const std::string &name) const
- {
- for (std::map<std::string, Link*>::const_iterator i = m_links.begin() ; i != m_links.end() ; i++)
- if (i->second->joint->name == name)
- return i->second;
- return NULL;
- }
-
- void URDF::getLinks(std::vector<Link*> &links) const
- {
- std::vector<Link*> localLinks;
- for (std::map<std::string, Link*>::const_iterator i = m_links.begin() ; i != m_links.end() ; i++)
- localLinks.push_back(i->second);
- std::sort(localLinks.begin(), localLinks.end(), SortByName<Link>());
- links.insert(links.end(), localLinks.begin(), localLinks.end());
- }
-
- URDF::Frame* URDF::getFrame(const std::string &name) const
- {
- std::map<std::string, Frame*>::const_iterator it = m_frames.find(name);
- return it == m_frames.end() ? NULL : it->second;
- }
-
- void URDF::getFrames(std::vector<Frame*> &frames) const
- {
- std::vector<Frame*> localFrames;
- for (std::map<std::string, Frame*>::const_iterator i = m_frames.begin() ; i != m_frames.end() ; i++)
- localFrames.push_back(i->second);
- std::sort(localFrames.begin(), localFrames.end(), SortByName<Frame>());
- frames.insert(frames.end(), localFrames.begin(), localFrames.end());
- }
-
- unsigned int URDF::getDisjointPartCount(void) const
- {
- return m_linkRoots.size();
- }
-
- URDF::Link* URDF::getDisjointPart(unsigned int index) const
- {
- if (index < m_linkRoots.size())
- return m_linkRoots[index];
- else
- return NULL;
- }
-
- bool URDF::isRoot(const Link* link) const
- {
- for (unsigned int i = 0 ; i < m_linkRoots.size() ; ++i)
- if (link == m_linkRoots[i])
- return true;
- return false;
- }
-
- void URDF::getGroupNames(std::vector<std::string> &groups) const
- {
- std::vector<std::string> localGroups;
- for (std::map<std::string, Group*>::con...
[truncated message content] |
