From f0457da0c3de5ac83a07753596d160d4661cafaa Mon Sep 17 00:00:00 2001 From: Victor Lopez <3469405+v-lopez@users.noreply.github.com> Date: Wed, 16 Jun 2021 12:07:48 +0200 Subject: [PATCH] Foxy migration (#25) --- CMakeLists.txt | 95 ++++--- CONTRIBUTING.md | 57 +++++ LICENSE | 202 +++++++++++++++ README.md | 9 +- config/joystick.yaml | 21 +- config/twist_mux_locks.yaml | 36 ++- config/twist_mux_topics.yaml | 42 ++- include/twist_mux/params_helpers.hpp | 70 +++++ include/twist_mux/topic_handle.h | 209 --------------- include/twist_mux/topic_handle.hpp | 241 ++++++++++++++++++ include/twist_mux/twist_mux.h | 100 -------- include/twist_mux/twist_mux.hpp | 112 ++++++++ include/twist_mux/twist_mux_diagnostics.h | 69 ----- include/twist_mux/twist_mux_diagnostics.hpp | 79 ++++++ .../twist_mux/twist_mux_diagnostics_status.h | 60 ----- .../twist_mux_diagnostics_status.hpp | 77 ++++++ include/twist_mux/utils.h | 44 ---- include/twist_mux/utils.hpp | 58 +++++ include/twist_mux/xmlrpc_helpers.h | 111 -------- launch/twist_mux_launch.py | 71 ++++++ package.xml | 48 ++-- scripts/joystick_relay.py | 189 +++++++++----- src/twist_marker.cpp | 138 ++++++---- src/twist_mux.cpp | 184 +++++++------ src/twist_mux_diagnostics.cpp | 128 +++++----- src/twist_mux_node.cpp | 65 +++-- test/rate_publishers.py | 99 +++---- test/system.test.py | 211 +++++++++++++++ test/system_blackbox.py | 59 +++-- test/system_config.yaml | 47 ++-- 30 files changed, 1838 insertions(+), 1093 deletions(-) create mode 100644 CONTRIBUTING.md create mode 100644 LICENSE create mode 100644 include/twist_mux/params_helpers.hpp delete mode 100644 include/twist_mux/topic_handle.h create mode 100644 include/twist_mux/topic_handle.hpp delete mode 100644 include/twist_mux/twist_mux.h create mode 100644 include/twist_mux/twist_mux.hpp delete mode 100644 include/twist_mux/twist_mux_diagnostics.h create mode 100644 include/twist_mux/twist_mux_diagnostics.hpp delete mode 100644 include/twist_mux/twist_mux_diagnostics_status.h create mode 100644 include/twist_mux/twist_mux_diagnostics_status.hpp delete mode 100644 include/twist_mux/utils.h create mode 100644 include/twist_mux/utils.hpp delete mode 100644 include/twist_mux/xmlrpc_helpers.h create mode 100644 launch/twist_mux_launch.py create mode 100644 test/system.test.py diff --git a/CMakeLists.txt b/CMakeLists.txt index d104a98..40ba596 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,57 +1,78 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) + project(twist_mux) -find_package(catkin REQUIRED - COMPONENTS - roscpp - std_msgs - geometry_msgs - visualization_msgs - diagnostic_updater) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -find_package(Boost REQUIRED) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}") +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - roscpp - std_msgs - geometry_msgs - visualization_msgs - diagnostic_updater) +include_directories(include) -include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +set( + DEPENDENCIES + "rclcpp" + "std_msgs" + "geometry_msgs" + "visualization_msgs" + "diagnostic_updater" +) add_executable(twist_mux - src/twist_mux.cpp - src/twist_mux_node.cpp - src/twist_mux_diagnostics.cpp) -target_link_libraries(twist_mux ${catkin_LIBRARIES}) + src/twist_mux.cpp + src/twist_mux_node.cpp + src/twist_mux_diagnostics.cpp +) +ament_target_dependencies(twist_mux ${DEPENDENCIES}) add_executable(twist_marker - src/twist_marker.cpp) -target_link_libraries(twist_marker ${catkin_LIBRARIES}) + src/twist_marker.cpp +) +ament_target_dependencies(twist_marker ${DEPENDENCIES}) -install(TARGETS twist_mux twist_marker - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install( + TARGETS twist_mux twist_marker + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) -install(PROGRAMS scripts/joystick_relay.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +# Pending to test ROS2 migration +#install( +# PROGRAMS scripts/joystick_relay.py +# DESTINATION lib/${PROJECT_NAME} +#) foreach(dir launch config) - install(DIRECTORY ${dir}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) + install(DIRECTORY ${dir}/ + DESTINATION share/${PROJECT_NAME}/${dir}) endforeach() -install(DIRECTORY include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" +install( + DIRECTORY include/ + DESTINATION include ) -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() - add_rostest(test/system.test) + # find_package(launch_testing_ament_cmake) + # add_launch_test(test/system.test.py) endif() + +ament_export_include_directories(include) + +ament_export_dependencies(${DEPENDENCIES}) + +ament_package() diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..357f91d --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,57 @@ +# Contributing Guidelines +Thank you for your interest in contributing to this project +Whether it's a bug report, new feature, correction, or additional +documentation, we greatly value feedback and contributions from our community. + +Please read through this document before submitting any issues or pull requests to ensure we have all the necessary +information to effectively respond to your bug report or contribution. + + +## Reporting Bugs/Feature Requests +We welcome you to use the GitHub issue tracker to report bugs or suggest features. + +When filing an issue, please check existing open issues, or recently closed, issues to make sure + somebody else hasn't already reported the issue. +Please try to include as much information as you can. Details like these are incredibly useful: + +* A reproducible test case or series of steps +* The version of our code being used +* Any modifications you've made relevant to the bug +* Anything unusual about your environment or deployment + + +## Contributing via Pull Requests +Contributions via pull requests are much appreciated. +Before sending us a pull request, please ensure that: + +1. You are working against the latest source on the *master* branch. +2. You check existing open, and recently merged, pull requests to make sure someone else hasn't addressed the problem already. +3. You open an issue to discuss any significant work - we would hate for your time to be wasted. + +To send us a pull request, please: + +1. Fork the repository. +2. Modify the source; please focus on the specific change you are contributing. + If you also reformat all the code, it will be hard for us to focus on your change. +3. Ensure local tests pass. (`colcon test`) +4. Commit to your fork using clear commit messages. +5. Send a pull request, answering any default questions in the pull request interface. +6. Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation. + +GitHub provides additional documentation on [forking a repository](https://help.github.com/articles/fork-a-repo/) and +[creating a pull request](https://help.github.com/articles/creating-a-pull-request/). + + +## Licensing +Any contribution that you make to this repository will be under the Apache 2 License, as dictated by that [license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/README.md b/README.md index 4e1592b..10f85ec 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,9 @@ twist_mux ========= -Twist multiplexer with support for [geometry_msgs/Twist](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html) topics and [std_msgs/Bool](http://docs.ros.org/api/std_msgs/html/msg/Bool.html) locks with priorities. +Twist multiplexer with support for +[geometry_msgs/Twist](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html) +topics and +[std_msgs/Bool](http://docs.ros.org/api/std_msgs/html/msg/Bool.html) locks with priorities. -See [documentation](http://wiki.ros.org/twist_mux). - -[![Build Status](https://travis-ci.org/ros-teleop/twist_mux.png?branch=jade-devel)](https://travis-ci.org/ros-teleop/twist_mux) +See [documentation](http://wiki.ros.org/twist_mux). \ No newline at end of file diff --git a/config/joystick.yaml b/config/joystick.yaml index 6e2d8c9..a328837 100644 --- a/config/joystick.yaml +++ b/config/joystick.yaml @@ -1,10 +1,11 @@ -priority: True - -turbo: - linear_forward_min : 0.5 - linear_forward_max : 1.0 - linear_backward_min : 0.25 - linear_backward_max : 0.5 - angular_min : 0.7 - angular_max : 1.2 - steps : 4 +joystick_relay: + ros__parameters: + priority: True + turbo: + linear_forward_min : 0.5 + linear_forward_max : 1.0 + linear_backward_min : 0.25 + linear_backward_max : 0.5 + angular_min : 0.7 + angular_max : 1.2 + steps : 4 diff --git a/config/twist_mux_locks.yaml b/config/twist_mux_locks.yaml index 5d3f519..418f0da 100644 --- a/config/twist_mux_locks.yaml +++ b/config/twist_mux_locks.yaml @@ -1,6 +1,6 @@ # Locks to stop the twist inputs. # For each lock: -# - topic : input topic that provides the lock; it must be of type std_msgs::Bool?!!! +# - topic : input topic that provides the lock; it must be of type std_msgs::Bool?!!! # - timeout : == 0.0 -> not used # > 0.0 -> the lock is supposed to published at a certain frequency in order # to detect that the publisher is alive; the timeout in seconds allows @@ -8,21 +8,19 @@ # - priority: priority in the range [0, 255], so all the topics with priority lower than it # will be stopped/disabled -locks: -- - name : pause - topic : pause_navigation - timeout : 0.0 - # Same priority as joystick control, so it'll not block it. - priority: 100 -- - name : loop_closure - topic : stop_closing_loop - timeout : 0.0 - priority: 200 -- - name : joystick - topic : joy_priority - timeout : 0.0 - priority: 100 - +twist_mux: + ros__parameters: + locks: + pause: + topic : pause_navigation + timeout : 0.0 + # Same priority as joystick control, so it'll not block it. + priority: 100 + loop_closure: + topic : stop_closing_loop + timeout : 0.0 + priority: 200 + joystick : + topic : joy_priority + timeout : 0.0 + priority: 100 diff --git a/config/twist_mux_topics.yaml b/config/twist_mux_topics.yaml index 2d223eb..45628d4 100644 --- a/config/twist_mux_topics.yaml +++ b/config/twist_mux_topics.yaml @@ -1,28 +1,26 @@ # Input topics handled/muxed. # For each topic: -# - name : name identifier to select the topic +# - name : name identifier to select the topic (*sub-namespace, see below) # - topic : input topic of geometry_msgs::Twist type # - timeout : timeout in seconds to start discarding old messages, and use 0.0 speed instead # - priority: priority in the range [0, 255]; the higher the more priority over other topics -topics: -- - name : navigation - topic : nav_vel - timeout : 0.5 - priority: 10 -- - name : joystick - topic : joy_vel - timeout : 0.5 - priority: 100 -- - name : keyboard - topic : key_vel - timeout : 0.5 - priority: 90 -- - name : tablet - topic : tab_vel - timeout : 0.5 - priority: 100 +twist_mux: + ros__parameters: + topics: + navigation: + topic : nav_vel + timeout : 0.5 + priority: 10 + joystick: + topic : joy_vel + timeout : 0.5 + priority: 100 + keyboard: + topic : key_vel + timeout : 0.5 + priority: 90 + tablet: + topic : tab_vel + timeout : 0.5 + priority: 100 diff --git a/include/twist_mux/params_helpers.hpp b/include/twist_mux/params_helpers.hpp new file mode 100644 index 0000000..4522e32 --- /dev/null +++ b/include/twist_mux/params_helpers.hpp @@ -0,0 +1,70 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 PAL Robotics S.L. 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 HOLDER 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 Paul Mathieu + * @author Jeremie Deray + */ + +#ifndef TWIST_MUX__PARAMS_HELPERS_HPP_ +#define TWIST_MUX__PARAMS_HELPERS_HPP_ + +#include + +#include +#include +#include + +namespace twist_mux +{ +class ParamsHelperException : public std::runtime_error +{ +public: + explicit ParamsHelperException(const std::string & what) + : std::runtime_error(what) + { + } +}; + +template +void fetch_param(std::shared_ptr nh, const std::string & param_name, T & output) +{ + rclcpp::Parameter param; + if (!nh->get_parameter(param_name, param)) { + std::ostringstream err_msg; + err_msg << "could not load parameter '" << param_name << "'. (namespace: " << + nh->get_namespace() << ")"; + throw ParamsHelperException(err_msg.str()); + } + + output = param.get_value(); +} + +} // namespace twist_mux + +#endif // TWIST_MUX__PARAMS_HELPERS_HPP_ diff --git a/include/twist_mux/topic_handle.h b/include/twist_mux/topic_handle.h deleted file mode 100644 index a24cbaa..0000000 --- a/include/twist_mux/topic_handle.h +++ /dev/null @@ -1,209 +0,0 @@ -/********************************************************************* - * Software License Agreement (CC BY-NC-SA 4.0 License) - * - * Copyright (c) 2014, PAL Robotics, S.L. - * All rights reserved. - * - * This work is licensed under the Creative Commons - * Attribution-NonCommercial-ShareAlike 4.0 International License. - * - * To view a copy of this license, visit - * http://creativecommons.org/licenses/by-nc-sa/4.0/ - * or send a letter to - * Creative Commons, 444 Castro Street, Suite 900, - * Mountain View, California, 94041, USA. - *********************************************************************/ - -/* - * @author Enrique Fernandez - * @author Siegfried Gevatter - */ - -#ifndef TOPIC_HANDLE_H -#define TOPIC_HANDLE_H - -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include - -namespace twist_mux -{ - -template -class TopicHandle_ : boost::noncopyable -{ -public: - - typedef int priority_type; - - /** - * @brief TopicHandle_ - * @param nh Node handle - * @param name Name identifier - * @param topic Topic name - * @param timeout Timeout to consider that the messages are old; note - * that initially the message stamp is set to 0.0, so the message has - * expired - * @param priority Priority of the topic - */ - TopicHandle_(ros::NodeHandle& nh, const std::string& name, const std::string& topic, double timeout, priority_type priority, TwistMux* mux) - : nh_(nh) - , name_(name) - , topic_(topic) - , timeout_(timeout) - , priority_(clamp(priority, priority_type(0), priority_type(255))) - , mux_(mux) - , stamp_(0.0) - { - ROS_INFO_STREAM - ( - "Topic handler '" << name_ << "' subscribed to topic '" << topic_ << - "': timeout = " << ((timeout_) ? std::to_string(timeout_) + "s" : "None") << - ", priority = " << static_cast(priority_) - ); - } - - virtual ~TopicHandle_() - { - subscriber_.shutdown(); - } - - /** - * @brief hasExpired - * @return true if the message has expired; false otherwise. - * If the timeout is set to 0.0, this function always returns - * false - */ - bool hasExpired() const - { - return (timeout_ > 0.0) and - ((ros::Time::now() - stamp_).toSec() > timeout_); - } - - const std::string& getName() const - { - return name_; - } - - const std::string& getTopic() const - { - return topic_; - } - - const double& getTimeout() const - { - return timeout_; - } - - /** - * @brief getPriority Priority getter - * @return Priority - */ - const priority_type& getPriority() const - { - return priority_; - } - - const T& getStamp() const - { - return stamp_; - } - - const T& getMessage() const - { - return msg_; - } - -protected: - ros::NodeHandle nh_; - - std::string name_; - std::string topic_; - ros::Subscriber subscriber_; - double timeout_; - priority_type priority_; - -protected: - TwistMux* mux_; - - ros::Time stamp_; - T msg_; -}; - -class VelocityTopicHandle : public TopicHandle_ -{ -private: - typedef TopicHandle_ base_type; - -public: - typedef typename base_type::priority_type priority_type; - - VelocityTopicHandle(ros::NodeHandle& nh, const std::string& name, const std::string& topic, double timeout, priority_type priority, TwistMux* mux) - : base_type(nh, name, topic, timeout, priority, mux) - { - subscriber_ = nh_.subscribe(topic_, 1, &VelocityTopicHandle::callback, this); - } - - bool isMasked(priority_type lock_priority) const - { - return hasExpired() or (getPriority() < lock_priority); - } - - void callback(const geometry_msgs::TwistConstPtr& msg) - { - stamp_ = ros::Time::now(); - msg_ = *msg; - - // Check if this twist has priority. - // Note that we have to check all the locks because they might time out - // and since we have several topics we must look for the highest one in - // all the topic list; so far there's no O(1) solution. - if (mux_->hasPriority(*this)) - { - mux_->publishTwist(msg); - } - } -}; - -class LockTopicHandle : public TopicHandle_ -{ -private: - typedef TopicHandle_ base_type; - -public: - typedef typename base_type::priority_type priority_type; - - LockTopicHandle(ros::NodeHandle& nh, const std::string& name, const std::string& topic, double timeout, priority_type priority, TwistMux* mux) - : base_type(nh, name, topic, timeout, priority, mux) - { - subscriber_ = nh_.subscribe(topic_, 1, &LockTopicHandle::callback, this); - } - - /** - * @brief isLocked - * @return true if has expired or locked (i.e. bool message data is true) - */ - bool isLocked() const - { - return hasExpired() or getMessage().data; - } - - void callback(const std_msgs::BoolConstPtr& msg) - { - stamp_ = ros::Time::now(); - msg_ = *msg; - } -}; - -} // namespace twist_mux - -#endif // TOPIC_HANDLE_H diff --git a/include/twist_mux/topic_handle.hpp b/include/twist_mux/topic_handle.hpp new file mode 100644 index 0000000..5dc2015 --- /dev/null +++ b/include/twist_mux/topic_handle.hpp @@ -0,0 +1,241 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 PAL Robotics S.L. 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 HOLDER 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 Enrique Fernandez + * @author Siegfried Gevatter + * @author Jeremie Deray + * @author Brighten Lee + */ + +#ifndef TWIST_MUX__TOPIC_HANDLE_HPP_ +#define TWIST_MUX__TOPIC_HANDLE_HPP_ + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace twist_mux +{ +template +class TopicHandle_ +{ +public: + // Not copy constructible + TopicHandle_(TopicHandle_ &) = delete; + TopicHandle_(const TopicHandle_ &) = delete; + + // Not copy assignable + TopicHandle_ & operator=(TopicHandle_ &) = delete; + TopicHandle_ & operator=(const TopicHandle_ &) = delete; + + typedef int priority_type; + + /** + * @brief TopicHandle_ + * @param nh Node handle + * @param name Name identifier + * @param topic Topic name + * @param timeout Timeout to consider that the messages are old; note + * that initially the message stamp is set to 0.0, so the message has + * expired + * @param priority Priority of the topic + */ + TopicHandle_( + const std::string & name, const std::string & topic, const rclcpp::Duration & timeout, + priority_type priority, TwistMux * mux) + : name_(name), + topic_(topic), + timeout_(timeout), + priority_(clamp(priority, priority_type(0), priority_type(255))), + mux_(mux), + stamp_(0) + { + RCLCPP_INFO( + mux_->get_logger(), + "Topic handler '%s' subscribed to topic '%s': timeout = %s , priority = %d.", + name_.c_str(), topic_.c_str(), + ((timeout_.seconds() > 0) ? std::to_string(timeout_.seconds()) + "s" : "None").c_str(), + static_cast(priority_)); + } + + virtual ~TopicHandle_() = default; + + /** + * @brief hasExpired + * @return true if the message has expired; false otherwise. + * If the timeout is set to 0.0, this function always returns + * false + */ + bool hasExpired() const + { + return (timeout_.seconds() > 0.0) && ( + (mux_->now().seconds() - stamp_.seconds()) > timeout_.seconds()); + } + + const std::string & getName() const + { + return name_; + } + + const std::string & getTopic() const + { + return topic_; + } + + const rclcpp::Duration & getTimeout() const + { + return timeout_; + } + + /** + * @brief getPriority Priority getter + * @return Priority + */ + const priority_type & getPriority() const + { + return priority_; + } + + const T & getStamp() const + { + return stamp_; + } + + const T & getMessage() const + { + return msg_; + } + +protected: + std::string name_; + std::string topic_; + typename rclcpp::Subscription::SharedPtr subscriber_; + rclcpp::Duration timeout_; + priority_type priority_; + +protected: + TwistMux * mux_; + + rclcpp::Time stamp_; + T msg_; +}; + +class VelocityTopicHandle : public TopicHandle_ +{ +private: + typedef TopicHandle_ base_type; + + // https://index.ros.org/doc/ros2/About-Quality-of-Service-Settings + // rmw_qos_profile_t twist_qos_profile = rmw_qos_profile_sensor_data; + +public: + typedef typename base_type::priority_type priority_type; + + VelocityTopicHandle( + const std::string & name, const std::string & topic, const rclcpp::Duration & timeout, + priority_type priority, TwistMux * mux) + : base_type(name, topic, timeout, priority, mux) + { + subscriber_ = mux_->create_subscription( + topic_, rclcpp::SystemDefaultsQoS(), + std::bind(&VelocityTopicHandle::callback, this, std::placeholders::_1)); + + // subscriber_ = nh_.create_subscription( + // topic_, twist_qos_profile, + // std::bind(&VelocityTopicHandle::callback, this, std::placeholders::_1)); + } + + bool isMasked(priority_type lock_priority) const + { + // std::cout << hasExpired() << " / " << (getPriority() < lock_priority) << std::endl; + return hasExpired() || (getPriority() < lock_priority); + } + + void callback(const geometry_msgs::msg::Twist::ConstSharedPtr msg) + { + stamp_ = mux_->now(); + msg_ = *msg; + + // Check if this twist has priority. + // Note that we have to check all the locks because they might time out + // and since we have several topics we must look for the highest one in + // all the topic list; so far there's no O(1) solution. + if (mux_->hasPriority(*this)) { + mux_->publishTwist(msg); + } + } +}; + +class LockTopicHandle : public TopicHandle_ +{ +private: + typedef TopicHandle_ base_type; + + // https://index.ros.org/doc/ros2/About-Quality-of-Service-Settings + // rmw_qos_profile_t lock_qos_profile = rmw_qos_profile_sensor_data; + +public: + typedef typename base_type::priority_type priority_type; + + LockTopicHandle( + const std::string & name, const std::string & topic, const rclcpp::Duration & timeout, + priority_type priority, TwistMux * mux) + : base_type(name, topic, timeout, priority, mux) + { + subscriber_ = mux_->create_subscription( + topic_, rclcpp::SystemDefaultsQoS(), + std::bind(&LockTopicHandle::callback, this, std::placeholders::_1)); + } + + /** + * @brief isLocked + * @return true if has expired or locked (i.e. bool message data is true) + */ + bool isLocked() const + { + return hasExpired() || getMessage().data; + } + + void callback(const std_msgs::msg::Bool::ConstSharedPtr msg) + { + stamp_ = mux_->now(); + msg_ = *msg; + } +}; + +} // namespace twist_mux + +#endif // TWIST_MUX__TOPIC_HANDLE_HPP_ diff --git a/include/twist_mux/twist_mux.h b/include/twist_mux/twist_mux.h deleted file mode 100644 index 8af663f..0000000 --- a/include/twist_mux/twist_mux.h +++ /dev/null @@ -1,100 +0,0 @@ -/********************************************************************* - * Software License Agreement (CC BY-NC-SA 4.0 License) - * - * Copyright (c) 2014, PAL Robotics, S.L. - * All rights reserved. - * - * This work is licensed under the Creative Commons - * Attribution-NonCommercial-ShareAlike 4.0 International License. - * - * To view a copy of this license, visit - * http://creativecommons.org/licenses/by-nc-sa/4.0/ - * or send a letter to - * Creative Commons, 444 Castro Street, Suite 900, - * Mountain View, California, 94041, USA. - *********************************************************************/ - -/* - * @author Enrique Fernandez - * @author Siegfried Gevatter - */ - -#ifndef TWIST_MUX_H -#define TWIST_MUX_H - -#include -#include -#include - -#include - -namespace twist_mux -{ - -// Forwarding declarations: -class TwistMuxDiagnostics; -struct TwistMuxDiagnosticsStatus; -class VelocityTopicHandle; -class LockTopicHandle; - -/** - * @brief The TwistMux class implements a top-level twist multiplexer module - * that priorize different velocity command topic inputs according to locks. - */ -class TwistMux -{ -public: - - // @todo use this type alias when the compiler supports this C++11 feat. - //template - //using handle_container = std::list; - // @todo alternatively we do the following: - typedef std::list velocity_topic_container; - typedef std::list lock_topic_container; - - TwistMux(int window_size = 10); - ~TwistMux(); - - bool hasPriority(const VelocityTopicHandle& twist); - - void publishTwist(const geometry_msgs::TwistConstPtr& msg); - - void updateDiagnostics(const ros::TimerEvent& event); - -protected: - - typedef TwistMuxDiagnostics diagnostics_type; - typedef TwistMuxDiagnosticsStatus status_type; - - ros::Timer diagnostics_timer_; - - static constexpr double DIAGNOSTICS_PERIOD = 1.0; - - /** - * @brief velocity_hs_ Velocity topics' handles. - * Note that if we use a vector, as a consequence of the re-allocation and - * the fact that we have a subscriber inside with a pointer to 'this', we - * must reserve the number of handles initially. - */ - // @todo use handle_container (see above) - //handle_container velocity_hs_; - //handle_container lock_hs_; - boost::shared_ptr velocity_hs_; - boost::shared_ptr lock_hs_; - - ros::Publisher cmd_pub_; - - geometry_msgs::Twist last_cmd_; - - template - void getTopicHandles(ros::NodeHandle& nh, ros::NodeHandle& nh_priv, const std::string& param_name, std::list& topic_hs); - - int getLockPriority(); - - boost::shared_ptr diagnostics_; - boost::shared_ptr status_; -}; - -} // namespace twist_mux - -#endif // TWIST_MUX_H diff --git a/include/twist_mux/twist_mux.hpp b/include/twist_mux/twist_mux.hpp new file mode 100644 index 0000000..517e7c0 --- /dev/null +++ b/include/twist_mux/twist_mux.hpp @@ -0,0 +1,112 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 PAL Robotics S.L. 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 HOLDER 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 Enrique Fernandez + * @author Siegfried Gevatter + * @author Jeremie Deray + */ + +#ifndef TWIST_MUX__TWIST_MUX_HPP_ +#define TWIST_MUX__TWIST_MUX_HPP_ + +#include +#include +#include + +#include +#include +#include + +using std::chrono_literals::operator""s; + +namespace twist_mux +{ +// Forwarding declarations: +class TwistMuxDiagnostics; +struct TwistMuxDiagnosticsStatus; +class VelocityTopicHandle; +class LockTopicHandle; + +/** + * @brief The TwistMux class implements a top-level twist multiplexer module + * that priorize different velocity command topic inputs according to locks. + */ +class TwistMux : public rclcpp::Node +{ +public: + template + using handle_container = std::list; + + using velocity_topic_container = handle_container; + using lock_topic_container = handle_container; + + explicit TwistMux(int window_size = 10); + ~TwistMux() = default; + + void init(); + + bool hasPriority(const VelocityTopicHandle & twist); + + void publishTwist(const geometry_msgs::msg::Twist::ConstSharedPtr & msg); + + void updateDiagnostics(); + +protected: + typedef TwistMuxDiagnostics diagnostics_type; + typedef TwistMuxDiagnosticsStatus status_type; + + rclcpp::TimerBase::SharedPtr diagnostics_timer_; + + static constexpr std::chrono::duration DIAGNOSTICS_PERIOD = 1s; + + /** + * @brief velocity_hs_ Velocity topics' handles. + * Note that if we use a vector, as a consequence of the re-allocation and + * the fact that we have a subscriber inside with a pointer to 'this', we + * must reserve the number of handles initially. + */ + std::shared_ptr velocity_hs_; + std::shared_ptr lock_hs_; + + rclcpp::Publisher::SharedPtr cmd_pub_; + + geometry_msgs::msg::Twist last_cmd_; + + template + void getTopicHandles(const std::string & param_name, handle_container & topic_hs); + + int getLockPriority(); + + std::shared_ptr diagnostics_; + std::shared_ptr status_; +}; + +} // namespace twist_mux + +#endif // TWIST_MUX__TWIST_MUX_HPP_ diff --git a/include/twist_mux/twist_mux_diagnostics.h b/include/twist_mux/twist_mux_diagnostics.h deleted file mode 100644 index 5f01eff..0000000 --- a/include/twist_mux/twist_mux_diagnostics.h +++ /dev/null @@ -1,69 +0,0 @@ -/********************************************************************* - * Software License Agreement (CC BY-NC-SA 4.0 License) - * - * Copyright (c) 2014, PAL Robotics, S.L. - * All rights reserved. - * - * This work is licensed under the Creative Commons - * Attribution-NonCommercial-ShareAlike 4.0 International License. - * - * To view a copy of this license, visit - * http://creativecommons.org/licenses/by-nc-sa/4.0/ - * or send a letter to - * Creative Commons, 444 Castro Street, Suite 900, - * Mountain View, California, 94041, USA. - *********************************************************************/ - -/* - * @author Enrique Fernandez - */ - -#ifndef TWIST_MUX_DIAGNOSTICS_H -#define TWIST_MUX_DIAGNOSTICS_H - -#include - -#include -#include - -#include - -namespace twist_mux -{ - -class TwistMuxDiagnostics -{ - public: - typedef TwistMuxDiagnosticsStatus status_type; - - static constexpr double MAIN_LOOP_TIME_MIN = 0.2; // [s] - static constexpr double READING_AGE_MIN = 3.0; // [s] - - TwistMuxDiagnostics(); - virtual ~TwistMuxDiagnostics(); - - void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); - - void update(); - - void updateStatus(const status_type::ConstPtr& status); - - private: - - /** - * @brief Levels - */ - enum - { - OK = diagnostic_msgs::DiagnosticStatus::OK, - WARN = diagnostic_msgs::DiagnosticStatus::WARN, - ERROR = diagnostic_msgs::DiagnosticStatus::ERROR - }; - - diagnostic_updater::Updater diagnostic_; - status_type status_; -}; - -} // namespace twist_mux - -#endif // TWIST_MUX_DIAGNOSTICS_H diff --git a/include/twist_mux/twist_mux_diagnostics.hpp b/include/twist_mux/twist_mux_diagnostics.hpp new file mode 100644 index 0000000..8cf1534 --- /dev/null +++ b/include/twist_mux/twist_mux_diagnostics.hpp @@ -0,0 +1,79 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 PAL Robotics S.L. 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 HOLDER 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 Enrique Fernandez + * @author Jeremie Deray + * @author Brighten Lee + */ + +#ifndef TWIST_MUX__TWIST_MUX_DIAGNOSTICS_HPP_ +#define TWIST_MUX__TWIST_MUX_DIAGNOSTICS_HPP_ + +#include + +#include + +#include + +namespace twist_mux +{ +class TwistMuxDiagnostics +{ +public: + typedef TwistMuxDiagnosticsStatus status_type; + + static constexpr double MAIN_LOOP_TIME_MIN = 0.2; // [s] + static constexpr double READING_AGE_MIN = 3.0; // [s] + + explicit TwistMuxDiagnostics(TwistMux * mux); + virtual ~TwistMuxDiagnostics() = default; + + void diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + + void update(); + + void updateStatus(const status_type::ConstPtr & status); + +private: + /** + * @brief Levels + */ + enum + { + OK = diagnostic_msgs::msg::DiagnosticStatus::OK, + WARN = diagnostic_msgs::msg::DiagnosticStatus::WARN, + ERROR = diagnostic_msgs::msg::DiagnosticStatus::ERROR + }; + + std::shared_ptr diagnostic_; + std::shared_ptr status_; +}; +} // namespace twist_mux + +#endif // TWIST_MUX__TWIST_MUX_DIAGNOSTICS_HPP_ diff --git a/include/twist_mux/twist_mux_diagnostics_status.h b/include/twist_mux/twist_mux_diagnostics_status.h deleted file mode 100644 index 790a299..0000000 --- a/include/twist_mux/twist_mux_diagnostics_status.h +++ /dev/null @@ -1,60 +0,0 @@ -/********************************************************************* - * Software License Agreement (CC BY-NC-SA 4.0 License) - * - * Copyright (c) 2014, PAL Robotics, S.L. - * All rights reserved. - * - * This work is licensed under the Creative Commons - * Attribution-NonCommercial-ShareAlike 4.0 International License. - * - * To view a copy of this license, visit - * http://creativecommons.org/licenses/by-nc-sa/4.0/ - * or send a letter to - * Creative Commons, 444 Castro Street, Suite 900, - * Mountain View, California, 94041, USA. - *********************************************************************/ - -/* - * @author Enrique Fernandez - */ - -#ifndef TWIST_MUX_DIAGNOSTICS_STATUS_H -#define TWIST_MUX_DIAGNOSTICS_STATUS_H - -#include -#include - -#include - -namespace twist_mux -{ - -struct TwistMuxDiagnosticsStatus -{ - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; - - double reading_age; - ros::Time last_loop_update; - double main_loop_time; - - LockTopicHandle::priority_type priority; - - boost::shared_ptr velocity_hs; - boost::shared_ptr lock_hs; - - TwistMuxDiagnosticsStatus() - : reading_age(0), - last_loop_update(ros::Time::now()), - main_loop_time(0), - priority(0) - { - } -}; - -typedef TwistMuxDiagnosticsStatus::Ptr TwistMuxDiagnosticsStatusPtr; -typedef TwistMuxDiagnosticsStatus::ConstPtr TwistMuxDiagnosticsStatusConstPtr; - -} // namespace twist_mux - -#endif // TWIST_MUX_DIAGNOSTICS_STATUS_H diff --git a/include/twist_mux/twist_mux_diagnostics_status.hpp b/include/twist_mux/twist_mux_diagnostics_status.hpp new file mode 100644 index 0000000..a90a4fc --- /dev/null +++ b/include/twist_mux/twist_mux_diagnostics_status.hpp @@ -0,0 +1,77 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 PAL Robotics S.L. 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 HOLDER 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 Enrique Fernandez + * @author Jeremie Deray + * @author Brighten Lee + */ + +#ifndef TWIST_MUX__TWIST_MUX_DIAGNOSTICS_STATUS_HPP_ +#define TWIST_MUX__TWIST_MUX_DIAGNOSTICS_STATUS_HPP_ + +#include +#include + +#include + +#include + +namespace twist_mux +{ +struct TwistMuxDiagnosticsStatus +{ + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; + + double reading_age; + rclcpp::Time last_loop_update; + double main_loop_time; + + LockTopicHandle::priority_type priority; + + std::shared_ptr velocity_hs; + std::shared_ptr lock_hs; + + TwistMuxDiagnosticsStatus() + : reading_age(0), + last_loop_update(rclcpp::Clock().now()), + main_loop_time(0), + priority(0) + { + velocity_hs = std::make_shared(); + lock_hs = std::make_shared(); + } +}; + +typedef TwistMuxDiagnosticsStatus::Ptr TwistMuxDiagnosticsStatusPtr; +typedef TwistMuxDiagnosticsStatus::ConstPtr TwistMuxDiagnosticsStatusConstPtr; + +} // namespace twist_mux + +#endif // TWIST_MUX__TWIST_MUX_DIAGNOSTICS_STATUS_HPP_ diff --git a/include/twist_mux/utils.h b/include/twist_mux/utils.h deleted file mode 100644 index cef47dc..0000000 --- a/include/twist_mux/utils.h +++ /dev/null @@ -1,44 +0,0 @@ -/********************************************************************* - * Software License Agreement (CC BY-NC-SA 4.0 License) - * - * Copyright (c) 2014, PAL Robotics, S.L. - * All rights reserved. - * - * This work is licensed under the Creative Commons - * Attribution-NonCommercial-ShareAlike 4.0 International License. - * - * To view a copy of this license, visit - * http://creativecommons.org/licenses/by-nc-sa/4.0/ - * or send a letter to - * Creative Commons, 444 Castro Street, Suite 900, - * Mountain View, California, 94041, USA. - *********************************************************************/ - -/* - * @author Enrique Fernandez - * @author Siegfried Gevatter - */ - -#ifndef UTILS_H -#define UTILS_H - -// This could be taken from #include -// but it seems that all versions of Boost have it. - -/** - * @brief Clamp a value to the range [min, max] - * @param x Value - * @param min Min value of the range [min, max] - * @param max Max value of the range [min, max] - * @return Value clamped to the range [min, max] - */ -template -static T clamp(T x, T min, T max) -{ - if ( x < min) x = min; - else if (max < x) x = max; - return x; -} - -#endif // UTILS_H - diff --git a/include/twist_mux/utils.hpp b/include/twist_mux/utils.hpp new file mode 100644 index 0000000..5f97806 --- /dev/null +++ b/include/twist_mux/utils.hpp @@ -0,0 +1,58 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 PAL Robotics S.L. 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 HOLDER 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 Enrique Fernandez + * @author Siegfried Gevatter + */ + +#ifndef TWIST_MUX__UTILS_HPP_ +#define TWIST_MUX__UTILS_HPP_ + +// This could be taken from #include +// but it seems that all versions of Boost have it. + +/** + * @brief Clamp a value to the range [min, max] + * @param x Value + * @param min Min value of the range [min, max] + * @param max Max value of the range [min, max] + * @return Value clamped to the range [min, max] + */ +template +static T clamp(T x, T min, T max) +{ + if (x < min) { + x = min; + } else if (max < x) { + x = max; + } + return x; +} + +#endif // TWIST_MUX__UTILS_HPP_ diff --git a/include/twist_mux/xmlrpc_helpers.h b/include/twist_mux/xmlrpc_helpers.h deleted file mode 100644 index 3ee0dae..0000000 --- a/include/twist_mux/xmlrpc_helpers.h +++ /dev/null @@ -1,111 +0,0 @@ -/* - * Software License Agreement (Modified BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * 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 PAL Robotics, S.L. 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 Paul Mathieu. */ - -#ifndef XMLRPCHELPERS_H -#define XMLRPCHELPERS_H - -#include -#include - -namespace xh -{ - -class XmlrpcHelperException : public ros::Exception -{ -public: - XmlrpcHelperException(const std::string& what) - : ros::Exception(what) {} -}; - -typedef XmlRpc::XmlRpcValue Struct; -typedef XmlRpc::XmlRpcValue Array; - -template -void fetchParam(ros::NodeHandle nh, const std::string& param_name, T& output) -{ - XmlRpc::XmlRpcValue val; - if (!nh.getParamCached(param_name, val)) - { - std::ostringstream err_msg; - err_msg << "could not load parameter '" << param_name << "'. (namespace: " - << nh.getNamespace() << ")"; - throw XmlrpcHelperException(err_msg.str()); - } - - output = static_cast(val); -} - -void checkArrayItem(const Array& col, int index) -{ - if (col.getType() != XmlRpc::XmlRpcValue::TypeArray) - throw XmlrpcHelperException("not an array"); - if(index >= col.size()) - { - std::ostringstream err_msg; - err_msg << "index '" << index << "' is over array capacity"; - throw XmlrpcHelperException(err_msg.str()); - } -} - -void checkStructMember(const Struct& col, const std::string& member) -{ - if (col.getType() != XmlRpc::XmlRpcValue::TypeStruct) - throw XmlrpcHelperException("not a struct"); - if (!col.hasMember(member)) - { - std::ostringstream err_msg; - err_msg << "could not find member '" << member << "'"; - throw XmlrpcHelperException(err_msg.str()); - } -} - -template -void getArrayItem(Array& col, int index, T& output) // XXX: XmlRpcValue::operator[] is not const -{ - checkArrayItem(col, index); - output = static_cast(col[index]); -} - -template -void getStructMember(Struct& col, const std::string& member, T& output) -{ - checkStructMember(col, member); - output = static_cast(col[member]); -} - -} // namespace xh - -#endif // XMLRPCHELPERS_H diff --git a/launch/twist_mux_launch.py b/launch/twist_mux_launch.py new file mode 100644 index 0000000..719876b --- /dev/null +++ b/launch/twist_mux_launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +# Copyright 2020 Gaitech Korea Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Author: Brighten Lee + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + default_config_locks = os.path.join(get_package_share_directory('twist_mux'), + 'config', 'twist_mux_locks.yaml') + default_config_topics = os.path.join(get_package_share_directory('twist_mux'), + 'config', 'twist_mux_topics.yaml') + default_config_joystick = os.path.join(get_package_share_directory('twist_mux'), + 'config', 'joystick.yaml') + + return LaunchDescription([ + DeclareLaunchArgument( + 'config_locks', + default_value=default_config_locks, + description='Default locks config file'), + DeclareLaunchArgument( + 'config_topics', + default_value=default_config_topics, + description='Default topics config file'), + DeclareLaunchArgument( + 'config_joy', + default_value=default_config_joystick, + description='Default joystick config file'), + DeclareLaunchArgument( + 'cmd_vel_out', + default_value='twist_mux/cmd_vel', + description='cmd vel output topic'), + Node( + package='twist_mux', + executable='twist_mux', + output='screen', + remappings={('/cmd_vel_out', LaunchConfiguration('cmd_vel_out'))}, + parameters=[ + LaunchConfiguration('config_locks'), + LaunchConfiguration('config_topics'), + LaunchConfiguration('config_joy')] + ), + + Node( + package='twist_mux', + executable='twist_marker', + output='screen', + remappings={('/twist', LaunchConfiguration('cmd_vel_out'))}, + parameters=[{ + 'frame_id': 'base_link', + 'scale': 1.0, + 'vertical_position': 2.0}]) + ]) diff --git a/package.xml b/package.xml index 3ab101e..14f0640 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,6 @@ - + + twist_mux 3.1.0 @@ -8,41 +9,32 @@ Enrique Fernandez + CC BY-NC-SA 4.0 + Brighten Lee + Jeremie Deray Enrique Fernandez Siegfried-A. Gevatter Pujals - CC BY-NC-SA 4.0 - - catkin + ament_cmake - roscpp - std_msgs - geometry_msgs - visualization_msgs - diagnostic_updater + rclcpp + std_msgs + geometry_msgs + visualization_msgs + diagnostic_updater - roscpp - std_msgs - geometry_msgs - visualization_msgs - twist_mux_msgs - diagnostic_updater + ament_lint_auto - - rostest - rospy - rostopic + ament_cmake_xmllint + ament_lint_common + + launch + launch_testing + launch_testing_ament_cmake + launch_testing_ros - - - control - twist - cmd_vel - multiplex - mux - - + ament_cmake diff --git a/scripts/joystick_relay.py b/scripts/joystick_relay.py index 4de298c..9882006 100755 --- a/scripts/joystick_relay.py +++ b/scripts/joystick_relay.py @@ -1,22 +1,39 @@ -#! /usr/bin/python +#! /usr/bin/python3 # -*- coding: utf-8 -*- + # # twist_mux: joystick_relay.py # -# Copyright (c) 2013 PAL Robotics SL. All Rights Reserved +# Copyright (c) 2020 PAL Robotics S.L. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. # # Authors: # * Enrique Fernandez # * Siegfried-A. Gevatter +# * Jeremie Deray + + +import rclpy +import numpy as np -import rospy -import actionlib -from twist_mux_msgs.msg import JoyPriorityAction, JoyTurboAction from geometry_msgs.msg import Twist +from rclpy.action import ActionServer +from rclpy.node import Node from std_msgs.msg import Bool +from twist_mux_msgs.action import JoyPriority, JoyTurbo from visualization_msgs.msg import Marker -import numpy as np class Velocity(object): @@ -26,72 +43,93 @@ def __init__(self, min_velocity, max_velocity, num_steps): self._max = max_velocity self._num_steps = num_steps if self._num_steps > 1: - self._step_incr = (max_velocity - min_velocity) / (self._num_steps - 1) + self._step_incr = (max_velocity - min_velocity) / \ + (self._num_steps - 1) else: # If num_steps is one, we always use the minimum velocity. self._step_incr = 0 def __call__(self, value, step=1): """ - Takes a value in the range [0, 1] and the step and returns the + Compute the velocity. + + Take a value in the range [0, 1] and the step and returns the velocity (usually m/s or rad/s). """ assert step > 0 and step <= self._num_steps max_value = self._min + self._step_incr * (step - 1) return value * max_value + class ServiceLikeActionServer(object): - def __init__(self, action_name, action_type, callback): + def __init__(self, node, action_name, action_type, callback): self._action_type = action_type self._callback = callback - self._server = actionlib.SimpleActionServer(action_name, action_type, - self._cb, False) - self._server.start() + self._server = ActionServer( + node, + action_type, + action_name, + self._cb) def _cb(self, goal): self._callback() - result = self._action_type().action_result - self._server.set_succeeded(result) + goal.succeed() + return self._action_type().Result() + class VelocityControl: - def __init__(self): - self._num_steps = rospy.get_param('~turbo/steps', 1) + def __init__(self, node): + self._node = node + self._num_steps = self._node.declare_parameter('turbo/steps', 1) - forward_min = rospy.get_param('~turbo/linear_forward_min', 1.0) - forward_max = rospy.get_param('~turbo/linear_forward_max', 1.0) + forward_min = self._node.declare_parameter( + 'turbo/linear_forward_min', 1.0) + forward_max = self._node.declare_parameter( + 'turbo/linear_forward_max', 1.0) self._forward = Velocity(forward_min, forward_max, self._num_steps) - backward_min = rospy.get_param('~turbo/linear_backward_min', forward_min) - backward_max = rospy.get_param('~turbo/linear_backward_max', forward_max) + backward_min = self._node.declare_parameter( + 'turbo/linear_backward_min', forward_min) + backward_max = self._node.declare_parameter( + 'turbo/linear_backward_max', forward_max) self._backward = Velocity(backward_min, backward_max, self._num_steps) - lateral_min = rospy.get_param('~turbo/linear_lateral_min', 1.0) - lateral_max = rospy.get_param('~turbo/linear_lateral_max', 1.0) + lateral_min = self._node.declare_parameter( + 'turbo/linear_lateral_min', 1.0) + lateral_max = self._node.declare_parameter( + 'turbo/linear_lateral_max', 1.0) self._lateral = Velocity(lateral_min, lateral_max, self._num_steps) - angular_min = rospy.get_param('~turbo/angular_min', 1.0) - angular_max = rospy.get_param('~turbo/angular_max', 1.0) + angular_min = self._node.declare_parameter('turbo/angular_min', 1.0) + angular_max = self._node.declare_parameter('turbo/angular_max', 1.0) self._angular = Velocity(angular_min, angular_max, self._num_steps) default_init_step = np.floor((self._num_steps + 1)/2.0) - init_step = rospy.get_param('~turbo/init_step', default_init_step) + init_step = self._node.declare_parameter( + 'turbo/init_step', default_init_step) + if init_step < 0 or init_step > self._num_steps: self._init_step = default_init_step - rospy.logwarn('Initial step %d outside range [1, %d]!' - ' Falling back to default %d' % - (init_step, self._num_steps, default_init_step)) + self._node.get_logger().warn('Initial step %d outside range [1, %d]!' + ' Falling back to default %d' % + (init_step, self._num_steps, default_init_step)) else: self._init_step = init_step + self.reset_turbo() def validate_twist(self, cmd): if cmd.linear.z or cmd.angular.x or cmd.angular.y: - rospy.logerr("Joystick provided invalid values, only linear.x, linear.y and angular.z may be non-zero.") + self._node.get_logger().error( + 'Joystick provided invalid values, only linear.x, ' + 'linear.y and angular.z may be non-zero.') return False if abs(cmd.linear.x) > 1.0 or abs(cmd.linear.y) > 1.0 or abs(cmd.angular.z) > 1.0: - rospy.logerr("Joystick provided invalid values (%d, %d, %d), not in [-1, 1] range." % (cmd.linear.x, cmd.linear.y, cmd.angular.z)) + self._node.get_logger().error( + "Joystick provided invalid values (%d, %d, %d), not in [-1, 1] range." + % (cmd.linear.x, cmd.linear.y, cmd.angular.z)) return False return True @@ -99,11 +137,14 @@ def scale_twist(self, cmd): twist = Twist() if self.validate_twist(cmd): if cmd.linear.x >= 0: - twist.linear.x = self._forward(cmd.linear.x, self._current_step) + twist.linear.x = self._forward( + cmd.linear.x, self._current_step) else: - twist.linear.x = self._backward(cmd.linear.x, self._current_step) + twist.linear.x = self._backward( + cmd.linear.x, self._current_step) twist.linear.y = self._lateral(cmd.linear.y, self._current_step) - twist.angular.z = self._angular(cmd.angular.z, self._current_angular_step) + twist.angular.z = self._angular( + cmd.angular.z, self._current_angular_step) return twist def increase_turbo(self): @@ -128,13 +169,15 @@ def reset_turbo(self): self._current_step = self._init_step self._current_angular_step = self._init_step + class TextMarker(object): - def __init__(self, scale = 1.0, z = 0.0): - self._pub = rospy.Publisher('text_marker', Marker, queue_size=1, latch=True) + def __init__(self, node, scale=1.0, z=0.0): + # TODO latch + self._pub = node.create_publisher(Marker, 'text_marker', 1) self._scale = scale - self._z = z + self._z = z # Build marker self._marker = Marker() @@ -155,50 +198,61 @@ def __init__(self, scale = 1.0, z = 0.0): self._marker.color.g = 1.0 self._marker.color.b = 1.0 - def update(self, joystick_priority, add = True): + def update(self, joystick_priority, add=True): if add: - self._marker.action = Marker.ADD + self._marker.action = Marker.ADD self._marker.text = "Manual" if joystick_priority else "Autonomous" else: - self._marker.action = Marker.DELETE + self._marker.action = Marker.DELETE self._pub.publish(self._marker) -class JoystickRelay: + +class JoystickRelay(Node): def __init__(self): - self._current_priority = rospy.get_param("~priority", True) + super().__init__('joystick_relay') + + self._current_priority = self.declare_parameter('priority', True) self._velocity_control = VelocityControl() - self._marker = TextMarker(0.5, 2.0) + self._marker = TextMarker(self, 0.5, 2.0) - self._pub_cmd = rospy.Publisher('joy_vel_out', Twist, queue_size=1) - self._subscriber = rospy.Subscriber('joy_vel_in', Twist, self._forward_cmd, queue_size=1) + self._pub_cmd = self.create_publisher(Twist, 'joy_vel_out', 1) + self._subscriber = self.create_subscription( + Twist, 'joy_vel_in', self._forward_cmd, 1) - self._pub_priority = rospy.Publisher('joy_priority', Bool, queue_size=1, latch=True) + # TODO Latch + self._pub_priority = self.create_publisher(Bool, 'joy_priority', 1) # Wait for subscribers and publish initial joy_priority: self._pub_priority.publish(self._current_priority) self._marker.update(self._current_priority) # Marker timer (required to update the marker when the robot doesn't receive velocities): - rospy.Timer(rospy.Duration(1.0), self._timer_callback) + self._timer = self.create_timer(1.0, self._timer_callback) # Action servers to change priority & the currently active turbo step. # We aren't using services because they aren't supported by joy_teleop. - self._server_priority = ServiceLikeActionServer('joy_priority_action', JoyPriorityAction, - self._toggle_priority) - self._server_increase = ServiceLikeActionServer('joy_turbo_increase', JoyTurboAction, - self._velocity_control.increase_turbo) - self._server_decrease = ServiceLikeActionServer('joy_turbo_decrease', JoyTurboAction, - self._velocity_control.decrease_turbo) - self._server_angular_increase = ServiceLikeActionServer('joy_turbo_angular_increase', JoyTurboAction, - self._velocity_control.increase_angular_turbo) - self._server_angular_decrease = ServiceLikeActionServer('joy_turbo_angular_decrease', JoyTurboAction, - self._velocity_control.decrease_angular_turbo) - self._server_reset = ServiceLikeActionServer('joy_turbo_reset', JoyTurboAction, - self._velocity_control.reset_turbo) + self._server_priority = ServiceLikeActionServer( + self, 'joy_priority_action', JoyPriority, + self._toggle_priority) + self._server_increase = ServiceLikeActionServer( + self, 'joy_turbo_increase', JoyTurbo, + self._velocity_control.increase_turbo) + self._server_decrease = ServiceLikeActionServer( + self, 'joy_turbo_decrease', JoyTurbo, + self._velocity_control.decrease_turbo) + self._server_angular_increase = ServiceLikeActionServer( + self, 'joy_turbo_angular_increase', JoyTurbo, + self._velocity_control.increase_angular_turbo) + self._server_angular_decrease = ServiceLikeActionServer( + self, 'joy_turbo_angular_decrease', JoyTurbo, + self._velocity_control.decrease_angular_turbo) + self._server_reset = ServiceLikeActionServer( + self, 'joy_turbo_reset', JoyTurbo, + self._velocity_control.reset_turbo) def _forward_cmd(self, cmd): if self._current_priority: @@ -208,7 +262,8 @@ def _forward_cmd(self, cmd): def _toggle_priority(self): self._current_priority = not self._current_priority - rospy.loginfo("Toggled joy_priority, current status is: %s", self._current_priority) + self.get_logger().info("Toggled joy_priority, current status is: %s", + self._current_priority) self._pub_priority.publish(self._current_priority) self._marker.update(self._current_priority) @@ -219,9 +274,17 @@ def _toggle_priority(self): def _timer_callback(self, event): self._marker.update(self._current_priority) -if __name__ == '__main__': - rospy.init_node('joystick_relay') - server = JoystickRelay() +def main(args=None): + rclpy.init(args=args) - rospy.spin() + node = JoystickRelay() + + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/twist_marker.cpp b/src/twist_marker.cpp index 14ecfa1..b62b092 100644 --- a/src/twist_marker.cpp +++ b/src/twist_marker.cpp @@ -1,42 +1,54 @@ -/********************************************************************* - * Software License Agreement (CC BY-NC-SA 4.0 License) - * - * Copyright (c) 2014, PAL Robotics, S.L. - * All rights reserved. - * - * This work is licensed under the Creative Commons - * Attribution-NonCommercial-ShareAlike 4.0 International License. - * - * To view a copy of this license, visit - * http://creativecommons.org/licenses/by-nc-sa/4.0/ - * or send a letter to - * Creative Commons, 444 Castro Street, Suite 900, - * Mountain View, California, 94041, USA. - *********************************************************************/ +// Copyright 2020 PAL Robotics S.L. +// +// 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 PAL Robotics S.L. 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 HOLDER 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 Enrique Fernandez + * @author Jeremie Deray + * @author Brighten Lee */ -#include -#include -#include -#include +#include +#include +#include +#include +#include #include class TwistMarker { public: - - TwistMarker(double scale = 1.0, double z = 0.0, const std::string& frame_id = "base_footprint") - : frame_id_(frame_id) - , scale_(scale) - , z_(z) + TwistMarker(std::string & frame_id, double scale, double z) + : frame_id_(frame_id), scale_(scale), z_(z) { // ID and type: marker_.id = 0; - marker_.type = visualization_msgs::Marker::ARROW; + marker_.type = visualization_msgs::msg::Marker::ARROW; // Frame ID: marker_.header.frame_id = frame_id_; @@ -56,74 +68,90 @@ class TwistMarker marker_.color.r = 0.0; marker_.color.g = 1.0; marker_.color.b = 0.0; + + // Error when all points are zero: + marker_.points[1].z = 0.01; } - void update(const geometry_msgs::Twist& twist) + void update(const geometry_msgs::msg::Twist & twist) { + using std::abs; + marker_.points[1].x = twist.linear.x; - if (fabs(twist.linear.y) > fabs(twist.angular.z)) - { + if (abs(twist.linear.y) > abs(twist.angular.z)) { marker_.points[1].y = twist.linear.y; - } - else - { + } else { marker_.points[1].y = twist.angular.z; } } - const visualization_msgs::Marker& getMarker() + const visualization_msgs::msg::Marker & getMarker() { return marker_; } private: - visualization_msgs::Marker marker_; + visualization_msgs::msg::Marker marker_; std::string frame_id_; double scale_; double z_; }; -class TwistMarkerPublisher +class TwistMarkerPublisher : public rclcpp::Node { public: - - TwistMarkerPublisher(double scale = 1.0, double z = 0.0) - : marker_(scale, z) + TwistMarkerPublisher() + : Node("twist_marker") { - ros::NodeHandle nh; + std::string frame_id; + double scale; + double z; + + this->declare_parameter("frame_id"); + this->declare_parameter("scale"); + this->declare_parameter("vertical_position"); + + this->get_parameter_or("frame_id", frame_id, "base_footprint"); + this->get_parameter_or("scale", scale, 1.0); + this->get_parameter_or("vertical_position", z, 2.0); - pub_ = nh.advertise("marker", 1, true); - sub_ = nh.subscribe("twist", 1, &TwistMarkerPublisher::callback, this); + marker_ = std::make_shared(frame_id, scale, z); + + sub_ = this->create_subscription( + "twist", rclcpp::SystemDefaultsQoS(), + std::bind(&TwistMarkerPublisher::callback, this, std::placeholders::_1)); + + pub_ = + this->create_publisher( + "marker", + rclcpp::QoS(rclcpp::KeepLast(1))); } - void callback(const geometry_msgs::TwistConstPtr& twist) + void callback(const geometry_msgs::msg::Twist::ConstSharedPtr twist) { - marker_.update(*twist); + marker_->update(*twist); - pub_.publish(marker_.getMarker()); + pub_->publish(marker_->getMarker()); } private: - ros::Subscriber sub_; - ros::Publisher pub_; + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr pub_; - TwistMarker marker_; + std::shared_ptr marker_ = nullptr; }; -int -main(int argc, char *argv[]) +int main(int argc, char * argv[]) { - ros::init(argc, argv, "twist_marker"); + rclcpp::init(argc, argv); - TwistMarkerPublisher t(1.0, 2.0); + auto twist_mux_node = std::make_shared(); - while (ros::ok()) - { - ros::spin(); - } + rclcpp::spin(twist_mux_node); + + rclcpp::shutdown(); return EXIT_SUCCESS; } - diff --git a/src/twist_mux.cpp b/src/twist_mux.cpp index 4a67ea4..fbc056e 100644 --- a/src/twist_mux.cpp +++ b/src/twist_mux.cpp @@ -1,30 +1,47 @@ -/********************************************************************* - * Software License Agreement (CC BY-NC-SA 4.0 License) - * - * Copyright (c) 2014, PAL Robotics, S.L. - * All rights reserved. - * - * This work is licensed under the Creative Commons - * Attribution-NonCommercial-ShareAlike 4.0 International License. - * - * To view a copy of this license, visit - * http://creativecommons.org/licenses/by-nc-sa/4.0/ - * or send a letter to - * Creative Commons, 444 Castro Street, Suite 900, - * Mountain View, California, 94041, USA. - *********************************************************************/ +// Copyright 2020 PAL Robotics S.L. +// +// 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 PAL Robotics S.L. 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 HOLDER 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 Enrique Fernandez * @author Siegfried Gevatter + * @author Jeremie Deray */ -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include /** * @brief hasIncreasedAbsVelocity Check if the absolute velocity has increased @@ -33,7 +50,9 @@ * @param new_twist New velocity * @return true is any of the absolute velocity components has increased */ -bool hasIncreasedAbsVelocity(const geometry_msgs::Twist& old_twist, const geometry_msgs::Twist& new_twist) +bool hasIncreasedAbsVelocity( + const geometry_msgs::msg::Twist & old_twist, + const geometry_msgs::msg::Twist & new_twist) { const auto old_linear_x = std::abs(old_twist.linear.x); const auto new_linear_x = std::abs(new_twist.linear.x); @@ -41,77 +60,88 @@ bool hasIncreasedAbsVelocity(const geometry_msgs::Twist& old_twist, const geomet const auto old_angular_z = std::abs(old_twist.angular.z); const auto new_angular_z = std::abs(new_twist.angular.z); - return (old_linear_x < new_linear_x ) or - (old_angular_z < new_angular_z); + return (old_linear_x < new_linear_x) || (old_angular_z < new_angular_z); } namespace twist_mux { +// see e.g. https://stackoverflow.com/a/40691657 +constexpr std::chrono::duration TwistMux::DIAGNOSTICS_PERIOD; TwistMux::TwistMux(int window_size) +: Node("twist_mux", "", + rclcpp::NodeOptions().allow_undeclared_parameters( + true).automatically_declare_parameters_from_overrides(true)) { - ros::NodeHandle nh; - ros::NodeHandle nh_priv("~"); +} +void TwistMux::init() +{ /// Get topics and locks: - velocity_hs_ = boost::make_shared(); - lock_hs_ = boost::make_shared(); - getTopicHandles(nh, nh_priv, "topics", *velocity_hs_); - getTopicHandles(nh, nh_priv, "locks" , *lock_hs_ ); + velocity_hs_ = std::make_shared(); + lock_hs_ = std::make_shared(); + getTopicHandles("topics", *velocity_hs_); + getTopicHandles("locks", *lock_hs_); /// Publisher for output topic: - cmd_pub_ = nh.advertise("cmd_vel_out", 1); + cmd_pub_ = + this->create_publisher( + "cmd_vel_out", + rclcpp::QoS(rclcpp::KeepLast(1))); /// Diagnostics: - diagnostics_ = boost::make_shared(); - status_ = boost::make_shared(); + diagnostics_ = std::make_shared(this); + status_ = std::make_shared(); status_->velocity_hs = velocity_hs_; - status_->lock_hs = lock_hs_; + status_->lock_hs = lock_hs_; - diagnostics_timer_ = nh.createTimer(ros::Duration(DIAGNOSTICS_PERIOD), &TwistMux::updateDiagnostics, this); + diagnostics_timer_ = this->create_wall_timer( + DIAGNOSTICS_PERIOD, [this]() -> void { + updateDiagnostics(); + }); } -TwistMux::~TwistMux() -{} - -void TwistMux::updateDiagnostics(const ros::TimerEvent& event) +void TwistMux::updateDiagnostics() { status_->priority = getLockPriority(); diagnostics_->updateStatus(status_); } -void TwistMux::publishTwist(const geometry_msgs::TwistConstPtr& msg) +void TwistMux::publishTwist(const geometry_msgs::msg::Twist::ConstSharedPtr & msg) { - cmd_pub_.publish(*msg); + cmd_pub_->publish(*msg); } template -void TwistMux::getTopicHandles(ros::NodeHandle& nh, ros::NodeHandle& nh_priv, const std::string& param_name, std::list& topic_hs) +void TwistMux::getTopicHandles(const std::string & param_name, std::list & topic_hs) { - try - { - xh::Array output; - xh::fetchParam(nh_priv, param_name, output); - - xh::Struct output_i; - std::string name, topic; - double timeout; - int priority; - for (int i = 0; i < output.size(); ++i) - { - xh::getArrayItem(output, i, output_i); - - xh::getStructMember(output_i, "name" , name ); - xh::getStructMember(output_i, "topic" , topic ); - xh::getStructMember(output_i, "timeout" , timeout ); - xh::getStructMember(output_i, "priority", priority); - - topic_hs.emplace_back(nh, name, topic, timeout, priority, this); + RCLCPP_DEBUG(get_logger(), "getTopicHandles: %s", param_name.c_str()); + + rcl_interfaces::msg::ListParametersResult list = list_parameters({param_name}, 10); + + try { + for (auto prefix : list.prefixes) { + RCLCPP_DEBUG(get_logger(), "Prefix: %s", prefix.c_str()); + + std::string topic; + double timeout = 0; + int priority = 0; + + auto nh = std::shared_ptr(this, [](rclcpp::Node *) {}); + + fetch_param(nh, prefix + ".topic", topic); + fetch_param(nh, prefix + ".timeout", timeout); + fetch_param(nh, prefix + ".priority", priority); + + RCLCPP_DEBUG(get_logger(), "Retrieved topic: %s", topic.c_str()); + RCLCPP_DEBUG(get_logger(), "Listed prefix: %.2f", timeout); + RCLCPP_DEBUG(get_logger(), "Listed prefix: %d", priority); + + topic_hs.emplace_back(prefix, topic, std::chrono::duration(timeout), priority, this); } - } - catch (const xh::XmlrpcHelperException& e) - { - ROS_FATAL_STREAM("Error parsing params: " << e.what()); + } catch (const ParamsHelperException & e) { + RCLCPP_FATAL(get_logger(), "Error parsing params '%s':\n\t%s", param_name.c_str(), e.what()); + throw e; } } @@ -121,24 +151,21 @@ int TwistMux::getLockPriority() /// max_element on the priority of lock topic handles satisfying /// that is locked: - for (const auto& lock_h : *lock_hs_) - { - if (lock_h.isLocked()) - { + for (const auto & lock_h : *lock_hs_) { + if (lock_h.isLocked()) { auto tmp = lock_h.getPriority(); - if (priority < tmp) - { + if (priority < tmp) { priority = tmp; } } } - ROS_DEBUG_STREAM("Priority = " << static_cast(priority)); + RCLCPP_DEBUG(get_logger(), "Priority = %d.", static_cast(priority)); return priority; } -bool TwistMux::hasPriority(const VelocityTopicHandle& twist) +bool TwistMux::hasPriority(const VelocityTopicHandle & twist) { const auto lock_priority = getLockPriority(); @@ -147,13 +174,10 @@ bool TwistMux::hasPriority(const VelocityTopicHandle& twist) /// max_element on the priority of velocity topic handles satisfying /// that is NOT masked by the lock priority: - for (const auto& velocity_h : *velocity_hs_) - { - if (not velocity_h.isMasked(lock_priority)) - { + for (const auto & velocity_h : *velocity_hs_) { + if (!velocity_h.isMasked(lock_priority)) { const auto velocity_priority = velocity_h.getPriority(); - if (priority < velocity_priority) - { + if (priority < velocity_priority) { priority = velocity_priority; velocity_name = velocity_h.getName(); } @@ -163,4 +187,4 @@ bool TwistMux::hasPriority(const VelocityTopicHandle& twist) return twist.getName() == velocity_name; } -} // namespace twist_mux +} // namespace twist_mux diff --git a/src/twist_mux_diagnostics.cpp b/src/twist_mux_diagnostics.cpp index 9d311ab..b61ce2b 100644 --- a/src/twist_mux_diagnostics.cpp +++ b/src/twist_mux_diagnostics.cpp @@ -1,95 +1,103 @@ -/********************************************************************* - * Software License Agreement (CC BY-NC-SA 4.0 License) - * - * Copyright (c) 2014, PAL Robotics, S.L. - * All rights reserved. - * - * This work is licensed under the Creative Commons - * Attribution-NonCommercial-ShareAlike 4.0 International License. - * - * To view a copy of this license, visit - * http://creativecommons.org/licenses/by-nc-sa/4.0/ - * or send a letter to - * Creative Commons, 444 Castro Street, Suite 900, - * Mountain View, California, 94041, USA. - *********************************************************************/ +// Copyright 2020 PAL Robotics S.L. +// +// 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 PAL Robotics S.L. 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 HOLDER 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 Enrique Fernandez + * @author Brighten Lee */ -#include -#include +#include +#include + +#include -#include +#include namespace twist_mux { - -TwistMuxDiagnostics::TwistMuxDiagnostics() +TwistMuxDiagnostics::TwistMuxDiagnostics(TwistMux * mux) { - diagnostic_.add("Twist mux status", this, &TwistMuxDiagnostics::diagnostics); - diagnostic_.setHardwareID("none"); -} + diagnostic_ = std::make_shared(mux); + status_ = std::make_shared(); -TwistMuxDiagnostics::~TwistMuxDiagnostics() -{} + diagnostic_->add("Twist mux status", this, &TwistMuxDiagnostics::diagnostics); + diagnostic_->setHardwareID("none"); +} void TwistMuxDiagnostics::update() { - diagnostic_.update(); + diagnostic_->force_update(); } -void TwistMuxDiagnostics::updateStatus(const status_type::ConstPtr& status) +void TwistMuxDiagnostics::updateStatus(const status_type::ConstPtr & status) { - ROS_DEBUG_THROTTLE(1.0, "Updating status."); - - status_.velocity_hs = status->velocity_hs; - status_.lock_hs = status->lock_hs; - status_.priority = status->priority; + status_->velocity_hs = status->velocity_hs; + status_->lock_hs = status->lock_hs; + status_->priority = status->priority; - status_.main_loop_time = status->main_loop_time; - status_.reading_age = status->reading_age; + status_->main_loop_time = status->main_loop_time; + status_->reading_age = status->reading_age; update(); } -void TwistMuxDiagnostics::diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) +void TwistMuxDiagnostics::diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) { /// Check if the loop period is quick enough - if (status_.main_loop_time > MAIN_LOOP_TIME_MIN) + if (status_->main_loop_time > MAIN_LOOP_TIME_MIN) { stat.summary(ERROR, "loop time too long"); - else if (status_.reading_age > READING_AGE_MIN) + } else if (status_->reading_age > READING_AGE_MIN) { stat.summary(ERROR, "data received is too old"); - else + } else { stat.summary(OK, "ok"); - - for (const auto& velocity_h : *status_.velocity_hs) - { - stat.addf("velocity " + velocity_h.getName(), - " %s (listening to %s @ %fs with priority #%d)", - (velocity_h.isMasked(status_.priority) ? "masked" : "unmasked"), - velocity_h.getTopic().c_str(), - velocity_h.getTimeout(), - static_cast(velocity_h.getPriority())); } - for (const auto& lock_h : *status_.lock_hs) - { - stat.addf("lock " + lock_h.getName(), - " %s (listening to %s @ %fs with priority #%d)", - (lock_h.isLocked() ? "locked" : "free"), - lock_h.getTopic().c_str(), - lock_h.getTimeout(), - static_cast(lock_h.getPriority())); + for (auto & velocity_h : *status_->velocity_hs) { + stat.addf( + "velocity " + velocity_h.getName(), " %s (listening to %s @ %fs with priority #%d)", + (velocity_h.isMasked(status_->priority) ? "masked" : "unmasked"), + velocity_h.getTopic().c_str(), + velocity_h.getTimeout().seconds(), static_cast(velocity_h.getPriority())); } - stat.add("current priority", static_cast(status_.priority)); + for (const auto & lock_h : *status_->lock_hs) { + stat.addf( + "lock " + lock_h.getName(), " %s (listening to %s @ %fs with priority #%d)", + (lock_h.isLocked() ? "locked" : "free"), lock_h.getTopic().c_str(), + lock_h.getTimeout().seconds(), + static_cast(lock_h.getPriority())); + } - stat.add("loop time in [sec]", status_.main_loop_time); - stat.add("data age in [sec]", status_.reading_age); + stat.add("current priority", static_cast(status_->priority)); - ROS_DEBUG_THROTTLE(1.0, "Publishing diagnostics."); + stat.add("loop time in [sec]", status_->main_loop_time); + stat.add("data age in [sec]", status_->reading_age); } -} // namespace twist_mux +} // namespace twist_mux diff --git a/src/twist_mux_node.cpp b/src/twist_mux_node.cpp index f38b731..b3bcfd9 100644 --- a/src/twist_mux_node.cpp +++ b/src/twist_mux_node.cpp @@ -1,39 +1,52 @@ -/********************************************************************* - * Software License Agreement (CC BY-NC-SA 4.0 License) - * - * Copyright (c) 2014, PAL Robotics, S.L. - * All rights reserved. - * - * This work is licensed under the Creative Commons - * Attribution-NonCommercial-ShareAlike 4.0 International License. - * - * To view a copy of this license, visit - * http://creativecommons.org/licenses/by-nc-sa/4.0/ - * or send a letter to - * Creative Commons, 444 Castro Street, Suite 900, - * Mountain View, California, 94041, USA. - *********************************************************************/ +// Copyright 2020 PAL Robotics S.L. +// +// 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 PAL Robotics S.L. 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 HOLDER 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 Enrique Fernandez * @author Siegfried Gevatter + * @author Jeremie Deray */ -#include -#include +#include -int -main(int argc, char *argv[]) +#include + +int main(int argc, char * argv[]) { - ros::init(argc, argv, "twist_mux"); + rclcpp::init(argc, argv); + + auto twist_mux_node = std::make_shared(); - twist_mux::TwistMux mux; + twist_mux_node->init(); - while (ros::ok()) - { - ros::spin(); - } + rclcpp::spin(twist_mux_node); + + rclcpp::shutdown(); return EXIT_SUCCESS; } - diff --git a/test/rate_publishers.py b/test/rate_publishers.py index 88c9fbd..32868eb 100644 --- a/test/rate_publishers.py +++ b/test/rate_publishers.py @@ -1,41 +1,45 @@ #!/usr/bin/env python -# -*- coding: utf-8 -*- +# Copyright (c) 2020 PAL Robotics S.L. All rights reserved. # -# twist_mux: rate_publishers.py +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at # -# Copyright (c) 2014 PAL Robotics SL. -# All Rights Reserved -# -# Permission to use, copy, modify, and/or distribute this software for -# any purpose with or without fee is hereby granted, provided that the -# above copyright notice and this permission notice appear in all -# copies. -# -# THE SOFTWARE IS PROVIDED "AS IS" AND ISC DISCLAIMS ALL WARRANTIES WITH -# REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF -# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL ISC BE LIABLE FOR ANY -# SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES -# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN -# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT -# OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +# http://www.apache.org/licenses/LICENSE-2.0 # +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + # Authors: # * Siegfried-A. Gevatter import sys -import thread +import _thread +from time import sleep + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSDurabilityPolicy -import rospy -class _RatePublisher(object): +class _RatePublisher(Node): # How many seconds before the expected time a message may # be published. _tolerance = 0.01 - def __init__(self, topic, msg_type, latch=False): + def __init__(self, topic, msg_type, context, latch=False): + super().__init__('rate_publisher_'+topic, context=context) self._topic = topic - self._publisher = rospy.Publisher(topic, msg_type, latch=True) + latching_qos = QoSProfile( + depth=1, + durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) + self._publisher = self.create_publisher( + msg_type, topic, qos_profile=latching_qos) self._message = None self._period = None # 1 / freq self._last_pub = 0 @@ -51,18 +55,18 @@ def stop(self): def publish_once(self): msg = self._message() if callable(self._message) else self._message self._publisher.publish(msg) - self._last_pub = rospy.Time.now() + self._last_pub = self.get_clock().now() def spin_once(self): """ - Gives _RatePublisher a chance to publish a stored message. + Give _RatePublisher a chance to publish a stored message. This method returns the remaining time until the next scheduled - publication (or None). + publication (or -1). """ if not self._period: - return None - elapsed = (rospy.Time.now() - self._last_pub).to_sec() + return -1 + elapsed = (self.get_clock().now() - self._last_pub).nanoseconds / 1e9 if elapsed >= (self._period - self._tolerance): self.publish_once() return self._period @@ -71,18 +75,18 @@ def spin_once(self): class RatePublishers(object): """ - A class for managing several ROS publishers repeating messages - with different rates. + Manages several ROS publishers repeating messages with different rates. The main purpose of this class is for unit testing. """ - def __init__(self): + def __init__(self, context): self._publishers = {} + self._context = context def add_topic(self, topic, msg_type): """ - Adds a topic for future publication. + Add a topic for future publication. This creates a rospy.Publisher internally. Note that the publisher will latch the topic; if that wasn't the case, @@ -91,13 +95,14 @@ def add_topic(self, topic, msg_type): connect. """ assert topic not in self._publishers - rate_publisher = _RatePublisher(topic, msg_type, latch=True) + rate_publisher = _RatePublisher( + topic, msg_type, self._context, latch=True) self._publishers[topic] = rate_publisher return rate_publisher def pub(self, topic, message, rate=None): """ - Publishes `message' on the given topic. + Publish `message' on the given topic. If `rate' is not None, the message will be repeated at the given rate (expected to be in Hz) until pub() or stop() @@ -109,22 +114,21 @@ def pub(self, topic, message, rate=None): self._publishers[topic].pub(message, rate) def stop(self, topic): - """ - Stops repeating any message on the given topic. - """ + """Stop repeating any message on the given topic.""" self._publishers[topic].stop() def spin_once(self): """ - Publishes any scheduled messages and returns the amount of - time until it should be called again. + Publish scheduled messages. + + Return the amount of time until it should be called again. """ # TODO: Create a class that spawns a global thread and provides # createTimer and createWallTimer, just like NodeHandle # does in rospy? - next_timeout = sys.maxint - for topic in self._publishers.itervalues(): - next_timeout = min(topic.spin_once(), next_timeout) + next_timeout = sys.maxsize + for topic, pub in self._publishers.items(): + next_timeout = min(pub.spin_once(), next_timeout) return next_timeout @@ -139,17 +143,18 @@ def add(self, m): self._members.append(m) def spin(self): - while not rospy.core.is_shutdown() and not self._shutdown: + while rclpy.ok() and not self._shutdown: try: for m in self._members: m.spin_once() - rospy.sleep(0.01) # FIXME - except Exception, e: - rospy.logfatal(e) + sleep(0.01) # FIXME + except Exception as e: + print(str(e)) + raise e def spin_thread(self): - rospy.loginfo("Spawning thread for TopicTestManager...") - thread.start_new_thread(self.spin, ()) + print("Spawning thread for TopicTestManager...") + _thread.start_new_thread(self.spin, ()) def shutdown(self): self._shutdown = True diff --git a/test/system.test.py b/test/system.test.py new file mode 100644 index 0000000..a8ea4a9 --- /dev/null +++ b/test/system.test.py @@ -0,0 +1,211 @@ +# Copyright 2019 Canonical, Ltd +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from rate_publishers import RatePublishers, TimeoutManager +import os +import unittest +import sys + +import launch +from launch.actions.execute_process import ExecuteProcess + +import launch_ros.actions + +import launch_testing + +import time +import threading +from rclpy.executors import MultiThreadedExecutor + +import rclpy + +from std_msgs.msg import Bool +from geometry_msgs.msg import Twist + +sys.path.append(os.path.abspath(os.path.dirname(os.path.realpath(__file__)))) + + +def generate_test_description(): + # Necessary to get real-time stdout from python processes: + proc_env = os.environ.copy() + proc_env['PYTHONUNBUFFERED'] = '1' + + dir_path = os.path.dirname(os.path.realpath(__file__)) + + parameters_file = os.path.join( + dir_path, 'system_config.yaml' + ) + + twist_mux = launch_ros.actions.Node( + package='twist_mux', executable='twist_mux', + parameters=[parameters_file], env=proc_env) + + publisher = ExecuteProcess( + cmd=['ros2 topic pub /lock_1 std_msgs/Bool "data: False" -r 20'], + shell=True, env=proc_env + ) + + # system_blackbox = launch_ros.actions.Node( + # package='twist_mux', node_executable='system_blackbox.py', env=proc_env) + + return launch.LaunchDescription([ + twist_mux, + publisher, + # system_blackbox, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ]) + + +def twist(x=0.0, r=0.0): + """Return a Twist for the given linear and rotation speed.""" + t = Twist() + t.linear.x = x + t.angular.z = r + return t + + +class TestTwistMux(unittest.TestCase): + + # Maximum time (in seconds) that it may take for a message + # to be received by the target node. + MESSAGE_TIMEOUT = 0.3 + + # Value (in seconds) >= the highest topic/lock timeout. + TOPIC_TIMEOUT = 1.0 + + @classmethod + def setUpClass(cls): + + cls.context = rclpy.Context() + rclpy.init(context=cls.context) + + cls.node = rclpy.create_node( + 'node', namespace='ns', context=cls.context) + + # Aim at emulating a 'wait_for_msg' + cls._subscription = cls.node.create_subscription( + Twist, 'cmd_vel_out', cls._cb, 1) + cls._msg = None + + cls.executor = MultiThreadedExecutor( + context=cls.context, num_threads=2) + cls.executor.add_node(cls.node) + + cls._publishers = RatePublishers(cls.context) + cls._vel1 = cls._publishers.add_topic('vel_1', Twist) + cls._vel2 = cls._publishers.add_topic('vel_2', Twist) + cls._vel3 = cls._publishers.add_topic('vel_3', Twist) + + cls._lock1 = cls._publishers.add_topic('lock_1', Bool) + cls._lock2 = cls._publishers.add_topic('lock_2', Bool) + + cls.executor.add_node(cls._vel1) + cls.executor.add_node(cls._vel2) + cls.executor.add_node(cls._vel3) + cls.executor.add_node(cls._lock1) + cls.executor.add_node(cls._lock2) + + cls._timeout_manager = TimeoutManager() + cls._timeout_manager.add(cls._publishers) + cls._timeout_manager.spin_thread() + + cls.exec_thread = threading.Thread(target=cls.executor.spin) + cls.exec_thread.start() + + def _cb(self, msg): + self._msg = msg + + def _wait(self, timeout): + start = self.node.get_clock().now() + self._msg = None + while (timeout > ((self.node.get_clock().now() - start).nanoseconds / 1e9)): + if self._msg is not None: + return self._msg + time.sleep(0.01) + return self._msg + + def tearDown(self): + # Reset all topics. + twist_msg = twist(0.0, 0.0) + unlock = Bool() + unlock.data = False + + self._vel1.pub(twist_msg) + self._vel2.pub(twist_msg) + self._vel3.pub(twist_msg) + + self._lock1.pub(unlock) + self._lock2.pub(unlock) + + # Wait for previously published messages to time out, + # since we aren't restarting twist_mux. + # + # This sleeping time must be higher than any of the + # timeouts in system_test_config.yaml. + # + # TODO(artivis) use rate once available + time.sleep(self.MESSAGE_TIMEOUT + self.TOPIC_TIMEOUT) + + self.node.destroy_node() + rclpy.shutdown(context=self.context) + + @classmethod + def _vel_cmd(cls): + # TODO(artivis) use rate once available + time.sleep(cls.MESSAGE_TIMEOUT) + # TODO wait_for_msg-like functionnality not yet available + # https://github.com/ros2/rclcpp/issues/520 + return cls._wait(cls, cls.MESSAGE_TIMEOUT) + + def test_empty(self): + try: + self._vel_cmd() + self.fail('twist_mux should not be publishing without any input') + except Exception: + e = sys.exc_info()[0] + print(e) + pass + + def test_basic(self): + t = twist(2.0) + self._vel1.pub(t, rate=5) + self.assertEqual(t, self._vel_cmd()) + +# def test_basic_with_priorities(self): +# t1 = twist(2.0) +# t2 = twist(0.0, 1.0) + +# # Publish twist from input1 @ 3Hz, it should be used. +# self._vel1.pub(t1, rate=5) +# self.assertEqual(t1, self._vel_cmd()) + +# # Publish twist from input3, it should have priority +# # over the one from input1. +# # self._vel3.pub(t2, rate=10) +# self.assertEqual(t2, self._vel_cmd()) + +# # Stop publishing input 3 and wait for it to timeout. +# # Speed should fall back to input 1. +# # self._vel3.stop() +# time.sleep(0.5) # input is 0.3 in .yaml file +# self.assertEqual(t1, self._vel_cmd()) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + + def test_exit_code(self): + # Check that all processes in the launch exit with code 0 + launch_testing.asserts.assertExitCodes(self.proc_info) diff --git a/test/system_blackbox.py b/test/system_blackbox.py index cc9d1fd..f044c58 100755 --- a/test/system_blackbox.py +++ b/test/system_blackbox.py @@ -3,42 +3,43 @@ # # twist_mux: system_blackbox.py # -# Copyright (c) 2014 PAL Robotics SL. -# All Rights Reserved +# Copyright (c) 2020 PAL Robotics S.L. All rights reserved. # -# Permission to use, copy, modify, and/or distribute this software for -# any purpose with or without fee is hereby granted, provided that the -# above copyright notice and this permission notice appear in all -# copies. -# -# THE SOFTWARE IS PROVIDED "AS IS" AND ISC DISCLAIMS ALL WARRANTIES WITH -# REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF -# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL ISC BE LIABLE FOR ANY -# SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES -# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN -# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT -# OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at # +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + # Authors: # * Siegfried-A. Gevatter import unittest import rospy +import time + from std_msgs.msg import Bool from geometry_msgs.msg import Twist from rate_publishers import RatePublishers, TimeoutManager + def twist(x=0.0, r=0.0): - """ - Returns a Twist for the given linear and rotation speed. - """ + """Return a Twist for the given linear and rotation speed.""" t = Twist() t.linear.x = x t.angular.z = r return t + class TestTwistMux(unittest.TestCase): # Maximum time (in seconds) that it may take for a message @@ -64,26 +65,31 @@ def setUpClass(cls): def tearDown(self): # Reset all topics. - t = twist(0, 0) - l = Bool(False) + twist_msg = twist(0, 0) + unlock = Bool(False) - self._vel1.pub(t) - self._vel2.pub(t) - self._vel3.pub(t) + self._vel1.pub(twist_msg) + self._vel2.pub(twist_msg) + self._vel3.pub(twist_msg) - self._lock1.pub(l) - self._lock2.pub(l) + self._lock1.pub(unlock) + self._lock2.pub(unlock) # Wait for previously published messages to time out, # since we aren't restarting twist_mux. # # This sleeping time must be higher than any of the # timeouts in system_test_config.yaml. - rospy.sleep(self.MESSAGE_TIMEOUT + self.TOPIC_TIMEOUT) + # + # TODO(artivis) use rate once available + time.sleep(self.MESSAGE_TIMEOUT + self.TOPIC_TIMEOUT) @classmethod def _vel_cmd(cls): - rospy.sleep(cls.MESSAGE_TIMEOUT) + # TODO(artivis) use rate once available + time.sleep(cls.MESSAGE_TIMEOUT) + # TODO wait_for_msg-like functionnality not yet available + # https://github.com/ros2/rclcpp/issues/520 return rospy.wait_for_message('cmd_vel_out', Twist, timeout=cls.MESSAGE_TIMEOUT) @@ -120,6 +126,7 @@ def test_basic_with_priorities(self): # TODO: test limits, test timeouts, etc. + if __name__ == '__main__': import rostest PKG_NAME = 'twist_mux' diff --git a/test/system_config.yaml b/test/system_config.yaml index a65283b..278e6b4 100644 --- a/test/system_config.yaml +++ b/test/system_config.yaml @@ -1,23 +1,24 @@ -topics: -- name : input1 - topic : vel_1 - timeout : 0.7 - priority: 5 -- name : input2 - topic : vel_2 - timeout : 0.0 - priority: 1 -- name : input3 - topic : vel_3 - timeout : 0.3 - priority: 10 - -locks: -- name : emergency_255 - topic : lock_1 - timeout : 0.5 - priority: 255 -- name : emergency_10 - topic : lock_2 - timeout : 0.0 - priority: 10 +twist_mux: + ros__parameters: + topics: + input1: + topic : vel_1 + timeout : 0.7 + priority: 5 + input2: + topic : vel_2 + timeout : 0.0 + priority: 1 + input3: + topic : vel_3 + timeout : 0.3 + priority: 10 + locks: + emergency_255: + topic : lock_1 + timeout : 0.5 + priority: 255 + emergency_10: + topic : lock_2 + timeout : 0.0 + priority: 10