From 696945bf05f6d1ac64249a7047fef8fa3a60ffef Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Wed, 9 Nov 2016 18:13:51 +0900 Subject: [PATCH] Add server to suppress realtime monitoring of - suppress_body_avoidance - suppress_collision_avoidance - suppress_contact_safety - suppress_cuff_interaction - suppress_gravity_compensation - suppress_hand_overwrench_safety --- CMakeLists.txt | 1 + cfg/SuppressRealtimeMonitoringServer.cfg | 59 +++++++++++++ .../suppress_realtime_monitoring_server.py | 86 +++++++++++++++++++ 3 files changed, 146 insertions(+) create mode 100755 cfg/SuppressRealtimeMonitoringServer.cfg create mode 100755 scripts/suppress_realtime_monitoring_server.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 1fe4c3c..bd43180 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ generate_dynamic_reconfigure_options( cfg/PositionFFJointTrajectoryActionServer.cfg cfg/GripperActionServer.cfg cfg/HeadActionServer.cfg + cfg/SuppressRealtimeMonitoringServer.cfg ) add_dependencies(${PROJECT_NAME}_gencfg baxter_core_msgs_generate_messages_py) diff --git a/cfg/SuppressRealtimeMonitoringServer.cfg b/cfg/SuppressRealtimeMonitoringServer.cfg new file mode 100755 index 0000000..f332ec6 --- /dev/null +++ b/cfg/SuppressRealtimeMonitoringServer.cfg @@ -0,0 +1,59 @@ +#!/usr/bin/env python + +# Copyright (c) 2016, Kentaro Wada +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# 2. 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. +# 3. Neither the name of the Kentaro Wada 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. + +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator +from dynamic_reconfigure.parameter_generator_catkin import bool_t + + +PKG = 'baxter_interface' +NAME = 'SuppressRealtimeMonitoringServer' + + +gen = ParameterGenerator() + +arms = ['left', 'right'] + +params = [ + 'suppress_body_avoidance', + 'suppress_collision_avoidance', + 'suppress_contact_safety', + 'suppress_cuff_interaction', + 'suppress_gravity_compensation', + 'suppress_hand_overwrench_safety', +] + +for arm in arms: + for param in params: + gen.add( + arm + '_' + param, bool_t, 0, + param.capitalize().replace('_', ' ') + '.', + False, + ) + +exit(gen.generate(PKG, '', NAME)) diff --git a/scripts/suppress_realtime_monitoring_server.py b/scripts/suppress_realtime_monitoring_server.py new file mode 100755 index 0000000..36c0db9 --- /dev/null +++ b/scripts/suppress_realtime_monitoring_server.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python + +# Copyright (c) 2016, Kentaro Wada +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# 2. 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. +# 3. Neither the name of the Kentaro Wada 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. + +import os + +from dynamic_reconfigure.server import Server +import rospy +from std_msgs.msg import Empty + +from baxter_interface.cfg import SuppressRealtimeMonitoringServerConfig + + +class SuppressRealtimeMonitoringServer(object): + + arms = ['left', 'right'] + + params = [ + 'suppress_body_avoidance', + 'suppress_collision_avoidance', + 'suppress_contact_safety', + 'suppress_cuff_interaction', + 'suppress_gravity_compensation', + 'suppress_hand_overwrench_safety', + ] + + def __init__(self): + # Initialize config and set dynamic parameter server + self.config = {} + self.dynparam_server = Server( + SuppressRealtimeMonitoringServerConfig, self._dynparam_cb) + # Initialize config and publishers + self.publishers = {} + for arm in self.arms: + for param in self.params: + key = arm + '_' + param + self.publishers[key] = rospy.Publisher( + os.path.join('/robot/limb', arm, param), Empty, + queue_size=1) + # Set timer with 10Hz + # and it's enough because 5Hz is requested for supression + self.timer = rospy.Timer( + rospy.Duration(1.0 / 10), self.suppress_timer_cb) + + def _dynparam_cb(self, config, level): + for arm in self.arms: + for param in self.params: + key = arm + '_' + param + self.config[key] = config[key] + return config + + def suppress_timer_cb(self, event): + for key, pub in self.publishers.items(): + if self.config[key]: + pub.publish(Empty()) + + +if __name__ == '__main__': + rospy.init_node('suppress_realtime_monitoring_server') + server = SuppressRealtimeMonitoringServer() + rospy.spin()