Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[DiffDrive] add enable/disable #772

Merged
merged 6 commits into from
Apr 21, 2021
Merged

[DiffDrive] add enable/disable #772

Changes from 2 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
591948b
add enable/disable diffdrive
Apr 18, 2021
8e09d66
remove debug
Apr 18, 2021
570ebd1
do not subscribe to enable if topic is empty
Apr 19, 2021
bac8ffa
add test
Apr 19, 2021
4cc8d00
lint and style
Apr 20, 2021
d7e9d58
change enable type to bool and renamed to enabled
Apr 20, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 30 additions & 1 deletion src/systems/diff_drive/DiffDrive.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,10 @@ class ignition::gazebo::systems::DiffDrivePrivate
/// \param[in] _msg Velocity message
public: void OnCmdVel(const ignition::msgs::Twist &_msg);

/// \brief Callback for enable/disable subscription
/// \param[in] _msg Boolean message
public: void OnEnable(const ignition::msgs::Boolean &_msg);

/// \brief Update odometry and publish an odometry message.
/// \param[in] _info System update information.
/// \param[in] _ecm The EntityComponentManager of the given simulation
Expand Down Expand Up @@ -139,6 +143,9 @@ class ignition::gazebo::systems::DiffDrivePrivate
/// \brief Last target velocity requested.
public: msgs::Twist targetVel;

/// \brief Last enable/disable requested.
public: msgs::Boolean enable;
doisyg marked this conversation as resolved.
Show resolved Hide resolved

/// \brief A mutex to protect the target velocity command.
public: std::mutex mutex;

Expand Down Expand Up @@ -274,6 +281,15 @@ void DiffDrive::Configure(const Entity &_entity,
this->dataPtr->node.Subscribe(topic, &DiffDrivePrivate::OnCmdVel,
this->dataPtr.get());

// Subscribe to enable/disable
std::vector<std::string> enableTopics;
enableTopics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + "/enable");
auto enableTopic = validTopic(enableTopics);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This method may return an empty string. You should check this


this->dataPtr->node.Subscribe(enableTopic, &DiffDrivePrivate::OnEnable,
this->dataPtr.get());
this->dataPtr->enable.set_data(true);

std::vector<std::string> odomTopics;
if (_sdf->HasElement("odom_topic"))
{
Expand Down Expand Up @@ -565,7 +581,20 @@ void DiffDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info,
void DiffDrivePrivate::OnCmdVel(const msgs::Twist &_msg)
{
std::lock_guard<std::mutex> lock(this->mutex);
this->targetVel = _msg;
if (this->enable.data()) {
this->targetVel = _msg;
}
}

void DiffDrivePrivate::OnEnable(const msgs::Boolean &_msg)
{
std::lock_guard<std::mutex> lock(this->mutex);
this->enable = _msg;
if (!this->enable.data()) {
math::Vector3d zeroVector{0, 0, 0};
msgs::Set(this->targetVel.mutable_linear(), zeroVector);
msgs::Set(this->targetVel.mutable_angular(), zeroVector);
}
}

IGNITION_ADD_PLUGIN(DiffDrive,
Expand Down