////////////////////////////////////////////////////////////////////////////////
// Publish hand wheel, gas pedal, and brake pedal on ROS
void DRCVehicleROSPlugin::RosPublishStates()
{
if (this->world->GetSimTime() - this->lastRosPublishTime >=
this->rosPublishPeriod)
{
// Update time
this->lastRosPublishTime = this->world->GetSimTime();
// Publish Float64 messages
std_msgs::Float64 msg_steer, msg_brake, msg_gas, msg_hand_brake;
msg_steer.data = GetHandWheelState();
this->pubHandWheelState.publish(msg_steer);
msg_brake.data = GetBrakePedalPercent();
this->pubBrakePedalState.publish(msg_brake);
msg_gas.data = GetGasPedalPercent();
this->pubGasPedalState.publish(msg_gas);
msg_hand_brake.data = GetHandBrakePercent();
this->pubHandBrakeState.publish(msg_hand_brake);
// Publish Int8
std_msgs::Int8 msg_key, msg_direction;
msg_key.data = static_cast<int8_t>(GetKeyState());
this->pubKeyState.publish(msg_key);
msg_direction.data = static_cast<int8_t>(GetDirectionState());
this->pubDirectionState.publish(msg_direction);
}
}
////////////////////////////////////////////////////////////////////////////////
// Publish hand wheel, gas pedal, and brake pedal on ROS
void DRCVehicleROSPlugin::RosPublishStates()
{
if (this->world->GetSimTime() - this->lastRosPublishTime >=
this->rosPublishPeriod)
{
// Update time
this->lastRosPublishTime = this->world->GetSimTime();
// Publish Float64 messages
std_msgs::Float64 msg_steer, msg_brake, msg_gas, msg_hand_brake, velocityflwheel,velocityfrwheel,
velocityblwheel,velocitybrwheel;
msg_steer.data = GetHandWheelState();
this->pubHandWheelState.publish(msg_steer);
msg_brake.data = GetBrakePedalPercent();
this->pubBrakePedalState.publish(msg_brake);
msg_gas.data = GetGasPedalPercent();
this->pubGasPedalState.publish(msg_gas);
msg_hand_brake.data = GetHandBrakePercent();
this->pubHandBrakeState.publish(msg_hand_brake);
//Publish the velocities of the wheels, instead of making a function that returns the values I
//thought it would be easier to just to make it public.
//Front Left Wheel
velocityflwheel.data = DRCVehiclePlugin::flWheelStatePUB;
this->flWheelStatePUB.publish(velocityflwheel);
//Front Right Wheel
velocityfrwheel.data = DRCVehiclePlugin::frWheelStatePUB;
this->frWheelStatePUB.publish(velocityfrwheel);
//Back Left Wheel
velocityblwheel.data = DRCVehiclePlugin::blWheelStatePUB;
this->blWheelStatePUB.publish(velocityblwheel);
//Back Right Wheel
velocitybrwheel.data = DRCVehiclePlugin::brWheelStatePUB;
this->brWheelStatePUB.publish(velocitybrwheel);
// Publish Int8
std_msgs::Int8 msg_key, msg_direction;
msg_key.data = static_cast<int8_t>(GetKeyState());
this->pubKeyState.publish(msg_key);
msg_direction.data = static_cast<int8_t>(GetDirectionState());
this->pubDirectionState.publish(msg_direction);
}
}
