From 7089664574bbcf8532c8fb4e17207b93b2f2b850 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 14 Oct 2020 20:57:02 +0200 Subject: [PATCH 01/12] Updated with recent ros2_control changes Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 4 +- Docker/Dockerfile | 5 +- .../default_robot_hw_sim.hpp | 38 +-- .../src/default_robot_hw_sim.cpp | 314 ++++++------------ .../src/gazebo_ros2_control_plugin.cpp | 59 ++-- 5 files changed, 149 insertions(+), 271 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 7a1b40f9..07ffd2c6 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -29,10 +29,8 @@ jobs: git clone https://github.com/ros-controls/ros2_control git clone https://github.com/ros-controls/ros2_controllers git clone https://github.com/ddengster/ros2_control/ -b coffeebot_deps ros2_control_ddengster - git clone https://github.com/ros-controls/control_toolbox/ -b ros2-master touch ros2_control_ddengster/COLCON_IGNORE - cp -r ros2_control_ddengster/joint_limits_interface ros2_control_ddengster/transmission_interface ros2_control - sed -i '/rclcpp/d' ros2_control/joint_limits_interface/CMakeLists.txt + cp -r ros2_control_ddengster/transmission_interface ros2_control rosdep update rosdep install --from-paths ./ -i -y --rosdistro foxy \ --ignore-src diff --git a/Docker/Dockerfile b/Docker/Dockerfile index 3769c83b..2feddc93 100644 --- a/Docker/Dockerfile +++ b/Docker/Dockerfile @@ -16,13 +16,10 @@ RUN mkdir -p /home/ros2_ws/src \ && cd /home/ros2_ws/src/ \ && git clone https://github.com/ros-simulation/gazebo_ros2_control \ && git clone https://github.com/ros-controls/ros2_control \ - && git clone https://github.com/ros-controls/control_toolbox -b ros2-master \ && git clone https://github.com/ros-controls/ros2_controllers \ && git clone https://github.com/ddengster/ros2_control/ -b coffeebot_deps ros2_control_ddengster \ && touch ros2_control_ddengster/COLCON_IGNORE \ - && cp -r ros2_control_ddengster/joint_limits_interface ros2_control_ddengster/transmission_interface ros2_control \ - && sed -i '/rclcpp/d' ros2_control/joint_limits_interface/CMakeLists.txt \ - && sed -i '/rclcpp/d' ros2_control/joint_limits_interface/package.xml \ + && cp -r ros2_control_ddengster/transmission_interface ros2_control \ && rosdep update \ && rosdep install --from-paths ./ -i -y --rosdistro foxy \ --ignore-src diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp index d0359a64..f3aba2a8 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp @@ -40,10 +40,6 @@ // ros_control #include "control_toolbox/pid_ros.hpp" -#if 0 // @todo -#include -#include -#endif #include "joint_limits_interface/joint_limits.hpp" #include "joint_limits_interface/joint_limits_interface.hpp" @@ -93,7 +89,7 @@ class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim // Return the joint's type, lower position limit, upper position limit, and effort limit. void registerJointLimits( const std::string & joint_name, - const hardware_interface::JointStateHandle & joint_handle, + const std::shared_ptr & joint_handle, const ControlMethod ctrl_method, const rclcpp::Node::SharedPtr & joint_limit_nh, const urdf::Model * const urdf_model, @@ -103,20 +99,6 @@ class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim unsigned int n_dof_; -#if 0 // @todo - hardware_interface::JointStateInterface js_interface_; - hardware_interface::EffortJointInterface ej_interface_; - hardware_interface::VelocityJointInterface vj_interface_; - hardware_interface::PositionJointInterface pj_interface_; -#endif -#if 0 // @todo - joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_; - joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_; - joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_; - joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_; - joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_; - joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_; -#endif std::vector joint_names_; std::vector joint_types_; std::vector joint_lower_limits_; @@ -136,22 +118,12 @@ class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim std::vector sim_joints_; - std::vector joint_states_; - std::vector joint_cmds_; - std::vector joint_eff_cmdhandle_; - std::vector joint_vel_cmdhandle_; + std::vector> joint_states_; + std::vector> joint_cmds_; + std::vector> joint_eff_cmdhandle_; + std::vector> joint_vel_cmdhandle_; std::vector joint_opmodehandles_; - // limits - std::vector joint_pos_limit_handles_; - std::vector - joint_pos_soft_limit_handles_; - std::vector joint_eff_limit_handles_; - std::vector - joint_eff_soft_limit_handles_; - std::vector joint_vel_limit_handles_; - std::vector joint_vel_soft_limit_handles_; - std::string physics_type_; // e_stop_active_ is true if the emergency stop is active. diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index 0f92bd15..f9a1412a 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -49,53 +49,45 @@ bool DefaultRobotHWSim::initSim( const urdf::Model * const urdf_model, std::vector transmissions) { - // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each - // parameter's name is "joint_limits/". An example is "joint_limits/axle_joint". rclcpp::Node::SharedPtr & joint_limit_nh = model_nh; // Resize vectors to our DOF n_dof_ = transmissions.size(); + joint_names_.resize(n_dof_); + joint_control_methods_.resize(n_dof_); + joint_types_.resize(n_dof_); joint_lower_limits_.resize(n_dof_); joint_upper_limits_.resize(n_dof_); joint_effort_limits_.resize(n_dof_); joint_vel_limits_.resize(n_dof_); - joint_control_methods_.resize(n_dof_); -#if 0 - pid_controllers_.resize(n_dof_); -#endif + joint_position_.resize(n_dof_); joint_velocity_.resize(n_dof_); joint_effort_.resize(n_dof_); joint_effort_command_.resize(n_dof_); joint_position_command_.resize(n_dof_); joint_velocity_command_.resize(n_dof_); - joint_opmodes_.resize(n_dof_); - joint_states_.resize(n_dof_); joint_cmds_.resize(n_dof_); + joint_opmodes_.resize(n_dof_); joint_opmodehandles_.resize(n_dof_); - joint_eff_cmdhandle_.resize(n_dof_); - joint_vel_cmdhandle_.resize(n_dof_); - joint_pos_limit_handles_.resize(n_dof_); - joint_pos_soft_limit_handles_.resize(n_dof_); - joint_eff_limit_handles_.resize(n_dof_); - joint_eff_soft_limit_handles_.resize(n_dof_); - joint_vel_limit_handles_.resize(n_dof_); + joint_vel_cmdhandle_.resize(n_dof_); + joint_eff_cmdhandle_.resize(n_dof_); // Initialize values for (unsigned int j = 0; j < n_dof_; j++) { // Check that this transmission has one joint if (transmissions[j].joints_.empty()) { - std::cerr << "Transmission " << transmissions[j].name_ << - " has no associated joints."; + RCLCPP_ERROR(joint_limit_nh->get_logger(), "Transmission %s has no associated joints.", + transmissions[j].name_.c_str()); continue; } else if (transmissions[j].joints_.size() > 1) { - std::cerr << "Transmission " << transmissions[j].name_ << - " has more than one joint. Currently the default robot hardware simulation " << - " interface only supports one."; + RCLCPP_ERROR(joint_limit_nh->get_logger(), "Transmission " + " has more than one joint. Currently the default robot hardware simulation " + " interface only supports one.", transmissions[j].name_.c_str()); continue; } @@ -106,25 +98,26 @@ bool DefaultRobotHWSim::initSim( { // TODO(anyone): Deprecate HW interface specification in actuators in ROS J joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; - std::cerr << "The element of tranmission " << - transmissions[j].name_ << - " should be nested inside the element, not . " << - "The transmission will be properly loaded, but please update " << - "your robot model to remain compatible with future versions of the plugin."; + RCLCPP_ERROR(joint_limit_nh->get_logger(), "The element of tranmission " + "%s should be nested inside the element, not . " + "The transmission will be properly loaded, but please update " + "your robot model to remain compatible with future versions of the plugin.", + transmissions[j].name_.c_str()); } if (joint_interfaces.empty()) { - std::cerr << "Joint " << transmissions[j].name_ << - " of transmission " << transmissions[j].name_ << - " does not specify any hardware interface. " << - "Not adding it to the robot hardware simulation."; + RCLCPP_ERROR(joint_limit_nh->get_logger(), "Joint %s of transmission %s" + " does not specify any hardware interface. " + "Not adding it to the robot hardware simulation.", + transmissions[j].joints_[0].name_.c_str(), + transmissions[j].name_.c_str()); continue; } else if (joint_interfaces.size() > 1) { - std::cerr << "Joint " << transmissions[j].name_ << - " of transmission " << transmissions[j].name_ << - " specifies multiple hardware interfaces. " << + RCLCPP_ERROR(joint_limit_nh->get_logger(), "Joint %s of transmission %s" + " specifies multiple hardware interfaces. " "Currently the default robot hardware simulation interface only supports one. " - "Using the first entry"; - // continue; + "Using the first entry", + transmissions[j].joints_[0].name_.c_str(), + transmissions[j].name_.c_str()); } // Add data from transmission @@ -139,82 +132,52 @@ bool DefaultRobotHWSim::initSim( const std::string & hardware_interface = joint_interfaces.front(); // Debug - std::cerr << "Loading joint '" << joint_names_[j] << - "' of type '" << hardware_interface << "'" << std::endl; + RCLCPP_INFO(joint_limit_nh->get_logger(), "Loading joint '%s' of type '%s'", + joint_names_[j].c_str(), hardware_interface.c_str()); - // Create joint state interface for all joints -#if 0 // @todo - js_interface_.registerHandle( - hardware_interface::JointStateHandle( - joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); -#endif + register_joint(joint_names_[j], "effort", 0); + register_joint(joint_names_[j], "position", 0); + register_joint(joint_names_[j], "position_command", 0); + register_joint(joint_names_[j], "velocity_command", 0); + register_joint(joint_names_[j], "velocity", 0); // Decide what kind of command interface this actuator/joint has - hardware_interface::JointStateHandle joint_handle; - if (hardware_interface == "EffortJointInterface" || - hardware_interface == "hardware_interface/EffortJointInterface") + std::shared_ptr joint_handle; + if (hardware_interface == "hardware_interface/EffortJointInterface") { RCLCPP_INFO( joint_limit_nh->get_logger(), "joint %s is configured in EFFORT mode", transmissions[j].joints_[0].name_.c_str()); + // Create effort joint interface joint_control_methods_[j] = EFFORT; -#if 0 // @todo - joint_handle = hardware_interface::JointHandle( - js_interface_.getHandle(joint_names_[j]), - &joint_effort_command_[j]); - - ej_interface_.registerHandle(joint_handle); -#endif - } else { - if (hardware_interface == "PositionJointInterface" || - hardware_interface == "hardware_interface/PositionJointInterface") - { - // Create position joint interface - joint_control_methods_[j] = POSITION; - RCLCPP_INFO( - joint_limit_nh->get_logger(), "joint %s is configured in POSITION mode", - transmissions[j].joints_[0].name_.c_str()); -#if 0 // @todo - joint_handle = hardware_interface::JointHandle( - js_interface_.getHandle(joint_names_[j]), - &joint_position_command_[j]); - pj_interface_.registerHandle(joint_handle); -#endif - } else { - if (hardware_interface == "VelocityJointInterface" || - hardware_interface == "hardware_interface/VelocityJointInterface") - { - // Create velocity joint interface - joint_control_methods_[j] = VELOCITY; - RCLCPP_INFO( - joint_limit_nh->get_logger(), "joint %s is configured in VELOCITY mode", - transmissions[j].joints_[0].name_.c_str()); -#if 0 // @todo - joint_handle = hardware_interface::JointHandle( - js_interface_.getHandle(joint_names_[j]), - &joint_velocity_command_[j]); - vj_interface_.registerHandle(joint_handle); -#endif - } else { - std::cerr << "No matching hardware interface found for '" << - hardware_interface << "' while loading interfaces for " << joint_names_[j] << std::endl; - return false; - } - } - } - if (hardware_interface == "EffortJointInterface" || - hardware_interface == "PositionJointInterface" || - hardware_interface == "VelocityJointInterface") + joint_handle = std::make_shared( + joint_names_[j], "effort", &joint_effort_command_[j]); + } else if (hardware_interface == "hardware_interface/PositionJointInterface") { - std::cerr << "Deprecated syntax, please prepend 'hardware_interface/' to '" << - hardware_interface << "' within the tag in joint '" << - joint_names_[j] << "'." << std::endl; + // Create position joint interface + joint_control_methods_[j] = POSITION; + RCLCPP_INFO( + joint_limit_nh->get_logger(), "joint %s is configured in POSITION mode", + transmissions[j].joints_[0].name_.c_str()); + joint_handle = std::make_shared( + joint_names_[j], "position", &joint_position_command_[j]); + } else if (hardware_interface == "hardware_interface/VelocityJointInterface") + { + // Create velocity joint interface + joint_control_methods_[j] = VELOCITY; + RCLCPP_INFO( + joint_limit_nh->get_logger(), "joint %s is configured in VELOCITY mode", + transmissions[j].joints_[0].name_.c_str()); + joint_handle = std::make_shared( + joint_names_[j], "velocity", &joint_velocity_command_[j]); + } else { + std::cerr << "No matching hardware interface found for '" << + hardware_interface << "' while loading interfaces for " << joint_names_[j] << std::endl; + return false; } // Get the gazebo joint that corresponds to the robot joint. - // RCLCPP_DEBUG_STREAM(LOGGER, "Getting pointer to gazebo joint: " - // << joint_names_[j]); gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); if (!joint) { std::cerr << "This robot has a joint named \"" << joint_names_[j] << @@ -239,13 +202,9 @@ bool DefaultRobotHWSim::initSim( &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], &joint_effort_limits_[j], &joint_vel_limits_[j]); if (joint_control_methods_[j] != EFFORT) { - // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or - // joint->SetParam("vel") to control the joint. try { pid_controllers_.push_back( - control_toolbox::PidROS( - joint_limit_nh, - transmissions[j].joints_[0].name_)); + control_toolbox::PidROS(joint_limit_nh, transmissions[j].joints_[0].name_)); joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".p"); joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".i"); joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".d"); @@ -273,81 +232,26 @@ bool DefaultRobotHWSim::initSim( } joint->SetParam("fmax", 0, joint_effort_limits_[j]); } - joint_states_[j] = hardware_interface::JointStateHandle( - joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]); - if (register_joint_state_handle(&joint_states_[j]) != hardware_interface::return_type::OK) { - RCLCPP_WARN_ONCE(LOGGER, "Failed to register joint state handle."); - } - joint_cmds_[j] = hardware_interface::JointCommandHandle( - joint_names_[j], &joint_position_command_[j]); - if (hardware_interface == "PositionJointInterface" || - hardware_interface == "hardware_interface/PositionJointInterface") - { - if (register_joint_command_handle(&joint_cmds_[j]) != hardware_interface::return_type::OK) { - RCLCPP_WARN_ONCE(LOGGER, "Failed to register joint commands."); - } - } + joint_cmds_[j] = std::make_shared( + joint_names_[j], "position_command", &joint_position_command_[j]); + + joint_eff_cmdhandle_[j] = std::make_shared( + joint_names_[j], "effort_command", &joint_effort_command_[j]); + + joint_vel_cmdhandle_[j] = std::make_shared( + joint_names_[j], "velocity_command", &joint_velocity_command_[j]); joint_opmodehandles_[j] = hardware_interface::OperationModeHandle( joint_names_[j], &joint_opmodes_[j]); + if (register_operation_mode_handle(&joint_opmodehandles_[j]) != hardware_interface::return_type::OK) { RCLCPP_WARN_ONCE(LOGGER, "Failed to register joint_opm ode handles."); } - - joint_limits_interface::JointLimits limits; // hack, refactor registerjointhandle - limits.has_position_limits = true; - limits.min_position = joint_lower_limits_[j]; - limits.max_position = joint_upper_limits_[j]; - limits.max_velocity = joint_vel_limits_[j]; - limits.has_velocity_limits = true; - limits.max_effort = joint_effort_limits_[j]; - limits.has_effort_limits = true; - - joint_pos_limit_handles_[j] = joint_limits_interface::PositionJointSaturationHandle( - joint_states_[j], joint_cmds_[j], limits); - - joint_eff_cmdhandle_[j] = hardware_interface::JointCommandHandle( - joint_names_[j], &joint_effort_command_[j]); - // should be register_joint_effort_command_handle, but there's only 1 buffer for now - if (hardware_interface == "EffortJointInterface" || - hardware_interface == "hardware_interface/EffortJointInterface") - { - if (register_joint_command_handle(&joint_eff_cmdhandle_[j]) != - hardware_interface::return_type::OK) - { - RCLCPP_WARN_ONCE(LOGGER, "Failed to register joint effort command handle."); - } - } - - joint_eff_limit_handles_[j] = joint_limits_interface::EffortJointSaturationHandle( - joint_states_[j], joint_eff_cmdhandle_[j], limits); - - joint_vel_cmdhandle_[j] = hardware_interface::JointCommandHandle( - joint_names_[j], &joint_velocity_command_[j]); - if (hardware_interface == "VelocityJointInterface" || - hardware_interface == "hardware_interface/VelocityJointInterface") - { - if (register_joint_command_handle(&joint_vel_cmdhandle_[j]) != - hardware_interface::return_type::OK) - { - std::cerr << "Failed to register joint velocity command handle." << std::endl; - } - } - - joint_vel_limit_handles_[j] = joint_limits_interface::VelocityJointSaturationHandle( - joint_states_[j], joint_vel_cmdhandle_[j], limits); - // register not implemented } -#if 0 // @todo - // Register interfaces - registerInterface(&js_interface_); - registerInterface(&ej_interface_); - registerInterface(&pj_interface_); - registerInterface(&vj_interface_); -#endif + // Initialize the emergency stop code. e_stop_active_ = false; last_e_stop_active_ = false; @@ -384,25 +288,7 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) } else { last_e_stop_active_ = false; } -#if 0 // @todo - ej_sat_interface_.enforceLimits(period); - ej_limits_interface_.enforceLimits(period); - pj_sat_interface_.enforceLimits(period); - pj_limits_interface_.enforceLimits(period); - vj_sat_interface_.enforceLimits(period); - vj_limits_interface_.enforceLimits(period); -#else - for (auto eff_limit_handle : joint_eff_limit_handles_) { - eff_limit_handle.enforceLimits(period); - } - for (auto pos_limit_handle : joint_pos_limit_handles_) { - pos_limit_handle.enforceLimits(period); - } - for (auto vel_limit_handle : joint_vel_limit_handles_) { - vel_limit_handle.enforceLimits(period); - } -#endif for (unsigned int j = 0; j < n_dof_; j++) { switch (joint_control_methods_[j]) { case EFFORT: @@ -413,17 +299,26 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) break; case POSITION: - sim_joints_[j]->SetPosition(0, joint_position_command_[j], true); + { + std::shared_ptr joint_handle_pos = + std::make_shared(joint_names_[j], "position_command"); + get_joint_handle(*joint_handle_pos); + sim_joints_[j]->SetPosition(0, joint_handle_pos->get_value(), true); + } break; case POSITION_PID: { + std::shared_ptr joint_handle_pos = + std::make_shared(joint_names_[j], "position_command"); + get_joint_handle(*joint_handle_pos); + double error; switch (joint_types_[j]) { case urdf::Joint::REVOLUTE: angles::shortest_angular_distance_with_limits( joint_position_[j], - joint_position_command_[j], + joint_handle_pos->get_value(), joint_lower_limits_[j], joint_upper_limits_[j], error); @@ -431,10 +326,10 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) case urdf::Joint::CONTINUOUS: error = angles::shortest_angular_distance( joint_position_[j], - joint_position_command_[j]); + joint_handle_pos->get_value()); break; default: - error = joint_position_command_[j] - joint_position_[j]; + error = joint_handle_pos->get_value() - joint_position_[j]; } const double effort_limit = joint_effort_limits_[j]; @@ -447,25 +342,36 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) break; case VELOCITY: - if (physics_type_.compare("ode") == 0) { - sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]); - } else { - sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]); + { + std::shared_ptr joint_handle_vel = + std::make_shared(joint_names_[j], "velocity_command"); + get_joint_handle(*joint_handle_vel); + + if (physics_type_.compare("ode") == 0) { + sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_handle_vel->get_value()); + } else { + sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_handle_vel->get_value()); + } } break; case VELOCITY_PID: - double error; - if (e_stop_active_) { - error = -joint_velocity_[j]; - } else { - error = joint_velocity_command_[j] - joint_velocity_[j]; + { + std::shared_ptr joint_handle_vel = + std::make_shared(joint_names_[j], "velocity_command"); + get_joint_handle(*joint_handle_vel); + double error; + if (e_stop_active_) { + error = -joint_handle_vel->get_value(); + } else { + error = joint_handle_vel->get_value() - joint_velocity_[j]; + } + const double effort_limit = joint_effort_limits_[j]; + double effort = ignition::math::clamp( + pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); + sim_joints_[j]->SetForce(0, effort); } - const double effort_limit = joint_effort_limits_[j]; - double effort = ignition::math::clamp( - pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); - sim_joints_[j]->SetForce(0, effort); break; } } @@ -481,7 +387,7 @@ void DefaultRobotHWSim::eStopActive(const bool active) // Return the joint's type, lower position limit, upper position limit, and effort limit. void DefaultRobotHWSim::registerJointLimits( const std::string & joint_name, - const hardware_interface::JointStateHandle & joint_handle, + const std::shared_ptr & joint_handle, const ControlMethod ctrl_method, const rclcpp::Node::SharedPtr & joint_limit_nh, const urdf::Model * const urdf_model, diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index af535225..9f39d555 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -276,8 +276,9 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element new controller_manager::ControllerManager( impl_->robot_hw_sim_, impl_->executor_, "gazebo_controller_manager")); -#if 1 - // @todo:Coded example here. should disable when spawn functionality of controller manager is up + + // TODO(anyone): Coded example here. should disable when spawn functionality of + // controller manager is up auto load_params_from_yaml_node = [](rclcpp_lifecycle::LifecycleNode::SharedPtr lc_node, YAML::Node & yaml_node, const std::string & prefix) { @@ -313,7 +314,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } else { node->set_parameter(rclcpp::Parameter(key, val_str)); } - return; } else if (yaml_node.Type() == YAML::NodeType::Map) { for (auto yaml_node_it : yaml_node) { @@ -339,7 +339,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element node->declare_parameter(key); } node->set_parameter({rclcpp::Parameter(key, val)}); - if (key == "joints") { if (!node->has_parameter("write_op_modes")) { node->declare_parameter("write_op_modes"); @@ -366,20 +365,23 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element }; auto load_params_from_yaml = [&](rclcpp_lifecycle::LifecycleNode::SharedPtr lc_node, const std::string & yaml_config_file, const std::string & prefix) - { - if (yaml_config_file.empty()) { - throw std::runtime_error("yaml config file path is empty"); + { + if (yaml_config_file.empty()) { + throw std::runtime_error("yaml config file path is empty"); + } + YAML::Node root_node = YAML::LoadFile(yaml_config_file); + for (auto yaml : root_node) { + auto nodename = yaml.first.as(); + RCLCPP_ERROR(impl_->model_nh_->get_logger(), "nodename: %s", nodename.c_str()); + if (nodename == prefix) { + load_params_from_yaml_node(lc_node, yaml.second, prefix); } + } + }; - YAML::Node root_node = YAML::LoadFile(yaml_config_file); - for (auto yaml : root_node) { - auto nodename = yaml.first.as(); - RCLCPP_ERROR(impl_->model_nh_->get_logger(), "nodename: %s", nodename.c_str()); - if (nodename == prefix) { - load_params_from_yaml_node(lc_node, yaml.second, prefix); - } - } - }; + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {}; + std::vector stop_controllers = {}; YAML::Node root_node = YAML::LoadFile(impl_->param_file_); for (auto yaml : root_node) { @@ -397,10 +399,24 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element controller->get_lifecycle_node(), impl_->param_file_, controller_name); + if(controller_name != "joint_state_controller") { + controller->get_lifecycle_node()->configure(); + } + start_controllers.push_back(controller_name); } } } } + auto switch_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::switch_controller, impl_->controller_manager_, + start_controllers, stop_controllers, + 2, true, rclcpp::Duration(0, 0)); // STRICT_: 2 + while (std::future_status::timeout == switch_future.wait_for(std::chrono::milliseconds(100))) + { + impl_->controller_manager_->update(); + } + switch_future.get(); auto spin = [this]() { @@ -410,17 +426,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element }; impl_->thread_executor_spin_ = std::thread(spin); - if (impl_->controller_manager_->configure() != - controller_interface::return_type::SUCCESS) - { - RCLCPP_ERROR(impl_->model_nh_->get_logger(), "cm failed to configure"); - } - if (impl_->controller_manager_->activate() != - controller_interface::return_type::SUCCESS) - { - RCLCPP_ERROR(impl_->model_nh_->get_logger(), "cm failed to activate"); - } -#endif // Listen to the update event. This event is broadcast every simulation iteration. impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( From 5bfa75b82b71bfc48dff79c70e9e3148ee678a56 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 14 Oct 2020 21:12:50 +0200 Subject: [PATCH 02/12] make linters happy Signed-off-by: ahcorde --- .../src/default_robot_hw_sim.cpp | 27 +++++++++-------- .../src/gazebo_ros2_control_plugin.cpp | 29 +++++++++---------- 2 files changed, 29 insertions(+), 27 deletions(-) diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index f9a1412a..1a792e60 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -81,11 +81,13 @@ bool DefaultRobotHWSim::initSim( for (unsigned int j = 0; j < n_dof_; j++) { // Check that this transmission has one joint if (transmissions[j].joints_.empty()) { - RCLCPP_ERROR(joint_limit_nh->get_logger(), "Transmission %s has no associated joints.", + RCLCPP_ERROR( + joint_limit_nh->get_logger(), "Transmission %s has no associated joints.", transmissions[j].name_.c_str()); continue; } else if (transmissions[j].joints_.size() > 1) { - RCLCPP_ERROR(joint_limit_nh->get_logger(), "Transmission " + RCLCPP_ERROR( + joint_limit_nh->get_logger(), "Transmission " " has more than one joint. Currently the default robot hardware simulation " " interface only supports one.", transmissions[j].name_.c_str()); continue; @@ -98,21 +100,24 @@ bool DefaultRobotHWSim::initSim( { // TODO(anyone): Deprecate HW interface specification in actuators in ROS J joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; - RCLCPP_ERROR(joint_limit_nh->get_logger(), "The element of tranmission " + RCLCPP_ERROR( + joint_limit_nh->get_logger(), "The element of tranmission " "%s should be nested inside the element, not . " "The transmission will be properly loaded, but please update " "your robot model to remain compatible with future versions of the plugin.", transmissions[j].name_.c_str()); } if (joint_interfaces.empty()) { - RCLCPP_ERROR(joint_limit_nh->get_logger(), "Joint %s of transmission %s" + RCLCPP_ERROR( + joint_limit_nh->get_logger(), "Joint %s of transmission %s" " does not specify any hardware interface. " "Not adding it to the robot hardware simulation.", transmissions[j].joints_[0].name_.c_str(), transmissions[j].name_.c_str()); continue; } else if (joint_interfaces.size() > 1) { - RCLCPP_ERROR(joint_limit_nh->get_logger(), "Joint %s of transmission %s" + RCLCPP_ERROR( + joint_limit_nh->get_logger(), "Joint %s of transmission %s" " specifies multiple hardware interfaces. " "Currently the default robot hardware simulation interface only supports one. " "Using the first entry", @@ -132,7 +137,8 @@ bool DefaultRobotHWSim::initSim( const std::string & hardware_interface = joint_interfaces.front(); // Debug - RCLCPP_INFO(joint_limit_nh->get_logger(), "Loading joint '%s' of type '%s'", + RCLCPP_INFO( + joint_limit_nh->get_logger(), "Loading joint '%s' of type '%s'", joint_names_[j].c_str(), hardware_interface.c_str()); register_joint(joint_names_[j], "effort", 0); @@ -143,8 +149,7 @@ bool DefaultRobotHWSim::initSim( // Decide what kind of command interface this actuator/joint has std::shared_ptr joint_handle; - if (hardware_interface == "hardware_interface/EffortJointInterface") - { + if (hardware_interface == "hardware_interface/EffortJointInterface") { RCLCPP_INFO( joint_limit_nh->get_logger(), "joint %s is configured in EFFORT mode", transmissions[j].joints_[0].name_.c_str()); @@ -153,8 +158,7 @@ bool DefaultRobotHWSim::initSim( joint_control_methods_[j] = EFFORT; joint_handle = std::make_shared( joint_names_[j], "effort", &joint_effort_command_[j]); - } else if (hardware_interface == "hardware_interface/PositionJointInterface") - { + } else if (hardware_interface == "hardware_interface/PositionJointInterface") { // Create position joint interface joint_control_methods_[j] = POSITION; RCLCPP_INFO( @@ -162,8 +166,7 @@ bool DefaultRobotHWSim::initSim( transmissions[j].joints_[0].name_.c_str()); joint_handle = std::make_shared( joint_names_[j], "position", &joint_position_command_[j]); - } else if (hardware_interface == "hardware_interface/VelocityJointInterface") - { + } else if (hardware_interface == "hardware_interface/VelocityJointInterface") { // Create velocity joint interface joint_control_methods_[j] = VELOCITY; RCLCPP_INFO( diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 9f39d555..b874a5bf 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -365,19 +365,19 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element }; auto load_params_from_yaml = [&](rclcpp_lifecycle::LifecycleNode::SharedPtr lc_node, const std::string & yaml_config_file, const std::string & prefix) - { - if (yaml_config_file.empty()) { - throw std::runtime_error("yaml config file path is empty"); - } - YAML::Node root_node = YAML::LoadFile(yaml_config_file); - for (auto yaml : root_node) { - auto nodename = yaml.first.as(); - RCLCPP_ERROR(impl_->model_nh_->get_logger(), "nodename: %s", nodename.c_str()); - if (nodename == prefix) { - load_params_from_yaml_node(lc_node, yaml.second, prefix); + { + if (yaml_config_file.empty()) { + throw std::runtime_error("yaml config file path is empty"); } - } - }; + YAML::Node root_node = YAML::LoadFile(yaml_config_file); + for (auto yaml : root_node) { + auto nodename = yaml.first.as(); + RCLCPP_ERROR(impl_->model_nh_->get_logger(), "nodename: %s", nodename.c_str()); + if (nodename == prefix) { + load_params_from_yaml_node(lc_node, yaml.second, prefix); + } + } + }; // Start controller, will take effect at the end of the update function std::vector start_controllers = {}; @@ -399,7 +399,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element controller->get_lifecycle_node(), impl_->param_file_, controller_name); - if(controller_name != "joint_state_controller") { + if (controller_name != "joint_state_controller") { controller->get_lifecycle_node()->configure(); } start_controllers.push_back(controller_name); @@ -412,8 +412,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element &controller_manager::ControllerManager::switch_controller, impl_->controller_manager_, start_controllers, stop_controllers, 2, true, rclcpp::Duration(0, 0)); // STRICT_: 2 - while (std::future_status::timeout == switch_future.wait_for(std::chrono::milliseconds(100))) - { + while (std::future_status::timeout == switch_future.wait_for(std::chrono::milliseconds(100))) { impl_->controller_manager_->update(); } switch_future.get(); From 379a895cca45ce09b24f1ef2d7db486501e93082 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 14 Oct 2020 21:21:10 +0200 Subject: [PATCH 03/12] make linters happy Signed-off-by: ahcorde --- .../include/gazebo_ros2_control/default_robot_hw_sim.hpp | 1 + gazebo_ros2_control/src/default_robot_hw_sim.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp index f3aba2a8..efc85db9 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp @@ -35,6 +35,7 @@ #define GAZEBO_ROS2_CONTROL__DEFAULT_ROBOT_HW_SIM_HPP_ #include +#include #include #include diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index 1a792e60..c7e785a6 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -32,6 +32,7 @@ */ #include +#include #include #include From e7d7bb1275da48e0fd50164ccd671421fb4444d1 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 15 Oct 2020 15:05:18 +0200 Subject: [PATCH 04/12] Included velocity controller Signed-off-by: ahcorde --- gazebo_ros2_control_demos/CMakeLists.txt | 4 +- .../config/cartpole_controller_velocity.yaml | 17 ++ .../examples/example_velocity.cpp | 167 ++---------------- gazebo_ros2_control_demos/package.xml | 3 + .../urdf/test_cart_velocity.xacro.urdf | 2 +- 5 files changed, 41 insertions(+), 152 deletions(-) create mode 100644 gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml diff --git a/gazebo_ros2_control_demos/CMakeLists.txt b/gazebo_ros2_control_demos/CMakeLists.txt index 34bfa83f..27a49626 100644 --- a/gazebo_ros2_control_demos/CMakeLists.txt +++ b/gazebo_ros2_control_demos/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(std_msgs REQUIRED) install(DIRECTORY launch @@ -36,8 +37,7 @@ ament_target_dependencies(example_position add_executable(example_velocity examples/example_velocity.cpp) ament_target_dependencies(example_velocity rclcpp - rclcpp_action - control_msgs + std_msgs ) if(BUILD_TESTING) diff --git a/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml b/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml new file mode 100644 index 00000000..03f7ac24 --- /dev/null +++ b/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml @@ -0,0 +1,17 @@ +cart_pole_controller: + ros__parameters: + type: velocity_controllers/JointGroupVelocityController + joints: + - slider_to_cart + write_op_modes: + - slider_to_cart + state_publish_rate: 25 + action_monitor_rate: 20 + constraints: + stopped_velocity_tolerance: 0.05 + goal_time: 5 + +joint_state_controller: + ros__parameters: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/gazebo_ros2_control_demos/examples/example_velocity.cpp b/gazebo_ros2_control_demos/examples/example_velocity.cpp index 528af503..42f3e9f5 100644 --- a/gazebo_ros2_control_demos/examples/example_velocity.cpp +++ b/gazebo_ros2_control_demos/examples/example_velocity.cpp @@ -17,171 +17,40 @@ #include #include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" std::shared_ptr node; -bool common_goal_accepted = false; -rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN; -int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; - -void common_goal_response( - std::shared_future::SharedPtr> future) -{ - RCLCPP_DEBUG( - node->get_logger(), "common_goal_response time: %f", - rclcpp::Clock().now().seconds()); - auto goal_handle = future.get(); - if (!goal_handle) { - common_goal_accepted = false; - printf("Goal rejected\n"); - } else { - common_goal_accepted = true; - printf("Goal accepted\n"); - } -} - -void common_result_response( - const rclcpp_action::ClientGoalHandle - ::WrappedResult & result) -{ - printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); - common_resultcode = result.code; - common_action_result_code = result.result->error_code; - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - printf("SUCCEEDED result code\n"); - break; - case rclcpp_action::ResultCode::ABORTED: - printf("Goal was aborted\n"); - return; - case rclcpp_action::ResultCode::CANCELED: - printf("Goal was canceled\n"); - return; - default: - printf("Unknown result code\n"); - return; - } -} - -void common_feedback( - rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback) -{ - std::cout << "feedback->desired.positions :"; - for (auto & x : feedback->desired.positions) { - std::cout << x << "\t"; - } - std::cout << std::endl; - std::cout << "feedback->desired.velocities :"; - for (auto & x : feedback->desired.velocities) { - std::cout << x << "\t"; - } - std::cout << std::endl; - std::cout << std::endl; -} int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - node = std::make_shared("trajectory_test_node"); - - std::cout << "node created" << std::endl; - - rclcpp_action::Client::SharedPtr action_client; - action_client = rclcpp_action::create_client( - node->get_node_base_interface(), - node->get_node_graph_interface(), - node->get_node_logging_interface(), - node->get_node_waitables_interface(), - "/cart_pole_controller/follow_joint_trajectory"); - - bool response = - action_client->wait_for_action_server(std::chrono::seconds(1)); - if (!response) { - throw std::runtime_error("could not get action server"); - } - std::cout << "Created action server" << std::endl; - - std::vector joint_names = {"slider_to_cart"}; - - std::vector points; - trajectory_msgs::msg::JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap - point.positions.resize(joint_names.size()); - point.velocities.resize(joint_names.size()); - - point.positions[0] = 0.0; - point.velocities[0] = 0.0; - - trajectory_msgs::msg::JointTrajectoryPoint point2; - point2.time_from_start = rclcpp::Duration::from_seconds(1.0); - point2.positions.resize(joint_names.size()); - point2.velocities.resize(joint_names.size()); - point2.positions[0] = -1.0; - point2.velocities[0] = -1.0; - - trajectory_msgs::msg::JointTrajectoryPoint point3; - point3.time_from_start = rclcpp::Duration::from_seconds(2.0); - point3.positions.resize(joint_names.size()); - point3.velocities.resize(joint_names.size()); - point3.positions[0] = 1.0; - point3.velocities[0] = 1.0; - - trajectory_msgs::msg::JointTrajectoryPoint point4; - point4.time_from_start = rclcpp::Duration::from_seconds(3.0); - point4.positions.resize(joint_names.size()); - point4.velocities.resize(joint_names.size()); - point4.positions[0] = 0.0; - point4.velocities[0] = 0.0; + node = std::make_shared("velocity_test_node"); - points.push_back(point); - points.push_back(point2); - points.push_back(point3); - points.push_back(point4); + auto publisher = node->create_publisher("/commands", 10); - rclcpp_action::Client::SendGoalOptions opt; - opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1); - opt.result_callback = std::bind(common_result_response, std::placeholders::_1); - opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2); + RCLCPP_INFO(node->get_logger(), "node created"); - control_msgs::action::FollowJointTrajectory_Goal goal_msg; - goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); - goal_msg.trajectory.joint_names = joint_names; - goal_msg.trajectory.points = points; + std_msgs::msg::Float64MultiArray commands; - auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); + using namespace std::chrono_literals; - if (rclcpp::spin_until_future_complete(node, goal_handle_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); - return 1; - } - RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); + commands.data.push_back(0); + publisher->publish(commands); + std::this_thread::sleep_for(1s); - rclcpp_action::ClientGoalHandle::SharedPtr - goal_handle = goal_handle_future.get(); - if (!goal_handle) { - RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); - return 1; - } - RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server"); + commands.data[0] = 1; + publisher->publish(commands); + std::this_thread::sleep_for(1s); - // Wait for the server to be done with the goal - auto result_future = action_client->async_get_result(goal_handle); - RCLCPP_INFO(node->get_logger(), "Waiting for result"); - if (rclcpp::spin_until_future_complete(node, result_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node->get_logger(), "get result call failed :("); - return 1; - } + commands.data[0] = -1; + publisher->publish(commands); + std::this_thread::sleep_for(1s); - std::cout << "async_send_goal" << std::endl; + commands.data[0] = 0; + publisher->publish(commands); + std::this_thread::sleep_for(1s); rclcpp::shutdown(); return 0; diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index b74c7ed1..49a0e780 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -19,6 +19,7 @@ control_msgs rclcpp rclcpp_action + std_msgs ament_index_python control_msgs @@ -31,7 +32,9 @@ launch_ros robot_state_publisher rclcpp + std_msgs transmission_interface + velocity_controllers xacro ament_cmake_gtest diff --git a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index 490a1f7c..854cf6ba 100644 --- a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -61,7 +61,7 @@ gazebo_ros2_control/DefaultRobotHWSim - $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + $(find gazebo_ros2_control_demos)/config/cartpole_controller_velocity.yaml From 1925122271a7a3dd93af169eba1b8c289418e926 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 16 Oct 2020 11:42:41 +0200 Subject: [PATCH 05/12] Included joint limit Signed-off-by: ahcorde --- .../default_robot_hw_sim.hpp | 2 + .../src/default_robot_hw_sim.cpp | 86 +++++++------------ 2 files changed, 35 insertions(+), 53 deletions(-) diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp index efc85db9..c6739636 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp @@ -91,6 +91,7 @@ class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim void registerJointLimits( const std::string & joint_name, const std::shared_ptr & joint_handle, + const std::shared_ptr & joint_cmd, const ControlMethod ctrl_method, const rclcpp::Node::SharedPtr & joint_limit_nh, const urdf::Model * const urdf_model, @@ -121,6 +122,7 @@ class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim std::vector> joint_states_; std::vector> joint_cmds_; + std::vector> joint_handles_; std::vector> joint_eff_cmdhandle_; std::vector> joint_vel_cmdhandle_; std::vector joint_opmodehandles_; diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index c7e785a6..d392f332 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -149,7 +149,6 @@ bool DefaultRobotHWSim::initSim( register_joint(joint_names_[j], "velocity", 0); // Decide what kind of command interface this actuator/joint has - std::shared_ptr joint_handle; if (hardware_interface == "hardware_interface/EffortJointInterface") { RCLCPP_INFO( joint_limit_nh->get_logger(), "joint %s is configured in EFFORT mode", @@ -157,24 +156,24 @@ bool DefaultRobotHWSim::initSim( // Create effort joint interface joint_control_methods_[j] = EFFORT; - joint_handle = std::make_shared( - joint_names_[j], "effort", &joint_effort_command_[j]); + joint_handles_.push_back(std::make_shared( + joint_names_[j], "effort", &joint_effort_command_[j])); } else if (hardware_interface == "hardware_interface/PositionJointInterface") { // Create position joint interface joint_control_methods_[j] = POSITION; RCLCPP_INFO( joint_limit_nh->get_logger(), "joint %s is configured in POSITION mode", transmissions[j].joints_[0].name_.c_str()); - joint_handle = std::make_shared( - joint_names_[j], "position", &joint_position_command_[j]); + joint_handles_.push_back(std::make_shared( + joint_names_[j], "position", &joint_position_command_[j])); } else if (hardware_interface == "hardware_interface/VelocityJointInterface") { // Create velocity joint interface joint_control_methods_[j] = VELOCITY; RCLCPP_INFO( joint_limit_nh->get_logger(), "joint %s is configured in VELOCITY mode", transmissions[j].joints_[0].name_.c_str()); - joint_handle = std::make_shared( - joint_names_[j], "velocity", &joint_velocity_command_[j]); + joint_handles_.push_back(std::make_shared( + joint_names_[j], "velocity", &joint_velocity_command_[j])); } else { std::cerr << "No matching hardware interface found for '" << hardware_interface << "' while loading interfaces for " << joint_names_[j] << std::endl; @@ -200,8 +199,26 @@ bool DefaultRobotHWSim::initSim( std::cerr << "No physics type found." << std::endl; } + joint_cmds_[j] = std::make_shared( + joint_names_[j], "position_command", &joint_position_command_[j]); + + joint_eff_cmdhandle_[j] = std::make_shared( + joint_names_[j], "effort_command", &joint_effort_command_[j]); + + joint_vel_cmdhandle_[j] = std::make_shared( + joint_names_[j], "velocity_command", &joint_velocity_command_[j]); + + joint_opmodehandles_[j] = hardware_interface::OperationModeHandle( + joint_names_[j], &joint_opmodes_[j]); + + if (register_operation_mode_handle(&joint_opmodehandles_[j]) != + hardware_interface::return_type::OK) + { + RCLCPP_WARN_ONCE(LOGGER, "Failed to register joint_opm ode handles."); + } + registerJointLimits( - joint_names_[j], joint_handle, joint_control_methods_[j], + joint_names_[j], joint_handles_[j], joint_cmds_[j], joint_control_methods_[j], joint_limit_nh, urdf_model, &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], &joint_effort_limits_[j], &joint_vel_limits_[j]); @@ -236,24 +253,6 @@ bool DefaultRobotHWSim::initSim( } joint->SetParam("fmax", 0, joint_effort_limits_[j]); } - - joint_cmds_[j] = std::make_shared( - joint_names_[j], "position_command", &joint_position_command_[j]); - - joint_eff_cmdhandle_[j] = std::make_shared( - joint_names_[j], "effort_command", &joint_effort_command_[j]); - - joint_vel_cmdhandle_[j] = std::make_shared( - joint_names_[j], "velocity_command", &joint_velocity_command_[j]); - - joint_opmodehandles_[j] = hardware_interface::OperationModeHandle( - joint_names_[j], &joint_opmodes_[j]); - - if (register_operation_mode_handle(&joint_opmodehandles_[j]) != - hardware_interface::return_type::OK) - { - RCLCPP_WARN_ONCE(LOGGER, "Failed to register joint_opm ode handles."); - } } // Initialize the emergency stop code. @@ -392,6 +391,7 @@ void DefaultRobotHWSim::eStopActive(const bool active) void DefaultRobotHWSim::registerJointLimits( const std::string & joint_name, const std::shared_ptr & joint_handle, + const std::shared_ptr & joint_cmd, const ControlMethod ctrl_method, const rclcpp::Node::SharedPtr & joint_limit_nh, const urdf::Model * const urdf_model, @@ -482,29 +482,20 @@ void DefaultRobotHWSim::registerJointLimits( switch (ctrl_method) { case EFFORT: { -#if 0 // @todo const joint_limits_interface::EffortJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - ej_limits_interface_.registerHandle(limits_handle); -#endif + limits_handle(joint_handle, joint_cmd, limits, soft_limits); } break; case POSITION: { -#if 0 // @todo const joint_limits_interface::PositionJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - pj_limits_interface_.registerHandle(limits_handle); -#endif + limits_handle(joint_handle, joint_cmd, limits, soft_limits); } break; case VELOCITY: { -#if 0 // @todo - const joint_limits_interface::VelocityJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - vj_limits_interface_.registerHandle(limits_handle); -#endif + // const joint_limits_interface::VelocityJointSoftLimitsHandle + // limits_handle(joint_handle, joint_cmd, limits, soft_limits); } break; } @@ -512,31 +503,20 @@ void DefaultRobotHWSim::registerJointLimits( switch (ctrl_method) { case EFFORT: { -#if 0 const joint_limits_interface::EffortJointSaturationHandle - sat_handle(joint_handle, limits); - - ej_sat_interface_.registerHandle(sat_handle); -#endif + sat_handle(joint_handle, joint_cmd, limits); } break; case POSITION: { -#if 0 // @todo const joint_limits_interface::PositionJointSaturationHandle - sat_handle(joint_handle, limits); - - pj_sat_interface_.registerHandle(sat_handle); -#endif + sat_handle(joint_handle, joint_cmd, limits); } break; case VELOCITY: { -#if 0 // @todo const joint_limits_interface::VelocityJointSaturationHandle - sat_handle(joint_handle, limits); - vj_sat_interface_.registerHandle(sat_handle); -#endif + sat_handle(joint_handle, joint_cmd,limits); } break; } From 550bb8e342b2cba865413412716ba39464fb3d7c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 16 Oct 2020 11:52:48 +0200 Subject: [PATCH 06/12] make linters happy Signed-off-by: ahcorde --- .../src/default_robot_hw_sim.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index d392f332..1ad244bf 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -156,24 +156,27 @@ bool DefaultRobotHWSim::initSim( // Create effort joint interface joint_control_methods_[j] = EFFORT; - joint_handles_.push_back(std::make_shared( - joint_names_[j], "effort", &joint_effort_command_[j])); + joint_handles_.push_back( + std::make_shared( + joint_names_[j], "effort", &joint_effort_command_[j])); } else if (hardware_interface == "hardware_interface/PositionJointInterface") { // Create position joint interface joint_control_methods_[j] = POSITION; RCLCPP_INFO( joint_limit_nh->get_logger(), "joint %s is configured in POSITION mode", transmissions[j].joints_[0].name_.c_str()); - joint_handles_.push_back(std::make_shared( - joint_names_[j], "position", &joint_position_command_[j])); + joint_handles_.push_back( + std::make_shared( + joint_names_[j], "position", &joint_position_command_[j])); } else if (hardware_interface == "hardware_interface/VelocityJointInterface") { // Create velocity joint interface joint_control_methods_[j] = VELOCITY; RCLCPP_INFO( joint_limit_nh->get_logger(), "joint %s is configured in VELOCITY mode", transmissions[j].joints_[0].name_.c_str()); - joint_handles_.push_back(std::make_shared( - joint_names_[j], "velocity", &joint_velocity_command_[j])); + joint_handles_.push_back( + std::make_shared( + joint_names_[j], "velocity", &joint_velocity_command_[j])); } else { std::cerr << "No matching hardware interface found for '" << hardware_interface << "' while loading interfaces for " << joint_names_[j] << std::endl; @@ -516,7 +519,7 @@ void DefaultRobotHWSim::registerJointLimits( case VELOCITY: { const joint_limits_interface::VelocityJointSaturationHandle - sat_handle(joint_handle, joint_cmd,limits); + sat_handle(joint_handle, joint_cmd, limits); } break; } From a6030db928b6fcecff636769ebe9d9eca1f1fee5 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 19 Oct 2020 13:35:44 +0200 Subject: [PATCH 07/12] Included suggestions Signed-off-by: ahcorde --- .../default_robot_hw_sim.hpp | 57 ++- .../src/default_robot_hw_sim.cpp | 446 ++++++++++-------- 2 files changed, 274 insertions(+), 229 deletions(-) diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp index c6739636..c411c04a 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp @@ -56,12 +56,12 @@ #include "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" -// gazebo_ros_control -#include "gazebo_ros2_control/robot_hw_sim.hpp" - // URDF #include "urdf/model.h" +// gazebo_ros_control +#include "gazebo_ros2_control/robot_hw_sim.hpp" + namespace gazebo_ros2_control { @@ -84,23 +84,21 @@ class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim // Methods used to control a joint. enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID}; +private: + rclcpp::Node::SharedPtr nh_; + protected: // Register the limits of the joint specified by joint_name and joint_handle. The limits are // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. // Return the joint's type, lower position limit, upper position limit, and effort limit. void registerJointLimits( - const std::string & joint_name, - const std::shared_ptr & joint_handle, - const std::shared_ptr & joint_cmd, - const ControlMethod ctrl_method, - const rclcpp::Node::SharedPtr & joint_limit_nh, + size_t joint_nr, const urdf::Model * const urdf_model, int * const joint_type, double * const lower_limit, double * const upper_limit, double * const effort_limit, double * const vel_limit); unsigned int n_dof_; - std::vector joint_names_; std::vector joint_types_; std::vector joint_lower_limits_; @@ -110,24 +108,43 @@ class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim std::vector joint_control_methods_; std::vector pid_controllers_; std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_effort_command_; - std::vector joint_position_command_; + + /// \brief stores the joint positions on ESTOP activation + /// During ESTOP these positions will override the output position command value. std::vector last_joint_position_command_; - std::vector joint_velocity_command_; - std::vector joint_opmodes_; + /// \brief handles to the joints from within Gazebo std::vector sim_joints_; - std::vector> joint_states_; - std::vector> joint_cmds_; - std::vector> joint_handles_; - std::vector> joint_eff_cmdhandle_; - std::vector> joint_vel_cmdhandle_; + /// \brief The current positions of the joints + std::vector joint_pos_stateh_; + /// \brief The current velocities of the joints + std::vector joint_vel_stateh_; + /// \brief The current effort forces applied to the joints + std::vector joint_eff_stateh_; + + /// \brief The current position command value (if control mode is POSITION) of the joints + std::vector joint_pos_cmdh_; + /// \brief The current effort command value (if control mode is EFFORT) of the joints + std::vector joint_eff_cmdh_; + /// \brief The current velocity command value (if control mode is VELOCITY) of the joints + std::vector joint_vel_cmdh_; + + /// \brief The operational mode (active/inactive) state of the joints + std::vector joint_opmodes_; + + /// \brief operational mode handles of the joints pointing to values in the joint_opmodes_ + /// collection std::vector joint_opmodehandles_; + /// \brief Limits for the joints if defined in the URDF or Node parameters + /// The implementation of the joint limit will be chosen based on the URDF parameters and could be + /// one of the hard (saturation) or soft limits based on the control mode (effort, position or + /// velocity) + std::vector> joint_limit_handles_; + std::string physics_type_; + bool usingODE; // e_stop_active_ is true if the emergency stop is active. bool e_stop_active_, last_e_stop_active_; diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index 1ad244bf..3b153170 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -41,8 +41,6 @@ namespace gazebo_ros2_control { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("default_robot_hw_sim"); - bool DefaultRobotHWSim::initSim( const std::string & robot_namespace, rclcpp::Node::SharedPtr & model_nh, @@ -50,50 +48,68 @@ bool DefaultRobotHWSim::initSim( const urdf::Model * const urdf_model, std::vector transmissions) { - rclcpp::Node::SharedPtr & joint_limit_nh = model_nh; + // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each + // parameter's name is "joint_limits/". An example is "joint_limits/axle_joint". + nh_ = model_nh; // Resize vectors to our DOF n_dof_ = transmissions.size(); - joint_names_.resize(n_dof_); - joint_control_methods_.resize(n_dof_); - joint_types_.resize(n_dof_); joint_lower_limits_.resize(n_dof_); joint_upper_limits_.resize(n_dof_); joint_effort_limits_.resize(n_dof_); joint_vel_limits_.resize(n_dof_); - - joint_position_.resize(n_dof_); - joint_velocity_.resize(n_dof_); - joint_effort_.resize(n_dof_); - joint_effort_command_.resize(n_dof_); - joint_position_command_.resize(n_dof_); - joint_velocity_command_.resize(n_dof_); - - joint_cmds_.resize(n_dof_); + joint_control_methods_.resize(n_dof_); joint_opmodes_.resize(n_dof_); joint_opmodehandles_.resize(n_dof_); - joint_vel_cmdhandle_.resize(n_dof_); - joint_eff_cmdhandle_.resize(n_dof_); + joint_pos_stateh_.resize(n_dof_, hardware_interface::JointHandle("position")); + joint_vel_stateh_.resize(n_dof_, hardware_interface::JointHandle("velocity")); + joint_eff_stateh_.resize(n_dof_, hardware_interface::JointHandle("effort")); + joint_pos_cmdh_.resize(n_dof_, hardware_interface::JointHandle("position_command")); + joint_eff_cmdh_.resize(n_dof_, hardware_interface::JointHandle("effort_command")); + joint_vel_cmdh_.resize(n_dof_, hardware_interface::JointHandle("velocity_command")); + + joint_limit_handles_.resize(n_dof_); + + // get physics engine type +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics(); +#else + gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine(); +#endif + physics_type_ = physics->GetType(); + if (physics_type_.empty()) { + RCLCPP_ERROR(nh_->get_logger(), "No physics engine configured in Gazebo."); + return false; + } + + // special handing since ODE uses a different set/get interface then the other engines + usingODE = (physics_type_.compare("ode") == 0); // Initialize values for (unsigned int j = 0; j < n_dof_; j++) { + // + // Perform some validation on the URDF joint and actuator spec + // + // Check that this transmission has one joint if (transmissions[j].joints_.empty()) { - RCLCPP_ERROR( - joint_limit_nh->get_logger(), "Transmission %s has no associated joints.", - transmissions[j].name_.c_str()); + RCLCPP_WARN_STREAM( + nh_->get_logger(), "Transmission " << transmissions[j].name_ << + " has no associated joints."); continue; } else if (transmissions[j].joints_.size() > 1) { - RCLCPP_ERROR( - joint_limit_nh->get_logger(), "Transmission " - " has more than one joint. Currently the default robot hardware simulation " - " interface only supports one.", transmissions[j].name_.c_str()); + RCLCPP_WARN_STREAM( + nh_->get_logger(), "Transmission " << transmissions[j].name_ << + " has more than one joint. Currently the default robot hardware simulation " << + " interface only supports one."); continue; } + std::string joint_name = joint_names_[j] = transmissions[j].joints_[0].name_; + std::vector joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_; if (joint_interfaces.empty() && !(transmissions[j].actuators_.empty()) && @@ -101,152 +117,158 @@ bool DefaultRobotHWSim::initSim( { // TODO(anyone): Deprecate HW interface specification in actuators in ROS J joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; - RCLCPP_ERROR( - joint_limit_nh->get_logger(), "The element of tranmission " - "%s should be nested inside the element, not . " - "The transmission will be properly loaded, but please update " - "your robot model to remain compatible with future versions of the plugin.", - transmissions[j].name_.c_str()); + RCLCPP_WARN_STREAM( + nh_->get_logger(), "The element of transmission " << + transmissions[j].name_ << " should be nested inside the element," << + " not . The transmission will be properly loaded, but please update " << + "your robot model to remain compatible with future versions of the plugin."); } if (joint_interfaces.empty()) { - RCLCPP_ERROR( - joint_limit_nh->get_logger(), "Joint %s of transmission %s" - " does not specify any hardware interface. " - "Not adding it to the robot hardware simulation.", - transmissions[j].joints_[0].name_.c_str(), - transmissions[j].name_.c_str()); + RCLCPP_WARN_STREAM( + nh_->get_logger(), "Joint " << transmissions[j].name_ << + " of transmission " << transmissions[j].name_ << + " does not specify any hardware interface. " << + "Not adding it to the robot hardware simulation."); continue; } else if (joint_interfaces.size() > 1) { - RCLCPP_ERROR( - joint_limit_nh->get_logger(), "Joint %s of transmission %s" - " specifies multiple hardware interfaces. " - "Currently the default robot hardware simulation interface only supports one. " - "Using the first entry", - transmissions[j].joints_[0].name_.c_str(), - transmissions[j].name_.c_str()); + RCLCPP_WARN_STREAM( + nh_->get_logger(), "Joint " << transmissions[j].name_ << + " of transmission " << transmissions[j].name_ << + " specifies multiple hardware interfaces. " << + "Currently the default robot hardware simulation interface only supports one." << + "Using the first entry"); + // only a warning, allow joint to continue } - // Add data from transmission - joint_names_[j] = transmissions[j].joints_[0].name_; - joint_position_[j] = 1.0; - joint_velocity_[j] = 0.0; - joint_effort_[j] = 1.0; // N/m for continuous joints - joint_effort_command_[j] = 0.0; - joint_position_command_[j] = 0.0; - joint_velocity_command_[j] = 0.0; - - const std::string & hardware_interface = joint_interfaces.front(); - - // Debug - RCLCPP_INFO( - joint_limit_nh->get_logger(), "Loading joint '%s' of type '%s'", - joint_names_[j].c_str(), hardware_interface.c_str()); - - register_joint(joint_names_[j], "effort", 0); - register_joint(joint_names_[j], "position", 0); - register_joint(joint_names_[j], "position_command", 0); - register_joint(joint_names_[j], "velocity_command", 0); - register_joint(joint_names_[j], "velocity", 0); - + std::string hardware_interface = joint_interfaces.front(); // Decide what kind of command interface this actuator/joint has if (hardware_interface == "hardware_interface/EffortJointInterface") { - RCLCPP_INFO( - joint_limit_nh->get_logger(), "joint %s is configured in EFFORT mode", - transmissions[j].joints_[0].name_.c_str()); - - // Create effort joint interface joint_control_methods_[j] = EFFORT; - joint_handles_.push_back( - std::make_shared( - joint_names_[j], "effort", &joint_effort_command_[j])); } else if (hardware_interface == "hardware_interface/PositionJointInterface") { - // Create position joint interface joint_control_methods_[j] = POSITION; - RCLCPP_INFO( - joint_limit_nh->get_logger(), "joint %s is configured in POSITION mode", - transmissions[j].joints_[0].name_.c_str()); - joint_handles_.push_back( - std::make_shared( - joint_names_[j], "position", &joint_position_command_[j])); } else if (hardware_interface == "hardware_interface/VelocityJointInterface") { - // Create velocity joint interface joint_control_methods_[j] = VELOCITY; - RCLCPP_INFO( - joint_limit_nh->get_logger(), "joint %s is configured in VELOCITY mode", - transmissions[j].joints_[0].name_.c_str()); - joint_handles_.push_back( - std::make_shared( - joint_names_[j], "velocity", &joint_velocity_command_[j])); } else { - std::cerr << "No matching hardware interface found for '" << - hardware_interface << "' while loading interfaces for " << joint_names_[j] << std::endl; - return false; - } - - // Get the gazebo joint that corresponds to the robot joint. - gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); - if (!joint) { - std::cerr << "This robot has a joint named \"" << joint_names_[j] << - "\" which is not in the gazebo model." << std::endl; + RCLCPP_WARN_STREAM( + nh_->get_logger(), "No matching joint interface '" << + hardware_interface << "' for joint " << joint_name); + RCLCPP_INFO( + nh_->get_logger(), + " Expecting one of 'hardware_interface/{EffortJointInterface |" + " PositionJointInterface | VelocityJointInterface}'"); return false; } - sim_joints_.push_back(joint); - joint_position_[j] = joint->Position(0); - joint_velocity_[j] = joint->GetVelocity(0); - // get physics engine type - gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics(); + // + // Accept this URDF joint as valid and link with the gazebo joint of the same name + // - physics_type_ = physics->GetType(); - if (physics_type_.empty()) { - std::cerr << "No physics type found." << std::endl; + // I think it's safe to only skip this joint and not abort if there is no match found + gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name); + if (!simjoint) { + RCLCPP_WARN_STREAM( + nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name << + "' which is not in the gazebo model."); + continue; } + sim_joints_.push_back(simjoint); + + // Accept this joint and continue configuration + RCLCPP_INFO_STREAM( + nh_->get_logger(), "Loading joint '" << joint_name << "' of type '" << + hardware_interface << "'"); + + /// + /// \brief a helper function for registering joint state or command handles + /// + auto register_joint_interface = [this, &joint_name, &j]( + const char * interface_name, + std::vector & joint_iface_handles) + { + // set joint name and interface on our stored handle + joint_iface_handles[j] = hardware_interface::JointHandle(joint_name, interface_name); - joint_cmds_[j] = std::make_shared( - joint_names_[j], "position_command", &joint_position_command_[j]); - - joint_eff_cmdhandle_[j] = std::make_shared( - joint_names_[j], "effort_command", &joint_effort_command_[j]); - - joint_vel_cmdhandle_[j] = std::make_shared( - joint_names_[j], "velocity_command", &joint_velocity_command_[j]); + // register the joint with the underlying RobotHardware implementation + if (register_joint( + joint_name, interface_name, + 0.0) != hardware_interface::return_type::OK) + { + RCLCPP_ERROR_STREAM( + nh_->get_logger(), "cant register " << interface_name << + " state handle for joint " << joint_name); + return false; + } - joint_opmodehandles_[j] = hardware_interface::OperationModeHandle( - joint_names_[j], &joint_opmodes_[j]); + // now retrieve the handle with the bound value reference (bound directly to the + // DynamicJointState msg in RobotHardware) + if (get_joint_handle(joint_iface_handles[j]) != hardware_interface::return_type::OK) { + RCLCPP_ERROR_STREAM( + nh_->get_logger(), "state handle " << interface_name << " failure for joint " << + joint_name); + return false; + } - if (register_operation_mode_handle(&joint_opmodehandles_[j]) != - hardware_interface::return_type::OK) + // verify handle references a target value + if (!joint_iface_handles[j]) { + RCLCPP_ERROR_STREAM( + nh_->get_logger(), interface_name << " handle for joint " << joint_name << + " is null"); + return false; + } + return true; + }; + + // register the state handles + if ( + !register_joint_interface("position", joint_pos_stateh_) || + !register_joint_interface("velocity", joint_vel_stateh_) || + !register_joint_interface("effort", joint_eff_stateh_)) { - RCLCPP_WARN_ONCE(LOGGER, "Failed to register joint_opm ode handles."); + RCLCPP_ERROR_STREAM( + nh_->get_logger(), + "plugin aborting due to previous " << joint_name << " joint registration errors"); + return false; } + // Register the command handle + switch (joint_control_methods_[j]) { + case EFFORT: register_joint_interface("effort_command", joint_eff_cmdh_); break; + case POSITION: register_joint_interface("position_command", joint_pos_cmdh_); break; + case VELOCITY: register_joint_interface("velocity_command", joint_vel_cmdh_); break; + } + + // get the current state of the sim joint to initialize our ROS control joints + // @note(guru-florida): perhaps we dont need this if readSim() is called after init anyway + readSim(rclcpp::Time(), rclcpp::Duration(0, 0)); + + // setup joint limits registerJointLimits( - joint_names_[j], joint_handles_[j], joint_cmds_[j], joint_control_methods_[j], - joint_limit_nh, urdf_model, + j, + urdf_model, &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], &joint_effort_limits_[j], &joint_vel_limits_[j]); if (joint_control_methods_[j] != EFFORT) { try { pid_controllers_.push_back( - control_toolbox::PidROS(joint_limit_nh, transmissions[j].joints_[0].name_)); - joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".p"); - joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".i"); - joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".d"); - joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".i_clamp_max"); - joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".i_clamp_min"); - joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".antiwindup"); + control_toolbox::PidROS(nh_, transmissions[j].joints_[0].name_)); + nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".p"); + nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".i"); + nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".d"); + nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".i_clamp_max"); + nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".i_clamp_min"); + nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".antiwindup"); if (pid_controllers_[j].initPid()) { switch (joint_control_methods_[j]) { case POSITION: joint_control_methods_[j] = POSITION_PID; RCLCPP_INFO( - joint_limit_nh->get_logger(), "joint %s is configured in POSITION_PID mode", + nh_->get_logger(), "joint %s is configured in POSITION_PID mode", transmissions[j].joints_[0].name_.c_str()); break; case VELOCITY: joint_control_methods_[j] = VELOCITY_PID; RCLCPP_INFO( - joint_limit_nh->get_logger(), "joint %s is configured in VELOCITY_PID mode", + nh_->get_logger(), "joint %s is configured in VELOCITY_PID mode", transmissions[j].joints_[0].name_.c_str()); break; } @@ -254,7 +276,17 @@ bool DefaultRobotHWSim::initSim( } catch (const std::exception & e) { RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "%s", e.what()); } - joint->SetParam("fmax", 0, joint_effort_limits_[j]); + simjoint->SetParam("fmax", 0, joint_effort_limits_[j]); + } + + // set joints operation mode to ACTIVE and register handle for controlling opmode + joint_opmodes_[j] = hardware_interface::OperationMode::ACTIVE; + joint_opmodehandles_[j] = hardware_interface::OperationModeHandle( + joint_name, &joint_opmodes_[j]); + if (register_operation_mode_handle(&joint_opmodehandles_[j]) != + hardware_interface::return_type::OK) + { + RCLCPP_WARN_STREAM(nh_->get_logger(), "cant register opmode handle for joint" << joint_name); } } @@ -265,20 +297,22 @@ bool DefaultRobotHWSim::initSim( return true; } -void DefaultRobotHWSim::readSim(rclcpp::Time time, rclcpp::Duration period) +void DefaultRobotHWSim::readSim(rclcpp::Time, rclcpp::Duration) { for (unsigned int j = 0; j < n_dof_; j++) { // Gazebo has an interesting API... double position = sim_joints_[j]->Position(0); + if (joint_types_[j] == urdf::Joint::PRISMATIC) { - joint_position_[j] = position; + joint_pos_stateh_[j].set_value(position); } else { - joint_position_[j] += angles::shortest_angular_distance( - joint_position_[j], - position); + double prev_position = joint_pos_stateh_[j].get_value(); + joint_pos_stateh_[j].set_value( + prev_position + + angles::shortest_angular_distance(prev_position, position)); } - joint_velocity_[j] = sim_joints_[j]->GetVelocity(0); - joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0)); + joint_vel_stateh_[j].set_value(sim_joints_[j]->GetVelocity(0)); + joint_eff_stateh_[j].set_value(sim_joints_[j]->GetForce((unsigned int)(0))); } } @@ -287,97 +321,89 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) // If the E-stop is active, joints controlled by position commands will maintain their positions. if (e_stop_active_) { if (!last_e_stop_active_) { - last_joint_position_command_ = joint_position_; + last_joint_position_command_.clear(); + std::transform( + joint_pos_stateh_.begin(), joint_pos_stateh_.end(), + std::back_inserter(last_joint_position_command_), + [](const hardware_interface::JointHandle & ph) {return ph.get_value();}); last_e_stop_active_ = true; } - joint_position_command_ = last_joint_position_command_; + for (int i = 0; i < n_dof_; i++) { + joint_pos_cmdh_[i].set_value(last_joint_position_command_[i]); + } } else { last_e_stop_active_ = false; } - + for (auto & limit_handle : joint_limit_handles_) { + if (limit_handle) { + limit_handle->enforce_limits(period); + } + } for (unsigned int j = 0; j < n_dof_; j++) { switch (joint_control_methods_[j]) { case EFFORT: { - const double effort = e_stop_active_ ? 0 : joint_effort_command_[j]; + const double effort = e_stop_active_ ? 0 : joint_eff_cmdh_[j].get_value(); sim_joints_[j]->SetForce(0, effort); } break; case POSITION: - { - std::shared_ptr joint_handle_pos = - std::make_shared(joint_names_[j], "position_command"); - get_joint_handle(*joint_handle_pos); - sim_joints_[j]->SetPosition(0, joint_handle_pos->get_value(), true); - } + sim_joints_[j]->SetPosition(0, joint_pos_cmdh_[j].get_value(), true); break; case POSITION_PID: { - std::shared_ptr joint_handle_pos = - std::make_shared(joint_names_[j], "position_command"); - get_joint_handle(*joint_handle_pos); - double error; switch (joint_types_[j]) { case urdf::Joint::REVOLUTE: angles::shortest_angular_distance_with_limits( - joint_position_[j], - joint_handle_pos->get_value(), + joint_pos_stateh_[j].get_value(), + joint_pos_cmdh_[j].get_value(), joint_lower_limits_[j], joint_upper_limits_[j], error); break; case urdf::Joint::CONTINUOUS: error = angles::shortest_angular_distance( - joint_position_[j], - joint_handle_pos->get_value()); + joint_pos_stateh_[j].get_value(), + joint_pos_cmdh_[j].get_value()); break; default: - error = joint_handle_pos->get_value() - joint_position_[j]; + error = joint_pos_cmdh_[j].get_value() - joint_pos_stateh_[j].get_value(); } const double effort_limit = joint_effort_limits_[j]; - double effort = ignition::math::clamp( + + const double effort = ignition::math::clamp( pid_controllers_[j].computeCommand(error, period), -effort_limit, effort_limit); + sim_joints_[j]->SetForce(0, effort); sim_joints_[j]->SetParam("friction", 0, 0.0); } break; case VELOCITY: - { - std::shared_ptr joint_handle_vel = - std::make_shared(joint_names_[j], "velocity_command"); - get_joint_handle(*joint_handle_vel); - - if (physics_type_.compare("ode") == 0) { - sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_handle_vel->get_value()); - } else { - sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_handle_vel->get_value()); - } + if (usingODE) { + sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_vel_cmdh_[j].get_value()); + } else { + sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_vel_cmdh_[j].get_value()); } break; case VELOCITY_PID: - { - std::shared_ptr joint_handle_vel = - std::make_shared(joint_names_[j], "velocity_command"); - get_joint_handle(*joint_handle_vel); - double error; - if (e_stop_active_) { - error = -joint_handle_vel->get_value(); - } else { - error = joint_handle_vel->get_value() - joint_velocity_[j]; - } - const double effort_limit = joint_effort_limits_[j]; - double effort = ignition::math::clamp( - pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); - sim_joints_[j]->SetForce(0, effort); + double error; + if (e_stop_active_) { + error = -joint_vel_stateh_[j].get_value(); + } else { + error = joint_vel_cmdh_[j].get_value() - joint_vel_stateh_[j].get_value(); } + const double effort_limit = joint_effort_limits_[j]; + const double effort = ignition::math::clamp( + pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); + sim_joints_[j]->SetForce(0, effort); break; } } @@ -392,16 +418,15 @@ void DefaultRobotHWSim::eStopActive(const bool active) // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. // Return the joint's type, lower position limit, upper position limit, and effort limit. void DefaultRobotHWSim::registerJointLimits( - const std::string & joint_name, - const std::shared_ptr & joint_handle, - const std::shared_ptr & joint_cmd, - const ControlMethod ctrl_method, - const rclcpp::Node::SharedPtr & joint_limit_nh, + size_t joint_nr, const urdf::Model * const urdf_model, int * const joint_type, double * const lower_limit, double * const upper_limit, double * const effort_limit, double * const vel_limit) { + const std::string & joint_name = joint_names_[joint_nr]; + const ControlMethod ctrl_method = joint_control_methods_[joint_nr]; + *joint_type = urdf::Joint::UNKNOWN; *lower_limit = -std::numeric_limits::max(); *upper_limit = std::numeric_limits::max(); @@ -418,17 +443,16 @@ void DefaultRobotHWSim::registerJointLimits( if (urdf_joint != NULL) { *joint_type = urdf_joint->type; // Get limits from the URDF file. - if (joint_limits_interface::getJointLimits(urdf_joint->name, joint_limit_nh, limits)) { + if (joint_limits_interface::getJointLimits(urdf_joint->name, nh_, limits)) { has_limits = true; } - if (joint_limits_interface::getSoftJointLimits( - urdf_joint->name, joint_limit_nh, - soft_limits)) - { + if (joint_limits_interface::getSoftJointLimits(urdf_joint->name, nh_, soft_limits)) { has_soft_limits = true; } // @note (ddeng): these joint limits arent input into the node, so fetch them from the urdf + // TODO(guru-florida): These are now a part of the node as of 2020-09-03, + // so what one do we take? limits.min_position = urdf_joint->limits->lower; limits.max_position = urdf_joint->limits->upper; limits.has_position_limits = true; @@ -444,14 +468,11 @@ void DefaultRobotHWSim::registerJointLimits( *effort_limit = limits.max_effort; *vel_limit = limits.max_velocity; - // urdf_joint->safety->k_position; has_limits = true; } } - // Get limits from the parameter server. - // @note: no longer using parameter servers - if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits)) { + if (joint_limits_interface::getJointLimits(joint_name, nh_, limits)) { has_limits = true; } @@ -485,20 +506,24 @@ void DefaultRobotHWSim::registerJointLimits( switch (ctrl_method) { case EFFORT: { - const joint_limits_interface::EffortJointSoftLimitsHandle - limits_handle(joint_handle, joint_cmd, limits, soft_limits); + joint_limit_handles_[joint_nr] = + std::make_unique( + joint_pos_cmdh_[joint_nr], joint_vel_cmdh_[joint_nr], joint_eff_cmdh_[joint_nr], + limits); } break; case POSITION: { - const joint_limits_interface::PositionJointSoftLimitsHandle - limits_handle(joint_handle, joint_cmd, limits, soft_limits); + joint_limit_handles_[joint_nr] = + std::make_unique( + joint_pos_stateh_[joint_nr], joint_pos_cmdh_[joint_nr], limits, soft_limits); } break; case VELOCITY: { - // const joint_limits_interface::VelocityJointSoftLimitsHandle - // limits_handle(joint_handle, joint_cmd, limits, soft_limits); + joint_limit_handles_[joint_nr] = + std::make_unique( + joint_vel_stateh_[joint_nr], joint_vel_cmdh_[joint_nr], limits); } break; } @@ -506,26 +531,29 @@ void DefaultRobotHWSim::registerJointLimits( switch (ctrl_method) { case EFFORT: { - const joint_limits_interface::EffortJointSaturationHandle - sat_handle(joint_handle, joint_cmd, limits); + joint_limit_handles_[joint_nr] = + std::make_unique( + joint_pos_cmdh_[joint_nr], joint_vel_cmdh_[joint_nr], joint_eff_cmdh_[joint_nr], + limits); } break; case POSITION: { - const joint_limits_interface::PositionJointSaturationHandle - sat_handle(joint_handle, joint_cmd, limits); + joint_limit_handles_[joint_nr] = + std::make_unique( + joint_pos_stateh_[joint_nr], joint_pos_cmdh_[joint_nr], limits); } break; case VELOCITY: { - const joint_limits_interface::VelocityJointSaturationHandle - sat_handle(joint_handle, joint_cmd, limits); + joint_limit_handles_[joint_nr] = + std::make_unique( + joint_vel_stateh_[joint_nr], joint_vel_cmdh_[joint_nr], limits); } break; } } } - } // namespace gazebo_ros2_control PLUGINLIB_EXPORT_CLASS(gazebo_ros2_control::DefaultRobotHWSim, gazebo_ros2_control::RobotHWSim) From 4c7e633ee81a3ac1e13efece8117484c9641a5ca Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 20 Oct 2020 10:28:31 +0200 Subject: [PATCH 08/12] readSim outside the joint loop Signed-off-by: ahcorde --- gazebo_ros2_control/src/default_robot_hw_sim.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index 3b153170..cc1c051d 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -237,10 +237,6 @@ bool DefaultRobotHWSim::initSim( case VELOCITY: register_joint_interface("velocity_command", joint_vel_cmdh_); break; } - // get the current state of the sim joint to initialize our ROS control joints - // @note(guru-florida): perhaps we dont need this if readSim() is called after init anyway - readSim(rclcpp::Time(), rclcpp::Duration(0, 0)); - // setup joint limits registerJointLimits( j, @@ -290,6 +286,10 @@ bool DefaultRobotHWSim::initSim( } } + // get the current state of the sim joint to initialize our ROS control joints + // @note(guru-florida): perhaps we dont need this if readSim() is called after init anyway + readSim(rclcpp::Time(), rclcpp::Duration(0, 0)); + // Initialize the emergency stop code. e_stop_active_ = false; last_e_stop_active_ = false; From 05db9aa017d42d2cb1caccd2e7914b193be06e08 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 22 Oct 2020 11:03:03 +0200 Subject: [PATCH 09/12] Fixed joint handlers Signed-off-by: ahcorde --- .../src/default_robot_hw_sim.cpp | 35 ++++++++++++------- 1 file changed, 23 insertions(+), 12 deletions(-) diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index cc1c051d..25cfb913 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -299,20 +299,31 @@ bool DefaultRobotHWSim::initSim( void DefaultRobotHWSim::readSim(rclcpp::Time, rclcpp::Duration) { - for (unsigned int j = 0; j < n_dof_; j++) { - // Gazebo has an interesting API... - double position = sim_joints_[j]->Position(0); + for (unsigned int j = 0; j < joint_names_.size(); j++) { + auto joint_handle = std::make_shared( + joint_names_[j], "position"); + if (get_joint_handle(*joint_handle) == hardware_interface::return_type::OK) { + double position = sim_joints_[j]->Position(0); + if (joint_types_[j] == urdf::Joint::PRISMATIC) { + joint_handle->set_value(position); + } else { + double prev_position = joint_handle->get_value(); + joint_handle->set_value( + prev_position + + angles::shortest_angular_distance(prev_position, position)); + } + } + joint_handle = std::make_shared( + joint_names_[j], "velocity"); + if (get_joint_handle(*joint_handle) == hardware_interface::return_type::OK) { + joint_handle->set_value(sim_joints_[j]->GetVelocity(0)); + } - if (joint_types_[j] == urdf::Joint::PRISMATIC) { - joint_pos_stateh_[j].set_value(position); - } else { - double prev_position = joint_pos_stateh_[j].get_value(); - joint_pos_stateh_[j].set_value( - prev_position + - angles::shortest_angular_distance(prev_position, position)); + joint_handle = std::make_shared( + joint_names_[j], "effort"); + if (get_joint_handle(*joint_handle) == hardware_interface::return_type::OK) { + joint_handle->set_value(sim_joints_[j]->GetForce((unsigned int)(0))); } - joint_vel_stateh_[j].set_value(sim_joints_[j]->GetVelocity(0)); - joint_eff_stateh_[j].set_value(sim_joints_[j]->GetForce((unsigned int)(0))); } } From 77a2b17e0ec32e973d1657ebc3b5bd4bdac81d62 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Tue, 27 Oct 2020 04:09:09 -0400 Subject: [PATCH 10/12] Ahcorde/update/ros2 control (#38) * removed plugin's parseTransmissionsFromURDF since it just calls a 1-liner and returns true always, thus not handling errors. plugin now calls transmission parser directly and catches error exception * moved declaration of parameters to precede joint PID constructor as the PID constructor's ParameterInterface will cause parameter events to fire (and occasionally cause a SEGV). Also added defaults to force floating type on parameter to fix log errors due to wrong parameter type * updated transmission info member names to sync with ros2_controls master * split initSim() joint loop to first register all joint handles, then handles are fetched, then a second loop continues joint limit and PID setup * uncrustify and cpplint fixes * fix for CI, disabled ddengster branch checkout and overwrite since master now contains TransmissionInfo --- .github/workflows/ci.yaml | 6 +- .../default_robot_hw_sim.hpp | 17 ++- .../src/default_robot_hw_sim.cpp | 109 ++++++++++++++---- .../src/gazebo_ros2_control_plugin.cpp | 21 ++-- 4 files changed, 108 insertions(+), 45 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 07ffd2c6..404c710a 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -28,9 +28,9 @@ jobs: cd /home/ros2_ws/src/ git clone https://github.com/ros-controls/ros2_control git clone https://github.com/ros-controls/ros2_controllers - git clone https://github.com/ddengster/ros2_control/ -b coffeebot_deps ros2_control_ddengster - touch ros2_control_ddengster/COLCON_IGNORE - cp -r ros2_control_ddengster/transmission_interface ros2_control + # git clone https://github.com/ddengster/ros2_control/ -b coffeebot_deps ros2_control_ddengster + # touch ros2_control_ddengster/COLCON_IGNORE + # cp -r ros2_control_ddengster/transmission_interface ros2_control rosdep update rosdep install --from-paths ./ -i -y --rosdistro foxy \ --ignore-src diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp index c411c04a..a1023cac 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp @@ -88,9 +88,10 @@ class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim rclcpp::Node::SharedPtr nh_; protected: - // Register the limits of the joint specified by joint_name and joint_handle. The limits are - // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. - // Return the joint's type, lower position limit, upper position limit, and effort limit. + /// \brief Register the limits of the joint specified by joint_name and joint_handle. + /// The limits are retrieved from joint_limit_nh. If urdf_model is not NULL, limits + /// are retrieved from it also. Return the joint's type, lower position limit, upper + /// position limit, and effort limit. void registerJointLimits( size_t joint_nr, const urdf::Model * const urdf_model, @@ -98,6 +99,16 @@ class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim double * const upper_limit, double * const effort_limit, double * const vel_limit); + /// \brief Refreshes all valid handle references in a collection. + /// Requests from the RobotHardware updated handle references. This is required after + /// any call to RobotHardware::register_joint() and before a handle is used or bound + /// to a controller since the internal implementation of register_joint binds to doubles + /// inside a std::vector and thus growth of that vector invalidates all existing + /// iterators (i.e. handles). + /// See https://github.com/ros-controls/ros2_control/issues/212 + /// If a handle in the collection is unset (no joint name or interface name) then it is skipped. + void bindJointHandles(std::vector & joint_iface_handles); + unsigned int n_dof_; std::vector joint_names_; std::vector joint_types_; diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index 25cfb913..d2363837 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -95,45 +95,45 @@ bool DefaultRobotHWSim::initSim( // // Check that this transmission has one joint - if (transmissions[j].joints_.empty()) { + if (transmissions[j].joints.empty()) { RCLCPP_WARN_STREAM( - nh_->get_logger(), "Transmission " << transmissions[j].name_ << + nh_->get_logger(), "Transmission " << transmissions[j].name << " has no associated joints."); continue; - } else if (transmissions[j].joints_.size() > 1) { + } else if (transmissions[j].joints.size() > 1) { RCLCPP_WARN_STREAM( - nh_->get_logger(), "Transmission " << transmissions[j].name_ << + nh_->get_logger(), "Transmission " << transmissions[j].name << " has more than one joint. Currently the default robot hardware simulation " << " interface only supports one."); continue; } - std::string joint_name = joint_names_[j] = transmissions[j].joints_[0].name_; + std::string joint_name = joint_names_[j] = transmissions[j].joints[0].name; - std::vector joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_; + std::vector joint_interfaces = transmissions[j].joints[0].interfaces; if (joint_interfaces.empty() && - !(transmissions[j].actuators_.empty()) && - !(transmissions[j].actuators_[0].hardware_interfaces_.empty())) + !(transmissions[j].actuators.empty()) && + !(transmissions[j].actuators[0].interfaces.empty())) { // TODO(anyone): Deprecate HW interface specification in actuators in ROS J - joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; + joint_interfaces = transmissions[j].actuators[0].interfaces; RCLCPP_WARN_STREAM( nh_->get_logger(), "The element of transmission " << - transmissions[j].name_ << " should be nested inside the element," << + transmissions[j].name << " should be nested inside the element," << " not . The transmission will be properly loaded, but please update " << "your robot model to remain compatible with future versions of the plugin."); } if (joint_interfaces.empty()) { RCLCPP_WARN_STREAM( - nh_->get_logger(), "Joint " << transmissions[j].name_ << - " of transmission " << transmissions[j].name_ << + nh_->get_logger(), "Joint " << transmissions[j].name << + " of transmission " << transmissions[j].name << " does not specify any hardware interface. " << "Not adding it to the robot hardware simulation."); continue; } else if (joint_interfaces.size() > 1) { RCLCPP_WARN_STREAM( - nh_->get_logger(), "Joint " << transmissions[j].name_ << - " of transmission " << transmissions[j].name_ << + nh_->get_logger(), "Joint " << transmissions[j].name << + " of transmission " << transmissions[j].name << " specifies multiple hardware interfaces. " << "Currently the default robot hardware simulation interface only supports one." << "Using the first entry"); @@ -237,6 +237,36 @@ bool DefaultRobotHWSim::initSim( case VELOCITY: register_joint_interface("velocity_command", joint_vel_cmdh_); break; } + // set joints operation mode to ACTIVE and register handle for controlling opmode + joint_opmodes_[j] = hardware_interface::OperationMode::ACTIVE; + joint_opmodehandles_[j] = hardware_interface::OperationModeHandle( + joint_name, &joint_opmodes_[j]); + if (register_operation_mode_handle(&joint_opmodehandles_[j]) != + hardware_interface::return_type::OK) + { + RCLCPP_WARN_STREAM(nh_->get_logger(), "cant register opmode handle for joint" << joint_name); + } + } + + // since handles references may have changed due to underlying DynamicJointState msg + // vectors resizing and reallocating we need to get these handles again + // any handles not registered are skipped, such as the command handles if they arent + // involved in the control method + bindJointHandles(joint_pos_stateh_); + bindJointHandles(joint_vel_stateh_); + bindJointHandles(joint_eff_stateh_); + bindJointHandles(joint_pos_cmdh_); + bindJointHandles(joint_vel_cmdh_); + bindJointHandles(joint_eff_cmdh_); + + + // + // Complete initialization of limits, PID controllers, etc now that registered handles are bound + // + for (size_t j = 0; j < joint_names_.size(); j++) { + auto simjoint = sim_joints_[j]; + assert(simjoint); + // setup joint limits registerJointLimits( j, @@ -245,27 +275,27 @@ bool DefaultRobotHWSim::initSim( &joint_effort_limits_[j], &joint_vel_limits_[j]); if (joint_control_methods_[j] != EFFORT) { try { + nh_->declare_parameter(transmissions[j].joints[0].name + ".p", 25.0); + nh_->declare_parameter(transmissions[j].joints[0].name + ".i", 10.0); + nh_->declare_parameter(transmissions[j].joints[0].name + ".d", 5.0); + nh_->declare_parameter(transmissions[j].joints[0].name + ".i_clamp_max", 3.0); + nh_->declare_parameter(transmissions[j].joints[0].name + ".i_clamp_min", 3.0); + nh_->declare_parameter(transmissions[j].joints[0].name + ".antiwindup", false); pid_controllers_.push_back( - control_toolbox::PidROS(nh_, transmissions[j].joints_[0].name_)); - nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".p"); - nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".i"); - nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".d"); - nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".i_clamp_max"); - nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".i_clamp_min"); - nh_->declare_parameter(transmissions[j].joints_[0].name_ + ".antiwindup"); + control_toolbox::PidROS(nh_, transmissions[j].joints[0].name)); if (pid_controllers_[j].initPid()) { switch (joint_control_methods_[j]) { case POSITION: joint_control_methods_[j] = POSITION_PID; RCLCPP_INFO( nh_->get_logger(), "joint %s is configured in POSITION_PID mode", - transmissions[j].joints_[0].name_.c_str()); + transmissions[j].joints[0].name.c_str()); break; case VELOCITY: joint_control_methods_[j] = VELOCITY_PID; RCLCPP_INFO( nh_->get_logger(), "joint %s is configured in VELOCITY_PID mode", - transmissions[j].joints_[0].name_.c_str()); + transmissions[j].joints[0].name.c_str()); break; } } @@ -278,11 +308,13 @@ bool DefaultRobotHWSim::initSim( // set joints operation mode to ACTIVE and register handle for controlling opmode joint_opmodes_[j] = hardware_interface::OperationMode::ACTIVE; joint_opmodehandles_[j] = hardware_interface::OperationModeHandle( - joint_name, &joint_opmodes_[j]); + joint_names_[j], &joint_opmodes_[j]); if (register_operation_mode_handle(&joint_opmodehandles_[j]) != hardware_interface::return_type::OK) { - RCLCPP_WARN_STREAM(nh_->get_logger(), "cant register opmode handle for joint" << joint_name); + RCLCPP_WARN_STREAM( + nh_->get_logger(), + "cant register opmode handle for joint" << joint_names_[j]); } } @@ -565,6 +597,33 @@ void DefaultRobotHWSim::registerJointLimits( } } } + +void DefaultRobotHWSim::bindJointHandles( + std::vector & joint_iface_handles) +{ + for (auto & jh : joint_iface_handles) { + // some handles, especially command handles, may not be registered so skip these + if (jh.get_name().empty() || jh.get_interface_name().empty()) { + continue; + } + + // now retrieve the handle with the bound value reference (bound directly to the + // DynamicJointState msg in RobotHardware) + if (get_joint_handle(jh) != hardware_interface::return_type::OK) { + RCLCPP_ERROR_STREAM( + nh_->get_logger(), "state handle " << jh.get_interface_name() << " failure for joint " << + jh.get_name()); + continue; + } + + // verify handle references a target value + if (!jh) { + RCLCPP_ERROR_STREAM( + nh_->get_logger(), jh.get_interface_name() << " handle for joint " << jh.get_name() << + " is null"); + } + } +} } // namespace gazebo_ros2_control PLUGINLIB_EXPORT_CLASS(gazebo_ros2_control::DefaultRobotHWSim, gazebo_ros2_control::RobotHWSim) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index b874a5bf..1c27e8b9 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -60,9 +60,6 @@ class GazeboRosControlPrivate // Get the URDF XML from the parameter server std::string getURDF(std::string param_name) const; - // Get Transmissions from the URDF - bool parseTransmissionsFromURDF(const std::string & urdf_string); - void eStopCB(const std::shared_ptr e_stop_active); // Node Handles @@ -236,11 +233,14 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element // Read urdf from ros parameter server then // setup actuators and mechanism control node. // This call will block if ROS is not properly initialized. - const std::string urdf_string = impl_->getURDF(impl_->robot_description_); - if (!impl_->parseTransmissionsFromURDF(urdf_string)) { - RCLCPP_ERROR( + std::string urdf_string; + try { + urdf_string = impl_->getURDF(impl_->robot_description_); + impl_->transmissions_ = transmission_interface::parse_transmissions_from_urdf(urdf_string); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM( impl_->model_nh_->get_logger(), - "Error parsing URDF in gazebo_ros2_control plugin, plugin not active.\n"); + "Error parsing URDF in gazebo_ros2_control plugin, plugin not active : " << ex.what()); return; } @@ -547,13 +547,6 @@ std::string GazeboRosControlPrivate::getURDF(std::string param_name) const return urdf_string; } -// Get Transmissions from the URDF -bool GazeboRosControlPrivate::parseTransmissionsFromURDF(const std::string & urdf_string) -{ - transmission_interface::TransmissionParser::parse(urdf_string, transmissions_); - return true; -} - // Emergency stop callback void GazeboRosControlPrivate::eStopCB(const std::shared_ptr e_stop_active) { From 7ea40c22f58211c759918aa4e01f82e92241e4cf Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 27 Oct 2020 09:10:39 +0100 Subject: [PATCH 11/12] Clean up CI and Dockerfile Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 3 --- Docker/Dockerfile | 3 --- 2 files changed, 6 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 404c710a..feb46c4e 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -28,9 +28,6 @@ jobs: cd /home/ros2_ws/src/ git clone https://github.com/ros-controls/ros2_control git clone https://github.com/ros-controls/ros2_controllers - # git clone https://github.com/ddengster/ros2_control/ -b coffeebot_deps ros2_control_ddengster - # touch ros2_control_ddengster/COLCON_IGNORE - # cp -r ros2_control_ddengster/transmission_interface ros2_control rosdep update rosdep install --from-paths ./ -i -y --rosdistro foxy \ --ignore-src diff --git a/Docker/Dockerfile b/Docker/Dockerfile index 2feddc93..2b079289 100644 --- a/Docker/Dockerfile +++ b/Docker/Dockerfile @@ -17,9 +17,6 @@ RUN mkdir -p /home/ros2_ws/src \ && git clone https://github.com/ros-simulation/gazebo_ros2_control \ && git clone https://github.com/ros-controls/ros2_control \ && git clone https://github.com/ros-controls/ros2_controllers \ - && git clone https://github.com/ddengster/ros2_control/ -b coffeebot_deps ros2_control_ddengster \ - && touch ros2_control_ddengster/COLCON_IGNORE \ - && cp -r ros2_control_ddengster/transmission_interface ros2_control \ && rosdep update \ && rosdep install --from-paths ./ -i -y --rosdistro foxy \ --ignore-src From cae6c8c143ebf36c3c627d0603e7de489bc68d9c Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Tue, 3 Nov 2020 05:06:21 -0500 Subject: [PATCH 12/12] Fixed velocity controller example (#40) * removed plugin's parseTransmissionsFromURDF since it just calls a 1-liner and returns true always, thus not handling errors. plugin now calls transmission parser directly and catches error exception * moved declaration of parameters to precede joint PID constructor as the PID constructor's ParameterInterface will cause parameter events to fire (and occasionally cause a SEGV). Also added defaults to force floating type on parameter to fix log errors due to wrong parameter type * updated transmission info member names to sync with ros2_controls master * split initSim() joint loop to first register all joint handles, then handles are fetched, then a second loop continues joint limit and PID setup * uncrustify and cpplint fixes * fix for CI, disabled ddengster branch checkout and overwrite since master now contains TransmissionInfo * removed duplicate registering of opmode handle * fixed velocity example due to declare_parameter default values pushing velocity controller into PID mode --- .../src/default_robot_hw_sim.cpp | 61 +++++++++---------- 1 file changed, 28 insertions(+), 33 deletions(-) diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index d2363837..32c76367 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -275,28 +275,35 @@ bool DefaultRobotHWSim::initSim( &joint_effort_limits_[j], &joint_vel_limits_[j]); if (joint_control_methods_[j] != EFFORT) { try { - nh_->declare_parameter(transmissions[j].joints[0].name + ".p", 25.0); - nh_->declare_parameter(transmissions[j].joints[0].name + ".i", 10.0); - nh_->declare_parameter(transmissions[j].joints[0].name + ".d", 5.0); - nh_->declare_parameter(transmissions[j].joints[0].name + ".i_clamp_max", 3.0); - nh_->declare_parameter(transmissions[j].joints[0].name + ".i_clamp_min", 3.0); + nh_->declare_parameter(transmissions[j].joints[0].name + ".p"); + nh_->declare_parameter(transmissions[j].joints[0].name + ".i"); + nh_->declare_parameter(transmissions[j].joints[0].name + ".d"); + nh_->declare_parameter(transmissions[j].joints[0].name + ".i_clamp_max"); + nh_->declare_parameter(transmissions[j].joints[0].name + ".i_clamp_min"); nh_->declare_parameter(transmissions[j].joints[0].name + ".antiwindup", false); - pid_controllers_.push_back( - control_toolbox::PidROS(nh_, transmissions[j].joints[0].name)); - if (pid_controllers_[j].initPid()) { - switch (joint_control_methods_[j]) { - case POSITION: - joint_control_methods_[j] = POSITION_PID; - RCLCPP_INFO( - nh_->get_logger(), "joint %s is configured in POSITION_PID mode", - transmissions[j].joints[0].name.c_str()); - break; - case VELOCITY: - joint_control_methods_[j] = VELOCITY_PID; - RCLCPP_INFO( - nh_->get_logger(), "joint %s is configured in VELOCITY_PID mode", - transmissions[j].joints[0].name.c_str()); - break; + + if (nh_->get_parameter(transmissions[j].joints[0].name + ".p").get_type() == + rclcpp::PARAMETER_DOUBLE && + nh_->get_parameter(transmissions[j].joints[0].name + ".i").get_type() == + rclcpp::PARAMETER_DOUBLE && + nh_->get_parameter(transmissions[j].joints[0].name + ".d").get_type() == + rclcpp::PARAMETER_DOUBLE) + { + pid_controllers_.push_back( + control_toolbox::PidROS(nh_, transmissions[j].joints[0].name)); + if (pid_controllers_[j].initPid()) { + switch (joint_control_methods_[j]) { + case POSITION: joint_control_methods_[j] = POSITION_PID; + RCLCPP_INFO( + nh_->get_logger(), "joint %s is configured in POSITION_PID mode", + transmissions[j].joints[0].name.c_str()); + break; + case VELOCITY: joint_control_methods_[j] = VELOCITY_PID; + RCLCPP_INFO( + nh_->get_logger(), "joint %s is configured in VELOCITY_PID mode", + transmissions[j].joints[0].name.c_str()); + break; + } } } } catch (const std::exception & e) { @@ -304,18 +311,6 @@ bool DefaultRobotHWSim::initSim( } simjoint->SetParam("fmax", 0, joint_effort_limits_[j]); } - - // set joints operation mode to ACTIVE and register handle for controlling opmode - joint_opmodes_[j] = hardware_interface::OperationMode::ACTIVE; - joint_opmodehandles_[j] = hardware_interface::OperationModeHandle( - joint_names_[j], &joint_opmodes_[j]); - if (register_operation_mode_handle(&joint_opmodehandles_[j]) != - hardware_interface::return_type::OK) - { - RCLCPP_WARN_STREAM( - nh_->get_logger(), - "cant register opmode handle for joint" << joint_names_[j]); - } } // get the current state of the sim joint to initialize our ROS control joints