From 682d6576a3b2c0022823600b4ee6f1d6d789caee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 5 Feb 2021 18:33:33 +0100 Subject: [PATCH] Fixed state interfaces (#53) Signed-off-by: ahcorde --- gazebo_ros2_control/src/gazebo_system.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 1602af1f..943116b1 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -168,18 +168,18 @@ bool GazeboSystem::initSim( for (unsigned int i = 0; i < hardware_info.joints[j].state_interfaces.size(); i++) { if (hardware_info.joints[j].state_interfaces[i].name == "position") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); - this->dataPtr->joint_pos_cmd_[j] = std::make_shared( - joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joint_position_cmd_[j]); + this->dataPtr->joint_pos_state_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joint_position_[j]); } if (hardware_info.joints[j].state_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); - this->dataPtr->joint_vel_cmd_[j] = std::make_shared( - joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joint_velocity_cmd_[j]); + this->dataPtr->joint_vel_state_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joint_velocity_[j]); } if (hardware_info.joints[j].state_interfaces[i].name == "effort") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); - this->dataPtr->joint_eff_cmd_[j] = std::make_shared( - joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joint_effort_cmd_[j]); + this->dataPtr->joint_eff_state_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joint_effort_[j]); } } }