From 1bb2bc797ecf4929aca6532278d1dad391ec22c9 Mon Sep 17 00:00:00 2001 From: Tom Queen Date: Fri, 13 May 2022 09:52:54 +0100 Subject: [PATCH 1/3] F improve realtime publisher fast pid divisor (#213) * Copy realtime publisher to sr_core * Change priority of the realtime publisher thread to be equal to maximum - 10 * Adding buffers. Not compiling yet. * sh_joint_effort_controller is now using the new circular buffer * New joint_state_contrller that uses the circular buffers and the new realtime_publisher * remove publish rate divider for data recording * adding faster rt pubs to position controlers * setting deadband average from 50 to 1 * adding divisor Co-authored-by: Toni Oliver Co-authored-by: carebare47 Co-authored-by: Brice --- sr_mechanism_controllers/CMakeLists.txt | 1 + .../controller_plugins.xml | 3 + .../joint_state_controller.h | 104 +++++++ .../sr_controller.hpp | 9 +- .../src/joint_state_controller.cpp | 205 ++++++++++++ .../src/srh_joint_effort_controller.cpp | 88 +++--- .../src/srh_joint_position_controller.cpp | 148 +++++---- ...joint_variable_pid_position_controller.cpp | 2 +- .../src/srh_joint_velocity_controller.cpp | 2 +- .../include/sr_utilities/realtime_publisher.h | 292 ++++++++++++++++++ 10 files changed, 751 insertions(+), 103 deletions(-) create mode 100644 sr_mechanism_controllers/include/sr_mechanism_controllers/joint_state_controller.h create mode 100644 sr_mechanism_controllers/src/joint_state_controller.cpp create mode 100644 sr_utilities/include/sr_utilities/realtime_publisher.h diff --git a/sr_mechanism_controllers/CMakeLists.txt b/sr_mechanism_controllers/CMakeLists.txt index cbaefb03d..53557852b 100644 --- a/sr_mechanism_controllers/CMakeLists.txt +++ b/sr_mechanism_controllers/CMakeLists.txt @@ -49,6 +49,7 @@ add_library(sr_mechanism_controllers example/srh_syntouch_controllers.cpp src/sr_controller.cpp src/sr_plain_pid.cpp + src/joint_state_controller.cpp ) add_dependencies(sr_mechanism_controllers ${catkin_EXPORTED_TARGETS}) target_link_libraries(sr_mechanism_controllers ${catkin_LIBRARIES}) diff --git a/sr_mechanism_controllers/controller_plugins.xml b/sr_mechanism_controllers/controller_plugins.xml index c88a99492..6751e123e 100644 --- a/sr_mechanism_controllers/controller_plugins.xml +++ b/sr_mechanism_controllers/controller_plugins.xml @@ -26,4 +26,7 @@ + diff --git a/sr_mechanism_controllers/include/sr_mechanism_controllers/joint_state_controller.h b/sr_mechanism_controllers/include/sr_mechanism_controllers/joint_state_controller.h new file mode 100644 index 000000000..73f4c9b81 --- /dev/null +++ b/sr_mechanism_controllers/include/sr_mechanism_controllers/joint_state_controller.h @@ -0,0 +1,104 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, hiDOF INC. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/* + * Author: Wim Meeussen + */ + +#pragma once + + +#include +#include +#include +#include +#include +#include +#include + +namespace controller +{ + +/** + * \brief Controller that publishes the state of all joints in a robot. + * + * This controller publishes the state of all resources registered to a \c hardware_interface::JointStateInterface to a + * topic of type \c sensor_msgs/JointState. The following is a basic configuration of the controller. + * + * \code + * joint_state_controller: + * type: joint_state_controller/JointStateController + * publish_rate: 50 + * \endcode + * + * It's possible to optionally specify a set of extra joints not contained in a + * \c hardware_interface::JointStateInterface with custom (and static) default values. The following is an example + * configuration specifying extra joints. + * + * \code + * joint_state_controller: + * type: joint_state_controller/JointStateController + * publish_rate: 50 + * extra_joints: + * - name: 'extra1' + * position: 10.0 + * velocity: 20.0 + * effort: 30.0 + * + * - name: 'extra2' + * position: -10.0 + * + * - name: 'extra3' + * \endcode + * + * An unspecified position, velocity or acceleration defaults to zero. + */ +class JointStateController: public controller_interface::Controller +{ +public: + JointStateController() : publish_rate_(0.0) {} + + virtual bool init(hardware_interface::JointStateInterface* hw, + ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh); + virtual void starting(const ros::Time& time); + virtual void update(const ros::Time& time, const ros::Duration& /*period*/); + virtual void stopping(const ros::Time& /*time*/); + +private: + std::vector joint_state_; + std::shared_ptr > realtime_pub_; + boost::scoped_ptr > msg_buffer_; + sensor_msgs::JointState msg_; + ros::Time last_publish_time_; + double publish_rate_; + unsigned int num_hw_joints_; ///< Number of joints present in the JointStateInterface, excluding extra joints + + void addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg); +}; + +} \ No newline at end of file diff --git a/sr_mechanism_controllers/include/sr_mechanism_controllers/sr_controller.hpp b/sr_mechanism_controllers/include/sr_mechanism_controllers/sr_controller.hpp index 1877c4b09..79d5e6967 100644 --- a/sr_mechanism_controllers/include/sr_mechanism_controllers/sr_controller.hpp +++ b/sr_mechanism_controllers/include/sr_mechanism_controllers/sr_controller.hpp @@ -28,13 +28,14 @@ #define _SRH_CONTROLLER_HPP_ #include +#include #include #include #include #include #include -#include +#include #include #include #include @@ -88,6 +89,7 @@ class SrController : bool has_j2; /**< true if this is a joint 0. */ double command_; /**< Last commanded position. */ + double last_commanded_effort, last_error_position; protected: // true if this is joint 0 @@ -145,8 +147,11 @@ class SrController : ros::NodeHandle node_, n_tilde_; std::string joint_name_; - boost::scoped_ptr > controller_state_publisher_; + + boost::scoped_ptr > msg_buffer_; + control_msgs::JointControllerState msg_; boost::scoped_ptr friction_compensator; diff --git a/sr_mechanism_controllers/src/joint_state_controller.cpp b/sr_mechanism_controllers/src/joint_state_controller.cpp new file mode 100644 index 000000000..7853a3f81 --- /dev/null +++ b/sr_mechanism_controllers/src/joint_state_controller.cpp @@ -0,0 +1,205 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, hiDOF INC. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF Inc nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/* + * Author: Wim Meeussen + */ + +#include +#include +#include + +#include "sr_mechanism_controllers/joint_state_controller.h" + +namespace controller +{ + + bool JointStateController::init(hardware_interface::JointStateInterface* hw, + ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) + { + // List of joints to be published + std::vector joint_names; + + // Get list of joints: This allows specifying a desired order, or + // alternatively, only publish states for a subset of joints. If the + // parameter is not set, all joint states will be published in the order + // specified by the hardware interface. + if (controller_nh.getParam("joints", joint_names)) { + ROS_INFO_STREAM("Joints parameter specified, publishing specified joints in desired order."); + } else { + // get all joint names from the hardware interface + joint_names = hw->getNames(); + } + + num_hw_joints_ = joint_names.size(); + for (unsigned i=0; i(root_nh, "joint_states", queue_size)); + msg_buffer_.reset(new boost::circular_buffer(queue_size)); + + // get joints and allocate message + for (unsigned i=0; igetHandle(joint_names[i])); + msg_.name.push_back(joint_names[i]); + msg_.position.push_back(0.0); + msg_.velocity.push_back(0.0); + msg_.effort.push_back(0.0); + } + addExtraJoints(controller_nh, msg_); + + return true; + } + + void JointStateController::starting(const ros::Time& time) + { + // initialize time + last_publish_time_ = time; + } + + void JointStateController::update(const ros::Time& time, const ros::Duration& /*period*/) + { + // limit rate of publishing + if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0/publish_rate_) < time){ + + // populate joint state message: + // - fill only joints that are present in the JointStateInterface, i.e. indices [0, num_hw_joints_) + // - leave unchanged extra joints, which have static values, i.e. indices from num_hw_joints_ onwards + msg_.header.stamp = time; + for (unsigned i=0; ipush_back(msg_); + + // try to publish + if (realtime_pub_->trylock()){ + // we're actually publishing, so increment time + last_publish_time_ = last_publish_time_ + ros::Duration(1.0/publish_rate_); + + while(!msg_buffer_->empty()) + { + realtime_pub_->msg_buffer_->push_back(msg_buffer_->front()); + msg_buffer_->pop_front(); + } + + realtime_pub_->unlockAndPublish(); + } + } + } + + void JointStateController::stopping(const ros::Time& /*time*/) + {} + + void JointStateController::addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg) + { + + // Preconditions + XmlRpc::XmlRpcValue list; + if (!nh.getParam("extra_joints", list)) + { + ROS_DEBUG("No extra joints specification found."); + return; + } + + if (list.getType() != XmlRpc::XmlRpcValue::TypeArray) + { + ROS_ERROR("Extra joints specification is not an array. Ignoring."); + return; + } + + for(std::size_t i = 0; i < list.size(); ++i) + { + XmlRpc::XmlRpcValue& elem = list[i]; + + if (elem.getType() != XmlRpc::XmlRpcValue::TypeStruct) + { + ROS_ERROR_STREAM("Extra joint specification is not a struct, but rather '" << elem.getType() << + "'. Ignoring."); + continue; + } + + if (!elem.hasMember("name")) + { + ROS_ERROR_STREAM("Extra joint does not specify name. Ignoring."); + continue; + } + + const std::string name = elem["name"]; + if (std::find(msg.name.begin(), msg.name.end(), name) != msg.name.end()) + { + ROS_WARN_STREAM("Joint state interface already contains specified extra joint '" << name << "'."); + continue; + } + + const bool has_pos = elem.hasMember("position"); + const bool has_vel = elem.hasMember("velocity"); + const bool has_eff = elem.hasMember("effort"); + + const XmlRpc::XmlRpcValue::Type typeDouble = XmlRpc::XmlRpcValue::TypeDouble; + if (has_pos && elem["position"].getType() != typeDouble) + { + ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default position. Ignoring."); + continue; + } + if (has_vel && elem["velocity"].getType() != typeDouble) + { + ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default velocity. Ignoring."); + continue; + } + if (has_eff && elem["effort"].getType() != typeDouble) + { + ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default effort. Ignoring."); + continue; + } + + // State of extra joint + const double pos = has_pos ? static_cast(elem["position"]) : 0.0; + const double vel = has_vel ? static_cast(elem["velocity"]) : 0.0; + const double eff = has_eff ? static_cast(elem["effort"]) : 0.0; + + // Add extra joints to message + msg.name.push_back(name); + msg.position.push_back(pos); + msg.velocity.push_back(vel); + msg.effort.push_back(eff); + } + } + +} + +PLUGINLIB_EXPORT_CLASS( controller::JointStateController, controller_interface::ControllerBase) \ No newline at end of file diff --git a/sr_mechanism_controllers/src/srh_joint_effort_controller.cpp b/sr_mechanism_controllers/src/srh_joint_effort_controller.cpp index 29d84bbee..6503762ad 100644 --- a/sr_mechanism_controllers/src/srh_joint_effort_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_effort_controller.cpp @@ -70,8 +70,10 @@ namespace controller return false; } - controller_state_publisher_.reset(new realtime_tools::RealtimePublisher - (node_, "state", 1)); + int queue_size = 50; + controller_state_publisher_.reset(new sr_utilities::RealtimePublisher + (node_, "state", queue_size)); + msg_buffer_.reset(new boost::circular_buffer(queue_size)); ROS_DEBUG(" --------- "); ROS_DEBUG_STREAM("Init: " << joint_name_); @@ -167,51 +169,67 @@ namespace controller initialized_ = true; command_ = 0.0; } + int divisor = 1; + double commanded_effort = 0.0; + if (loop_count_ % divisor == 0) + { + // The commanded effort is the error directly: + // the PID loop for the force controller is running on the + // motorboard. + commanded_effort = command_; // clamp_command(command_) // do not use urdf effort limits; - // The commanded effort is the error directly: - // the PID loop for the force controller is running on the - // motorboard. - double commanded_effort = command_; // clamp_command(command_) // do not use urdf effort limits; - - // Clamps the effort - commanded_effort = min(commanded_effort, (max_force_demand * max_force_factor_)); - commanded_effort = max(commanded_effort, -(max_force_demand * max_force_factor_)); + // Clamps the effort + commanded_effort = min(commanded_effort, (max_force_demand * max_force_factor_)); + commanded_effort = max(commanded_effort, -(max_force_demand * max_force_factor_)); - // Friction compensation - if (has_j2) - { - commanded_effort += friction_compensator->friction_compensation( - joint_state_->position_ + joint_state_2->position_, joint_state_->velocity_ + joint_state_2->velocity_, - static_cast(commanded_effort), friction_deadband); + // Friction compensation + if (has_j2) + { + commanded_effort += friction_compensator->friction_compensation( + joint_state_->position_ + joint_state_2->position_, joint_state_->velocity_ + joint_state_2->velocity_, + static_cast(commanded_effort), friction_deadband); + } + else + { + commanded_effort += friction_compensator->friction_compensation(joint_state_->position_, joint_state_->velocity_, + static_cast(commanded_effort), + friction_deadband); + } + last_commanded_effort = commanded_effort; } else { - commanded_effort += friction_compensator->friction_compensation(joint_state_->position_, joint_state_->velocity_, - static_cast(commanded_effort), - friction_deadband); + commanded_effort = last_commanded_effort; } joint_state_->commanded_effort_ = commanded_effort; - if (loop_count_ % 10 == 0) + if (loop_count_ % divisor == 0) { + msg_.header.stamp = time; + msg_.set_point = command_; + msg_.process_value = joint_state_->effort_; + // @todo compute the derivative of the effort. + msg_.process_value_dot = -1.0; + msg_.error = commanded_effort - joint_state_->effort_; + msg_.time_step = period.toSec(); + msg_.command = commanded_effort; + + double dummy; + getGains(msg_.p, + msg_.i, + msg_.d, + msg_.i_clamp, + dummy); + msg_buffer_->push_back(msg_); + if (controller_state_publisher_ && controller_state_publisher_->trylock()) { - controller_state_publisher_->msg_.header.stamp = time; - controller_state_publisher_->msg_.set_point = command_; - controller_state_publisher_->msg_.process_value = joint_state_->effort_; - // @todo compute the derivative of the effort. - controller_state_publisher_->msg_.process_value_dot = -1.0; - controller_state_publisher_->msg_.error = commanded_effort - joint_state_->effort_; - controller_state_publisher_->msg_.time_step = period.toSec(); - controller_state_publisher_->msg_.command = commanded_effort; - - double dummy; - getGains(controller_state_publisher_->msg_.p, - controller_state_publisher_->msg_.i, - controller_state_publisher_->msg_.d, - controller_state_publisher_->msg_.i_clamp, - dummy); + while(!msg_buffer_->empty()) + { + controller_state_publisher_->msg_buffer_->push_back(msg_buffer_->front()); + msg_buffer_->pop_front(); + } controller_state_publisher_->unlockAndPublish(); } } diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp index c33d651e6..3c03ce3c7 100644 --- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp @@ -80,8 +80,10 @@ namespace controller return false; } - controller_state_publisher_.reset(new realtime_tools::RealtimePublisher - (node_, "state", 1)); + int queue_size = 50; + controller_state_publisher_.reset(new sr_utilities::RealtimePublisher + (node_, "state", queue_size)); + msg_buffer_.reset(new boost::circular_buffer(queue_size)); ROS_DEBUG(" --------- "); ROS_DEBUG_STREAM("Init: " << joint_name_); @@ -200,91 +202,109 @@ namespace controller resetJointState(); initialized_ = true; } - if (has_j2) - { - command_ = joint_state_->commanded_position_ + joint_state_2->commanded_position_; - } - else - { - command_ = joint_state_->commanded_position_; - } - command_ = clamp_command(command_); - // Compute position demand from position error: double error_position = 0.0; double commanded_effort = 0.0; - - if (has_j2) - { - error_position = (joint_state_->position_ + joint_state_2->position_) - command_; - } - else - { - error_position = joint_state_->position_ - command_; - } - - bool in_deadband = hysteresis_deadband.is_in_deadband(command_, error_position, position_deadband); - - // don't compute the error if we're in the deadband. - if (in_deadband) + + int divisor = 1; + if (loop_count_ % divisor == 0) { - error_position = 0.0; - } - - commanded_effort = pid_controller_position_->computeCommand(-error_position, period); + if (has_j2) + { + command_ = joint_state_->commanded_position_ + joint_state_2->commanded_position_; + } + else + { + command_ = joint_state_->commanded_position_; + } + command_ = clamp_command(command_); - // clamp the result to max force - commanded_effort = min(commanded_effort, (max_force_demand * max_force_factor_)); - commanded_effort = max(commanded_effort, -(max_force_demand * max_force_factor_)); - if (!in_deadband) - { if (has_j2) { - commanded_effort += friction_compensator->friction_compensation( - joint_state_->position_ + joint_state_2->position_, - joint_state_->velocity_ + joint_state_2->velocity_, - static_cast(commanded_effort), - friction_deadband); + error_position = (joint_state_->position_ + joint_state_2->position_) - command_; } else { - commanded_effort += friction_compensator->friction_compensation(joint_state_->position_, - joint_state_->velocity_, - static_cast(commanded_effort), - friction_deadband); + error_position = joint_state_->position_ - command_; } - } - joint_state_->commanded_effort_ = commanded_effort; + // setting nb_errors_for_avg to 1: + bool in_deadband = hysteresis_deadband.is_in_deadband(command_, error_position, position_deadband, 5.0, 1); - if (loop_count_ % 10 == 0) - { - if (controller_state_publisher_ && controller_state_publisher_->trylock()) + // don't compute the error if we're in the deadband. + if (in_deadband) + { + error_position = 0.0; + } + + commanded_effort = pid_controller_position_->computeCommand(-error_position, period * divisor); + + // clamp the result to max force + commanded_effort = min(commanded_effort, (max_force_demand * max_force_factor_)); + commanded_effort = max(commanded_effort, -(max_force_demand * max_force_factor_)); + + if (!in_deadband) { - controller_state_publisher_->msg_.header.stamp = time; - controller_state_publisher_->msg_.set_point = command_; if (has_j2) { - controller_state_publisher_->msg_.process_value = joint_state_->position_ + joint_state_2->position_; - controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_ + joint_state_2->velocity_; + commanded_effort += friction_compensator->friction_compensation( + joint_state_->position_ + joint_state_2->position_, + joint_state_->velocity_ + joint_state_2->velocity_, + static_cast(commanded_effort), + friction_deadband); } else { - controller_state_publisher_->msg_.process_value = joint_state_->position_; - controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_; + commanded_effort += friction_compensator->friction_compensation(joint_state_->position_, + joint_state_->velocity_, + static_cast(commanded_effort), + friction_deadband); } + } + last_commanded_effort = commanded_effort; + last_error_position = error_position; + } + else + { + commanded_effort = last_commanded_effort; + error_position = last_error_position; + } - controller_state_publisher_->msg_.error = error_position; - controller_state_publisher_->msg_.time_step = period.toSec(); - controller_state_publisher_->msg_.command = commanded_effort; + joint_state_->commanded_effort_ = commanded_effort; - double dummy; - getGains(controller_state_publisher_->msg_.p, - controller_state_publisher_->msg_.i, - controller_state_publisher_->msg_.d, - controller_state_publisher_->msg_.i_clamp, - dummy); + if (loop_count_ % divisor == 0) + { + msg_.header.stamp = time; + msg_.set_point = command_; + if (has_j2) + { + msg_.process_value = joint_state_->position_ + joint_state_2->position_; + msg_.process_value_dot = joint_state_->velocity_ + joint_state_2->velocity_; + } + else + { + msg_.process_value = joint_state_->position_; + msg_.process_value_dot = joint_state_->velocity_; + } + msg_.error = error_position; + msg_.time_step = period.toSec(); + msg_.command = commanded_effort; + + double dummy; + getGains(msg_.p, + msg_.i, + msg_.d, + msg_.i_clamp, + dummy); + msg_buffer_->push_back(msg_); + if (controller_state_publisher_ && controller_state_publisher_->trylock()) + { + while(!msg_buffer_->empty()) + { + controller_state_publisher_->msg_buffer_->push_back(msg_buffer_->front()); + msg_buffer_->pop_front(); + } controller_state_publisher_->unlockAndPublish(); } } diff --git a/sr_mechanism_controllers/src/srh_joint_variable_pid_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_variable_pid_position_controller.cpp index 84610467b..4caa4ca9a 100644 --- a/sr_mechanism_controllers/src/srh_joint_variable_pid_position_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_variable_pid_position_controller.cpp @@ -93,7 +93,7 @@ namespace controller double dummy; pid_controller_position_->getGains(p_init_, i_init_, d_init_, i_clamp_, dummy); - controller_state_publisher_.reset(new realtime_tools::RealtimePublisher + controller_state_publisher_.reset(new sr_utilities::RealtimePublisher (node_, "state", 1)); ROS_DEBUG(" --------- "); diff --git a/sr_mechanism_controllers/src/srh_joint_velocity_controller.cpp b/sr_mechanism_controllers/src/srh_joint_velocity_controller.cpp index 3d3e95a9f..cc357606b 100644 --- a/sr_mechanism_controllers/src/srh_joint_velocity_controller.cpp +++ b/sr_mechanism_controllers/src/srh_joint_velocity_controller.cpp @@ -80,7 +80,7 @@ namespace controller return false; } - controller_state_publisher_.reset(new realtime_tools::RealtimePublisher + controller_state_publisher_.reset(new sr_utilities::RealtimePublisher (node_, "state", 1)); ROS_DEBUG(" --------- "); diff --git a/sr_utilities/include/sr_utilities/realtime_publisher.h b/sr_utilities/include/sr_utilities/realtime_publisher.h new file mode 100644 index 000000000..34a776ebe --- /dev/null +++ b/sr_utilities/include/sr_utilities/realtime_publisher.h @@ -0,0 +1,292 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + /* +* Copyright 2021 Shadow Robot Company Ltd. +* +* This program is free software: you can redistribute it and/or modify it +* under the terms of the GNU General Public License as published by the Free +* Software Foundation version 2 of the License. +* +* This program is distributed in the hope that it will be useful, but WITHOUT +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for +* more details. +* +* You should have received a copy of the GNU General Public License along +* with this program. If not, see . +*/ + +/* + * Publishing ROS messages is difficult, as the publish function is + * not realtime safe. This class provides the proper locking so that + * you can call publish in realtime and a separate (non-realtime) + * thread will ensure that the message gets published over ROS. + * + * Author: Stuart Glaser + */ +#ifndef SR_UTILITIES__REALTIME_PUBLISHER_H_ +#define SR_UTILITIES__REALTIME_PUBLISHER_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sr_utilities { + +template +class RealtimePublisher +{ + +public: + // TODO delete this field when all controllers have been adapted + Msg msg_; + /// The msg_buffer_ contains the msgs that will be inserted in the outgoing_msg_buffer_ + boost::scoped_ptr > msg_buffer_; + + /** \brief Constructor for the realtime publisher + * + * \param node the nodehandle that specifies the namespace (or prefix) that is used to advertise the ROS topic + * \param topic the topic name to advertise + * \param queue_size the size of the outgoing ROS buffer + * \param latched . optional argument (defaults to false) to specify is publisher is latched or not + */ + RealtimePublisher(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false) + : topic_(topic), node_(node), is_running_(false), keep_running_(false), turn_(LOOP_NOT_STARTED) + { + construct(queue_size, latched); + } + + RealtimePublisher() + : is_running_(false), keep_running_(false), turn_(LOOP_NOT_STARTED) + { + } + + /// Destructor + ~RealtimePublisher() + { + stop(); + while (is_running()) + { + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + + if (thread_.joinable()) + { + thread_.join(); + } + publisher_.shutdown(); + } + + void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false) + { + topic_ = topic; + node_ = node; + construct(queue_size, latched); + } + + /// Stop the realtime publisher from sending out more ROS messages + void stop() + { + keep_running_ = false; +#ifdef NON_POLLING + updated_cond_.notify_one(); // So the publishing loop can exit +#endif + } + + /** \brief Try to get the data lock from realtime + * + * To publish data from the realtime loop, you need to run trylock to + * attempt to get unique access to the msg_ variable. Trylock returns + * true if the lock was aquired, and false if it failed to get the lock. + */ + bool trylock() + { + if (msg_mutex_.try_lock()) + { + if (turn_ == REALTIME) + { + return true; + } + else + { + msg_mutex_.unlock(); + return false; + } + } + else + { + return false; + } + } + + /** \brief Unlock the msg_ variable + * + * After a successful trylock and after the data is written to the mgs_ + * variable, the lock has to be released for the message to get + * published on the specified topic. + */ + void unlockAndPublish() + { + turn_ = NON_REALTIME; + msg_mutex_.unlock(); +#ifdef NON_POLLING + updated_cond_.notify_one(); +#endif + } + + /** \brief Get the data lock form non-realtime + * + * To publish data from the realtime loop, you need to run trylock to + * attempt to get unique access to the msg_ variable. Trylock returns + * true if the lock was aquired, and false if it failed to get the lock. + */ + void lock() + { +#ifdef NON_POLLING + msg_mutex_.lock(); +#else + // never actually block on the lock + while (!msg_mutex_.try_lock()) + { + std::this_thread::sleep_for(std::chrono::microseconds(200)); + } +#endif + } + + /** \brief Unlocks the data without publishing anything + * + */ + void unlock() + { + msg_mutex_.unlock(); + } + +private: + // non-copyable + RealtimePublisher(const RealtimePublisher &) = delete; + RealtimePublisher & operator=(const RealtimePublisher &) = delete; + + void construct(int queue_size, bool latched=false) + { + publisher_ = node_.advertise(topic_, queue_size, latched); + msg_buffer_.reset(new boost::circular_buffer(queue_size)); + outgoing_msg_buffer_.reset(new boost::circular_buffer(queue_size)); + keep_running_ = true; + thread_ = std::thread(&RealtimePublisher::publishingLoop, this); + } + + + bool is_running() const { return is_running_; } + + void publishingLoop() + { + // Set to realtime scheduler for this thread + struct sched_param thread_param; + int policy = SCHED_FIFO; + thread_param.sched_priority = sched_get_priority_max(policy) - 10; + pthread_setschedparam(pthread_self(), policy, &thread_param); + + is_running_ = true; + turn_ = REALTIME; + + while (keep_running_) + { + Msg outgoing; + + // Locks msg_ and copies it + lock(); + while (turn_ != NON_REALTIME && keep_running_) + { +#ifdef NON_POLLING + updated_cond_.wait(lock); +#else + unlock(); + std::this_thread::sleep_for(std::chrono::microseconds(500)); + lock(); +#endif + } + + while(!msg_buffer_->empty()) + { + outgoing_msg_buffer_->push_back(msg_buffer_->front()); + msg_buffer_->pop_front(); + } + // outgoing = msg_; + turn_ = REALTIME; + + unlock(); + + // Sends the outgoing message + if (keep_running_) + { + while(!outgoing_msg_buffer_->empty()) + { + publisher_.publish(outgoing_msg_buffer_->front()); + outgoing_msg_buffer_->pop_front(); + } + } + // publisher_.publish(outgoing); + } + is_running_ = false; + } + + std::string topic_; + ros::NodeHandle node_; + ros::Publisher publisher_; + std::atomic is_running_; + std::atomic keep_running_; + + std::thread thread_; + + std::mutex msg_mutex_; // Protects msg_ + +#ifdef NON_POLLING + std::condition_variable updated_cond_; +#endif + + enum {REALTIME, NON_REALTIME, LOOP_NOT_STARTED}; + std::atomic turn_; // Who's turn is it to use msg_? + /// The msg_buffer_ contains the data that will get published on the ROS topic. + boost::scoped_ptr > outgoing_msg_buffer_; +}; + +template +using RealtimePublisherSharedPtr = std::shared_ptr >; + +} + +#endif \ No newline at end of file From b74cf8908a86d390f25a7a5a42c5ef7661335bf6 Mon Sep 17 00:00:00 2001 From: Tom Queen Date: Fri, 13 May 2022 09:54:47 +0100 Subject: [PATCH 2/3] F fix j0 pos vel filter (#214) * Fixed timeout in python test (#169) and reduced CPU load while waiting (#170) * Fix#171 handfinder ns (#173) * Fixing hand finders to support namespace * Additional fix of the same namespace problem * B#171 handfinder ns (#172) * Fixing sr_utilities tests to handle namespaced tests * Added namespaced test to sr_utilities * Fixing J0 (J1 and J2) position and velocity filter Co-authored-by: GuiHome Co-authored-by: Toni Oliver --- .../sr_hardware_interface/sr_actuator.hpp | 2 + .../src/joint_0_transmission.cpp | 14 +- sr_utilities/scripts/joint_0_publisher.py | 2 +- .../scripts/sr_utilities/hand_finder.py | 21 ++- sr_utilities/src/sr_hand_finder.cpp | 6 +- sr_utilities/test/test_hand_finder.cpp | 61 ++++---- sr_utilities/test/test_hand_finder.py | 132 +++++++++--------- sr_utilities/test/test_hand_finder.test | 8 +- 8 files changed, 131 insertions(+), 115 deletions(-) diff --git a/sr_hardware_interface/include/sr_hardware_interface/sr_actuator.hpp b/sr_hardware_interface/include/sr_hardware_interface/sr_actuator.hpp index ffed53b69..10f0b06bd 100644 --- a/sr_hardware_interface/include/sr_hardware_interface/sr_actuator.hpp +++ b/sr_hardware_interface/include/sr_hardware_interface/sr_actuator.hpp @@ -114,6 +114,8 @@ class SrActuatorState std::vector raw_sensor_values_; std::vector calibrated_sensor_values_; + std::vector filtered_calibrated_position_values_; + std::vector filtered_calibrated_velocity_values_; /** * a vector containing human readable flags: diff --git a/sr_mechanism_model/src/joint_0_transmission.cpp b/sr_mechanism_model/src/joint_0_transmission.cpp index 830a9bf6e..656e279c1 100644 --- a/sr_mechanism_model/src/joint_0_transmission.cpp +++ b/sr_mechanism_model/src/joint_0_transmission.cpp @@ -60,18 +60,18 @@ namespace sr_mechanism_model // (joint 0 is composed of the 2 calibrated values: joint 1 and joint 2) SrMotorActuator *act = static_cast(actuator_); - size_t size = act->motor_state_.calibrated_sensor_values_.size(); + size_t size = act->motor_state_.filtered_calibrated_position_values_.size(); if (size == 2) { ROS_DEBUG_STREAM("READING pos " << act->state_.position_ - << " J1 " << act->motor_state_.calibrated_sensor_values_[0] - << " J2 " << act->motor_state_.calibrated_sensor_values_[1]); + << " J1 " << act->motor_state_.filtered_calibrated_position_values_[0] + << " J2 " << act->motor_state_.filtered_calibrated_position_values_[1]); - joint_->position_ = act->motor_state_.calibrated_sensor_values_[0]; - joint2_->position_ = act->motor_state_.calibrated_sensor_values_[1]; + joint_->position_ = act->motor_state_.filtered_calibrated_position_values_[0]; + joint2_->position_ = act->motor_state_.filtered_calibrated_position_values_[1]; - joint_->velocity_ = act->state_.velocity_ / 2.0; - joint2_->velocity_ = act->state_.velocity_ / 2.0; + joint_->velocity_ = act->motor_state_.filtered_calibrated_velocity_values_[0]; + joint2_->velocity_ = act->motor_state_.filtered_calibrated_velocity_values_[1]; joint_->effort_ = act->state_.last_measured_effort_; joint2_->effort_ = act->state_.last_measured_effort_; diff --git a/sr_utilities/scripts/joint_0_publisher.py b/sr_utilities/scripts/joint_0_publisher.py index 3c2e3fce0..9293ecb1c 100755 --- a/sr_utilities/scripts/joint_0_publisher.py +++ b/sr_utilities/scripts/joint_0_publisher.py @@ -22,7 +22,7 @@ class Joint0Publisher: def __init__(self): rospy.init_node('joint_0_publisher', anonymous=True) - rospy.wait_for_message("/joint_states", JointState) + rospy.wait_for_message("joint_states", JointState) self.sub = rospy.Subscriber("joint_states", JointState, self.callback) self.pub = rospy.Publisher("joint_0s/joint_states", JointState, queue_size=1) self.joint_state_msg = JointState() diff --git a/sr_utilities/scripts/sr_utilities/hand_finder.py b/sr_utilities/scripts/sr_utilities/hand_finder.py index 14473bc12..e2962690c 100755 --- a/sr_utilities/scripts/sr_utilities/hand_finder.py +++ b/sr_utilities/scripts/sr_utilities/hand_finder.py @@ -18,6 +18,9 @@ import rospy import rospkg from urdf_parser_py.urdf import URDF +import time + +DEFAULT_TIMEOUT = 60.0 class HandConfig(object): @@ -46,16 +49,16 @@ def get_default_joints(cls): 'THJ5', 'WRJ1', 'WRJ2'] return joints - def __init__(self, mapping, joint_prefix): + def __init__(self, mapping, joint_prefix, timeout=DEFAULT_TIMEOUT): """ """ self.joints = {} hand_joints = [] joints = self.get_default_joints() - TIMEOUT_WAIT_FOR_PARAMS_IN_SECS = 60.0 + TIMEOUT_WAIT_FOR_PARAMS_IN_SECS = timeout start_time = rospy.get_time() - while not rospy.has_param("/robot_description"): + while not rospy.has_param("robot_description"): if (rospy.get_time() - start_time > TIMEOUT_WAIT_FOR_PARAMS_IN_SECS): rospy.logwarn("No robot_description found on parameter server." "Joint names are loaded for 5 finger hand") @@ -70,6 +73,8 @@ def __init__(self, mapping, joint_prefix): "in joint_prefix parameters") self.joints[mapping[hand]] = hand_joints return + else: + time.sleep(1) robot_description = rospy.get_param('robot_description') @@ -110,7 +115,7 @@ class HandFinder(object): using this library to handle prefixes, joint prefixes etc... """ - def __init__(self): + def __init__(self, timeout=DEFAULT_TIMEOUT): """ Parses the parameter server to extract the necessary information. """ @@ -119,12 +124,14 @@ def __init__(self): self._hand_h = False self._hand_h_parameters = {} - TIMEOUT_WAIT_FOR_PARAMS_IN_SECS = 60.0 + TIMEOUT_WAIT_FOR_PARAMS_IN_SECS = timeout self.wait_for_hand_params(TIMEOUT_WAIT_FOR_PARAMS_IN_SECS) self.hand_config = HandConfig(self._hand_parameters["mapping"], self._hand_parameters["joint_prefix"]) - self.hand_joints = HandJoints(self.hand_config.mapping, self.hand_config.joint_prefix).joints + self.hand_joints = HandJoints(self.hand_config.mapping, self.hand_config.joint_prefix, timeout).joints + self.calibration_path = HandCalibration(self.hand_config.mapping).calibration_path + self.hand_control_tuning = HandControllerTuning(self.hand_config.mapping) def wait_for_hand_params(self, timeout_in_secs): start_time = rospy.get_time() @@ -132,6 +139,8 @@ def wait_for_hand_params(self, timeout_in_secs): if (rospy.get_time() - start_time > timeout_in_secs): rospy.logerr("No hand is detected") break + else: + time.sleep(1) if rospy.has_param("/hand"): rospy.loginfo("Found hand E") self._hand_e = True diff --git a/sr_utilities/src/sr_hand_finder.cpp b/sr_utilities/src/sr_hand_finder.cpp index 7f4448b70..1a776d134 100644 --- a/sr_utilities/src/sr_hand_finder.cpp +++ b/sr_utilities/src/sr_hand_finder.cpp @@ -56,11 +56,11 @@ HandConfig SrHandFinder::get_hand_parameters() SrHandFinder::SrHandFinder() { - if (ros::param::has("hand")) + if (ros::param::has("/hand")) { std::map mapping_map; - ros::param::get("hand/mapping", hand_config_.mapping_); - ros::param::get("hand/joint_prefix", hand_config_.joint_prefix_); + ros::param::get("/hand/mapping", hand_config_.mapping_); + ros::param::get("/hand/joint_prefix", hand_config_.joint_prefix_); std::pair pair; BOOST_FOREACH(pair, hand_config_.mapping_) diff --git a/sr_utilities/test/test_hand_finder.cpp b/sr_utilities/test/test_hand_finder.cpp index 2a2c7948f..fb5c48aeb 100644 --- a/sr_utilities/test/test_hand_finder.cpp +++ b/sr_utilities/test/test_hand_finder.cpp @@ -54,9 +54,9 @@ void ASSERT_MAP_HAS_VALUE(const map &map, const string &value) TEST(SrHandFinder, hand_absent_test) { - if (ros::param::has("hand")) + if (ros::param::has("/hand")) { - ros::param::del("hand"); + ros::param::del("/hand"); } if (ros::param::has("robot_description")) @@ -71,9 +71,9 @@ TEST(SrHandFinder, hand_absent_test) TEST(SrHandFinder, one_hand_no_robot_description_finder_test) { - if (ros::param::has("hand")) + if (ros::param::has("/hand")) { - ros::param::del("hand"); + ros::param::del("/hand"); } if (ros::param::has("robot_description")) @@ -81,8 +81,8 @@ TEST(SrHandFinder, one_hand_no_robot_description_finder_test) ros::param::del("robot_description"); } - ros::param::set("hand/mapping/1", "right"); - ros::param::set("hand/joint_prefix/1", "rh_"); + ros::param::set("/hand/mapping/1", "right"); + ros::param::set("/hand/joint_prefix/1", "rh_"); const string joint_names[] = { @@ -112,9 +112,9 @@ TEST(SrHandFinder, one_hand_no_robot_description_finder_test) TEST(SrHandFinder, one_hand_no_mapping_no_robot_description_finder_test) { - if (ros::param::has("hand")) + if (ros::param::has("/hand")) { - ros::param::del("hand"); + ros::param::del("/hand"); } if (ros::param::has("robot_description")) @@ -122,8 +122,8 @@ TEST(SrHandFinder, one_hand_no_mapping_no_robot_description_finder_test) ros::param::del("robot_description"); } - ros::param::set("hand/mapping/1", ""); - ros::param::set("hand/joint_prefix/1", "rh_"); + ros::param::set("/hand/mapping/1", ""); + ros::param::set("/hand/joint_prefix/1", "rh_"); const string joint_names[] = { @@ -153,9 +153,9 @@ TEST(SrHandFinder, one_hand_no_mapping_no_robot_description_finder_test) TEST(SrHandFinder, one_hand_no_prefix_no_robot_description_finder_test) { - if (ros::param::has("hand")) + if (ros::param::has("/hand")) { - ros::param::del("hand"); + ros::param::del("/hand"); } if (ros::param::has("robot_description")) @@ -163,8 +163,8 @@ TEST(SrHandFinder, one_hand_no_prefix_no_robot_description_finder_test) ros::param::del("robot_description"); } - ros::param::set("hand/mapping/1", "rh"); - ros::param::set("hand/joint_prefix/1", ""); + ros::param::set("/hand/mapping/1", "rh"); + ros::param::set("/hand/joint_prefix/1", ""); const string joint_names[] = { @@ -194,9 +194,9 @@ TEST(SrHandFinder, one_hand_no_prefix_no_robot_description_finder_test) TEST(SrHandFinder, one_hand_no_mapping_no_prefix_no_robot_description_finder_test) { - if (ros::param::has("hand")) + if (ros::param::has("/hand")) { - ros::param::del("hand"); + ros::param::del("/hand"); } if (ros::param::has("robot_description")) @@ -204,8 +204,8 @@ TEST(SrHandFinder, one_hand_no_mapping_no_prefix_no_robot_description_finder_tes ros::param::del("robot_description"); } - ros::param::set("hand/mapping/1", ""); - ros::param::set("hand/joint_prefix/1", ""); + ros::param::set("/hand/mapping/1", ""); + ros::param::set("/hand/joint_prefix/1", ""); const string joint_names[] = { @@ -235,9 +235,9 @@ TEST(SrHandFinder, one_hand_no_mapping_no_prefix_no_robot_description_finder_tes TEST(SrHandFinder, one_hand_robot_description_exists_finger_test) { - if (ros::param::has("hand")) + if (ros::param::has("/hand")) { - ros::param::del("hand"); + ros::param::del("/hand"); } if (ros::param::has("robot_description")) @@ -246,11 +246,12 @@ TEST(SrHandFinder, one_hand_robot_description_exists_finger_test) } string right_hand_description; - ros::param::get("right_hand_description", right_hand_description); + ros::param::get("/right_hand_description", right_hand_description); ros::param::set("robot_description", right_hand_description); - ros::param::set("hand/mapping/1", "right"); - ros::param::set("hand/joint_prefix/1", "rh_"); + ros::param::set("/hand/mapping/1", "right"); + ros::param::set("/hand/joint_prefix/1", "rh_"); + string ethercat_path = ros::package::getPath("sr_ethercat_hand_config"); const string joint_names[] = { @@ -277,9 +278,9 @@ TEST(SrHandFinder, one_hand_robot_description_exists_finger_test) TEST(SrHandFinder, two_hand_robot_description_exists_finder_test) { - if (ros::param::has("hand")) + if (ros::param::has("/hand")) { - ros::param::del("hand"); + ros::param::del("/hand"); } if (ros::param::has("robot_description")) @@ -287,13 +288,13 @@ TEST(SrHandFinder, two_hand_robot_description_exists_finder_test) ros::param::del("robot_description"); } - ros::param::set("hand/mapping/1", "right"); - ros::param::set("hand/joint_prefix/1", "rh_"); - ros::param::set("hand/mapping/2", "left"); - ros::param::set("hand/joint_prefix/2", "lh_"); + ros::param::set("/hand/mapping/1", "right"); + ros::param::set("/hand/joint_prefix/1", "rh_"); + ros::param::set("/hand/mapping/2", "left"); + ros::param::set("/hand/joint_prefix/2", "lh_"); string two_hands_description; - ros::param::get("two_hands_description", two_hands_description); + ros::param::get("/two_hands_description", two_hands_description); ros::param::set("robot_description", two_hands_description); const string joint_names[] = diff --git a/sr_utilities/test/test_hand_finder.py b/sr_utilities/test/test_hand_finder.py index 904888495..4298c66b1 100755 --- a/sr_utilities/test/test_hand_finder.py +++ b/sr_utilities/test/test_hand_finder.py @@ -27,29 +27,29 @@ class TestHandFinder(unittest.TestCase): def test_no_hand_no_robot_description_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 0, "correct parameters without a hand") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 0, "correct parameters without a hand") self.assertEqual(len(hand_finder.get_hand_joints()), 0, "correct joints without a hand") def test_one_hand_no_robot_description_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - rospy.set_param("hand/joint_prefix/1", "rh_") - rospy.set_param("hand/mapping/1", "right") + rospy.set_param("/hand/joint_prefix/1", "rh_") + rospy.set_param("/hand/mapping/1", "right") - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) # hand params self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping") @@ -65,16 +65,16 @@ def test_one_hand_no_robot_description_finder(self): self.assertIn("rh_FFJ3", hand_finder.get_hand_joints()["right"], "FFJ3 joint should be in the joints list") def test_one_hand_no_mapping_no_robot_description_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - rospy.set_param("hand/joint_prefix/1", "rh_") - rospy.set_param("hand/mapping/1", "") + rospy.set_param("/hand/joint_prefix/1", "rh_") + rospy.set_param("/hand/mapping/1", "") - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) # hand params self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping") @@ -91,16 +91,16 @@ def test_one_hand_no_mapping_no_robot_description_finder(self): self.assertIn("rh_FFJ3", hand_finder.get_hand_joints()["1"], "FFJ3 joint should be in the joints list") def test_one_hand_no_prefix_no_robot_description_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - rospy.set_param("hand/joint_prefix/1", "") - rospy.set_param("hand/mapping/1", "rh") + rospy.set_param("/hand/joint_prefix/1", "") + rospy.set_param("/hand/mapping/1", "rh") - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) # hand params self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping") @@ -116,16 +116,16 @@ def test_one_hand_no_prefix_no_robot_description_finder(self): self.assertIn("FFJ3", hand_finder.get_hand_joints()["rh"], "FFJ3 joint should be in the joints list") def test_one_hand_no_prefix_no_mapping_no_robot_description_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - rospy.set_param("hand/joint_prefix/1", "") - rospy.set_param("hand/mapping/1", "") + rospy.set_param("/hand/joint_prefix/1", "") + rospy.set_param("/hand/mapping/1", "") - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) # hand params self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping") @@ -141,18 +141,18 @@ def test_one_hand_no_prefix_no_mapping_no_robot_description_finder(self): self.assertIn("FFJ3", hand_finder.get_hand_joints()["1"], "FFJ3 joint should be in the joints list") def test_two_hand_no_robot_description_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - rospy.set_param("hand/joint_prefix/1", "rh_") - rospy.set_param("hand/mapping/1", "right") - rospy.set_param("hand/joint_prefix/2", "lh_") - rospy.set_param("hand/mapping/2", "left") + rospy.set_param("/hand/joint_prefix/1", "rh_") + rospy.set_param("/hand/mapping/1", "right") + rospy.set_param("/hand/joint_prefix/2", "lh_") + rospy.set_param("/hand/mapping/2", "left") - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) # hand params self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.") @@ -177,32 +177,32 @@ def test_two_hand_no_robot_description_finder(self): self.assertIn("lh_FFJ1", hand_finder.get_hand_joints()["left"], "FFJ1 joint should be in the joints list") def test_no_hand_robot_description_exists_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - rospy.set_param("robot_description", rospy.get_param("two_hands_description")) + rospy.set_param("robot_description", rospy.get_param("/two_hands_description")) - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 0, "correct parameters without a hand") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 0, "correct parameters without a hand") self.assertEqual(len(hand_finder.get_hand_joints()), 0, "correct joints without a hand") def test_one_hand_robot_description_exists_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - rospy.set_param("hand/joint_prefix/1", "rh_") - rospy.set_param("hand/mapping/1", "right") - rospy.set_param("robot_description", rospy.get_param("right_hand_description")) + rospy.set_param("/hand/joint_prefix/1", "rh_") + rospy.set_param("/hand/mapping/1", "right") + rospy.set_param("robot_description", rospy.get_param("/right_hand_description")) - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) # hand params self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping") @@ -220,17 +220,17 @@ def test_one_hand_robot_description_exists_finder(self): self.assertIn("rh_RFJ4", hand_finder.get_hand_joints()["right"], "RFJ4 joint should be in the joints list") def test_one_hand_no_mapping_robot_description_exists_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - rospy.set_param("hand/joint_prefix/1", "rh_") - rospy.set_param("hand/mapping/1", "") - rospy.set_param("robot_description", rospy.get_param("right_hand_description")) + rospy.set_param("/hand/joint_prefix/1", "rh_") + rospy.set_param("/hand/mapping/1", "") + rospy.set_param("robot_description", rospy.get_param("/right_hand_description")) - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) # hand params self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping") @@ -247,17 +247,17 @@ def test_one_hand_no_mapping_robot_description_exists_finder(self): self.assertIn("rh_RFJ4", hand_finder.get_hand_joints()["1"], "RFJ4 joint should be in the joints list") def test_one_hand_no_prefix_robot_description_exists_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - rospy.set_param("hand/joint_prefix/1", "") - rospy.set_param("hand/mapping/1", "rh") - rospy.set_param("robot_description", rospy.get_param("right_hand_description_no_prefix")) + rospy.set_param("/hand/joint_prefix/1", "") + rospy.set_param("/hand/mapping/1", "rh") + rospy.set_param("robot_description", rospy.get_param("/right_hand_description_no_prefix")) - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) # hand params self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping") @@ -274,17 +274,17 @@ def test_one_hand_no_prefix_robot_description_exists_finder(self): self.assertIn("RFJ4", hand_finder.get_hand_joints()["rh"], "RFJ4 joint should be in the joints list") def test_one_hand_no_prefix_no_ns_robot_description_exists_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - rospy.set_param("hand/joint_prefix/1", "") - rospy.set_param("hand/mapping/1", "") - rospy.set_param("robot_description", rospy.get_param("right_hand_description_no_prefix")) + rospy.set_param("/hand/joint_prefix/1", "") + rospy.set_param("/hand/mapping/1", "") + rospy.set_param("robot_description", rospy.get_param("/right_hand_description_no_prefix")) - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) # hand params self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping") @@ -301,19 +301,19 @@ def test_one_hand_no_prefix_no_ns_robot_description_exists_finder(self): self.assertIn("RFJ4", hand_finder.get_hand_joints()["1"], "RFJ4 joint should be in the joints list") def test_two_hand_robot_description_exists_finder(self): - if rospy.has_param("hand"): - rospy.delete_param("hand") + if rospy.has_param("/hand"): + rospy.delete_param("/hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") - rospy.set_param("hand/joint_prefix/1", "rh_") - rospy.set_param("hand/mapping/1", "right") - rospy.set_param("hand/joint_prefix/2", "lh_") - rospy.set_param("hand/mapping/2", "left") - rospy.set_param("robot_description", rospy.get_param("two_hands_description")) + rospy.set_param("/hand/joint_prefix/1", "rh_") + rospy.set_param("/hand/mapping/1", "right") + rospy.set_param("/hand/joint_prefix/2", "lh_") + rospy.set_param("/hand/mapping/2", "left") + rospy.set_param("robot_description", rospy.get_param("/two_hands_description")) - hand_finder = HandFinder() + hand_finder = HandFinder(1.0) # hand params self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 2, "It should be two mappings") diff --git a/sr_utilities/test/test_hand_finder.test b/sr_utilities/test/test_hand_finder.test index d86a85f06..f2312f8f0 100644 --- a/sr_utilities/test/test_hand_finder.test +++ b/sr_utilities/test/test_hand_finder.test @@ -4,6 +4,10 @@ - - + + + + + + From 07fba96e2eb98022defa0fc8c4de337401500e74 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Tue, 21 Jun 2022 15:16:02 +0100 Subject: [PATCH 3/3] updates --- sr_utilities/scripts/sr_utilities/hand_finder.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/sr_utilities/scripts/sr_utilities/hand_finder.py b/sr_utilities/scripts/sr_utilities/hand_finder.py index e2962690c..a9ed88073 100755 --- a/sr_utilities/scripts/sr_utilities/hand_finder.py +++ b/sr_utilities/scripts/sr_utilities/hand_finder.py @@ -130,8 +130,6 @@ def __init__(self, timeout=DEFAULT_TIMEOUT): self.hand_config = HandConfig(self._hand_parameters["mapping"], self._hand_parameters["joint_prefix"]) self.hand_joints = HandJoints(self.hand_config.mapping, self.hand_config.joint_prefix, timeout).joints - self.calibration_path = HandCalibration(self.hand_config.mapping).calibration_path - self.hand_control_tuning = HandControllerTuning(self.hand_config.mapping) def wait_for_hand_params(self, timeout_in_secs): start_time = rospy.get_time()