simulation
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
nodding_io.py
[詳解]
1
#!/usr/bin/env python3
2
"""Gazeboの首振り関節を動かし、計測した関節の状態をnodding_lidarへ渡す。"""
3
4
import
math
5
6
import
rclpy
7
from
rclpy.node
import
Node
8
from
rclpy.qos
import
qos_profile_sensor_data
9
from
sensor_msgs.msg
import
JointState, LaserScan
10
from
std_msgs.msg
import
Float64
11
12
13
class
NoddingIO
(Node):
14
def
__init__
(self):
15
super().
__init__
(
'nodding_io'
)
16
self.declare_parameter(
'amplitude_deg'
, 20.0)
17
self.declare_parameter(
'period_sec'
, 2.0)
18
self.
_cmd
= self.create_publisher(Float64,
'/nodding_cmd'
, 10)
19
self.
_servo
= self.create_publisher(JointState,
'/servo_state'
, 30)
20
self.
_scan
= self.create_publisher(LaserScan,
'/scan'
, qos_profile_sensor_data)
21
self.create_subscription(
22
JointState,
'/joint_states'
, self.
_on_joints
, qos_profile_sensor_data)
23
self.create_subscription(
24
LaserScan,
'/scan_raw'
, self.
_on_scan
, qos_profile_sensor_data)
25
self.create_timer(0.02, self.
_command
)
26
27
def
_command
(self):
28
stamp = self.get_clock().now()
29
if
stamp.nanoseconds == 0:
30
return
31
amplitude = math.radians(self.get_parameter(
'amplitude_deg'
).value)
32
period = self.get_parameter(
'period_sec'
).value
33
if
period <= 0.0:
34
return
35
phase = 2.0 * math.pi * (stamp.nanoseconds * 1e-9 / period)
36
msg = Float64()
37
msg.data = -amplitude * math.cos(phase)
38
self.
_cmd
.publish(msg)
39
40
def
_on_joints
(self, source):
41
for
index, name
in
enumerate(source.name):
42
if
name.split(
'::'
)[-1] !=
'nodding_joint'
:
43
continue
44
if
index >= len(source.position)
or
index >= len(source.velocity):
45
return
46
state = JointState()
47
state.header = source.header
48
state.name = [
'nodding_joint'
]
49
state.position = [source.position[index]]
50
state.velocity = [source.velocity[index]]
51
self.
_servo
.publish(state)
52
return
53
54
def
_on_scan
(self, scan):
55
# Gazebo samples the entire scan at one simulation instant. Its rays
56
# have no individual measurement times, unlike the rotating real S1.
57
scan.time_increment = 0.0
58
scan.scan_time = 0.0
59
self.
_scan
.publish(scan)
60
61
62
def
main
(args=None):
63
rclpy.init(args=args)
64
node =
NoddingIO
()
65
try
:
66
rclpy.spin(node)
67
finally
:
68
node.destroy_node()
69
rclpy.shutdown()
70
71
72
if
__name__ ==
'__main__'
:
73
main
()
nodding_io.NoddingIO
定義
nodding_io.py:13
nodding_io.NoddingIO.__init__
__init__(self)
定義
nodding_io.py:14
nodding_io.NoddingIO._on_joints
_on_joints
定義
nodding_io.py:22
nodding_io.NoddingIO._servo
_servo
定義
nodding_io.py:19
nodding_io.NoddingIO._command
_command
定義
nodding_io.py:25
nodding_io.NoddingIO._on_scan
_on_scan
定義
nodding_io.py:24
nodding_io.NoddingIO._scan
_scan
定義
nodding_io.py:20
nodding_io.NoddingIO._cmd
_cmd
定義
nodding_io.py:18
nodding_io.main
main(args=None)
定義
nodding_io.py:62
scripts
nodding_io.py
構築:
1.18.0