Main Page
Related Pages
Namespaces
Classes
Files
File List
File Members
sample
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
13
class
SampleCollisionDetectorClient
(
object
):
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'
)
48
app =
SampleCollisionDetectorClient
()
49
rospy.spin()
sample_collision_detector_client.SampleCollisionDetectorClient._joint_cb
def _joint_cb(self, msg)
Definition:
sample_collision_detector_client.py:31
sample_collision_detector_client.SampleCollisionDetectorClient.root_link_pose
root_link_pose
Definition:
sample_collision_detector_client.py:19
srv
sample_collision_detector_client.SampleCollisionDetectorClient.pub
pub
Definition:
sample_collision_detector_client.py:24
sample_collision_detector_client.SampleCollisionDetectorClient.joint_states
joint_states
Definition:
sample_collision_detector_client.py:18
sample_collision_detector_client.SampleCollisionDetectorClient.req_caller
req_caller
Definition:
sample_collision_detector_client.py:22
object
msg
sample_collision_detector_client.SampleCollisionDetectorClient
Definition:
sample_collision_detector_client.py:13
sample_collision_detector_client.SampleCollisionDetectorClient.sub
sub
Definition:
sample_collision_detector_client.py:25
sample_collision_detector_client.SampleCollisionDetectorClient.__init__
def __init__(self)
Definition:
sample_collision_detector_client.py:15
sample_collision_detector_client.SampleCollisionDetectorClient._timer_cb
def _timer_cb(self, event)
Definition:
sample_collision_detector_client.py:35
jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47