From 7c23285a9d68f41b89071cc2fcf331509efdb67c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 2 Oct 2022 18:35:11 +0200 Subject: [PATCH] Added separate storage for joint names with state and command interfaces. --- gazebo_ros2_control/src/gazebo_system.cpp | 46 +++++++++++++++++------ 1 file changed, 34 insertions(+), 12 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 39844e14..13c484f2 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -53,9 +53,12 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief last time the write method was called. rclcpp::Time last_update_sim_time_ros_; - /// \brief vector with the joint's names. + /// \brief vector with the joint's names that have state interfaces. std::vector joint_names_; + /// \brief vector with the joint's names that have command interfaces. + std::vector joint_names_cmd_; + /// \brief vector with the control method defined in the URDF for each joint. std::vector joint_control_methods_; @@ -143,6 +146,7 @@ void GazeboSystem::registerJoints( this->dataPtr->n_dof_ = hardware_info.joints.size(); this->dataPtr->joint_names_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_names_cmd_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_control_methods_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_position_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_velocity_.resize(this->dataPtr->n_dof_); @@ -153,7 +157,25 @@ void GazeboSystem::registerJoints( for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) { auto & joint_info = hardware_info.joints[j]; - std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name; + std::string joint_name = joint_info.name; + if (!joint_info.state_interfaces.empty()) + { + this->dataPtr->joint_names_[j] = joint_info.name; + } + else + { + // set to empty string if no state interfaces for this index + this->dataPtr->joint_names_[j] = ""; + } + if (!joint_info.command_interfaces.empty()) + { + this->dataPtr->joint_names_cmd_[j] = joint_info.name; + } + else + { + // set to empty string if no state interfaces for this index + this->dataPtr->joint_names_cmd_[j] = ""; + } gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name); this->dataPtr->sim_joints_.push_back(simjoint); @@ -468,20 +490,20 @@ GazeboSystem::perform_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) { - for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { + for (unsigned int j = 0; j < this->dataPtr->joint_names_cmd_.size(); j++) { for (const std::string & interface_name : stop_interfaces) { // Clear joint control method bits corresponding to stop interfaces - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + if (interface_name == (this->dataPtr->joint_names_cmd_[j] + "/" + hardware_interface::HW_IF_POSITION)) { this->dataPtr->joint_control_methods_[j] &= static_cast(VELOCITY & EFFORT); } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + if (interface_name == (this->dataPtr->joint_names_cmd_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) { this->dataPtr->joint_control_methods_[j] &= static_cast(POSITION & EFFORT); } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + if (interface_name == (this->dataPtr->joint_names_cmd_[j] + "/" + hardware_interface::HW_IF_EFFORT)) { this->dataPtr->joint_control_methods_[j] &= @@ -491,17 +513,17 @@ GazeboSystem::perform_command_mode_switch( // Set joint control method bits corresponding to start interfaces for (const std::string & interface_name : start_interfaces) { - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + if (interface_name == (this->dataPtr->joint_names_cmd_[j] + "/" + hardware_interface::HW_IF_POSITION)) { this->dataPtr->joint_control_methods_[j] |= POSITION; } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + if (interface_name == (this->dataPtr->joint_names_cmd_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) { this->dataPtr->joint_control_methods_[j] |= VELOCITY; } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + if (interface_name == (this->dataPtr->joint_names_cmd_[j] + "/" + hardware_interface::HW_IF_EFFORT)) { this->dataPtr->joint_control_methods_[j] |= EFFORT; @@ -517,7 +539,7 @@ hardware_interface::return_type GazeboSystem::read( const rclcpp::Duration & period) { for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { - if (this->dataPtr->sim_joints_[j]) { + if (this->dataPtr->sim_joints_[j] && !this->dataPtr->joint_names_[j].empty()) { this->dataPtr->joint_position_[j] = this->dataPtr->sim_joints_[j]->Position(0); this->dataPtr->joint_velocity_[j] = this->dataPtr->sim_joints_[j]->GetVelocity(0); this->dataPtr->joint_effort_[j] = this->dataPtr->sim_joints_[j]->GetForce(0u); @@ -586,8 +608,8 @@ hardware_interface::return_type GazeboSystem::write( } } - for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { - if (this->dataPtr->sim_joints_[j]) { + for (unsigned int j = 0; j < this->dataPtr->joint_names_cmd_.size(); j++) { + if (this->dataPtr->sim_joints_[j] && !this->dataPtr->joint_names_cmd_[j].empty()) { if (this->dataPtr->joint_control_methods_[j] & POSITION) { this->dataPtr->sim_joints_[j]->SetPosition(0, this->dataPtr->joint_position_cmd_[j], true); }