test_bumper.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of the Willow Garage nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 #
34 
35 ## Gazebo test bumper for a box
36 
37 PKG = 'gazebo_plugins'
38 NAME = 'test_bumper'
39 
40 import math
41 import roslib
42 roslib.load_manifest(PKG)
43 
44 import sys, unittest
45 import os, os.path, threading, time
46 import rospy, rostest
47 from nav_msgs.msg import *
48 from geometry_msgs.msg import Pose
49 from geometry_msgs.msg import Twist
50 from geometry_msgs.msg import Wrench
51 from geometry_msgs.msg import Vector3
52 from gazebo_msgs.msg import ContactsState
53 from gazebo_msgs.msg import ContactState
54 
55 class BumperTest(unittest.TestCase):
56  def __init__(self, *args):
57  super(BumperTest, self).__init__(*args)
58  self.success = False
59 
60  self.sample_count = 0
61 
62  self.min_samples_topic = "~min_samples"
63  self.min_samples = 1000
64 
65  # in seconds
66  self.test_duration_topic = "~test_duration"
67  self.test_duration = 10.0
68 
69  # test start time in seconds
70  self.test_start_time_topic = "~test_start_time"
71  self.test_start_time = 0.0
72 
73  self.bumper_topic_name = "~bumper_topic_name"
74  self.bumper_topic = "/test_bumper"
75  self.bumper_state = ContactsState()
76 
77  self.fz_sum = 0
78  self.fz_avg = 0
79 
80  def bumperStateInput(self, contacts_state):
81  self.bumper_state = contacts_state
82  self.sample_count+=1
83  self.fz_sum += contacts_state.states[0].total_wrench.force.z
84  self.fz_avg = self.fz_sum / self.sample_count
85 
86  def checkContact(self):
87  # see if total wrench is close to 98.1N
88  if self.sample_count > 20:
89  if abs(self.fz_avg - 98.1) < 0.01:
90  print "z force ",self.fz_avg
91  self.success = True
92 
93 
94  def test_bumper(self):
95  print "LNK\n"
96  rospy.init_node(NAME, anonymous=True)
97  self.bumper_topic = rospy.get_param(self.bumper_topic_name,self.bumper_topic);
98  self.min_samples = rospy.get_param(self.min_samples_topic,self.min_samples);
99  self.test_duration = rospy.get_param(self.test_duration_topic,self.test_duration);
100  self.test_start_time = rospy.get_param(self.test_start_time_topic,self.test_start_time);
101 
102  while not rospy.is_shutdown() and time.time() < self.test_start_time:
103  rospy.stdinfo("Waiting for test to start at time [%s]"% self.test_start_time)
104  time.sleep(0.1)
105 
106  print "subscribe"
107  rospy.Subscriber(self.bumper_topic, ContactsState, self.bumperStateInput)
108 
109  start_t = time.time()
110  timeout_t = start_t + self.test_duration
111  while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
112  self.checkContact()
113  time.sleep(0.1)
114  self.assert_(self.success)
115 
116 if __name__ == '__main__':
117  print "Waiting for test to start at time "
118  rostest.run(PKG, sys.argv[0], BumperTest, sys.argv) #, text_mode=True)
119 
120 
def bumperStateInput(self, contacts_state)
Definition: test_bumper.py:80
def __init__(self, args)
Definition: test_bumper.py:56


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39