urdf_geometry_parser.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Irstea
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Irstea nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 
37 #include <cmath>
38 
39 namespace urdf_geometry_parser{
40 
41  /*
42  * \brief Check if the link is modeled as a cylinder
43  * \param link Link
44  * \return true if the link is modeled as a Cylinder; false otherwise
45  */
46  static bool isCylinder(urdf::LinkConstSharedPtr& link)
47  {
48  if (!link)
49  {
50  ROS_ERROR("Link == NULL.");
51  return false;
52  }
53 
54  if (!link->collision)
55  {
56  ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf.");
57  return false;
58  }
59 
60  if (!link->collision->geometry)
61  {
62  ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
63  return false;
64  }
65 
66  if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
67  {
68  ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry");
69  return false;
70  }
71 
72  return true;
73  }
74 
75  /*
76  * \brief Get the wheel radius
77  * \param [in] wheel_link Wheel link
78  * \param [out] wheel_radius Wheel radius [m]
79  * \return true if the wheel radius was found; false otherwise
80  */
81  static bool getWheelRadius(urdf::LinkConstSharedPtr wheel_link, double& wheel_radius)
82  {
83  if (!isCylinder(wheel_link))
84  {
85  ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!");
86  return false;
87  }
88 
89  wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
90  return true;
91  }
92 
93 
94  UrdfGeometryParser::UrdfGeometryParser(ros::NodeHandle& root_nh, const std::string& base_link):
95  base_link_(base_link)
96  {
97  // Parse robot description
98  const std::string model_param_name = "robot_description";
99  bool res = root_nh.hasParam(model_param_name);
100  std::string robot_model_str="";
101  if (!res || !root_nh.getParam(model_param_name,robot_model_str))
102  {
103  ROS_ERROR("Robot descripion couldn't be retrieved from param server.");
104  }
105  else
106  {
107  model_ = urdf::parseURDF(robot_model_str);
108  if(!model_)
109  ROS_ERROR_STREAM("Could not parse the urdf robot model "<<model_param_name);
110  }
111  }
112 
113  bool UrdfGeometryParser::getTransformVector(const std::string& joint_name, const std::string& parent_link_name
114  , urdf::Vector3 &transform_vector)
115  {
116  if(model_)
117  {
118  urdf::JointConstSharedPtr joint(model_->getJoint(joint_name));
119  if (!joint)
120  {
121  ROS_ERROR_STREAM(joint_name
122  << " couldn't be retrieved from model description");
123  return false;
124  }
125 
126  transform_vector = joint->parent_to_joint_origin_transform.position;
127  while(joint->parent_link_name != parent_link_name)
128  {
129  urdf::LinkConstSharedPtr link_parent(model_->getLink(joint->parent_link_name));
130  if (!link_parent || !link_parent->parent_joint)
131  {
132  ROS_ERROR_STREAM(joint->parent_link_name
133  << " couldn't be retrieved from model description or his parent joint");
134  return false;
135  }
136  joint = link_parent->parent_joint;
137  transform_vector = transform_vector + joint->parent_to_joint_origin_transform.position;
138  }
139  return true;
140  }
141  else
142  return false;
143  }
144 
145  bool UrdfGeometryParser::getDistanceBetweenJoints(const std::string& first_joint_name,
146  const std::string& second_joint_name,
147  double& distance)
148  {
149  urdf::Vector3 first_transform;
150  if(!getTransformVector(first_joint_name, base_link_, first_transform))
151  return false;
152 
153  urdf::Vector3 second_transform;
154  if(!getTransformVector(second_joint_name, base_link_, second_transform))
155  return false;
156 
157  // Calculate the Euclidean distance using the Pythagorean formula
158  distance = std::sqrt(std::pow(first_transform.x - second_transform.x, 2)
159  + std::pow(first_transform.y - second_transform.y, 2));
160  return true;
161  }
162 
163  bool UrdfGeometryParser::getJointRadius(const std::string& joint_name,
164  double& radius)
165  {
166  if(model_)
167  {
168  urdf::JointConstSharedPtr joint(model_->getJoint(joint_name));
169  // Get wheel radius
170  if (!getWheelRadius(model_->getLink(joint->child_link_name), radius))
171  {
172  ROS_ERROR_STREAM("Couldn't retrieve " << joint_name << " wheel radius");
173  return false;
174  }
175  return true;
176  }
177  else
178  return false;
179  }
180 
181  bool UrdfGeometryParser::getJointSteeringLimits(const std::string& joint_name,
182  double& steering_limit)
183  {
184  if(model_)
185  {
186  urdf::JointConstSharedPtr joint(model_->getJoint(joint_name));
187  if(joint->type == urdf::Joint::REVOLUTE)
188  {
189  const double lower_steering_limit = fabs(joint->limits->lower);
190  const double upper_steering_limit = fabs(joint->limits->upper);
191  if(lower_steering_limit > upper_steering_limit)
192  steering_limit = upper_steering_limit;
193  else
194  steering_limit = lower_steering_limit;
195  ROS_DEBUG_STREAM("Joint "<<joint_name<<" steering limit is "<<steering_limit*180.0/M_PI<<" in degrees");
196  return true;
197  }
198  ROS_ERROR_STREAM("Couldn't get joint "<<joint_name<<" steering limit, is it of type REVOLUTE ?");
199  }
200  return false;
201  }
202 }
static bool getWheelRadius(urdf::LinkConstSharedPtr wheel_link, double &wheel_radius)
static bool isCylinder(urdf::LinkConstSharedPtr &link)
bool getTransformVector(const std::string &joint_name, const std::string &parent_link_name, urdf::Vector3 &transform_vector)
Get transform vector between the joint and parent_link.
bool getJointSteeringLimits(const std::string &joint_name, double &steering_limit)
Get joint steering limit from the URDF considering the upper and lower limit is the same...
#define ROS_DEBUG_STREAM(args)
UrdfGeometryParser(ros::NodeHandle &root_nh, const std::string &base_link)
bool getJointRadius(const std::string &joint_name, double &radius)
Get joint radius from the URDF.
bool getParam(const std::string &key, std::string &s) const
bool getDistanceBetweenJoints(const std::string &first_joint_name, const std::string &second_joint_name, double &distance)
Get distance between two joints from the URDF.
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)


urdf_geometry_parser
Author(s): Vincent Rousseau
autogenerated on Thu Feb 4 2021 03:11:52