simulation
読み取り中…
検索中…
一致する文字列を見つけられません
nodding_io.py
[詳解]
1#!/usr/bin/env python3
2"""Gazeboの首振り関節を動かし、計測した関節の状態をnodding_lidarへ渡す。"""
3
4import math
5
6import rclpy
7from rclpy.node import Node
8from rclpy.qos import qos_profile_sensor_data
9from sensor_msgs.msg import JointState, LaserScan
10from std_msgs.msg import Float64
11
12
13class 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
62def 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
72if __name__ == '__main__':
73 main()
main(args=None)