diff --git a/examples/worlds/ackermann_steering.sdf b/examples/worlds/ackermann_steering.sdf new file mode 100644 index 0000000000..7dc5408ff2 --- /dev/null +++ b/examples/worlds/ackermann_steering.sdf @@ -0,0 +1,449 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 50 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 2 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + 0.554283 0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + 0.554283 -0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + front_left_wheel_steering_link + chassis + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + 1 + + + + + chassis + front_right_wheel_steering_link + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + + + + + front_left_wheel_steering_link + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_right_wheel_steering_link + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + front_left_wheel_steering_joint + front_right_wheel_steering_joint + 1.0 + 0.5 + 1.0 + 1.25 + 0.3 + -1 + 1 + -3 + 3 + + + + + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index ff95341dac..b4054fcc35 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -70,6 +70,7 @@ function(gz_add_system system_name) INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${IGNITION_GAZEBO_PLUGIN_INSTALL_DIR}) endfunction() +add_subdirectory(ackermann_steering) add_subdirectory(air_pressure) add_subdirectory(altimeter) add_subdirectory(apply_joint_force) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc new file mode 100644 index 0000000000..74b19a3ee3 --- /dev/null +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -0,0 +1,778 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "AckermannSteering.hh" + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/CanonicalLink.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointVelocityCmd.hh" +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +#include "SpeedLimiter.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Velocity command. +struct Commands +{ + /// \brief Linear velocity. + double lin; + + /// \brief Angular velocity. + double ang; + + Commands() : lin(0.0), ang(0.0) {} +}; + +class ignition::gazebo::systems::AckermannSteeringPrivate +{ + /// \brief Callback for velocity subscription + /// \param[in] _msg Velocity message + public: void OnCmdVel(const ignition::msgs::Twist &_msg); + + /// \brief Update odometry and publish an odometry message. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Update the linear and angular velocities. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief Entity of the left joint + public: std::vector leftJoints; + + /// \brief Entity of the right joint + public: std::vector rightJoints; + + /// \brief Entity of the left steering joint + public: std::vector leftSteeringJoints; + + /// \brief Entity of the right steering joint + public: std::vector rightSteeringJoints; + + /// \brief Name of left joint + public: std::vector leftJointNames; + + /// \brief Name of right joint + public: std::vector rightJointNames; + + /// \brief Name of left steering joint + public: std::vector leftSteeringJointNames; + + /// \brief Name of right steering joint + public: std::vector rightSteeringJointNames; + + /// \brief Calculated speed of left wheel joint(s) + public: double leftJointSpeed{0}; + + /// \brief Calculated speed of right wheel joint(s) + public: double rightJointSpeed{0}; + + /// \brief Calculated speed of left joint + public: double leftSteeringJointSpeed{0}; + + /// \brief Calculated speed of right joint + public: double rightSteeringJointSpeed{0}; + + /// \brief Distance between left and right wheels + public: double wheelSeparation{1.0}; + + /// \brief Distance between left and right wheel kingpins + public: double kingpinWidth{0.8}; + + /// \brief Distance between front and back wheels + public: double wheelBase{1.0}; + + /// \brief Maximum turning angle to limit steering to + public: double steeringLimit{0.5}; + + /// \brief Wheel radius + public: double wheelRadius{0.2}; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief The model's canonical link. + public: Link canonicalLink{kNullEntity}; + + /// \brief Update period calculated from . + public: std::chrono::steady_clock::duration odomPubPeriod{0}; + + /// \brief Last sim time odom was published. + public: std::chrono::steady_clock::duration lastOdomPubTime{0}; + + /// \brief Ackermann steering odometry message publisher. + public: transport::Node::Publisher odomPub; + + /// \brief Odometry X value + public: double odomX{0.0}; + + /// \brief Odometry Y value + public: double odomY{0.0}; + + /// \brief Odometry yaw value + public: double odomYaw{0.0}; + + /// \brief Odometry old left value + public: double odomOldLeft{0.0}; + + /// \brief Odometry old right value + public: double odomOldRight{0.0}; + + /// \brief Odometry last time value + public: std::chrono::steady_clock::duration lastOdomTime{0}; + + /// \brief Linear velocity limiter. + public: std::unique_ptr limiterLin; + + /// \brief Angular velocity limiter. + public: std::unique_ptr limiterAng; + + /// \brief Previous control command. + public: Commands last0Cmd; + + /// \brief Previous control command to last0Cmd. + public: Commands last1Cmd; + + /// \brief Last target velocity requested. + public: msgs::Twist targetVel; + + /// \brief A mutex to protect the target velocity command. + public: std::mutex mutex; + + /// \brief frame_id from sdf. + public: std::string sdfFrameId; + + /// \brief child_frame_id from sdf. + public: std::string sdfChildFrameId; +}; + +////////////////////////////////////////////////// +AckermannSteering::AckermannSteering() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void AckermannSteering::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "AckermannSteering plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + // Get the canonical link + std::vector links = _ecm.ChildrenByComponents( + this->dataPtr->model.Entity(), components::CanonicalLink()); + if (!links.empty()) + this->dataPtr->canonicalLink = Link(links[0]); + + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto ptr = const_cast(_sdf.get()); + + // Get params from SDF + sdf::ElementPtr sdfElem = ptr->GetElement("left_joint"); + while (sdfElem) + { + this->dataPtr->leftJointNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("left_joint"); + } + sdfElem = ptr->GetElement("right_joint"); + while (sdfElem) + { + this->dataPtr->rightJointNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("right_joint"); + } + sdfElem = ptr->GetElement("left_steering_joint"); + while (sdfElem) + { + this->dataPtr->leftSteeringJointNames.push_back( + sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("left_steering_joint"); + } + sdfElem = ptr->GetElement("right_steering_joint"); + while (sdfElem) + { + this->dataPtr->rightSteeringJointNames.push_back( + sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("right_steering_joint"); + } + + this->dataPtr->wheelSeparation = _sdf->Get("wheel_separation", + this->dataPtr->wheelSeparation).first; + this->dataPtr->kingpinWidth = _sdf->Get("kingpin_width", + this->dataPtr->kingpinWidth).first; + this->dataPtr->wheelBase = _sdf->Get("wheel_base", + this->dataPtr->wheelBase).first; + this->dataPtr->steeringLimit = _sdf->Get("steering_limit", + this->dataPtr->steeringLimit).first; + this->dataPtr->wheelRadius = _sdf->Get("wheel_radius", + this->dataPtr->wheelRadius).first; + + // Parse speed limiter parameters. + bool hasVelocityLimits = false; + bool hasAccelerationLimits = false; + bool hasJerkLimits = false; + double minVel = std::numeric_limits::lowest(); + double maxVel = std::numeric_limits::max(); + double minAccel = std::numeric_limits::lowest(); + double maxAccel = std::numeric_limits::max(); + double minJerk = std::numeric_limits::lowest(); + double maxJerk = std::numeric_limits::max(); + + if (_sdf->HasElement("min_velocity")) + { + minVel = _sdf->Get("min_velocity"); + hasVelocityLimits = true; + } + if (_sdf->HasElement("max_velocity")) + { + maxVel = _sdf->Get("max_velocity"); + hasVelocityLimits = true; + } + if (_sdf->HasElement("min_acceleration")) + { + minAccel = _sdf->Get("min_acceleration"); + hasAccelerationLimits = true; + } + if (_sdf->HasElement("max_acceleration")) + { + maxAccel = _sdf->Get("max_acceleration"); + hasAccelerationLimits = true; + } + if (_sdf->HasElement("min_jerk")) + { + minJerk = _sdf->Get("min_jerk"); + hasJerkLimits = true; + } + if (_sdf->HasElement("max_jerk")) + { + maxJerk = _sdf->Get("max_jerk"); + hasJerkLimits = true; + } + + // Instantiate the speed limiters. + this->dataPtr->limiterLin = std::make_unique( + hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, + minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); + + this->dataPtr->limiterAng = std::make_unique( + hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, + minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); + + double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; + if (odomFreq > 0) + { + std::chrono::duration odomPer{1 / odomFreq}; + this->dataPtr->odomPubPeriod = + std::chrono::duration_cast(odomPer); + } + + // Subscribe to commands + std::vector topics; + if (_sdf->HasElement("topic")) + { + topics.push_back(_sdf->Get("topic")); + } + topics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"); + auto topic = validTopic(topics); + if (topic.empty()) + { + ignerr << "AckermannSteering plugin received invalid model name " + << "Failed to initialize." << std::endl; + return; + } + + this->dataPtr->node.Subscribe(topic, &AckermannSteeringPrivate::OnCmdVel, + this->dataPtr.get()); + + std::vector odomTopics; + if (_sdf->HasElement("odom_topic")) + { + odomTopics.push_back(_sdf->Get("odom_topic")); + } + odomTopics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + + "/odometry"); + auto odomTopic = validTopic(odomTopics); + if (topic.empty()) + { + ignerr << "AckermannSteering plugin received invalid model name " + << "Failed to initialize." << std::endl; + return; + } + + this->dataPtr->odomPub = this->dataPtr->node.Advertise( + odomTopic); + + if (_sdf->HasElement("frame_id")) + this->dataPtr->sdfFrameId = _sdf->Get("frame_id"); + + if (_sdf->HasElement("child_frame_id")) + this->dataPtr->sdfChildFrameId = _sdf->Get("child_frame_id"); + + ignmsg << "AckermannSteering subscribing to twist messages on [" << + topic << "]" << std::endl; +} + +////////////////////////////////////////////////// +void AckermannSteering::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("AckermannSteering::PreUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + // If the joints haven't been identified yet, look for them + static std::set warnedModels; + auto modelName = this->dataPtr->model.Name(_ecm); + if (this->dataPtr->leftJoints.empty() || + this->dataPtr->rightJoints.empty() || + this->dataPtr->leftSteeringJoints.empty() || + this->dataPtr->rightSteeringJoints.empty()) + { + bool warned{false}; + for (const std::string &name : this->dataPtr->leftJointNames) + { + Entity joint = this->dataPtr->model.JointByName(_ecm, name); + if (joint != kNullEntity) + this->dataPtr->leftJoints.push_back(joint); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find left joint [" << name << "] for model [" + << modelName << "]" << std::endl; + warned = true; + } + } + + for (const std::string &name : this->dataPtr->rightJointNames) + { + Entity joint = this->dataPtr->model.JointByName(_ecm, name); + if (joint != kNullEntity) + this->dataPtr->rightJoints.push_back(joint); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find right joint [" << name << "] for model [" + << modelName << "]" << std::endl; + warned = true; + } + } + for (const std::string &name : this->dataPtr->leftSteeringJointNames) + { + Entity joint = this->dataPtr->model.JointByName(_ecm, name); + if (joint != kNullEntity) + this->dataPtr->leftSteeringJoints.push_back(joint); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find left steering joint [" + << name << "] for model [" + << modelName << "]" << std::endl; + warned = true; + } + } + + for (const std::string &name : this->dataPtr->rightSteeringJointNames) + { + Entity joint = this->dataPtr->model.JointByName(_ecm, name); + if (joint != kNullEntity) + this->dataPtr->rightSteeringJoints.push_back(joint); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find right steering joint [" << + name << "] for model [" << modelName << "]" << std::endl; + warned = true; + } + } + if (warned) + { + warnedModels.insert(modelName); + } + } + + if (this->dataPtr->leftJoints.empty() || this->dataPtr->rightJoints.empty() || + this->dataPtr->leftSteeringJoints.empty() || + this->dataPtr->rightSteeringJoints.empty()) + return; + + if (warnedModels.find(modelName) != warnedModels.end()) + { + ignmsg << "Found joints for model [" << modelName + << "], plugin will start working." << std::endl; + warnedModels.erase(modelName); + } + + // Nothing left to do if paused. + if (_info.paused) + return; + + for (Entity joint : this->dataPtr->leftJoints) + { + // Update wheel velocity + auto vel = _ecm.Component(joint); + + if (vel == nullptr) + { + _ecm.CreateComponent( + joint, components::JointVelocityCmd({this->dataPtr->leftJointSpeed})); + } + else + { + *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed}); + } + } + + for (Entity joint : this->dataPtr->rightJoints) + { + // Update wheel velocity + auto vel = _ecm.Component(joint); + + if (vel == nullptr) + { + _ecm.CreateComponent(joint, + components::JointVelocityCmd({this->dataPtr->rightJointSpeed})); + } + else + { + *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed}); + } + } + + // Update steering + for (Entity joint : this->dataPtr->leftSteeringJoints) + { + auto vel = _ecm.Component(joint); + + if (vel == nullptr) + { + _ecm.CreateComponent( + joint, components::JointVelocityCmd( + {this->dataPtr->leftSteeringJointSpeed})); + } + else + { + *vel = components::JointVelocityCmd( + {this->dataPtr->leftSteeringJointSpeed}); + } + } + + for (Entity joint : this->dataPtr->rightSteeringJoints) + { + auto vel = _ecm.Component(joint); + + if (vel == nullptr) + { + _ecm.CreateComponent(joint, + components::JointVelocityCmd( + {this->dataPtr->rightSteeringJointSpeed})); + } + else + { + *vel = components::JointVelocityCmd( + {this->dataPtr->rightSteeringJointSpeed}); + } + } + + // Create the left and right side joint position components if they + // don't exist. + auto leftPos = _ecm.Component( + this->dataPtr->leftJoints[0]); + if (!leftPos) + { + _ecm.CreateComponent(this->dataPtr->leftJoints[0], + components::JointPosition()); + } + + auto rightPos = _ecm.Component( + this->dataPtr->rightJoints[0]); + if (!rightPos) + { + _ecm.CreateComponent(this->dataPtr->rightJoints[0], + components::JointPosition()); + } + + auto leftSteeringPos = _ecm.Component( + this->dataPtr->leftSteeringJoints[0]); + if (!leftSteeringPos) + { + _ecm.CreateComponent(this->dataPtr->leftSteeringJoints[0], + components::JointPosition()); + } + + auto rightSteeringPos = _ecm.Component( + this->dataPtr->rightSteeringJoints[0]); + if (!rightSteeringPos) + { + _ecm.CreateComponent(this->dataPtr->rightSteeringJoints[0], + components::JointPosition()); + } +} + +////////////////////////////////////////////////// +void AckermannSteering::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("AckermannSteering::PostUpdate"); + // Nothing left to do if paused. + if (_info.paused) + return; + + this->dataPtr->UpdateVelocity(_info, _ecm); + this->dataPtr->UpdateOdometry(_info, _ecm); +} + +////////////////////////////////////////////////// +void AckermannSteeringPrivate::UpdateOdometry( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("AckermannSteering::UpdateOdometry"); + // Initialize, if not already initialized. + + if (this->leftJoints.empty() || this->rightJoints.empty() || + this->leftSteeringJoints.empty() || this->rightSteeringJoints.empty()) + return; + + // Get the first joint positions for the left and right side. + auto leftPos = _ecm.Component(this->leftJoints[0]); + auto rightPos = _ecm.Component( + this->rightJoints[0]); + auto leftSteeringPos = _ecm.Component( + this->leftSteeringJoints[0]); + auto rightSteeringPos = _ecm.Component( + this->rightSteeringJoints[0]); + + // Abort if the joints were not found or just created. + if (!leftPos || !rightPos || leftPos->Data().empty() || + rightPos->Data().empty() || + !leftSteeringPos || !rightSteeringPos || + leftSteeringPos->Data().empty() || + rightSteeringPos->Data().empty()) + { + return; + } + + // Calculate the odometry + double phi = 0.5 * (leftSteeringPos->Data()[0] + rightSteeringPos->Data()[0]); + double radius = this->wheelBase / tan(phi); + double dist = 0.5 * this->wheelRadius * + ((leftPos->Data()[0] - this->odomOldLeft) + + (rightPos->Data()[0] - this->odomOldRight)); + double deltaAngle = dist / radius; + this->odomYaw += deltaAngle; + this->odomYaw = math::Angle(this->odomYaw).Normalized().Radian(); + this->odomX += dist * cos(this->odomYaw); + this->odomY += dist * sin(this->odomYaw); + auto odomTimeDiff = _info.simTime - this->lastOdomTime; + double tdiff = std::chrono::duration(odomTimeDiff).count(); + double odomLinearVelocity = dist / tdiff; + double odomAngularVelocity = deltaAngle / tdiff; + this->lastOdomTime = _info.simTime; + this->odomOldLeft = leftPos->Data()[0]; + this->odomOldRight = rightPos->Data()[0]; + + // Throttle odometry publishing + auto diff = _info.simTime - this->lastOdomPubTime; + if (diff > std::chrono::steady_clock::duration::zero() && + diff < this->odomPubPeriod) + { + return; + } + this->lastOdomPubTime = _info.simTime; + + // Construct the odometry message and publish it. + msgs::Odometry msg; + msg.mutable_pose()->mutable_position()->set_x(this->odomX); + msg.mutable_pose()->mutable_position()->set_y(this->odomY); + + math::Quaterniond orientation(0, 0, this->odomYaw); + msgs::Set(msg.mutable_pose()->mutable_orientation(), orientation); + + msg.mutable_twist()->mutable_linear()->set_x(odomLinearVelocity); + msg.mutable_twist()->mutable_angular()->set_z(odomAngularVelocity); + + // Set the time stamp in the header + msg.mutable_header()->mutable_stamp()->CopyFrom( + convert(_info.simTime)); + + // Set the frame id. + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + if (this->sdfFrameId.empty()) + { + frame->add_value(this->model.Name(_ecm) + "/odom"); + } + else + { + frame->add_value(this->sdfFrameId); + } + + std::optional linkName = this->canonicalLink.Name(_ecm); + if (this->sdfChildFrameId.empty()) + { + if (linkName) + { + auto childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName); + } + } + else + { + auto childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(this->sdfChildFrameId); + } + + // Publish the message + this->odomPub.Publish(msg); +} + +////////////////////////////////////////////////// +void AckermannSteeringPrivate::UpdateVelocity( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("AckermannSteering::UpdateVelocity"); + + double linVel; + double angVel; + { + std::lock_guard lock(this->mutex); + linVel = this->targetVel.linear().x(); + angVel = this->targetVel.angular().z(); + } + + const double dt = std::chrono::duration(_info.dt).count(); + + // Limit the target velocity if needed. + this->limiterLin->Limit(linVel, this->last0Cmd.lin, this->last1Cmd.lin, dt); + this->limiterAng->Limit(angVel, this->last0Cmd.ang, this->last1Cmd.ang, dt); + + // Update history of commands. + this->last1Cmd = last0Cmd; + this->last0Cmd.lin = linVel; + this->last0Cmd.ang = angVel; + + // Convert the target velocities to joint velocities and angles + double turningRadius = linVel / angVel; + double minimumTurningRadius = this->wheelBase / sin(this->steeringLimit); + if ((turningRadius >= 0.0) && (turningRadius < minimumTurningRadius)) + { + turningRadius = minimumTurningRadius; + } + if ((turningRadius <= 0.0) && (turningRadius > -minimumTurningRadius)) + { + turningRadius = -minimumTurningRadius; + } + // special case for angVel of zero + if (fabs(angVel) < 0.001) + { + turningRadius = 1000000000.0; + } + + double leftSteeringJointAngle = + atan(this->wheelBase / (turningRadius - (this->kingpinWidth / 2.0))); + double rightSteeringJointAngle = + atan(this->wheelBase / (turningRadius + (this->kingpinWidth / 2.0))); + double phi = atan(this->wheelBase / turningRadius); + + // Partially simulate a simple differential + this->rightJointSpeed = + (linVel * (1.0 + (this->wheelSeparation * tan(phi)) / + (2.0 * this->wheelBase))) / this->wheelRadius; + this->leftJointSpeed = + (linVel * (1.0 - (this->wheelSeparation * tan(phi)) / + (2.0 * this->wheelBase))) / this->wheelRadius; + + auto leftSteeringPos = _ecm.Component( + this->leftSteeringJoints[0]); + auto rightSteeringPos = _ecm.Component( + this->rightSteeringJoints[0]); + + // Abort if the joints were not found or just created. + if (!leftSteeringPos || !rightSteeringPos || + leftSteeringPos->Data().empty() || + rightSteeringPos->Data().empty()) + { + return; + } + + double leftDelta = leftSteeringJointAngle - leftSteeringPos->Data()[0]; + double rightDelta = rightSteeringJointAngle - rightSteeringPos->Data()[0]; + + // Simple proportional control with a gain of 1 + // Adding programmable PID values might be a future feature. + // Works as is for tested cases + this->leftSteeringJointSpeed = leftDelta; + this->rightSteeringJointSpeed = rightDelta; +} + +////////////////////////////////////////////////// +void AckermannSteeringPrivate::OnCmdVel(const msgs::Twist &_msg) +{ + std::lock_guard lock(this->mutex); + this->targetVel = _msg; +} + +IGNITION_ADD_PLUGIN(AckermannSteering, + ignition::gazebo::System, + AckermannSteering::ISystemConfigure, + AckermannSteering::ISystemPreUpdate, + AckermannSteering::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(AckermannSteering, + "ignition::gazebo::systems::AckermannSteering") diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh new file mode 100644 index 0000000000..c2a7d2d49c --- /dev/null +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_ACKERMANNSTEERING_HH_ +#define IGNITION_GAZEBO_SYSTEMS_ACKERMANNSTEERING_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class AckermannSteeringPrivate; + + /// \brief Ackermann steering controller which can be attached to a model + /// with any number of left and right wheels. + /// + /// # System Parameters + /// + /// ``: Name of a joint that controls a left wheel. This + /// element can appear multiple times, and must appear at least once. + /// + /// ``: Name of a joint that controls a right wheel. This + /// element can appear multiple times, and must appear at least once. + /// + /// ``: Name of a joint that controls steering for + /// left wheel. This element must appear once. Vehicles that steer + /// rear wheels are not currently correctly supported. + /// + /// ``: Name of a joint that controls steering for + /// right wheel. This element must appear once. + /// + /// ``: Distance between wheels, in meters. This element + /// is optional, although it is recommended to be included with an + /// appropriate value. The default value is 1.0m. + /// + /// ``: Distance between wheel kingpins, in meters. This + /// element is optional, although it is recommended to be included with an + /// appropriate value. The default value is 0.8m. Generally a bit smaller + /// than the wheel_separation. + /// + /// ``: Distance between front and rear wheels, in meters. This + /// element is optional, although it is recommended to be included with an + /// appropriate value. The default value is 1.0m. + /// + /// ``: Value to limit steering to. Can be calculated by + /// measuring the turning radius and wheel_base of the real vehicle. + /// steering_limit = asin(wheel_base / turning_radius) + /// The default value is 0.5 radians. + /// + /// ``: Wheel radius in meters. This element is optional, + /// although it is recommended to be included with an appropriate value. The + /// default value is 0.2m. + /// + /// ``: Odometry publication frequency. This + /// element is optional, and the default value is 50Hz. + /// + /// '': Minimum velocity [m/s], usually <= 0. + /// '': Maximum velocity [m/s], usually >= 0. + /// '': Minimum acceleration [m/s^2], usually <= 0. + /// '': Maximum acceleration [m/s^2], usually >= 0. + /// '': jerk [m/s^3], usually <= 0. + /// '': jerk [m/s^3], usually >= 0. + /// + /// ``: Custom topic that this system will subscribe to in order to + /// receive command velocity messages. This element if optional, and the + /// default value is `/model/{name_of_model}/cmd_vel`. + /// + /// ``: Custom topic on which this system will publish odometry + /// messages. This element if optional, and the default value is + /// `/model/{name_of_model}/odometry`. + /// + /// A robot with rear drive and front steering would have one each + /// of left_joint, right_joint, left_steering_joint and + /// right_steering_joint + /// + /// References: + /// https://github.com/ignitionrobotics/ign-gazebo/tree/ign-gazebo4/src/systems/diff_drive + /// https://www.auto.tuwien.ac.at/bib/pdf_TR/TR0183.pdf + /// https://github.com/froohoo/ackermansteer/blob/master/ackermansteer/ + + + class IGNITION_GAZEBO_VISIBLE AckermannSteering + : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: AckermannSteering(); + + /// \brief Destructor + public: ~AckermannSteering() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/src/systems/ackermann_steering/CMakeLists.txt b/src/systems/ackermann_steering/CMakeLists.txt new file mode 100644 index 0000000000..69af11afea --- /dev/null +++ b/src/systems/ackermann_steering/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(ackermann-steering + SOURCES + AckermannSteering.cc + SpeedLimiter.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/ackermann_steering/SpeedLimiter.cc b/src/systems/ackermann_steering/SpeedLimiter.cc new file mode 100644 index 0000000000..ebae2b5b81 --- /dev/null +++ b/src/systems/ackermann_steering/SpeedLimiter.cc @@ -0,0 +1,209 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * 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 PAL Robotics 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: Enrique Fernández + * Modified: Carlos Agüero + */ + +#include + +#include "SpeedLimiter.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private SpeedLimiter data class. +class ignition::gazebo::systems::SpeedLimiterPrivate +{ + /// \brief Class constructor. + /// \param [in] _hasVelocityLimits if true, applies velocity limits. + /// \param [in] _hasAccelerationLimits if true, applies acceleration limits. + /// \param [in] _hasJerkLimits if true, applies jerk limits. + /// \param [in] _minVelocity Minimum velocity [m/s], usually <= 0. + /// \param [in] _maxVelocity Maximum velocity [m/s], usually >= 0. + /// \param [in] _minAcceleration Minimum acceleration [m/s^2], usually <= 0. + /// \param [in] _maxAcceleration Maximum acceleration [m/s^2], usually >= 0. + /// \param [in] _minJerk Minimum jerk [m/s^3], usually <= 0. + /// \param [in] _maxJerk Maximum jerk [m/s^3], usually >= 0. + public: SpeedLimiterPrivate(bool _hasVelocityLimits, + bool _hasAccelerationLimits, + bool _hasJerkLimits, + double _minVelocity, + double _maxVelocity, + double _minAcceleration, + double _maxAcceleration, + double _minJerk, + double _maxJerk) + : hasVelocityLimits(_hasVelocityLimits), + hasAccelerationLimits(_hasAccelerationLimits), + hasJerkLimits(_hasJerkLimits), + minVelocity(_minVelocity), + maxVelocity(_maxVelocity), + minAcceleration(_minAcceleration), + maxAcceleration(_maxAcceleration), + minJerk(_minJerk), + maxJerk(_maxJerk) + { + } + + /// \brief Enable/Disable velocity. + public: bool hasVelocityLimits; + + /// \brief Enable/Disable acceleration. + public: bool hasAccelerationLimits; + + /// \brief Enable/Disable jerk. + public: bool hasJerkLimits; + + /// \brief Minimum velocity limit. + public: double minVelocity; + + /// \brief Maximum velocity limit. + public: double maxVelocity; + + /// \brief Minimum acceleration limit. + public: double minAcceleration; + + /// \brief Maximum acceleration limit. + public: double maxAcceleration; + + /// \brief Minimum jerk limit. + public: double minJerk; + + /// \brief Maximum jerk limit. + public: double maxJerk; +}; + +////////////////////////////////////////////////// +SpeedLimiter::SpeedLimiter(bool _hasVelocityLimits, + bool _hasAccelerationLimits, + bool _hasJerkLimits, + double _minVelocity, + double _maxVelocity, + double _minAcceleration, + double _maxAcceleration, + double _minJerk, + double _maxJerk) + : dataPtr(std::make_unique(_hasVelocityLimits, + _hasAccelerationLimits, _hasJerkLimits, _minVelocity, _maxVelocity, + _minAcceleration, _maxAcceleration, _minJerk, _maxJerk)) +{ +} + +////////////////////////////////////////////////// +SpeedLimiter::~SpeedLimiter() +{ +} + +////////////////////////////////////////////////// +double SpeedLimiter::Limit(double &_v, double _v0, double _v1, double _dt) const +{ + const double tmp = _v; + + this->LimitJerk(_v, _v0, _v1, _dt); + this->LimitAcceleration(_v, _v0, _dt); + this->LimitVelocity(_v); + + if (ignition::math::equal(tmp, 0.0)) + return 1.0; + else + return _v / tmp; +} + +////////////////////////////////////////////////// +double SpeedLimiter::LimitVelocity(double &_v) const +{ + const double tmp = _v; + + if (this->dataPtr->hasVelocityLimits) + { + _v = ignition::math::clamp( + _v, this->dataPtr->minVelocity, this->dataPtr->maxVelocity); + } + + if (ignition::math::equal(tmp, 0.0)) + return 1.0; + else + return _v / tmp; +} + +////////////////////////////////////////////////// +double SpeedLimiter::LimitAcceleration(double &_v, double _v0, double _dt) const +{ + const double tmp = _v; + + if (this->dataPtr->hasAccelerationLimits) + { + const double dvMin = this->dataPtr->minAcceleration * _dt; + const double dvMax = this->dataPtr->maxAcceleration * _dt; + + const double dv = ignition::math::clamp(_v - _v0, dvMin, dvMax); + + _v = _v0 + dv; + } + + if (ignition::math::equal(tmp, 0.0)) + return 1.0; + else + return _v / tmp; +} + +////////////////////////////////////////////////// +double SpeedLimiter::LimitJerk(double &_v, double _v0, double _v1, double _dt) + const +{ + const double tmp = _v; + + if (this->dataPtr->hasJerkLimits) + { + const double dv = _v - _v0; + const double dv0 = _v0 - _v1; + + const double dt2 = 2. * _dt * _dt; + + const double daMin = this->dataPtr->minJerk * dt2; + const double daMax = this->dataPtr->maxJerk * dt2; + + const double da = ignition::math::clamp(dv - dv0, daMin, daMax); + + _v = _v0 + dv0 + da; + } + + if (ignition::math::equal(tmp, 0.0)) + return 1.0; + else + return _v / tmp; +} diff --git a/src/systems/ackermann_steering/SpeedLimiter.hh b/src/systems/ackermann_steering/SpeedLimiter.hh new file mode 100644 index 0000000000..5a86ac2b10 --- /dev/null +++ b/src/systems/ackermann_steering/SpeedLimiter.hh @@ -0,0 +1,130 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * 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 PAL Robotics 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: Enrique Fernández + * Modified: Carlos Agüero + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration. + class SpeedLimiterPrivate; + + /// \brief Class to limit velocity, acceleration and jerk. + /// \ref https://github.com/ros-controls/ros_controllers/tree/melodic-devel/diff_drive_controller + class IGNITION_GAZEBO_VISIBLE SpeedLimiter + { + /// \brief Constructor. + /// \param [in] _hasVelocityLimits if true, applies velocity limits. + /// \param [in] _hasAccelerationLimits if true, applies acceleration limits. + /// \param [in] _hasJerkLimits if true, applies jerk limits. + /// \param [in] _minVelocity Minimum velocity [m/s], usually <= 0. + /// \param [in] _maxVelocity Maximum velocity [m/s], usually >= 0. + /// \param [in] _minAcceleration Minimum acceleration [m/s^2], usually <= 0. + /// \param [in] _maxAcceleration Maximum acceleration [m/s^2], usually >= 0. + /// \param [in] _minJerk Minimum jerk [m/s^3], usually <= 0. + /// \param [in] _maxJerk Maximum jerk [m/s^3], usually >= 0. + public: SpeedLimiter(bool _hasVelocityLimits = false, + bool _hasAccelerationLimits = false, + bool _hasJerkLimits = false, + double _minVelocity = 0.0, + double _maxVelocity = 0.0, + double _minAcceleration = 0.0, + double _maxAcceleration = 0.0, + double _minJerk = 0.0, + double _maxJerk = 0.0); + + /// \brief Destructor. + public: ~SpeedLimiter(); + + /// \brief Limit the velocity and acceleration. + /// \param [in, out] _v Velocity [m/s]. + /// \param [in] _v0 Previous velocity to v [m/s]. + /// \param [in] _v1 Previous velocity to v0 [m/s]. + /// \param [in] _dt Time step [s]. + /// \return Limiting factor (1.0 if none). + public: double Limit(double &_v, + double _v0, + double _v1, + double _dt) const; + + /// \brief Limit the velocity. + /// \param [in, out] _v Velocity [m/s]. + /// \return Limiting factor (1.0 if none). + public: double LimitVelocity(double &_v) const; + + /// \brief Limit the acceleration. + /// \param [in, out] _v Velocity [m/s]. + /// \param [in] _v0 Previous velocity [m/s]. + /// \param [in] _dt Time step [s]. + /// \return Limiting factor (1.0 if none). + public: double LimitAcceleration(double &_v, + double _v0, + double _dt) const; + + /// \brief Limit the jerk. + /// \param [in, out] _v Velocity [m/s]. + /// \param [in] _v0 Previous velocity to v [m/s]. + /// \param [in] _v1 Previous velocity to v0 [m/s]. + /// \param [in] _dt Time step [s]. + /// \return Limiting factor (1.0 if none). + /// \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control. + public: double LimitJerk(double &_v, + double _v0, + double _v1, + double _dt) const; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 7250621ca8..683b7a9472 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -1,6 +1,7 @@ set(TEST_TYPE "INTEGRATION") set(tests + ackermann_steering_system.cc air_pressure_system.cc altimeter_system.cc apply_joint_force_system.cc diff --git a/test/integration/ackermann_steering_system.cc b/test/integration/ackermann_steering_system.cc new file mode 100644 index 0000000000..a4279b242f --- /dev/null +++ b/test/integration/ackermann_steering_system.cc @@ -0,0 +1,425 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" + +#define tol 10e-4 + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test AckermannSteering system +class AckermannSteeringTest : public ::testing::TestWithParam +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + /// \param[in] _odomTopic Odometry topic. + protected: void TestPublishCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic, + const std::string &_odomTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, poses.size()); + + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + + // Get odometry messages + double period{1.0 / 50.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + double msgTime = + static_cast(_msg.header().stamp().sec()) + + static_cast(_msg.header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); + lastMsgTime = msgTime; + + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + + // Publish command and check that vehicle moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + node.Subscribe(_odomTopic, odomCb); + + msgs::Twist msg; + + // Avoid wheel slip by limiting acceleration + // Avoid wheel slip by limiting acceleration (1 m/s^2) + // and max velocity (0.5 m/s). + // See parameters + // in "/test/worlds/ackermann_steering.sdf". + test::Relay velocityRamp; + const double desiredLinVel = 10.5; + const double desiredAngVel = 0.1; + velocityRamp.OnPreUpdate( + [&](const gazebo::UpdateInfo &/*_info*/, + const gazebo::EntityComponentManager &) + { + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, desiredAngVel)); + pub.Publish(msg); + }); + + server.AddSystem(velocityRamp.systemPtr); + + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + // Odometry calculates the pose of a point that is located half way between + // the four wheels, not the origin of the model. + // Since the model origin is offset, the model position will + // change based on orientation. To find the final pose of the model, + // we have to do the following similarity transformation + math::Pose3d tOdomModel(-0.1, 0, -0.325, 0, 0, 0); + auto finalModelFramePose = + tOdomModel * odomPoses.back() * tOdomModel.Inverse(); + + // Odom for 3s + ASSERT_FALSE(odomPoses.empty()); + EXPECT_EQ(150u, odomPoses.size()); + + EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X()); + EXPECT_LT(poses[0].Pos().Y(), poses[3999].Pos().Y()); + EXPECT_NEAR(poses[0].Pos().Z(), poses[3999].Pos().Z(), tol); + EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); + EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); + EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); + + // The value from odometry will be close, but not exactly the ground truth + // pose of the robot model. This is partially due to throttling the + // odometry publisher, partially due to a frame difference between the + // odom frame and the model frame, and partially due to wheel slip. + EXPECT_NEAR(poses[1020].Pos().X(), odomPoses[0].Pos().X(), 1e-2); + EXPECT_NEAR(poses[1020].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); + // Originally: + // EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); + // EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); + // Reduced to 25cm as I couldn't find a friction model that generated 1cm + // accuracy + EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 0.25); + EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 0.25); + + // Max velocities/accelerations expectations. + // Moving time. + double t = 3.0; + double d = poses[3999].Pos().Distance(poses[1000].Pos()); + const double v0 = 0; + double v = d / t; + double a = (v - v0) / t; + EXPECT_LT(v, 0.5); + EXPECT_LT(a, 1); + } +}; + +///////////////////////////////////////////////// +TEST_P(AckermannSteeringTest, PublishCmd) +{ + TestPublishCmd(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "/test/worlds/ackermann_steering.sdf"), + "/model/vehicle/cmd_vel", "/model/vehicle/odometry"); +} + +///////////////////////////////////////////////// +TEST_P(AckermannSteeringTest, PublishCmdCustomTopics) +{ + TestPublishCmd(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "/test/worlds/ackermann_steering_custom_topics.sdf"), + "/model/foo/cmdvel", "/model/bar/odom"); +} + +///////////////////////////////////////////////// +TEST_P(AckermannSteeringTest, SkidPublishCmd) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "/test/worlds/ackermann_steering_slow_odom.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, poses.size()); + + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + + // Publish command and check that vehicle moved + double period{1.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + double msgTime = + static_cast(_msg.header().stamp().sec()) + + static_cast(_msg.header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); + lastMsgTime = msgTime; + + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle/cmd_vel"); + node.Subscribe("/model/vehicle/odometry", odomCb); + + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2)); + + pub.Publish(msg); + + server.Run(true, 3000, false); + + // Poses for 4s + EXPECT_EQ(4000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 3 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); + + // Odom for 3s + ASSERT_FALSE(odomPoses.empty()); + EXPECT_EQ(3u, odomPoses.size()); + + EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X()); + EXPECT_LT(poses[0].Pos().Y(), poses[3999].Pos().Y()); + EXPECT_NEAR(poses[0].Pos().Z(), poses[3999].Pos().Z(), tol); + EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); + EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); + EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); +} + +///////////////////////////////////////////////// +TEST_P(AckermannSteeringTest, OdomFrameId) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "/test/worlds/ackermann_steering.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + unsigned int odomPosesCount = 0; + std::function odomCb = + [&odomPosesCount](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + ASSERT_GT(_msg.header().data_size(), 1); + + EXPECT_STREQ(_msg.header().data(0).key().c_str(), "frame_id"); + EXPECT_STREQ( + _msg.header().data(0).value().Get(0).c_str(), "vehicle/odom"); + + EXPECT_STREQ(_msg.header().data(1).key().c_str(), "child_frame_id"); + EXPECT_STREQ( + _msg.header().data(1).value().Get(0).c_str(), "vehicle/chassis"); + + odomPosesCount++; + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle/cmd_vel"); + node.Subscribe("/model/vehicle/odometry", odomCb); + + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2)); + + pub.Publish(msg); + + server.Run(true, 100, false); + + int sleep = 0; + int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse + for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) // NOLINT + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + EXPECT_EQ(5u, odomPosesCount); +} + +///////////////////////////////////////////////// +TEST_P(AckermannSteeringTest, OdomCustomFrameId) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "/test/worlds/ackermann_steering_custom_frame_id.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + unsigned int odomPosesCount = 0; + std::function odomCb = + [&odomPosesCount](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + ASSERT_GT(_msg.header().data_size(), 1); + + EXPECT_STREQ(_msg.header().data(0).key().c_str(), "frame_id"); + EXPECT_STREQ(_msg.header().data(0).value().Get(0).c_str(), "odom"); + + EXPECT_STREQ(_msg.header().data(1).key().c_str(), "child_frame_id"); + EXPECT_STREQ( + _msg.header().data(1).value().Get(0).c_str(), "base_footprint"); + + odomPosesCount++; + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle/cmd_vel"); + node.Subscribe("/model/vehicle/odometry", odomCb); + + server.Run(true, 100, false); + + int sleep = 0; + int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse + for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + EXPECT_EQ(5u, odomPosesCount); +} + +// Run multiple times +INSTANTIATE_TEST_SUITE_P(ServerRepeat, AckermannSteeringTest, + ::testing::Range(1, 2)); diff --git a/test/worlds/ackermann_steering.sdf b/test/worlds/ackermann_steering.sdf new file mode 100644 index 0000000000..f93f0750e0 --- /dev/null +++ b/test/worlds/ackermann_steering.sdf @@ -0,0 +1,391 @@ + + + + + + 0.001 + 1.0 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 50 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + 0.554283 0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + 0.554283 -0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + front_left_wheel_steering_link + chassis + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + 1 + + + + + chassis + front_right_wheel_steering_link + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + + + + + front_left_wheel_steering_link + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_right_wheel_steering_link + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + front_left_wheel_steering_joint + front_right_wheel_steering_joint + 1.0 + 0.5 + 1.0 + 1.25 + 0.3 + -0.5 + 0.5 + -1 + 1 + + + + diff --git a/test/worlds/ackermann_steering_custom_frame_id.sdf b/test/worlds/ackermann_steering_custom_frame_id.sdf new file mode 100644 index 0000000000..0ea704d323 --- /dev/null +++ b/test/worlds/ackermann_steering_custom_frame_id.sdf @@ -0,0 +1,439 @@ + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 50 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 2 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + 0.554283 0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + 0.554283 -0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + front_left_wheel_steering_link + chassis + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + 1 + + + + + chassis + front_right_wheel_steering_link + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + + + + + front_left_wheel_steering_link + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_right_wheel_steering_link + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + front_left_wheel_steering_joint + front_right_wheel_steering_joint + 1.0 + 0.5 + 1.0 + 1.25 + 0.3 + -1 + 1 + -3 + 3 + odom + base_footprint + + + + + + diff --git a/test/worlds/ackermann_steering_custom_topics.sdf b/test/worlds/ackermann_steering_custom_topics.sdf new file mode 100644 index 0000000000..d86dbc3cfe --- /dev/null +++ b/test/worlds/ackermann_steering_custom_topics.sdf @@ -0,0 +1,439 @@ + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 50 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + 0.554283 0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + 0.554283 -0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + front_left_wheel_steering_link + chassis + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + 1 + + + + + chassis + front_right_wheel_steering_link + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + + + + + front_left_wheel_steering_link + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_right_wheel_steering_link + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + front_left_wheel_steering_joint + front_right_wheel_steering_joint + 1.0 + 0.5 + 1.0 + 1.25 + 0.3 + /model/foo/cmdvel + /model/bar/odom + -0.5 + 0.5 + -1 + 1 + + + + + + diff --git a/test/worlds/ackermann_steering_limited_joints_pub.sdf b/test/worlds/ackermann_steering_limited_joints_pub.sdf new file mode 100644 index 0000000000..7e29c8f173 --- /dev/null +++ b/test/worlds/ackermann_steering_limited_joints_pub.sdf @@ -0,0 +1,445 @@ + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 50 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 2 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + 0.554283 0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + 0.554283 -0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + front_left_wheel_steering_link + chassis + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + 1 + + + + + chassis + front_right_wheel_steering_link + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + + + + + front_left_wheel_steering_link + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_right_wheel_steering_link + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + front_left_wheel_steering_joint + front_right_wheel_steering_joint + 1.0 + 0.5 + 1.0 + 1.25 + 0.3 + -1 + 1 + -3 + 3 + + + + left_wheel_joint + right_wheel_joint + + right_wheel_joint + + + + + diff --git a/test/worlds/ackermann_steering_slow_odom.sdf b/test/worlds/ackermann_steering_slow_odom.sdf new file mode 100644 index 0000000000..725d387dd0 --- /dev/null +++ b/test/worlds/ackermann_steering_slow_odom.sdf @@ -0,0 +1,392 @@ + + + + + + 0.001 + 1.0 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 50 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + 0.554283 0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + 0.554283 -0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 11 11 11 + 11 11 11 + + + + + + front_left_wheel_steering_link + chassis + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + 1 + + + + + chassis + front_right_wheel_steering_link + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + + + + + front_left_wheel_steering_link + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_right_wheel_steering_link + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + front_left_wheel_steering_joint + front_right_wheel_steering_joint + 1.0 + 0.5 + 1.0 + 1.25 + 0.3 + -0.5 + 0.5 + -1 + 1 + 1 + + + +