sample_collision_detector_client.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 from __future__ import division
4 
5 from geometry_msgs.msg import PoseStamped
6 from jsk_recognition_msgs.srv import CheckCollision
7 from jsk_recognition_msgs.srv import CheckCollisionRequest
8 import rospy
9 from sensor_msgs.msg import JointState
10 from std_msgs.msg import Bool
11 
12 
14 
15  def __init__(self):
16  super(self.__class__, self).__init__()
17 
18  self.joint_states = None
19  self.root_link_pose = PoseStamped()
20  self.root_link_pose.pose.orientation.w = 1
21 
22  self.req_caller = rospy.ServiceProxy(
23  '~check_collision', CheckCollision)
24  self.pub = rospy.Publisher('~output', Bool, queue_size=1)
25  self.sub = rospy.Subscriber(
26  '~joint_states', JointState, self._joint_cb, queue_size=1)
27 
28  rate = rospy.get_param('~request_rate', 1.0)
29  rospy.Timer(rospy.Duration(1 / rate), self._timer_cb)
30 
31  def _joint_cb(self, msg):
32  self.joint_states = msg
33  self.root_link_pose.header = msg.header
34 
35  def _timer_cb(self, event):
36  if self.joint_states is None:
37  return
38 
39  req = CheckCollisionRequest()
40  req.joint = self.joint_states
41  req.pose = self.root_link_pose
42  res = self.req_caller(req)
43  self.pub.publish(data=res.result)
44 
45 
46 if __name__ == '__main__':
47  rospy.init_node('sample_collision_detector_client')
49  rospy.spin()


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47