Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add support for spherical wheels #21

Open
wants to merge 1 commit into
base: kinetic-devel
Choose a base branch
from
Open

add support for spherical wheels #21

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
66 changes: 53 additions & 13 deletions src/urdf_geometry_parser.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,15 @@
namespace urdf_geometry_parser{

/*
* \brief Check if the link is modeled as a cylinder
* \param link Link
* \return true if the link is modeled as a Cylinder; false otherwise
*/
static bool isCylinder(urdf::LinkConstSharedPtr& link)
* \brief Check that a link exists and has a geometry collision.
* \param link The link
* \return true if the link has a collision element with geometry
*/
static bool hasCollisionGeometry(const urdf::LinkConstSharedPtr& link)
{
if (!link)
{
ROS_ERROR("Link == NULL.");
ROS_ERROR("Link pointer is null.");
return false;
}

Expand All @@ -62,10 +62,45 @@ namespace urdf_geometry_parser{
ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
return false;
}
return true;
}

/*
* \brief Check if the link is modeled as a cylinder
* \param link Link
* \return true if the link is modeled as a Cylinder; false otherwise
*/
static bool isCylinder(const urdf::LinkConstSharedPtr& link)
{
if (!hasCollisionGeometry(link))
{
return false;
}

if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
{
ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry");
ROS_DEBUG_STREAM("Link " << link->name << " does not have cylinder geometry");
return false;
}

return true;
}

/*
* \brief Check if the link is modeled as a sphere
* \param link Link
* \return true if the link is modeled as a Sphere; false otherwise
*/
static bool isSphere(const urdf::LinkConstSharedPtr& link)
{
if (!hasCollisionGeometry(link))
{
return false;
}

if (link->collision->geometry->type != urdf::Geometry::SPHERE)
{
ROS_DEBUG_STREAM("Link " << link->name << " does not have sphere geometry");
return false;
}

Expand All @@ -78,16 +113,21 @@ namespace urdf_geometry_parser{
* \param [out] wheel_radius Wheel radius [m]
* \return true if the wheel radius was found; false otherwise
*/
static bool getWheelRadius(urdf::LinkConstSharedPtr wheel_link, double& wheel_radius)
static bool getWheelRadius(const urdf::LinkConstSharedPtr& wheel_link, double& wheel_radius)
{
if (!isCylinder(wheel_link))
if (isCylinder(wheel_link))
{
ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!");
return false;
wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
return true;
}
else if (isSphere(wheel_link))
{
wheel_radius = (static_cast<urdf::Sphere*>(wheel_link->collision->geometry.get()))->radius;
return true;
}

wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
return true;
ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder or sphere!");
return false;
}


Expand Down