From 2b682ab12f78a7efdee8c3085093a54a95855afe Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 30 Aug 2019 13:07:05 -0700 Subject: [PATCH] Port to ROS 2 (#44) * Rename headers Now with .hpp suffix * Add linter tests * Uncrustify * Lint * Do not run flake8 linter test * Add LICENSE and CONTRIBUTING.md This satisfies the ament_copyright linter. * Fix deprecation warning * Use rcutils log macro instead of custom macro * Use tf2::BufferCoreInterface Using this abstract interface enables use of the interactive marker client with other buffer implementations. Also, switched to using shared pointers to avoid potential dangling reference. * Replace 'init' topic with a ROS service It seems more intuitive for clients to call a service to get the latest state for interactive markers, versus subscribing to a latched topic and waiting for the server to publish. * Add tests for InteractiveMarkerServer * Add getter for client state Useful for introspection, especially during testing. * Add tests for InteractiveMarkerClient * Merge SingleClient logic into InteractiveMarkerClient. Now it is assumed that there is maximum one server per client. This helps simplify the implementation. * Update doc-block comment style * Renaming of variables for clarity * Remove old test code and unused single_client.hpp * Refactor tools docs * Remove notion of server ID * Refactor interactive marker client docs * Update copyright license * Make shared library * Add feedback publisher to client * Implement client request timeout If a response for the request isn't received within the provided timeout, then try again. * Add logic for 'extrapolation into future' error Rename custom exception and throw it in replace of tf2 exceptions. * Default to C++14 and set stricter compiler flags Fix compiler warnings * Fix Windows compiler warnings * Remove StateMachine class The functionality it provided is not needed. * Fix Clang warnings * Move message context header into same directory as other headers * Use visibility macros --- CMakeLists.txt | 133 ++- CONTRIBUTING.md | 3 + LICENSE | 32 + .../detail/message_context.h | 93 -- .../detail/single_client.h | 146 --- .../detail/state_machine.h | 95 -- include/interactive_markers/exceptions.hpp | 54 + .../interactive_marker_client.h | 215 ---- .../interactive_marker_client.hpp | 350 +++++++ .../interactive_marker_server.h | 228 ----- .../interactive_marker_server.hpp | 290 ++++++ include/interactive_markers/menu_handler.h | 143 --- include/interactive_markers/menu_handler.hpp | 158 +++ .../interactive_markers/message_context.hpp | 97 ++ include/interactive_markers/tools.h | 91 -- include/interactive_markers/tools.hpp | 135 +++ .../visibility_control.hpp | 70 ++ package.xml | 34 +- src/interactive_marker_client.cpp | 566 ++++++----- src/interactive_marker_server.cpp | 598 ++++++----- .../interactive_marker_server.py | 43 +- src/interactive_markers/menu_handler.py | 43 +- src/menu_handler.cpp | 284 +++--- src/message_context.cpp | 289 +++--- src/single_client.cpp | 314 ------ src/test/bursty_tf.cpp | 181 ---- src/test/client_test.cpp | 925 ------------------ src/test/missing_tf.cpp | 182 ---- src/test/server_client_test.cpp | 250 ----- src/test/server_test.cpp | 111 --- src/tools.cpp | 451 ++++----- test/cpp_client.test | 3 - test/cpp_server.test | 3 - test/cpp_server_client.test | 3 - .../interactive_marker_fixtures.hpp | 74 ++ .../mock_interactive_marker_client.hpp | 110 +++ .../mock_interactive_marker_server.hpp | 131 +++ .../test_interactive_marker_client.cpp | 340 +++++++ .../test_interactive_marker_server.cpp | 364 +++++++ test/interactive_markers/timed_expect.hpp | 85 ++ 40 files changed, 3597 insertions(+), 4120 deletions(-) create mode 100644 CONTRIBUTING.md create mode 100644 LICENSE delete mode 100644 include/interactive_markers/detail/message_context.h delete mode 100644 include/interactive_markers/detail/single_client.h delete mode 100644 include/interactive_markers/detail/state_machine.h create mode 100644 include/interactive_markers/exceptions.hpp delete mode 100644 include/interactive_markers/interactive_marker_client.h create mode 100644 include/interactive_markers/interactive_marker_client.hpp delete mode 100644 include/interactive_markers/interactive_marker_server.h create mode 100644 include/interactive_markers/interactive_marker_server.hpp delete mode 100644 include/interactive_markers/menu_handler.h create mode 100644 include/interactive_markers/menu_handler.hpp create mode 100644 include/interactive_markers/message_context.hpp delete mode 100644 include/interactive_markers/tools.h create mode 100644 include/interactive_markers/tools.hpp create mode 100644 include/interactive_markers/visibility_control.hpp delete mode 100644 src/single_client.cpp delete mode 100644 src/test/bursty_tf.cpp delete mode 100644 src/test/client_test.cpp delete mode 100644 src/test/missing_tf.cpp delete mode 100644 src/test/server_client_test.cpp delete mode 100644 src/test/server_test.cpp delete mode 100644 test/cpp_client.test delete mode 100644 test/cpp_server.test delete mode 100644 test/cpp_server_client.test create mode 100644 test/interactive_markers/interactive_marker_fixtures.hpp create mode 100644 test/interactive_markers/mock_interactive_marker_client.hpp create mode 100644 test/interactive_markers/mock_interactive_marker_server.hpp create mode 100644 test/interactive_markers/test_interactive_marker_client.cpp create mode 100644 test/interactive_markers/test_interactive_marker_server.cpp create mode 100644 test/interactive_markers/timed_expect.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8305df78..2202b1f0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,67 +1,92 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(interactive_markers) -find_package(catkin REQUIRED - rosconsole - roscpp - rospy - rostest - std_msgs - tf - visualization_msgs -) -catkin_package( - INCLUDE_DIRS include - LIBRARIES interactive_markers - CATKIN_DEPENDS roscpp rosconsole rospy tf visualization_msgs -) -catkin_python_setup() -include_directories(include ${catkin_INCLUDE_DIRS}) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -add_library(${PROJECT_NAME} -src/interactive_marker_server.cpp -src/tools.cpp -src/menu_handler.cpp -src/interactive_marker_client.cpp -src/single_client.cpp -src/message_context.cpp -) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rmw REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/interactive_marker_server.cpp + src/tools.cpp + src/menu_handler.cpp + src/interactive_marker_client.cpp + src/message_context.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC include) +ament_target_dependencies(${PROJECT_NAME} + "rclcpp" + "rmw" + "tf2" + "tf2_geometry_msgs" + "visualization_msgs") install(TARGETS ${PROJECT_NAME} - DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -install(DIRECTORY include/interactive_markers/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h") + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) -# C++ Unit Tests -# -if(CATKIN_ENABLE_TESTING) - include_directories(${GTEST_INCLUDE_DIRS}) +install( + DIRECTORY include/ + DESTINATION include) - add_executable(server_test EXCLUDE_FROM_ALL src/test/server_test.cpp) - target_link_libraries(server_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) - add_dependencies(tests server_test) - add_rostest(test/cpp_server.test) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} + PRIVATE "INTERACTIVE_MARKERS_BUILDING_LIBRARY") - add_executable(client_test EXCLUDE_FROM_ALL src/test/client_test.cpp) - target_link_libraries(client_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) - add_dependencies(tests client_test) - add_rostest(test/cpp_client.test) +ament_export_dependencies("rclcpp") +ament_export_dependencies("rmw") +ament_export_dependencies("tf2") +ament_export_dependencies("tf2_geometry_msgs") +ament_export_dependencies("visualization_msgs") +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) - add_executable(server_client_test EXCLUDE_FROM_ALL src/test/server_client_test.cpp) - target_link_libraries(server_client_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) - add_dependencies(tests server_client_test) - add_rostest(test/cpp_server_client.test) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(std_msgs REQUIRED) - # Test program to simulate Interactive Marker with missing tf information - add_executable(bursty_tf EXCLUDE_FROM_ALL src/test/bursty_tf.cpp) - target_link_libraries(bursty_tf ${PROJECT_NAME}) - add_dependencies(tests bursty_tf) + set(ament_cmake_flake8_FOUND TRUE) + ament_lint_auto_find_test_dependencies() - # Test program to simulate Interactive Marker with wrong tf information - add_executable(missing_tf EXCLUDE_FROM_ALL src/test/missing_tf.cpp) - target_link_libraries(missing_tf ${PROJECT_NAME}) - add_dependencies(tests missing_tf) + ament_add_gtest(test_interactive_marker_server + test/${PROJECT_NAME}/test_interactive_marker_server.cpp + ) + ament_target_dependencies(test_interactive_marker_server + "geometry_msgs" + "rclcpp" + "std_msgs" + ) + target_link_libraries(test_interactive_marker_server + ${PROJECT_NAME} + ) + + ament_add_gtest(test_interactive_marker_client + test/${PROJECT_NAME}/test_interactive_marker_client.cpp + ) + ament_target_dependencies(test_interactive_marker_client + "geometry_msgs" + "rclcpp" + "std_msgs" + "tf2" + ) + target_link_libraries(test_interactive_marker_client + ${PROJECT_NAME} + ) endif() + +ament_package() diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..5e190bdc --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository will +be under the BSD license 2.0, as dictated by that +[license](https://opensource.org/licenses/BSD-3-Clause). diff --git a/LICENSE b/LICENSE new file mode 100644 index 00000000..a9c1baef --- /dev/null +++ b/LICENSE @@ -0,0 +1,32 @@ +Copyright (c) 2012, Willow Garage, Inc. +Copyright (c) 2019, Open Source Robotics Foundation, Inc. +All rights reserved. + +Software License Agreement (BSD License 2.0) + +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 copyright holder 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. diff --git a/include/interactive_markers/detail/message_context.h b/include/interactive_markers/detail/message_context.h deleted file mode 100644 index 684ac3ef..00000000 --- a/include/interactive_markers/detail/message_context.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: David Gossow - *//* - * message_context.h - * - * Created on: Jul 17, 2012 - * Author: gossow - */ - -#ifndef MESSAGE_CONTEXT_H_ -#define MESSAGE_CONTEXT_H_ - -#include - -#include -#include - -namespace interactive_markers -{ - -template -class MessageContext -{ -public: - MessageContext( tf::Transformer& tf, - const std::string& target_frame, - const typename MsgT::ConstPtr& msg, - bool enable_autocomplete_transparency = true); - - MessageContext& operator=( const MessageContext& other ); - - // transform all messages with timestamp into target frame - void getTfTransforms(); - - typename MsgT::Ptr msg; - - // return true if tf info is complete - bool isReady(); - -private: - - void init(); - - bool getTransform( std_msgs::Header& header, geometry_msgs::Pose& pose_msg ); - - void getTfTransforms( std::vector& msg_vec, std::list& indices ); - void getTfTransforms( std::vector& msg_vec, std::list& indices ); - - // array indices of marker/pose updates with missing tf info - std::list open_marker_idx_; - std::list open_pose_idx_; - tf::Transformer& tf_; - std::string target_frame_; - bool enable_autocomplete_transparency_; -}; - -class InitFailException: public tf::TransformException -{ -public: - InitFailException(const std::string errorDescription) : tf::TransformException(errorDescription) { ; } -}; - - -} - -#endif /* MESSAGE_CONTEXT_H_ */ diff --git a/include/interactive_markers/detail/single_client.h b/include/interactive_markers/detail/single_client.h deleted file mode 100644 index b1299913..00000000 --- a/include/interactive_markers/detail/single_client.h +++ /dev/null @@ -1,146 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: David Gossow - *//* - * single_client.h - * - * Created on: Jul 17, 2012 - * Author: gossow - */ - -#ifndef SINGLE_CLIENT_H_ -#define SINGLE_CLIENT_H_ - -#include -#include - -#include -#include -#include - -#include -#include - -#include - -#include - -#include "message_context.h" -#include "state_machine.h" -#include "../interactive_marker_client.h" - - -namespace interactive_markers -{ - -class SingleClient -{ -public: - - SingleClient( - const std::string& server_id, - tf::Transformer& tf, - const std::string& target_frame, - const InteractiveMarkerClient::CbCollection& callbacks ); - - ~SingleClient(); - - // Process message from the update channel - void process(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& msg, bool enable_autocomplete_transparency = true); - - // Process message from the init channel - void process(const visualization_msgs::InteractiveMarkerInit::ConstPtr& msg, bool enable_autocomplete_transparency = true); - - // true if INIT messages are not needed anymore - bool isInitialized(); - - // transform all messages with missing transforms - void update(); - -private: - - // check if we can go from init state to normal operation - void checkInitFinished(); - - void checkKeepAlive(); - - enum StateT - { - INIT, - RECEIVING, - TF_ERROR - }; - - StateMachine state_; - - // updateTf implementation (for one queue) - void transformInitMsgs( ); - void transformUpdateMsgs( ); - - void pushUpdates(); - - void errorReset( std::string error_msg ); - - // sequence number and time of first ever received update - uint64_t first_update_seq_num_; - - // sequence number and time of last received update - uint64_t last_update_seq_num_; - ros::Time last_update_time_; - - // true if the last outgoing update is too long ago - // and we've already sent a notification of that - bool update_time_ok_; - - typedef MessageContext UpdateMessageContext; - typedef MessageContext InitMessageContext; - - // Queue of Updates waiting for tf and numbering - typedef std::deque< UpdateMessageContext > M_UpdateMessageContext; - typedef std::deque< InitMessageContext > M_InitMessageContext; - - // queue for update messages - M_UpdateMessageContext update_queue_; - - // queue for init messages - M_InitMessageContext init_queue_; - - tf::Transformer& tf_; - std::string target_frame_; - - const InteractiveMarkerClient::CbCollection& callbacks_; - - std::string server_id_; - - bool warn_keepalive_; -}; - -} - -#endif /* SINGLE_CLIENT_H_ */ diff --git a/include/interactive_markers/detail/state_machine.h b/include/interactive_markers/detail/state_machine.h deleted file mode 100644 index c8c043d0..00000000 --- a/include/interactive_markers/detail/state_machine.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: David Gossow - */ -/* - * state_machine.h - * - * Created on: Jul 17, 2012 - * Author: gossow - */ - -#ifndef INTERACTIVE_MARKERS_STATE_MACHINE_H_ -#define INTERACTIVE_MARKERS_STATE_MACHINE_H_ - -#include - -namespace interactive_markers -{ - -// Helper class for state management -template -class StateMachine -{ -public: - StateMachine( std::string name, StateT init_state ); - StateMachine& operator=( StateT state ); - operator StateT(); - ros::Duration getDuration(); -private: - StateT state_; - ros::Time chg_time_; - std::string name_; -}; - -template -StateMachine::StateMachine( std::string name, StateT init_state ) -: state_(init_state) -, chg_time_(ros::Time::now()) -, name_(name) -{ -}; - -template -StateMachine& StateMachine::operator=( StateT state ) -{ - if ( state_ != state ) - { - ROS_DEBUG( "Setting state of %s to %lu", name_.c_str(), (int64_t)state ); - state_ = state; - chg_time_=ros::Time::now(); - } - return *this; -} - -template -ros::Duration StateMachine::getDuration() -{ - return ros::Time::now()-chg_time_; -} - -template -StateMachine::operator StateT() -{ - return state_; -} - -} - -#endif /* STATE_MACHINE_H_ */ diff --git a/include/interactive_markers/exceptions.hpp b/include/interactive_markers/exceptions.hpp new file mode 100644 index 00000000..54d83b21 --- /dev/null +++ b/include/interactive_markers/exceptions.hpp @@ -0,0 +1,54 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Open Source Robotics Foundation, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#ifndef INTERACTIVE_MARKERS__EXCEPTIONS_HPP_ +#define INTERACTIVE_MARKERS__EXCEPTIONS_HPP_ + +#include + +#include "tf2/exceptions.h" + +namespace interactive_markers +{ +namespace exceptions +{ + +class TransformError : public tf2::TransformException +{ +public: + explicit TransformError(const std::string error) + : tf2::TransformException(error) {} +}; + +} // namespace exceptions +} // namespace interactive_markers + +#endif // INTERACTIVE_MARKERS__EXCEPTIONS_HPP_ diff --git a/include/interactive_markers/interactive_marker_client.h b/include/interactive_markers/interactive_marker_client.h deleted file mode 100644 index 4450bb86..00000000 --- a/include/interactive_markers/interactive_marker_client.h +++ /dev/null @@ -1,215 +0,0 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: David Gossow - */ - -#ifndef INTERACTIVE_MARKER_CLIENT -#define INTERACTIVE_MARKER_CLIENT - -#include -#include -#include -#include - -#include - -#include -#include - -#include - -#include -#include - -#include "detail/state_machine.h" - -namespace interactive_markers -{ - -class SingleClient; - -/// Acts as a client to one or multiple Interactive Marker servers. -/// Handles topic subscription, error detection and tf transformations. -/// -/// The output is an init message followed by a stream of updates -/// for each server. In case of an error (e.g. message loss, tf failure), -/// the connection to the sending server is reset. -/// -/// All timestamped messages are being transformed into the target frame, -/// while for non-timestamped messages it is ensured that the necessary -/// tf transformation will be available. -class InteractiveMarkerClient : boost::noncopyable -{ -public: - - enum StatusT { - OK = 0, - WARN = 1, - ERROR = 2 - }; - - typedef visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr; - typedef visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr; - - typedef boost::function< void ( const UpdateConstPtr& ) > UpdateCallback; - typedef boost::function< void ( const InitConstPtr& ) > InitCallback; - typedef boost::function< void ( const std::string& ) > ResetCallback; - typedef boost::function< void ( StatusT, const std::string&, const std::string& ) > StatusCallback; - - /// @param tf The tf transformer to use. - /// @param target_frame tf frame to transform timestamped messages into. - /// @param topic_ns The topic namespace (will subscribe to topic_ns/update, topic_ns/init) - InteractiveMarkerClient( tf::Transformer& tf, - const std::string& target_frame = "", - const std::string &topic_ns = "" ); - - /// Will cause a 'reset' call for all server ids - ~InteractiveMarkerClient(); - - /// Subscribe to the topics topic_ns/update and topic_ns/init - void subscribe( std::string topic_ns ); - - /// Unsubscribe, clear queues & call reset callbacks - void shutdown(); - - /// Update tf info, call callbacks - void update(); - - /// Change the target frame and reset the connection - void setTargetFrame( std::string target_frame ); - - /// Set callback for init messages - void setInitCb( const InitCallback& cb ); - - /// Set callback for update messages - void setUpdateCb( const UpdateCallback& cb ); - - /// Set callback for resetting one server connection - void setResetCb( const ResetCallback& cb ); - - /// Set callback for status updates - void setStatusCb( const StatusCallback& cb ); - - void setEnableAutocompleteTransparency( bool enable ) { enable_autocomplete_transparency_ = enable;} - -private: - - // Process message from the init or update channel - template - void process( const MsgConstPtrT& msg ); - - ros::NodeHandle nh_; - - enum StateT - { - IDLE, - INIT, - RUNNING - }; - - StateMachine state_; - - std::string topic_ns_; - - ros::Subscriber update_sub_; - ros::Subscriber init_sub_; - - // subscribe to the init channel - void subscribeInit(); - - // subscribe to the init channel - void subscribeUpdate(); - - void statusCb( StatusT status, const std::string& server_id, const std::string& msg ); - - typedef boost::shared_ptr SingleClientPtr; - typedef boost::unordered_map M_SingleClient; - M_SingleClient publisher_contexts_; - boost::mutex publisher_contexts_mutex_; - - tf::Transformer& tf_; - std::string target_frame_; - -public: - // for internal usage - struct CbCollection - { - void initCb( const InitConstPtr& i ) const { - if (init_cb_) init_cb_( i ); } - void updateCb( const UpdateConstPtr& u ) const { - if (update_cb_) update_cb_( u ); } - void resetCb( const std::string& s ) const { - if (reset_cb_) reset_cb_(s); } - void statusCb( StatusT s, const std::string& id, const std::string& m ) const { - if (status_cb_) status_cb_(s,id,m); } - - void setInitCb( InitCallback init_cb ) { - init_cb_ = init_cb; - } - void setUpdateCb( UpdateCallback update_cb ) { - update_cb_ = update_cb; - } - void setResetCb( ResetCallback reset_cb ) { - reset_cb_ = reset_cb; - } - void setStatusCb( StatusCallback status_cb ) { - status_cb_ = status_cb; - } - - private: - InitCallback init_cb_; - UpdateCallback update_cb_; - ResetCallback reset_cb_; - StatusCallback status_cb_; - }; - - // handle init message - void processInit( const InitConstPtr& msg ); - - // handle update message - void processUpdate( const UpdateConstPtr& msg ); - -private: - CbCollection callbacks_; - - // this is the real (external) status callback - StatusCallback status_cb_; - - // this allows us to detect if a server died (in most cases) - int last_num_publishers_; - - // if false, auto-completed markers will have alpha = 1.0 - bool enable_autocomplete_transparency_; -}; - - - -} - -#endif diff --git a/include/interactive_markers/interactive_marker_client.hpp b/include/interactive_markers/interactive_marker_client.hpp new file mode 100644 index 00000000..ee6d2944 --- /dev/null +++ b/include/interactive_markers/interactive_marker_client.hpp @@ -0,0 +1,350 @@ +// Copyright (c) 2011, Willow Garage, Inc. +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Author: David Gossow + +#ifndef INTERACTIVE_MARKERS__INTERACTIVE_MARKER_CLIENT_HPP_ +#define INTERACTIVE_MARKERS__INTERACTIVE_MARKER_CLIENT_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/logger.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "visualization_msgs/msg/interactive_marker_feedback.hpp" +#include "visualization_msgs/msg/interactive_marker_update.hpp" +#include "visualization_msgs/srv/get_interactive_markers.hpp" + +#include "interactive_markers/message_context.hpp" +#include "interactive_markers/visibility_control.hpp" + +namespace tf2 +{ +class BufferCoreInterface; +} + +namespace interactive_markers +{ + +/// Acts as a client to one or multiple Interactive Marker servers. +/** + * Handles topic subscription, error detection and tf transformations. + * + * After connecting to a provided namespace, the client sends a service request + * to an available interactive marker server to get an initial set of markers. + * Once initialized, feedback messages may be sent from the client to the server + * as well as update messages received from the server. + * + * In case of an error (e.g. update message loss or tf failure), the connection + * to the server is reset. + * + * All timestamped messages are being transformed into the target frame, + * while for non-timestamped messages it is ensured that the necessary + * tf transformation will be available. + */ +class InteractiveMarkerClient +{ +public: + enum Status + { + DEBUG = 0, + INFO, + WARN, + ERROR + }; + + enum State + { + IDLE, + INITIALIZE, + RUNNING + }; + + typedef std::function + UpdateCallback; + typedef std::function + InitializeCallback; + typedef std::function ResetCallback; + typedef std::function StatusCallback; + + /// Constructor. + /** + * \param node_base_interface The node base interface for creating the service client. + * \param topics_interface The node topics interface for creating publishers and subscriptions. + * \param graph_interface The node graph interface for querying the ROS graph. + * \param clock_interface The node clock interface for getting the current time. + * \param logging_interface The node logging interface for logging messages. + * \param tf_buffer_core The tf transformer to use. + * \param target_frame The tf frame to transform timestamped messages into. + * \param topic_namespace The interactive marker topic namespace. + * This is the namespace used for the underlying ROS service and topics for communication + * between the client and server. + * If the namespace is not empty, then connect() is called. + * \param request_timeout Timeout in seconds before retrying to request interactive markers from + * a connected server. + */ + template> + InteractiveMarkerClient( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr graph_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, + std::shared_ptr tf_buffer_core, + const std::string & target_frame = "", + const std::string & topic_namespace = "", + const std::chrono::duration & request_timeout = std::chrono::seconds(1)) + : node_base_interface_(node_base_interface), + topics_interface_(topics_interface), + services_interface_(services_interface), + graph_interface_(graph_interface), + client_id_(node_base_interface->get_fully_qualified_name()), + clock_(clock_interface->get_clock()), + logger_(logging_interface->get_logger()), + state_(IDLE), + tf_buffer_core_(tf_buffer_core), + target_frame_(target_frame), + topic_namespace_(topic_namespace), + request_timeout_(request_timeout), + initial_response_msg_(0), + first_update_(true), + last_update_sequence_number_(0u), + enable_autocomplete_transparency_(true) + { + connect(topic_namespace_); + } + + /// Constructor. + /** + * \param node The object from which to get the desired node interfaces. + * \param tf_buffer_core The tf transformer to use. + * \param target_frame The tf frame to transform timestamped messages into. + * \param topic_namespace The interactive marker topic namespace. + * This is the namespace used for the underlying ROS service and topics for communication + * between the client and server. + * If the namespace is not empty, then connect() is called. + * \param request_timeout Timeout in seconds before retrying to request interactive markers from + * a connected server. + */ + template> + InteractiveMarkerClient( + NodePtr node, + std::shared_ptr tf_buffer_core, + const std::string & target_frame = "", + const std::string & topic_namespace = "", + const std::chrono::duration & request_timeout = std::chrono::seconds(1)) + : InteractiveMarkerClient( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_services_interface(), + node->get_node_graph_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + tf_buffer_core, + target_frame, + topic_namespace, + request_timeout) + { + } + + /// Destructor. + /** + * Calls reset(). + */ + INTERACTIVE_MARKERS_PUBLIC + ~InteractiveMarkerClient(); + + /// Connect to a server in a given namespace. + INTERACTIVE_MARKERS_PUBLIC + void connect(std::string topic_namespace); + + /// Disconnect from a server and clear the update queue. + INTERACTIVE_MARKERS_PUBLIC + void disconnect(); + + /// Update the internal state and call registered callbacks. + INTERACTIVE_MARKERS_PUBLIC + void update(); + + /// Publish a feedback message to the server. + INTERACTIVE_MARKERS_PUBLIC + void publishFeedback(visualization_msgs::msg::InteractiveMarkerFeedback feedback); + + /// Change the target frame. + /** + * This resets the connection. + */ + INTERACTIVE_MARKERS_PUBLIC + void setTargetFrame(std::string target_frame); + + /// Set the initialization callback. + /** + * The registered function is called when the client successfully initializes with a connected + * server. + */ + INTERACTIVE_MARKERS_PUBLIC + void setInitializeCallback(const InitializeCallback & cb); + + /// Set the callback for update messages. + /** + * If the client is connected and initialized, the registered function is called whenever an + * update message is received. + */ + INTERACTIVE_MARKERS_PUBLIC + void setUpdateCallback(const UpdateCallback & cb); + + /// Set the reset callback. + /** + * The registered function is called whenver the connection is reset. + */ + INTERACTIVE_MARKERS_PUBLIC + void setResetCallback(const ResetCallback & cb); + + /// Set the callback for status updates. + /** + * The registered function is called whenever there is a status message. + */ + INTERACTIVE_MARKERS_PUBLIC + void setStatusCallback(const StatusCallback & cb); + + INTERACTIVE_MARKERS_PUBLIC + inline void setEnableAutocompleteTransparency(bool enable) + { + enable_autocomplete_transparency_ = enable; + } + + INTERACTIVE_MARKERS_PUBLIC + inline State getState() const + { + return state_; + } + +private: + typedef MessageContext + InitialMessageContext; + typedef MessageContext UpdateMessageContext; + + // Disable copying + InteractiveMarkerClient(const InteractiveMarkerClient &) = delete; + InteractiveMarkerClient & operator=(const InteractiveMarkerClient &) = delete; + + // Clear messages from the update queue + void reset(); + + // Make a service request to get interactive markers from a server + void requestInteractiveMarkers(); + + void processInitialMessage( + rclcpp::Client::SharedFuture future); + + void processUpdate( + visualization_msgs::msg::InteractiveMarkerUpdate::SharedPtr msg); + + bool transformInitialMessage(); + + bool transformUpdateMessages(); + + bool checkInitializeFinished(); + + void pushUpdates(); + + void changeState(const State & new_state); + + void updateStatus(const Status status, const std::string & msg); + + // Node interfaces + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr graph_interface_; + + std::string client_id_; + + rclcpp::Clock::SharedPtr clock_; + + rclcpp::Logger logger_; + + State state_; + + rclcpp::Client::SharedPtr + get_interactive_markers_client_; + + rclcpp::Publisher::SharedPtr feedback_pub_; + rclcpp::SubscriptionBase::SharedPtr update_sub_; + + std::shared_ptr tf_buffer_core_; + + std::string target_frame_; + + std::string topic_namespace_; + + std::recursive_mutex mutex_; + + // Time of the last request for interactive markers + rclcpp::Time request_time_; + + // Timeout while waiting for service response message + rclcpp::Duration request_timeout_; + + // The response message from the request to get interactive markers + std::shared_ptr initial_response_msg_; + + // Queue of update messages from the server + std::deque update_queue_; + + // true if no updates have been received since the last response message + bool first_update_; + + // Sequence number last update message received + uint64_t last_update_sequence_number_; + + // if false, auto-completed markers will have alpha = 1.0 + bool enable_autocomplete_transparency_; + + // User callbacks + InitializeCallback initialize_callback_; + UpdateCallback update_callback_; + ResetCallback reset_callback_; + StatusCallback status_callback_; +}; // class InteractiveMarkerClient + +} // namespace interactive_markers + +#endif // INTERACTIVE_MARKERS__INTERACTIVE_MARKER_CLIENT_HPP_ diff --git a/include/interactive_markers/interactive_marker_server.h b/include/interactive_markers/interactive_marker_server.h deleted file mode 100644 index dea538c9..00000000 --- a/include/interactive_markers/interactive_marker_server.h +++ /dev/null @@ -1,228 +0,0 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: David Gossow - */ - -#ifndef INTERACTIVE_MARKER_SERVER -#define INTERACTIVE_MARKER_SERVER - -#include -#include - -#include -#include -#include - -#include -#include - - -#include -#include - -namespace interactive_markers -{ - -/// Acts as a server to one or many GUIs (e.g. rviz) displaying a set of interactive markers -/// -/// Note: Keep in mind that changes made by calling insert(), erase(), setCallback() etc. -/// are not applied until calling applyChanges(). -class InteractiveMarkerServer : boost::noncopyable -{ -public: - - typedef visualization_msgs::InteractiveMarkerFeedbackConstPtr FeedbackConstPtr; - typedef boost::function< void ( const FeedbackConstPtr& ) > FeedbackCallback; - - static const uint8_t DEFAULT_FEEDBACK_CB = 255; - - /// @param topic_ns The interface will use the topics topic_ns/update and - /// topic_ns/feedback for communication. - /// @param server_id If you run multiple servers on the same topic from - /// within the same node, you will need to assign different names to them. - /// Otherwise, leave this empty. - /// @param spin_thread If set to true, will spin up a thread for message handling. - /// All callbacks will be called from that thread. - InteractiveMarkerServer( const std::string &topic_ns, const std::string &server_id="", bool spin_thread = false ); - - /// Destruction of the interface will lead to all managed markers being cleared. - ~InteractiveMarkerServer(); - - /// Add or replace a marker without changing its callback functions. - /// Note: Changes to the marker will not take effect until you call applyChanges(). - /// The callback changes immediately. - /// @param int_marker The marker to be added or replaced - void insert( const visualization_msgs::InteractiveMarker &int_marker ); - - /// Add or replace a marker and its callback functions - /// Note: Changes to the marker will not take effect until you call applyChanges(). - /// The callback changes immediately. - /// @param int_marker The marker to be added or replaced - /// @param feedback_cb Function to call on the arrival of a feedback message. - /// @param feedback_type Type of feedback for which to call the feedback. - void insert( const visualization_msgs::InteractiveMarker &int_marker, - FeedbackCallback feedback_cb, - uint8_t feedback_type=DEFAULT_FEEDBACK_CB ); - - /// Update the pose of a marker with the specified name - /// Note: This change will not take effect until you call applyChanges() - /// @return true if a marker with that name exists - /// @param name Name of the interactive marker - /// @param pose The new pose - /// @param header Header replacement. Leave this empty to use the previous one. - bool setPose( const std::string &name, - const geometry_msgs::Pose &pose, - const std_msgs::Header &header=std_msgs::Header() ); - - /// Erase the marker with the specified name - /// Note: This change will not take effect until you call applyChanges(). - /// @return true if a marker with that name exists - /// @param name Name of the interactive marker - bool erase( const std::string &name ); - - /// Clear all markers. - /// Note: This change will not take effect until you call applyChanges(). - void clear(); - - /// Return whether the server contains any markers. - /// Note: Does not include markers inserted since the last applyChanges(). - /// @return true if the server contains no markers - bool empty() const; - - /// Return the number of markers contained in the server - /// Note: Does not include markers inserted since the last applyChanges(). - /// @return The number of markers contained in the server - std::size_t size() const; - - /// Add or replace a callback function for the specified marker. - /// Note: This change will not take effect until you call applyChanges(). - /// The server will try to call any type-specific callback first. - /// If none is set, it will call the default callback. - /// If a callback for the given type already exists, it will be replaced. - /// To unset a type-specific callback, pass in an empty one. - /// @param name Name of the interactive marker - /// @param feedback_cb Function to call on the arrival of a feedback message. - /// @param feedback_type Type of feedback for which to call the feedback. - /// Leave this empty to make this the default callback. - bool setCallback( const std::string &name, FeedbackCallback feedback_cb, - uint8_t feedback_type=DEFAULT_FEEDBACK_CB ); - - /// Apply changes made since the last call to this method & - /// broadcast an update to all clients. - void applyChanges(); - - /// Get marker by name - /// @param name Name of the interactive marker - /// @param[out] int_marker Output message - /// @return true if a marker with that name exists - bool get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const; - -private: - - struct MarkerContext - { - ros::Time last_feedback; - std::string last_client_id; - FeedbackCallback default_feedback_cb; - boost::unordered_map feedback_cbs; - visualization_msgs::InteractiveMarker int_marker; - }; - - typedef boost::unordered_map< std::string, MarkerContext > M_MarkerContext; - - // represents an update to a single marker - struct UpdateContext - { - enum { - FULL_UPDATE, - POSE_UPDATE, - ERASE - } update_type; - visualization_msgs::InteractiveMarker int_marker; - FeedbackCallback default_feedback_cb; - boost::unordered_map feedback_cbs; - }; - - typedef boost::unordered_map< std::string, UpdateContext > M_UpdateContext; - - // main loop when spinning our own thread - // - process callbacks in our callback queue - // - process pending goals - void spinThread(); - - // update marker pose & call user callback - void processFeedback( const FeedbackConstPtr& feedback ); - - // send an empty update to keep the client GUIs happy - void keepAlive(); - - // increase sequence number & publish an update - void publish( visualization_msgs::InteractiveMarkerUpdate &update ); - - // publish the current complete state to the latched "init" topic. - void publishInit(); - - // Update pose, schedule update without locking - void doSetPose( M_UpdateContext::iterator update_it, - const std::string &name, - const geometry_msgs::Pose &pose, - const std_msgs::Header &header ); - - // contains the current state of all markers - M_MarkerContext marker_contexts_; - - // updates that have to be sent on the next publish - M_UpdateContext pending_updates_; - - // topic namespace to use - std::string topic_ns_; - - mutable boost::recursive_mutex mutex_; - - // these are needed when spinning up a dedicated thread - boost::scoped_ptr spin_thread_; - ros::NodeHandle node_handle_; - ros::CallbackQueue callback_queue_; - volatile bool need_to_terminate_; - - // this is needed when running in non-threaded mode - ros::Timer keep_alive_timer_; - - ros::Publisher init_pub_; - ros::Publisher update_pub_; - ros::Subscriber feedback_sub_; - - uint64_t seq_num_; - - std::string server_id_; -}; - -} - -#endif diff --git a/include/interactive_markers/interactive_marker_server.hpp b/include/interactive_markers/interactive_marker_server.hpp new file mode 100644 index 00000000..67a13a88 --- /dev/null +++ b/include/interactive_markers/interactive_marker_server.hpp @@ -0,0 +1,290 @@ +// Copyright (c) 2011, Willow Garage, Inc. +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Author: David Gossow + +#ifndef INTERACTIVE_MARKERS__INTERACTIVE_MARKER_SERVER_HPP_ +#define INTERACTIVE_MARKERS__INTERACTIVE_MARKER_SERVER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "visualization_msgs/msg/interactive_marker_feedback.hpp" +#include "visualization_msgs/msg/interactive_marker_update.hpp" +#include "visualization_msgs/srv/get_interactive_markers.hpp" + +#include "interactive_markers/visibility_control.hpp" + +namespace interactive_markers +{ + +/// A server to one or many clients (e.g. rviz) displaying a set of interactive markers. +/** + * Note that changes made by calling insert(), erase(), setCallback() etc. are not applied until + * calling applyChanges(). + */ +class InteractiveMarkerServer +{ +public: + typedef visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr FeedbackConstSharedPtr; + typedef std::function FeedbackCallback; + + static const uint8_t DEFAULT_FEEDBACK_CB = 255; + + /// Constructor. + /** + * \param topic_namepsace The namespace for the ROS services and topics used for communication + * with interactive marker clients. + * \param base_interface Node base interface. + * \param clock_interface Node clock interface. + * \param logging_interface Node logging interface. + * \param topics_interface Node topics interface. + * \param service_interface Node service interface. + */ + INTERACTIVE_MARKERS_PUBLIC + InteractiveMarkerServer( + const std::string & topic_namespace, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface); + + template + InteractiveMarkerServer( + const std::string & topic_namespace, + NodePtr node) + : InteractiveMarkerServer( + topic_namespace, + node->get_node_base_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + node->get_node_topics_interface(), + node->get_node_services_interface()) + { + } + + /// Destruction of the interface will lead to all managed markers being cleared. + INTERACTIVE_MARKERS_PUBLIC + ~InteractiveMarkerServer(); + + /// Add or replace a marker without changing its callback functions. + /** + * Note, changes to the marker will not take effect until you call applyChanges(). + * The callback changes immediately. + * + * \param marker The marker to be added or replaced. + */ + INTERACTIVE_MARKERS_PUBLIC + void insert(const visualization_msgs::msg::InteractiveMarker & marker); + + /// Add or replace a marker and its callback functions. + /** + * Note, changes to the marker will not take effect until you call applyChanges(). + * The callback changes immediately. + * + * \param marker The marker to be added or replaced. + * \param feedback_callback Function to call on the arrival of a feedback message. + * \param feedback_type Type of feedback for which to call the feedback callback. + */ + INTERACTIVE_MARKERS_PUBLIC + void insert( + const visualization_msgs::msg::InteractiveMarker & marker, + FeedbackCallback feedback_callback, + uint8_t feedback_type = DEFAULT_FEEDBACK_CB); + + /// Update the pose of a marker by name. + /** + * Note, this change will not take effect until you call applyChanges(). + * + * \param name Name of the interactive marker to update. + * \param pose The new pose. + * \param header Header replacement. + * Leave this empty to use the previous one. + * \return true if a marker with the provided name exists, false otherwise. + */ + INTERACTIVE_MARKERS_PUBLIC + bool setPose( + const std::string & name, + const geometry_msgs::msg::Pose & pose, + const std_msgs::msg::Header & header = std_msgs::msg::Header()); + + /// Erase a marker by name. + /** + * Note, this change will not take effect until you call applyChanges(). + * \param name Name of the interactive marker to erase. + * \return true if a marker with the provided name exists, false otherwise. + */ + INTERACTIVE_MARKERS_PUBLIC + bool erase(const std::string & name); + + /// Clear all markers. + /** + * Note, this change will not take effect until you call applyChanges(). + */ + INTERACTIVE_MARKERS_PUBLIC + void clear(); + + /// Return whether the server contains any markers. + /** + * Does not include markers inserted since the last applyChanges(). + * + * \return true if the server contains no markers, false otherwise. + */ + INTERACTIVE_MARKERS_PUBLIC + bool empty() const; + + /// Return the number of markers contained in the server. + /** + * Does not include markers inserted since the last applyChanges(). + * \return The number of markers contained in the server. + */ + INTERACTIVE_MARKERS_PUBLIC + std::size_t size() const; + + /// Add or replace a callback function for a marker. + /** + * Note, this change will not take effect until you call applyChanges(). + * The server will try to call any type-specific callback first. + * If none is set, it will call the default callback. + * If a callback for the given type already exists, it will be replaced. + * To unset a type-specific callback, pass in an empty one. + * + * \param name Name of an existing interactive marker. + * \param feedback_callback Function to call on the arrival of a feedback message. + * \param feedback_type Type of feedback for which to call the feedback callback. + * Leave this empty to make this the default callback. + * \return true if the setting the callback was successful, false if the provided + * name does not match an existing marker. + */ + INTERACTIVE_MARKERS_PUBLIC + bool setCallback( + const std::string & name, + FeedbackCallback feedback_cb, + uint8_t feedback_type = DEFAULT_FEEDBACK_CB); + + /// Apply changes made since the last call to this method and broadcast an update to all clients. + INTERACTIVE_MARKERS_PUBLIC + void applyChanges(); + + /// Get a marker by name. + /** + * \param[in] name Name of the interactive marker. + * \param[out] marker Output message. + * Not set if a marker with the provided name does not exist. + * \return true if a marker with the provided name exists, false otherwise. + */ + INTERACTIVE_MARKERS_PUBLIC + bool get(std::string name, visualization_msgs::msg::InteractiveMarker & int_marker) const; + +private: + // Disable copying + InteractiveMarkerServer(const InteractiveMarkerServer &) = delete; + InteractiveMarkerServer & operator=(const InteractiveMarkerServer &) = delete; + + struct MarkerContext + { + rclcpp::Time last_feedback; + std::string last_client_id; + FeedbackCallback default_feedback_cb; + std::unordered_map feedback_cbs; + visualization_msgs::msg::InteractiveMarker int_marker; + }; + + typedef std::unordered_map M_MarkerContext; + + // represents an update to a single marker + struct UpdateContext + { + enum + { + FULL_UPDATE, + POSE_UPDATE, + ERASE + } update_type; + visualization_msgs::msg::InteractiveMarker int_marker; + FeedbackCallback default_feedback_cb; + std::unordered_map feedback_cbs; + }; + + typedef std::unordered_map M_UpdateContext; + + void getInteractiveMarkersCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + // update marker pose & call user callback + void processFeedback(visualization_msgs::msg::InteractiveMarkerFeedback::SharedPtr feedback); + + // increase sequence number & publish an update + void publish(visualization_msgs::msg::InteractiveMarkerUpdate & update); + + // Update pose, schedule update without locking + void doSetPose( + M_UpdateContext::iterator update_it, + const std::string & name, + const geometry_msgs::msg::Pose & pose, + const std_msgs::msg::Header & header); + + // contains the current state of all markers + M_MarkerContext marker_contexts_; + + // updates that have to be sent on the next publish + M_UpdateContext pending_updates_; + + // topic namespace to use + std::string topic_namespace_; + + mutable std::recursive_mutex mutex_; + + rclcpp::Service::SharedPtr + get_interactive_markers_service_; + rclcpp::Publisher::SharedPtr update_pub_; + rclcpp::Subscription::SharedPtr feedback_sub_; + + rclcpp::Context::SharedPtr context_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; + + uint64_t sequence_number_; +}; // class InteractiveMarkerServer + +} // namespace interactive_markers + +#endif // INTERACTIVE_MARKERS__INTERACTIVE_MARKER_SERVER_HPP_ diff --git a/include/interactive_markers/menu_handler.h b/include/interactive_markers/menu_handler.h deleted file mode 100644 index 774b1287..00000000 --- a/include/interactive_markers/menu_handler.h +++ /dev/null @@ -1,143 +0,0 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: David Gossow - */ - -#ifndef INTERACTIVE_MARKER_MENU_HANDLER -#define INTERACTIVE_MARKER_MENU_HANDLER - -#include -#include - -#include -#include - -namespace interactive_markers -{ - -// Simple non-intrusive helper class which creates a menu and maps its -// entries to function callbacks -class MenuHandler -{ -public: - - typedef uint32_t EntryHandle; - - typedef visualization_msgs::InteractiveMarkerFeedbackConstPtr FeedbackConstPtr; - typedef boost::function< void ( const FeedbackConstPtr& ) > FeedbackCallback; - - enum CheckState { - NO_CHECKBOX, - CHECKED, - UNCHECKED - }; - - MenuHandler( ); - - /// Insert top-level entry with feedback function - EntryHandle insert( const std::string &title, const FeedbackCallback &feedback_cb ); - - /// Insert top-level entry with custom (client-side) command - EntryHandle insert( const std::string &title, - const uint8_t command_type = visualization_msgs::MenuEntry::FEEDBACK, - const std::string &command="" ); - - /// Insert second-level entry with feedback function - EntryHandle insert( EntryHandle parent, const std::string &title, - const FeedbackCallback &feedback_cb ); - - /// Insert second-level entry with custom (client-side) command - EntryHandle insert( EntryHandle parent, const std::string &title, - const uint8_t command_type = visualization_msgs::MenuEntry::FEEDBACK, - const std::string &command="" ); - - /// Specify if an entry should be visible or hidden - bool setVisible( EntryHandle handle, bool visible ); - - /// Specify if an entry is checked or can't be checked at all - bool setCheckState( EntryHandle handle, CheckState check_state ); - - /// Get the current state of an entry - /// @return true if the entry exists - bool getCheckState( EntryHandle handle, CheckState &check_state ) const; - - /// Copy current menu state into the marker given by the specified name & - /// divert callback for MENU_SELECT feedback to this manager - bool apply( InteractiveMarkerServer &server, const std::string &marker_name ); - - /// Re-apply to all markers that this was applied to previously - bool reApply( InteractiveMarkerServer &server ); - - /// Get the title for the given menu entry - /// @return true if the entry exists - bool getTitle( EntryHandle handle, std::string &title ) const; - -private: - - struct EntryContext - { - std::string title; - std::string command; - uint8_t command_type; - std::vector sub_entries; - bool visible; - CheckState check_state; - FeedbackCallback feedback_cb; - }; - - // Call registered callback functions for given feedback command - void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ); - - // Create and push MenuEntry objects from handles_in onto - // entries_out. Calls itself recursively to add the entire menu - // tree. - bool pushMenuEntries( std::vector& handles_in, - std::vector& entries_out, - EntryHandle parent_handle ); - - visualization_msgs::MenuEntry makeEntry( EntryContext& context, EntryHandle handle, EntryHandle parent_handle ); - - // Insert without adding a top-level entry - EntryHandle doInsert( const std::string &title, - const uint8_t command_type, - const std::string &command, - const FeedbackCallback &feedback_cb ); - - std::vector top_level_handles_; - - boost::unordered_map entry_contexts_; - - EntryHandle current_handle_; - - std::set managed_markers_; -}; - -} - -#endif diff --git a/include/interactive_markers/menu_handler.hpp b/include/interactive_markers/menu_handler.hpp new file mode 100644 index 00000000..5eef206f --- /dev/null +++ b/include/interactive_markers/menu_handler.hpp @@ -0,0 +1,158 @@ +// Copyright (c) 2011, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Author: David Gossow + +#ifndef INTERACTIVE_MARKERS__MENU_HANDLER_HPP_ +#define INTERACTIVE_MARKERS__MENU_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "visualization_msgs/msg/menu_entry.hpp" + +#include "interactive_markers/interactive_marker_server.hpp" +#include "interactive_markers/visibility_control.hpp" + +namespace interactive_markers +{ + +// Simple non-intrusive helper class which creates a menu and maps its +// entries to function callbacks +class MenuHandler +{ +public: + typedef uint32_t EntryHandle; + + typedef visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr FeedbackConstPtr; + typedef std::function FeedbackCallback; + + enum CheckState + { + NO_CHECKBOX, + CHECKED, + UNCHECKED + }; + + MenuHandler(); + + /// Insert top-level entry with feedback function + EntryHandle insert(const std::string & title, const FeedbackCallback & feedback_cb); + + /// Insert top-level entry with custom (client-side) command + EntryHandle insert( + const std::string & title, + const uint8_t command_type = visualization_msgs::msg::MenuEntry::FEEDBACK, + const std::string & command = ""); + + /// Insert second-level entry with feedback function + EntryHandle insert( + EntryHandle parent, const std::string & title, + const FeedbackCallback & feedback_cb); + + /// Insert second-level entry with custom (client-side) command + EntryHandle insert( + EntryHandle parent, const std::string & title, + const uint8_t command_type = visualization_msgs::msg::MenuEntry::FEEDBACK, + const std::string & command = ""); + + /// Specify if an entry should be visible or hidden + bool setVisible(EntryHandle handle, bool visible); + + /// Specify if an entry is checked or can't be checked at all + bool setCheckState(EntryHandle handle, CheckState check_state); + + /// Get the current state of an entry + /// @return true if the entry exists + bool getCheckState(EntryHandle handle, CheckState & check_state) const; + + /// Copy current menu state into the marker given by the specified name & + /// divert callback for MENU_SELECT feedback to this manager + bool apply(InteractiveMarkerServer & server, const std::string & marker_name); + + /// Re-apply to all markers that this was applied to previously + bool reApply(InteractiveMarkerServer & server); + + /// Get the title for the given menu entry + /// @return true if the entry exists + bool getTitle(EntryHandle handle, std::string & title) const; + +private: + struct EntryContext + { + std::string title; + std::string command; + uint8_t command_type; + std::vector sub_entries; + bool visible; + CheckState check_state; + FeedbackCallback feedback_cb; + }; + + // Call registered callback functions for given feedback command + void processFeedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); + + // Create and push MenuEntry objects from handles_in onto + // entries_out. Calls itself recursively to add the entire menu + // tree. + bool pushMenuEntries( + std::vector & handles_in, + std::vector & entries_out, + EntryHandle parent_handle); + + visualization_msgs::msg::MenuEntry makeEntry( + EntryContext & context, EntryHandle handle, + EntryHandle parent_handle); + + // Insert without adding a top-level entry + EntryHandle doInsert( + const std::string & title, + const uint8_t command_type, + const std::string & command, + const FeedbackCallback & feedback_cb); + + std::vector top_level_handles_; + + std::unordered_map entry_contexts_; + + EntryHandle current_handle_; + + std::set managed_markers_; +}; // class MenuHandler + +} // namespace interactive_markers + +#endif // INTERACTIVE_MARKERS__MENU_HANDLER_HPP_ diff --git a/include/interactive_markers/message_context.hpp b/include/interactive_markers/message_context.hpp new file mode 100644 index 00000000..72c05504 --- /dev/null +++ b/include/interactive_markers/message_context.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2012, Willow Garage, Inc. +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Author: David Gossow + +#ifndef INTERACTIVE_MARKERS__MESSAGE_CONTEXT_HPP_ +#define INTERACTIVE_MARKERS__MESSAGE_CONTEXT_HPP_ + +#include +#include +#include +#include + +#include "visualization_msgs/msg/interactive_marker_init.hpp" +#include "visualization_msgs/msg/interactive_marker_update.hpp" + +namespace tf2 +{ +class BufferCoreInterface; +} + +namespace interactive_markers +{ + +template +class MessageContext +{ +public: + MessageContext( + std::shared_ptr tf_buffer_core, + const std::string & target_frame, + typename MsgT::SharedPtr msg, + bool enable_autocomplete_transparency = true); + + MessageContext & operator=(const MessageContext & other); + + // transform all messages with timestamp into target frame + void getTfTransforms(); + + typename MsgT::SharedPtr msg; + + // return true if tf info is complete + bool isReady(); + +private: + void init(); + + bool getTransform(std_msgs::msg::Header & header, geometry_msgs::msg::Pose & pose_msg); + + void getTfTransforms( + std::vector & msg_vec, + std::list & indices); + void getTfTransforms( + std::vector & msg_vec, + std::list & indices); + + // array indices of marker/pose updates with missing tf info + std::list open_marker_idx_; + std::list open_pose_idx_; + std::shared_ptr tf_buffer_core_; + std::string target_frame_; + bool enable_autocomplete_transparency_; +}; // class MessageContext + +} // namespace interactive_markers + +#endif // INTERACTIVE_MARKERS__MESSAGE_CONTEXT_HPP_ diff --git a/include/interactive_markers/tools.h b/include/interactive_markers/tools.h deleted file mode 100644 index 88772c22..00000000 --- a/include/interactive_markers/tools.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef RVIZ_INTERACTIVE_MARKER_TOOLS_H -#define RVIZ_INTERACTIVE_MARKER_TOOLS_H - -#include - -namespace interactive_markers -{ - -/** @brief fill in default values & insert default controls when none are specified. - * - * This also calls uniqueifyControlNames(). - * @param msg interactive marker to be completed */ -void autoComplete( visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency = true ); - -/// @brief fill in default values & insert default controls when none are specified -/// @param msg interactive marker which contains the control -/// @param control the control to be completed -void autoComplete( const visualization_msgs::InteractiveMarker &msg, - visualization_msgs::InteractiveMarkerControl &control, bool enable_autocomplete_transparency = true ); - -/** @brief Make sure all the control names are unique within the given msg. - * - * Appends _u0 _u1 etc to repeated names (not including the first of each). - * This is called by autoComplete( visualization_msgs::InteractiveMarker &msg ). */ -void uniqueifyControlNames( visualization_msgs::InteractiveMarker& msg ); - -/// make a quaternion with a fixed local x axis. -/// The rotation around that axis will be chosen automatically. -/// @param x,y,z the designated x axis -geometry_msgs::Quaternion makeQuaternion( float x, float y, float z ); - - -/// --- marker helpers --- - -/// @brief make a default-style arrow marker based on the properties of the given interactive marker -/// @param msg the interactive marker that this will go into -/// @param control the control where to insert the arrow marker -/// @param pos how far from the center should the arrow be, and on which side -void makeArrow( const visualization_msgs::InteractiveMarker &msg, - visualization_msgs::InteractiveMarkerControl &control, float pos ); - -/// @brief make a default-style disc marker (e.g for rotating) based on the properties of the given interactive marker -/// @param msg the interactive marker that this will go into -/// @param width width of the disc, relative to its inner radius -void makeDisc( const visualization_msgs::InteractiveMarker &msg, - visualization_msgs::InteractiveMarkerControl &control, float width = 0.3 ); - -/// @brief make a box which shows the given text and is view facing -/// @param msg the interactive marker that this will go into -/// @param text the text to display -void makeViewFacingButton( const visualization_msgs::InteractiveMarker &msg, - visualization_msgs::InteractiveMarkerControl &control, std::string text ); - -/// assign an RGB value to the given marker based on the given orientation -void assignDefaultColor(visualization_msgs::Marker &marker, const geometry_msgs::Quaternion &quat ); - -/// create a control which shows the description of the interactive marker -visualization_msgs::InteractiveMarkerControl makeTitle( const visualization_msgs::InteractiveMarker &msg ); - -} - -#endif diff --git a/include/interactive_markers/tools.hpp b/include/interactive_markers/tools.hpp new file mode 100644 index 00000000..2d01475f --- /dev/null +++ b/include/interactive_markers/tools.hpp @@ -0,0 +1,135 @@ +// Copyright (c) 2011, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef INTERACTIVE_MARKERS__TOOLS_HPP_ +#define INTERACTIVE_MARKERS__TOOLS_HPP_ + +#include + +#include "visualization_msgs/msg/interactive_marker.hpp" + +#include "interactive_markers/visibility_control.hpp" + +namespace interactive_markers +{ + +/// Fill in default values and insert default controls when none are specified. +/** + * This also calls uniqueifyControlNames(). + * + * \param msg[inout] Interactive marker to be completed. + * \param enable_autocomplete_transparency If false, auto-completed markers will have alpha = 1.0. + */ +INTERACTIVE_MARKERS_PUBLIC +void autoComplete( + visualization_msgs::msg::InteractiveMarker & msg, + bool enable_autocomplete_transparency = true); + +/// Fill in default values and insert default controls when none are specified. +/** + * \param msg[in] Interactive marker which contains the control. + * \param control[inout] The control to be completed. + * \param enable_autocomplete_transparency If false, auto-completed markers will have alpha = 1.0. + */ +INTERACTIVE_MARKERS_PUBLIC +void autoComplete( + const visualization_msgs::msg::InteractiveMarker & msg, + visualization_msgs::msg::InteractiveMarkerControl & control, + bool enable_autocomplete_transparency = true); + +/// Make sure all of the control names are unique within the given message. +/** + * Appends "_u0", "_u1", etc. to repeated names (not including the first of each). + * + * \param msg[inout] Interactive marker for which control names are made unique. + */ +INTERACTIVE_MARKERS_PUBLIC +void uniqueifyControlNames(visualization_msgs::msg::InteractiveMarker & msg); + +/// Make a default-style arrow marker. +/** + * \param msg[in] The interactive marker that the arrow markers properties will be based on. + * \param control[inout] The control where the arrow marker is inserted. + * \param pos[in] How far from the center should the arrow be, and on which side. + */ +INTERACTIVE_MARKERS_PUBLIC +void makeArrow( + const visualization_msgs::msg::InteractiveMarker & msg, + visualization_msgs::msg::InteractiveMarkerControl & control, + float pos); + +/// Make a default-style disc marker (e.g for rotating). +/** + * \param msg[in] The interactive marker that the disc markers properties will be based on. + * \param control[inout] The control where the disc marker is inserted. + * \param width[in] The width of the disc relative to its inner radius. + */ +INTERACTIVE_MARKERS_PUBLIC +void makeDisc( + const visualization_msgs::msg::InteractiveMarker & msg, + visualization_msgs::msg::InteractiveMarkerControl & control, + double width = 0.3); + +/// Make view facing button with text. +/** + * \param msg[in] The interactive marker that the buttons properties will be based on. + * \param control[inout] The control where the button is inserted. + * \param text[in] The text to display on the button. + */ +INTERACTIVE_MARKERS_PUBLIC +void makeViewFacingButton( + const visualization_msgs::msg::InteractiveMarker & msg, + visualization_msgs::msg::InteractiveMarkerControl & control, + std::string text); + +/// Assign an RGB value to the given marker based on the given orientation. +/** + * \param marker[inout] The marker to color. + * \param quat[in] The orientation that determines the color. + */ +INTERACTIVE_MARKERS_PUBLIC +void assignDefaultColor( + visualization_msgs::msg::Marker & marker, + const geometry_msgs::msg::Quaternion & quat); + +/// Create a control which shows the description of the interactive marker +/** + * \param msg[in] The interactive marker to describe. + * \return A control that shows the description of the provided interactive marker. + */ +INTERACTIVE_MARKERS_PUBLIC +visualization_msgs::msg::InteractiveMarkerControl makeTitle( + const visualization_msgs::msg::InteractiveMarker & msg); + +} // namespace interactive_markers + +#endif // INTERACTIVE_MARKERS__TOOLS_HPP_ diff --git a/include/interactive_markers/visibility_control.hpp b/include/interactive_markers/visibility_control.hpp new file mode 100644 index 00000000..e9548868 --- /dev/null +++ b/include/interactive_markers/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Open Source Robotics Foundation, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// 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. + +#ifndef INTERACTIVE_MARKERS__VISIBILITY_CONTROL_HPP_ +#define INTERACTIVE_MARKERS__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define INTERACTIVE_MARKERS_EXPORT __attribute__ ((dllexport)) + #define INTERACTIVE_MARKERS_IMPORT __attribute__ ((dllimport)) + #else + #define INTERACTIVE_MARKERS_EXPORT __declspec(dllexport) + #define INTERACTIVE_MARKERS_IMPORT __declspec(dllimport) + #endif + #ifdef INTERACTIVE_MARKERS_BUILDING_LIBRARY + #define INTERACTIVE_MARKERS_PUBLIC INTERACTIVE_MARKERS_EXPORT + #else + #define INTERACTIVE_MARKERS_PUBLIC INTERACTIVE_MARKERS_IMPORT + #endif + #define INTERACTIVE_MARKERS_PUBLIC_TYPE INTERACTIVE_MARKERS_PUBLIC + #define INTERACTIVE_MARKERS_LOCAL +#else + #define INTERACTIVE_MARKERS_EXPORT __attribute__ ((visibility("default"))) + #define INTERACTIVE_MARKERS_IMPORT + #if __GNUC__ >= 4 + #define INTERACTIVE_MARKERS_PUBLIC __attribute__ ((visibility("default"))) + #define INTERACTIVE_MARKERS_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define INTERACTIVE_MARKERS_PUBLIC + #define INTERACTIVE_MARKERS_LOCAL + #endif + #define INTERACTIVE_MARKERS_PUBLIC_TYPE +#endif + +#endif // INTERACTIVE_MARKERS__VISIBILITY_CONTROL_HPP_ diff --git a/package.xml b/package.xml index fbda419b..35c4e790 100644 --- a/package.xml +++ b/package.xml @@ -1,31 +1,31 @@ - + interactive_markers 3D interactive marker communication library for RViz and similar tools. William Woodall - BSD + BSD 1.11.4 David Gossow http://ros.org/wiki/interactive_markers - catkin + ament_cmake - rosconsole - roscpp - rospy - rostest - std_msgs - tf - visualization_msgs + rclcpp + rmw + tf2 + tf2_geometry_msgs + visualization_msgs - rosconsole - roscpp - rospy - rostest - std_msgs - tf - visualization_msgs + ament_cmake_gtest + ament_lint_auto + ament_lint_common + geometry_msgs + std_msgs + + + ament_cmake + diff --git a/src/interactive_marker_client.cpp b/src/interactive_marker_client.cpp index 3e3c5ef4..abb30bc4 100644 --- a/src/interactive_marker_client.cpp +++ b/src/interactive_marker_client.cpp @@ -1,294 +1,424 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: David Gossow - */ - -#include "interactive_markers/interactive_marker_client.h" -#include "interactive_markers/detail/single_client.h" - -#include -#include - -//#define DBG_MSG( ... ) ROS_DEBUG_NAMED( "interactive_markers", __VA_ARGS__ ); -#define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ ); -//#define DBG_MSG( ... ) printf(" "); printf( __VA_ARGS__ ); printf("\n"); +// Copyright (c) 2011, Willow Garage, Inc. +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Author: David Gossow + +#include +#include +#include +#include + +#include "visualization_msgs/srv/get_interactive_markers.hpp" + +#include "interactive_markers/exceptions.hpp" +#include "interactive_markers/interactive_marker_client.hpp" + +using namespace std::placeholders; namespace interactive_markers { -InteractiveMarkerClient::InteractiveMarkerClient( - tf::Transformer& tf, - const std::string& target_frame, - const std::string &topic_ns ) -: state_("InteractiveMarkerClient",IDLE) -, tf_(tf) -, last_num_publishers_(0) -, enable_autocomplete_transparency_(true) +const size_t MAX_UPDATE_QUEUE_SIZE = 100u; + +InteractiveMarkerClient::~InteractiveMarkerClient() { - target_frame_ = target_frame; - if ( !topic_ns.empty() ) - { - subscribe( topic_ns ); +} + +void InteractiveMarkerClient::connect(std::string topic_namespace) +{ + changeState(IDLE); + topic_namespace_ = topic_namespace; + + // Terminate any existing connection + disconnect(); + + // Don't do anything if no namespace is provided + if (topic_namespace_.empty()) { + return; + } + + try { + get_interactive_markers_client_ = + rclcpp::create_client( + node_base_interface_, + graph_interface_, + services_interface_, + topic_namespace_ + "/get_interactive_markers", + rmw_qos_profile_services_default, + nullptr); + + rclcpp::QoS feedback_qos(rclcpp::KeepLast(100)); + feedback_pub_ = rclcpp::create_publisher( + topics_interface_, + topic_namespace_ + "/feedback", + feedback_qos); + + rclcpp::QoS update_qos(rclcpp::KeepLast(100)); + update_sub_ = rclcpp::create_subscription( + topics_interface_, + topic_namespace_ + "/update", + update_qos, + std::bind(&InteractiveMarkerClient::processUpdate, this, _1)); + } catch (rclcpp::exceptions::InvalidNodeError & ex) { + updateStatus(ERROR, "Failed to connect: " + std::string(ex.what())); + disconnect(); + return; + } catch (rclcpp::exceptions::NameValidationError & ex) { + updateStatus(ERROR, "Failed to connect: " + std::string(ex.what())); + disconnect(); + return; } - callbacks_.setStatusCb( boost::bind( &InteractiveMarkerClient::statusCb, this, _1, _2, _3 ) ); + + updateStatus(INFO, "Connected on namespace: " + topic_namespace_); } -InteractiveMarkerClient::~InteractiveMarkerClient() +void InteractiveMarkerClient::disconnect() { - shutdown(); + get_interactive_markers_client_.reset(); + feedback_pub_.reset(); + update_sub_.reset(); + reset(); } -/// Subscribe to given topic -void InteractiveMarkerClient::subscribe( std::string topic_ns ) +void InteractiveMarkerClient::publishFeedback( + visualization_msgs::msg::InteractiveMarkerFeedback feedback) { - topic_ns_ = topic_ns; - subscribeUpdate(); - subscribeInit(); + feedback.client_id = client_id_; + feedback_pub_->publish(feedback); } -void InteractiveMarkerClient::setInitCb( const InitCallback& cb ) +void InteractiveMarkerClient::update() { - callbacks_.setInitCb( cb ); + if (!get_interactive_markers_client_) { + // Disconnected + return; + } + + const bool server_ready = get_interactive_markers_client_->service_is_ready(); + + switch (state_) { + case IDLE: + if (server_ready) { + changeState(INITIALIZE); + } + break; + + case INITIALIZE: + if (!server_ready) { + updateStatus(WARN, "Server not available during initialization, resetting"); + changeState(IDLE); + break; + } + // If there's an unexpected error, reset + if (!transformInitialMessage()) { + changeState(IDLE); + break; + } + if (checkInitializeFinished()) { + changeState(RUNNING); + } + break; + + case RUNNING: + if (!server_ready) { + updateStatus(WARN, "Server not available while running, resetting"); + changeState(IDLE); + break; + } + // If there's an unexpected error, reset + if (!transformUpdateMessages()) { + changeState(IDLE); + break; + } + pushUpdates(); + break; + + default: + updateStatus(ERROR, "Invalid state in update: " + std::to_string(getState())); + } } -void InteractiveMarkerClient::setUpdateCb( const UpdateCallback& cb ) +void InteractiveMarkerClient::setTargetFrame(std::string target_frame) { - callbacks_.setUpdateCb( cb ); + if (target_frame_ == target_frame) { + return; + } + + target_frame_ = target_frame; + updateStatus(INFO, "Target frame is now " + target_frame_); + + // Call reset for change to take effect + // This will cause interactive markers to be requested again from the server + // The additional request might be avoided by doing something else, but this is easier + changeState(IDLE); } -void InteractiveMarkerClient::setResetCb( const ResetCallback& cb ) +void InteractiveMarkerClient::setInitializeCallback(const InitializeCallback & cb) { - callbacks_.setResetCb( cb ); + initialize_callback_ = cb; } -void InteractiveMarkerClient::setStatusCb( const StatusCallback& cb ) +void InteractiveMarkerClient::setUpdateCallback(const UpdateCallback & cb) { - status_cb_ = cb; + update_callback_ = cb; } -void InteractiveMarkerClient::setTargetFrame( std::string target_frame ) +void InteractiveMarkerClient::setResetCallback(const ResetCallback & cb) { - target_frame_ = target_frame; - DBG_MSG("Target frame is now %s", target_frame_.c_str() ); + reset_callback_ = cb; +} - switch ( state_ ) - { - case IDLE: - break; - - case INIT: - case RUNNING: - shutdown(); - subscribeUpdate(); - subscribeInit(); - break; - } +void InteractiveMarkerClient::setStatusCallback(const StatusCallback & cb) +{ + status_callback_ = cb; } -void InteractiveMarkerClient::shutdown() +void InteractiveMarkerClient::reset() { - switch ( state_ ) - { - case IDLE: - break; - - case INIT: - case RUNNING: - init_sub_.shutdown(); - update_sub_.shutdown(); - boost::lock_guard lock(publisher_contexts_mutex_); - publisher_contexts_.clear(); - last_num_publishers_=0; - state_=IDLE; - break; + std::unique_lock lock(mutex_); + state_ = IDLE; + first_update_ = true; + initial_response_msg_.reset(); + update_queue_.clear(); + if (reset_callback_) { + reset_callback_(); } } -void InteractiveMarkerClient::subscribeUpdate() +void InteractiveMarkerClient::requestInteractiveMarkers() { - if ( !topic_ns_.empty() ) - { - try - { - update_sub_ = nh_.subscribe( topic_ns_+"/update", 100, &InteractiveMarkerClient::processUpdate, this ); - DBG_MSG( "Subscribed to update topic: %s", (topic_ns_+"/update").c_str() ); - } - catch( ros::Exception& e ) - { - callbacks_.statusCb( ERROR, "General", "Error subscribing: " + std::string(e.what()) ); - return; - } + if (!get_interactive_markers_client_) { + updateStatus(ERROR, "Interactive markers requested when client is disconnected"); + return; } - callbacks_.statusCb( OK, "General", "Waiting for messages."); + if (!get_interactive_markers_client_->wait_for_service(std::chrono::seconds(1))) { + updateStatus(WARN, "Service is not ready during request for interactive markers"); + return; + } + updateStatus(INFO, "Sending request for interactive markers"); + + auto callback = std::bind(&InteractiveMarkerClient::processInitialMessage, this, _1); + auto request = std::make_shared(); + get_interactive_markers_client_->async_send_request( + request, + callback); + request_time_ = clock_->now(); } -void InteractiveMarkerClient::subscribeInit() +void InteractiveMarkerClient::processInitialMessage( + rclcpp::Client::SharedFuture future) { - if ( state_ != INIT && !topic_ns_.empty() ) + updateStatus(INFO, "Service response received for initialization"); + auto response = future.get(); { - try - { - init_sub_ = nh_.subscribe( topic_ns_+"/update_full", 100, &InteractiveMarkerClient::processInit, this ); - DBG_MSG( "Subscribed to init topic: %s", (topic_ns_+"/update_full").c_str() ); - state_ = INIT; - } - catch( ros::Exception& e ) - { - callbacks_.statusCb( ERROR, "General", "Error subscribing: " + std::string(e.what()) ); - } + std::unique_lock lock(mutex_); + initial_response_msg_ = std::make_shared( + tf_buffer_core_, target_frame_, response, enable_autocomplete_transparency_); } } -template -void InteractiveMarkerClient::process( const MsgConstPtrT& msg ) +void InteractiveMarkerClient::processUpdate( + visualization_msgs::msg::InteractiveMarkerUpdate::SharedPtr msg) { - callbacks_.statusCb( OK, "General", "Receiving messages."); + // Ignore legacy "keep alive" messages + if (msg->type == msg->KEEP_ALIVE) { + RCLCPP_WARN_ONCE( + logger_, + "KEEP_ALIVE message ignored. " + "Servers are no longer expected to publish this type of message."); + return; + } - // get caller ID of the sending entity - if ( msg->server_id.empty() ) - { - callbacks_.statusCb( ERROR, "General", "Received message with empty server_id!"); + if (!first_update_ && msg->seq_num != last_update_sequence_number_ + 1) { + std::ostringstream oss; + oss << "Update sequence number is out of order. " << last_update_sequence_number_ + 1 << + " (expected) vs. " << msg->seq_num << " (received)"; + updateStatus(WARN, oss.str()); + // Change state to IDLE to cause reset + changeState(IDLE); return; } - SingleClientPtr client; - { - boost::lock_guard lock(publisher_contexts_mutex_); + updateStatus(DEBUG, "Received update with sequence number " + std::to_string(msg->seq_num)); - M_SingleClient::iterator context_it = publisher_contexts_.find(msg->server_id); + first_update_ = false; + last_update_sequence_number_ = msg->seq_num; - // If we haven't seen this publisher before, we need to reset the - // display and listen to the init topic, plus of course add this - // publisher to our list. - if ( context_it == publisher_contexts_.end() ) - { - DBG_MSG( "New publisher detected: %s", msg->server_id.c_str() ); + if (update_queue_.size() > MAX_UPDATE_QUEUE_SIZE) { + updateStatus( + WARN, + "Update queue too large. Erasing message with sequence number " + + std::to_string(update_queue_.begin()->msg->seq_num)); + update_queue_.pop_back(); + } - SingleClientPtr pc(new SingleClient( msg->server_id, tf_, target_frame_, callbacks_ )); - context_it = publisher_contexts_.insert( std::make_pair(msg->server_id,pc) ).first; - client = pc; + update_queue_.push_front(UpdateMessageContext( + tf_buffer_core_, target_frame_, msg, enable_autocomplete_transparency_)); +} - // we need to subscribe to the init topic again - subscribeInit(); - } +bool InteractiveMarkerClient::transformInitialMessage() +{ + std::unique_lock lock(mutex_); + if (!initial_response_msg_) { + // We haven't received a response yet + return true; + } - client = context_it->second; + try { + initial_response_msg_->getTfTransforms(); + } catch (const exceptions::TransformError & e) { + std::ostringstream oss; + oss << "Resetting due to transform error: " << e.what(); + updateStatus(ERROR, oss.str()); + return false; } - // forward init/update to respective context - client->process( msg, enable_autocomplete_transparency_ ); + return true; } -void InteractiveMarkerClient::processInit( const InitConstPtr& msg ) +bool InteractiveMarkerClient::transformUpdateMessages() { - process(msg); + std::unique_lock lock(mutex_); + for (auto it = update_queue_.begin(); it != update_queue_.end(); ++it) { + try { + it->getTfTransforms(); + } catch (const exceptions::TransformError & e) { + std::ostringstream oss; + oss << "Resetting due to transform error: " << e.what(); + updateStatus(ERROR, oss.str()); + return false; + } + } + return true; } -void InteractiveMarkerClient::processUpdate( const UpdateConstPtr& msg ) +bool InteractiveMarkerClient::checkInitializeFinished() { - process(msg); + std::unique_lock lock(mutex_); + if (!initial_response_msg_) { + // We haven't received a response yet, check for timeout + if ((clock_->now() - request_time_) > request_timeout_) { + updateStatus( + WARN, "Did not receive response with interactive markers, resending request..."); + requestInteractiveMarkers(); + } + + return false; + } + + const uint64_t & response_sequence_number = initial_response_msg_->msg->sequence_number; + if (!initial_response_msg_->isReady()) { + updateStatus(DEBUG, "Initialization: Waiting for TF info"); + return false; + } + + // Prune old update messages + while (!update_queue_.empty() && update_queue_.back().msg->seq_num <= response_sequence_number) { + updateStatus( + DEBUG, + "Omitting update with sequence number " + std::to_string(update_queue_.back().msg->seq_num)); + update_queue_.pop_back(); + } + + if (initialize_callback_) { + initialize_callback_(initial_response_msg_->msg); + } + + updateStatus(DEBUG, "Initialized"); + + return true; } -void InteractiveMarkerClient::update() +void InteractiveMarkerClient::pushUpdates() { - switch ( state_ ) - { - case IDLE: - break; - - case INIT: - case RUNNING: - { - // check if one publisher has gone offline - if ( update_sub_.getNumPublishers() < last_num_publishers_ ) - { - callbacks_.statusCb( ERROR, "General", "Server is offline. Resetting." ); - shutdown(); - subscribeUpdate(); - subscribeInit(); - return; + std::unique_lock lock(mutex_); + while (!update_queue_.empty() && update_queue_.back().isReady()) { + visualization_msgs::msg::InteractiveMarkerUpdate::SharedPtr msg = update_queue_.back().msg; + updateStatus(DEBUG, "Pushing update with sequence number " + std::to_string(msg->seq_num)); + if (update_callback_) { + update_callback_(msg); } - last_num_publishers_ = update_sub_.getNumPublishers(); - - // check if all single clients are finished with the init channels - bool initialized = true; - boost::lock_guard lock(publisher_contexts_mutex_); - M_SingleClient::iterator it; - for ( it = publisher_contexts_.begin(); it!=publisher_contexts_.end(); ++it ) - { - // Explicitly reference the pointer to the client here, because the client - // might call user code, which might call shutdown(), which will delete - // the publisher_contexts_ map... - - SingleClientPtr single_client = it->second; - single_client->update(); - if ( !single_client->isInitialized() ) - { - initialized = false; - } + update_queue_.pop_back(); + } +} - if ( publisher_contexts_.empty() ) - break; // Yep, someone called shutdown()... - } - if ( state_ == INIT && initialized ) - { - init_sub_.shutdown(); - state_ = RUNNING; - } - if ( state_ == RUNNING && !initialized ) - { - subscribeInit(); - } - break; +void InteractiveMarkerClient::changeState(const State & new_state) +{ + if (state_ == new_state) { + return; } + + updateStatus(DEBUG, "Change state to: " + std::to_string(new_state)); + + switch (new_state) { + case IDLE: + reset(); + break; + + case INITIALIZE: + requestInteractiveMarkers(); + break; + + case RUNNING: + break; + + default: + updateStatus(ERROR, "Invalid state when changing state: " + std::to_string(new_state)); + return; } + state_ = new_state; } -void InteractiveMarkerClient::statusCb( StatusT status, const std::string& server_id, const std::string& msg ) +void InteractiveMarkerClient::updateStatus(const Status status, const std::string & msg) { - switch ( status ) - { - case OK: - DBG_MSG( "%s: %s (Status: OK)", server_id.c_str(), msg.c_str() ); - break; - case WARN: - DBG_MSG( "%s: %s (Status: WARNING)", server_id.c_str(), msg.c_str() ); - break; - case ERROR: - DBG_MSG( "%s: %s (Status: ERROR)", server_id.c_str(), msg.c_str() ); - break; + switch (status) { + case DEBUG: + RCLCPP_DEBUG(logger_, "%s", msg.c_str()); + break; + case INFO: + RCLCPP_INFO(logger_, "%s", msg.c_str()); + break; + case WARN: + RCLCPP_WARN(logger_, "%s", msg.c_str()); + break; + case ERROR: + RCLCPP_ERROR(logger_, "%s", msg.c_str()); + break; } - if ( status_cb_ ) - { - status_cb_( status, server_id, msg ); + if (status_callback_) { + status_callback_(status, msg); } } -} +} // namespace interactive_markers diff --git a/src/interactive_marker_server.cpp b/src/interactive_marker_server.cpp index d960c881..69034312 100644 --- a/src/interactive_marker_server.cpp +++ b/src/interactive_marker_server.cpp @@ -1,200 +1,195 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: David Gossow - */ - -#include "interactive_markers/interactive_marker_server.h" - -#include - -#include -#include +// Copyright (c) 2011, Willow Garage, Inc. +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Author: David Gossow + +#include +#include +#include +#include + +#include "interactive_markers/interactive_marker_server.hpp" + +#include "rmw/rmw.h" +#include "rclcpp/qos.hpp" + +using visualization_msgs::msg::InteractiveMarkerFeedback; +using visualization_msgs::msg::InteractiveMarkerUpdate; namespace interactive_markers { -InteractiveMarkerServer::InteractiveMarkerServer( const std::string &topic_ns, const std::string &server_id, bool spin_thread ) : - topic_ns_(topic_ns), - seq_num_(0) +InteractiveMarkerServer::InteractiveMarkerServer( + const std::string & topic_namespace, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface) +: topic_namespace_(topic_namespace), + context_(base_interface->get_context()), + clock_(clock_interface->get_clock()), + logger_(logging_interface->get_logger()), + sequence_number_(0) { - if ( spin_thread ) - { - // if we're spinning our own thread, we'll also need our own callback queue - node_handle_.setCallbackQueue( &callback_queue_ ); - } - - if (!server_id.empty()) - { - server_id_ = ros::this_node::getName() + "/" + server_id; - } - else - { - server_id_ = ros::this_node::getName(); - } - - std::string update_topic = topic_ns + "/update"; - std::string init_topic = update_topic + "_full"; - std::string feedback_topic = topic_ns + "/feedback"; - - init_pub_ = node_handle_.advertise( init_topic, 100, true ); - update_pub_ = node_handle_.advertise( update_topic, 100 ); - feedback_sub_ = node_handle_.subscribe( feedback_topic, 100, &InteractiveMarkerServer::processFeedback, this ); - - keep_alive_timer_ = node_handle_.createTimer(ros::Duration(0.5f), boost::bind( &InteractiveMarkerServer::keepAlive, this ) ); - - if ( spin_thread ) - { - need_to_terminate_ = false; - spin_thread_.reset( new boost::thread(boost::bind(&InteractiveMarkerServer::spinThread, this)) ); - } - - publishInit(); + const std::string update_topic = topic_namespace + "/update"; + const std::string feedback_topic = topic_namespace + "/feedback"; + + get_interactive_markers_service_ = rclcpp::create_service< + visualization_msgs::srv::GetInteractiveMarkers>( + base_interface, + services_interface, + topic_namespace + "/get_interactive_markers", + std::bind( + &InteractiveMarkerServer::getInteractiveMarkersCallback, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3), + rmw_qos_profile_services_default, + base_interface->get_default_callback_group()); + + rclcpp::QoS update_qos(rclcpp::KeepLast(100)); + // update_qos.durability_volatile(); + update_pub_ = rclcpp::create_publisher( + topics_interface, update_topic, update_qos); + + rclcpp::QoS feedback_qos(rclcpp::KeepLast(100)); + // feedback_qos.durability_volatile(); + feedback_sub_ = rclcpp::create_subscription( + topics_interface, + feedback_topic, + feedback_qos, + std::bind(&InteractiveMarkerServer::processFeedback, this, std::placeholders::_1)); } - InteractiveMarkerServer::~InteractiveMarkerServer() { - if (spin_thread_.get()) - { - need_to_terminate_ = true; - spin_thread_->join(); - } - - if ( node_handle_.ok() ) - { + if (rclcpp::ok(context_)) { clear(); applyChanges(); } } - -void InteractiveMarkerServer::spinThread() -{ - while (node_handle_.ok()) - { - if (need_to_terminate_) - { - break; - } - callback_queue_.callAvailable(ros::WallDuration(0.033f)); - } -} - - void InteractiveMarkerServer::applyChanges() { - boost::recursive_mutex::scoped_lock lock( mutex_ ); + std::unique_lock lock(mutex_); - if ( pending_updates_.empty() ) - { + if (pending_updates_.empty()) { + RCLCPP_INFO(logger_, "No changes to apply"); return; } - M_UpdateContext::iterator update_it; + visualization_msgs::msg::InteractiveMarkerUpdate update; + update.type = visualization_msgs::msg::InteractiveMarkerUpdate::UPDATE; - visualization_msgs::InteractiveMarkerUpdate update; - update.type = visualization_msgs::InteractiveMarkerUpdate::UPDATE; + update.markers.reserve(marker_contexts_.size()); + update.poses.reserve(marker_contexts_.size()); + update.erases.reserve(marker_contexts_.size()); - update.markers.reserve( marker_contexts_.size() ); - update.poses.reserve( marker_contexts_.size() ); - update.erases.reserve( marker_contexts_.size() ); - - for ( update_it = pending_updates_.begin(); update_it != pending_updates_.end(); update_it++ ) + for ( + auto update_it = pending_updates_.begin(); + update_it != pending_updates_.end(); + update_it++) { - M_MarkerContext::iterator marker_context_it = marker_contexts_.find( update_it->first ); + M_MarkerContext::iterator marker_context_it = marker_contexts_.find(update_it->first); - switch ( update_it->second.update_type ) - { + switch (update_it->second.update_type) { case UpdateContext::FULL_UPDATE: - { - if ( marker_context_it == marker_contexts_.end() ) { - ROS_DEBUG("Creating new context for %s", update_it->first.c_str()); - // create a new int_marker context - marker_context_it = marker_contexts_.insert( std::make_pair( update_it->first, MarkerContext() ) ).first; - // copy feedback cbs, in case they have been set before the marker context was created - marker_context_it->second.default_feedback_cb = update_it->second.default_feedback_cb; - marker_context_it->second.feedback_cbs = update_it->second.feedback_cbs; + if (marker_context_it == marker_contexts_.end()) { + RCLCPP_INFO(logger_, "Creating new context for %s", update_it->first.c_str()); + // create a new int_marker context + marker_context_it = marker_contexts_.insert( + std::make_pair(update_it->first, MarkerContext())).first; + // Initialize fields + marker_context_it->second.last_feedback = rclcpp::Time(0, 0u, clock_->get_clock_type()); + marker_context_it->second.default_feedback_cb = update_it->second.default_feedback_cb; + marker_context_it->second.feedback_cbs = update_it->second.feedback_cbs; + } else { + RCLCPP_INFO(logger_, "Updating existing context for '%s'", update_it->first.c_str()); + } + marker_context_it->second.int_marker = update_it->second.int_marker; + update.markers.push_back(marker_context_it->second.int_marker); + break; } - marker_context_it->second.int_marker = update_it->second.int_marker; - - update.markers.push_back( marker_context_it->second.int_marker ); - break; - } - case UpdateContext::POSE_UPDATE: - { - if ( marker_context_it == marker_contexts_.end() ) - { - ROS_ERROR( "Pending pose update for non-existing marker found. This is a bug in InteractiveMarkerInterface." ); - } - else { - marker_context_it->second.int_marker.pose = update_it->second.int_marker.pose; - marker_context_it->second.int_marker.header = update_it->second.int_marker.header; - - visualization_msgs::InteractiveMarkerPose pose_update; - pose_update.header = marker_context_it->second.int_marker.header; - pose_update.pose = marker_context_it->second.int_marker.pose; - pose_update.name = marker_context_it->second.int_marker.name; - update.poses.push_back( pose_update ); + if (marker_context_it == marker_contexts_.end()) { + RCLCPP_ERROR( + logger_, + "Pending pose update for non-existing marker found. This is a bug in " + "InteractiveMarkerInterface."); + } else { + RCLCPP_INFO(logger_, "Updating pose for '%s'", update_it->first.c_str()); + marker_context_it->second.int_marker.pose = update_it->second.int_marker.pose; + marker_context_it->second.int_marker.header = update_it->second.int_marker.header; + + visualization_msgs::msg::InteractiveMarkerPose pose_update; + pose_update.header = marker_context_it->second.int_marker.header; + pose_update.pose = marker_context_it->second.int_marker.pose; + pose_update.name = marker_context_it->second.int_marker.name; + update.poses.push_back(pose_update); + } + break; } - break; - } case UpdateContext::ERASE: - { - if ( marker_context_it != marker_contexts_.end() ) { - marker_contexts_.erase( update_it->first ); - update.erases.push_back( update_it->first ); + RCLCPP_INFO(logger_, "Erasing '%s'", update_it->first.c_str()); + if (marker_context_it != marker_contexts_.end()) { + marker_contexts_.erase(update_it->first); + update.erases.push_back(update_it->first); + } + break; } - break; - } } } - seq_num_++; + sequence_number_++; - publish( update ); - publishInit(); + publish(update); pending_updates_.clear(); } - -bool InteractiveMarkerServer::erase( const std::string &name ) +bool InteractiveMarkerServer::erase(const std::string & name) { - boost::recursive_mutex::scoped_lock lock( mutex_ ); + std::unique_lock lock(mutex_); - if (marker_contexts_.end() == marker_contexts_.find(name) && - pending_updates_.end() == pending_updates_.find(name)) + if ( + marker_contexts_.end() == marker_contexts_.find(name) && + pending_updates_.end() == pending_updates_.find(name)) { return false; } @@ -204,294 +199,277 @@ bool InteractiveMarkerServer::erase( const std::string &name ) void InteractiveMarkerServer::clear() { - boost::recursive_mutex::scoped_lock lock( mutex_ ); + std::unique_lock lock(mutex_); // erase all markers pending_updates_.clear(); M_MarkerContext::iterator it; - for ( it = marker_contexts_.begin(); it != marker_contexts_.end(); it++ ) - { + for (it = marker_contexts_.begin(); it != marker_contexts_.end(); it++) { pending_updates_[it->first].update_type = UpdateContext::ERASE; } } - bool InteractiveMarkerServer::empty() const { return marker_contexts_.empty(); } - std::size_t InteractiveMarkerServer::size() const { return marker_contexts_.size(); } - -bool InteractiveMarkerServer::setPose( const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header ) +bool InteractiveMarkerServer::setPose( + const std::string & name, + const geometry_msgs::msg::Pose & pose, + const std_msgs::msg::Header & header) { - boost::recursive_mutex::scoped_lock lock( mutex_ ); + std::unique_lock lock(mutex_); - M_MarkerContext::iterator marker_context_it = marker_contexts_.find( name ); - M_UpdateContext::iterator update_it = pending_updates_.find( name ); + M_MarkerContext::iterator marker_context_it = marker_contexts_.find(name); + M_UpdateContext::iterator update_it = pending_updates_.find(name); // if there's no marker and no pending addition for it, we can't update the pose - if ( marker_context_it == marker_contexts_.end() && - ( update_it == pending_updates_.end() || update_it->second.update_type != UpdateContext::FULL_UPDATE ) ) + if ( + marker_context_it == marker_contexts_.end() && + (update_it == pending_updates_.end() || + update_it->second.update_type != UpdateContext::FULL_UPDATE)) { return false; } // keep the old header - if ( header.frame_id.empty() ) - { - if ( marker_context_it != marker_contexts_.end() ) - { - doSetPose( update_it, name, pose, marker_context_it->second.int_marker.header ); - } - else if ( update_it != pending_updates_.end() ) - { - doSetPose( update_it, name, pose, update_it->second.int_marker.header ); - } - else - { - BOOST_ASSERT_MSG(false, "Marker does not exist and there is no pending creation."); + if (header.frame_id.empty()) { + if (marker_context_it != marker_contexts_.end()) { + doSetPose(update_it, name, pose, marker_context_it->second.int_marker.header); + } else if (update_it != pending_updates_.end()) { + doSetPose(update_it, name, pose, update_it->second.int_marker.header); + } else { + RCLCPP_WARN(logger_, "Marker does not exist and there is no pending creation."); return false; } - } - else - { - doSetPose( update_it, name, pose, header ); + } else { + doSetPose(update_it, name, pose, header); } return true; } -bool InteractiveMarkerServer::setCallback( const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type ) +bool InteractiveMarkerServer::setCallback( + const std::string & name, + FeedbackCallback feedback_cb, + uint8_t feedback_type) { - boost::recursive_mutex::scoped_lock lock( mutex_ ); + std::unique_lock lock(mutex_); - M_MarkerContext::iterator marker_context_it = marker_contexts_.find( name ); - M_UpdateContext::iterator update_it = pending_updates_.find( name ); + M_MarkerContext::iterator marker_context_it = marker_contexts_.find(name); + M_UpdateContext::iterator update_it = pending_updates_.find(name); - if ( marker_context_it == marker_contexts_.end() && update_it == pending_updates_.end() ) - { + if (marker_context_it == marker_contexts_.end() && update_it == pending_updates_.end()) { return false; } // we need to overwrite both the callbacks for the actual marker // and the update, if there's any - - if ( marker_context_it != marker_contexts_.end() ) - { + if (marker_context_it != marker_contexts_.end()) { // the marker exists, so we can just overwrite the existing callbacks - if ( feedback_type == DEFAULT_FEEDBACK_CB ) - { + if (feedback_type == DEFAULT_FEEDBACK_CB) { + RCLCPP_INFO(logger_, "Replacing default callback for marker '%s'", name.c_str()); marker_context_it->second.default_feedback_cb = feedback_cb; - } - else - { - if ( feedback_cb ) - { + } else { + if (feedback_cb) { + RCLCPP_INFO( + logger_, "Replacing callback type %u for marker '%s'", feedback_type, name.c_str()); marker_context_it->second.feedback_cbs[feedback_type] = feedback_cb; - } - else - { - marker_context_it->second.feedback_cbs.erase( feedback_type ); + } else { + RCLCPP_INFO(logger_, "Erasing callback for marker '%s'", name.c_str()); + marker_context_it->second.feedback_cbs.erase(feedback_type); } } } - if ( update_it != pending_updates_.end() ) - { - if ( feedback_type == DEFAULT_FEEDBACK_CB ) - { + if (update_it != pending_updates_.end()) { + if (feedback_type == DEFAULT_FEEDBACK_CB) { + RCLCPP_INFO(logger_, "Setting default callback for marker '%s'", name.c_str()); update_it->second.default_feedback_cb = feedback_cb; - } - else - { - if ( feedback_cb ) - { + } else { + if (feedback_cb) { update_it->second.feedback_cbs[feedback_type] = feedback_cb; - } - else - { - update_it->second.feedback_cbs.erase( feedback_type ); + RCLCPP_INFO( + logger_, "Setting callback type %u for marker '%s'", feedback_type, name.c_str()); + } else { + RCLCPP_INFO(logger_, "Erasing callback for marker '%s'", name.c_str()); + update_it->second.feedback_cbs.erase(feedback_type); } } } return true; } -void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker ) +void InteractiveMarkerServer::insert(const visualization_msgs::msg::InteractiveMarker & marker) { - boost::recursive_mutex::scoped_lock lock( mutex_ ); + std::unique_lock lock(mutex_); - M_UpdateContext::iterator update_it = pending_updates_.find( int_marker.name ); - if ( update_it == pending_updates_.end() ) - { - update_it = pending_updates_.insert( std::make_pair( int_marker.name, UpdateContext() ) ).first; + M_UpdateContext::iterator update_it = pending_updates_.find(marker.name); + if (update_it == pending_updates_.end()) { + update_it = pending_updates_.insert(std::make_pair(marker.name, UpdateContext())).first; } update_it->second.update_type = UpdateContext::FULL_UPDATE; - update_it->second.int_marker = int_marker; + update_it->second.int_marker = marker; + RCLCPP_INFO(logger_, "Marker inserted with name '%s'", marker.name.c_str()); } -void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker, - FeedbackCallback feedback_cb, uint8_t feedback_type) +void InteractiveMarkerServer::insert( + const visualization_msgs::msg::InteractiveMarker & marker, + FeedbackCallback feedback_cb, + uint8_t feedback_type) { - insert( int_marker ); + insert(marker); - setCallback( int_marker.name, feedback_cb, feedback_type ); + setCallback(marker.name, feedback_cb, feedback_type); } -bool InteractiveMarkerServer::get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const +bool InteractiveMarkerServer::get( + std::string name, + visualization_msgs::msg::InteractiveMarker & marker) const { - boost::recursive_mutex::scoped_lock lock( mutex_ ); + std::unique_lock lock(mutex_); - M_UpdateContext::const_iterator update_it = pending_updates_.find( name ); + M_UpdateContext::const_iterator update_it = pending_updates_.find(name); - if ( update_it == pending_updates_.end() ) - { - M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find( name ); - if ( marker_context_it == marker_contexts_.end() ) - { + if (update_it == pending_updates_.end()) { + M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find(name); + if (marker_context_it == marker_contexts_.end()) { return false; } - int_marker = marker_context_it->second.int_marker; + marker = marker_context_it->second.int_marker; return true; } // if there's an update pending, we'll have to account for that - switch ( update_it->second.update_type ) - { + switch (update_it->second.update_type) { case UpdateContext::ERASE: return false; case UpdateContext::POSE_UPDATE: - { - M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find( name ); - if ( marker_context_it == marker_contexts_.end() ) { - return false; + M_MarkerContext::const_iterator marker_context_it = marker_contexts_.find(name); + if (marker_context_it == marker_contexts_.end()) { + return false; + } + marker = marker_context_it->second.int_marker; + marker.pose = update_it->second.int_marker.pose; + return true; } - int_marker = marker_context_it->second.int_marker; - int_marker.pose = update_it->second.int_marker.pose; - return true; - } case UpdateContext::FULL_UPDATE: - int_marker = update_it->second.int_marker; + marker = update_it->second.int_marker; return true; } return false; } -void InteractiveMarkerServer::publishInit() +void InteractiveMarkerServer::getInteractiveMarkersCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) { - boost::recursive_mutex::scoped_lock lock( mutex_ ); - - visualization_msgs::InteractiveMarkerInit init; - init.server_id = server_id_; - init.seq_num = seq_num_; - init.markers.reserve( marker_contexts_.size() ); + (void)request_header; + (void)request; + RCLCPP_INFO(logger_, "Responding to request to get interactive markers"); + response->sequence_number = sequence_number_; + response->markers.reserve(marker_contexts_.size()); M_MarkerContext::iterator it; - for ( it = marker_contexts_.begin(); it != marker_contexts_.end(); it++ ) - { - ROS_DEBUG( "Publishing %s", it->second.int_marker.name.c_str() ); - init.markers.push_back( it->second.int_marker ); + for (it = marker_contexts_.begin(); it != marker_contexts_.end(); it++) { + RCLCPP_INFO(logger_, "Sending marker '%s'", it->second.int_marker.name.c_str()); + response->markers.push_back(it->second.int_marker); } - - init_pub_.publish( init ); } -void InteractiveMarkerServer::processFeedback( const FeedbackConstPtr& feedback ) +void InteractiveMarkerServer::processFeedback( + visualization_msgs::msg::InteractiveMarkerFeedback::SharedPtr feedback) { - boost::recursive_mutex::scoped_lock lock( mutex_ ); + RCLCPP_INFO(logger_, "Feedback message received"); + std::unique_lock lock(mutex_); - M_MarkerContext::iterator marker_context_it = marker_contexts_.find( feedback->marker_name ); + M_MarkerContext::iterator marker_context_it = marker_contexts_.find(feedback->marker_name); // ignore feedback for non-existing markers - if ( marker_context_it == marker_contexts_.end() ) - { + if (marker_context_it == marker_contexts_.end()) { return; } - MarkerContext &marker_context = marker_context_it->second; + MarkerContext & marker_context = marker_context_it->second; // if two callers try to modify the same marker, reject (timeout= 1 sec) - if ( marker_context.last_client_id != feedback->client_id && - (ros::Time::now() - marker_context.last_feedback).toSec() < 1.0 ) + if (marker_context.last_client_id != feedback->client_id && + (clock_->now() - marker_context.last_feedback).seconds() < 1.0) { - ROS_DEBUG( "Rejecting feedback for %s: conflicting feedback from separate clients.", feedback->marker_name.c_str() ); + RCLCPP_INFO(logger_, "Rejecting feedback for %s: conflicting feedback from separate clients.", + feedback->marker_name.c_str()); return; } - marker_context.last_feedback = ros::Time::now(); + marker_context.last_feedback = clock_->now(); marker_context.last_client_id = feedback->client_id; - if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE ) - { - if ( marker_context.int_marker.header.stamp == ros::Time(0) ) - { + if (feedback->event_type == visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE) { + if (marker_context.int_marker.header.stamp == rclcpp::Time()) { // keep the old header - doSetPose( pending_updates_.find( feedback->marker_name ), feedback->marker_name, feedback->pose, marker_context.int_marker.header ); - } - else - { - doSetPose( pending_updates_.find( feedback->marker_name ), feedback->marker_name, feedback->pose, feedback->header ); + doSetPose(pending_updates_.find( + feedback->marker_name), feedback->marker_name, feedback->pose, + marker_context.int_marker.header); + } else { + doSetPose(pending_updates_.find( + feedback->marker_name), feedback->marker_name, feedback->pose, feedback->header); } } // call feedback handler - boost::unordered_map::iterator feedback_cb_it = marker_context.feedback_cbs.find( feedback->event_type ); - if ( feedback_cb_it != marker_context.feedback_cbs.end() && feedback_cb_it->second ) - { + std::unordered_map::iterator feedback_cb_it = + marker_context.feedback_cbs.find(feedback->event_type); + if (feedback_cb_it != marker_context.feedback_cbs.end() && feedback_cb_it->second) { // call type-specific callback - feedback_cb_it->second( feedback ); - } - else if ( marker_context.default_feedback_cb ) - { + RCLCPP_INFO( + logger_, + "Calling feedback callback %u for marker '%s'", + feedback->event_type, + feedback->marker_name.c_str()); + feedback_cb_it->second(feedback); + } else if (marker_context.default_feedback_cb) { // call default callback - marker_context.default_feedback_cb( feedback ); + RCLCPP_INFO( + logger_, "Calling default feedback callback for marker '%s'", feedback->marker_name.c_str()); + marker_context.default_feedback_cb(feedback); } } - -void InteractiveMarkerServer::keepAlive() +void InteractiveMarkerServer::publish(visualization_msgs::msg::InteractiveMarkerUpdate & update) { - visualization_msgs::InteractiveMarkerUpdate empty_update; - empty_update.type = visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE; - publish( empty_update ); + update.seq_num = sequence_number_; + update_pub_->publish(update); } - -void InteractiveMarkerServer::publish( visualization_msgs::InteractiveMarkerUpdate &update ) -{ - update.server_id = server_id_; - update.seq_num = seq_num_; - update_pub_.publish( update ); -} - - -void InteractiveMarkerServer::doSetPose( M_UpdateContext::iterator update_it, const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header ) +void InteractiveMarkerServer::doSetPose( + M_UpdateContext::iterator update_it, + const std::string & name, + const geometry_msgs::msg::Pose & pose, + const std_msgs::msg::Header & header) { - if ( update_it == pending_updates_.end() ) - { - update_it = pending_updates_.insert( std::make_pair( name, UpdateContext() ) ).first; + if (update_it == pending_updates_.end()) { + update_it = pending_updates_.insert(std::make_pair(name, UpdateContext())).first; update_it->second.update_type = UpdateContext::POSE_UPDATE; - } - else if ( update_it->second.update_type != UpdateContext::FULL_UPDATE ) - { + } else if (update_it->second.update_type != UpdateContext::FULL_UPDATE) { update_it->second.update_type = UpdateContext::POSE_UPDATE; } update_it->second.int_marker.pose = pose; update_it->second.int_marker.header = header; - ROS_DEBUG( "Marker '%s' is now at %f, %f, %f", update_it->first.c_str(), pose.position.x, pose.position.y, pose.position.z ); + RCLCPP_INFO(logger_, "Marker '%s' is now at %f, %f, %f", + update_it->first.c_str(), pose.position.x, pose.position.y, pose.position.z); } - -} +} // namespace interactive_markers diff --git a/src/interactive_markers/interactive_marker_server.py b/src/interactive_markers/interactive_marker_server.py index 542fab30..4c8c03e0 100644 --- a/src/interactive_markers/interactive_marker_server.py +++ b/src/interactive_markers/interactive_marker_server.py @@ -3,28 +3,33 @@ # Copyright (c) 2011, Willow Garage, Inc. # All rights reserved. # +# Software License Agreement (BSD License 2.0) +# # Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: +# modification, are permitted provided that the following conditions +# are met: # -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# * 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 Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. # -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# 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: Michael Ferguson diff --git a/src/interactive_markers/menu_handler.py b/src/interactive_markers/menu_handler.py index 77a96bc4..3c36df61 100644 --- a/src/interactive_markers/menu_handler.py +++ b/src/interactive_markers/menu_handler.py @@ -3,28 +3,33 @@ # Copyright (c) 2011, Willow Garage, Inc. # All rights reserved. # +# Software License Agreement (BSD License 2.0) +# # Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: +# modification, are permitted provided that the following conditions +# are met: # -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# * 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 Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. # -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# 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: Michael Ferguson diff --git a/src/menu_handler.cpp b/src/menu_handler.cpp index cb6d60fb..773d887e 100644 --- a/src/menu_handler.cpp +++ b/src/menu_handler.cpp @@ -1,102 +1,130 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: David Gossow - */ - -#include "interactive_markers/menu_handler.h" - -#include -#include +// Copyright (c) 2011, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Author: David Gossow + +#include +#include +#include +#include +#include +#include + +#include "interactive_markers/menu_handler.hpp" + +// TODO(jacobperron): Remove this macro when it is available upstream +// See: https://github.com/ros2/rcutils/pull/112 +#ifndef RCUTILS_ASSERT_MSG +#define RCUTILS_ASSERT_MSG(cond, ...) \ + do { \ + if (!(cond)) { \ + RCUTILS_LOG_FATAL("ASSERTION FAILED\n\tfile = %s\n\tline = %d\n\tcond = %s\n\tmessage = ", \ + __FILE__, __LINE__, #cond); \ + RCUTILS_LOG_FATAL(__VA_ARGS__); \ + RCUTILS_LOG_FATAL("\n"); \ + std::terminate(); \ + } \ + } while (false) +#endif namespace interactive_markers { -MenuHandler::MenuHandler() : - current_handle_(1) +MenuHandler::MenuHandler() +: current_handle_(1) { - } -MenuHandler::EntryHandle MenuHandler::insert( const std::string &title, - const FeedbackCallback &feedback_cb ) +MenuHandler::EntryHandle MenuHandler::insert( + const std::string & title, + const FeedbackCallback & feedback_cb) { - EntryHandle handle = doInsert( title, visualization_msgs::MenuEntry::FEEDBACK, "", feedback_cb ); - top_level_handles_.push_back( handle ); + EntryHandle handle = + doInsert(title, visualization_msgs::msg::MenuEntry::FEEDBACK, "", feedback_cb); + top_level_handles_.push_back(handle); return handle; } -MenuHandler::EntryHandle MenuHandler::insert( const std::string &title, - const uint8_t command_type, - const std::string &command ) +MenuHandler::EntryHandle MenuHandler::insert( + const std::string & title, + const uint8_t command_type, + const std::string & command) { - EntryHandle handle = doInsert( title, command_type, command, FeedbackCallback() ); - top_level_handles_.push_back( handle ); + EntryHandle handle = doInsert(title, command_type, command, FeedbackCallback()); + top_level_handles_.push_back(handle); return handle; } -MenuHandler::EntryHandle MenuHandler::insert( EntryHandle parent, const std::string &title, - const FeedbackCallback &feedback_cb ) +MenuHandler::EntryHandle MenuHandler::insert( + EntryHandle parent, const std::string & title, + const FeedbackCallback & feedback_cb) { - boost::unordered_map::iterator parent_context = - entry_contexts_.find( parent ); + std::unordered_map::iterator parent_context = + entry_contexts_.find(parent); - ROS_ASSERT_MSG ( parent_context != entry_contexts_.end(), "Parent menu entry %u not found.", parent ); + RCUTILS_ASSERT_MSG( + parent_context != entry_contexts_.end(), "Parent menu entry %u not found.", parent); - EntryHandle handle = doInsert( title, visualization_msgs::MenuEntry::FEEDBACK, "", feedback_cb ); - parent_context->second.sub_entries.push_back( handle ); + EntryHandle handle = + doInsert(title, visualization_msgs::msg::MenuEntry::FEEDBACK, "", feedback_cb); + parent_context->second.sub_entries.push_back(handle); return handle; } -MenuHandler::EntryHandle MenuHandler::insert( EntryHandle parent, const std::string &title, - const uint8_t command_type, - const std::string &command ) +MenuHandler::EntryHandle MenuHandler::insert( + EntryHandle parent, const std::string & title, + const uint8_t command_type, + const std::string & command) { - boost::unordered_map::iterator parent_context = - entry_contexts_.find( parent ); + std::unordered_map::iterator parent_context = + entry_contexts_.find(parent); - ROS_ASSERT_MSG ( parent_context != entry_contexts_.end(), "Parent menu entry %u not found.", parent ); + RCUTILS_ASSERT_MSG( + parent_context != entry_contexts_.end(), "Parent menu entry %u not found.", parent); - EntryHandle handle = doInsert( title, command_type, command, FeedbackCallback() ); - parent_context->second.sub_entries.push_back( handle ); + EntryHandle handle = doInsert(title, command_type, command, FeedbackCallback()); + parent_context->second.sub_entries.push_back(handle); return handle; } -bool MenuHandler::setVisible( EntryHandle handle, bool visible ) +bool MenuHandler::setVisible(EntryHandle handle, bool visible) { - boost::unordered_map::iterator context = - entry_contexts_.find( handle ); + std::unordered_map::iterator context = + entry_contexts_.find(handle); - if ( context == entry_contexts_.end() ) - { + if (context == entry_contexts_.end()) { return false; } @@ -105,13 +133,12 @@ bool MenuHandler::setVisible( EntryHandle handle, bool visible ) } -bool MenuHandler::setCheckState( EntryHandle handle, CheckState check_state ) +bool MenuHandler::setCheckState(EntryHandle handle, CheckState check_state) { - boost::unordered_map::iterator context = - entry_contexts_.find( handle ); + std::unordered_map::iterator context = + entry_contexts_.find(handle); - if ( context == entry_contexts_.end() ) - { + if (context == entry_contexts_.end()) { return false; } @@ -120,13 +147,12 @@ bool MenuHandler::setCheckState( EntryHandle handle, CheckState check_state ) } -bool MenuHandler::getCheckState( EntryHandle handle, CheckState &check_state ) const +bool MenuHandler::getCheckState(EntryHandle handle, CheckState & check_state) const { - boost::unordered_map::const_iterator context = - entry_contexts_.find( handle ); + std::unordered_map::const_iterator context = + entry_contexts_.find(handle); - if ( context == entry_contexts_.end() ) - { + if (context == entry_contexts_.end()) { check_state = NO_CHECKBOX; return false; } @@ -136,79 +162,78 @@ bool MenuHandler::getCheckState( EntryHandle handle, CheckState &check_state ) c } -bool MenuHandler::apply( InteractiveMarkerServer &server, const std::string &marker_name ) +bool MenuHandler::apply(InteractiveMarkerServer & server, const std::string & marker_name) { - visualization_msgs::InteractiveMarker int_marker; + visualization_msgs::msg::InteractiveMarker int_marker; - if ( !server.get( marker_name, int_marker ) ) - { + if (!server.get(marker_name, int_marker)) { // This marker has been deleted on the server, so forget it. - managed_markers_.erase( marker_name ); + managed_markers_.erase(marker_name); return false; } int_marker.menu_entries.clear(); - pushMenuEntries( top_level_handles_, int_marker.menu_entries, 0 ); + pushMenuEntries(top_level_handles_, int_marker.menu_entries, 0); - server.insert( int_marker ); - server.setCallback( marker_name, boost::bind( &MenuHandler::processFeedback, this, _1 ), visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT ); - managed_markers_.insert( marker_name ); + server.insert(int_marker); + server.setCallback( + marker_name, + std::bind(&MenuHandler::processFeedback, this, std::placeholders::_1), + visualization_msgs::msg::InteractiveMarkerFeedback::MENU_SELECT); + managed_markers_.insert(marker_name); return true; } -bool MenuHandler::pushMenuEntries( std::vector& handles_in, - std::vector& entries_out, - EntryHandle parent_handle ) +bool MenuHandler::pushMenuEntries( + std::vector & handles_in, + std::vector & entries_out, + EntryHandle parent_handle) { - for ( unsigned t = 0; t < handles_in.size(); t++ ) - { + for (unsigned t = 0; t < handles_in.size(); t++) { EntryHandle handle = handles_in[t]; - boost::unordered_map::iterator context_it = - entry_contexts_.find( handle ); + std::unordered_map::iterator context_it = + entry_contexts_.find(handle); - if ( context_it == entry_contexts_.end() ) - { - ROS_ERROR( "Internal error: context handle not found! This is a bug in MenuHandler." ); + if (context_it == entry_contexts_.end()) { + RCUTILS_LOG_ERROR("Internal error: context handle not found! This is a bug in MenuHandler."); return false; } - EntryContext& context = context_it->second; + EntryContext & context = context_it->second; - if ( !context.visible ) - { + if (!context.visible) { continue; } - entries_out.push_back( makeEntry( context, handle, parent_handle )); - if( false == pushMenuEntries( context.sub_entries, entries_out, handle )) - { + entries_out.push_back(makeEntry(context, handle, parent_handle)); + if (false == pushMenuEntries(context.sub_entries, entries_out, handle)) { return false; } } return true; } -bool MenuHandler::reApply( InteractiveMarkerServer &server ) +bool MenuHandler::reApply(InteractiveMarkerServer & server) { bool success = true; std::set::iterator it = managed_markers_.begin(); - while ( it != managed_markers_.end() ) - { + while (it != managed_markers_.end()) { // apply() may delete the entry "it" is pointing to, so // pre-compute the next iterator. std::set::iterator next_it = it; next_it++; - success = apply( server, *it ) && success; + success = apply(server, *it) && success; it = next_it; } return success; } -MenuHandler::EntryHandle MenuHandler::doInsert( const std::string &title, - const uint8_t command_type, - const std::string &command, - const FeedbackCallback &feedback_cb ) +MenuHandler::EntryHandle MenuHandler::doInsert( + const std::string & title, + const uint8_t command_type, + const std::string & command, + const FeedbackCallback & feedback_cb) { EntryHandle handle = current_handle_; current_handle_++; @@ -225,20 +250,22 @@ MenuHandler::EntryHandle MenuHandler::doInsert( const std::string &title, return handle; } -visualization_msgs::MenuEntry MenuHandler::makeEntry( EntryContext& context, EntryHandle handle, EntryHandle parent_handle ) +visualization_msgs::msg::MenuEntry MenuHandler::makeEntry( + EntryContext & context, + EntryHandle handle, + EntryHandle parent_handle) { - visualization_msgs::MenuEntry menu_entry; + visualization_msgs::msg::MenuEntry menu_entry; - switch ( context.check_state ) - { + switch (context.check_state) { case NO_CHECKBOX: menu_entry.title = context.title; break; case CHECKED: - menu_entry.title = "[x] "+context.title; + menu_entry.title = "[x] " + context.title; break; case UNCHECKED: - menu_entry.title = "[ ] "+context.title; + menu_entry.title = "[ ] " + context.title; break; } @@ -251,24 +278,23 @@ visualization_msgs::MenuEntry MenuHandler::makeEntry( EntryContext& context, Ent } -void MenuHandler::processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) +void MenuHandler::processFeedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) { - boost::unordered_map::iterator context = - entry_contexts_.find( (EntryHandle) feedback->menu_entry_id ); + std::unordered_map::iterator context = + entry_contexts_.find( (EntryHandle) feedback->menu_entry_id); - if ( context != entry_contexts_.end() && context->second.feedback_cb ) - { - context->second.feedback_cb( feedback ); + if (context != entry_contexts_.end() && context->second.feedback_cb) { + context->second.feedback_cb(feedback); } } -bool MenuHandler::getTitle( EntryHandle handle, std::string &title ) const +bool MenuHandler::getTitle(EntryHandle handle, std::string & title) const { - boost::unordered_map::const_iterator context = - entry_contexts_.find( handle ); + std::unordered_map::const_iterator context = + entry_contexts_.find(handle); - if ( context == entry_contexts_.end() ) - { + if (context == entry_contexts_.end()) { return false; } @@ -276,6 +302,4 @@ bool MenuHandler::getTitle( EntryHandle handle, std::string &title ) const return true; } - - -} +} // namespace interactive_markers diff --git a/src/message_context.cpp b/src/message_context.cpp index 75b2a981..dda6b47d 100644 --- a/src/message_context.cpp +++ b/src/message_context.cpp @@ -1,63 +1,73 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: David Gossow - */ - -#include "interactive_markers/detail/message_context.h" -#include "interactive_markers/tools.h" - -#include - -#define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ ); -//#define DBG_MSG( ... ) printf(" "); printf( __VA_ARGS__ ); printf("\n"); +// Copyright (c) 2011, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Author: David Gossow + +#include + +#include +#include +#include +#include + +#include "tf2/buffer_core_interface.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "visualization_msgs/srv/get_interactive_markers.hpp" + +#include "interactive_markers/exceptions.hpp" +#include "interactive_markers/message_context.hpp" +#include "interactive_markers/tools.hpp" namespace interactive_markers { template MessageContext::MessageContext( - tf::Transformer& tf, - const std::string& target_frame, - const typename MsgT::ConstPtr& _msg, - bool enable_autocomplete_transparency) -: tf_(tf) -, target_frame_(target_frame) -, enable_autocomplete_transparency_(enable_autocomplete_transparency) + std::shared_ptr tf_buffer_core, + const std::string & target_frame, + typename MsgT::SharedPtr _msg, + bool enable_autocomplete_transparency) +: tf_buffer_core_(tf_buffer_core), + target_frame_(target_frame), + enable_autocomplete_transparency_(enable_autocomplete_transparency) { // copy message, as we will be modifying it - msg = boost::make_shared( *_msg ); + msg = std::make_shared(*_msg); init(); } template -MessageContext& MessageContext::operator=( const MessageContext& other ) +MessageContext & MessageContext::operator=(const MessageContext & other) { open_marker_idx_ = other.open_marker_idx_; open_pose_idx_ = other.open_pose_idx_; @@ -67,101 +77,97 @@ MessageContext& MessageContext::operator=( const MessageContext -bool MessageContext::getTransform( std_msgs::Header& header, geometry_msgs::Pose& pose_msg ) +bool MessageContext::getTransform( + std_msgs::msg::Header & header, + geometry_msgs::msg::Pose & pose_msg) { - try - { - if ( header.frame_id != target_frame_ ) - { + try { + if (header.frame_id != target_frame_) { // get transform - tf::StampedTransform transform; - tf_.lookupTransform( target_frame_, header.frame_id, header.stamp, transform ); - DBG_MSG( "Transform %s -> %s at time %f is ready.", header.frame_id.c_str(), target_frame_.c_str(), header.stamp.toSec() ); + geometry_msgs::msg::TransformStamped transform = tf_buffer_core_->lookupTransform( + target_frame_, header.frame_id, tf2::timeFromSec(rclcpp::Time(header.stamp).seconds())); + RCUTILS_LOG_DEBUG("Transform %s -> %s at time %f is ready.", + header.frame_id.c_str(), target_frame_.c_str(), rclcpp::Time(header.stamp).seconds()); // if timestamp is given, transform message into target frame - if ( header.stamp != ros::Time(0) ) - { - tf::Pose pose; - tf::poseMsgToTF( pose_msg, pose ); - pose = transform * pose; + if (header.stamp != rclcpp::Time(0)) { + geometry_msgs::msg::PoseStamped pose_stamped_msg; + pose_stamped_msg.header = header; + pose_stamped_msg.pose = pose_msg; + tf2::doTransform(pose_stamped_msg, pose_stamped_msg, transform); // store transformed pose in original message - tf::poseTFToMsg( pose, pose_msg ); - ROS_DEBUG_STREAM("Changing " << header.frame_id << " to "<< target_frame_); + pose_msg = pose_stamped_msg.pose; + RCUTILS_LOG_DEBUG("Changing %s to %s", header.frame_id.c_str(), target_frame_.c_str()); header.frame_id = target_frame_; } } - } - catch ( tf::ExtrapolationException& e ) - { - ros::Time latest_time; - std::string error_string; - - tf_.getLatestCommonTime( target_frame_, header.frame_id, latest_time, &error_string ); - - // if we have some tf info and it is newer than the requested time, - // we are very unlikely to ever receive the old tf info in the future. - if ( latest_time != ros::Time(0) && latest_time > header.stamp ) - { - std::ostringstream s; - s << "The init message contains an old timestamp and cannot be transformed "; - s << "('" << header.frame_id << "' to '" << target_frame_ - << "' at time " << header.stamp << ")."; - throw InitFailException( s.str() ); + } catch (tf2::ExtrapolationException) { + // Get latest common time + // Call lookupTransform with time=0 and use the stamp on the resultant transform. + geometry_msgs::msg::TransformStamped transform = + tf_buffer_core_->lookupTransform( + target_frame_, header.frame_id, tf2::TimePoint()); + rclcpp::Time latest_time = transform.header.stamp; + + if (latest_time != rclcpp::Time(0) && latest_time > header.stamp) { + std::ostringstream oss; + oss << "The message contains an old timestamp and cannot be transformed " << + "('" << header.frame_id << "' to '" << target_frame_ << "' at time " << + rclcpp::Time(header.stamp).seconds() << ")."; + throw exceptions::TransformError(oss.str()); } return false; + } catch (tf2::TransformException & e) { + throw exceptions::TransformError(e.what()); } return true; - // all other exceptions need to be handled outside } template -void MessageContext::getTfTransforms( std::vector& msg_vec, std::list& indices ) +void MessageContext::getTfTransforms( + std::vector & msg_vec, std::list & indices) { std::list::iterator idx_it; - for ( idx_it = indices.begin(); idx_it != indices.end(); ) - { - visualization_msgs::InteractiveMarker& im_msg = msg_vec[ *idx_it ]; + for (idx_it = indices.begin(); idx_it != indices.end(); ) { + visualization_msgs::msg::InteractiveMarker & im_msg = msg_vec[*idx_it]; // transform interactive marker - bool success = getTransform( im_msg.header, im_msg.pose ); + bool success = getTransform(im_msg.header, im_msg.pose); // transform regular markers - for ( unsigned c = 0; c %s at time %f is not ready.", im_msg.header.frame_id.c_str(), target_frame_.c_str(), im_msg.header.stamp.toSec() ); + } else { + RCUTILS_LOG_DEBUG("Transform %s -> %s at time %f is not ready.", + im_msg.header.frame_id.c_str(), target_frame_.c_str(), rclcpp::Time( + im_msg.header.stamp).seconds()); ++idx_it; } } } template -void MessageContext::getTfTransforms( std::vector& msg_vec, std::list& indices ) +void MessageContext::getTfTransforms( + std::vector & msg_vec, + std::list & indices) { std::list::iterator idx_it; - for ( idx_it = indices.begin(); idx_it != indices.end(); ) - { - visualization_msgs::InteractiveMarkerPose& msg = msg_vec[ *idx_it ]; - if ( getTransform( msg.header, msg.pose ) ) - { + for (idx_it = indices.begin(); idx_it != indices.end(); ) { + visualization_msgs::msg::InteractiveMarkerPose & msg = msg_vec[*idx_it]; + if (getTransform(msg.header, msg.pose)) { idx_it = indices.erase(idx_it); - } - else - { - DBG_MSG( "Transform %s -> %s at time %f is not ready.", msg.header.frame_id.c_str(), target_frame_.c_str(), msg.header.stamp.toSec() ); + } else { + RCUTILS_LOG_DEBUG("Transform %s -> %s at time %f is not ready.", + msg.header.frame_id.c_str(), target_frame_.c_str(), rclcpp::Time( + msg.header.stamp).seconds()); ++idx_it; } } @@ -174,26 +180,22 @@ bool MessageContext::isReady() } template<> -void MessageContext::init() +void MessageContext::init() { // mark all transforms as being missing - for ( size_t i=0; imarkers.size(); i++ ) - { - open_marker_idx_.push_back( i ); + for (size_t i = 0; i < msg->markers.size(); i++) { + open_marker_idx_.push_back(i); } - for ( size_t i=0; iposes.size(); i++ ) - { - open_pose_idx_.push_back( i ); + for (size_t i = 0; i < msg->poses.size(); i++) { + open_pose_idx_.push_back(i); } - for( unsigned i=0; imarkers.size(); i++ ) - { - autoComplete( msg->markers[i], enable_autocomplete_transparency_ ); + for (unsigned i = 0; i < msg->markers.size(); i++) { + autoComplete(msg->markers[i], enable_autocomplete_transparency_); } - for( unsigned i=0; iposes.size(); i++ ) - { + for (unsigned i = 0; i < msg->poses.size(); i++) { // correct empty orientation - if ( msg->poses[i].pose.orientation.w == 0 && msg->poses[i].pose.orientation.x == 0 && - msg->poses[i].pose.orientation.y == 0 && msg->poses[i].pose.orientation.z == 0 ) + if (msg->poses[i].pose.orientation.w == 0 && msg->poses[i].pose.orientation.x == 0 && + msg->poses[i].pose.orientation.y == 0 && msg->poses[i].pose.orientation.z == 0) { msg->poses[i].pose.orientation.w = 1; } @@ -201,45 +203,38 @@ void MessageContext::init() } template<> -void MessageContext::init() +void MessageContext::init() { // mark all transforms as being missing - for ( size_t i=0; imarkers.size(); i++ ) - { - open_marker_idx_.push_back( i ); + for (size_t i = 0; i < msg->markers.size(); i++) { + open_marker_idx_.push_back(i); } - for( unsigned i=0; imarkers.size(); i++ ) - { - autoComplete( msg->markers[i], enable_autocomplete_transparency_ ); + for (unsigned i = 0; i < msg->markers.size(); i++) { + autoComplete(msg->markers[i], enable_autocomplete_transparency_); } } template<> -void MessageContext::getTfTransforms( ) +void MessageContext::getTfTransforms() { - getTfTransforms( msg->markers, open_marker_idx_ ); - getTfTransforms( msg->poses, open_pose_idx_ ); - if ( isReady() ) - { - DBG_MSG( "Update message with seq_num=%lu is ready.", msg->seq_num ); + getTfTransforms(msg->markers, open_marker_idx_); + getTfTransforms(msg->poses, open_pose_idx_); + if (isReady()) { + RCUTILS_LOG_DEBUG("Update message with seq_num=%" PRIu64 " is ready.", msg->seq_num); } } template<> -void MessageContext::getTfTransforms( ) +void MessageContext::getTfTransforms() { - getTfTransforms( msg->markers, open_marker_idx_ ); - if ( isReady() ) - { - DBG_MSG( "Init message with seq_num=%lu is ready.", msg->seq_num ); + getTfTransforms(msg->markers, open_marker_idx_); + if (isReady()) { + RCUTILS_LOG_DEBUG("Response message with seq_num=%" PRIu64 " is ready.", msg->sequence_number); } } // explicit template instantiation -template class MessageContext; -template class MessageContext; - - -} - +template class MessageContext; +template class MessageContext; +} // namespace interactive_markers diff --git a/src/single_client.cpp b/src/single_client.cpp deleted file mode 100644 index a8dd9af3..00000000 --- a/src/single_client.cpp +++ /dev/null @@ -1,314 +0,0 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: David Gossow - */ - -#include "interactive_markers/detail/single_client.h" - -#include -#include - -#define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ ); -//#define DBG_MSG( ... ) printf(" "); printf( __VA_ARGS__ ); printf("\n"); - -namespace interactive_markers -{ - -SingleClient::SingleClient( - const std::string& server_id, - tf::Transformer& tf, - const std::string& target_frame, - const InteractiveMarkerClient::CbCollection& callbacks -) -: state_(server_id,INIT) -, first_update_seq_num_(-1) -, last_update_seq_num_(-1) -, tf_(tf) -, target_frame_(target_frame) -, callbacks_(callbacks) -, server_id_(server_id) -, warn_keepalive_(false) -{ - callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "Waiting for init message." ); -} - -SingleClient::~SingleClient() -{ - callbacks_.resetCb( server_id_ ); -} - -void SingleClient::process(const visualization_msgs::InteractiveMarkerInit::ConstPtr& msg, bool enable_autocomplete_transparency) -{ - DBG_MSG( "%s: received init #%lu", server_id_.c_str(), msg->seq_num ); - - switch (state_) - { - case INIT: - if ( init_queue_.size() > 5 ) - { - DBG_MSG( "Init queue too large. Erasing init message with id %lu.", init_queue_.begin()->msg->seq_num ); - init_queue_.pop_back(); - } - init_queue_.push_front( InitMessageContext(tf_, target_frame_, msg, enable_autocomplete_transparency ) ); - callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "Init message received." ); - break; - - case RECEIVING: - case TF_ERROR: - break; - } -} - -void SingleClient::process(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& msg, bool enable_autocomplete_transparency) -{ - if ( first_update_seq_num_ == (uint64_t)-1 ) - { - first_update_seq_num_ = msg->seq_num; - } - - last_update_time_ = ros::Time::now(); - - if ( msg->type == msg->KEEP_ALIVE ) - { - DBG_MSG( "%s: received keep-alive #%lu", server_id_.c_str(), msg->seq_num ); - if (last_update_seq_num_ != (uint64_t)-1 && msg->seq_num != last_update_seq_num_ ) - { - std::ostringstream s; - s << "Sequence number of update is out of order. Expected: " << last_update_seq_num_ << " Received: " << msg->seq_num; - errorReset( s.str() ); - return; - } - last_update_seq_num_ = msg->seq_num; - return; - } - else - { - DBG_MSG( "%s: received update #%lu", server_id_.c_str(), msg->seq_num ); - if (last_update_seq_num_ != (uint64_t)-1 && msg->seq_num != last_update_seq_num_+1 ) - { - std::ostringstream s; - s << "Sequence number of update is out of order. Expected: " << last_update_seq_num_+1 << " Received: " << msg->seq_num; - errorReset( s.str() ); - return; - } - last_update_seq_num_ = msg->seq_num; - } - - switch (state_) - { - case INIT: - if ( update_queue_.size() > 100 ) - { - DBG_MSG( "Update queue too large. Erasing update message with id %lu.", update_queue_.begin()->msg->seq_num ); - update_queue_.pop_back(); - } - update_queue_.push_front( UpdateMessageContext(tf_, target_frame_, msg, enable_autocomplete_transparency) ); - break; - - case RECEIVING: - update_queue_.push_front( UpdateMessageContext(tf_, target_frame_, msg, enable_autocomplete_transparency) ); - break; - - case TF_ERROR: - break; - } -} - -void SingleClient::update() -{ - switch (state_) - { - case INIT: - transformInitMsgs(); - transformUpdateMsgs(); - checkInitFinished(); - break; - - case RECEIVING: - transformUpdateMsgs(); - pushUpdates(); - checkKeepAlive(); - if ( update_queue_.size() > 100 ) - { - errorReset( "Update queue overflow. Resetting connection." ); - } - break; - - case TF_ERROR: - if ( state_.getDuration().toSec() > 1.0 ) - { - callbacks_.statusCb( InteractiveMarkerClient::ERROR, server_id_, "1 second has passed. Re-initializing." ); - state_ = INIT; - } - break; - } -} - -void SingleClient::checkKeepAlive() -{ - double time_since_upd = (ros::Time::now() - last_update_time_).toSec(); - if ( time_since_upd > 2.0 ) - { - std::ostringstream s; - s << "No update received for " << round(time_since_upd) << " seconds."; - callbacks_.statusCb( InteractiveMarkerClient::WARN, server_id_, s.str() ); - warn_keepalive_ = true; - } - else if ( warn_keepalive_ ) - { - warn_keepalive_ = false; - callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "OK" ); - } -} - -void SingleClient::checkInitFinished() -{ - // check for all init messages received so far if tf info is ready - // and the consecutive update exists. - // If so, omit all updates with lower sequence number, - // switch to RECEIVING mode and treat the init message like a regular update. - - if (last_update_seq_num_ == (uint64_t)-1) - { - callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "Initialization: Waiting for first update/keep-alive message." ); - return; - } - - M_InitMessageContext::iterator init_it; - for ( init_it = init_queue_.begin(); init_it!=init_queue_.end(); ++init_it ) - { - uint64_t init_seq_num = init_it->msg->seq_num; - bool next_up_exists = init_seq_num >= first_update_seq_num_ && init_seq_num <= last_update_seq_num_; - - if ( !init_it->isReady() ) - { - callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "Initialization: Waiting for tf info." ); - } - else if ( next_up_exists ) - { - DBG_MSG( "Init message with seq_id=%lu is ready & in line with updates. Switching to receive mode.", init_seq_num ); - while ( !update_queue_.empty() && update_queue_.back().msg->seq_num <= init_seq_num ) - { - DBG_MSG( "Omitting update with seq_id=%lu", update_queue_.back().msg->seq_num ); - update_queue_.pop_back(); - } - - callbacks_.initCb( init_it->msg ); - callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "Receiving updates." ); - - init_queue_.clear(); - state_ = RECEIVING; - - pushUpdates(); - break; - } - } -} - -void SingleClient::transformInitMsgs() -{ - M_InitMessageContext::iterator it; - for ( it = init_queue_.begin(); it!=init_queue_.end(); ) - { - try - { - it->getTfTransforms(); - } - catch ( std::runtime_error& e ) - { - // we want to notify the user, but also keep the init message - // in case it is the only one we will receive. - std::ostringstream s; - s << "Cannot get tf info for init message with sequence number " << it->msg->seq_num << ". Error: " << e.what(); - callbacks_.statusCb( InteractiveMarkerClient::WARN, server_id_, s.str() ); - } - ++it; - } -} - -void SingleClient::transformUpdateMsgs( ) -{ - M_UpdateMessageContext::iterator it; - for ( it = update_queue_.begin(); it!=update_queue_.end(); ++it ) - { - try - { - it->getTfTransforms(); - } - catch ( std::runtime_error& e ) - { - std::ostringstream s; - s << "Resetting due to tf error: " << e.what(); - errorReset( s.str() ); - return; - } - catch ( ... ) - { - std::ostringstream s; - s << "Resetting due to unknown exception"; - errorReset( s.str() ); - } - } -} - -void SingleClient::errorReset( std::string error_msg ) -{ - // if we get an error here, we re-initialize everything - state_ = TF_ERROR; - update_queue_.clear(); - init_queue_.clear(); - first_update_seq_num_ = -1; - last_update_seq_num_ = -1; - warn_keepalive_ = false; - - callbacks_.statusCb( InteractiveMarkerClient::ERROR, server_id_, error_msg ); - callbacks_.resetCb( server_id_ ); -} - -void SingleClient::pushUpdates() -{ - if( !update_queue_.empty() && update_queue_.back().isReady() ) - { - callbacks_.statusCb( InteractiveMarkerClient::OK, server_id_, "OK" ); - } - while( !update_queue_.empty() && update_queue_.back().isReady() ) - { - DBG_MSG("Pushing out update #%lu.", update_queue_.back().msg->seq_num ); - callbacks_.updateCb( update_queue_.back().msg ); - update_queue_.pop_back(); - } -} - -bool SingleClient::isInitialized() -{ - return (state_ != INIT); -} - -} - diff --git a/src/test/bursty_tf.cpp b/src/test/bursty_tf.cpp deleted file mode 100644 index 312a461a..00000000 --- a/src/test/bursty_tf.cpp +++ /dev/null @@ -1,181 +0,0 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -#include - -#include -#include - -#include - -#include - -boost::shared_ptr server; - -using namespace visualization_msgs; - -Marker makeBox( InteractiveMarker &msg ) -{ - Marker marker; - - marker.type = Marker::CUBE; - marker.scale.x = msg.scale * 0.45; - marker.scale.y = msg.scale * 0.45; - marker.scale.z = msg.scale * 0.45; - marker.color.r = 0.5; - marker.color.g = 0.5; - marker.color.b = 0.5; - marker.color.a = 1.0; - - return marker; -} - -InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg ) -{ - InteractiveMarkerControl control; - control.always_visible = true; - control.markers.push_back( makeBox(msg) ); - msg.controls.push_back( control ); - - return msg.controls.back(); -} - -// %Tag(6DOF)% -void make6DofMarker( bool fixed ) -{ - InteractiveMarker int_marker; - int_marker.header.frame_id = "/base_link"; - int_marker.header.stamp = ros::Time::now(); - int_marker.scale = 1; - - int_marker.name = "simple_6dof"; - int_marker.description = "Simple 6-DOF Control"; - - // insert a box - makeBoxControl(int_marker); - - InteractiveMarkerControl control; - - if ( fixed ) - { - int_marker.name += "_fixed"; - int_marker.description += "\n(fixed orientation)"; - control.orientation_mode = InteractiveMarkerControl::FIXED; - } - - control.orientation.w = 1; - control.orientation.x = 1; - control.orientation.y = 0; - control.orientation.z = 0; - control.name = "rotate_x"; - control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; - int_marker.controls.push_back(control); - control.name = "move_x"; - control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; - int_marker.controls.push_back(control); - - control.orientation.w = 1; - control.orientation.x = 0; - control.orientation.y = 1; - control.orientation.z = 0; - control.name = "rotate_z"; - control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; - int_marker.controls.push_back(control); - control.name = "move_z"; - control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; - int_marker.controls.push_back(control); - - control.orientation.w = 1; - control.orientation.x = 0; - control.orientation.y = 0; - control.orientation.z = 1; - control.name = "rotate_y"; - control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; - int_marker.controls.push_back(control); - control.name = "move_y"; - control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; - int_marker.controls.push_back(control); - - server->insert(int_marker); -} - -void frameCallback(const ros::TimerEvent&) -{ - static tf::TransformBroadcaster br; - static bool sending = true; - - geometry_msgs::Pose pose; - pose.orientation.w = 1; - - ros::Time time = ros::Time::now(); - - std_msgs::Header header; - header.frame_id = "/base_link"; - header.stamp = time; - - int seconds = time.toSec(); - - ROS_INFO( "%.3f", time.toSec() ); - - if ( seconds % 2 < 1 ) - { - tf::Transform t; - t.setOrigin(tf::Vector3(0.0, 0.0, 1.0)); - t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0)); - if (!sending) ROS_INFO("on"); - sending = true; - br.sendTransform(tf::StampedTransform(t, time, "base_link", "bursty_frame")); - usleep(10000); - } - else - { - if (sending) ROS_INFO("off"); - sending = false; - } - - server->setPose("simple_6dof",pose,header); - server->applyChanges(); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "bursty_tf"); - ros::NodeHandle n; - - server.reset( new interactive_markers::InteractiveMarkerServer("bursty_tf","",false) ); - - // create a timer to update the published transforms - ros::Timer frame_timer = n.createTimer(ros::Duration(0.5), frameCallback); - - make6DofMarker(false); - server->applyChanges(); - - ros::spin(); -} diff --git a/src/test/client_test.cpp b/src/test/client_test.cpp deleted file mode 100644 index 0ff45c44..00000000 --- a/src/test/client_test.cpp +++ /dev/null @@ -1,925 +0,0 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -#include -#include - -#include - -#include - -#include -#include - -#define DBG_MSG( ... ) printf( __VA_ARGS__ ); printf("\n"); -#define DBG_MSG_STREAM( ... ) std::cout << __VA_ARGS__ << std::endl; - -using namespace interactive_markers; - -struct Msg -{ - enum { - INIT, - KEEP_ALIVE, - UPDATE, - POSE, - DELETE, - TF_INFO - } type; - - Msg() - { - type = INIT; - seq_num = 0; - } - - uint64_t seq_num; - - std::string server_id; - std::string frame_id; - ros::Time stamp; - - std::vector expect_reset_calls; - std::vector expect_init_seq_num; - std::vector expect_update_seq_num; -}; - -std::string target_frame = "target_frame"; - - -class SequenceTest -{ - typedef visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr; - typedef visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr; - - std::vector recv_init_msgs; - std::vector recv_update_msgs; - std::vector recv_reset_calls; - - void resetReceivedMsgs() - { - recv_init_msgs.clear(); - recv_update_msgs.clear(); - recv_reset_calls.clear(); - } - - void updateCb( const UpdateConstPtr& msg ) - { - DBG_MSG_STREAM( "updateCb called." ); - recv_update_msgs.push_back( *msg ); - } - - void initCb( const InitConstPtr& msg ) - { - DBG_MSG_STREAM( "initCb called." ); - recv_init_msgs.push_back( *msg ); - } - - void statusCb( InteractiveMarkerClient::StatusT status, - const std::string& server_id, - const std::string& msg ) - { - std::string status_string[]={"INFO","WARN","ERROR"}; - ASSERT_TRUE( (unsigned)status < 3 ); - DBG_MSG_STREAM( "(" << status_string[(unsigned)status] << ") " << server_id << ": " << msg ); - } - - void resetCb( const std::string& server_id ) - { - DBG_MSG_STREAM( "resetCb called." ); - recv_reset_calls.push_back(server_id); - } - -public: - void test( std::vector messages ) - { - tf::Transformer tf; - - //tf.setTransform(); - - // create an interactive marker server on the topic namespace simple_marker - - visualization_msgs::InteractiveMarker int_marker; - int_marker.pose.orientation.w=1; - - std::string topic_ns ="im_client_test"; - - interactive_markers::InteractiveMarkerClient client(tf, target_frame, topic_ns ); - - client.setInitCb( boost::bind(&SequenceTest::initCb, this, _1 ) ); - client.setUpdateCb( boost::bind(&SequenceTest::updateCb, this, _1 ) ); - client.setResetCb( boost::bind(&SequenceTest::resetCb, this, _1 ) ); - client.setStatusCb( boost::bind(&SequenceTest::statusCb, this, _1, _2, _3 ) ); - - std::map< int, visualization_msgs::InteractiveMarkerInit > sent_init_msgs; - std::map< int, visualization_msgs::InteractiveMarkerUpdate > sent_update_msgs; - - for ( size_t i=0; itype = visualization_msgs::InteractiveMarkerUpdate::UPDATE; - update_msg_out->seq_num=msg.seq_num; - - update_msg_out->erases.push_back( int_marker.name ); - client.processUpdate( update_msg_out ); - sent_update_msgs[msg.seq_num]=*update_msg_out; - break; - } - case Msg::TF_INFO: - { - DBG_MSG_STREAM( i << " TF_INFO: " << msg.frame_id << " -> " << target_frame << " at time " << msg.stamp.toSec() ); - tf::StampedTransform stf; - stf.frame_id_=msg.frame_id; - stf.child_frame_id_=target_frame; - stf.stamp_=msg.stamp; - stf.setIdentity(); - tf.setTransform( stf, msg.server_id ); - break; - } - } - - /* - ASSERT_EQ( 0, recv_update_msgs.size() ); - ASSERT_EQ( 0, recv_init_msgs.size() ); - ASSERT_EQ( 0, recv_reset_calls.size() ); - */ - - client.update(); - - ASSERT_EQ( msg.expect_update_seq_num.size(), recv_update_msgs.size() ); - ASSERT_EQ( msg.expect_init_seq_num.size(), recv_init_msgs.size() ); - ASSERT_EQ( msg.expect_reset_calls.size(), recv_reset_calls.size() ); - - for ( size_t u=0; u seq; - - msg.type=Msg::INIT; - msg.seq_num=0; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.expect_init_seq_num.push_back(0); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - -TEST(InteractiveMarkerClient, init_simple2) -{ - Msg msg; - - std::vector seq; - - msg.type=Msg::INIT; - msg.seq_num=0; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - - msg.type=Msg::UPDATE; - msg.expect_init_seq_num.push_back(0); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - - -TEST(InteractiveMarkerClient, init_many_inits) -{ - Msg msg; - - std::vector seq; - - for ( int i=0; i<200; i++ ) - { - msg.type=Msg::INIT; - msg.seq_num=i; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - } - - // this update should be ommitted - msg.type=Msg::UPDATE; - msg.expect_init_seq_num.push_back(msg.seq_num); - seq.push_back(msg); - - // this update should go through - msg.seq_num++; - msg.expect_init_seq_num.clear(); - msg.expect_update_seq_num.push_back(msg.seq_num); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - -TEST(InteractiveMarkerClient, init_many_updates) -{ - Msg msg; - - std::vector seq; - - for ( int i=0; i<200; i++ ) - { - msg.type=Msg::UPDATE; - msg.seq_num=i; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - } - - msg.type=Msg::INIT; - msg.seq_num=190; - msg.expect_init_seq_num.push_back(msg.seq_num); - for ( int i=191; i<200; i++ ) - { - msg.expect_update_seq_num.push_back(i); - } - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - -TEST(InteractiveMarkerClient, init_invalid_tf) -{ - Msg msg; - - std::vector seq; - - msg.type=Msg::INIT; - msg.seq_num=0; - msg.server_id="server1"; - msg.frame_id="invalid_frame"; - seq.push_back(msg); - - msg.type=Msg::INIT; - msg.seq_num=1; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.expect_init_seq_num.push_back(1); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - -TEST(InteractiveMarkerClient, init_wait_tf) -{ - Msg msg; - - std::vector seq; - - // initial tf info needed so wait_frame is in the tf tree - msg.type=Msg::TF_INFO; - msg.server_id="server1"; - msg.frame_id="wait_frame"; - msg.stamp=ros::Time(1.0); - seq.push_back(msg); - - // send init message that lives in the future - msg.type=Msg::INIT; - msg.seq_num=0; - msg.stamp=ros::Time(2.0); - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - seq.push_back(msg); - - // send tf info -> message should get passed through - msg.type=Msg::TF_INFO; - msg.expect_init_seq_num.push_back(0); - seq.push_back(msg); - - // send update message that lives in the future - msg.type=Msg::UPDATE; - msg.seq_num=1; - msg.stamp=ros::Time(3.0); - msg.expect_init_seq_num.clear(); - seq.push_back(msg); - - // send tf info -> message should get passed through - msg.type=Msg::TF_INFO; - msg.expect_update_seq_num.push_back(1); - seq.push_back(msg); - - // send pose message that lives in the future - msg.type=Msg::POSE; - msg.seq_num=2; - msg.stamp=ros::Time(4.0); - msg.expect_update_seq_num.clear(); - seq.push_back(msg); - - // send tf info -> message should get passed through - msg.type=Msg::TF_INFO; - msg.expect_update_seq_num.push_back(2); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - - -TEST(InteractiveMarkerClient, init_wait_tf_zerotime) -{ - Msg msg; - - std::vector seq; - - // send init message with zero timestamp and non-existing tf frame - msg.type=Msg::INIT; - msg.server_id="server1"; - msg.frame_id="wait_frame"; - msg.seq_num=0; - msg.stamp=ros::Time(0.0); - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - seq.push_back(msg); - - // send tf info -> message should get passed through - msg.type=Msg::TF_INFO; - msg.stamp=ros::Time(1.0); - msg.expect_init_seq_num.push_back(0); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - - -TEST(InteractiveMarkerClient, init_wait_tf_inverse) -{ - // send messages with timestamps going backwards - // they should still be delivered in the right order - // according to their seq_num - - Msg msg; - - std::vector seq; - - // initial tf info needed so wait_frame is in the tf tree - msg.type=Msg::TF_INFO; - msg.server_id="server1"; - msg.frame_id="wait_frame"; - msg.stamp=ros::Time(1.0); - seq.push_back(msg); - - msg.type=Msg::INIT; - msg.seq_num=0; - msg.stamp=ros::Time(6); - seq.push_back(msg); - - msg.type=Msg::INIT; - msg.seq_num=1; - msg.stamp=ros::Time(5); - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.seq_num=0; - seq.push_back(msg); - - msg.type=Msg::UPDATE; - msg.seq_num=1; - msg.stamp=ros::Time(5); - seq.push_back(msg); - - msg.type=Msg::UPDATE; - msg.seq_num=2; - msg.stamp=ros::Time(4); - seq.push_back(msg); - - msg.type=Msg::UPDATE; - msg.seq_num=3; - msg.stamp=ros::Time(3); - seq.push_back(msg); - - msg.type=Msg::TF_INFO; - msg.stamp=ros::Time(2); - seq.push_back(msg); - - msg.type=Msg::TF_INFO; - msg.stamp=ros::Time(3); - seq.push_back(msg); - - msg.type=Msg::TF_INFO; - msg.stamp=ros::Time(4); - seq.push_back(msg); - - // as soon as tf info for init #1 is there, - // all messages should go through - msg.stamp=ros::Time(5); - msg.expect_init_seq_num.push_back(1); - msg.expect_update_seq_num.push_back(2); - msg.expect_update_seq_num.push_back(3); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - -TEST(InteractiveMarkerClient, wait_tf_inverse) -{ - // send messages with timestamps going backwards - // they should still be delivered in the right order - // according to their seq_num - - Msg msg; - - std::vector seq; - - // initial tf info needed so wait_frame is in the tf tree - msg.type=Msg::TF_INFO; - msg.server_id="server1"; - msg.frame_id="wait_frame"; - msg.stamp=ros::Time(1); - seq.push_back(msg); - - msg.type=Msg::INIT; - msg.seq_num=0; - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.seq_num=0; - msg.expect_init_seq_num.push_back(0); - seq.push_back(msg); - - msg.expect_init_seq_num.clear(); - - // init complete - - msg.type=Msg::UPDATE; - msg.seq_num=1; - msg.stamp=ros::Time(5); - seq.push_back(msg); - - msg.type=Msg::UPDATE; - msg.seq_num=2; - msg.stamp=ros::Time(4); - seq.push_back(msg); - - msg.type=Msg::UPDATE; - msg.seq_num=3; - msg.stamp=ros::Time(3); - seq.push_back(msg); - - msg.type=Msg::TF_INFO; - msg.stamp=ros::Time(3); - seq.push_back(msg); - - msg.type=Msg::TF_INFO; - msg.stamp=ros::Time(4); - seq.push_back(msg); - - // all messages should go through in the right order - msg.type=Msg::TF_INFO; - msg.stamp=ros::Time(5); - msg.expect_update_seq_num.push_back(1); - msg.expect_update_seq_num.push_back(2); - msg.expect_update_seq_num.push_back(3); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - - -TEST(InteractiveMarkerClient, wrong_seq_num1) -{ - Msg msg; - - std::vector seq; - - msg.type=Msg::INIT; - msg.seq_num=0; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.expect_init_seq_num.push_back(0); - seq.push_back(msg); - - msg.expect_init_seq_num.clear(); - - msg.type=Msg::KEEP_ALIVE; - msg.seq_num=1; - msg.expect_reset_calls.push_back(msg.server_id); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - -TEST(InteractiveMarkerClient, wrong_seq_num2) -{ - Msg msg; - - std::vector seq; - - msg.type=Msg::INIT; - msg.seq_num=1; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.expect_init_seq_num.push_back(1); - seq.push_back(msg); - - msg.expect_init_seq_num.clear(); - - msg.type=Msg::KEEP_ALIVE; - msg.seq_num=0; - msg.expect_reset_calls.push_back(msg.server_id); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - -TEST(InteractiveMarkerClient, wrong_seq_num3) -{ - Msg msg; - - std::vector seq; - - msg.type=Msg::INIT; - msg.seq_num=1; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.expect_init_seq_num.push_back(1); - seq.push_back(msg); - - msg.expect_init_seq_num.clear(); - - msg.type=Msg::UPDATE; - msg.seq_num=1; - msg.expect_reset_calls.push_back(msg.server_id); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - -TEST(InteractiveMarkerClient, wrong_seq_num4) -{ - Msg msg; - - std::vector seq; - - msg.type=Msg::INIT; - msg.seq_num=1; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.expect_init_seq_num.push_back(1); - seq.push_back(msg); - - msg.expect_init_seq_num.clear(); - - msg.type=Msg::UPDATE; - msg.seq_num=2; - msg.expect_update_seq_num.push_back(2); - seq.push_back(msg); - - msg.expect_update_seq_num.clear(); - - msg.type=Msg::UPDATE; - msg.seq_num=2; - msg.expect_reset_calls.push_back(msg.server_id); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - -TEST(InteractiveMarkerClient, wrong_seq_num5) -{ - Msg msg; - - std::vector seq; - - msg.type=Msg::INIT; - msg.seq_num=1; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.expect_init_seq_num.push_back(1); - seq.push_back(msg); - - msg.expect_init_seq_num.clear(); - - msg.type=Msg::UPDATE; - msg.seq_num=2; - msg.expect_update_seq_num.push_back(2); - seq.push_back(msg); - - msg.expect_update_seq_num.clear(); - - msg.type=Msg::UPDATE; - msg.seq_num=1; - msg.expect_reset_calls.push_back(msg.server_id); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - -TEST(InteractiveMarkerClient, wrong_seq_num6) -{ - Msg msg; - - std::vector seq; - - msg.type=Msg::INIT; - msg.seq_num=1; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.expect_init_seq_num.push_back(1); - seq.push_back(msg); - - msg.expect_init_seq_num.clear(); - - msg.type=Msg::UPDATE; - msg.seq_num=2; - msg.expect_update_seq_num.push_back(2); - seq.push_back(msg); - - msg.expect_update_seq_num.clear(); - - msg.type=Msg::UPDATE; - msg.seq_num=4; - msg.expect_reset_calls.push_back(msg.server_id); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - - -TEST(InteractiveMarkerClient, init_twoservers) -{ - Msg msg; - - std::vector seq; - - msg.type=Msg::INIT; - msg.seq_num=0; - msg.server_id="server1"; - msg.frame_id=target_frame; - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.expect_init_seq_num.push_back(0); - seq.push_back(msg); - - msg.expect_init_seq_num.clear(); - - msg.type=Msg::UPDATE; - msg.seq_num=1; - msg.expect_update_seq_num.push_back(1); - seq.push_back(msg); - - msg.expect_update_seq_num.clear(); - - msg.type=Msg::INIT; - msg.seq_num=0; - msg.server_id="server2"; - msg.frame_id=target_frame; - seq.push_back(msg); - - msg.type=Msg::KEEP_ALIVE; - msg.expect_init_seq_num.push_back(0); - seq.push_back(msg); - - msg.expect_init_seq_num.clear(); - - msg.type=Msg::UPDATE; - msg.seq_num=1; - msg.expect_update_seq_num.push_back(1); - seq.push_back(msg); - - SequenceTest t; - t.test(seq); -} - - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "im_client_test"); - //ros::NodeHandle nh; - return RUN_ALL_TESTS(); -} diff --git a/src/test/missing_tf.cpp b/src/test/missing_tf.cpp deleted file mode 100644 index 9838b040..00000000 --- a/src/test/missing_tf.cpp +++ /dev/null @@ -1,182 +0,0 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -#include - -#include -#include - -#include - -#include - -boost::shared_ptr server; - -using namespace visualization_msgs; - -Marker makeBox( InteractiveMarker &msg ) -{ - Marker marker; - - marker.type = Marker::CUBE; - marker.scale.x = msg.scale * 0.45; - marker.scale.y = msg.scale * 0.45; - marker.scale.z = msg.scale * 0.45; - marker.color.r = 0.5; - marker.color.g = 0.5; - marker.color.b = 0.5; - marker.color.a = 1.0; - - return marker; -} - -InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg ) -{ - InteractiveMarkerControl control; - control.always_visible = true; - control.markers.push_back( makeBox(msg) ); - msg.controls.push_back( control ); - - return msg.controls.back(); -} - -// %Tag(6DOF)% -void make6DofMarker( bool fixed ) -{ - InteractiveMarker int_marker; - int_marker.header.frame_id = "/base_link"; - int_marker.header.stamp = ros::Time::now(); - int_marker.scale = 1; - - int_marker.name = "simple_6dof"; - int_marker.description = "Simple 6-DOF Control"; - - // insert a box - makeBoxControl(int_marker); - - InteractiveMarkerControl control; - - if ( fixed ) - { - int_marker.name += "_fixed"; - int_marker.description += "\n(fixed orientation)"; - control.orientation_mode = InteractiveMarkerControl::FIXED; - } - - control.orientation.w = 1; - control.orientation.x = 1; - control.orientation.y = 0; - control.orientation.z = 0; - control.name = "rotate_x"; - control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; - int_marker.controls.push_back(control); - control.name = "move_x"; - control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; - int_marker.controls.push_back(control); - - control.orientation.w = 1; - control.orientation.x = 0; - control.orientation.y = 1; - control.orientation.z = 0; - control.name = "rotate_z"; - control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; - int_marker.controls.push_back(control); - control.name = "move_z"; - control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; - int_marker.controls.push_back(control); - - control.orientation.w = 1; - control.orientation.x = 0; - control.orientation.y = 0; - control.orientation.z = 1; - control.name = "rotate_y"; - control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; - int_marker.controls.push_back(control); - control.name = "move_y"; - control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; - int_marker.controls.push_back(control); - - server->insert(int_marker); -} - -void frameCallback(const ros::TimerEvent&) -{ - static tf::TransformBroadcaster br; - static bool sending = true; - - geometry_msgs::Pose pose; - pose.orientation.w = 1; - - ros::Time time = ros::Time::now(); - - std_msgs::Header header; - header.frame_id = "/base_link"; - header.stamp = time; - - int seconds = time.toSec(); - - ROS_INFO( "%.3f", time.toSec() ); - - if ( seconds % 4 < 2 ) - { - if (!sending) ROS_INFO("on"); - sending = true; - } - else - { - if (sending) ROS_INFO("off"); - sending = false; - header.frame_id = "missing_frame"; - } - - server->setPose("simple_6dof",pose,header); - server->applyChanges(); - - tf::Transform t; - t.setOrigin(tf::Vector3(0.0, 0.0, 1.0)); - t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0)); - br.sendTransform(tf::StampedTransform(t, time, "base_link", "bursty_frame")); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "bursty_tf"); - ros::NodeHandle n; - - server.reset( new interactive_markers::InteractiveMarkerServer("bursty_tf","",false) ); - - // create a timer to update the published transforms - ros::Timer frame_timer = n.createTimer(ros::Duration(0.5), frameCallback); - - make6DofMarker(false); - server->applyChanges(); - - ros::spin(); -} diff --git a/src/test/server_client_test.cpp b/src/test/server_client_test.cpp deleted file mode 100644 index 9addcc39..00000000 --- a/src/test/server_client_test.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -#include -#include - -#include - -#include - -#include -#include - -#define DBG_MSG( ... ) printf( __VA_ARGS__ ); printf("\n"); -#define DBG_MSG_STREAM( ... ) std::cout << __VA_ARGS__ << std::endl; - -using namespace interactive_markers; - -int update_calls; -int init_calls; -int reset_calls; -int status_calls; - - -std::string reset_server_id; - -typedef visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr; -typedef visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr; - -InitConstPtr init_msg; -UpdateConstPtr update_msg; - -void resetReceivedMsgs() -{ - update_calls = 0; - init_calls = 0; - reset_calls = 0; - status_calls = 0; - reset_server_id = ""; - init_msg.reset(); - update_msg.reset(); -} - -void updateCb( const UpdateConstPtr& msg ) -{ - DBG_MSG("updateCb called"); - update_calls++; - update_msg = msg; -} - -void initCb( const InitConstPtr& msg ) -{ - DBG_MSG("initCb called"); - init_calls++; - init_msg = msg; -} - -void statusCb( InteractiveMarkerClient::StatusT status, - const std::string& server_id, - const std::string& msg ) -{ - DBG_MSG("statusCb called"); - status_calls++; - DBG_MSG_STREAM( (int)status << " " << server_id << ": " << msg ); -} - -void resetCb( const std::string& server_id ) -{ - DBG_MSG("resetCb( %s ) called", server_id.c_str() ); - reset_calls++; - reset_server_id = server_id; -} - -void waitMsg() -{ - for(int i=0;i<10;i++) - { - ros::spinOnce(); - usleep(1000); - } -} - -TEST(InteractiveMarkerServerAndClient, connect_tf_error) -{ - tf::TransformListener tf; - - // create an interactive marker server on the topic namespace simple_marker - interactive_markers::InteractiveMarkerServer server("im_server_client_test","test_server",false); - visualization_msgs::InteractiveMarker int_marker; - int_marker.name = "marker1"; - int_marker.header.frame_id = "valid_frame"; - - waitMsg(); - - resetReceivedMsgs(); - - interactive_markers::InteractiveMarkerClient client(tf, "valid_frame", "im_server_client_test"); - client.setInitCb( &initCb ); - client.setStatusCb( &statusCb ); - client.setResetCb( &resetCb ); - client.setUpdateCb( &updateCb ); - - // Add one marker -> init should get called once - DBG_MSG("----------------------------------------"); - - server.insert(int_marker); - server.applyChanges(); - waitMsg(); - client.update(); - - ASSERT_EQ( 0, update_calls ); - ASSERT_EQ( 1, init_calls ); - ASSERT_EQ( 0, reset_calls ); - ASSERT_TRUE( init_msg ); - ASSERT_EQ( 1, init_msg->markers.size() ); - ASSERT_EQ( "marker1", init_msg->markers[0].name ); - ASSERT_EQ( "valid_frame", init_msg->markers[0].header.frame_id ); - - // Add another marker -> update should get called once - DBG_MSG("----------------------------------------"); - - resetReceivedMsgs(); - - int_marker.name = "marker2"; - - server.insert(int_marker); - server.applyChanges(); - waitMsg(); - client.update(); - - ASSERT_EQ( 1, update_calls ); - ASSERT_EQ( 0, init_calls ); - ASSERT_EQ( 0, reset_calls ); - ASSERT_TRUE( update_msg ); - ASSERT_EQ( 1, update_msg->markers.size() ); - ASSERT_EQ( 0, update_msg->poses.size() ); - ASSERT_EQ( 0, update_msg->erases.size() ); - ASSERT_EQ( "marker2", update_msg->markers[0].name ); - - // Make marker tf info invalid -> connection should be reset - DBG_MSG("----------------------------------------"); - - resetReceivedMsgs(); - - int_marker.header.frame_id = "invalid_frame"; - - server.insert(int_marker); - server.applyChanges(); - waitMsg(); - client.update(); - - ASSERT_EQ( 0, update_calls ); - ASSERT_EQ( 0, init_calls ); - ASSERT_EQ( 1, reset_calls ); - ASSERT_TRUE( reset_server_id.find("/test_server") != std::string::npos ); - - // Make marker tf info valid again -> connection should be successfully initialized again - DBG_MSG("----------------------------------------"); - - usleep(2000000); - waitMsg(); - client.update(); - - resetReceivedMsgs(); - - int_marker.header.frame_id = "valid_frame"; - - server.insert(int_marker); - server.applyChanges(); - waitMsg(); - client.update(); - - ASSERT_EQ( 0, update_calls ); - ASSERT_EQ( 1, init_calls ); - ASSERT_EQ( 0, reset_calls ); - - // Erase marker - DBG_MSG("----------------------------------------"); - - resetReceivedMsgs(); - - server.erase("marker1"); - server.applyChanges(); - waitMsg(); - client.update(); - - ASSERT_EQ( 1, update_calls ); - ASSERT_EQ( 0, init_calls ); - ASSERT_EQ( 0, reset_calls ); - ASSERT_TRUE( update_msg ); - ASSERT_EQ( 0, update_msg->markers.size() ); - ASSERT_EQ( 0, update_msg->poses.size() ); - ASSERT_EQ( 1, update_msg->erases.size() ); - ASSERT_EQ( "marker1", update_msg->erases[0] ); - - // Update pose - DBG_MSG("----------------------------------------"); - - resetReceivedMsgs(); - - server.setPose( "marker2", int_marker.pose, int_marker.header ); - server.applyChanges(); - waitMsg(); - client.update(); - - ASSERT_EQ( 1, update_calls ); - ASSERT_EQ( 0, init_calls ); - ASSERT_EQ( 0, reset_calls ); - ASSERT_TRUE( update_msg ); - ASSERT_EQ( 0, update_msg->markers.size() ); - ASSERT_EQ( 1, update_msg->poses.size() ); - ASSERT_EQ( 0, update_msg->erases.size() ); - ASSERT_EQ( "marker2", update_msg->poses[0].name ); -} - - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) -{ - ros::init(argc, argv, "im_server_client_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/test/server_test.cpp b/src/test/server_test.cpp deleted file mode 100644 index a7962b49..00000000 --- a/src/test/server_test.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -#include - -#include - -#include - -TEST(InteractiveMarkerServer, addRemove) -{ - // create an interactive marker server on the topic namespace simple_marker - interactive_markers::InteractiveMarkerServer server("im_server_test"); - - // create an interactive marker for our server - visualization_msgs::InteractiveMarker int_marker; - int_marker.name = "marker1"; - - // create a valid pose - geometry_msgs::Pose pose; - pose.orientation.w = 1.0; - - //insert, apply, erase, apply - server.insert(int_marker); - ASSERT_TRUE( server.get("marker1", int_marker) ); - - server.applyChanges(); - ASSERT_TRUE( server.get("marker1", int_marker) ); - - ASSERT_TRUE( server.erase( "marker1" ) ); - ASSERT_FALSE( server.get("marker1", int_marker) ); - - server.applyChanges(); - ASSERT_FALSE( server.get("marker1", int_marker) ); - - - //insert, erase, apply - server.insert(int_marker); - ASSERT_TRUE( server.get("marker1", int_marker) ); - - ASSERT_TRUE( server.erase( "marker1" ) ); - ASSERT_FALSE( server.get("marker1", int_marker) ); - - server.applyChanges(); - ASSERT_FALSE( server.get("marker1", int_marker) ); - - //insert, apply, clear, apply - server.insert(int_marker); - ASSERT_TRUE( server.get("marker1", int_marker) ); - - server.applyChanges(); - ASSERT_TRUE( server.get("marker1", int_marker) ); - - server.clear(); - ASSERT_FALSE( server.get("marker1", int_marker) ); - - server.applyChanges(); - ASSERT_FALSE( server.get("marker1", int_marker) ); - - //insert, setPose, apply, clear, apply - server.insert(int_marker); - ASSERT_TRUE( server.setPose("marker1", pose) ); - - server.applyChanges(); - server.clear(); - ASSERT_FALSE( server.get("marker1", int_marker) ); - - server.applyChanges(); - - // erase unknown marker - ASSERT_FALSE( server.erase("marker1") ); - - //avoid subscriber destruction warning - usleep(1000); -} - - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) -{ - ros::init(argc, argv, "im_server_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/tools.cpp b/src/tools.cpp index b8bb0b7f..eecc305b 100644 --- a/src/tools.cpp +++ b/src/tools.cpp @@ -1,70 +1,80 @@ -/* - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "interactive_markers/tools.h" - -#include -#include - -#include -#include - +// Copyright (c) 2011, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Needed for M_PI on Windows +#ifdef _MSC_VER +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#endif + +#include #include #include +#include +#include + +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.h" + +#include "interactive_markers/tools.hpp" namespace interactive_markers { -void autoComplete( visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency ) +void autoComplete( + visualization_msgs::msg::InteractiveMarker & msg, + bool enable_autocomplete_transparency) { // this is a 'delete' message. no need for action. - if ( msg.controls.empty() ) - { + if (msg.controls.empty()) { return; } // default size - if ( msg.scale == 0 ) - { + if (msg.scale == 0) { msg.scale = 1; } // correct empty orientation, normalize - if ( msg.pose.orientation.w == 0 && msg.pose.orientation.x == 0 && - msg.pose.orientation.y == 0 && msg.pose.orientation.z == 0 ) + if (msg.pose.orientation.w == 0 && msg.pose.orientation.x == 0 && + msg.pose.orientation.y == 0 && msg.pose.orientation.z == 0) { msg.pose.orientation.w = 1; } - //normalize quaternion - tf::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y, - msg.pose.orientation.z, msg.pose.orientation.w ); + // normalize quaternion + tf2::Quaternion int_marker_orientation(msg.pose.orientation.x, msg.pose.orientation.y, + msg.pose.orientation.z, msg.pose.orientation.w); int_marker_orientation.normalize(); msg.pose.orientation.x = int_marker_orientation.x(); msg.pose.orientation.y = int_marker_orientation.y(); @@ -72,67 +82,64 @@ void autoComplete( visualization_msgs::InteractiveMarker &msg, bool enable_autoc msg.pose.orientation.w = int_marker_orientation.w(); // complete the controls - for ( unsigned c=0; c names; - for( unsigned c = 0; c < msg.controls.size(); c++ ) - { + for (unsigned c = 0; c < msg.controls.size(); c++) { std::string name = msg.controls[c].name; - while( names.find( name ) != names.end() ) - { + while (names.find(name) != names.end()) { std::stringstream ss; ss << name << "_u" << uniqueification_number++; name = ss.str(); } msg.controls[c].name = name; - names.insert( name ); + names.insert(name); } } -void autoComplete( const visualization_msgs::InteractiveMarker &msg, - visualization_msgs::InteractiveMarkerControl &control, bool enable_autocomplete_transparency) +void autoComplete( + const visualization_msgs::msg::InteractiveMarker & msg, + visualization_msgs::msg::InteractiveMarkerControl & control, + bool enable_autocomplete_transparency) { // correct empty orientation - if ( control.orientation.w == 0 && control.orientation.x == 0 && - control.orientation.y == 0 && control.orientation.z == 0 ) + if (control.orientation.w == 0 && control.orientation.x == 0 && + control.orientation.y == 0 && control.orientation.z == 0) { control.orientation.w = 1; } // add default control handles if there are none - if ( control.markers.empty() ) - { - switch ( control.interaction_mode ) - { - case visualization_msgs::InteractiveMarkerControl::NONE: + if (control.markers.empty()) { + switch (control.interaction_mode) { + case visualization_msgs::msg::InteractiveMarkerControl::NONE: break; - case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS: + case visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS: control.markers.reserve(2); - makeArrow( msg, control, 1.0 ); - makeArrow( msg, control, -1.0 ); + makeArrow(msg, control, 1.0); + makeArrow(msg, control, -1.0); break; - case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE: - case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS: - case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE: - makeDisc( msg, control ); + case visualization_msgs::msg::InteractiveMarkerControl::MOVE_PLANE: + case visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS: + case visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE: + makeDisc(msg, control); break; - case visualization_msgs::InteractiveMarkerControl::BUTTON: + case visualization_msgs::msg::InteractiveMarkerControl::BUTTON: break; - case visualization_msgs::InteractiveMarkerControl::MENU: - makeViewFacingButton( msg, control, control.description ); + case visualization_msgs::msg::InteractiveMarkerControl::MENU: + makeViewFacingButton(msg, control, control.description); break; default: @@ -141,40 +148,36 @@ void autoComplete( const visualization_msgs::InteractiveMarker &msg, } // get interactive marker pose for later - tf::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y, - msg.pose.orientation.z, msg.pose.orientation.w ); - tf::Vector3 int_marker_position( msg.pose.position.x, msg.pose.position.y, msg.pose.position.z ); + tf2::Quaternion int_marker_orientation(msg.pose.orientation.x, msg.pose.orientation.y, + msg.pose.orientation.z, msg.pose.orientation.w); + tf2::Vector3 int_marker_position(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z); // fill in missing pose information into the markers - for ( unsigned m=0; m 0.0 ) - { + if (!enable_autocomplete_transparency && marker.color.a > 0.0) { marker.color.a = 1.0; } } } -void makeArrow( const visualization_msgs::InteractiveMarker &msg, - visualization_msgs::InteractiveMarkerControl &control, float pos ) +void makeArrow( + const visualization_msgs::msg::InteractiveMarker & msg, + visualization_msgs::msg::InteractiveMarkerControl & control, + float pos) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; // rely on the auto-completion for the correct orientation marker.pose.orientation = control.orientation; - marker.type = visualization_msgs::Marker::ARROW; - marker.scale.x = msg.scale * 0.15; // aleeper: changed from 0.3 due to Rviz fix - marker.scale.y = msg.scale * 0.25; // aleeper: changed from 0.5 due to Rviz fix + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.scale.x = msg.scale * 0.15; // aleeper: changed from 0.3 due to Rviz fix + marker.scale.y = msg.scale * 0.25; // aleeper: changed from 0.5 due to Rviz fix marker.scale.z = msg.scale * 0.2; assignDefaultColor(marker, control.orientation); float dist = fabs(pos); - float dir = pos > 0 ? 1 : -1; + float dir = pos > 0.0f ? 1.0f : -1.0f; - float inner = 0.5 * dist; - float outer = inner + 0.4; + float inner = 0.5f * dist; + float outer = inner + 0.4f; marker.points.resize(2); marker.points[0].x = dir * msg.scale * inner; marker.points[1].x = dir * msg.scale * outer; - control.markers.push_back( marker ); + control.markers.push_back(marker); } -void makeDisc( const visualization_msgs::InteractiveMarker &msg, - visualization_msgs::InteractiveMarkerControl &control, float width ) +void makeDisc( + const visualization_msgs::msg::InteractiveMarker & msg, + visualization_msgs::msg::InteractiveMarkerControl & control, + double width) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; // rely on the auto-completion for the correct orientation marker.pose.orientation = control.orientation; - marker.type = visualization_msgs::Marker::TRIANGLE_LIST; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; marker.scale.x = msg.scale; marker.scale.y = msg.scale; marker.scale.z = msg.scale; @@ -240,125 +246,120 @@ void makeDisc( const visualization_msgs::InteractiveMarker &msg, // compute points on a circle in the y-z plane int steps = 36; - std::vector circle1, circle2; + std::vector circle1, circle2; circle1.reserve(steps); circle2.reserve(steps); - geometry_msgs::Point v1,v2; + geometry_msgs::msg::Point v1, v2; - for ( int i=0; i(i) / static_cast(steps) * M_PI * 2.0; v1.y = 0.5 * cos(a); v1.z = 0.5 * sin(a); - v2.y = (1+width) * v1.y; - v2.z = (1+width) * v1.z; + v2.y = (1.0 + width) * v1.y; + v2.z = (1.0 + width) * v1.z; - circle1.push_back( v1 ); - circle2.push_back( v2 ); + circle1.push_back(v1); + circle2.push_back(v2); } - marker.points.resize(6*steps); + marker.points.resize(6 * steps); - std_msgs::ColorRGBA color; - color.r=color.g=color.b=color.a=1; + std_msgs::msg::ColorRGBA color; + color.r = color.g = color.b = color.a = 1; - switch ( control.interaction_mode ) - { - case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS: - { - marker.colors.resize(2*steps); - std_msgs::ColorRGBA base_color = marker.color; - for ( int i=0; i(i % 2); + color.r = base_color.r * t; + color.g = base_color.g * t; + color.b = base_color.b * t; + + marker.colors[c] = color; + marker.colors[c + 1] = color; + } + break; } - break; - } - case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE: - { - marker.colors.resize(2*steps); - std_msgs::ColorRGBA base_color = marker.color; - for ( int i=0; i(fabs(bt_x_axis.x())); + y = static_cast(fabs(bt_x_axis.y())); + z = static_cast(fabs(bt_x_axis.z())); - float max_xy = x>y ? x : y; - float max_yz = y>z ? y : z; + float max_xy = x > y ? x : y; + float max_yz = y > z ? y : z; float max_xyz = max_xy > max_yz ? max_xy : max_yz; marker.color.r = x / max_xyz; @@ -415,12 +420,12 @@ void assignDefaultColor(visualization_msgs::Marker &marker, const geometry_msgs: marker.color.a = 0.5; } - -visualization_msgs::InteractiveMarkerControl makeTitle( const visualization_msgs::InteractiveMarker &msg ) +visualization_msgs::msg::InteractiveMarkerControl makeTitle( + const visualization_msgs::msg::InteractiveMarker & msg) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; - marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker.scale.x = msg.scale * 0.15; marker.scale.y = msg.scale * 0.15; marker.scale.z = msg.scale * 0.15; @@ -431,15 +436,15 @@ visualization_msgs::InteractiveMarkerControl makeTitle( const visualization_msgs marker.pose.position.z = msg.scale * 1.4; marker.text = msg.description; - visualization_msgs::InteractiveMarkerControl control; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::NONE; - control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING; + visualization_msgs::msg::InteractiveMarkerControl control; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::NONE; + control.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::VIEW_FACING; control.always_visible = true; - control.markers.push_back( marker ); + control.markers.push_back(marker); - autoComplete( msg, control ); + autoComplete(msg, control); return control; } -} +} // namespace interactive_markers diff --git a/test/cpp_client.test b/test/cpp_client.test deleted file mode 100644 index 66c91fc3..00000000 --- a/test/cpp_client.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/test/cpp_server.test b/test/cpp_server.test deleted file mode 100644 index d167ec44..00000000 --- a/test/cpp_server.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/test/cpp_server_client.test b/test/cpp_server_client.test deleted file mode 100644 index 04f03eca..00000000 --- a/test/cpp_server_client.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/test/interactive_markers/interactive_marker_fixtures.hpp b/test/interactive_markers/interactive_marker_fixtures.hpp new file mode 100644 index 00000000..23a67ca8 --- /dev/null +++ b/test/interactive_markers/interactive_marker_fixtures.hpp @@ -0,0 +1,74 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Open Source Robotics Foundation, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#ifndef INTERACTIVE_MARKERS__INTERACTIVE_MARKER_FIXTURES_HPP_ +#define INTERACTIVE_MARKERS__INTERACTIVE_MARKER_FIXTURES_HPP_ +#include + +#include "visualization_msgs/msg/interactive_marker.hpp" +#include "visualization_msgs/msg/interactive_marker_control.hpp" +#include "visualization_msgs/msg/menu_entry.hpp" + +std::vector +get_interactive_markers() +{ + std::vector markers; + { + visualization_msgs::msg::InteractiveMarker marker; + marker.name = "test_marker_0"; + marker.header.frame_id = "test_frame_id"; + markers.push_back(marker); + } + { + visualization_msgs::msg::InteractiveMarker marker; + marker.name = "test_marker_1"; + marker.header.frame_id = "test_frame_id"; + marker.pose.position.x = 1.0; + marker.pose.orientation.w = 1.0; + marker.description = "My test marker description"; + marker.scale = 3.14f; + visualization_msgs::msg::MenuEntry menu_entry; + menu_entry.id = 42; + menu_entry.title = "My test menu title"; + menu_entry.command = "Some test command to be run"; + marker.menu_entries.push_back(menu_entry); + visualization_msgs::msg::InteractiveMarkerControl control; + control.name = "test_control_name"; + control.orientation.w = 1.0; + control.always_visible = true; + control.description = "My test control description"; + marker.controls.push_back(control); + markers.push_back(marker); + } + return markers; +} + +#endif // INTERACTIVE_MARKERS__INTERACTIVE_MARKER_FIXTURES_HPP_ diff --git a/test/interactive_markers/mock_interactive_marker_client.hpp b/test/interactive_markers/mock_interactive_marker_client.hpp new file mode 100644 index 00000000..43fd5359 --- /dev/null +++ b/test/interactive_markers/mock_interactive_marker_client.hpp @@ -0,0 +1,110 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Open Source Robotics Foundation, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#ifndef INTERACTIVE_MARKERS__MOCK_INTERACTIVE_MARKER_CLIENT_HPP_ +#define INTERACTIVE_MARKERS__MOCK_INTERACTIVE_MARKER_CLIENT_HPP_ +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "rclcpp/rclcpp.hpp" +#include "visualization_msgs/msg/interactive_marker.hpp" +#include "visualization_msgs/msg/interactive_marker_feedback.hpp" +#include "visualization_msgs/msg/interactive_marker_pose.hpp" +#include "visualization_msgs/msg/interactive_marker_update.hpp" +#include "visualization_msgs/srv/get_interactive_markers.hpp" + +class MockInteractiveMarkerClient : public rclcpp::Node +{ +public: + using SharedFuture = + rclcpp::Client::SharedFuture; + using SharedResponse = + rclcpp::Client::SharedResponse; + + MockInteractiveMarkerClient( + const std::string topic_namespace = "mock_interactive_markers", + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + void publishFeedback(const visualization_msgs::msg::InteractiveMarkerFeedback & feedback); + SharedFuture requestInteractiveMarkers(); + + uint32_t updates_received; + visualization_msgs::msg::InteractiveMarkerUpdate::SharedPtr last_update_message; + visualization_msgs::srv::GetInteractiveMarkers::Response::SharedPtr last_response_message; + + std::string topic_namespace_; + rclcpp::Client::SharedPtr client_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr subscription_; +}; // class MockInteractiveMarkerClient + +MockInteractiveMarkerClient::MockInteractiveMarkerClient( + const std::string topic_namespace, + const rclcpp::NodeOptions & options) +: rclcpp::Node("mock_interactive_marker_client", options), + updates_received(0), + topic_namespace_(topic_namespace) +{ + client_ = create_client( + topic_namespace_ + "/get_interactive_markers"); + + publisher_ = create_publisher( + topic_namespace_ + "/feedback", + 1); + + subscription_ = create_subscription( + topic_namespace_ + "/update", + 1, + [this](const visualization_msgs::msg::InteractiveMarkerUpdate::SharedPtr update) + { + RCLCPP_INFO(this->get_logger(), "Update received"); + ++this->updates_received; + last_update_message = update; + }); +} + +void MockInteractiveMarkerClient::publishFeedback( + const visualization_msgs::msg::InteractiveMarkerFeedback & feedback) +{ + publisher_->publish(feedback); +} + +MockInteractiveMarkerClient::SharedFuture +MockInteractiveMarkerClient::requestInteractiveMarkers() +{ + if (!client_->wait_for_service(std::chrono::seconds(2))) { + RCLCPP_WARN(get_logger(), "Reqested interactive markers, but service is not available"); + } + return client_->async_send_request( + std::make_shared()); +} +#endif // INTERACTIVE_MARKERS__MOCK_INTERACTIVE_MARKER_CLIENT_HPP_ diff --git a/test/interactive_markers/mock_interactive_marker_server.hpp b/test/interactive_markers/mock_interactive_marker_server.hpp new file mode 100644 index 00000000..6225bb2c --- /dev/null +++ b/test/interactive_markers/mock_interactive_marker_server.hpp @@ -0,0 +1,131 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Open Source Robotics Foundation, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#ifndef INTERACTIVE_MARKERS__MOCK_INTERACTIVE_MARKER_SERVER_HPP_ +#define INTERACTIVE_MARKERS__MOCK_INTERACTIVE_MARKER_SERVER_HPP_ +#include +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "rclcpp/rclcpp.hpp" +#include "visualization_msgs/msg/interactive_marker.hpp" +#include "visualization_msgs/msg/interactive_marker_feedback.hpp" +#include "visualization_msgs/msg/interactive_marker_pose.hpp" +#include "visualization_msgs/msg/interactive_marker_update.hpp" +#include "visualization_msgs/srv/get_interactive_markers.hpp" + +#include "interactive_marker_fixtures.hpp" + +class MockInteractiveMarkerServer : public rclcpp::Node +{ +public: + MockInteractiveMarkerServer( + const std::string topic_namespace = "mock_interactive_markers", + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + ~MockInteractiveMarkerServer(); + + void publishUpdate(const geometry_msgs::msg::Pose & pose = geometry_msgs::msg::Pose()); + + uint64_t requests_received_; + uint64_t feedback_received_; + + std::string topic_namespace_; + uint64_t sequence_number_; + std::vector markers_; + rclcpp::Service::SharedPtr service_; + rclcpp::Subscription::SharedPtr + subscription_; + rclcpp::Publisher::SharedPtr publisher_; +}; // class MockInteractiveMarkerServer + +MockInteractiveMarkerServer::MockInteractiveMarkerServer( + const std::string topic_namespace, + const rclcpp::NodeOptions & options) +: rclcpp::Node("mock_interactive_marker_server", options), + requests_received_(0), + feedback_received_(0), + topic_namespace_(topic_namespace), + sequence_number_(0) +{ + markers_ = get_interactive_markers(); + + service_ = create_service( + topic_namespace_ + "/get_interactive_markers", + [this](const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) + { + (void)request_header; + (void)request; + RCLCPP_INFO(this->get_logger(), "Interactive markers request received"); + response->sequence_number = this->sequence_number_; + response->markers = this->markers_; + ++this->requests_received_; + }); + + subscription_ = create_subscription( + topic_namespace_ + "/feedback", + 10, + [this](const visualization_msgs::msg::InteractiveMarkerFeedback::SharedPtr feedback) + { + (void)feedback; + RCLCPP_INFO(this->get_logger(), "Feedback received"); + ++this->feedback_received_; + }); + + publisher_ = create_publisher( + topic_namespace_ + "/update", + 10); + RCLCPP_INFO(get_logger(), "Mock server ready"); +} + +MockInteractiveMarkerServer::~MockInteractiveMarkerServer() +{ + RCLCPP_INFO(get_logger(), "Mock server destroyed"); +} + +void MockInteractiveMarkerServer::publishUpdate(const geometry_msgs::msg::Pose & pose) +{ + visualization_msgs::msg::InteractiveMarkerUpdate update; + update.seq_num = sequence_number_++; + update.type = update.UPDATE; + markers_[0].pose = pose; + visualization_msgs::msg::InteractiveMarkerPose marker_pose; + marker_pose.pose = markers_[0].pose; + marker_pose.header = markers_[0].header; + marker_pose.name = markers_[0].name; + update.poses.push_back(marker_pose); + + publisher_->publish(update); +} +#endif // INTERACTIVE_MARKERS__MOCK_INTERACTIVE_MARKER_SERVER_HPP_ diff --git a/test/interactive_markers/test_interactive_marker_client.cpp b/test/interactive_markers/test_interactive_marker_client.cpp new file mode 100644 index 00000000..443113d4 --- /dev/null +++ b/test/interactive_markers/test_interactive_marker_client.cpp @@ -0,0 +1,340 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Open Source Robotics Foundation, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#include + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "tf2/buffer_core.h" + +#include "interactive_marker_fixtures.hpp" +#include "mock_interactive_marker_server.hpp" +#include "timed_expect.hpp" + +#include "interactive_markers/interactive_marker_client.hpp" + +using ClientState = interactive_markers::InteractiveMarkerClient::State; + +TEST(TestInteractiveMarkerClientInitialize, construction_destruction) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_interactive_marker_client_node", ""); + auto buffer = std::make_shared(); + + { + interactive_markers::InteractiveMarkerClient client(node, buffer); + } + { + interactive_markers::InteractiveMarkerClient client( + node, buffer, "test_frame_id", "test_namespace", std::chrono::seconds(3)); + } + { + interactive_markers::InteractiveMarkerClient client( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_services_interface(), + node->get_node_graph_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + buffer, + "test_frame_id", + "test_namespace", + std::chrono::milliseconds(100)); + } + { + // Invalid namespace (shouldn't crash) + interactive_markers::InteractiveMarkerClient client( + node, buffer, "test_frame_id", "th!s//is/aninvalid_namespace?"); + } + + rclcpp::shutdown(); +} + +class TestInteractiveMarkerClient : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + target_frame_id_ = "test_target_frame_id"; + topic_namespace_ = "test_namespace"; + executor_ = std::make_unique(); + node_ = std::make_shared("test_interactive_marker_server_node", ""); + buffer_ = std::make_shared(); + client_ = std::make_unique( + node_, buffer_, target_frame_id_, topic_namespace_); + executor_->add_node(node_); + } + + void TearDown() + { + client_.reset(); + buffer_.reset(); + node_.reset(); + executor_.reset(); + } + + void makeTfDataAvailable(const std::vector & markers) + { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.frame_id = target_frame_id_; + transform_stamped.header.stamp = builtin_interfaces::msg::Time(node_->now()); + transform_stamped.transform.rotation.w = 1.0; + for (std::size_t i = 0u; i < markers.size(); ++i) { + transform_stamped.child_frame_id = markers[0].header.frame_id; + ASSERT_TRUE(buffer_->setTransform(transform_stamped, "mock_tf_authority")); + } + } + + std::string target_frame_id_; + std::string topic_namespace_; + std::unique_ptr executor_; + rclcpp::Node::SharedPtr node_; + std::shared_ptr buffer_; + std::unique_ptr client_; +}; // class TestInteractiveMarkerClient + +TEST_F(TestInteractiveMarkerClient, states) +{ + using namespace std::chrono_literals; + + // IDLE -> IDLE + { + client_.reset( + new interactive_markers::InteractiveMarkerClient( + node_, buffer_, target_frame_id_, topic_namespace_)); + EXPECT_EQ(client_->getState(), ClientState::IDLE); + client_->update(); + EXPECT_EQ(client_->getState(), ClientState::IDLE); + } + + // IDLE -> INITIALIZE -> IDLE -> INITIALIZE + { + client_.reset( + new interactive_markers::InteractiveMarkerClient( + node_, buffer_, target_frame_id_, topic_namespace_)); + client_->update(); + EXPECT_EQ(client_->getState(), ClientState::IDLE); + // Start server + auto mock_server = std::make_shared(topic_namespace_); + executor_->add_node(mock_server); + // Repeatedly call the client's update method until the server is detected + auto update_func = std::bind( + &interactive_markers::InteractiveMarkerClient::update, client_.get()); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::INITIALIZE, 3s, 10ms, (*executor_), update_func); + // Disconnect server + executor_->remove_node(mock_server); + mock_server.reset(); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::IDLE, 3s, 10ms, (*executor_), update_func); + // Re-start server + mock_server.reset(new MockInteractiveMarkerServer(topic_namespace_)); + executor_->add_node(mock_server); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::INITIALIZE, 3s, 10ms, (*executor_), update_func); + executor_->remove_node(mock_server); + } + + // IDLE -> INITIALIZE -> RUNNING -> IDLE + { + client_.reset( + new interactive_markers::InteractiveMarkerClient( + node_, buffer_, target_frame_id_, topic_namespace_)); + EXPECT_EQ(client_->getState(), ClientState::IDLE); + // Start server + auto mock_server = std::make_shared(topic_namespace_); + executor_->add_node(mock_server); + // Repeatedly call the client's update method until the server is detected + auto update_func = std::bind( + &interactive_markers::InteractiveMarkerClient::update, client_.get()); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::INITIALIZE, 3s, 10ms, (*executor_), update_func); + // Make the required TF data in order to finish initializing + makeTfDataAvailable(mock_server->markers_); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::RUNNING, 3s, 10ms, (*executor_), update_func); + // Disconnect server + executor_->remove_node(mock_server); + mock_server.reset(); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::IDLE, 3s, 10ms, (*executor_), update_func); + } + + // IDLE -> INITIALIZE -> RUNNING -> INITIALIZE -> IDLE + { + client_.reset( + new interactive_markers::InteractiveMarkerClient( + node_, buffer_, target_frame_id_, topic_namespace_)); + EXPECT_EQ(client_->getState(), ClientState::IDLE); + // Start server + auto mock_server = std::make_shared(topic_namespace_); + executor_->add_node(mock_server); + // Repeatedly call the client's update method until the server is detected + auto update_func = std::bind( + &interactive_markers::InteractiveMarkerClient::update, client_.get()); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::INITIALIZE, 3s, 10ms, (*executor_), update_func); + // Make the required TF data in order to finish initializing + makeTfDataAvailable(mock_server->markers_); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::RUNNING, 3s, 10ms, (*executor_), update_func); + // Cause a sync error by skipping a sequence number, which should cause state change + mock_server->publishUpdate(); + mock_server->sequence_number_ += 1; + mock_server->publishUpdate(); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::INITIALIZE, 3s, 10ms, (*executor_), update_func); + // Disconnect server + executor_->remove_node(mock_server); + mock_server.reset(); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::IDLE, 3s, 10ms, (*executor_), update_func); + } +} + +TEST_F(TestInteractiveMarkerClient, init_callback) +{ + using namespace std::chrono_literals; + + bool called = false; + auto callback = [&called](visualization_msgs::srv::GetInteractiveMarkers::Response::SharedPtr) + { + called = true; + }; + + client_->setInitializeCallback(callback); + + // Creating a server and publishing the required TF data should trigger the callback + auto mock_server = std::make_shared(topic_namespace_); + executor_->add_node(mock_server); + makeTfDataAvailable(mock_server->markers_); + auto update_func = std::bind( + &interactive_markers::InteractiveMarkerClient::update, client_.get()); + TIMED_EXPECT_EQ(called, true, 3s, 10ms, (*executor_), update_func); +} + +TEST_F(TestInteractiveMarkerClient, update_callback) +{ + using namespace std::chrono_literals; + + geometry_msgs::msg::Pose output_pose; + auto callback = [&output_pose](visualization_msgs::msg::InteractiveMarkerUpdate::SharedPtr msg) + { + ASSERT_EQ(msg->poses.size(), 1u); + output_pose = msg->poses[0].pose; + }; + + client_->setUpdateCallback(callback); + + // First, we need to get into the RUNNING state + auto mock_server = std::make_shared(topic_namespace_); + executor_->add_node(mock_server); + makeTfDataAvailable(mock_server->markers_); + auto update_func = std::bind( + &interactive_markers::InteractiveMarkerClient::update, client_.get()); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::RUNNING, 3s, 10ms, (*executor_), update_func); + + // Publish an update message + geometry_msgs::msg::Pose input_pose; + input_pose.position.x = 42.0; + input_pose.position.y = -2.2; + input_pose.position.z = 0.0; + input_pose.orientation.w = 0.5; + mock_server->publishUpdate(input_pose); + TIMED_EXPECT_EQ( + output_pose.position.x, input_pose.position.x, 3s, 10ms, (*executor_), update_func); + EXPECT_EQ(output_pose.position.y, input_pose.position.y); + EXPECT_EQ(output_pose.position.z, input_pose.position.z); + EXPECT_EQ(output_pose.orientation.w, input_pose.orientation.w); +} + +TEST_F(TestInteractiveMarkerClient, reset_callback) +{ + using namespace std::chrono_literals; + + bool reset_called = false; + auto callback = [&reset_called]() + { + reset_called = true; + }; + + client_->setResetCallback(callback); + + // Get out of the IDLE state by creating a server and publishing the required TF data + auto mock_server = std::make_shared(topic_namespace_); + executor_->add_node(mock_server); + makeTfDataAvailable(mock_server->markers_); + auto update_func = std::bind( + &interactive_markers::InteractiveMarkerClient::update, client_.get()); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::INITIALIZE, 3s, 10ms, (*executor_), update_func); + + // Disconnect server to cause reset + executor_->remove_node(mock_server); + mock_server.reset(); + TIMED_EXPECT_EQ( + client_->getState(), ClientState::IDLE, 3s, 10ms, (*executor_), update_func); + EXPECT_TRUE(reset_called); +} + +TEST_F(TestInteractiveMarkerClient, feedback) +{ + using namespace std::chrono_literals; + + auto mock_server = std::make_shared(topic_namespace_); + executor_->add_node(mock_server); + + EXPECT_EQ(mock_server->feedback_received_, 0u); + + // Publish a feedback message + visualization_msgs::msg::InteractiveMarkerFeedback feedback_msg; + client_->publishFeedback(feedback_msg); + TIMED_EXPECT_EQ(mock_server->feedback_received_, 1u, 3s, 10ms, (*executor_)); + + // Publish another + client_->publishFeedback(feedback_msg); + TIMED_EXPECT_EQ(mock_server->feedback_received_, 2u, 3s, 10ms, (*executor_)); +} diff --git a/test/interactive_markers/test_interactive_marker_server.cpp b/test/interactive_markers/test_interactive_marker_server.cpp new file mode 100644 index 00000000..4470b83f --- /dev/null +++ b/test/interactive_markers/test_interactive_marker_server.cpp @@ -0,0 +1,364 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Open Source Robotics Foundation, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#include + +#include +#include +#include +#include + +#include "interactive_marker_fixtures.hpp" +#include "mock_interactive_marker_client.hpp" +#include "timed_expect.hpp" + +#include "interactive_markers/interactive_marker_server.hpp" + +TEST(TestInteractiveMarkerServer, construction_and_destruction) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_construction_and_destruction_node", ""); + { + interactive_markers::InteractiveMarkerServer server("", node); + } + { + interactive_markers::InteractiveMarkerServer server("test_node_ptr_ctor_server", node); + } + { + interactive_markers::InteractiveMarkerServer server( + "test_node_interfaces_ctor_server", + node->get_node_base_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + node->get_node_topics_interface(), + node->get_node_services_interface()); + } + + rclcpp::shutdown(); +} + +TEST(TestInteractiveMarkerServer, insert) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_insert_node", ""); + interactive_markers::InteractiveMarkerServer server("test_insert_server", node); + + // Insert some markers + std::vector markers = get_interactive_markers(); + for (const auto marker : markers) { + server.insert(marker); + } + + // Expect empty until 'applyChanges' is called + EXPECT_TRUE(server.empty()); + EXPECT_EQ(server.size(), 0u); + server.applyChanges(); + EXPECT_FALSE(server.empty()); + EXPECT_EQ(server.size(), markers.size()); + + rclcpp::shutdown(); +} + +class TestInteractiveMarkerServerWithMarkers : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + const std::string topic_namespace = "test_namespace"; + node_ = std::make_shared("test_interactive_marker_server_node", ""); + + server_ = std::make_unique( + topic_namespace, node_); + + // Insert some markers + markers_ = get_interactive_markers(); + for (const auto marker : markers_) { + server_->insert(marker); + } + server_->applyChanges(); + + mock_client_ = std::make_shared(topic_namespace); + + executor_.add_node(node_); + executor_.add_node(mock_client_); + + // Wait for discovery (or timeout) + ASSERT_TRUE(mock_client_->client_->wait_for_service(std::chrono::seconds(3))); + } + + void TearDown() + { + mock_client_.reset(); + server_.reset(); + node_.reset(); + } + + rclcpp::executors::SingleThreadedExecutor executor_; + rclcpp::Node::SharedPtr node_; + std::unique_ptr server_; + std::shared_ptr mock_client_; + std::vector markers_; +}; // class TestInteractiveMarkerServer + +TEST_F(TestInteractiveMarkerServerWithMarkers, erase) +{ + // Erase a marker + EXPECT_TRUE(server_->erase(markers_[0].name)); + EXPECT_EQ(server_->size(), markers_.size()); + server_->applyChanges(); + EXPECT_EQ(server_->size(), markers_.size() - 1u); + + // Erase a marker that has just been inserted + server_->insert(markers_[0]); + EXPECT_TRUE(server_->erase(markers_[0].name)); + EXPECT_EQ(server_->size(), markers_.size() - 1u); + server_->applyChanges(); + EXPECT_EQ(server_->size(), markers_.size() - 1u); + + // Erase an invalid marker + EXPECT_FALSE(server_->erase("this_Is_the_name_0f_a_marker_that_doesn't ex1st")); + server_->applyChanges(); + EXPECT_EQ(server_->size(), markers_.size() - 1u); +} + +TEST_F(TestInteractiveMarkerServerWithMarkers, clear) +{ + // Clear all markers + server_->clear(); + EXPECT_EQ(server_->size(), markers_.size()); + server_->applyChanges(); + ASSERT_EQ(server_->size(), 0u); + + // Clear an empty server + server_->clear(); + EXPECT_EQ(server_->size(), 0u); +} + +TEST_F(TestInteractiveMarkerServerWithMarkers, get_marker_by_name) +{ + // Get markers + for (const auto input_marker : markers_) { + visualization_msgs::msg::InteractiveMarker output_marker; + EXPECT_TRUE(server_->get(input_marker.name, output_marker)); + EXPECT_EQ(output_marker.header.frame_id, input_marker.header.frame_id); + EXPECT_EQ(output_marker.pose.position.x, input_marker.pose.position.x); + EXPECT_EQ(output_marker.pose.orientation.w, input_marker.pose.orientation.w); + EXPECT_EQ(output_marker.name, input_marker.name); + EXPECT_EQ(output_marker.description, input_marker.description); + ASSERT_EQ(output_marker.menu_entries.size(), input_marker.menu_entries.size()); + for (std::size_t i = 0u; i < output_marker.menu_entries.size(); ++i) { + EXPECT_EQ(output_marker.menu_entries[i].id, input_marker.menu_entries[i].id); + EXPECT_EQ(output_marker.menu_entries[i].title, input_marker.menu_entries[i].title); + EXPECT_EQ(output_marker.menu_entries[i].command, input_marker.menu_entries[i].command); + } + ASSERT_EQ(output_marker.controls.size(), input_marker.controls.size()); + for (std::size_t i = 0u; i < output_marker.controls.size(); ++i) { + EXPECT_EQ(output_marker.controls[i].name, input_marker.controls[i].name); + EXPECT_EQ(output_marker.controls[i].always_visible, input_marker.controls[i].always_visible); + } + } + + // Get an invalid marker + { + visualization_msgs::msg::InteractiveMarker output_marker; + EXPECT_FALSE(server_->get("n0t_the_name_of_@ marker", output_marker)); + } + + // Get a pending erased marker + { + EXPECT_TRUE(server_->erase(markers_[0].name)); + visualization_msgs::msg::InteractiveMarker output_marker; + EXPECT_FALSE(server_->get(markers_[0].name, output_marker)); + } +} + +TEST_F(TestInteractiveMarkerServerWithMarkers, set_pose) +{ + // Set a pose + { + geometry_msgs::msg::Pose pose; + pose.position.x = 1.0; + pose.position.y = -2.0; + pose.position.z = 3.14; + pose.orientation.w = 0.5; + EXPECT_TRUE(server_->setPose(markers_[0].name, pose)); + server_->applyChanges(); + visualization_msgs::msg::InteractiveMarker output_marker; + EXPECT_TRUE(server_->get(markers_[0].name, output_marker)); + EXPECT_EQ(output_marker.pose.position.x, pose.position.x); + EXPECT_EQ(output_marker.pose.position.y, pose.position.y); + EXPECT_EQ(output_marker.pose.position.z, pose.position.z); + EXPECT_EQ(output_marker.pose.orientation.w, pose.orientation.w); + EXPECT_EQ(output_marker.header.frame_id, markers_[0].header.frame_id); + } + + // Set a pose with header update + { + geometry_msgs::msg::Pose pose; + pose.position.x = 1.0; + pose.position.y = -2.0; + pose.position.z = 3.14; + pose.orientation.w = 0.5; + std_msgs::msg::Header header; + header.frame_id = "test_updating_to_a_new_header"; + EXPECT_TRUE(server_->setPose(markers_[0].name, pose, header)); + server_->applyChanges(); + visualization_msgs::msg::InteractiveMarker output_marker; + EXPECT_TRUE(server_->get(markers_[0].name, output_marker)); + EXPECT_EQ(output_marker.pose.position.x, pose.position.x); + EXPECT_EQ(output_marker.pose.position.y, pose.position.y); + EXPECT_EQ(output_marker.pose.position.z, pose.position.z); + EXPECT_EQ(output_marker.pose.orientation.w, pose.orientation.w); + EXPECT_NE(output_marker.header.frame_id, markers_[0].header.frame_id); + EXPECT_EQ(output_marker.header.frame_id, header.frame_id); + } + + // Set pose of invalid marker + { + geometry_msgs::msg::Pose pose; + pose.orientation.w = 1.0; + EXPECT_FALSE(server_->setPose("test_n0t_a_valid_marker_n@me", pose)); + } +} + +TEST_F(TestInteractiveMarkerServerWithMarkers, set_callback) +{ + EXPECT_TRUE(server_->setCallback(markers_[0].name, nullptr)); + EXPECT_FALSE(server_->setCallback("test_n0t_a_valid_marker_n@me", nullptr)); +} + +TEST_F(TestInteractiveMarkerServerWithMarkers, feedback_communication) +{ + using namespace std::chrono_literals; + + // Register a callback function to capture output + visualization_msgs::msg::InteractiveMarkerFeedback output_feedback; + auto callback = + [&output_feedback] + (interactive_markers::InteractiveMarkerServer::FeedbackConstSharedPtr feedback) + { + output_feedback = *feedback; + }; + EXPECT_TRUE(server_->setCallback(markers_[0].name, callback)); + server_->applyChanges(); + + // Populate and publish mock feedback + visualization_msgs::msg::InteractiveMarkerFeedback feedback; + feedback.client_id = "test_client_id"; + feedback.marker_name = markers_[0].name; + feedback.event_type = feedback.POSE_UPDATE; + feedback.pose.position.x = -3.14; + feedback.pose.orientation.w = 1.0; + mock_client_->publishFeedback(feedback); + TIMED_EXPECT_EQ(output_feedback.client_id, feedback.client_id, 3s, 10ms, executor_); + EXPECT_EQ(output_feedback.marker_name, markers_[0].name); + EXPECT_EQ(output_feedback.pose.position.x, feedback.pose.position.x); + EXPECT_EQ(output_feedback.pose.orientation.w, feedback.pose.orientation.w); +} + +TEST_F(TestInteractiveMarkerServerWithMarkers, update_communication) +{ + using namespace std::chrono_literals; + + ASSERT_EQ(mock_client_->updates_received, 0u); + // This should not trigger an update publication + server_->applyChanges(); + // Adding a marker should trigger an update + visualization_msgs::msg::InteractiveMarker marker; + marker.name = "test_update_from_added_marker"; + server_->insert(marker); + server_->applyChanges(); + TIMED_EXPECT_EQ(mock_client_->updates_received, 1u, 3s, 10ms, executor_); + ASSERT_NE(mock_client_->last_update_message, nullptr); + EXPECT_EQ(mock_client_->last_update_message->markers.size(), 1u); + EXPECT_EQ(mock_client_->last_update_message->poses.size(), 0u); + EXPECT_EQ(mock_client_->last_update_message->erases.size(), 0u); + // Modifying a marker should trigger an update + geometry_msgs::msg::Pose pose; + pose.orientation.w = 1.0; + server_->setPose( + markers_[0].name, + pose); + server_->applyChanges(); + TIMED_EXPECT_EQ(mock_client_->updates_received, 2u, 3s, 10ms, executor_); + EXPECT_EQ(mock_client_->last_update_message->markers.size(), 0u); + EXPECT_EQ(mock_client_->last_update_message->poses.size(), 1u); + EXPECT_EQ(mock_client_->last_update_message->erases.size(), 0u); + // Erasing a marker should trigger an update + ASSERT_TRUE(server_->erase(markers_[0].name)); + server_->applyChanges(); + TIMED_EXPECT_EQ(mock_client_->updates_received, 3u, 3s, 10ms, executor_); + EXPECT_EQ(mock_client_->last_update_message->markers.size(), 0u); + EXPECT_EQ(mock_client_->last_update_message->poses.size(), 0u); + ASSERT_EQ(mock_client_->last_update_message->erases.size(), 1u); + EXPECT_EQ(mock_client_->last_update_message->erases[0], markers_[0].name); +} + +TEST_F(TestInteractiveMarkerServerWithMarkers, get_interactive_markers_communication) +{ + using namespace std::chrono_literals; + + MockInteractiveMarkerClient::SharedFuture future = mock_client_->requestInteractiveMarkers(); + auto ret = executor_.spin_until_future_complete( + future, 3000ms); + ASSERT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + visualization_msgs::srv::GetInteractiveMarkers::Response::SharedPtr response = future.get(); + ASSERT_EQ(response->markers.size(), markers_.size()); + for (std::size_t i = 0u; i < markers_.size(); ++i) { + EXPECT_EQ(response->markers[i].header.frame_id, markers_[i].header.frame_id); + EXPECT_EQ(response->markers[i].pose.position.x, markers_[i].pose.position.x); + EXPECT_EQ(response->markers[i].pose.orientation.w, markers_[i].pose.orientation.w); + EXPECT_EQ(response->markers[i].name, markers_[i].name); + EXPECT_EQ(response->markers[i].description, markers_[i].description); + ASSERT_EQ(response->markers[i].menu_entries.size(), markers_[i].menu_entries.size()); + for (std::size_t j = 0u; j < markers_[i].menu_entries.size(); ++j) { + EXPECT_EQ(response->markers[i].menu_entries[j].id, markers_[i].menu_entries[j].id); + EXPECT_EQ(response->markers[i].menu_entries[j].title, markers_[i].menu_entries[j].title); + EXPECT_EQ(response->markers[i].menu_entries[j].command, markers_[i].menu_entries[j].command); + } + ASSERT_EQ(response->markers[i].controls.size(), markers_[i].controls.size()); + for (std::size_t j = 0u; j < markers_[i].controls.size(); ++j) { + EXPECT_EQ(response->markers[i].controls[j].name, markers_[i].controls[j].name); + EXPECT_EQ( + response->markers[i].controls[j].always_visible, markers_[i].controls[j].always_visible); + } + } +} diff --git a/test/interactive_markers/timed_expect.hpp b/test/interactive_markers/timed_expect.hpp new file mode 100644 index 00000000..fa01086e --- /dev/null +++ b/test/interactive_markers/timed_expect.hpp @@ -0,0 +1,85 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 Open Source Robotics Foundation, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#ifndef INTERACTIVE_MARKERS__TIMED_EXPECT_HPP_ +#define INTERACTIVE_MARKERS__TIMED_EXPECT_HPP_ +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#define TIMED_EXPECT_EQ_6_ARGS(lhs, rhs, timeout, period, executor, func, ...) \ + do { \ + auto start_time = std::chrono::steady_clock::now(); \ + while (lhs != rhs && \ + (std::chrono::steady_clock::now() - start_time < timeout)) \ + { \ + executor.spin_once(std::chrono::nanoseconds(0)); \ + func(); \ + std::this_thread::sleep_for(period); \ + } \ + EXPECT_EQ(lhs, rhs); \ + } while (0) + +#define TIMED_EXPECT_EQ_5_ARGS(lhs, rhs, timeout, period, executor, ...) \ + TIMED_EXPECT_EQ_6_ARGS(lhs, rhs, timeout, period, executor, [] {}, UNUSED) + +#define TIMED_EXPECT_EQ_CHOOSER(lhs, rhs, timeout, period, executor, func, args, ...) args + +// Workaround for MSVC's preprocessor +// See: https://stackoverflow.com/a/5134656 +#define EXPAND(x) x + +/// Assert equality with a timeout. +/** + * This macro blocks until lhs == rhs or a timeout occurs. + * The provided executor spins for the duration of this macro. + * The optional function is also repeatedly called. + * + * The == and != operators must be defined for lhs and rhs types. + * + * \param lhs The left hand operand of the equality test. + * \param rhs The right hand operand of the equality test. + * \param timeout The duration after which the test will fail. + * \param period The period at which the executor spins and the provided function is called. + * \param executor The executor to spin. + * \param func The optional function to call. + */ +#define TIMED_EXPECT_EQ(...) \ + EXPAND(TIMED_EXPECT_EQ_CHOOSER( \ + __VA_ARGS__, \ + TIMED_EXPECT_EQ_6_ARGS(__VA_ARGS__, UNUSED), \ + TIMED_EXPECT_EQ_5_ARGS(__VA_ARGS__, UNUSED), \ + UNUSED)) + +#endif // INTERACTIVE_MARKERS__TIMED_EXPECT_HPP_