diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index d1e5c4637c..59dd701675 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -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 @@ -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( - this->dataPtr->frontLeftJoints[0]); - if (!frontLeftPos) - { - _ecm.CreateComponent(this->dataPtr->frontLeftJoints[0], - components::JointPosition()); - } - - auto frontRightPos = _ecm.Component( - this->dataPtr->frontRightJoints[0]); - if (!frontRightPos) - { - _ecm.CreateComponent(this->dataPtr->frontRightJoints[0], - components::JointPosition()); - } - - auto backLeftPos = _ecm.Component( - this->dataPtr->backLeftJoints[0]); - if (!backLeftPos) - { - _ecm.CreateComponent(this->dataPtr->backLeftJoints[0], - components::JointPosition()); - } - - auto backRightPos = _ecm.Component( - this->dataPtr->backRightJoints[0]); - if (!backRightPos) - { - _ecm.CreateComponent(this->dataPtr->backRightJoints[0], - components::JointPosition()); - } } ////////////////////////////////////////////////// @@ -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( - this->frontLeftJoints[0]); - auto frontRightPos = _ecm.Component( - this->frontRightJoints[0]); - auto backLeftPos = _ecm.Component( - this->backLeftJoints[0]); - auto backRightPos = _ecm.Component( - 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(_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); - // } - - // // 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); } //////////////////////////////////////////////////