pax_global_header00006660000000000000000000000064133675456270014534gustar00rootroot0000000000000052 comment=492b941ff26c9638dc2d3d96fe1741ca652045d2 ros-interactive-markers-1.11.4/000077500000000000000000000000001336754562700164005ustar00rootroot00000000000000ros-interactive-markers-1.11.4/.gitignore000066400000000000000000000000151336754562700203640ustar00rootroot00000000000000*~ lib build ros-interactive-markers-1.11.4/CHANGELOG.rst000066400000000000000000000054661336754562700204340ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package interactive_markers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.11.4 (2018-04-16) ------------------- * Fixed a crash when updates arrive, or are being processed, while shutdown is called (`#36 `_) * Contributors: Simon Schmeisser 1.11.3 (2016-08-24) ------------------- * The ``processFeedback`` function of the menu handler no longer catches the ``KeyErrors`` of the feedback_cb. See: `#29 `_ * Added the ``empty()`` and ``size()`` members to ``InteractiveMarkerServer`` interface. See: `#30 `_ * Contributors: Blake Anderson, Guglielmo Gemignani 1.11.2 (2016-08-24) ------------------- * Fix build when disabling tests with ``-DCATKIN_ENABLE_TESTING=OFF``. See: `#26 `_ * Fix use of uninitialized variables. See: `#24 `_ * Fix potential segfault when shutting down. See: `#25 `_ * Contributors: Alexis Ballier, David Gossow, Max Schwarz 1.11.1 (2014-12-16) ------------------- * Added explicit keyword argument queue_size for publisher in Python code and use the same default queue_size value as C++. * Fixed a SEGFAULT in setPose reported in `#18 `_ Previously, calling setPose() on an interactive marker causes a SEGFAULT if applyChanges() was not called on the server at least once since the marker was created. I traced the actual SEGFAULT to the doSetPose function. The value of header passed from setPose() is invalid because, in this case, marker_context_it = marker_contexts\_.end(). I added a check for this case and, if there is no marker is present, instead use the header from the pending update. * Contributors: David Gossow, Mike Koval, William Woodall, ipa-fxm 1.11.0 (2014-02-24) ------------------- * Adding William Woodall as maintainer * fix threading bugs Fix locking of data structures shared across threads. * Contributors: Acorn Pooley, William Woodall, hersh 1.10.2 (2014-02-03) ------------------- * fix regression in menu_handler.py fixes `#14 `_ * Contributors: William Woodall 1.10.1 (2014-01-27) ------------------- * cleanup python code and package contents * remove useless dependencies * Contributors: Vincent Rabaud, William Woodall 1.10.0 (2014-01-23) ------------------- * remove debug statement that could produce segfault; init_it->msg->markers may be empty * Contributors: Filip Jares ros-interactive-markers-1.11.4/CMakeLists.txt000066400000000000000000000041621336754562700211430ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) 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}) 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 ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) install(DIRECTORY include/interactive_markers/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h") # C++ Unit Tests # if(CATKIN_ENABLE_TESTING) include_directories(${GTEST_INCLUDE_DIRS}) 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) 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) 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) # 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) # 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) endif() ros-interactive-markers-1.11.4/include/000077500000000000000000000000001336754562700200235ustar00rootroot00000000000000ros-interactive-markers-1.11.4/include/interactive_markers/000077500000000000000000000000001336754562700240645ustar00rootroot00000000000000ros-interactive-markers-1.11.4/include/interactive_markers/detail/000077500000000000000000000000001336754562700253265ustar00rootroot00000000000000ros-interactive-markers-1.11.4/include/interactive_markers/detail/message_context.h000066400000000000000000000062021336754562700306670ustar00rootroot00000000000000/* * 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_ */ ros-interactive-markers-1.11.4/include/interactive_markers/detail/single_client.h000066400000000000000000000104551336754562700303230ustar00rootroot00000000000000/* * 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_ */ ros-interactive-markers-1.11.4/include/interactive_markers/detail/state_machine.h000066400000000000000000000054521336754562700303110ustar00rootroot00000000000000/* * 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_ */ ros-interactive-markers-1.11.4/include/interactive_markers/interactive_marker_client.h000066400000000000000000000151161336754562700314550ustar00rootroot00000000000000/* * 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 ros-interactive-markers-1.11.4/include/interactive_markers/interactive_marker_server.h000066400000000000000000000215631336754562700315100ustar00rootroot00000000000000/* * 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 ros-interactive-markers-1.11.4/include/interactive_markers/menu_handler.h000066400000000000000000000124261336754562700267030ustar00rootroot00000000000000/* * 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 ros-interactive-markers-1.11.4/include/interactive_markers/tools.h000066400000000000000000000107421336754562700254010ustar00rootroot00000000000000/* * 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 ros-interactive-markers-1.11.4/package.xml000066400000000000000000000017061336754562700205210ustar00rootroot00000000000000 interactive_markers 3D interactive marker communication library for RViz and similar tools. William Woodall BSD 1.11.4 David Gossow http://ros.org/wiki/interactive_markers catkin rosconsole roscpp rospy rostest std_msgs tf visualization_msgs rosconsole roscpp rospy rostest std_msgs tf visualization_msgs ros-interactive-markers-1.11.4/setup.py000066400000000000000000000003441336754562700201130ustar00rootroot00000000000000#!/usr/bin/env python from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['interactive_markers'], package_dir={'': 'src'} ) setup(**d) ros-interactive-markers-1.11.4/src/000077500000000000000000000000001336754562700171675ustar00rootroot00000000000000ros-interactive-markers-1.11.4/src/interactive_marker_client.cpp000066400000000000000000000200441336754562700251070ustar00rootroot00000000000000/* * 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"); 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) { target_frame_ = target_frame; if ( !topic_ns.empty() ) { subscribe( topic_ns ); } callbacks_.setStatusCb( boost::bind( &InteractiveMarkerClient::statusCb, this, _1, _2, _3 ) ); } InteractiveMarkerClient::~InteractiveMarkerClient() { shutdown(); } /// Subscribe to given topic void InteractiveMarkerClient::subscribe( std::string topic_ns ) { topic_ns_ = topic_ns; subscribeUpdate(); subscribeInit(); } void InteractiveMarkerClient::setInitCb( const InitCallback& cb ) { callbacks_.setInitCb( cb ); } void InteractiveMarkerClient::setUpdateCb( const UpdateCallback& cb ) { callbacks_.setUpdateCb( cb ); } void InteractiveMarkerClient::setResetCb( const ResetCallback& cb ) { callbacks_.setResetCb( cb ); } void InteractiveMarkerClient::setStatusCb( const StatusCallback& cb ) { status_cb_ = cb; } void InteractiveMarkerClient::setTargetFrame( std::string target_frame ) { target_frame_ = target_frame; DBG_MSG("Target frame is now %s", target_frame_.c_str() ); switch ( state_ ) { case IDLE: break; case INIT: case RUNNING: shutdown(); subscribeUpdate(); subscribeInit(); break; } } void InteractiveMarkerClient::shutdown() { 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; } } void InteractiveMarkerClient::subscribeUpdate() { 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; } } callbacks_.statusCb( OK, "General", "Waiting for messages."); } void InteractiveMarkerClient::subscribeInit() { if ( state_ != INIT && !topic_ns_.empty() ) { 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()) ); } } } template void InteractiveMarkerClient::process( const MsgConstPtrT& msg ) { callbacks_.statusCb( OK, "General", "Receiving messages."); // get caller ID of the sending entity if ( msg->server_id.empty() ) { callbacks_.statusCb( ERROR, "General", "Received message with empty server_id!"); return; } SingleClientPtr client; { boost::lock_guard lock(publisher_contexts_mutex_); M_SingleClient::iterator context_it = publisher_contexts_.find(msg->server_id); // 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() ); 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; // we need to subscribe to the init topic again subscribeInit(); } client = context_it->second; } // forward init/update to respective context client->process( msg, enable_autocomplete_transparency_ ); } void InteractiveMarkerClient::processInit( const InitConstPtr& msg ) { process(msg); } void InteractiveMarkerClient::processUpdate( const UpdateConstPtr& msg ) { process(msg); } void InteractiveMarkerClient::update() { 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; } 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; } 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::statusCb( StatusT status, const std::string& server_id, 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; } if ( status_cb_ ) { status_cb_( status, server_id, msg ); } } } ros-interactive-markers-1.11.4/src/interactive_marker_server.cpp000066400000000000000000000354261336754562700251510ustar00rootroot00000000000000/* * 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 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) { 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(); } InteractiveMarkerServer::~InteractiveMarkerServer() { if (spin_thread_.get()) { need_to_terminate_ = true; spin_thread_->join(); } if ( node_handle_.ok() ) { 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_ ); if ( pending_updates_.empty() ) { return; } M_UpdateContext::iterator update_it; 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() ); for ( update_it = pending_updates_.begin(); update_it != pending_updates_.end(); update_it++ ) { M_MarkerContext::iterator marker_context_it = marker_contexts_.find( update_it->first ); 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; } 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 ); } break; } case UpdateContext::ERASE: { if ( marker_context_it != marker_contexts_.end() ) { marker_contexts_.erase( update_it->first ); update.erases.push_back( update_it->first ); } break; } } } seq_num_++; publish( update ); publishInit(); pending_updates_.clear(); } bool InteractiveMarkerServer::erase( const std::string &name ) { boost::recursive_mutex::scoped_lock lock( mutex_ ); pending_updates_[name].update_type = UpdateContext::ERASE; return true; } void InteractiveMarkerServer::clear() { boost::recursive_mutex::scoped_lock lock( mutex_ ); // erase all markers pending_updates_.clear(); M_MarkerContext::iterator 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 ) { boost::recursive_mutex::scoped_lock lock( mutex_ ); 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 ) ) { 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."); return false; } } else { doSetPose( update_it, name, pose, header ); } return true; } bool InteractiveMarkerServer::setCallback( const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type ) { boost::recursive_mutex::scoped_lock lock( mutex_ ); 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() ) { 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() ) { // the marker exists, so we can just overwrite the existing callbacks if ( feedback_type == DEFAULT_FEEDBACK_CB ) { marker_context_it->second.default_feedback_cb = feedback_cb; } else { if ( feedback_cb ) { marker_context_it->second.feedback_cbs[feedback_type] = feedback_cb; } else { marker_context_it->second.feedback_cbs.erase( feedback_type ); } } } if ( update_it != pending_updates_.end() ) { if ( feedback_type == DEFAULT_FEEDBACK_CB ) { update_it->second.default_feedback_cb = feedback_cb; } else { if ( feedback_cb ) { update_it->second.feedback_cbs[feedback_type] = feedback_cb; } else { update_it->second.feedback_cbs.erase( feedback_type ); } } } return true; } void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker ) { boost::recursive_mutex::scoped_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; } update_it->second.update_type = UpdateContext::FULL_UPDATE; update_it->second.int_marker = int_marker; } void InteractiveMarkerServer::insert( const visualization_msgs::InteractiveMarker &int_marker, FeedbackCallback feedback_cb, uint8_t feedback_type) { insert( int_marker ); setCallback( int_marker.name, feedback_cb, feedback_type ); } bool InteractiveMarkerServer::get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const { boost::recursive_mutex::scoped_lock lock( mutex_ ); 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() ) { return false; } int_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 ) { 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; } 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; return true; } return false; } void InteractiveMarkerServer::publishInit() { 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() ); 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 ); } init_pub_.publish( init ); } void InteractiveMarkerServer::processFeedback( const FeedbackConstPtr& feedback ) { boost::recursive_mutex::scoped_lock lock( mutex_ ); 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() ) { return; } 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 ) { ROS_DEBUG( "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_client_id = feedback->client_id; if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE ) { if ( marker_context.int_marker.header.stamp == ros::Time(0) ) { // 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 ); } } // 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 ) { // call type-specific callback feedback_cb_it->second( feedback ); } else if ( marker_context.default_feedback_cb ) { // call default callback marker_context.default_feedback_cb( feedback ); } } void InteractiveMarkerServer::keepAlive() { visualization_msgs::InteractiveMarkerUpdate empty_update; empty_update.type = visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE; publish( empty_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 ) { 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 ) { 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 ); } } ros-interactive-markers-1.11.4/src/interactive_markers/000077500000000000000000000000001336754562700232305ustar00rootroot00000000000000ros-interactive-markers-1.11.4/src/interactive_markers/__init__.py000066400000000000000000000000001336754562700253270ustar00rootroot00000000000000ros-interactive-markers-1.11.4/src/interactive_markers/interactive_marker_server.py000066400000000000000000000412461336754562700310550ustar00rootroot00000000000000#!/usr/bin/env python # 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: Michael Ferguson import rospy from std_msgs.msg import Header from visualization_msgs.msg import InteractiveMarker from visualization_msgs.msg import InteractiveMarkerFeedback from visualization_msgs.msg import InteractiveMarkerInit from visualization_msgs.msg import InteractiveMarkerPose from visualization_msgs.msg import InteractiveMarkerUpdate from threading import Lock # Represents a single marker class MarkerContext: def __init__(self): self.last_feedback = rospy.Time.now() self.last_client_id = "" self.default_feedback_cb = None self.feedback_cbs = dict() self.int_marker = InteractiveMarker() # Represents an update to a single marker class UpdateContext: FULL_UPDATE = 0 POSE_UPDATE = 1 ERASE = 2 def __init__(self): self.update_type = self.FULL_UPDATE self.int_marker = InteractiveMarker() self.default_feedback_cb = None self.feedback_cbs = dict() ## Acts as a server 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: DEFAULT_FEEDBACK_CB = 255 ## @brief Create an InteractiveMarkerServer and associated ROS connections ## @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. def __init__(self, topic_ns, server_id="", q_size=100): self.topic_ns = topic_ns self.seq_num = 0 self.mutex = Lock() self.server_id = rospy.get_name() + server_id # contains the current state of all markers # string : MarkerContext self.marker_contexts = dict() # updates that have to be sent on the next publish # string : UpdateContext self.pending_updates = dict() self.init_pub = rospy.Publisher(topic_ns+"/update_full", InteractiveMarkerInit, latch=True, queue_size=100) self.update_pub = rospy.Publisher(topic_ns+"/update", InteractiveMarkerUpdate, queue_size=100) rospy.Subscriber(topic_ns+"/feedback", InteractiveMarkerFeedback, self.processFeedback, queue_size=q_size) rospy.Timer(rospy.Duration(0.5), self.keepAlive) self.publishInit() ## @brief Destruction of the interface will lead to all managed markers being cleared. def __del__(self): self.clear() self.applyChanges() ## @brief Add or replace a marker. ## 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_cb Function to call on the arrival of a feedback message. ## @param feedback_type Type of feedback for which to call the feedback. def insert(self, marker, feedback_cb=-1, feedback_type=DEFAULT_FEEDBACK_CB): with self.mutex: try: update = self.pending_updates[marker.name] except: update = UpdateContext() self.pending_updates[marker.name] = update update.update_type = UpdateContext.FULL_UPDATE update.int_marker = marker if feedback_cb != -1: self.setCallback(marker.name, feedback_cb, feedback_type) ## @brief Update the pose of a marker with the specified name ## Note: This change will not take effect until you call applyChanges() ## @param name Name of the interactive marker ## @param pose The new pose ## @param header Header replacement. Leave this empty to use the previous one. ## @return True if a marker with that name exists def setPose(self, name, pose, header=Header()): with self.mutex: try: marker_context = self.marker_contexts[name] except: marker_context = None try: update = self.pending_updates[name] except: update = None # if there's no marker and no pending addition for it, we can't update the pose if marker_context is None and update is None: return False if update is not None and update.update_type == UpdateContext.FULL_UPDATE: return False if header.frame_id is None or header.frame_id == "": # keep the old header self.doSetPose(update, name, pose, marker_context.int_marker.header) else: self.doSetPose(update, name, pose, header) return True ## @brief Erase the marker with the specified name ## Note: This change will not take effect until you call applyChanges(). ## @param name Name of the interactive marker ## @return True if a marker with that name exists def erase(self, name): with self.mutex: try: self.pending_updates[name].update_type = UpdateContext.ERASE return True except: try: self.marker_contexts[name] # check exists update = UpdateContext() update.update_type = UpdateContext.ERASE self.pending_updates[name] = update return True except: return False ## @brief Clear all markers. ## Note: This change will not take effect until you call applyChanges(). def clear(self): self.pending_updates = dict() for marker_name in self.marker_contexts.keys(): self.erase(marker_name) ## @brief 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. def setCallback(self, name, feedback_cb, feedback_type=DEFAULT_FEEDBACK_CB): with self.mutex: try: marker_context = self.marker_contexts[name] except: marker_context = None try: update = self.pending_updates[name] except: update = None if marker_context is None and update is None: return False # we need to overwrite both the callbacks for the actual marker # and the update, if there's any if marker_context: # the marker exists, so we can just overwrite the existing callbacks if feedback_type == self.DEFAULT_FEEDBACK_CB: marker_context.default_feedback_cb = feedback_cb else: if feedback_cb: marker_context.feedback_cbs[feedback_type] = feedback_cb else: del marker_context.feedback_cbs[feedback_type] if update: if feedback_type == self.DEFAULT_FEEDBACK_CB: update.default_feedback_cb = feedback_cb else: if feedback_cb: update.feedback_cbs[feedback_type] = feedback_cb else: del update.feedback_cbs[feedback_type] return True ## @brief Apply changes made since the last call to this method & ## broadcast an update to all clients. def applyChanges(self): with self.mutex: if len(self.pending_updates.keys()) == 0: return update_msg = InteractiveMarkerUpdate() update_msg.type = InteractiveMarkerUpdate.UPDATE for name, update in self.pending_updates.items(): if update.update_type == UpdateContext.FULL_UPDATE: try: marker_context = self.marker_contexts[name] except: rospy.logdebug("Creating new context for " + name) # create a new int_marker context marker_context = MarkerContext() marker_context.default_feedback_cb = update.default_feedback_cb marker_context.feedback_cbs = update.feedback_cbs self.marker_contexts[name] = marker_context marker_context.int_marker = update.int_marker update_msg.markers.append(marker_context.int_marker) elif update.update_type == UpdateContext.POSE_UPDATE: try: marker_context = self.marker_contexts[name] marker_context.int_marker.pose = update.int_marker.pose marker_context.int_marker.header = update.int_marker.header pose_update = InteractiveMarkerPose() pose_update.header = marker_context.int_marker.header pose_update.pose = marker_context.int_marker.pose pose_update.name = marker_context.int_marker.name update_msg.poses.append(pose_update) except: rospy.logerr("""\ Pending pose update for non-existing marker found. This is a bug in InteractiveMarkerInterface.""") elif update.update_type == UpdateContext.ERASE: try: marker_context = self.marker_contexts[name] del self.marker_contexts[name] update_msg.erases.append(name) except: pass self.pending_updates = dict() self.seq_num += 1 self.publish(update_msg) self.publishInit() ## @brief Get marker by name ## @param name Name of the interactive marker ## @return Marker if exists, None otherwise def get(self, name): try: update = self.pending_updates[name] except: try: marker_context = self.marker_contexts[name] except: return None return marker_context.int_marker # if there's an update pending, we'll have to account for that if update.update_type == UpdateContext.ERASE: return None elif update.update_type == UpdateContext.POSE_UPDATE: try: marker_context = self.marker_contexts[name] except: return None int_marker = marker_context.int_marker int_marker.pose = update.int_marker.pose return int_marker elif update.update_type == UpdateContext.FULL_UPDATE: return update.int_marker return None # update marker pose & call user callback. def processFeedback(self, feedback): with self.mutex: # ignore feedback for non-existing markers try: marker_context = self.marker_contexts[feedback.marker_name] except: return # if two callers try to modify the same marker, reject (timeout= 1 sec) if marker_context.last_client_id != feedback.client_id \ and (rospy.Time.now() - marker_context.last_feedback).to_sec() < 1.0: rospy.logdebug("Rejecting feedback for " + feedback.marker_name + ": conflicting feedback from separate clients.") return marker_context.last_feedback = rospy.Time.now() marker_context.last_client_id = feedback.client_id if feedback.event_type == feedback.POSE_UPDATE: if marker_context.int_marker.header.stamp == rospy.Time(): # keep the old header try: self.doSetPose(self.pending_updates[feedback.marker_name], feedback.marker_name, feedback.pose, marker_context.int_marker.header) except: self.doSetPose(None, feedback.marker_name, feedback.pose, marker_context.int_marker.header) else: try: self.doSetPose(self.pending_updates[feedback.marker_name], feedback.marker_name, feedback.pose, feedback.header) except: self.doSetPose(None, feedback.marker_name, feedback.pose, feedback.header) # call feedback handler try: feedback_cb = marker_context.feedback_cbs[feedback.event_type] feedback_cb(feedback) except KeyError: #try: marker_context.default_feedback_cb(feedback) #except: #pass # send an empty update to keep the client GUIs happy def keepAlive(self, msg): empty_msg = InteractiveMarkerUpdate() empty_msg.type = empty_msg.KEEP_ALIVE self.publish(empty_msg) # increase sequence number & publish an update def publish(self, update): update.server_id = self.server_id update.seq_num = self.seq_num self.update_pub.publish(update) # publish the current complete state to the latched "init" topic def publishInit(self): with self.mutex: init = InteractiveMarkerInit() init.server_id = self.server_id init.seq_num = self.seq_num for name, marker_context in self.marker_contexts.items(): rospy.logdebug("Publishing " + name) init.markers.append(marker_context.int_marker) self.init_pub.publish(init) # update pose, schedule update without locking def doSetPose(self, update, name, pose, header): if update is None: update = UpdateContext() update.update_type = UpdateContext.POSE_UPDATE self.pending_updates[name] = update elif update.update_type != UpdateContext.FULL_UPDATE: update.update_type = UpdateContext.POSE_UPDATE update.int_marker.pose = pose update.int_marker.header = header rospy.logdebug("Marker '" + name + "' is now at " + str(pose.position.x) + ", " + str(pose.position.y) + ", " + str(pose.position.z)) ros-interactive-markers-1.11.4/src/interactive_markers/menu_handler.py000066400000000000000000000163221336754562700262470ustar00rootroot00000000000000#!/usr/bin/env python # 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: Michael Ferguson import rospy from visualization_msgs.msg import InteractiveMarkerFeedback from visualization_msgs.msg import MenuEntry class EntryContext: def __init__(self): self.title = "" self.command = "" self.command_type = 0 self.sub_entries = list() self.visible = True self.check_state = 0 self.feedback_cb = None ## @brief Simple non-intrusive helper class which creates a menu and maps its ## entries to function callbacks class MenuHandler: NO_CHECKBOX = 0 CHECKED = 1 UNCHECKED = 2 def __init__(self): self.top_level_handles_ = list() # std::vector self.entry_contexts_ = dict() # boost::unordered_map self.current_handle_ = 1 self.managed_markers_ = set() # std::set ## Insert a new menu item def insert(self, title, parent=None, command_type=MenuEntry.FEEDBACK, command="", callback=None): handle = self.doInsert(title, command_type, command, callback) if parent is not None: try: parent_context = self.entry_contexts_[parent] parent_context.sub_entries.append(handle) except: rospy.logerr("Parent menu entry " + str(parent) + " not found.") return None else: self.top_level_handles_.append(handle) return handle ## Specify if an entry should be visible or hidden def setVisible(self, handle, visible): try: context = self.entry_contexts_[handle] context.visible = visible return True except: return False ## Specify if an entry is checked or can't be checked at all def setCheckState(self, handle, check_state): try: context = self.entry_contexts_[handle] context.check_state = check_state return True except: return False ## Get the current state of an entry ## @return CheckState if the entry exists and has checkbox, None otherwise def getCheckState(self, handle): try: context = self.entry_contexts_[handle] return context.check_state except: return None ## Copy current menu state into the marker given by the specified name & ## divert callback for MENU_SELECT feedback to this manager def apply(self, server, marker_name): marker = server.get(marker_name) if not marker: self.managed_markers_.remove(marker_name) return False marker.menu_entries = list() self.pushMenuEntries(self.top_level_handles_, marker.menu_entries, 0) server.insert(marker, self.processFeedback, InteractiveMarkerFeedback.MENU_SELECT) self.managed_markers_.add(marker_name) return True ## Re-apply to all markers that this was applied to previously def reApply(self, server): success = True # self.apply() might remove elements from # self.managed_markers_. To prevent errors, copy the # managed_markers sequence and iterate over the copy managed_markers = list(self.managed_markers_) for marker in managed_markers: success = self.apply(server, marker) and success return success ## @brief Get the title for the given menu entry ## @return The title, None if menu entry does not exist. def getTitle(self, handle): try: return self.entry_contexts_[handle].title except: return None # Call registered callback functions for given feedback command def processFeedback(self, feedback): try: context = self.entry_contexts_[feedback.menu_entry_id] except KeyError: return context.feedback_cb(feedback) # Create and push MenuEntry objects from handles_in onto # entries_out. Calls itself recursively to add the entire menu tree. def pushMenuEntries(self, handles_in, entries_out, parent_handle): for handle in handles_in: try: context = self.entry_contexts_[handle] if not context.visible: continue entries_out.append(self.makeEntry(context, handle, parent_handle)) if not self.pushMenuEntries(context.sub_entries, entries_out, handle): return False except: rospy.logerr("Internal error: context handle not found! This is a bug in MenuHandler.") return False return True def makeEntry(self, context, handle, parent_handle): menu_entry = MenuEntry() if context.check_state == self.NO_CHECKBOX: menu_entry.title = context.title elif context.check_state == self.CHECKED: menu_entry.title = "[x] "+context.title elif context.check_state == self.UNCHECKED: menu_entry.title = "[ ] "+context.title menu_entry.command = context.command menu_entry.command_type = context.command_type menu_entry.id = handle menu_entry.parent_id = parent_handle return menu_entry # Insert without adding a top-level entry def doInsert(self, title, command_type, command, feedback_cb): handle = self.current_handle_ self.current_handle_ += 1 context = EntryContext() context.title = title context.command = command context.command_type = command_type context.visible = True context.check_state = self.NO_CHECKBOX context.feedback_cb = feedback_cb self.entry_contexts_[handle] = context return handle ros-interactive-markers-1.11.4/src/menu_handler.cpp000066400000000000000000000207361336754562700223440ustar00rootroot00000000000000/* * 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 namespace interactive_markers { MenuHandler::MenuHandler() : current_handle_(1) { } 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 ); return handle; } 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 ); return handle; } MenuHandler::EntryHandle MenuHandler::insert( EntryHandle parent, const std::string &title, const FeedbackCallback &feedback_cb ) { boost::unordered_map::iterator parent_context = entry_contexts_.find( parent ); ROS_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 ); return handle; } 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 ); ROS_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 ); return handle; } bool MenuHandler::setVisible( EntryHandle handle, bool visible ) { boost::unordered_map::iterator context = entry_contexts_.find( handle ); if ( context == entry_contexts_.end() ) { return false; } context->second.visible = visible; return true; } bool MenuHandler::setCheckState( EntryHandle handle, CheckState check_state ) { boost::unordered_map::iterator context = entry_contexts_.find( handle ); if ( context == entry_contexts_.end() ) { return false; } context->second.check_state = check_state; return true; } bool MenuHandler::getCheckState( EntryHandle handle, CheckState &check_state ) const { boost::unordered_map::const_iterator context = entry_contexts_.find( handle ); if ( context == entry_contexts_.end() ) { check_state = NO_CHECKBOX; return false; } check_state = context->second.check_state; return true; } bool MenuHandler::apply( InteractiveMarkerServer &server, const std::string &marker_name ) { visualization_msgs::InteractiveMarker 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 ); return false; } int_marker.menu_entries.clear(); 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 ); return true; } bool MenuHandler::pushMenuEntries( std::vector& handles_in, std::vector& entries_out, EntryHandle parent_handle ) { for ( unsigned t = 0; t < handles_in.size(); t++ ) { EntryHandle handle = handles_in[t]; boost::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." ); return false; } EntryContext& context = context_it->second; if ( !context.visible ) { continue; } 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 success = true; std::set::iterator it = managed_markers_.begin(); 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; 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 ) { EntryHandle handle = current_handle_; current_handle_++; EntryContext context; context.title = title; context.command = command; context.command_type = command_type; context.visible = true; context.check_state = NO_CHECKBOX; context.feedback_cb = feedback_cb; entry_contexts_[handle] = context; return handle; } visualization_msgs::MenuEntry MenuHandler::makeEntry( EntryContext& context, EntryHandle handle, EntryHandle parent_handle ) { visualization_msgs::MenuEntry menu_entry; switch ( context.check_state ) { case NO_CHECKBOX: menu_entry.title = context.title; break; case CHECKED: menu_entry.title = "[x] "+context.title; break; case UNCHECKED: menu_entry.title = "[ ] "+context.title; break; } menu_entry.command = context.command; menu_entry.command_type = context.command_type; menu_entry.id = handle; menu_entry.parent_id = parent_handle; return menu_entry; } void MenuHandler::processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) { boost::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 ); } } bool MenuHandler::getTitle( EntryHandle handle, std::string &title ) const { boost::unordered_map::const_iterator context = entry_contexts_.find( handle ); if ( context == entry_contexts_.end() ) { return false; } title = context->second.title; return true; } } ros-interactive-markers-1.11.4/src/message_context.cpp000066400000000000000000000177561336754562700231030ustar00rootroot00000000000000/* * 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"); 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) { // copy message, as we will be modifying it msg = boost::make_shared( *_msg ); init(); } template MessageContext& MessageContext::operator=( const MessageContext& other ) { open_marker_idx_ = other.open_marker_idx_; open_pose_idx_ = other.open_pose_idx_; target_frame_ = other.target_frame_; enable_autocomplete_transparency_ = other.enable_autocomplete_transparency_; return *this; } template bool MessageContext::getTransform( std_msgs::Header& header, geometry_msgs::Pose& pose_msg ) { 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() ); // 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; // store transformed pose in original message tf::poseTFToMsg( pose, pose_msg ); ROS_DEBUG_STREAM("Changing " << header.frame_id << " to "<< target_frame_); 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() ); } return false; } return true; // all other exceptions need to be handled outside } template 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 ]; // transform interactive marker 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() ); ++idx_it; } } } template 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 ) ) { 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() ); ++idx_it; } } } template bool MessageContext::isReady() { return open_marker_idx_.empty() && open_pose_idx_.empty(); } template<> 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; iposes.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; iposes.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 ) { msg->poses[i].pose.orientation.w = 1; } } } template<> void MessageContext::init() { // mark all transforms as being missing for ( size_t i=0; imarkers.size(); i++ ) { open_marker_idx_.push_back( i ); } for( unsigned i=0; imarkers.size(); i++ ) { autoComplete( msg->markers[i], enable_autocomplete_transparency_ ); } } template<> 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 ); } } template<> void MessageContext::getTfTransforms( ) { getTfTransforms( msg->markers, open_marker_idx_ ); if ( isReady() ) { DBG_MSG( "Init message with seq_num=%lu is ready.", msg->seq_num ); } } // explicit template instantiation template class MessageContext; template class MessageContext; } ros-interactive-markers-1.11.4/src/single_client.cpp000066400000000000000000000224731336754562700225220ustar00rootroot00000000000000/* * 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); } } ros-interactive-markers-1.11.4/src/test/000077500000000000000000000000001336754562700201465ustar00rootroot00000000000000ros-interactive-markers-1.11.4/src/test/bursty_tf.cpp000066400000000000000000000124431336754562700226770ustar00rootroot00000000000000/* * 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(); } ros-interactive-markers-1.11.4/src/test/client_test.cpp000066400000000000000000000562251336754562700232010ustar00rootroot00000000000000/* * 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(); } ros-interactive-markers-1.11.4/src/test/missing_tf.cpp000066400000000000000000000124601336754562700230170ustar00rootroot00000000000000/* * 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(); } ros-interactive-markers-1.11.4/src/test/server_client_test.cpp000066400000000000000000000156371336754562700245710ustar00rootroot00000000000000/* * 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(); } ros-interactive-markers-1.11.4/src/test/server_test.cpp000066400000000000000000000070201336754562700232160ustar00rootroot00000000000000/* * 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) ); 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) ); 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(); //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(); } ros-interactive-markers-1.11.4/src/tools.cpp000066400000000000000000000315051336754562700210370ustar00rootroot00000000000000/* * 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 #include #include namespace interactive_markers { void autoComplete( visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency ) { // this is a 'delete' message. no need for action. if ( msg.controls.empty() ) { return; } // default size 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 ) { 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 ); int_marker_orientation.normalize(); msg.pose.orientation.x = int_marker_orientation.x(); msg.pose.orientation.y = int_marker_orientation.y(); msg.pose.orientation.z = int_marker_orientation.z(); 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++ ) { std::string name = msg.controls[c].name; 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 ); } } void autoComplete( const visualization_msgs::InteractiveMarker &msg, visualization_msgs::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 ) { 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: break; case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS: control.markers.reserve(2); 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 ); break; case visualization_msgs::InteractiveMarkerControl::BUTTON: break; case visualization_msgs::InteractiveMarkerControl::MENU: makeViewFacingButton( msg, control, control.description ); break; default: break; } } // 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 ); // fill in missing pose information into the markers for ( unsigned m=0; m 0.0 ) { marker.color.a = 1.0; } } } void makeArrow( const visualization_msgs::InteractiveMarker &msg, visualization_msgs::InteractiveMarkerControl &control, float pos ) { visualization_msgs::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.scale.z = msg.scale * 0.2; assignDefaultColor(marker, control.orientation); float dist = fabs(pos); float dir = pos > 0 ? 1 : -1; float inner = 0.5 * dist; float outer = inner + 0.4; 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 ); } void makeDisc( const visualization_msgs::InteractiveMarker &msg, visualization_msgs::InteractiveMarkerControl &control, float width ) { visualization_msgs::Marker marker; // rely on the auto-completion for the correct orientation marker.pose.orientation = control.orientation; marker.type = visualization_msgs::Marker::TRIANGLE_LIST; marker.scale.x = msg.scale; marker.scale.y = msg.scale; marker.scale.z = msg.scale; assignDefaultColor(marker, control.orientation); // compute points on a circle in the y-z plane int steps = 36; std::vector circle1, circle2; circle1.reserve(steps); circle2.reserve(steps); geometry_msgs::Point v1,v2; for ( int i=0; iy ? 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; marker.color.g = y / max_xyz; marker.color.b = z / max_xyz; marker.color.a = 0.5; } visualization_msgs::InteractiveMarkerControl makeTitle( const visualization_msgs::InteractiveMarker &msg ) { visualization_msgs::Marker marker; marker.type = visualization_msgs::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; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 1.0; marker.color.a = 1.0; 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; control.always_visible = true; control.markers.push_back( marker ); autoComplete( msg, control ); return control; } } ros-interactive-markers-1.11.4/test/000077500000000000000000000000001336754562700173575ustar00rootroot00000000000000ros-interactive-markers-1.11.4/test/cpp_client.test000066400000000000000000000001421336754562700223750ustar00rootroot00000000000000 ros-interactive-markers-1.11.4/test/cpp_server.test000066400000000000000000000001421336754562700224250ustar00rootroot00000000000000 ros-interactive-markers-1.11.4/test/cpp_server_client.test000066400000000000000000000001631336754562700237660ustar00rootroot00000000000000