urdf_vehicle_kinematic.cpp
Go to the documentation of this file.
00001 
00002 #include <urdf_vehicle_kinematic/urdf_vehicle_kinematic.h>
00003 
00004 static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2)
00005 {
00006   return std::sqrt(std::pow(vec1.x-vec2.x,2) +
00007                    std::pow(vec1.y-vec2.y,2) +
00008                    std::pow(vec1.z-vec2.z,2));
00009 }
00010 
00011 /*
00012  * \brief Check if the link is modeled as a cylinder
00013  * \param link Link
00014  * \return true if the link is modeled as a Cylinder; false otherwise
00015  */
00016 static bool isCylinder(const boost::shared_ptr<const urdf::Link>& link)
00017 {
00018   if (!link)
00019   {
00020     ROS_ERROR("Link == NULL.");
00021     return false;
00022   }
00023 
00024   if (!link->collision)
00025   {
00026     ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf.");
00027     return false;
00028   }
00029 
00030   if (!link->collision->geometry)
00031   {
00032     ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
00033     return false;
00034   }
00035 
00036   if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
00037   {
00038     ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry");
00039     return false;
00040   }
00041 
00042   return true;
00043 }
00044 
00045 /*
00046  * \brief Get the wheel radius
00047  * \param [in]  wheel_link   Wheel link
00048  * \param [out] wheel_radius Wheel radius [m]
00049  * \return true if the wheel radius was found; false otherwise
00050  */
00051 static bool getWheelRadius(const boost::shared_ptr<const urdf::Link>& wheel_link, double& wheel_radius)
00052 {
00053   if (!isCylinder(wheel_link))
00054   {
00055     ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!");
00056     return false;
00057   }
00058 
00059   wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
00060   return true;
00061 }
00062 
00063 namespace urdf_vehicle_kinematic{
00064   UrdfVehicleKinematic::UrdfVehicleKinematic(ros::NodeHandle& root_nh, const std::string& base_link):
00065     base_link_(base_link)
00066   {
00067     // Parse robot description
00068     const std::string model_param_name = "robot_description";
00069     bool res = root_nh.hasParam(model_param_name);
00070     std::string robot_model_str="";
00071     if (!res || !root_nh.getParam(model_param_name,robot_model_str))
00072     {
00073       ROS_ERROR("Robot descripion couldn't be retrieved from param server.");
00074     }
00075     else
00076     {
00077       model_ = urdf::parseURDF(robot_model_str);
00078       if(!model_)
00079         ROS_ERROR_STREAM("Could not parse the urdf robot model "<<model_param_name);
00080     }
00081   }
00082 
00083   bool UrdfVehicleKinematic::getTransformVector(const std::string& joint_name, const std::string& parent_link_name
00084                                                 , urdf::Vector3 &transform_vector)
00085   {
00086     if(model_)
00087     {
00088       boost::shared_ptr<const urdf::Joint> joint(model_->getJoint(joint_name));
00089       if (!joint)
00090       {
00091         ROS_ERROR_STREAM(joint_name
00092                                << " couldn't be retrieved from model description");
00093         return false;
00094       }
00095 
00096       transform_vector = joint->parent_to_joint_origin_transform.position;
00097       while(joint->parent_link_name != parent_link_name)
00098       {
00099         boost::shared_ptr<const urdf::Link> link_parent(model_->getLink(joint->parent_link_name));
00100         if (!link_parent || !link_parent->parent_joint)
00101         {
00102           ROS_ERROR_STREAM(joint->parent_link_name
00103                                  << " couldn't be retrieved from model description or his parent joint");
00104           return false;
00105         }
00106         joint = link_parent->parent_joint;
00107         transform_vector = transform_vector + joint->parent_to_joint_origin_transform.position;
00108       }
00109       return true;
00110     }
00111     else
00112       return false;
00113   }
00114 
00115   bool UrdfVehicleKinematic::getDistanceBetweenJoints(const std::string& first_joint_name,
00116                                                       const std::string& second_joint_name,
00117                                                       double& distance)
00118   {
00119     urdf::Vector3 first_transform;
00120     if(!getTransformVector(first_joint_name, base_link_, first_transform))
00121       return false;
00122 
00123     urdf::Vector3 second_transform;
00124     if(!getTransformVector(second_joint_name, base_link_, second_transform))
00125       return false;
00126 
00127     distance = euclideanOfVectors(first_transform,
00128                                second_transform);
00129     ROS_INFO_STREAM("first_transform : "<<first_transform.x<<","<<first_transform.y);
00130     ROS_INFO_STREAM("distance "<<distance);
00131     return true;
00132   }
00133 
00134   bool UrdfVehicleKinematic::getJointRadius(const std::string& joint_name,
00135                                             double& radius)
00136   {
00137     if(model_)
00138     {
00139       boost::shared_ptr<const urdf::Joint> joint(model_->getJoint(joint_name));
00140       // Get wheel radius
00141       if (!getWheelRadius(model_->getLink(joint->child_link_name), radius))
00142       {
00143         ROS_ERROR_STREAM("Couldn't retrieve " << joint_name << " wheel radius");
00144         return false;
00145       }
00146       return true;
00147     }
00148     else
00149       return false;
00150   }
00151 
00152   bool UrdfVehicleKinematic::getJointSteeringLimits(const std::string& joint_name,
00153                               double& steering_limit)
00154   {
00155     if(model_)
00156     {
00157       boost::shared_ptr<const urdf::Joint> joint(model_->getJoint(joint_name));
00158       if(joint->type == urdf::Joint::REVOLUTE)
00159       {
00160         const double lower_steering_limit = fabs(joint->limits->lower);
00161         const double upper_steering_limit = fabs(joint->limits->upper);
00162         if(lower_steering_limit > upper_steering_limit)
00163           steering_limit = upper_steering_limit;
00164         else
00165           steering_limit = lower_steering_limit;
00166         ROS_INFO_STREAM("Joint "<<joint_name<<" steering limit is "<<steering_limit*180.0/M_PI<<" in degrees");
00167         return true;
00168       }
00169       ROS_ERROR_STREAM("Couldn't get joint "<<joint_name<<" steering limit, is it of type REVOLUTE ?");
00170     }
00171     return false;
00172   }
00173 }


urdf_vehicle_kinematic
Author(s): Vincent Rousseau
autogenerated on Sat Jun 8 2019 20:06:17