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

implemented perform_command_mode_switch override in GazeboSystem #136

Merged
merged 2 commits into from
Jul 28, 2022
Merged

implemented perform_command_mode_switch override in GazeboSystem #136

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8e923b5
implemented perform_command_mode_switch override in GazeboSystem
ksotebeer Jun 14, 2022
0bab2b8
Update variable names in perform_command_mode_switch method
ksotebeer Jun 28, 2022
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
5 changes: 5 additions & 0 deletions gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,11 @@ class GazeboSystem : public GazeboSystemInterface
// Documentation Inherited
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;

// Documentation Inherited
hardware_interface::return_type perform_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces) override;

// Documentation Inherited
hardware_interface::return_type read(
const rclcpp::Time & time,
Expand Down
40 changes: 37 additions & 3 deletions gazebo_ros2_control/src/gazebo_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -252,7 +252,6 @@ void GazeboSystem::registerJoints(
for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) {
if (joint_info.command_interfaces[i].name == "position") {
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position");
this->dataPtr->joint_control_methods_[j] |= POSITION;
this->dataPtr->command_interfaces_.emplace_back(
joint_name + suffix,
hardware_interface::HW_IF_POSITION,
Expand All @@ -263,7 +262,6 @@ void GazeboSystem::registerJoints(
}
if (joint_info.command_interfaces[i].name == "velocity") {
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity");
this->dataPtr->joint_control_methods_[j] |= VELOCITY;
this->dataPtr->command_interfaces_.emplace_back(
joint_name + suffix,
hardware_interface::HW_IF_VELOCITY,
Expand All @@ -273,7 +271,6 @@ void GazeboSystem::registerJoints(
}
}
if (joint_info.command_interfaces[i].name == "effort") {
this->dataPtr->joint_control_methods_[j] |= EFFORT;
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort");
this->dataPtr->command_interfaces_.emplace_back(
joint_name + suffix,
Expand Down Expand Up @@ -454,6 +451,43 @@ CallbackReturn GazeboSystem::on_deactivate(const rclcpp_lifecycle::State & previ
return CallbackReturn::SUCCESS;
}

hardware_interface::return_type
GazeboSystem::perform_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces)
{
for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) {
for (const std::string & key : stop_interfaces) {
// Clear joint control method bits corresponding to stop interfaces
if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) {
this->dataPtr->joint_control_methods_[j] &= static_cast<ControlMethod_>(VELOCITY & EFFORT);
}
if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) {
this->dataPtr->joint_control_methods_[j] &= static_cast<ControlMethod_>(POSITION & EFFORT);
}
if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) {
destogl marked this conversation as resolved.
Show resolved Hide resolved
this->dataPtr->joint_control_methods_[j] &=
static_cast<ControlMethod_>(POSITION & VELOCITY);
destogl marked this conversation as resolved.
Show resolved Hide resolved
}
}

// Set joint control method bits corresponding to start interfaces
for (const std::string & key : start_interfaces) {
if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) {
this->dataPtr->joint_control_methods_[j] |= POSITION;
}
if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) {
this->dataPtr->joint_control_methods_[j] |= VELOCITY;
}
if (key == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) {
destogl marked this conversation as resolved.
Show resolved Hide resolved
this->dataPtr->joint_control_methods_[j] |= EFFORT;
}
}
}

return hardware_interface::return_type::OK;
}

hardware_interface::return_type GazeboSystem::read(
const rclcpp::Time & time,
const rclcpp::Duration & period)
Expand Down