Skip to content

Commit

Permalink
Merge pull request #13 from scpeters/pointer_typedef
Browse files Browse the repository at this point in the history
Create typedefs for shared pointers
  • Loading branch information
isucan committed Sep 24, 2015
2 parents 048f1af + 1f87c56 commit c70eb5b
Show file tree
Hide file tree
Showing 10 changed files with 313 additions and 51 deletions.
14 changes: 7 additions & 7 deletions urdf_model/include/urdf_model/joint.h
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,9 @@

#include <string>
#include <vector>
#include <boost/shared_ptr.hpp>

#include "urdf_model/pose.h"
#include "urdf_model/types.h"


namespace urdf{
Expand Down Expand Up @@ -138,7 +138,7 @@ class JointCalibration
public:
JointCalibration() { this->clear(); };
double reference_position;
boost::shared_ptr<double> rising, falling;
DoubleSharedPtr rising, falling;

void clear()
{
Expand Down Expand Up @@ -196,19 +196,19 @@ class Joint
Pose parent_to_joint_origin_transform;

/// Joint Dynamics
boost::shared_ptr<JointDynamics> dynamics;
JointDynamicsSharedPtr dynamics;

/// Joint Limits
boost::shared_ptr<JointLimits> limits;
JointLimitsSharedPtr limits;

/// Unsupported Hidden Feature
boost::shared_ptr<JointSafety> safety;
JointSafetySharedPtr safety;

/// Unsupported Hidden Feature
boost::shared_ptr<JointCalibration> calibration;
JointCalibrationSharedPtr calibration;

/// Option to Mimic another Joint
boost::shared_ptr<JointMimic> mimic;
JointMimicSharedPtr mimic;

void clear()
{
Expand Down
31 changes: 15 additions & 16 deletions urdf_model/include/urdf_model/link.h
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,10 @@
#include <string>
#include <vector>
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>

#include "joint.h"
#include "color.h"
#include "types.h"

namespace urdf{

Expand Down Expand Up @@ -150,10 +149,10 @@ class Visual
public:
Visual() { this->clear(); };
Pose origin;
boost::shared_ptr<Geometry> geometry;
GeometrySharedPtr geometry;

std::string material_name;
boost::shared_ptr<Material> material;
MaterialSharedPtr material;

void clear()
{
Expand All @@ -172,7 +171,7 @@ class Collision
public:
Collision() { this->clear(); };
Pose origin;
boost::shared_ptr<Geometry> geometry;
GeometrySharedPtr geometry;

void clear()
{
Expand All @@ -194,32 +193,32 @@ class Link
std::string name;

/// inertial element
boost::shared_ptr<Inertial> inertial;
InertialSharedPtr inertial;

/// visual element
boost::shared_ptr<Visual> visual;
VisualSharedPtr visual;

/// collision element
boost::shared_ptr<Collision> collision;
CollisionSharedPtr collision;

/// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array)
std::vector<boost::shared_ptr<Collision> > collision_array;
std::vector<CollisionSharedPtr> collision_array;

/// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array)
std::vector<boost::shared_ptr<Visual> > visual_array;
std::vector<VisualSharedPtr> visual_array;

/// Parent Joint element
/// explicitly stating "parent" because we want directional-ness for tree structure
/// every link can have one parent
boost::shared_ptr<Joint> parent_joint;
JointSharedPtr parent_joint;

std::vector<boost::shared_ptr<Joint> > child_joints;
std::vector<boost::shared_ptr<Link> > child_links;
std::vector<JointSharedPtr> child_joints;
std::vector<LinkSharedPtr> child_links;

boost::shared_ptr<Link> getParent() const
LinkSharedPtr getParent() const
{return parent_link_.lock();};

void setParent(const boost::shared_ptr<Link> &parent)
void setParent(const LinkSharedPtr &parent)
{ parent_link_ = parent; }

void clear()
Expand All @@ -236,7 +235,7 @@ class Link
};

private:
boost::weak_ptr<Link> parent_link_;
LinkWeakPtr parent_link_;

};

Expand Down
37 changes: 19 additions & 18 deletions urdf_model/include/urdf_model/model.h
Original file line number Diff line number Diff line change
Expand Up @@ -41,27 +41,28 @@
#include <map>
#include <boost/function.hpp>
#include <urdf_model/link.h>
#include <urdf_model/types.h>
#include <urdf_exception/exception.h>

namespace urdf {

class ModelInterface
{
public:
boost::shared_ptr<const Link> getRoot(void) const{return this->root_link_;};
boost::shared_ptr<const Link> getLink(const std::string& name) const
LinkConstSharedPtr getRoot(void) const{return this->root_link_;};
LinkConstSharedPtr getLink(const std::string& name) const
{
boost::shared_ptr<const Link> ptr;
LinkConstSharedPtr ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
return ptr;
};

boost::shared_ptr<const Joint> getJoint(const std::string& name) const
JointConstSharedPtr getJoint(const std::string& name) const
{
boost::shared_ptr<const Joint> ptr;
JointConstSharedPtr ptr;
if (this->joints_.find(name) == this->joints_.end())
ptr.reset();
else
Expand All @@ -71,9 +72,9 @@ class ModelInterface


const std::string& getName() const {return name_;};
void getLinks(std::vector<boost::shared_ptr<Link> >& links) const
void getLinks(std::vector<LinkSharedPtr >& links) const
{
for (std::map<std::string,boost::shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
for (std::map<std::string,LinkSharedPtr>::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
{
links.push_back(link->second);
}
Expand All @@ -89,9 +90,9 @@ class ModelInterface
};

/// non-const getLink()
void getLink(const std::string& name,boost::shared_ptr<Link> &link) const
void getLink(const std::string& name, LinkSharedPtr &link) const
{
boost::shared_ptr<Link> ptr;
LinkSharedPtr ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
Expand All @@ -100,9 +101,9 @@ class ModelInterface
};

/// non-const getMaterial()
boost::shared_ptr<Material> getMaterial(const std::string& name) const
MaterialSharedPtr getMaterial(const std::string& name) const
{
boost::shared_ptr<Material> ptr;
MaterialSharedPtr ptr;
if (this->materials_.find(name) == this->materials_.end())
ptr.reset();
else
Expand All @@ -113,7 +114,7 @@ class ModelInterface
void initTree(std::map<std::string, std::string> &parent_link_tree)
{
// loop through all joints, for every link, assign children links and children joints
for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
for (std::map<std::string, JointSharedPtr>::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
{
std::string parent_link_name = joint->second->parent_link_name;
std::string child_link_name = joint->second->child_link_name;
Expand All @@ -125,7 +126,7 @@ class ModelInterface
else
{
// find child and parent links
boost::shared_ptr<Link> child_link, parent_link;
LinkSharedPtr child_link, parent_link;
this->getLink(child_link_name, child_link);
if (!child_link)
{
Expand Down Expand Up @@ -160,7 +161,7 @@ class ModelInterface
this->root_link_.reset();

// find the links that have no parent in the tree
for (std::map<std::string, boost::shared_ptr<Link> >::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++)
for (std::map<std::string, LinkSharedPtr>::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++)
{
std::map<std::string, std::string >::const_iterator parent = parent_link_tree.find(l->first);
if (parent == parent_link_tree.end())
Expand All @@ -185,17 +186,17 @@ class ModelInterface


/// \brief complete list of Links
std::map<std::string, boost::shared_ptr<Link> > links_;
std::map<std::string, LinkSharedPtr> links_;
/// \brief complete list of Joints
std::map<std::string, boost::shared_ptr<Joint> > joints_;
std::map<std::string, JointSharedPtr> joints_;
/// \brief complete list of Materials
std::map<std::string, boost::shared_ptr<Material> > materials_;
std::map<std::string, MaterialSharedPtr> materials_;

/// \brief The name of the robot model
std::string name_;

/// \brief The root is always a link (the parent of the tree describing the robot)
boost::shared_ptr<Link> root_link_;
LinkSharedPtr root_link_;



Expand Down
106 changes: 106 additions & 0 deletions urdf_model/include/urdf_model/types.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,106 @@
/*********************************************************************
* 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: Steve Peters */

#ifndef URDF_MODEL_TYPES_H
#define URDF_MODEL_TYPES_H

#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>


namespace urdf{

// shared pointer used in joint.h
typedef boost::shared_ptr<double> DoubleSharedPtr;

class Collision;
class Geometry;
class Inertial;
class Joint;
class JointCalibration;
class JointDynamics;
class JointLimits;
class JointMimic;
class JointSafety;
class Link;
class Material;
class Visual;

// typedef shared pointers
typedef boost::shared_ptr<Collision> CollisionSharedPtr;
typedef boost::shared_ptr<Geometry> GeometrySharedPtr;
typedef boost::shared_ptr<Inertial> InertialSharedPtr;
typedef boost::shared_ptr<Joint> JointSharedPtr;
typedef boost::shared_ptr<JointCalibration> JointCalibrationSharedPtr;
typedef boost::shared_ptr<JointDynamics> JointDynamicsSharedPtr;
typedef boost::shared_ptr<JointLimits> JointLimitsSharedPtr;
typedef boost::shared_ptr<JointMimic> JointMimicSharedPtr;
typedef boost::shared_ptr<JointSafety> JointSafetySharedPtr;
typedef boost::shared_ptr<Link> LinkSharedPtr;
typedef boost::shared_ptr<Material> MaterialSharedPtr;
typedef boost::shared_ptr<Visual> VisualSharedPtr;

// typedef const shared pointers
typedef boost::shared_ptr<const Collision> CollisionConstSharedPtr;
typedef boost::shared_ptr<const Geometry> GeometryConstSharedPtr;
typedef boost::shared_ptr<const Inertial> InertialConstSharedPtr;
typedef boost::shared_ptr<const Joint> JointConstSharedPtr;
typedef boost::shared_ptr<const JointCalibration> JointCalibrationConstSharedPtr;
typedef boost::shared_ptr<const JointDynamics> JointDynamicsConstSharedPtr;
typedef boost::shared_ptr<const JointLimits> JointLimitsConstSharedPtr;
typedef boost::shared_ptr<const JointMimic> JointMimicConstSharedPtr;
typedef boost::shared_ptr<const JointSafety> JointSafetyConstSharedPtr;
typedef boost::shared_ptr<const Link> LinkConstSharedPtr;
typedef boost::shared_ptr<const Material> MaterialConstSharedPtr;
typedef boost::shared_ptr<const Visual> VisualConstSharedPtr;

// typedef weak pointers
typedef boost::weak_ptr<Collision> CollisionWeakPtr;
typedef boost::weak_ptr<Geometry> GeometryWeakPtr;
typedef boost::weak_ptr<Inertial> InertialWeakPtr;
typedef boost::weak_ptr<Joint> JointWeakPtr;
typedef boost::weak_ptr<JointCalibration> JointCalibrationWeakPtr;
typedef boost::weak_ptr<JointDynamics> JointDynamicsWeakPtr;
typedef boost::weak_ptr<JointLimits> JointLimitsWeakPtr;
typedef boost::weak_ptr<JointMimic> JointMimicWeakPtr;
typedef boost::weak_ptr<JointSafety> JointSafetyWeakPtr;
typedef boost::weak_ptr<Link> LinkWeakPtr;
typedef boost::weak_ptr<Material> MaterialWeakPtr;
typedef boost::weak_ptr<Visual> VisualWeakPtr;

}

#endif
5 changes: 2 additions & 3 deletions urdf_model_state/include/urdf_model_state/model_state.h
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,10 @@
#include <string>
#include <vector>
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>

#include "urdf_model/pose.h"
#include <urdf_model/twist.h>
#include "urdf_model_state/types.h"


namespace urdf{
Expand Down Expand Up @@ -131,7 +130,7 @@ class ModelState
this->joint_states.clear();
};

std::vector<boost::shared_ptr<JointState> > joint_states;
std::vector<JointStateSharedPtr> joint_states;

};

Expand Down
Loading

0 comments on commit c70eb5b

Please sign in to comment.