Skip to content

Commit

Permalink
MecanumDrive: remove odometry
Browse files Browse the repository at this point in the history 
Signed-off-by: Steve Peters <scpeters@openrobotics.org>
  • Loading branch information
@scpeters
scpeters committed Mar 24, 2021
1 parent 53792df commit 49eb8cb
Showing 1 changed file with 0 additions and 150 deletions.
150 changes: 0 additions & 150 deletions src/systems/mecanum_drive/MecanumDrive.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,13 +64,6 @@ class ignition::gazebo::systems::MecanumDrivePrivate
/// \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
Expand Down Expand Up @@ -496,40 +489,6 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
*vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed});
}
}

// Create the left and right side joint position components if they
// don't exist.
auto frontLeftPos = _ecm.Component<components::JointPosition>(
this->dataPtr->frontLeftJoints[0]);
if (!frontLeftPos)
{
_ecm.CreateComponent(this->dataPtr->frontLeftJoints[0],
components::JointPosition());
}

auto frontRightPos = _ecm.Component<components::JointPosition>(
this->dataPtr->frontRightJoints[0]);
if (!frontRightPos)
{
_ecm.CreateComponent(this->dataPtr->frontRightJoints[0],
components::JointPosition());
}

auto backLeftPos = _ecm.Component<components::JointPosition>(
this->dataPtr->backLeftJoints[0]);
if (!backLeftPos)
{
_ecm.CreateComponent(this->dataPtr->backLeftJoints[0],
components::JointPosition());
}

auto backRightPos = _ecm.Component<components::JointPosition>(
this->dataPtr->backRightJoints[0]);
if (!backRightPos)
{
_ecm.CreateComponent(this->dataPtr->backRightJoints[0],
components::JointPosition());
}
}

//////////////////////////////////////////////////
Expand All @@ -542,115 +501,6 @@ void MecanumDrive::PostUpdate(const UpdateInfo &_info,
return;

this->dataPtr->UpdateVelocity(_info, _ecm);
this->dataPtr->UpdateOdometry(_info, _ecm);
}

//////////////////////////////////////////////////
void MecanumDrivePrivate::UpdateOdometry(
const ignition::gazebo::UpdateInfo &_info,
const ignition::gazebo::EntityComponentManager &_ecm)
{
IGN_PROFILE("MecanumDrive::UpdateOdometry");
// Initialize, if not already initialized.
if (!this->odom.Initialized())
{
this->odom.Init(std::chrono::steady_clock::time_point(_info.simTime));
return;
}

if (this->frontLeftJoints.empty() || this->backLeftJoints.empty() ||
this->frontRightJoints.empty() || this->backRightJoints.empty())
{
return;
}

// Get the first joint positions.
auto frontLeftPos = _ecm.Component<components::JointPosition>(
this->frontLeftJoints[0]);
auto frontRightPos = _ecm.Component<components::JointPosition>(
this->frontRightJoints[0]);
auto backLeftPos = _ecm.Component<components::JointPosition>(
this->backLeftJoints[0]);
auto backRightPos = _ecm.Component<components::JointPosition>(
this->backRightJoints[0]);

// Abort if the joints were not found or just created.
if (!frontLeftPos || !frontRightPos || !backLeftPos || !backRightPos ||
frontLeftPos->Data().empty() || frontRightPos->Data().empty() ||
backLeftPos->Data().empty() || backRightPos->Data().empty())
{
return;
}

// this->odom.Update(leftPos->Data()[0], rightPos->Data()[0],
// std::chrono::steady_clock::time_point(_info.simTime));

// // Throttle 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->odom.X());
// msg.mutable_pose()->mutable_position()->set_y(this->odom.Y());

// math::Quaterniond orientation(0, 0, *this->odom.Heading());
// msgs::Set(msg.mutable_pose()->mutable_orientation(), orientation);

// msg.mutable_twist()->mutable_linear()->set_x(this->odom.LinearVelocity());
// msg.mutable_twist()->mutable_angular()->set_z(
// *this->odom.AngularVelocity());

// // Set the time stamp in the header
// msg.mutable_header()->mutable_stamp()->CopyFrom(
// convert<msgs::Time>(_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<std::string> 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);
// }

// // Construct the Pose_V/tf message and publish it.
// msgs::Pose_V tfMsg;
// ignition::msgs::Pose *tfMsgPose = nullptr;
// tfMsgPose = tfMsg.add_pose();
// tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header());
// tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position());
// tfMsgPose->mutable_orientation()->CopyFrom(
// msg.mutable_pose()->orientation());

// // Publish the messages
// this->odomPub.Publish(msg);
// this->tfPub.Publish(tfMsg);
}

//////////////////////////////////////////////////
Expand Down

0 comments on commit 49eb8cb

Please sign in to comment.