From 74b6dead63c1e897cc29cd34f38ed89349d1bcf1 Mon Sep 17 00:00:00 2001 From: nate koenig Date: Wed, 21 Dec 2016 16:07:06 -0800 Subject: [PATCH 1/7] Output pose info --- plugins/PriusHybridPlugin.cc | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/plugins/PriusHybridPlugin.cc b/plugins/PriusHybridPlugin.cc index 1ecada1..e9706b9 100644 --- a/plugins/PriusHybridPlugin.cc +++ b/plugins/PriusHybridPlugin.cc @@ -17,6 +17,7 @@ #include #include +#include #include "PriusHybridPlugin.hh" #include @@ -49,6 +50,9 @@ namespace gazebo /// \brief Ignition transport node public: ignition::transport::Node node; + /// \brief Ignition transport position pub + public: ignition::transport::Node::Publisher posePub; + /// \brief Physics update event connection public: event::ConnectionPtr updateConnection; @@ -82,6 +86,9 @@ namespace gazebo /// \brief PID control for steering wheel joint public: common::PID handWheelPID; + /// \brief Last pose msg time + public: common::Time lastMsgTime; + /// \brief Last sim time received public: common::Time lastSimTime; @@ -245,6 +252,10 @@ void PriusHybridPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) this->dataPtr->node.Subscribe("/cmd_vel", &PriusHybridPlugin::OnCmdVel, this); + this->dataPtr->posePub = this->dataPtr->node.Advertise( + "/prius_pose"); + + std::string handWheelJointName = this->dataPtr->model->GetName() + "::" + _sdf->Get("steering_wheel"); this->dataPtr->handWheelJoint = @@ -573,6 +584,13 @@ void PriusHybridPlugin::Update() return; } + if ((curTime - this->dataPtr->lastMsgTime) > .5) + { + this->dataPtr->posePub.Publish( + ignition::msgs::Convert(this->dataPtr->model->GetWorldPose().Ign())); + this->dataPtr->lastMsgTime = curTime; + } + this->dataPtr->handWheelState = this->dataPtr->handWheelJoint->GetAngle(0).Radian(); this->dataPtr->flSteeringState = From c46c98acd92c15ad3eaecfd30cbdf7579fcd82cf Mon Sep 17 00:00:00 2001 From: nkoenig Date: Thu, 12 Jan 2017 12:48:50 -0800 Subject: [PATCH 2/7] output data --- cmake/setup.sh.in | 2 + plugins/PriusHybridPlugin.cc | 87 ++++++++++++++++++++++++++---------- worlds/raceway.world | 6 +-- 3 files changed, 69 insertions(+), 26 deletions(-) diff --git a/cmake/setup.sh.in b/cmake/setup.sh.in index a3ba02c..8f18448 100644 --- a/cmake/setup.sh.in +++ b/cmake/setup.sh.in @@ -1,3 +1,5 @@ +#!/bin/sh + # This file is the entry point for priuscup users. It should establish # all environment necessary to use priuscup. diff --git a/plugins/PriusHybridPlugin.cc b/plugins/PriusHybridPlugin.cc index e9706b9..cd9551b 100644 --- a/plugins/PriusHybridPlugin.cc +++ b/plugins/PriusHybridPlugin.cc @@ -53,6 +53,9 @@ namespace gazebo /// \brief Ignition transport position pub public: ignition::transport::Node::Publisher posePub; + /// \brief Ignition transport position pub + public: ignition::transport::Node::Publisher consolePub; + /// \brief Physics update event connection public: event::ConnectionPtr updateConnection; @@ -217,6 +220,9 @@ namespace gazebo /// \brief Mutex to protect updates public: std::mutex mutex; + + /// \brief Odometer + public: double odom; }; } @@ -254,7 +260,8 @@ void PriusHybridPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) this->dataPtr->posePub = this->dataPtr->node.Advertise( "/prius_pose"); - + this->dataPtr->consolePub = + this->dataPtr->node.Advertise("/prius_console"); std::string handWheelJointName = this->dataPtr->model->GetName() + "::" + _sdf->Get("steering_wheel"); @@ -446,16 +453,16 @@ void PriusHybridPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) // again assumes wheel link is child of joint and has only one collision ignition::math::Vector3d flCenterPos = this->dataPtr->flWheelJoint->GetChild()->GetCollision(id) - ->GetWorldPose().pos.Ign(); + ->WorldPose().Pos(); ignition::math::Vector3d frCenterPos = this->dataPtr->frWheelJoint->GetChild()->GetCollision(id) - ->GetWorldPose().pos.Ign(); + ->WorldPose().Pos(); ignition::math::Vector3d blCenterPos = this->dataPtr->blWheelJoint->GetChild()->GetCollision(id) - ->GetWorldPose().pos.Ign(); + ->WorldPose().Pos(); ignition::math::Vector3d brCenterPos = this->dataPtr->brWheelJoint->GetChild()->GetCollision(id) - ->GetWorldPose().pos.Ign(); + ->WorldPose().Pos(); // track widths are computed first ignition::math::Vector3d vec3 = flCenterPos - frCenterPos; @@ -577,6 +584,7 @@ void PriusHybridPlugin::Update() { // has time been reset? this->dataPtr->lastSimTime = curTime; + this->dataPtr->lastMsgTime = curTime; return; } else if (ignition::math::equal(dt, 0.0)) @@ -584,19 +592,13 @@ void PriusHybridPlugin::Update() return; } - if ((curTime - this->dataPtr->lastMsgTime) > .5) - { - this->dataPtr->posePub.Publish( - ignition::msgs::Convert(this->dataPtr->model->GetWorldPose().Ign())); - this->dataPtr->lastMsgTime = curTime; - } - this->dataPtr->handWheelState = - this->dataPtr->handWheelJoint->GetAngle(0).Radian(); + + this->dataPtr->handWheelState = this->dataPtr->handWheelJoint->Position(0); this->dataPtr->flSteeringState = - this->dataPtr->flWheelSteeringJoint->GetAngle(0).Radian(); + this->dataPtr->flWheelSteeringJoint->Position(0); this->dataPtr->frSteeringState = - this->dataPtr->frWheelSteeringJoint->GetAngle(0).Radian(); + this->dataPtr->frWheelSteeringJoint->Position(0); this->dataPtr->flWheelState = this->dataPtr->flWheelJoint->GetVelocity(0); this->dataPtr->frWheelState = this->dataPtr->frWheelJoint->GetVelocity(0); @@ -729,25 +731,64 @@ void PriusHybridPlugin::Update() { this->dataPtr->handWheelCmd = 0; } + + if ((curTime - this->dataPtr->lastMsgTime) > .5) + { + this->dataPtr->posePub.Publish( + ignition::msgs::Convert(this->dataPtr->model->WorldPose())); + + ignition::msgs::Double_V consoleMsg; + + // linearVel (meter/sec) = (2*PI*r) * (rad/sec). + double linearVel = (2.0 * IGN_PI * this->dataPtr->flWheelRadius) * + ((this->dataPtr->flWheelState + this->dataPtr->frWheelState) * 0.5); + + // Convert meter/sec to miles/hour + linearVel *= 2.23694; + + // Distance traveled in miles. + this->dataPtr->odom += (fabs(linearVel) * dt/3600); + + // \todo: Actually compute MPG + double mpg = 1.0 / std::max(linearVel, 0.0); + + // Gear information: 1=drive, 2=reverse, 3=neutral + if (this->dataPtr->directionState == PriusHybridPluginPrivate::FORWARD) + consoleMsg.add_data(1.0); + else if (this->dataPtr->directionState == PriusHybridPluginPrivate::REVERSE) + consoleMsg.add_data(2.0); + else if (this->dataPtr->directionState == PriusHybridPluginPrivate::NEUTRAL) + consoleMsg.add_data(3.0); + + // MPH. A speedometer does not go negative. + consoleMsg.add_data(std::max(linearVel, 0.0)); + + // MPG + consoleMsg.add_data(mpg); + + // Miles + consoleMsg.add_data(this->dataPtr->odom); + + this->dataPtr->consolePub.Publish(consoleMsg); + this->dataPtr->lastMsgTime = curTime; + } } ///////////////////////////////////////////////// void PriusHybridPlugin::UpdateHandWheelRatio() { // The total range the steering wheel can rotate - this->dataPtr->handWheelHigh = - this->dataPtr->handWheelJoint->GetHighStop(0).Radian(); - this->dataPtr->handWheelLow = - this->dataPtr->handWheelJoint->GetLowStop(0).Radian(); + this->dataPtr->handWheelHigh = this->dataPtr->handWheelJoint->UpperLimit(0); + this->dataPtr->handWheelLow = this->dataPtr->handWheelJoint->LowerLimit(0); double handWheelRange = this->dataPtr->handWheelHigh - this->dataPtr->handWheelLow; double high = std::min( - this->dataPtr->flWheelSteeringJoint->GetHighStop(0).Radian(), - this->dataPtr->frWheelSteeringJoint->GetHighStop(0).Radian()); + this->dataPtr->flWheelSteeringJoint->UpperLimit(0), + this->dataPtr->frWheelSteeringJoint->UpperLimit(0)); high = std::min(high, this->dataPtr->maxSteer); double low = std::max( - this->dataPtr->flWheelSteeringJoint->GetLowStop(0).Radian(), - this->dataPtr->frWheelSteeringJoint->GetLowStop(0).Radian()); + this->dataPtr->flWheelSteeringJoint->LowerLimit(0), + this->dataPtr->frWheelSteeringJoint->LowerLimit(0)); low = std::max(low, -this->dataPtr->maxSteer); double tireAngleRange = high - low; diff --git a/worlds/raceway.world b/worlds/raceway.world index 31473b1..41e3c23 100644 --- a/worlds/raceway.world +++ b/worlds/raceway.world @@ -5,9 +5,8 @@ - 311.11 -160.80 21.48 0 0.36 2.41 + 107.81 -341.80 20 0 0.332 2.030003 - @@ -22,10 +21,11 @@ model://raceway + 25.5593 -2.5 0 0 0 -0.89187 - 286.74 -143.32 7.42 0 0 -2.228 + 95.619855 -308.245 7.532 0 0 3.081629 model://prius_hybrid From 681d1c1b4f429e881b62a72d65ad0e51d2cb278b Mon Sep 17 00:00:00 2001 From: nkoenig Date: Thu, 12 Jan 2017 13:01:40 -0800 Subject: [PATCH 3/7] Setup undo --- cmake/setup.sh.in | 2 -- 1 file changed, 2 deletions(-) diff --git a/cmake/setup.sh.in b/cmake/setup.sh.in index 8f18448..a3ba02c 100644 --- a/cmake/setup.sh.in +++ b/cmake/setup.sh.in @@ -1,5 +1,3 @@ -#!/bin/sh - # This file is the entry point for priuscup users. It should establish # all environment necessary to use priuscup. From 3a7b2dcdc796352b12934c18f89b46d1ef46a45d Mon Sep 17 00:00:00 2001 From: nkoenig Date: Thu, 12 Jan 2017 13:04:18 -0800 Subject: [PATCH 4/7] Updates --- plugins/PriusHybridPlugin.cc | 41 ++++++++++++++++++++++-------------- 1 file changed, 25 insertions(+), 16 deletions(-) diff --git a/plugins/PriusHybridPlugin.cc b/plugins/PriusHybridPlugin.cc index a9e42a2..8026222 100644 --- a/plugins/PriusHybridPlugin.cc +++ b/plugins/PriusHybridPlugin.cc @@ -261,9 +261,9 @@ void PriusHybridPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) this->dataPtr->node.Subscribe("/cmd_vel", &PriusHybridPlugin::OnCmdVel, this); this->dataPtr->posePub = this->dataPtr->node.Advertise( - "/prius_pose"); + "/prius/pose"); this->dataPtr->consolePub = - this->dataPtr->node.Advertise("/prius_console"); + this->dataPtr->node.Advertise("/prius/console"); std::string handWheelJointName = this->dataPtr->model->GetName() + "::" + _sdf->Get("steering_wheel"); @@ -602,8 +602,6 @@ void PriusHybridPlugin::Update() return; } - - this->dataPtr->handWheelState = this->dataPtr->handWheelJoint->Position(0); this->dataPtr->flSteeringState = this->dataPtr->flWheelSteeringJoint->Position(); @@ -620,8 +618,8 @@ void PriusHybridPlugin::Update() // PID (position) steering this->dataPtr->handWheelCmd = ignition::math::clamp(this->dataPtr->handWheelCmd, - -this->dataPtr->maxSteer / this->dataPtr->steeringRatio, - this->dataPtr->maxSteer / this->dataPtr->steeringRatio); + -this->dataPtr->maxSteer / this->dataPtr->steeringRatio, + this->dataPtr->maxSteer / this->dataPtr->steeringRatio); double steerError = this->dataPtr->handWheelState - this->dataPtr->handWheelCmd; double steerCmd = this->dataPtr->handWheelPID.Update(steerError, dt); @@ -648,17 +646,23 @@ void PriusHybridPlugin::Update() this->dataPtr->flSteeringState - this->dataPtr->flWheelSteeringCmd; double flwsCmd = this->dataPtr->flWheelSteeringPID.Update(flwsError, dt); this->dataPtr->flWheelSteeringJoint->SetForce(0, flwsCmd); - //this->dataPtr->flWheelSteeringJoint->SetPosition(0, this->dataPtr->flWheelSteeringCmd); - //this->dataPtr->flWheelSteeringJoint->SetLowStop(0, this->dataPtr->flWheelSteeringCmd); - //this->dataPtr->flWheelSteeringJoint->SetHighStop(0, this->dataPtr->flWheelSteeringCmd); + // this->dataPtr->flWheelSteeringJoint->SetPosition(0, + // this->dataPtr->flWheelSteeringCmd); + // this->dataPtr->flWheelSteeringJoint->SetLowStop(0, + // this->dataPtr->flWheelSteeringCmd); + // this->dataPtr->flWheelSteeringJoint->SetHighStop(0, + // this->dataPtr->flWheelSteeringCmd); double frwsError = this->dataPtr->frSteeringState - this->dataPtr->frWheelSteeringCmd; double frwsCmd = this->dataPtr->frWheelSteeringPID.Update(frwsError, dt); this->dataPtr->frWheelSteeringJoint->SetForce(0, frwsCmd); - //this->dataPtr->frWheelSteeringJoint->SetPosition(0, this->dataPtr->frWheelSteeringCmd); - //this->dataPtr->frWheelSteeringJoint->SetLowStop(0, this->dataPtr->frWheelSteeringCmd); - //this->dataPtr->frWheelSteeringJoint->SetHighStop(0, this->dataPtr->frWheelSteeringCmd); + // this->dataPtr->frWheelSteeringJoint->SetPosition(0, + // this->dataPtr->frWheelSteeringCmd); + // this->dataPtr->frWheelSteeringJoint->SetLowStop(0, + // this->dataPtr->frWheelSteeringCmd); + // this->dataPtr->frWheelSteeringJoint->SetHighStop(0, + // this->dataPtr->frWheelSteeringCmd); // Gas pedal torque. // Map gas torques to individual wheels. @@ -697,10 +701,14 @@ void PriusHybridPlugin::Update() brakePercent = ignition::math::clamp( brakePercent, this->dataPtr->minBrakePercent, 1.0); - this->dataPtr->flWheelJoint->SetParam("friction", 1, brakePercent * this->dataPtr->frontBrakeTorque); - this->dataPtr->frWheelJoint->SetParam("friction", 1, brakePercent * this->dataPtr->frontBrakeTorque); - this->dataPtr->blWheelJoint->SetParam("friction", 0, brakePercent * this->dataPtr->backBrakeTorque); - this->dataPtr->brWheelJoint->SetParam("friction", 0, brakePercent * this->dataPtr->backBrakeTorque); + this->dataPtr->flWheelJoint->SetParam("friction", 1, + brakePercent * this->dataPtr->frontBrakeTorque); + this->dataPtr->frWheelJoint->SetParam("friction", 1, + brakePercent * this->dataPtr->frontBrakeTorque); + this->dataPtr->blWheelJoint->SetParam("friction", 0, + brakePercent * this->dataPtr->backBrakeTorque); + this->dataPtr->brWheelJoint->SetParam("friction", 0, + brakePercent * this->dataPtr->backBrakeTorque); this->dataPtr->flWheelJoint->SetForce(1, flGasTorque); this->dataPtr->frWheelJoint->SetForce(1, frGasTorque); @@ -721,6 +729,7 @@ void PriusHybridPlugin::Update() this->dataPtr->handWheelCmd = 0; } + // Output prius car data. if ((curTime - this->dataPtr->lastMsgTime) > .5) { this->dataPtr->posePub.Publish( From 49be6d1d4f2e9d6fddb34bce93ee063d0744e8df Mon Sep 17 00:00:00 2001 From: nkoenig Date: Thu, 12 Jan 2017 14:08:02 -0800 Subject: [PATCH 5/7] Spacing --- plugins/PriusHybridPlugin.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/plugins/PriusHybridPlugin.cc b/plugins/PriusHybridPlugin.cc index 8026222..4caf845 100644 --- a/plugins/PriusHybridPlugin.cc +++ b/plugins/PriusHybridPlugin.cc @@ -724,6 +724,7 @@ void PriusHybridPlugin::Update() this->dataPtr->gasPedalPercent = 0.0; this->dataPtr->brakePedalPercent = 0.0; } + if ((curTime - this->dataPtr->lastSteeringCmdTime).Double() > 1.0) { this->dataPtr->handWheelCmd = 0; From c1a799f6b7a4d06aefbe7eabdbccdd847cee4c07 Mon Sep 17 00:00:00 2001 From: nkoenig Date: Thu, 12 Jan 2017 14:18:32 -0800 Subject: [PATCH 6/7] fix build --- plugins/PriusHybridPlugin.cc | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/plugins/PriusHybridPlugin.cc b/plugins/PriusHybridPlugin.cc index 8659002..adff2e0 100644 --- a/plugins/PriusHybridPlugin.cc +++ b/plugins/PriusHybridPlugin.cc @@ -608,10 +608,14 @@ void PriusHybridPlugin::Update() this->dataPtr->frSteeringAngle = this->dataPtr->frWheelSteeringJoint->Position(); - this->dataPtr->flWheelAngularVelocity = this->dataPtr->flWheelJoint->GetVelocity(1); - this->dataPtr->frWheelAngularVelocity = this->dataPtr->frWheelJoint->GetVelocity(1); - this->dataPtr->blWheelAngularVelocity = this->dataPtr->blWheelJoint->GetVelocity(0); - this->dataPtr->brWheelAngularVelocity = this->dataPtr->brWheelJoint->GetVelocity(0); + this->dataPtr->flWheelAngularVelocity = + this->dataPtr->flWheelJoint->GetVelocity(1); + this->dataPtr->frWheelAngularVelocity = + this->dataPtr->frWheelJoint->GetVelocity(1); + this->dataPtr->blWheelAngularVelocity = + this->dataPtr->blWheelJoint->GetVelocity(0); + this->dataPtr->brWheelAngularVelocity = + this->dataPtr->brWheelJoint->GetVelocity(0); this->dataPtr->lastSimTime = curTime; @@ -740,7 +744,8 @@ void PriusHybridPlugin::Update() // linearVel (meter/sec) = (2*PI*r) * (rad/sec). double linearVel = (2.0 * IGN_PI * this->dataPtr->flWheelRadius) * - ((this->dataPtr->flWheelState + this->dataPtr->frWheelState) * 0.5); + ((this->dataPtr->flWheelAngularVelocity + + this->dataPtr->frWheelAngularVelocity) * 0.5); // Convert meter/sec to miles/hour linearVel *= 2.23694; From fdcc8a5b50eab4d2c7c985ab64093934cb8c1436 Mon Sep 17 00:00:00 2001 From: Nathan Koenig Date: Thu, 12 Jan 2017 15:17:36 -0800 Subject: [PATCH 7/7] Close branch prius_model_plugin_nate