18 #include <gazebo/common/Assert.hh> 19 #include <gazebo/common/Console.hh> 20 #include <gazebo/physics/Joint.hh> 21 #include <gazebo/physics/Model.hh> 28 GZ_ASSERT(_world,
"ScoringPlugin::Load(): NULL world pointer");
29 GZ_ASSERT(_sdf,
"ScoringPlugin::Load(): NULL _sdf pointer");
37 gzerr <<
"Scoring disabled" << std::endl;
68 this->
score = _newScore;
193 gzerr <<
"Unable to release [" << jointName <<
"]" << std::endl;
196 this->lockJointNames.clear();
198 gzmsg <<
"Vehicle released" << std::endl;
220 if (!this->
sdf->HasElement(
"vehicle"))
222 gzerr <<
"Unable to find <vehicle> element in SDF." << std::endl;
228 if (!this->
sdf->HasElement(
"task_name"))
230 gzerr <<
"Unable to find <task_name> element in SDF." << std::endl;
233 this->
taskName = this->
sdf->Get<std::string>(
"task_name");
236 if (this->
sdf->HasElement(
"topic"))
237 this->
topic = this->
sdf->Get<std::string>(
"topic");
240 if (this->
sdf->HasElement(
"initial_state_duration"))
242 double value = this->
sdf->Get<
double>(
"initial_state_duration");
245 gzerr <<
"<initial_state_duration> value should not be negative." 253 if (this->
sdf->HasElement(
"ready_state_duration"))
255 double value = this->
sdf->Get<
double>(
"ready_state_duration");
258 gzerr <<
"<ready_state_duration> value should not be negative." 267 if (this->
sdf->HasElement(
"running_state_duration"))
269 double value = this->
sdf->Get<
double>(
"running_state_duration");
272 gzerr <<
"<running_state_duration> value should not be negative." 286 if (this->
sdf->HasElement(
"release_joints"))
288 auto releaseJointsElem = this->
sdf->GetElement(
"release_joints");
291 if (!releaseJointsElem->HasElement(
"joint"))
293 gzerr <<
"Unable to find <joint> element in SDF." << std::endl;
297 auto jointElem = releaseJointsElem->GetElement(
"joint");
303 if (!jointElem->HasElement(
"name"))
305 gzerr <<
"Unable to find <name> element in SDF." << std::endl;
309 const std::string jointName = jointElem->Get<std::string>(
"name");
313 jointElem = jointElem->GetNextElement(
"joint");
void UpdateTime()
Update all time-related variables.
std::string TaskName() const
Get the task name.
double Score() const
Get the current score.
void publish(const boost::shared_ptr< M > &message) const
void ReleaseVehicle()
Tries to release the vehicle in case is locked.
std::vector< std::string > lockJointNames
The name of the joints to be dettached during ReleaseVehicle().
gazebo::common::Time currentTime
Current time (simulation).
void UpdateTaskMessage()
Update the task stats message.
std::string TaskState() const
Get the task state.
gazebo::common::Time remainingTime
Remaining time since the start of the task (running state).
vmrc_gazebo::Task taskMsg
The next task message to be published.
void Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf)
double runningStateDuration
Duration (seconds) of the running state (max task time).
void Finish()
Finish the current task. This will set the "finished" flag in the task message to true...
ros::Publisher taskPub
Publisher for the task state.
virtual void OnRunning()
Callback executed when the task state transition into "running".
void UpdateTaskState()
Update the state of the current task.
gazebo::common::Time ElapsedTime() const
Elapsed time in the running state.
bool ParseSDFParameters()
Parse all SDF parameters.
std::string taskState
The task state.
gazebo::common::Time elapsedTime
void PublishStats()
Publish the task stats over a ROS topic.
std::unique_ptr< ros::NodeHandle > rosNode
ROS node handle.
void Update()
Callback executed at every world update.
gazebo::common::Time runningTime
Absolute time specifying the start of the running state.
gazebo::common::Time readyTime
Absolute time specifying the start of the ready state.
gazebo::physics::WorldPtr world
A world pointer.
gazebo::common::Time finishTime
Absolute time specifying the start of the finish state.
gazebo::physics::ModelPtr vehicleModel
Pointer to the vehicle to score.
sdf::ElementPtr sdf
Pointer to the SDF plugin element.
double initialStateDuration
Duration (seconds) of the initial state.
gazebo::common::Time lastStatsSent
Time at which the last message was sent.
gazebo::event::ConnectionPtr updateConnection
Pointer to the update event connection.
gazebo::common::Time RemainingTime() const
Remaining time in the running state.
std::string topic
Topic where the task stats are published.
double readyStateDuration
Duration (seconds) of the ready state.
std::string taskName
The name of the task.
virtual void OnFinished()
Callback executed when the task state transition into "finished".
bool timedOut
Whether the current task has timed out or not.
bool ParseJoints()
Parse the joints section of the SDF block.
void SetScore(double _newScore)
Set the score.
virtual void OnReady()
Callback executed when the task state transition into "ready".
std::string vehicleName
The name of the vehicle to score.