void PhysicsVehicleWheel::getConnectionDefault(Vector3* result) const
{
// projected strut length
getWheelDirection(result);
result->normalize();
float length = 0.58f * getStrutRestLength();
*result *= -length;
// nudge wheel contact point to outer edge of tire for stability
Vector3 nudge;
getWheelAxle(&nudge);
nudge *= nudge.dot(_initialOffset);
nudge.normalize();
*result += nudge * 0.068f * getWheelRadius(); // rough-in for tire width
// offset at bind time
*result += _initialOffset;
}
bool DiffDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
const std::string& left_wheel_name,
const std::string& right_wheel_name,
bool lookup_wheel_separation,
bool lookup_wheel_radius)
{
if (!(lookup_wheel_separation || lookup_wheel_radius))
{
// Short-circuit in case we don't need to look up anything, so we don't have to parse the URDF
return true;
}
// Parse robot description
const std::string model_param_name = "robot_description";
bool res = root_nh.hasParam(model_param_name);
std::string robot_model_str="";
if (!res || !root_nh.getParam(model_param_name,robot_model_str))
{
ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server.");
return false;
}
boost::shared_ptr<urdf::ModelInterface> model(urdf::parseURDF(robot_model_str));
boost::shared_ptr<const urdf::Joint> left_wheel_joint(model->getJoint(left_wheel_name));
boost::shared_ptr<const urdf::Joint> right_wheel_joint(model->getJoint(right_wheel_name));
if (lookup_wheel_separation)
{
// Get wheel separation
if (!left_wheel_joint)
{
ROS_ERROR_STREAM_NAMED(name_, left_wheel_name
<< " couldn't be retrieved from model description");
return false;
}
if (!right_wheel_joint)
{
ROS_ERROR_STREAM_NAMED(name_, right_wheel_name
<< " couldn't be retrieved from model description");
return false;
}
ROS_INFO_STREAM("left wheel to origin: " << left_wheel_joint->parent_to_joint_origin_transform.position.x << ","
<< left_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
<< left_wheel_joint->parent_to_joint_origin_transform.position.z);
ROS_INFO_STREAM("right wheel to origin: " << right_wheel_joint->parent_to_joint_origin_transform.position.x << ","
<< right_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
<< right_wheel_joint->parent_to_joint_origin_transform.position.z);
wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position,
right_wheel_joint->parent_to_joint_origin_transform.position);
}
if (lookup_wheel_radius)
{
// Get wheel radius
if (!getWheelRadius(model->getLink(left_wheel_joint->child_link_name), wheel_radius_))
{
ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius");
return false;
}
}
return true;
}
